diff --git a/hw/lsi53c895a.c b/hw/lsi53c895a.c
index 6b72793..983f6cb 100644
--- a/hw/lsi53c895a.c
+++ b/hw/lsi53c895a.c
@@ -18,7 +18,7 @@
 #include "dma.h"
 #include "block_int.h"
 
-//#define DEBUG_LSI
+#define DEBUG_LSI
 //#define DEBUG_LSI_REG
 
 #ifdef DEBUG_LSI
@@ -179,7 +179,8 @@ typedef struct lsi_request {
     SCSIDevice *dev;
     SCSIRequest *req;
     QEMUSGList sgl;
-    uint32_t finished;
+    uint32_t pending;
+    int out;
     QTAILQ_ENTRY(lsi_request) next;
 } lsi_request;
 
@@ -189,8 +190,6 @@ typedef struct {
     int ram_io_addr;
     uint32_t script_ram_base;
 
-    uint32_t enable_disconnect;
-
     int carry; /* ??? Should this be an a visible register somewhere?  */
     int sense;
     /* Action to take at the end of a MSG IN phase.
@@ -284,7 +283,6 @@ static inline int lsi_irq_on_rsl(LSIState *s)
 
 static void lsi_add_msg_byte(LSIState *s, uint8_t data);
 static void lsi_queue_command(LSIState *s);
-static void lsi_command_finish(LSIState *s);
 
 static void lsi_soft_reset(LSIState *s)
 {
@@ -446,10 +444,10 @@ static void lsi_update_irq(LSIState *s)
     qemu_set_irq(s->dev.irq[0], level);
 
     if (!level && lsi_irq_on_rsl(s) && !(s->scntl1 & LSI_SCNTL1_CON)) {
-        DPRINTF("Handled IRQs & disconnected, looking for finished "
+        DPRINTF("Handled IRQs & disconnected, looking for pending "
                 "processes\n");
         QTAILQ_FOREACH(p, &s->queue, next) {
-            if (p->finished) {
+            if (p->pending) {
                 lsi_reselect(s, p);
                 break;
             }
@@ -544,7 +542,6 @@ static void lsi_do_dma(LSIState *s, int out)
     SCSIDevice *dev;
 
     assert(s->current);
-    assert(!s->current->finished);
     assert(s->current->req->cmd.xfer > 0);
 
     id = (s->current->tag >> 8) & 0xf;
@@ -577,20 +574,6 @@ static void lsi_do_dma(LSIState *s, int out)
     if (s->current->req->cmd.xfer == s->current->sgl.size) {
         DPRINTF("Scatter list is complete, processing command\n");
         scsi_req_sgl(s->current->req, &s->current->sgl);
-
-        if (s->enable_disconnect && !s->command_complete &&
-            (s->current->tag & LSI_TAG_VALID)) {
-            /* Command did not complete immediately so disconnect.  */
-            lsi_add_msg_byte(s, 2); /* SAVE DATA POINTER */
-            lsi_add_msg_byte(s, 4); /* DISCONNECT */
-            /* wait data */
-            lsi_set_phase(s, PHASE_MI);
-            s->msg_action = 1;
-            lsi_queue_command(s);
-            lsi_resume_script(s);
-        } else {
-            /* lsi_command_complete() resumes scripts */;
-        }
     } else {
         lsi_resume_script(s);
     }
@@ -600,10 +583,15 @@ static void lsi_do_dma(LSIState *s, int out)
 /* Add a command to the queue.  */
 static void lsi_queue_command(LSIState *s)
 {
+    lsi_request *p = s->current;
+
     DPRINTF("Queueing tag=0x%x\n", s->current->tag);
     assert(s->current != NULL);
     QTAILQ_INSERT_TAIL(&s->queue, s->current, next);
     s->current = NULL;
+
+    p->pending = 0;
+    p->out = (s->sstat1 & PHASE_MASK) == PHASE_DO;
 }
 
 /* Queue a byte for a MSG IN phase.  */
@@ -622,8 +610,6 @@ static void lsi_reselect(LSIState *s, lsi_request *p)
 {
     int id;
 
-    assert(p->finished);
-    assert(p->tag & LSI_TAG_VALID);
     assert(s->current == NULL);
     QTAILQ_REMOVE(&s->queue, p, next);
     s->current = p;
@@ -637,7 +623,14 @@ static void lsi_reselect(LSIState *s, lsi_request *p)
     DPRINTF("Reselected target %d\n", id);
     s->scntl1 |= LSI_SCNTL1_CON;
     lsi_set_phase(s, PHASE_MI);
-    s->msg_action = 4;
+    s->msg_action = p->out ? 2 : 3;
+    /*
+     * Check if we need to force lsi_finish_command() to be called from
+     * lsi_do_msgin() for TEST_UNIT_READY and other non data lsi_requests
+     */
+    if (!(p->sgl.size))
+        s->msg_action = 4;
+
     lsi_add_msg_byte(s, 0x80);
 
     if (s->current->tag & LSI_TAG_VALID) {
@@ -652,35 +645,53 @@ static void lsi_reselect(LSIState *s, lsi_request *p)
 
 /* 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, lsi_request *p)
+static int lsi_queue_tag(LSIState *s, uint32_t tag, uint32_t arg)
 {
-    p->finished = 1;
-   /* Reselect if waiting for it, or if reselection triggers an IRQ
-      and the bus is free.
-      Since no interrupt stacking is implemented in the emulation, it
-      is also required that there are no pending interrupts waiting
-      for service from the device driver. */
-    if (s->waiting == 1 ||
-       (lsi_irq_on_rsl(s) && !(s->scntl1 & LSI_SCNTL1_CON) &&
-       !(s->istat0 & (LSI_ISTAT0_SIP | LSI_ISTAT0_DIP)))) {
-         /* Reselect device.  */
-          lsi_reselect(s, p);
-          return 0;
-     } else {
-          DPRINTF("Queueing IO tag=0x%x\n", p->tag);
-          return 1;
-     }
+    lsi_request *p;
+
+    QTAILQ_FOREACH(p, &s->queue, next) {
+        if (p->tag == tag) {
+            if (p->pending) {
+                BADF("Multiple IO pending for tag %d\n", tag);
+            }
+            p->pending = arg;
+            /* Reselect if waiting for it, or if reselection triggers an IRQ
+               and the bus is free.
+               Since no interrupt stacking is implemented in the emulation, it
+               is also required that there are no pending interrupts waiting
+               for service from the device driver. */
+            if (s->waiting == 1 ||
+                (lsi_irq_on_rsl(s) && !(s->scntl1 & LSI_SCNTL1_CON) &&
+                 !(s->istat0 & (LSI_ISTAT0_SIP | LSI_ISTAT0_DIP)))) {
+                /* Reselect device.  */
+                lsi_reselect(s, p);
+                return 0;
+            } else {
+                DPRINTF("Queueing IO tag=0x%x\n", tag);
+// Duplicate assignment..?
+                p->pending = arg;
+                return 1;
+            }
+        }
+    }
+    BADF("IO with unknown tag %d\n", tag);
+    return 1;
 }
 
-static void lsi_command_finish(LSIState *s)
+/* Used by lsi_command_complete() callback and lsi_do_msgin */
+static void lsi_finish_command(LSIState *s, SCSIRequest *req)
 {
-    SCSIRequest *req = s->current->req;
     int out;
+    
+    if (!(req)) {
+        printf("NULL SCSIRequest into lsi_finish_command()\n");
+        abort();
+    }
 
     DPRINTF("Command complete sense=%d\n", req->status);
     out = scsi_req_is_write(req);
     s->sense = req->status;
-    s->command_complete = 1;
+    s->command_complete = 2;
     if (s->waiting && req->xferlen != req->cmd.xfer) {
         /* Raise phase mismatch for short transfers.  */
 #if 1
@@ -711,18 +722,20 @@ static void lsi_command_complete(SCSIRequest *req)
 
     if (s->waiting == 1 || s->current == NULL || p != s->current ||
         (lsi_irq_on_rsl(s) && !(s->scntl1 & LSI_SCNTL1_CON))) {
-        if (lsi_queue_tag(s, p))
+        if (lsi_queue_tag(s, req->tag, 1))
             return;
     } else {
-        lsi_command_finish(s);
+        lsi_finish_command(s, req);
     }
     lsi_resume_script(s);
 }
 
 static void lsi_do_command(LSIState *s)
 {
-    SCSIRequest *req = s->current->req;
+    SCSIRequest *req;
+    SCSIDevice *dev;
     uint8_t buf[16];
+    uint32_t id;
 
     DPRINTF("Send command len=%d\n", s->dbc);
     if (s->dbc > 16)
@@ -731,19 +744,40 @@ static void lsi_do_command(LSIState *s)
     s->sfbr = buf[0];
     s->command_complete = 0;
 
+    id = (s->select_tag >> 8) & 0xf;
+    dev = s->bus.devs[id];
+    if (!dev) {
+        lsi_bad_selection(s, id);
+        return;
+    }
+
     assert(s->current == NULL);
     s->current = qemu_mallocz(sizeof(lsi_request));
     s->current->tag = s->select_tag;
     qemu_sglist_init(&s->current->sgl, 4);
-    req = scsi_req_get(s->current->dev, s->current->tag, s->current_lun);
+    req = scsi_req_get(dev, s->current->tag, s->current_lun);
     s->current->req = req;
     req->hba_private = s->current;
     scsi_req_parse(req, buf);
 
     lsi_set_phase(s, scsi_req_is_write(req) ? PHASE_DO : PHASE_DI);
     if (req->cmd.xfer == 0) {
-        s->waiting = 3;
         scsi_req_sgl(req, &s->current->sgl);
+
+        if (!s->command_complete) {
+            if (!(scsi_req_is_write(req))) {
+                /* Command did not complete immediately so disconnect.  */
+                lsi_add_msg_byte(s, 2); /* SAVE DATA POINTER */
+                lsi_add_msg_byte(s, 4); /* DISCONNECT */
+                /* wait data */
+                lsi_set_phase(s, PHASE_MI);
+                s->msg_action = 1;
+                lsi_queue_command(s);
+            } else {
+                /* wait command complete */
+                lsi_set_phase(s, PHASE_DI);
+            }
+        }
     }
 }
 
@@ -765,7 +799,7 @@ static void lsi_do_status(LSIState *s)
 static void lsi_do_msgin(LSIState *s)
 {
     int len;
-    DPRINTF("Message in len=%d/%d action=%d\n", s->dbc, s->msg_len, s->msg_action);
+    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)
@@ -786,8 +820,14 @@ static void lsi_do_msgin(LSIState *s)
         case 1:
             lsi_disconnect(s);
             break;
-        case 4:
-            lsi_command_finish(s);
+        case 2:
+            lsi_set_phase(s, PHASE_DO);
+            break;
+        case 3:
+            lsi_set_phase(s, PHASE_DI);
+            break;
+        case 4: // For TEST_UNIT_READY + Non Data CDBs with hw/scsi-generic.c
+            lsi_finish_command(s, s->current->req);
             break;
         default:
             abort();
@@ -901,7 +941,7 @@ static void lsi_wait_reselect(LSIState *s)
     DPRINTF("Wait Reselect\n");
 
     QTAILQ_FOREACH(p, &s->queue, next) {
-        if (p->finished) {
+        if (p->pending) {
             lsi_reselect(s, p);
             break;
         }
@@ -1560,7 +1600,8 @@ static void lsi_reg_writeb(LSIState *s, int offset, uint8_t val)
                 for (id = 0; id < s->bus.ndev; id++) {
                     if (s->bus.devs[id]) {
                         dev = &s->bus.devs[id]->qdev;
-                        dev->info->reset(dev);
+                        if (dev->info->reset)
+                            dev->info->reset(dev);
                     }
                 }
                 s->sstat0 |= LSI_SSTAT0_RST;
@@ -2162,10 +2203,6 @@ static PCIDeviceInfo lsi_info = {
     .qdev.vmsd  = &vmstate_lsi_scsi,
     .init       = lsi_scsi_init,
     .exit       = lsi_scsi_uninit,
-    .qdev.props = (Property[]) {
-        DEFINE_PROP_UINT32("disconnect", LSIState, enable_disconnect, 1),
-        DEFINE_PROP_END_OF_LIST(),
-    }
 };
 
 static void lsi53c895a_register_devices(void)
