--- qemu/hw/esp.c 2018/04/24 16:41:32 1.1.1.3 +++ qemu/hw/esp.c 2018/04/24 16:43:40 1.1.1.4 @@ -38,17 +38,14 @@ do { printf("ESP: set_irq(%d): %d\n", (i #define ESPDMA_REGS 4 #define ESPDMA_MAXADDR (ESPDMA_REGS * 4 - 1) #define ESP_MAXREG 0x3f -#define TI_BUFSZ 1024*1024 // XXX +#define TI_BUFSZ 32 #define DMA_VER 0xa0000000 #define DMA_INTR 1 #define DMA_INTREN 0x10 +#define DMA_WRITE_MEM 0x100 #define DMA_LOADED 0x04000000 typedef struct ESPState ESPState; -typedef int ESPDMAFunc(ESPState *s, - target_phys_addr_t phys_addr, - int transfer_size1); - struct ESPState { BlockDriverState **bd; uint8_t rregs[ESP_MAXREG]; @@ -57,12 +54,14 @@ struct ESPState { uint32_t espdmaregs[ESPDMA_REGS]; uint32_t ti_size; uint32_t ti_rptr, ti_wptr; - int ti_dir; uint8_t ti_buf[TI_BUFSZ]; + int sense; int dma; - ESPDMAFunc *dma_cb; - int64_t offset, len; - int target; + SCSIDevice *scsi_dev[MAX_DISKS]; + SCSIDevice *current_dev; + uint8_t cmdbuf[TI_BUFSZ]; + int cmdlen; + int do_cmd; }; #define STAT_DO 0x00 @@ -83,394 +82,200 @@ struct ESPState { #define SEQ_0 0x0 #define SEQ_CD 0x4 -/* XXX: stolen from ide.c, move to common ATAPI/SCSI library */ -static void lba_to_msf(uint8_t *buf, int lba) -{ - lba += 150; - buf[0] = (lba / 75) / 60; - buf[1] = (lba / 75) % 60; - buf[2] = lba % 75; -} - -static inline void cpu_to_ube16(uint8_t *buf, int val) -{ - buf[0] = val >> 8; - buf[1] = val; -} - -static inline void cpu_to_ube32(uint8_t *buf, unsigned int val) -{ - buf[0] = val >> 24; - buf[1] = val >> 16; - buf[2] = val >> 8; - buf[3] = val; -} - -/* same toc as bochs. Return -1 if error or the toc length */ -/* XXX: check this */ -static int cdrom_read_toc(int nb_sectors, uint8_t *buf, int msf, int start_track) -{ - uint8_t *q; - int len; - - if (start_track > 1 && start_track != 0xaa) - return -1; - q = buf + 2; - *q++ = 1; /* first session */ - *q++ = 1; /* last session */ - if (start_track <= 1) { - *q++ = 0; /* reserved */ - *q++ = 0x14; /* ADR, control */ - *q++ = 1; /* track number */ - *q++ = 0; /* reserved */ - if (msf) { - *q++ = 0; /* reserved */ - lba_to_msf(q, 0); - q += 3; - } else { - /* sector 0 */ - cpu_to_ube32(q, 0); - q += 4; - } - } - /* lead out track */ - *q++ = 0; /* reserved */ - *q++ = 0x16; /* ADR, control */ - *q++ = 0xaa; /* track number */ - *q++ = 0; /* reserved */ - if (msf) { - *q++ = 0; /* reserved */ - lba_to_msf(q, nb_sectors); - q += 3; - } else { - cpu_to_ube32(q, nb_sectors); - q += 4; - } - len = q - buf; - cpu_to_ube16(buf, len - 2); - return len; -} - -/* mostly same info as PearPc */ -static int cdrom_read_toc_raw(int nb_sectors, uint8_t *buf, int msf, - int session_num) -{ - uint8_t *q; - int len; - - q = buf + 2; - *q++ = 1; /* first session */ - *q++ = 1; /* last session */ - - *q++ = 1; /* session number */ - *q++ = 0x14; /* data track */ - *q++ = 0; /* track number */ - *q++ = 0xa0; /* lead-in */ - *q++ = 0; /* min */ - *q++ = 0; /* sec */ - *q++ = 0; /* frame */ - *q++ = 0; - *q++ = 1; /* first track */ - *q++ = 0x00; /* disk type */ - *q++ = 0x00; - - *q++ = 1; /* session number */ - *q++ = 0x14; /* data track */ - *q++ = 0; /* track number */ - *q++ = 0xa1; - *q++ = 0; /* min */ - *q++ = 0; /* sec */ - *q++ = 0; /* frame */ - *q++ = 0; - *q++ = 1; /* last track */ - *q++ = 0x00; - *q++ = 0x00; - - *q++ = 1; /* session number */ - *q++ = 0x14; /* data track */ - *q++ = 0; /* track number */ - *q++ = 0xa2; /* lead-out */ - *q++ = 0; /* min */ - *q++ = 0; /* sec */ - *q++ = 0; /* frame */ - if (msf) { - *q++ = 0; /* reserved */ - lba_to_msf(q, nb_sectors); - q += 3; - } else { - cpu_to_ube32(q, nb_sectors); - q += 4; - } - - *q++ = 1; /* session number */ - *q++ = 0x14; /* ADR, control */ - *q++ = 0; /* track number */ - *q++ = 1; /* point */ - *q++ = 0; /* min */ - *q++ = 0; /* sec */ - *q++ = 0; /* frame */ - if (msf) { - *q++ = 0; - lba_to_msf(q, 0); - q += 3; - } else { - *q++ = 0; - *q++ = 0; - *q++ = 0; - *q++ = 0; - } - - len = q - buf; - cpu_to_ube16(buf, len - 2); - return len; -} - -static int esp_write_dma_cb(ESPState *s, - target_phys_addr_t phys_addr, - int transfer_size1) -{ - DPRINTF("Write callback (offset %lld len %lld size %d trans_size %d)\n", - s->offset, s->len, s->ti_size, transfer_size1); - bdrv_write(s->bd[s->target], s->offset, s->ti_buf, s->len); - s->offset = 0; - s->len = 0; - s->target = 0; - return 0; -} - -static void handle_satn(ESPState *s) +static int get_cmd(ESPState *s, uint8_t *buf) { - uint8_t buf[32]; uint32_t dmaptr, dmalen; - unsigned int i; - int64_t nb_sectors; int target; dmalen = s->wregs[0] | (s->wregs[1] << 8); target = s->wregs[4] & 7; - DPRINTF("Select with ATN len %d target %d\n", dmalen, target); + DPRINTF("get_cmd: len %d target %d\n", dmalen, target); if (s->dma) { dmaptr = iommu_translate(s->espdmaregs[1]); - DPRINTF("DMA Direction: %c, addr 0x%8.8x\n", s->espdmaregs[0] & 0x100? 'w': 'r', dmaptr); + DPRINTF("DMA Direction: %c, addr 0x%8.8x\n", + s->espdmaregs[0] & DMA_WRITE_MEM ? 'w': 'r', dmaptr); cpu_physical_memory_read(dmaptr, buf, dmalen); } else { buf[0] = 0; memcpy(&buf[1], s->ti_buf, dmalen); dmalen++; } - for (i = 0; i < dmalen; i++) { - DPRINTF("Command %2.2x\n", buf[i]); - } - s->ti_dir = 0; + s->ti_size = 0; s->ti_rptr = 0; s->ti_wptr = 0; - if (target >= 4 || !s->bd[target]) { // No such drive + if (target >= 4 || !s->scsi_dev[target]) { + // No such drive s->rregs[4] = STAT_IN; s->rregs[5] = INTR_DC; s->rregs[6] = SEQ_0; s->espdmaregs[0] |= DMA_INTR; pic_set_irq(s->irq, 1); - return; + return 0; } - switch (buf[1]) { - case 0x0: - DPRINTF("Test Unit Ready (len %d)\n", buf[5]); - break; - case 0x12: - DPRINTF("Inquiry (len %d)\n", buf[5]); - memset(s->ti_buf, 0, 36); - if (bdrv_get_type_hint(s->bd[target]) == BDRV_TYPE_CDROM) { - s->ti_buf[0] = 5; - memcpy(&s->ti_buf[16], "QEMU CDROM ", 16); - } else { - s->ti_buf[0] = 0; - memcpy(&s->ti_buf[16], "QEMU HARDDISK ", 16); - } - memcpy(&s->ti_buf[8], "QEMU ", 8); - s->ti_buf[2] = 1; - s->ti_buf[3] = 2; - s->ti_buf[4] = 32; - s->ti_dir = 1; - s->ti_size = 36; - break; - case 0x1a: - DPRINTF("Mode Sense(6) (page %d, len %d)\n", buf[3], buf[5]); - break; - case 0x25: - DPRINTF("Read Capacity (len %d)\n", buf[5]); - memset(s->ti_buf, 0, 8); - bdrv_get_geometry(s->bd[target], &nb_sectors); - s->ti_buf[0] = (nb_sectors >> 24) & 0xff; - s->ti_buf[1] = (nb_sectors >> 16) & 0xff; - s->ti_buf[2] = (nb_sectors >> 8) & 0xff; - s->ti_buf[3] = nb_sectors & 0xff; - s->ti_buf[4] = 0; - s->ti_buf[5] = 0; - if (bdrv_get_type_hint(s->bd[target]) == BDRV_TYPE_CDROM) - s->ti_buf[6] = 8; // sector size 2048 - else - s->ti_buf[6] = 2; // sector size 512 - s->ti_buf[7] = 0; - s->ti_dir = 1; - s->ti_size = 8; - break; - case 0x28: - { - int64_t offset, len; - - if (bdrv_get_type_hint(s->bd[target]) == BDRV_TYPE_CDROM) { - offset = ((buf[3] << 24) | (buf[4] << 16) | (buf[5] << 8) | buf[6]) * 4; - len = ((buf[8] << 8) | buf[9]) * 4; - s->ti_size = len * 2048; - } else { - offset = (buf[3] << 24) | (buf[4] << 16) | (buf[5] << 8) | buf[6]; - len = (buf[8] << 8) | buf[9]; - s->ti_size = len * 512; - } - DPRINTF("Read (10) (offset %lld len %lld)\n", offset, len); - if (s->ti_size > TI_BUFSZ) { - DPRINTF("size too large %d\n", s->ti_size); - } - bdrv_read(s->bd[target], offset, s->ti_buf, len); - // XXX error handling - s->ti_dir = 1; - break; - } - case 0x2a: - { - int64_t offset, len; - - if (bdrv_get_type_hint(s->bd[target]) == BDRV_TYPE_CDROM) { - offset = ((buf[3] << 24) | (buf[4] << 16) | (buf[5] << 8) | buf[6]) * 4; - len = ((buf[8] << 8) | buf[9]) * 4; - s->ti_size = len * 2048; - } else { - offset = (buf[3] << 24) | (buf[4] << 16) | (buf[5] << 8) | buf[6]; - len = (buf[8] << 8) | buf[9]; - s->ti_size = len * 512; - } - DPRINTF("Write (10) (offset %lld len %lld)\n", offset, len); - if (s->ti_size > TI_BUFSZ) { - DPRINTF("size too large %d\n", s->ti_size); - } - s->dma_cb = esp_write_dma_cb; - s->offset = offset; - s->len = len; - s->target = target; - // XXX error handling - s->ti_dir = 0; - break; - } - case 0x43: - { - int start_track, format, msf, len; - - msf = buf[2] & 2; - format = buf[3] & 0xf; - start_track = buf[7]; - bdrv_get_geometry(s->bd[target], &nb_sectors); - DPRINTF("Read TOC (track %d format %d msf %d)\n", start_track, format, msf >> 1); - switch(format) { - case 0: - len = cdrom_read_toc(nb_sectors, buf, msf, start_track); - if (len < 0) - goto error_cmd; - s->ti_size = len; - break; - case 1: - /* multi session : only a single session defined */ - memset(buf, 0, 12); - buf[1] = 0x0a; - buf[2] = 0x01; - buf[3] = 0x01; - s->ti_size = 12; - break; - case 2: - len = cdrom_read_toc_raw(nb_sectors, buf, msf, start_track); - if (len < 0) - goto error_cmd; - s->ti_size = len; - break; - default: - error_cmd: - DPRINTF("Read TOC error\n"); - // XXX error handling - break; - } - s->ti_dir = 1; - break; + s->current_dev = s->scsi_dev[target]; + return dmalen; +} + +static void do_cmd(ESPState *s, uint8_t *buf) +{ + int32_t datalen; + int lun; + + DPRINTF("do_cmd: busid 0x%x\n", buf[0]); + lun = buf[0] & 7; + datalen = scsi_send_command(s->current_dev, 0, &buf[1], lun); + if (datalen == 0) { + s->ti_size = 0; + } else { + s->rregs[4] = STAT_IN | STAT_TC; + if (datalen > 0) { + s->rregs[4] |= STAT_DI; + s->ti_size = datalen; + } else { + s->rregs[4] |= STAT_DO; + s->ti_size = -datalen; } - default: - DPRINTF("Unknown SCSI command (%2.2x)\n", buf[1]); - break; } - s->rregs[4] = STAT_IN | STAT_TC | STAT_DI; s->rregs[5] = INTR_BS | INTR_FC; s->rregs[6] = SEQ_CD; s->espdmaregs[0] |= DMA_INTR; pic_set_irq(s->irq, 1); } -static void dma_write(ESPState *s, const uint8_t *buf, uint32_t len) +static void handle_satn(ESPState *s) +{ + uint8_t buf[32]; + int len; + + len = get_cmd(s, buf); + if (len) + do_cmd(s, buf); +} + +static void handle_satn_stop(ESPState *s) { - uint32_t dmaptr, dmalen; + s->cmdlen = get_cmd(s, s->cmdbuf); + if (s->cmdlen) { + DPRINTF("Set ATN & Stop: cmdlen %d\n", s->cmdlen); + s->do_cmd = 1; + s->espdmaregs[1] += s->cmdlen; + s->rregs[4] = STAT_IN | STAT_TC | STAT_CD; + s->rregs[5] = INTR_BS | INTR_FC; + s->rregs[6] = SEQ_CD; + s->espdmaregs[0] |= DMA_INTR; + pic_set_irq(s->irq, 1); + } +} - dmalen = s->wregs[0] | (s->wregs[1] << 8); - DPRINTF("Transfer status len %d\n", dmalen); +static void write_response(ESPState *s) +{ + uint32_t dmaptr; + + DPRINTF("Transfer status (sense=%d)\n", s->sense); + s->ti_buf[0] = s->sense; + s->ti_buf[1] = 0; if (s->dma) { dmaptr = iommu_translate(s->espdmaregs[1]); - DPRINTF("DMA Direction: %c\n", s->espdmaregs[0] & 0x100? 'w': 'r'); - cpu_physical_memory_write(dmaptr, buf, len); + DPRINTF("DMA Direction: %c\n", + s->espdmaregs[0] & DMA_WRITE_MEM ? 'w': 'r'); + cpu_physical_memory_write(dmaptr, s->ti_buf, 2); s->rregs[4] = STAT_IN | STAT_TC | STAT_ST; s->rregs[5] = INTR_BS | INTR_FC; s->rregs[6] = SEQ_CD; } else { - memcpy(s->ti_buf, buf, len); - s->ti_size = dmalen; + s->ti_size = 2; s->ti_rptr = 0; s->ti_wptr = 0; - s->rregs[7] = dmalen; + s->rregs[7] = 2; } s->espdmaregs[0] |= DMA_INTR; pic_set_irq(s->irq, 1); } -static const uint8_t okbuf[] = {0, 0}; +static void esp_command_complete(void *opaque, uint32_t tag, int sense) +{ + ESPState *s = (ESPState *)opaque; + + DPRINTF("SCSI Command complete\n"); + if (s->ti_size != 0) + DPRINTF("SCSI command completed unexpectedly\n"); + s->ti_size = 0; + if (sense) + DPRINTF("Command failed\n"); + s->sense = sense; + s->rregs[4] = STAT_IN | STAT_TC | STAT_ST; +} static void handle_ti(ESPState *s) { - uint32_t dmaptr, dmalen; + uint32_t dmaptr, dmalen, minlen, len, from, to; unsigned int i; + int to_device; + uint8_t buf[TARGET_PAGE_SIZE]; dmalen = s->wregs[0] | (s->wregs[1] << 8); - DPRINTF("Transfer Information len %d\n", dmalen); + if (dmalen==0) { + dmalen=0x10000; + } + + if (s->do_cmd) + minlen = (dmalen < 32) ? dmalen : 32; + else + minlen = (dmalen < s->ti_size) ? dmalen : s->ti_size; + DPRINTF("Transfer Information len %d\n", minlen); if (s->dma) { dmaptr = iommu_translate(s->espdmaregs[1]); - DPRINTF("DMA Direction: %c, addr 0x%8.8x\n", s->espdmaregs[0] & 0x100? 'w': 'r', dmaptr); - for (i = 0; i < s->ti_size; i++) { + /* Check if the transfer writes to to reads from the device. */ + to_device = (s->espdmaregs[0] & DMA_WRITE_MEM) == 0; + DPRINTF("DMA Direction: %c, addr 0x%8.8x %08x\n", + to_device ? 'r': 'w', dmaptr, s->ti_size); + from = s->espdmaregs[1]; + to = from + minlen; + for (i = 0; i < minlen; i += len, from += len) { dmaptr = iommu_translate(s->espdmaregs[1] + i); - if (s->ti_dir) - cpu_physical_memory_write(dmaptr, &s->ti_buf[i], 1); - else - cpu_physical_memory_read(dmaptr, &s->ti_buf[i], 1); - } - if (s->dma_cb) { - s->dma_cb(s, s->espdmaregs[1], dmalen); - s->dma_cb = NULL; + if ((from & TARGET_PAGE_MASK) != (to & TARGET_PAGE_MASK)) { + len = TARGET_PAGE_SIZE - (from & ~TARGET_PAGE_MASK); + } else { + len = to - from; + } + DPRINTF("DMA address p %08x v %08x len %08x, from %08x, to %08x\n", dmaptr, s->espdmaregs[1] + i, len, from, to); + s->ti_size -= len; + if (s->do_cmd) { + DPRINTF("command len %d + %d\n", s->cmdlen, len); + cpu_physical_memory_read(dmaptr, &s->cmdbuf[s->cmdlen], len); + s->ti_size = 0; + s->cmdlen = 0; + s->do_cmd = 0; + do_cmd(s, s->cmdbuf); + return; + } else { + if (to_device) { + cpu_physical_memory_read(dmaptr, buf, len); + scsi_write_data(s->current_dev, buf, len); + } else { + scsi_read_data(s->current_dev, buf, len); + cpu_physical_memory_write(dmaptr, buf, len); + } + } } - s->rregs[4] = STAT_IN | STAT_TC | STAT_ST; - s->rregs[5] = INTR_BS; + if (s->ti_size) { + s->rregs[4] = STAT_IN | STAT_TC | (to_device ? STAT_DO : STAT_DI); + } + s->rregs[5] = INTR_BS; s->rregs[6] = 0; + s->rregs[7] = 0; s->espdmaregs[0] |= DMA_INTR; - } else { - s->ti_size = dmalen; - s->ti_rptr = 0; - s->ti_wptr = 0; - s->rregs[7] = dmalen; - } + } else if (s->do_cmd) { + DPRINTF("command len %d\n", s->cmdlen); + s->ti_size = 0; + s->cmdlen = 0; + s->do_cmd = 0; + do_cmd(s, s->cmdbuf); + return; + } pic_set_irq(s->irq, 1); } @@ -484,9 +289,8 @@ static void esp_reset(void *opaque) s->ti_size = 0; s->ti_rptr = 0; s->ti_wptr = 0; - s->ti_dir = 0; s->dma = 0; - s->dma_cb = NULL; + s->do_cmd = 0; } static uint32_t esp_mem_readb(void *opaque, target_phys_addr_t addr) @@ -501,7 +305,12 @@ static uint32_t esp_mem_readb(void *opaq // FIFO if (s->ti_size > 0) { s->ti_size--; - s->rregs[saddr] = s->ti_buf[s->ti_rptr++]; + if ((s->rregs[4] & 6) == 0) { + /* Data in/out. */ + scsi_read_data(s->current_dev, &s->rregs[2], 0); + } else { + s->rregs[2] = s->ti_buf[s->ti_rptr++]; + } pic_set_irq(s->irq, 1); } if (s->ti_size == 0) { @@ -536,8 +345,17 @@ static void esp_mem_writeb(void *opaque, break; case 2: // FIFO - s->ti_size++; - s->ti_buf[s->ti_wptr++] = val & 0xff; + if (s->do_cmd) { + s->cmdbuf[s->cmdlen++] = val & 0xff; + } else if ((s->rregs[4] & 6) == 0) { + uint8_t buf; + buf = val & 0xff; + s->ti_size--; + scsi_write_data(s->current_dev, &buf, 0); + } else { + s->ti_size++; + s->ti_buf[s->ti_wptr++] = val & 0xff; + } break; case 3: s->rregs[saddr] = val; @@ -574,11 +392,11 @@ static void esp_mem_writeb(void *opaque, break; case 0x11: DPRINTF("Initiator Command Complete Sequence (%2.2x)\n", val); - dma_write(s, okbuf, 2); + write_response(s); break; case 0x12: DPRINTF("Message Accepted (%2.2x)\n", val); - dma_write(s, okbuf, 2); + write_response(s); s->rregs[5] = INTR_DC; s->rregs[6] = 0; break; @@ -586,11 +404,12 @@ static void esp_mem_writeb(void *opaque, DPRINTF("Set ATN (%2.2x)\n", val); break; case 0x42: + DPRINTF("Set ATN (%2.2x)\n", val); handle_satn(s); break; case 0x43: DPRINTF("Set ATN & stop (%2.2x)\n", val); - handle_satn(s); + handle_satn_stop(s); break; default: DPRINTF("Unhandled ESP command (%2.2x)\n", val); @@ -660,7 +479,7 @@ static void espdma_mem_writel(void *opaq val |= DMA_VER; break; case 1: - s->espdmaregs[0] = DMA_LOADED; + s->espdmaregs[0] |= DMA_LOADED; break; default: break; @@ -693,7 +512,6 @@ static void esp_save(QEMUFile *f, void * qemu_put_be32s(f, &s->ti_size); qemu_put_be32s(f, &s->ti_rptr); qemu_put_be32s(f, &s->ti_wptr); - qemu_put_be32s(f, &s->ti_dir); qemu_put_buffer(f, s->ti_buf, TI_BUFSZ); qemu_put_be32s(f, &s->dma); } @@ -714,7 +532,6 @@ static int esp_load(QEMUFile *f, void *o qemu_get_be32s(f, &s->ti_size); qemu_get_be32s(f, &s->ti_rptr); qemu_get_be32s(f, &s->ti_wptr); - qemu_get_be32s(f, &s->ti_dir); qemu_get_buffer(f, s->ti_buf, TI_BUFSZ); qemu_get_be32s(f, &s->dma); @@ -725,6 +542,7 @@ void esp_init(BlockDriverState **bd, int { ESPState *s; int esp_io_memory, espdma_io_memory; + int i; s = qemu_mallocz(sizeof(ESPState)); if (!s) @@ -743,5 +561,11 @@ void esp_init(BlockDriverState **bd, int register_savevm("esp", espaddr, 1, esp_save, esp_load, s); qemu_register_reset(esp_reset, s); + for (i = 0; i < MAX_DISKS; i++) { + if (bs_table[i]) { + s->scsi_dev[i] = + scsi_disk_init(bs_table[i], esp_command_complete, s); + } + } }