2 files changed, 16 insertions, 3 deletions

diff --git a/drivers/ata/ahci.h b/drivers/ata/ahci.h
index 1a2aacfdebc5..329cbbb91284 100644
--- a/drivers/ata/ahci.h
+++ b/drivers/ata/ahci.h
@@ -72,6 +72,7 @@ enum {
         AHCI_CMD_RESET          = (1 << 8),
         AHCI_CMD_CLR_BUSY       = (1 << 10),
 
+        RX_FIS_PIO_SETUP        = 0x20, /* offset of PIO Setup FIS data */
         RX_FIS_D2H_REG          = 0x40, /* offset of D2H Register FIS data */
         RX_FIS_SDB              = 0x58, /* offset of SDB FIS data */
         RX_FIS_UNK              = 0x60, /* offset of Unknown FIS data */
diff --git a/drivers/ata/libahci.c b/drivers/ata/libahci.c
index 524dbe8be163..ebc08d65b3dd 100644
--- a/drivers/ata/libahci.c
+++ b/drivers/ata/libahci.c
@@ -1752,12 +1752,24 @@ static unsigned int ahci_qc_issue(struct ata_queued_cmd *qc)
 static bool ahci_qc_fill_rtf(struct ata_queued_cmd *qc)
 {
         struct ahci_port_priv *pp = qc->ap->private_data;
-        u8 *d2h_fis = pp->rx_fis + RX_FIS_D2H_REG;
+        u8 *rx_fis = pp->rx_fis;
 
         if (pp->fbs_enabled)
-                d2h_fis += qc->dev->link->pmp * AHCI_RX_FIS_SZ;
+                rx_fis += qc->dev->link->pmp * AHCI_RX_FIS_SZ;
+
+        /*
+         * After a successful execution of an ATA PIO data-in command,
+         * the device doesn't send D2H Reg FIS to update the TF and
+         * the host should take TF and E_Status from the preceding PIO
+         * Setup FIS.
+         */
+        if (qc->tf.protocol == ATA_PROT_PIO && qc->dma_dir == DMA_FROM_DEVICE &&
+            !(qc->flags & ATA_QCFLAG_FAILED)) {
+                ata_tf_from_fis(rx_fis + RX_FIS_PIO_SETUP, &qc->result_tf);
+                qc->result_tf.command = (rx_fis + RX_FIS_PIO_SETUP)[15];
+        } else
+                ata_tf_from_fis(rx_fis + RX_FIS_D2H_REG, &qc->result_tf);
 
-        ata_tf_from_fis(d2h_fis, &qc->result_tf);
         return true;
 }