[Qemu-devel] [PATCH v2 17/30] ahci: construct PIO Setup FIS for PIO commands

[John Snow <jsnow@redhat.com>]

From: Paolo Bonzini <pbonzini@redhat.com>

PIO commands should put a PIO Setup FIS in the receive area when data
transfer ends.  Currently QEMU does not do this and only places the
D2H FIS at the end of the operation.

Signed-off-by: Paolo Bonzini <pbonzini@redhat.com>
Signed-off-by: John Snow <jsnow@redhat.com>
---
 hw/ide/ahci.c | 70 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 70 insertions(+)

diff --git a/hw/ide/ahci.c b/hw/ide/ahci.c
index b40ec06..4cda0d0 100644
--- a/hw/ide/ahci.c
+++ b/hw/ide/ahci.c
@@ -587,6 +587,71 @@ static void ahci_write_fis_sdb(AHCIState *s, int port, uint32_t finished)
     ahci_trigger_irq(s, &s->dev[port], PORT_IRQ_SDB_FIS);
 }
 
+static void ahci_write_fis_pio(AHCIDevice *ad, uint16_t len)
+{
+    AHCIPortRegs *pr = &ad->port_regs;
+    uint8_t *pio_fis, *cmd_fis;
+    uint64_t tbl_addr;
+    dma_addr_t cmd_len = 0x80;
+
+    if (!ad->res_fis || !(pr->cmd & PORT_CMD_FIS_RX)) {
+        return;
+    }
+
+    /* map cmd_fis */
+    tbl_addr = le64_to_cpu(ad->cur_cmd->tbl_addr);
+    cmd_fis = dma_memory_map(ad->hba->as, tbl_addr, &cmd_len,
+                             DMA_DIRECTION_TO_DEVICE);
+
+    if (cmd_fis == NULL) {
+        DPRINTF(ad->port_no, "dma_memory_map failed in ahci_write_fis_pio");
+        ahci_trigger_irq(ad->hba, ad, PORT_IRQ_HBUS_ERR);
+        return;
+    }
+
+    if (cmd_len != 0x80) {
+        DPRINTF(ad->port_no,
+                "dma_memory_map mapped too few bytes in ahci_write_fis_pio");
+        dma_memory_unmap(ad->hba->as, cmd_fis, cmd_len,
+                         DMA_DIRECTION_TO_DEVICE, cmd_len);
+        ahci_trigger_irq(ad->hba, ad, PORT_IRQ_HBUS_ERR);
+        return;
+    }
+
+    pio_fis = &ad->res_fis[RES_FIS_PSFIS];
+
+    pio_fis[0] = 0x5f;
+    pio_fis[1] = (ad->hba->control_regs.irqstatus ? (1 << 6) : 0);
+    pio_fis[2] = ad->port.ifs[0].status;
+    pio_fis[3] = ad->port.ifs[0].error;
+
+    pio_fis[4] = cmd_fis[4];
+    pio_fis[5] = cmd_fis[5];
+    pio_fis[6] = cmd_fis[6];
+    pio_fis[7] = cmd_fis[7];
+    pio_fis[8] = cmd_fis[8];
+    pio_fis[9] = cmd_fis[9];
+    pio_fis[10] = cmd_fis[10];
+    pio_fis[11] = cmd_fis[11];
+    pio_fis[12] = cmd_fis[12];
+    pio_fis[13] = cmd_fis[13];
+    pio_fis[14] = 0;
+    pio_fis[15] = ad->port.ifs[0].status;
+    pio_fis[16] = len & 255;
+    pio_fis[17] = len >> 8;
+    pio_fis[18] = 0;
+    pio_fis[19] = 0;
+
+    if (pio_fis[2] & ERR_STAT) {
+        ahci_trigger_irq(ad->hba, ad, PORT_IRQ_TF_ERR);
+    }
+
+    ahci_trigger_irq(ad->hba, ad, PORT_IRQ_PIOS_FIS);
+
+    dma_memory_unmap(ad->hba->as, cmd_fis, cmd_len,
+                     DMA_DIRECTION_TO_DEVICE, cmd_len);
+}
+
 static void ahci_write_fis_d2h(AHCIDevice *ad, uint8_t *cmd_fis)
 {
     AHCIPortRegs *pr = &ad->port_regs;
@@ -1031,6 +1096,11 @@ out:
     }
 
     s->end_transfer_func(s);
+
+    if (!(s->status & DRQ_STAT)) {
+        /* done with PIO send/receive */
+        ahci_write_fis_pio(ad, le32_to_cpu(ad->cur_cmd->status));
+    }
 }
 
 static void ahci_start_dma(IDEDMA *dma, IDEState *s,
-- 
1.9.3