diff --git a/hw/scsi/esp.c b/hw/scsi/esp.c
index 7ab195f884..81144ace83 100644
--- a/hw/scsi/esp.c
+++ b/hw/scsi/esp.c
@@ -801,6 +801,23 @@ static void esp_do_nodma(ESPState *s)
 
     switch (esp_get_phase(s)) {
     case STAT_MO:
+        /* Copy FIFO into cmdfifo */
+        n = esp_fifo_pop_buf(&s->fifo, buf, fifo8_num_used(&s->fifo));
+        n = MIN(fifo8_num_free(&s->cmdfifo), n);
+        fifo8_push_all(&s->cmdfifo, buf, n);
+        s->cmdfifo_cdb_offset += n;
+
+        /*
+         * Extra message out bytes received: update cmdfifo_cdb_offset
+         * and then switch to command phase
+         */
+        s->cmdfifo_cdb_offset = fifo8_num_used(&s->cmdfifo);
+        esp_set_phase(s, STAT_CD);
+        s->rregs[ESP_RSEQ] = SEQ_CD;
+        s->rregs[ESP_RINTR] |= INTR_BS;
+        esp_raise_irq(s);
+        break;
+
     case STAT_CD:
         /* Copy FIFO into cmdfifo */
         n = esp_fifo_pop_buf(&s->fifo, buf, fifo8_num_used(&s->fifo));
@@ -810,25 +827,14 @@ static void esp_do_nodma(ESPState *s)
         cmdlen = fifo8_num_used(&s->cmdfifo);
         trace_esp_handle_ti_cmd(cmdlen);
         s->ti_size = 0;
-        if (esp_get_phase(s) == STAT_CD) {
-            /* No command received */
-            if (s->cmdfifo_cdb_offset == fifo8_num_used(&s->cmdfifo)) {
-                return;
-            }
 
-            /* Command has been received */
-            do_cmd(s);
-        } else {
-            /*
-             * Extra message out bytes received: update cmdfifo_cdb_offset
-             * and then switch to command phase
-             */
-            s->cmdfifo_cdb_offset = fifo8_num_used(&s->cmdfifo);
-            esp_set_phase(s, STAT_CD);
-            s->rregs[ESP_RSEQ] = SEQ_CD;
-            s->rregs[ESP_RINTR] |= INTR_BS;
-            esp_raise_irq(s);
+        /* No command received */
+        if (s->cmdfifo_cdb_offset == fifo8_num_used(&s->cmdfifo)) {
+            return;
         }
+
+        /* Command has been received */
+        do_cmd(s);
         break;
 
     case STAT_DO: