esp.c: copy logic for do_cmd transfers from do_dma_pdma_cb() to esp_do_dma()

This is so that PDMA transfers can be performend by esp_do_dma() as well as
do_dma_pdma_cb().

Signed-off-by: Mark Cave-Ayland <mark.cave-ayland@ilande.co.uk>
Tested-by: Helge Deller <deller@gmx.de>
Tested-by: Thomas Huth <thuth@redhat.com>
Message-Id: <20240112125420.514425-34-mark.cave-ayland@ilande.co.uk>
Signed-off-by: Mark Cave-Ayland <mark.cave-ayland@ilande.co.uk>

1 files changed, 10 insertions, 1 deletions

diff --git a/hw/scsi/esp.c b/hw/scsi/esp.c
index 65b4baa..14284ef 100644
--- a/hw/scsi/esp.c
+++ b/hw/scsi/esp.c
@@ -656,9 +656,18 @@ static void esp_do_dma(ESPState *s)
             fifo8_push_all(&s->cmdfifo, buf, len);
             esp_set_tc(s, esp_get_tc(s) - len);
         } else {
+            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);
+            esp_set_tc(s, esp_get_tc(s) - n);
+
             esp_set_pdma_cb(s, DO_DMA_PDMA_CB);
             esp_raise_drq(s);
-            return;
+
+            /* Ensure we have received complete command after SATN and stop */
+            if (esp_get_tc(s) || fifo8_is_empty(&s->cmdfifo)) {
+                return;
+            }
         }
         trace_esp_handle_ti_cmd(cmdlen);
         s->ti_size = 0;