SCSI TCQ support.

git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@2139 c046a42c-6fe2-441c-8c8c-71466251a162

6 files changed, 916 insertions, 446 deletions

diff --git a/hw/esp.c b/hw/esp.c
index d67dbf3f3..991e5151f 100644
--- a/hw/esp.c
+++ b/hw/esp.c
@@ -64,8 +64,7 @@ struct ESPState {
     int do_cmd;
 
     uint32_t dma_left;
-    uint8_t async_buf[TARGET_PAGE_SIZE];
-    uint32_t async_ptr;
+    uint8_t *async_buf;
     uint32_t async_len;
 };
 
@@ -91,17 +90,16 @@ struct ESPState {
 
 static int get_cmd(ESPState *s, uint8_t *buf)
 {
-    uint32_t dmaptr, dmalen;
+    uint32_t dmalen;
     int target;
 
     dmalen = s->wregs[0] | (s->wregs[1] << 8);
     target = s->wregs[4] & 7;
     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] & DMA_WRITE_MEM ? 'w': 'r', dmaptr);
-	cpu_physical_memory_read(dmaptr, buf, dmalen);
+                s->espdmaregs[0] & DMA_WRITE_MEM ? 'w': 'r', s->espdmaregs[1]);
+        sparc_iommu_memory_read(s->espdmaregs[1], buf, dmalen);
     } else {
 	buf[0] = 0;
 	memcpy(&buf[1], s->ti_buf, dmalen);
@@ -112,6 +110,12 @@ static int get_cmd(ESPState *s, uint8_t *buf)
     s->ti_rptr = 0;
     s->ti_wptr = 0;
 
+    if (s->current_dev) {
+        /* Started a new command before the old one finished.  Cancel it.  */
+        scsi_cancel_io(s->current_dev, 0);
+        s->async_len = 0;
+    }
+
     if (target >= 4 || !s->scsi_dev[target]) {
         // No such drive
 	s->rregs[4] = STAT_IN;
@@ -137,12 +141,15 @@ static void do_cmd(ESPState *s, uint8_t *buf)
         s->ti_size = 0;
     } else {
         s->rregs[4] = STAT_IN | STAT_TC;
+        s->dma_left = 0;
         if (datalen > 0) {
             s->rregs[4] |= STAT_DI;
             s->ti_size = datalen;
+            scsi_read_data(s->current_dev, 0);
         } else {
             s->rregs[4] |= STAT_DO;
             s->ti_size = -datalen;
+            scsi_write_data(s->current_dev, 0);
         }
     }
     s->rregs[5] = INTR_BS | INTR_FC;
@@ -178,16 +185,13 @@ static void handle_satn_stop(ESPState *s)
 
 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] & DMA_WRITE_MEM ? 'w': 'r');
-	cpu_physical_memory_write(dmaptr, s->ti_buf, 2);
+        sparc_iommu_memory_write(s->espdmaregs[1], 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;
@@ -202,78 +206,89 @@ static void write_response(ESPState *s)
 
 }
 
+static void esp_dma_done(ESPState *s)
+{
+    s->rregs[4] |= STAT_IN | STAT_TC;
+    s->rregs[5] = INTR_BS;
+    s->rregs[6] = 0;
+    s->rregs[7] = 0;
+    s->espdmaregs[0] |= DMA_INTR;
+    pic_set_irq(s->irq, 1);
+}
+
 static void esp_do_dma(ESPState *s)
 {
-    uint32_t dmaptr, minlen, len, from, to;
+    uint32_t addr, len;
     int to_device;
+
     to_device = (s->espdmaregs[0] & DMA_WRITE_MEM) == 0;
-    from = s->espdmaregs[1];
-    minlen = s->dma_left;
-    to = from + minlen;
-    dmaptr = iommu_translate(s->espdmaregs[1]);
-    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], len, from, to);
+    addr = s->espdmaregs[1];
+    len = s->dma_left;
+    DPRINTF("DMA address %08x len %08x\n", addr, len);
     if (s->do_cmd) {
         s->espdmaregs[1] += len;
         s->ti_size -= len;
         DPRINTF("command len %d + %d\n", s->cmdlen, len);
-        cpu_physical_memory_read(dmaptr, &s->cmdbuf[s->cmdlen], len);
+        sparc_iommu_memory_read(addr, &s->cmdbuf[s->cmdlen], len);
         s->ti_size = 0;
         s->cmdlen = 0;
         s->do_cmd = 0;
         do_cmd(s, s->cmdbuf);
         return;
+    }
+    if (s->async_len == 0) {
+        /* Defer until data is available.  */
+        return;
+    }
+    if (len > s->async_len) {
+        len = s->async_len;
+    }
+    if (to_device) {
+        sparc_iommu_memory_read(addr, s->async_buf, len);
     } else {
-        s->async_len = len;
-        s->dma_left -= len;
+        sparc_iommu_memory_write(addr, s->async_buf, len);
+    }
+    s->ti_size -= len;
+    s->dma_left -= len;
+    s->async_buf += len;
+    s->async_len -= len;
+    s->espdmaregs[1] += len;
+    if (s->async_len == 0) {
         if (to_device) {
-            s->async_ptr = -1;
-            cpu_physical_memory_read(dmaptr, s->async_buf, len);
-            scsi_write_data(s->current_dev, s->async_buf, len);
+            scsi_write_data(s->current_dev, 0);
         } else {
-            s->async_ptr = dmaptr;
-            scsi_read_data(s->current_dev, s->async_buf, len);
+            scsi_read_data(s->current_dev, 0);
         }
     }
+    if (s->dma_left == 0) {
+        esp_dma_done(s);
+    }
 }
 
-static void esp_command_complete(void *opaque, uint32_t reason, int sense)
+static void esp_command_complete(void *opaque, int reason, uint32_t tag,
+                                 uint32_t arg)
 {
     ESPState *s = (ESPState *)opaque;
 
-    s->ti_size -= s->async_len;
-    s->espdmaregs[1] += s->async_len;
-    if (s->async_ptr != (uint32_t)-1) {
-        cpu_physical_memory_write(s->async_ptr, s->async_buf, s->async_len);
-    }
     if (reason == SCSI_REASON_DONE) {
         DPRINTF("SCSI Command complete\n");
         if (s->ti_size != 0)
             DPRINTF("SCSI command completed unexpectedly\n");
         s->ti_size = 0;
-        if (sense)
+        s->dma_left = 0;
+        s->async_len = 0;
+        if (arg)
             DPRINTF("Command failed\n");
-        s->sense = sense;
+        s->sense = arg;
+        s->rregs[4] = STAT_ST;
+        esp_dma_done(s);
+        s->current_dev = NULL;
     } else {
         DPRINTF("transfer %d/%d\n", s->dma_left, s->ti_size);
-    }
-    if (s->dma_left) {
-        esp_do_dma(s);
-    } else {
-        if (s->ti_size) {
-            s->rregs[4] |= STAT_IN | STAT_TC;
-        } else {
-            s->rregs[4] = STAT_IN | STAT_TC | STAT_ST;
-        }
-        s->rregs[5] = INTR_BS;
-	s->rregs[6] = 0;
-	s->rregs[7] = 0;
-	s->espdmaregs[0] |= DMA_INTR;
-        pic_set_irq(s->irq, 1);
+        s->async_len = arg;
+        s->async_buf = scsi_get_buf(s->current_dev, 0);
+        if (s->dma_left)
+            esp_do_dma(s);
     }
 }
 
@@ -333,7 +348,8 @@ static uint32_t esp_mem_readb(void *opaque, target_phys_addr_t addr)
 	    s->ti_size--;
             if ((s->rregs[4] & 6) == 0) {
                 /* Data in/out.  */
-                scsi_read_data(s->current_dev, &s->rregs[2], 0);
+                fprintf(stderr, "esp: PIO data read not implemented\n");
+                s->rregs[2] = 0;
             } else {
                 s->rregs[2] = s->ti_buf[s->ti_rptr++];
             }
@@ -378,7 +394,7 @@ static void esp_mem_writeb(void *opaque, target_phys_addr_t addr, uint32_t val)
             uint8_t buf;
             buf = val & 0xff;
             s->ti_size--;
-            scsi_write_data(s->current_dev, &buf, 0);
+            fprintf(stderr, "esp: PIO data write not implemented\n");
         } else {
             s->ti_size++;
             s->ti_buf[s->ti_wptr++] = val & 0xff;
@@ -590,8 +606,9 @@ void esp_init(BlockDriverState **bd, int irq, uint32_t espaddr, uint32_t espdadd
     qemu_register_reset(esp_reset, s);
     for (i = 0; i < MAX_DISKS; i++) {
         if (bs_table[i]) {
+            /* Command queueing is not implemented.  */
             s->scsi_dev[i] =
-                scsi_disk_init(bs_table[i], esp_command_complete, s);
+                scsi_disk_init(bs_table[i], 0, esp_command_complete, s);
         }
     }
 }
diff --git a/hw/iommu.c b/hw/iommu.c
index e7d96c81c..83001bd7f 100644
--- a/hw/iommu.c
+++ b/hw/iommu.c
@@ -186,21 +186,62 @@ static CPUWriteMemoryFunc *iommu_mem_write[3] = {
     iommu_mem_writew,
 };
 
-uint32_t iommu_translate_local(void *opaque, uint32_t addr)
+static uint32_t iommu_page_get_flags(IOMMUState *s, uint32_t addr)
 {
-    IOMMUState *s = opaque;
-    uint32_t iopte, pa, tmppte;
+    uint32_t iopte;
 
     iopte = s->regs[1] << 4;
     addr &= ~s->iostart;
     iopte += (addr >> (PAGE_SHIFT - 2)) & ~3;
-    pa = ldl_phys(iopte);
+    return ldl_phys(iopte);
+}
+
+static uint32_t iommu_translate_pa(IOMMUState *s, uint32_t addr, uint32_t pa)
+{
+    uint32_t tmppte;
+
     tmppte = pa;
     pa = ((pa & IOPTE_PAGE) << 4) + (addr & PAGE_MASK);
-    DPRINTF("xlate dva %x => pa %x (iopte[%x] = %x)\n", addr, pa, iopte, tmppte);
+    DPRINTF("xlate dva %x => pa %x (iopte = %x)\n", addr, pa, tmppte);
     return pa;
 }
 
+uint32_t iommu_translate_local(void *opaque, uint32_t addr)
+{
+    uint32_t flags;
+    flags = iommu_page_get_flags(opaque, addr);
+    return iommu_translate_pa(opaque, addr, flags);
+}
+
+void sparc_iommu_memory_rw_local(void *opaque, target_phys_addr_t addr,
+                                 uint8_t *buf, int len, int is_write)
+{
+    int l, flags;
+    target_ulong page, phys_addr;
+    void * p;
+
+    while (len > 0) {
+        page = addr & TARGET_PAGE_MASK;
+        l = (page + TARGET_PAGE_SIZE) - addr;
+        if (l > len)
+            l = len;
+        flags = iommu_page_get_flags(opaque, page);
+        if (!(flags & IOPTE_VALID))
+            return;
+        phys_addr = iommu_translate_pa(opaque, addr, flags);
+        if (is_write) {
+            if (!(flags & IOPTE_WRITE))
+                return;
+            cpu_physical_memory_write(phys_addr, buf, len);
+        } else {
+            cpu_physical_memory_read(phys_addr, buf, len);
+        }
+        len -= l;
+        buf += l;
+        addr += l;
+    }
+}
+
 static void iommu_save(QEMUFile *f, void *opaque)
 {
     IOMMUState *s = opaque;
diff --git a/hw/lsi53c895a.c b/hw/lsi53c895a.c
index 8f5672534..41c1ff22d 100644
--- a/hw/lsi53c895a.c
+++ b/hw/lsi53c895a.c
@@ -19,11 +19,11 @@
 #define DPRINTF(fmt, args...) \
 do { printf("lsi_scsi: " fmt , ##args); } while (0)
 #define BADF(fmt, args...) \
-do { fprintf(stderr, "lsi_scsi: " fmt , ##args); exit(1);} while (0)
+do { fprintf(stderr, "lsi_scsi: error: " fmt , ##args); exit(1);} while (0)
 #else
 #define DPRINTF(fmt, args...) do {} while(0)
 #define BADF(fmt, args...) \
-do { fprintf(stderr, "lsi_scsi: " fmt , ##args); } while (0)
+do { fprintf(stderr, "lsi_scsi: error: " fmt , ##args);} while (0)
 #endif
 
 #define LSI_SCNTL0_TRG    0x01
@@ -152,26 +152,46 @@ do { fprintf(stderr, "lsi_scsi: " fmt , ##args); } while (0)
 /* The HBA is ID 7, so for simplicitly limit to 7 devices.  */
 #define LSI_MAX_DEVS      7
 
-/* Size of internal DMA buffer for async IO requests.  */
-#define LSI_DMA_BLOCK_SIZE 0x10000
+/* Maximum length of MSG IN data.  */
+#define LSI_MAX_MSGIN_LEN 8
+
+/* Flag set if this is a tagged command.  */
+#define LSI_TAG_VALID     (1 << 16)
+
+typedef struct {
+    uint32_t tag;
+    uint32_t pending;
+    int out;
+} lsi_queue;
 
 typedef struct {
     PCIDevice pci_dev;
     int mmio_io_addr;
     int ram_io_addr;
     uint32_t script_ram_base;
-    uint32_t data_len;
 
     int carry; /* ??? Should this be an a visible register somewhere?  */
     int sense;
-    uint8_t msg;
+    /* Action to take at the end of a MSG IN phase.
+       0 = COMMAND, 1 = disconect, 2 = DATA OUT, 3 = DATA IN.  */
+    int msg_action;
+    int msg_len;
+    uint8_t msg[LSI_MAX_MSGIN_LEN];
     /* 0 if SCRIPTS are running or stopped.
      * 1 if a Wait Reselect instruction has been issued.
-     * 2 if a DMA operation is in progress.  */
+     * 2 if processing DMA from lsi_execute_script.
+     * 3 if a DMA operation is in progress.  */
     int waiting;
     SCSIDevice *scsi_dev[LSI_MAX_DEVS];
     SCSIDevice *current_dev;
     int current_lun;
+    /* The tag is a combination of the device ID and the SCSI tag.  */
+    uint32_t current_tag;
+    uint32_t current_dma_len;
+    uint8_t *dma_buf;
+    lsi_queue *queue;
+    int queue_len;
+    int active_commands;
 
     uint32_t dsa;
     uint32_t temp;
@@ -208,10 +228,12 @@ typedef struct {
     uint8_t sxfer;
     uint8_t socl;
     uint8_t sdid;
+    uint8_t ssid;
     uint8_t sfbr;
     uint8_t stest1;
     uint8_t stest2;
     uint8_t stest3;
+    uint8_t sidl;
     uint8_t stime0;
     uint8_t respid0;
     uint8_t respid1;
@@ -231,7 +253,6 @@ typedef struct {
     uint32_t csbc;
     uint32_t scratch[13]; /* SCRATCHA-SCRATCHR */
 
-    uint8_t dma_buf[LSI_DMA_BLOCK_SIZE];
     /* Script ram is stored as 32-bit words in host byteorder.  */
     uint32_t script_ram[2048];
 } LSIState;
@@ -280,6 +301,7 @@ static void lsi_soft_reset(LSIState *s)
     s->stest1 = 0;
     s->stest2 = 0;
     s->stest3 = 0;
+    s->sidl = 0;
     s->stime0 = 0;
     s->respid0 = 0x80;
     s->respid1 = 0;
@@ -409,68 +431,194 @@ static void lsi_bad_phase(LSIState *s, int out, int new_phase)
     lsi_set_phase(s, new_phase);
 }
 
+
+/* Resume SCRIPTS execution after a DMA operation.  */
+static void lsi_resume_script(LSIState *s)
+{
+    if (s->waiting != 2) {
+        s->waiting = 0;
+        lsi_execute_script(s);
+    } else {
+        s->waiting = 0;
+    }
+}
+
 /* Initiate a SCSI layer data transfer.  */
 static void lsi_do_dma(LSIState *s, int out)
 {
     uint32_t count;
+    uint32_t addr;
 
-    count = s->dbc;
-    if (count > LSI_DMA_BLOCK_SIZE)
-        count = LSI_DMA_BLOCK_SIZE;
-    DPRINTF("DMA addr=0x%08x len=%d avail=%d\n",
-            addr, count, s->data_len);
-    /* ??? Too long transfers are truncated. Don't know if this is the
-       correct behavior.  */
-    if (count > s->data_len) {
-        /* If the DMA length is greater than the device data length then
-           a phase mismatch will occur.  */
-        count = s->data_len;
-        s->dbc = count;
-        lsi_bad_phase(s, out, PHASE_ST);
+    if (!s->current_dma_len) {
+        /* Wait until data is available.  */
+        DPRINTF("DMA no data available\n");
+        return;
     }
 
+    count = s->dbc;
+    if (count > s->current_dma_len)
+        count = s->current_dma_len;
+    DPRINTF("DMA addr=0x%08x len=%d\n", s->dnad, count);
+
+    addr = s->dnad;
     s->csbc += count;
+    s->dnad += count;
+    s->dbc -= count;
+
+    if (s->dma_buf == NULL) {
+        s->dma_buf = scsi_get_buf(s->current_dev, s->current_tag);
+    }
 
     /* ??? Set SFBR to first data byte.  */
-    if ((s->sstat1 & PHASE_MASK) == PHASE_DO) {
-        cpu_physical_memory_read(s->dnad, s->dma_buf, count);
-        scsi_write_data(s->current_dev, s->dma_buf, count);
+    if (out) {
+        cpu_physical_memory_read(addr, s->dma_buf, count);
+    } else {
+        cpu_physical_memory_write(addr, s->dma_buf, count);
+    }
+    s->current_dma_len -= count;
+    if (s->current_dma_len == 0) {
+        s->dma_buf = NULL;
+        if (out) {
+            /* Write the data.  */
+            scsi_write_data(s->current_dev, s->current_tag);
+        } else {
+            /* Request any remaining data.  */
+            scsi_read_data(s->current_dev, s->current_tag);
+        }
+    } else {
+        s->dma_buf += count;
+        lsi_resume_script(s);
+    }
+}
+
+
+/* Add a command to the queue.  */
+static void lsi_queue_command(LSIState *s)
+{
+    lsi_queue *p;
+
+    DPRINTF("Queueing tag=0x%x\n", s->current_tag);
+    if (s->queue_len == s->active_commands) {
+        s->queue_len++;
+        s->queue = realloc(s->queue, s->queue_len * sizeof(lsi_queue));
+    }
+    p = &s->queue[s->active_commands++];
+    p->tag = s->current_tag;
+    p->pending = 0;
+    p->out = (s->sstat1 & PHASE_MASK) == PHASE_DO;
+}
+
+/* Queue a byte for a MSG IN phase.  */
+static void lsi_add_msg_byte(LSIState *s, uint8_t data)
+{
+    if (s->msg_len >= LSI_MAX_MSGIN_LEN) {
+        BADF("MSG IN data too long\n");
     } else {
-        scsi_read_data(s->current_dev, s->dma_buf, count);
+        DPRINTF("MSG IN 0x%02x\n", data);
+        s->msg[s->msg_len++] = data;
     }
-    /* If the DMA did not complete then suspend execution.  */
-    if (s->dbc)
-        s->waiting = 2;
+}
+
+/* Perform reselection to continue a command.  */
+static void lsi_reselect(LSIState *s, uint32_t tag)
+{
+    lsi_queue *p;
+    int n;
+    int id;
+
+    p = NULL;
+    for (n = 0; n < s->active_commands; n++) {
+        p = &s->queue[n];
+        if (p->tag == tag)
+            break;
+    }
+    if (n == s->active_commands) {
+        BADF("Reselected non-existant command tag=0x%x\n", tag);
+        return;
+    }
+    id = (tag >> 8) & 0xf;
+    s->ssid = id | 0x80;
+    DPRINTF("Reselected target %d\n", id);
+    s->current_dev = s->scsi_dev[id];
+    s->current_tag = tag;
+    s->scntl1 |= LSI_SCNTL1_CON;
+    lsi_set_phase(s, PHASE_MI);
+    s->msg_action = p->out ? 2 : 3;
+    s->current_dma_len = p->pending;
+    s->dma_buf = NULL;
+    lsi_add_msg_byte(s, 0x80);
+    if (s->current_tag & LSI_TAG_VALID) {
+        lsi_add_msg_byte(s, 0x20);
+        lsi_add_msg_byte(s, tag & 0xff);
+    }
+
+    s->active_commands--;
+    if (n != s->active_commands) {
+        s->queue[n] = s->queue[s->active_commands];
+    }
+}
+
+/* Record that data is available for a queued command.  Returns zero if
+   the device was reselected, nonzero if the IO is deferred.  */
+static int lsi_queue_tag(LSIState *s, uint32_t tag, uint32_t arg)
+{
+    lsi_queue *p;
+    int i;
+    for (i = 0; i < s->active_commands; i++) {
+        p = &s->queue[i];
+        if (p->tag == tag) {
+            if (p->pending) {
+                BADF("Multiple IO pending for tag %d\n", tag);
+            }
+            p->pending = arg;
+            if (s->waiting == 1) {
+                /* Reselect device.  */
+                lsi_reselect(s, tag);
+                return 0;
+            } else {
+               DPRINTF("Queueing IO tag=0x%x\n", tag);
+                p->pending = arg;
+                return 1;
+            }
+        }
+    }
+    BADF("IO with unknown tag %d\n", tag);
+    return 1;
 }
 
 /* Callback to indicate that the SCSI layer has completed a transfer.  */
-static void lsi_command_complete(void *opaque, uint32_t reason, int sense)
+static void lsi_command_complete(void *opaque, int reason, uint32_t tag,
+                                 uint32_t arg)
 {
     LSIState *s = (LSIState *)opaque;
-    uint32_t count;
     int out;
 
-    out = ((s->sstat1 & PHASE_MASK) == PHASE_DO);
-    count = s->dbc;
-    if (count > LSI_DMA_BLOCK_SIZE)
-        count = LSI_DMA_BLOCK_SIZE;
-    if (!out)
-        cpu_physical_memory_write(s->dnad, s->dma_buf, count);
-    s->dnad += count;
-    s->dbc -= count;
-
+    out = (s->sstat1 & PHASE_MASK) == PHASE_DO;
     if (reason == SCSI_REASON_DONE) {
-        DPRINTF("Command complete sense=%d\n", sense);
-        s->sense = sense;
-        lsi_set_phase(s, PHASE_ST);
+        DPRINTF("Command complete sense=%d\n", (int)arg);
+        s->sense = arg;
+        if (s->waiting && s->dbc != 0) {
+            /* Raise phase mismatch for short transfers.  */
+            lsi_bad_phase(s, out, PHASE_ST);
+        } else {
+            lsi_set_phase(s, PHASE_ST);
+        }
+        lsi_resume_script(s);
+        return;
     }
 
-    if (s->dbc) {
+    if (s->waiting == 1 || tag != s->current_tag) {
+        if (lsi_queue_tag(s, tag, arg))
+            return;
+    }
+    DPRINTF("Data ready tag=0x%x len=%d\n", tag, arg);
+    s->current_dma_len = arg;
+    if (!s->waiting)
+        return;
+    if (s->waiting == 1 || s->dbc == 0) {
+        lsi_resume_script(s);
+    } else {
         lsi_do_dma(s, out);
-    } else if (s->waiting == 2) {
-        /* Restart SCRIPTS execution.  */
-        s->waiting = 0;
-        lsi_execute_script(s);
     }
 }
 
@@ -484,27 +632,37 @@ static void lsi_do_command(LSIState *s)
         s->dbc = 16;
     cpu_physical_memory_read(s->dnad, buf, s->dbc);
     s->sfbr = buf[0];
-    n = scsi_send_command(s->current_dev, 0, buf, s->current_lun);
+    n = scsi_send_command(s->current_dev, s->current_tag, buf, s->current_lun);
     if (n > 0) {
-        s->data_len = n;
         lsi_set_phase(s, PHASE_DI);
+        scsi_read_data(s->current_dev, s->current_tag);
     } else if (n < 0) {
-        s->data_len = -n;
         lsi_set_phase(s, PHASE_DO);
+        scsi_write_data(s->current_dev, s->current_tag);
+    }
+    if (n && s->current_dma_len == 0) {
+        /* Command did not complete immediately so disconnect.  */
+        lsi_add_msg_byte(s, 2); /* SAVE DATA POINTER */
+        lsi_add_msg_byte(s, 4); /* DISCONNECT */
+        lsi_set_phase(s, PHASE_MI);
+        s->msg_action = 1;
+        lsi_queue_command(s);
     }
 }
 
 static void lsi_do_status(LSIState *s)
 {
+    uint8_t sense;
     DPRINTF("Get status len=%d sense=%d\n", s->dbc, s->sense);
     if (s->dbc != 1)
         BADF("Bad Status move\n");
     s->dbc = 1;
-    s->msg = s->sense;
-    cpu_physical_memory_write(s->dnad, &s->msg, 1);
-    s->sfbr = s->msg;
+    sense = s->sense;
+    s->sfbr = sense;
+    cpu_physical_memory_write(s->dnad, &sense, 1);
     lsi_set_phase(s, PHASE_MI);
-    s->msg = 0; /* COMMAND COMPLETE */
+    s->msg_action = 1;
+    lsi_add_msg_byte(s, 0); /* COMMAND COMPLETE */
 }
 
 static void lsi_disconnect(LSIState *s)
@@ -515,55 +673,114 @@ static void lsi_disconnect(LSIState *s)
 
 static void lsi_do_msgin(LSIState *s)
 {
-    DPRINTF("Message in len=%d\n", s->dbc);
-    s->dbc = 1;
-    s->sfbr = s->msg;
-    cpu_physical_memory_write(s->dnad, &s->msg, 1);
-    if (s->msg == 0) {
-        lsi_disconnect(s);
+    int len;
+    DPRINTF("Message in len=%d/%d\n", s->dbc, s->msg_len);
+    s->sfbr = s->msg[0];
+    len = s->msg_len;
+    if (len > s->dbc)
+        len = s->dbc;
+    cpu_physical_memory_write(s->dnad, s->msg, len);
+    /* Linux drivers rely on the last byte being in the SIDL.  */
+    s->sidl = s->msg[len - 1];
+    s->msg_len -= len;
+    if (s->msg_len) {
+        memmove(s->msg, s->msg + len, s->msg_len);
     } else {
         /* ??? Check if ATN (not yet implemented) is asserted and maybe
            switch to PHASE_MO.  */
-        lsi_set_phase(s, PHASE_CMD);
+        switch (s->msg_action) {
+        case 0:
+            lsi_set_phase(s, PHASE_CMD);
+            break;
+        case 1:
+            lsi_disconnect(s);
+            break;
+        case 2:
+            lsi_set_phase(s, PHASE_DO);
+            break;
+        case 3:
+            lsi_set_phase(s, PHASE_DI);
+            break;
+        default:
+            abort();
+        }
     }
 }
 
+/* Read the next byte during a MSGOUT phase.  */
+static uint8_t lsi_get_msgbyte(LSIState *s)
+{
+    uint8_t data;
+    cpu_physical_memory_read(s->dnad, &data, 1);
+    s->dnad++;
+    s->dbc--;
+    return data;
+}
+
 static void lsi_do_msgout(LSIState *s)
 {
     uint8_t msg;
+    int len;
 
     DPRINTF("MSG out len=%d\n", s->dbc);
-    if (s->dbc != 1) {
-        /* Multibyte messages not implemented.  */
-        s->msg = 7; /* MESSAGE REJECT */
-        //s->dbc = 1;
-        //lsi_bad_phase(s, 1, PHASE_MI);
-        lsi_set_phase(s, PHASE_MI);
-        return;
-    }
-    cpu_physical_memory_read(s->dnad, &msg, 1);
-    s->sfbr = msg;
-    s->dnad++;
-
-    switch (msg) {
-    case 0x00:
-        DPRINTF("Got Disconnect\n");
-        lsi_disconnect(s);
-        return;
-    case 0x08:
-        DPRINTF("Got No Operation\n");
-        lsi_set_phase(s, PHASE_CMD);
-        return;
-    }
-    if ((msg & 0x80) == 0) {
-        DPRINTF("Unimplemented message 0x%d\n", msg);
-        s->msg = 7; /* MESSAGE REJECT */
-        lsi_bad_phase(s, 1, PHASE_MI);
-        return;
+    while (s->dbc) {
+        msg = lsi_get_msgbyte(s);
+        s->sfbr = msg;
+
+        switch (msg) {
+        case 0x00:
+            DPRINTF("MSG: Disconnect\n");
+            lsi_disconnect(s);
+            break;
+        case 0x08:
+            DPRINTF("MSG: No Operation\n");
+            lsi_set_phase(s, PHASE_CMD);
+            break;
+        case 0x01:
+            len = lsi_get_msgbyte(s);
+            msg = lsi_get_msgbyte(s);
+            DPRINTF("Extended message 0x%x (len %d)\n", msg, len);
+            switch (msg) {
+            case 1:
+                DPRINTF("SDTR (ignored)\n");
+                s->dbc -= 2;
+                break;
+            case 3:
+                DPRINTF("WDTR (ignored)\n");
+                s->dbc -= 1;
+                break;
+            default:
+                goto bad;
+            }
+            break;
+        case 0x20: /* SIMPLE queue */
+            s->current_tag |= lsi_get_msgbyte(s) | LSI_TAG_VALID;
+            DPRINTF("SIMPLE queue tag=0x%x\n", s->current_tag & 0xff);
+            break;
+        case 0x21: /* HEAD of queue */
+            BADF("HEAD queue not implemented\n");
+            s->current_tag |= lsi_get_msgbyte(s) | LSI_TAG_VALID;
+            break;
+        case 0x22: /* ORDERED queue */
+            BADF("ORDERED queue not implemented\n");
+            s->current_tag |= lsi_get_msgbyte(s) | LSI_TAG_VALID;
+            break;
+        default:
+            if ((msg & 0x80) == 0) {
+                goto bad;
+            }
+            s->current_lun = msg & 7;
+            DPRINTF("Select LUN %d\n", s->current_lun);
+            lsi_set_phase(s, PHASE_CMD);
+            break;
+        }
     }
-    s->current_lun = msg & 7;
-    DPRINTF("Select LUN %d\n", s->current_lun);
-    lsi_set_phase(s, PHASE_CMD);
+    return;
+bad:
+    BADF("Unimplemented message 0x%02x\n", msg);
+    lsi_set_phase(s, PHASE_MI);
+    lsi_add_msg_byte(s, 7); /* MESSAGE REJECT */
+    s->msg_action = 0;
 }
 
 /* Sign extend a 24-bit value.  */
@@ -588,6 +805,23 @@ static void lsi_memcpy(LSIState *s, uint32_t dest, uint32_t src, int count)
     }
 }
 
+static void lsi_wait_reselect(LSIState *s)
+{
+    int i;
+    DPRINTF("Wait Reselect\n");
+    if (s->current_dma_len)
+        BADF("Reselect with pending DMA\n");
+    for (i = 0; i < s->active_commands; i++) {
+        if (s->queue[i].pending) {
+            lsi_reselect(s, s->queue[i].tag);
+            break;
+        }
+    }
+    if (s->current_dma_len == 0) {
+        s->waiting = 1;
+    }
+}
+
 static void lsi_execute_script(LSIState *s)
 {
     uint32_t insn;
@@ -632,10 +866,16 @@ again:
         s->dnad = addr;
         switch (s->sstat1 & 0x7) {
         case PHASE_DO:
+            s->waiting = 2;
             lsi_do_dma(s, 1);
+            if (s->waiting)
+                s->waiting = 3;
             break;
         case PHASE_DI:
+            s->waiting = 2;
             lsi_do_dma(s, 0);
+            if (s->waiting)
+                s->waiting = 3;
             break;
         case PHASE_CMD:
             lsi_do_command(s);
@@ -679,9 +919,13 @@ again:
             s->dnad = addr;
             switch (opcode) {
             case 0: /* Select */
+                s->sdid = id;
+                if (s->current_dma_len && (s->ssid & 0xf) == id) {
+                    DPRINTF("Already reselected by target %d\n", id);
+                    break;
+                }
                 s->sstat0 |= LSI_SSTAT0_WOA;
                 s->scntl1 &= ~LSI_SCNTL1_IARB;
-                s->sdid = id;
                 if (id >= LSI_MAX_DEVS || !s->scsi_dev[id]) {
                     DPRINTF("Selected absent target %d\n", id);
                     lsi_script_scsi_interrupt(s, 0, LSI_SIST1_STO);
@@ -694,6 +938,7 @@ again:
                    it only applies in low-level mode (unimplemented).
                 lsi_script_scsi_interrupt(s, LSI_SIST0_CMP, 0); */
                 s->current_dev = s->scsi_dev[id];
+                s->current_tag = id << 8;
                 s->scntl1 |= LSI_SCNTL1_CON;
                 if (insn & (1 << 3)) {
                     s->socl |= LSI_SOCL_ATN;
@@ -705,8 +950,7 @@ again:
                 s->scntl1 &= ~LSI_SCNTL1_CON;
                 break;
             case 2: /* Wait Reselect */
-                DPRINTF("Wait Reselect\n");
-                s->waiting = 1;
+                lsi_wait_reselect(s);
                 break;
             case 3: /* Set */
                 DPRINTF("Set%s%s%s%s\n",
@@ -755,9 +999,9 @@ again:
             data8 = (insn >> 8) & 0xff;
             opcode = (insn >> 27) & 7;
             operator = (insn >> 24) & 7;
-            DPRINTF("%s reg 0x%x %s data8 %d%s\n",
+            DPRINTF("%s reg 0x%x %s data8=0x%02x sfbr=0x%02x%s\n",
                     opcode_names[opcode - 5], reg,
-                    operator_names[operator], data8,
+                    operator_names[operator], data8, s->sfbr,
                     (insn & (1 << 23)) ? " SFBR" : "");
             op0 = op1 = 0;
             switch (opcode) {
@@ -923,8 +1167,9 @@ again:
             n = (insn & 7);
             reg = (insn >> 16) & 0xff;
             if (insn & (1 << 24)) {
-                DPRINTF("Load reg 0x%x size %d addr 0x%08x\n", reg, n, addr);
                 cpu_physical_memory_read(addr, data, n);
+                DPRINTF("Load reg 0x%x size %d addr 0x%08x = %08x\n", reg, n,
+                        addr, *(int *)data);
                 for (i = 0; i < n; i++) {
                     lsi_reg_writeb(s, reg + i, data[i]);
                 }
@@ -977,6 +1222,8 @@ static uint8_t lsi_reg_readb(LSIState *s, int offset)
         return s->sdid;
     case 0x07: /* GPREG0 */
         return 0x7f;
+    case 0xa: /* SSID */
+        return s->ssid;
     case 0xb: /* SBCL */
         /* ??? This is not correct. However it's (hopefully) only
            used for diagnostics, so should be ok.  */
@@ -1065,13 +1312,22 @@ static uint8_t lsi_reg_readb(LSIState *s, int offset)
         return s->stest2;
     case 0x4f: /* STEST3 */
         return s->stest3;
+    case 0x50: /* SIDL */
+        /* This is needed by the linux drivers.  We currently only update it
+           during the MSG IN phase.  */
+        return s->sidl;
     case 0x52: /* STEST4 */
         return 0xe0;
     case 0x56: /* CCNTL0 */
         return s->ccntl0;
     case 0x57: /* CCNTL1 */
         return s->ccntl1;
-    case 0x58: case 0x59: /* SBDL */
+    case 0x58: /* SBDL */
+        /* Some drivers peek at the data bus during the MSG IN phase.  */
+        if ((s->sstat1 & PHASE_MASK) == PHASE_MI)
+            return s->msg[0];
+        return 0;
+    case 0x59: /* SBDL high */
         return 0;
     CASE_GET_REG32(mmrs, 0xa0)
     CASE_GET_REG32(mmws, 0xa4)
@@ -1143,8 +1399,18 @@ static void lsi_reg_writeb(LSIState *s, int offset, uint8_t val)
     case 0x05: /* SXFER */
         s->sxfer = val;
         break;
+    case 0x06: /* SDID */
+        if ((val & 0xf) != (s->ssid & 0xf))
+            BADF("Destination ID does not match SSID\n");
+        s->sdid = val & 0xf;
+        break;
     case 0x07: /* GPREG0 */
         break;
+    case 0x08: /* SFBR */
+        /* The CPU is not allowed to write to this register.  However the
+           SCRIPTS register move instructions are.  */
+        s->sfbr = val;
+        break;
     case 0x0c: case 0x0d: case 0x0e: case 0x0f:
         /* Linux writes to these readonly registers on startup.  */
         return;
@@ -1555,7 +1821,7 @@ void lsi_scsi_attach(void *opaque, BlockDriverState *bd, int id)
         scsi_disk_destroy(s->scsi_dev[id]);
     }
     DPRINTF("Attaching block device %d\n", id);
-    s->scsi_dev[id] = scsi_disk_init(bd, lsi_command_complete, s);
+    s->scsi_dev[id] = scsi_disk_init(bd, 1, lsi_command_complete, s);
 }
 
 void *lsi_scsi_init(PCIBus *bus, int devfn)
@@ -1587,6 +1853,9 @@ void *lsi_scsi_init(PCIBus *bus, int devfn)
                            PCI_ADDRESS_SPACE_MEM, lsi_mmio_mapfunc);
     pci_register_io_region((struct PCIDevice *)s, 2, 0x2000,
                            PCI_ADDRESS_SPACE_MEM, lsi_ram_mapfunc);
+    s->queue = qemu_malloc(sizeof(lsi_queue));
+    s->queue_len = 1;
+    s->active_commands = 0;
 
     lsi_soft_reset(s);
 
diff --git a/hw/scsi-disk.c b/hw/scsi-disk.c
index 213e8d293..c6280fd55 100644
--- a/hw/scsi-disk.c
+++ b/hw/scsi-disk.c
@@ -7,6 +7,10 @@
  * Written by Paul Brook
  *
  * This code is licenced under the LGPL.
+ *
+ * Note that this file only handles the SCSI architecture model and device
+ * commands.  Emultion of interface/link layer protocols is handled by
+ * the host adapter emulation.
  */
 
 //#define DEBUG_SCSI
@@ -28,231 +32,241 @@ do { fprintf(stderr, "scsi-disk: " fmt , ##args); } while (0)
 #define SENSE_HARDWARE_ERROR  4
 #define SENSE_ILLEGAL_REQUEST 5
 
-struct SCSIDevice
-{
-    int command;
+#define SCSI_DMA_BUF_SIZE    65536
+
+typedef struct SCSIRequest {
+    SCSIDevice *dev;
     uint32_t tag;
-    BlockDriverState *bdrv;
-    /* The qemu block layer uses a fixed 512 byte sector size.
-       This is the number of 512 byte blocks in a single scsi sector.  */
-    int cluster_size;
-    /* When transfering data buf_pos and buf_len contain a partially
-       transferred block of data (or response to a command), and
-       sector/sector_count identify any remaining sectors.
-       Both sector and sector_count are in terms of qemu 512 byte blocks.  */
     /* ??? We should probably keep track of whether the data trasfer is
        a read or a write.  Currently we rely on the host getting it right.  */
+    /* Both sector and sector_count are in terms of qemu 512 byte blocks.  */
     int sector;
     int sector_count;
-    int buf_pos;
+    /* The amounnt of data in the buffer.  */
     int buf_len;
-    int sense;
+    uint8_t dma_buf[SCSI_DMA_BUF_SIZE];
     BlockDriverAIOCB *aiocb;
-    /* Data still to be transfered after this request completes.  */
-    uint8_t *aiodata;
-    uint32_t aiolen;
-    char buf[512];
+    struct SCSIRequest *next;
+} SCSIRequest;
+
+struct SCSIDevice
+{
+    BlockDriverState *bdrv;
+    SCSIRequest *requests;
+    /* The qemu block layer uses a fixed 512 byte sector size.
+       This is the number of 512 byte blocks in a single scsi sector.  */
+    int cluster_size;
+    int sense;
+    int tcq;
     /* Completion functions may be called from either scsi_{read,write}_data
        or from the AIO completion routines.  */
     scsi_completionfn completion;
     void *opaque;
 };
 
-static void scsi_command_complete(SCSIDevice *s, int sense)
+/* Global pool of SCSIRequest structures.  */
+static SCSIRequest *free_requests = NULL;
+
+static SCSIRequest *scsi_new_request(SCSIDevice *s, uint32_t tag)
 {
-    s->sense = sense;
-    s->completion(s->opaque, SCSI_REASON_DONE, sense);
+    SCSIRequest *r;
+
+    if (free_requests) {
+        r = free_requests;
+        free_requests = r->next;
+    } else {
+        r = qemu_malloc(sizeof(SCSIRequest));
+    }
+    r->dev = s;
+    r->tag = tag;
+    r->sector_count = 0;
+    r->buf_len = 0;
+    r->aiocb = NULL;
+
+    r->next = s->requests;
+    s->requests = r;
+    return r;
 }
 
-static void scsi_transfer_complete(SCSIDevice *s)
+static void scsi_remove_request(SCSIRequest *r)
 {
-    s->completion(s->opaque, SCSI_REASON_DATA, 0);
-    s->aiocb = NULL;
+    SCSIRequest *last;
+    SCSIDevice *s = r->dev;
+
+    if (s->requests == r) {
+        s->requests = r->next;
+    } else {
+        last = s->requests;
+        while (last && last->next != r)
+            last = last->next;
+        if (last) {
+            last->next = r->next;
+        } else {
+            BADF("Orphaned request\n");
+        }
+    }
+    r->next = free_requests;
+    free_requests = r;
 }
 
-static void scsi_read_complete(void * opaque, int ret)
+static SCSIRequest *scsi_find_request(SCSIDevice *s, uint32_t tag)
 {
-    SCSIDevice *s = (SCSIDevice *)opaque;
+    SCSIRequest *r;
 
-    if (ret) {
-        DPRINTF("IO error\n");
-        scsi_command_complete(s, SENSE_HARDWARE_ERROR);
-    }
+    r = s->requests;
+    while (r && r->tag != tag)
+        r = r->next;
 
-    if (s->aiolen) {
-        /* Read the remaining data.  Full and partial sectors are transferred
-           separately.  */
-        scsi_read_data(s, s->aiodata, s->aiolen);
-    } else {
-        if (s->buf_len == 0 && s->sector_count == 0)
-            scsi_command_complete(s, SENSE_NO_SENSE);
-        else
-            scsi_transfer_complete(s);
-    }
+    return r;
+}
+
+/* Helper function for command completion.  */
+static void scsi_command_complete(SCSIRequest *r, int sense)
+{
+    SCSIDevice *s = r->dev;
+    uint32_t tag;
+    DPRINTF("Command complete tag=0x%x sense=%d\n", r->tag, sense);
+    s->sense = sense;
+    tag = r->tag;
+    scsi_remove_request(r);
+    s->completion(s->opaque, SCSI_REASON_DONE, tag, sense);
 }
 
 /* Cancel a pending data transfer.  */
-void scsi_cancel_io(SCSIDevice *s)
+void scsi_cancel_io(SCSIDevice *s, uint32_t tag)
 {
-    if (!s->aiocb) {
-        BADF("Cancel with no pending IO\n");
+    SCSIRequest *r;
+    DPRINTF("Cancel tag=0x%x\n", tag);
+    r = scsi_find_request(s, tag);
+    if (r) {
+        if (r->aiocb)
+            bdrv_aio_cancel(r->aiocb);
+        r->aiocb = NULL;
+        scsi_remove_request(r);
+    }
+}
+
+static void scsi_read_complete(void * opaque, int ret)
+{
+    SCSIRequest *r = (SCSIRequest *)opaque;
+    SCSIDevice *s = r->dev;
+
+    if (ret) {
+        DPRINTF("IO error\n");
+        scsi_command_complete(r, SENSE_HARDWARE_ERROR);
         return;
     }
-    bdrv_aio_cancel(s->aiocb);
-    s->aiocb = NULL;
+    DPRINTF("Data ready tag=0x%x len=%d\n", r->tag, r->buf_len);
+
+    s->completion(s->opaque, SCSI_REASON_DATA, r->tag, r->buf_len);
 }
 
-/* Read data from a scsi device.  Returns nonzero on failure.
-   The transfer may complete asynchronously.  */
-int scsi_read_data(SCSIDevice *s, uint8_t *data, uint32_t len)
+/* Read more data from scsi device into buffer.  */
+void scsi_read_data(SCSIDevice *s, uint32_t tag)
 {
+    SCSIRequest *r;
     uint32_t n;
 
-    DPRINTF("Read %d (%d/%d)\n", len, s->buf_len, s->sector_count);
-    if (s->buf_len == 0 && s->sector_count == 0)
-        return 1;
-
-    if (s->buf_len) {
-        n = s->buf_len;
-        if (n > len)
-            n = len;
-        memcpy(data, s->buf + s->buf_pos, n);
-        s->buf_pos += n;
-        s->buf_len -= n;
-        data += n;
-        len -= n;
-        if (s->buf_len == 0)
-            s->buf_pos = 0;
+    r = scsi_find_request(s, tag);
+    if (!r) {
+        BADF("Bad read tag 0x%x\n", tag);
+        /* ??? This is the wrong error.  */
+        scsi_command_complete(r, SENSE_HARDWARE_ERROR);
+        return;
     }
-
-    n = len / 512;
-    if (n > s->sector_count)
-      n = s->sector_count;
-
-    if (n != 0) {
-        s->aiolen = len - n * 512;
-        s->aiodata = data + n * 512;
-        s->aiocb = bdrv_aio_read(s->bdrv, s->sector, data, n,
-                                 scsi_read_complete, s);
-        if (s->aiocb == NULL)
-            scsi_command_complete(s, SENSE_HARDWARE_ERROR);
-        s->sector += n;
-        s->sector_count -= n;
-        return 0;
+    if (r->sector_count == (uint32_t)-1) {
+        DPRINTF("Read buf_len=%d\n", r->buf_len);
+        r->sector_count = 0;
+        s->completion(s->opaque, SCSI_REASON_DATA, r->tag, r->buf_len);
+        return;
     }
-
-    if (len && s->sector_count) {
-        /* TODO: Make this use AIO.  */
-        bdrv_read(s->bdrv, s->sector, s->buf, 1);
-        s->sector++;
-        s->sector_count--;
-        s->buf_pos = 0;
-        s->buf_len = 512;
-        /* Recurse to complete the partial read.  */
-        return scsi_read_data(s, data, len);
+    DPRINTF("Read sector_count=%d\n", r->sector_count);
+    if (r->sector_count == 0) {
+        scsi_command_complete(r, SENSE_NO_SENSE);
+        return;
     }
 
-    if (len != 0)
-        return 1;
-
-    if (s->buf_len == 0 && s->sector_count == 0)
-        scsi_command_complete(s, SENSE_NO_SENSE);
-    else
-        scsi_transfer_complete(s);
-
-    return 0;
+    n = r->sector_count;
+    if (n > SCSI_DMA_BUF_SIZE / 512)
+        n = SCSI_DMA_BUF_SIZE / 512;
+
+    r->buf_len = n * 512;
+    r->aiocb = bdrv_aio_read(s->bdrv, r->sector, r->dma_buf, n,
+                             scsi_read_complete, r);
+    if (r->aiocb == NULL)
+        scsi_command_complete(r, SENSE_HARDWARE_ERROR);
+    r->sector += n;
+    r->sector_count -= n;
 }
 
 static void scsi_write_complete(void * opaque, int ret)
 {
-    SCSIDevice *s = (SCSIDevice *)opaque;
+    SCSIRequest *r = (SCSIRequest *)opaque;
+    SCSIDevice *s = r->dev;
+    uint32_t len;
 
     if (ret) {
         fprintf(stderr, "scsi-disc: IO write error\n");
         exit(1);
     }
 
-    if (s->sector_count == 0)
-        scsi_command_complete(s, SENSE_NO_SENSE);
-    else
-        scsi_transfer_complete(s);
-}
-
-static uint32_t scsi_write_partial_sector(SCSIDevice *s, uint8_t *data,
-                                          uint32_t len)
-{
-    int n;
-
-    n = 512 - s->buf_len;
-    if (n > len)
-        n = len;
-
-    memcpy(s->buf + s->buf_len, data, n);
-    data += n;
-    s->buf_len += n;
-    len -= n;
-    if (s->buf_len == 512) {
-        /* A full sector has been accumulated. Write it to disk.  */
-        /* TODO: Make this use async IO.  */
-        bdrv_write(s->bdrv, s->sector, s->buf, 1);
-        s->buf_len = 0;
-        s->sector++;
-        s->sector_count--;
+    r->aiocb = NULL;
+    if (r->sector_count == 0) {
+        scsi_command_complete(r, SENSE_NO_SENSE);
+    } else {
+        len = r->sector_count * 512;
+        if (len > SCSI_DMA_BUF_SIZE) {
+            len = SCSI_DMA_BUF_SIZE;
+        }
+        r->buf_len = len;
+        DPRINTF("Write complete tag=0x%x more=%d\n", r->tag, len);
+        s->completion(s->opaque, SCSI_REASON_DATA, r->tag, len);
     }
-    return n;
 }
 
 /* Write data to a scsi device.  Returns nonzero on failure.
    The transfer may complete asynchronously.  */
-int scsi_write_data(SCSIDevice *s, uint8_t *data, uint32_t len)
+int scsi_write_data(SCSIDevice *s, uint32_t tag)
 {
+    SCSIRequest *r;
     uint32_t n;
 
-    DPRINTF("Write %d (%d/%d)\n", len, s->buf_len, s->sector_count);
-    if (s->buf_pos != 0) {
-        BADF("Bad state on write\n");
+    DPRINTF("Write data tag=0x%x\n", tag);
+    r = scsi_find_request(s, tag);
+    if (!r) {
+        BADF("Bad write tag 0x%x\n", tag);
+        scsi_command_complete(r, SENSE_HARDWARE_ERROR);
         return 1;
     }
-
-    if (s->sector_count == 0)
-        return 1;
-
-    if (s->buf_len != 0 || len < 512) {
-        n = scsi_write_partial_sector(s, data, len);
-        len -= n;
-        data += n;
+    if (r->aiocb)
+        BADF("Data transfer already in progress\n");
+    n = r->buf_len / 512;
+    if (n) {
+        r->aiocb = bdrv_aio_write(s->bdrv, r->sector, r->dma_buf, n,
+                                  scsi_write_complete, r);
+        if (r->aiocb == NULL)
+            scsi_command_complete(r, SENSE_HARDWARE_ERROR);
+        r->sector += n;
+        r->sector_count -= n;
+    } else {
+        /* Invoke completion routine to fetch data from host.  */
+        scsi_write_complete(r, 0);
     }
 
-    n = len / 512;
-    if (n > s->sector_count)
-        return 1;
+    return 0;
+}
 
-    if (n != 0) {
-        s->aiocb = bdrv_aio_write(s->bdrv, s->sector, data, n,
-                                   scsi_write_complete, s);
-        if (s->aiocb == NULL)
-            scsi_command_complete(s, SENSE_HARDWARE_ERROR);
-        data += n * 512;
-        len -= n * 512;
-        s->sector += n;
-        s->sector_count -= n;
-    }
+/* Return a pointer to the data buffer.  */
+uint8_t *scsi_get_buf(SCSIDevice *s, uint32_t tag)
+{
+    SCSIRequest *r;
 
-    if (len) {
-        if (s->sector_count == 0)
-            return 1;
-        /* Complete a partial write.  */
-        scsi_write_partial_sector(s, data, len);
+    r = scsi_find_request(s, tag);
+    if (!r) {
+        BADF("Bad buffer tag 0x%x\n", tag);
+        return NULL;
     }
-    if (n == 0) {
-        /* Transfer completes immediately.  */
-        if (s->sector_count == 0)
-            scsi_command_complete(s, SENSE_NO_SENSE);
-        else
-            scsi_transfer_complete(s);
-    }
-
-    return 0;
+    return r->dma_buf;
 }
 
 /* Execute a scsi command.  Returns the length of the data expected by the
@@ -267,15 +281,23 @@ int32_t scsi_send_command(SCSIDevice *s, uint32_t tag, uint8_t *buf, int lun)
     uint32_t len;
     int cmdlen;
     int is_write;
-
-    s->command = buf[0];
-    s->tag = tag;
-    s->sector_count = 0;
-    s->buf_pos = 0;
-    s->buf_len = 0;
+    uint8_t command;
+    uint8_t *outbuf;
+    SCSIRequest *r;
+
+    command = buf[0];
+    r = scsi_find_request(s, tag);
+    if (r) {
+        BADF("Tag 0x%x already in use\n", tag);
+        scsi_cancel_io(s, tag);
+    }
+    /* ??? Tags are not unique for different luns.  We only implement a
+       single lun, so this should not matter.  */
+    r = scsi_new_request(s, tag);
+    outbuf = r->dma_buf;
     is_write = 0;
-    DPRINTF("Command: 0x%02x", buf[0]);
-    switch (s->command >> 5) {
+    DPRINTF("Command: lun=%d tag=0x%x data=0x%02x", lun, tag, buf[0]);
+    switch (command >> 5) {
     case 0:
         lba = buf[3] | (buf[2] << 8) | ((buf[1] & 0x1f) << 16);
         len = buf[4];
@@ -298,7 +320,7 @@ int32_t scsi_send_command(SCSIDevice *s, uint32_t tag, uint8_t *buf, int lun)
         cmdlen = 12;
         break;
     default:
-        BADF("Unsupported command length, command %x\n", s->command);
+        BADF("Unsupported command length, command %x\n", command);
         goto fail;
     }
 #ifdef DEBUG_SCSI
@@ -315,7 +337,7 @@ int32_t scsi_send_command(SCSIDevice *s, uint32_t tag, uint8_t *buf, int lun)
         DPRINTF("Unimplemented LUN %d\n", lun ? lun : buf[1] >> 5);
         goto fail;
     }
-    switch (s->command) {
+    switch (command) {
     case 0x0:
 	DPRINTF("Test Unit Ready\n");
 	break;
@@ -324,33 +346,35 @@ int32_t scsi_send_command(SCSIDevice *s, uint32_t tag, uint8_t *buf, int lun)
         if (len < 4)
             goto fail;
         memset(buf, 0, 4);
-        s->buf[0] = 0xf0;
-        s->buf[1] = 0;
-        s->buf[2] = s->sense;
-        s->buf_len = 4;
+        outbuf[0] = 0xf0;
+        outbuf[1] = 0;
+        outbuf[2] = s->sense;
+        r->buf_len = 4;
         break;
     case 0x12:
         DPRINTF("Inquiry (len %d)\n", len);
         if (len < 36) {
             BADF("Inquiry buffer too small (%d)\n", len);
         }
-	memset(s->buf, 0, 36);
+	memset(outbuf, 0, 36);
 	if (bdrv_get_type_hint(s->bdrv) == BDRV_TYPE_CDROM) {
-	    s->buf[0] = 5;
-            s->buf[1] = 0x80;
-	    memcpy(&s->buf[16], "QEMU CD-ROM    ", 16);
+	    outbuf[0] = 5;
+            outbuf[1] = 0x80;
+	    memcpy(&outbuf[16], "QEMU CD-ROM    ", 16);
 	} else {
-	    s->buf[0] = 0;
-	    memcpy(&s->buf[16], "QEMU HARDDISK  ", 16);
+	    outbuf[0] = 0;
+	    memcpy(&outbuf[16], "QEMU HARDDISK  ", 16);
 	}
-	memcpy(&s->buf[8], "QEMU   ", 8);
-        memcpy(&s->buf[32], QEMU_VERSION, 4);
+	memcpy(&outbuf[8], "QEMU   ", 8);
+        memcpy(&outbuf[32], QEMU_VERSION, 4);
         /* Identify device as SCSI-3 rev 1.
            Some later commands are also implemented. */
-	s->buf[2] = 3;
-	s->buf[3] = 2; /* Format 2 */
-	s->buf[4] = 32;
-	s->buf_len = 36;
+	outbuf[2] = 3;
+	outbuf[3] = 2; /* Format 2 */
+	outbuf[4] = 32;
+        /* Sync data transfer and TCQ.  */
+        outbuf[7] = 0x10 | (s->tcq ? 0x02 : 0);
+	r->buf_len = 36;
 	break;
     case 0x16:
         DPRINTF("Reserve(6)\n");
@@ -365,17 +389,17 @@ int32_t scsi_send_command(SCSIDevice *s, uint32_t tag, uint8_t *buf, int lun)
     case 0x1a:
     case 0x5a:
         {
-            char *p;
+            uint8_t *p;
             int page;
 
             page = buf[2] & 0x3f;
             DPRINTF("Mode Sense (page %d, len %d)\n", page, len);
-            p = s->buf;
+            p = outbuf;
             memset(p, 0, 4);
-            s->buf[1] = 0; /* Default media type.  */
-            s->buf[3] = 0; /* Block descriptor length.  */
+            outbuf[1] = 0; /* Default media type.  */
+            outbuf[3] = 0; /* Block descriptor length.  */
             if (bdrv_get_type_hint(s->bdrv) == BDRV_TYPE_CDROM) {
-                s->buf[2] = 0x80; /* Readonly.  */
+                outbuf[2] = 0x80; /* Readonly.  */
             }
             p += 4;
             if ((page == 8 || page == 0x3f)) {
@@ -415,10 +439,10 @@ int32_t scsi_send_command(SCSIDevice *s, uint32_t tag, uint8_t *buf, int lun)
                 p[21] = (16 * 176) & 0xff;
                 p += 21;
             }
-            s->buf_len = p - s->buf;
-            s->buf[0] = s->buf_len - 4;
-            if (s->buf_len > len)
-                s->buf_len = len;
+            r->buf_len = p - outbuf;
+            outbuf[0] = r->buf_len - 4;
+            if (r->buf_len > len)
+                r->buf_len = len;
         }
         break;
     case 0x1b:
@@ -431,36 +455,36 @@ int32_t scsi_send_command(SCSIDevice *s, uint32_t tag, uint8_t *buf, int lun)
     case 0x25:
 	DPRINTF("Read Capacity\n");
         /* The normal LEN field for this command is zero.  */
-	memset(s->buf, 0, 8);
+	memset(outbuf, 0, 8);
 	bdrv_get_geometry(s->bdrv, &nb_sectors);
         /* Returned value is the address of the last sector.  */
         if (nb_sectors) {
             nb_sectors--;
-            s->buf[0] = (nb_sectors >> 24) & 0xff;
-            s->buf[1] = (nb_sectors >> 16) & 0xff;
-            s->buf[2] = (nb_sectors >> 8) & 0xff;
-            s->buf[3] = nb_sectors & 0xff;
-            s->buf[4] = 0;
-            s->buf[5] = 0;
-            s->buf[6] = s->cluster_size * 2;
-            s->buf[7] = 0;
-            s->buf_len = 8;
+            outbuf[0] = (nb_sectors >> 24) & 0xff;
+            outbuf[1] = (nb_sectors >> 16) & 0xff;
+            outbuf[2] = (nb_sectors >> 8) & 0xff;
+            outbuf[3] = nb_sectors & 0xff;
+            outbuf[4] = 0;
+            outbuf[5] = 0;
+            outbuf[6] = s->cluster_size * 2;
+            outbuf[7] = 0;
+            r->buf_len = 8;
         } else {
-            scsi_command_complete(s, SENSE_NOT_READY);
+            scsi_command_complete(r, SENSE_NOT_READY);
             return 0;
         }
 	break;
     case 0x08:
     case 0x28:
         DPRINTF("Read (sector %d, count %d)\n", lba, len);
-        s->sector = lba * s->cluster_size;
-        s->sector_count = len * s->cluster_size;
+        r->sector = lba * s->cluster_size;
+        r->sector_count = len * s->cluster_size;
         break;
     case 0x0a:
     case 0x2a:
         DPRINTF("Write (sector %d, count %d)\n", lba, len);
-        s->sector = lba * s->cluster_size;
-        s->sector_count = len * s->cluster_size;
+        r->sector = lba * s->cluster_size;
+        r->sector_count = len * s->cluster_size;
         is_write = 1;
         break;
     case 0x35:
@@ -478,18 +502,18 @@ int32_t scsi_send_command(SCSIDevice *s, uint32_t tag, uint8_t *buf, int lun)
             DPRINTF("Read TOC (track %d format %d msf %d)\n", start_track, format, msf >> 1);
             switch(format) {
             case 0:
-                toclen = cdrom_read_toc(nb_sectors, s->buf, msf, start_track);
+                toclen = cdrom_read_toc(nb_sectors, outbuf, msf, start_track);
                 break;
             case 1:
                 /* multi session : only a single session defined */
                 toclen = 12;
-                memset(s->buf, 0, 12);
-                s->buf[1] = 0x0a;
-                s->buf[2] = 0x01;
-                s->buf[3] = 0x01;
+                memset(outbuf, 0, 12);
+                outbuf[1] = 0x0a;
+                outbuf[2] = 0x01;
+                outbuf[3] = 0x01;
                 break;
             case 2:
-                toclen = cdrom_read_toc_raw(nb_sectors, s->buf, msf, start_track);
+                toclen = cdrom_read_toc_raw(nb_sectors, outbuf, msf, start_track);
                 break;
             default:
                 goto error_cmd;
@@ -497,7 +521,7 @@ int32_t scsi_send_command(SCSIDevice *s, uint32_t tag, uint8_t *buf, int lun)
             if (toclen > 0) {
                 if (len > toclen)
                   len = toclen;
-                s->buf_len = len;
+                r->buf_len = len;
                 break;
             }
         error_cmd:
@@ -506,11 +530,11 @@ int32_t scsi_send_command(SCSIDevice *s, uint32_t tag, uint8_t *buf, int lun)
         }
     case 0x46:
         DPRINTF("Get Configuration (rt %d, maxlen %d)\n", buf[1] & 3, len);
-        memset(s->buf, 0, 8);
+        memset(outbuf, 0, 8);
         /* ??? This shoud probably return much more information.  For now
            just return the basic header indicating the CD-ROM profile.  */
-        s->buf[7] = 8; // CD-ROM
-        s->buf_len = 8;
+        outbuf[7] = 8; // CD-ROM
+        r->buf_len = 8;
         break;
     case 0x56:
         DPRINTF("Reserve(10)\n");
@@ -526,21 +550,27 @@ int32_t scsi_send_command(SCSIDevice *s, uint32_t tag, uint8_t *buf, int lun)
         DPRINTF("Report LUNs (len %d)\n", len);
         if (len < 16)
             goto fail;
-        memset(s->buf, 0, 16);
-        s->buf[3] = 8;
-        s->buf_len = 16;
+        memset(outbuf, 0, 16);
+        outbuf[3] = 8;
+        r->buf_len = 16;
         break;
     default:
 	DPRINTF("Unknown SCSI command (%2.2x)\n", buf[0]);
     fail:
-        scsi_command_complete(s, SENSE_ILLEGAL_REQUEST);
+        scsi_command_complete(r, SENSE_ILLEGAL_REQUEST);
 	return 0;
     }
-    if (s->sector_count == 0 && s->buf_len == 0) {
-        scsi_command_complete(s, SENSE_NO_SENSE);
+    if (r->sector_count == 0 && r->buf_len == 0) {
+        scsi_command_complete(r, SENSE_NO_SENSE);
+    }
+    len = r->sector_count * 512 + r->buf_len;
+    if (is_write) {
+        return -len;
+    } else {
+        if (!r->sector_count)
+            r->sector_count = -1;
+        return len;
     }
-    len = s->sector_count * 512 + s->buf_len;
-    return is_write ? -len : len;
 }
 
 void scsi_disk_destroy(SCSIDevice *s)
@@ -549,6 +579,7 @@ void scsi_disk_destroy(SCSIDevice *s)
 }
 
 SCSIDevice *scsi_disk_init(BlockDriverState *bdrv,
+                           int tcq,
                            scsi_completionfn completion,
                            void *opaque)
 {
@@ -556,6 +587,7 @@ SCSIDevice *scsi_disk_init(BlockDriverState *bdrv,
 
     s = (SCSIDevice *)qemu_mallocz(sizeof(SCSIDevice));
     s->bdrv = bdrv;
+    s->tcq = tcq;
     s->completion = completion;
     s->opaque = opaque;
     if (bdrv_get_type_hint(s->bdrv) == BDRV_TYPE_CDROM) {
diff --git a/hw/sun4m.c b/hw/sun4m.c
index 203732f21..09a157c0a 100644
--- a/hw/sun4m.c
+++ b/hw/sun4m.c
@@ -199,6 +199,18 @@ uint32_t iommu_translate(uint32_t addr)
     return iommu_translate_local(iommu, addr);
 }
 
+void sparc_iommu_memory_read(target_phys_addr_t addr,
+                           uint8_t *buf, int len)
+{
+    return sparc_iommu_memory_rw_local(iommu, addr, buf, len, 0);
+}
+
+void sparc_iommu_memory_write(target_phys_addr_t addr,
+                           uint8_t *buf, int len)
+{
+    return sparc_iommu_memory_rw_local(iommu, addr, buf, len, 1);
+}
+
 static void *slavio_misc;
 
 void qemu_system_powerdown(void)
diff --git a/hw/usb-msd.c b/hw/usb-msd.c
index e4cfca0fe..4530a1cea 100644
--- a/hw/usb-msd.c
+++ b/hw/usb-msd.c
@@ -32,8 +32,12 @@ enum USBMSDMode {
 typedef struct {
     USBDevice dev;
     enum USBMSDMode mode;
+    uint32_t scsi_len;
+    uint8_t *scsi_buf;
+    uint32_t usb_len;
+    uint8_t *usb_buf;
     uint32_t data_len;
-    uint32_t transfer_len;
+    uint32_t residue;
     uint32_t tag;
     BlockDriverState *bs;
     SCSIDevice *scsi_dev;
@@ -42,6 +46,23 @@ typedef struct {
     USBPacket *packet;
 } MSDState;
 
+struct usb_msd_cbw {
+    uint32_t sig;
+    uint32_t tag;
+    uint32_t data_len;
+    uint8_t flags;
+    uint8_t lun;
+    uint8_t cmd_len;
+    uint8_t cmd[16];
+};
+
+struct usb_msd_csw {
+    uint32_t sig;
+    uint32_t tag;
+    uint32_t residue;
+    uint8_t status;
+};
+
 static const uint8_t qemu_msd_dev_descriptor[] = {
 	0x12,       /*  u8 bLength; */
 	0x01,       /*  u8 bDescriptorType; Device */
@@ -107,26 +128,90 @@ static const uint8_t qemu_msd_config_descriptor[] = {
 	0x00        /*  u8  ep_bInterval; */
 };
 
-static void usb_msd_command_complete(void *opaque, uint32_t reason, int fail)
+static void usb_msd_copy_data(MSDState *s)
+{
+    uint32_t len;
+    len = s->usb_len;
+    if (len > s->scsi_len)
+        len = s->scsi_len;
+    if (s->mode == USB_MSDM_DATAIN) {
+        memcpy(s->usb_buf, s->scsi_buf, len);
+    } else {
+        memcpy(s->scsi_buf, s->usb_buf, len);
+    }
+    s->usb_len -= len;
+    s->scsi_len -= len;
+    s->usb_buf += len;
+    s->scsi_buf += len;
+    s->data_len -= len;
+    if (s->scsi_len == 0) {
+        if (s->mode == USB_MSDM_DATAIN) {
+            scsi_read_data(s->scsi_dev, s->tag);
+        } else if (s->mode == USB_MSDM_DATAOUT) {
+            scsi_write_data(s->scsi_dev, s->tag);
+        }
+    }
+}
+
+static void usb_msd_send_status(MSDState *s)
+{
+    struct usb_msd_csw csw;
+
+    csw.sig = cpu_to_le32(0x53425355);
+    csw.tag = cpu_to_le32(s->tag);
+    csw.residue = s->residue;
+    csw.status = s->result;
+    memcpy(s->usb_buf, &csw, 13);
+}
+
+static void usb_msd_command_complete(void *opaque, int reason, uint32_t tag,
+                                     uint32_t arg)
 {
     MSDState *s = (MSDState *)opaque;
-    USBPacket *p;
+    USBPacket *p = s->packet;
 
-    s->data_len -= s->transfer_len;
-    s->transfer_len = 0;
+    if (tag != s->tag) {
+        fprintf(stderr, "usb-msd: Unexpected SCSI Tag 0x%x\n", tag);
+    }
     if (reason == SCSI_REASON_DONE) {
-        DPRINTF("Command complete %d\n", fail);
-        s->result = fail;
-        s->mode = USB_MSDM_CSW;
+        DPRINTF("Command complete %d\n", arg);
+        s->residue = s->data_len;
+        s->result = arg != 0;
+        if (s->packet) {
+            if (s->data_len == 0 && s->mode == USB_MSDM_DATAOUT) {
+                /* A deferred packet with no write data remaining must be
+                   the status read packet.  */
+                usb_msd_send_status(s);
+                s->mode = USB_MSDM_CBW;
+            } else {
+                if (s->data_len) {
+                    s->data_len -= s->usb_len;
+                    if (s->mode == USB_MSDM_DATAIN)
+                        memset(s->usb_buf, 0, s->usb_len);
+                    s->usb_len = 0;
+                }
+                if (s->data_len == 0)
+                    s->mode = USB_MSDM_CSW;
+            }
+            s->packet = NULL;
+            usb_packet_complete(p);
+        } else if (s->data_len == 0) {
+            s->mode = USB_MSDM_CSW;
+        }
+        return;
     }
-    if (s->packet) {
-        /* Set s->packet to NULL before calling usb_packet_complete because
-           annother request may be issues before usb_packet_complete returns.
-         */
-        DPRINTF("Packet complete %p\n", p);
-        p = s->packet;
-        s->packet = NULL;
-        usb_packet_complete(p);
+    s->scsi_len = arg;
+    s->scsi_buf = scsi_get_buf(s->scsi_dev, tag);
+    if (p) {
+        usb_msd_copy_data(s);
+        if (s->usb_len == 0) {
+            /* Set s->packet to NULL before calling usb_packet_complete
+               because annother request may be issued before
+               usb_packet_complete returns.  */
+            DPRINTF("Packet complete %p\n", p);
+            s->packet = NULL;
+            usb_packet_complete(p);
+        }
     }
 }
 
@@ -251,28 +336,12 @@ static int usb_msd_handle_control(USBDevice *dev, int request, int value,
     return ret;
 }
 
-struct usb_msd_cbw {
-    uint32_t sig;
-    uint32_t tag;
-    uint32_t data_len;
-    uint8_t flags;
-    uint8_t lun;
-    uint8_t cmd_len;
-    uint8_t cmd[16];
-};
-
-struct usb_msd_csw {
-    uint32_t sig;
-    uint32_t tag;
-    uint32_t residue;
-    uint8_t status;
-};
-
 static void usb_msd_cancel_io(USBPacket *p, void *opaque)
 {
     MSDState *s = opaque;
-    scsi_cancel_io(s->scsi_dev);
+    scsi_cancel_io(s->scsi_dev, s->tag);
     s->packet = NULL;
+    s->scsi_len = 0;
 }
 
 static int usb_msd_handle_data(USBDevice *dev, USBPacket *p)
@@ -280,7 +349,6 @@ static int usb_msd_handle_data(USBDevice *dev, USBPacket *p)
     MSDState *s = (MSDState *)dev;
     int ret = 0;
     struct usb_msd_cbw cbw;
-    struct usb_msd_csw csw;
     uint8_t devep = p->devep;
     uint8_t *data = p->data;
     int len = p->len;
@@ -318,7 +386,17 @@ static int usb_msd_handle_data(USBDevice *dev, USBPacket *p)
             }
             DPRINTF("Command tag 0x%x flags %08x len %d data %d\n",
                     s->tag, cbw.flags, cbw.cmd_len, s->data_len);
+            s->residue = 0;
             scsi_send_command(s->scsi_dev, s->tag, cbw.cmd, 0);
+            /* ??? Should check that USB and SCSI data transfer
+               directions match.  */
+            if (s->residue == 0) {
+                if (s->mode == USB_MSDM_DATAIN) {
+                    scsi_read_data(s->scsi_dev, s->tag);
+                } else if (s->mode == USB_MSDM_DATAOUT) {
+                    scsi_write_data(s->scsi_dev, s->tag);
+                }
+            }
             ret = len;
             break;
 
@@ -327,17 +405,24 @@ static int usb_msd_handle_data(USBDevice *dev, USBPacket *p)
             if (len > s->data_len)
                 goto fail;
 
-            s->transfer_len = len;
-            if (scsi_write_data(s->scsi_dev, data, len))
-                goto fail;
-
-            if (s->transfer_len == 0) {
-                ret = len;
-            } else {
+            s->usb_buf = data;
+            s->usb_len = len;
+            if (s->scsi_len) {
+                usb_msd_copy_data(s);
+            }
+            if (s->residue && s->usb_len) {
+                s->data_len -= s->usb_len;
+                if (s->data_len == 0)
+                    s->mode = USB_MSDM_CSW;
+                s->usb_len = 0;
+            }
+            if (s->usb_len) {
                 DPRINTF("Deferring packet %p\n", p);
                 usb_defer_packet(p, usb_msd_cancel_io, s);
                 s->packet = p;
                 ret = USB_RET_ASYNC;
+            } else {
+                ret = len;
             }
             break;
 
@@ -352,37 +437,51 @@ static int usb_msd_handle_data(USBDevice *dev, USBPacket *p)
             goto fail;
 
         switch (s->mode) {
+        case USB_MSDM_DATAOUT:
+            if (s->data_len != 0 || len < 13)
+                goto fail;
+            /* Waiting for SCSI write to complete.  */
+            usb_defer_packet(p, usb_msd_cancel_io, s);
+            s->packet = p;
+            ret = USB_RET_ASYNC;
+            break;
+
         case USB_MSDM_CSW:
             DPRINTF("Command status %d tag 0x%x, len %d\n",
                     s->result, s->tag, len);
             if (len < 13)
                 goto fail;
 
-            csw.sig = cpu_to_le32(0x53425355);
-            csw.tag = cpu_to_le32(s->tag);
-            csw.residue = 0;
-            csw.status = s->result;
-            memcpy(data, &csw, 13);
-            ret = 13;
+            s->usb_len = len;
+            s->usb_buf = data;
+            usb_msd_send_status(s);
             s->mode = USB_MSDM_CBW;
+            ret = 13;
             break;
 
         case USB_MSDM_DATAIN:
             DPRINTF("Data in %d/%d\n", len, s->data_len);
             if (len > s->data_len)
                 len = s->data_len;
-
-            s->transfer_len = len;
-            if (scsi_read_data(s->scsi_dev, data, len))
-                goto fail;
-
-            if (s->transfer_len == 0) {
-                ret = len;
-            } else {
+            s->usb_buf = data;
+            s->usb_len = len;
+            if (s->scsi_len) {
+                usb_msd_copy_data(s);
+            }
+            if (s->residue && s->usb_len) {
+                s->data_len -= s->usb_len;
+                memset(s->usb_buf, 0, s->usb_len);
+                if (s->data_len == 0)
+                    s->mode = USB_MSDM_CSW;
+                s->usb_len = 0;
+            }
+            if (s->usb_len) {
                 DPRINTF("Deferring packet %p\n", p);
                 usb_defer_packet(p, usb_msd_cancel_io, s);
                 s->packet = p;
                 ret = USB_RET_ASYNC;
+            } else {
+                ret = len;
             }
             break;
 
@@ -436,7 +535,7 @@ USBDevice *usb_msd_init(const char *filename)
     snprintf(s->dev.devname, sizeof(s->dev.devname), "QEMU USB MSD(%.16s)",
              filename);
 
-    s->scsi_dev = scsi_disk_init(bdrv, usb_msd_command_complete, s);
+    s->scsi_dev = scsi_disk_init(bdrv, 0, usb_msd_command_complete, s);
     usb_msd_handle_reset((USBDevice *)s);
     return (USBDevice *)s;
  fail: