[PATCH] ahci: set transfer mode according to the capabilities of connected drive

[Gerd Hoffmann]

Use case: cf cards behind sata-ide bridge, which might not support
the default transfer mode.

Based on a patch by Werner Zeh ,
with some minor tweaks applied.

Signed-off-by: Gerd Hoffmann 
---
 src/hw/ahci.c | 58 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 src/hw/ata.h  |  5 +++++
 2 files changed, 63 insertions(+)

diff --git a/src/hw/ahci.c b/src/hw/ahci.c
index 9310850..ea862f2 100644
--- a/src/hw/ahci.c
+++ b/src/hw/ahci.c
@@ -515,6 +515,64 @@ static int ahci_port_setup(struct ahci_port_s *port)
                               , ata_extract_version(buffer)
                               , (u32)adjsize, adjprefix);
         port->prio = bootprio_find_ata_device(ctrl->pci_tmp, pnr, 0);
+
+        s8 multi_dma = -1;
+        s8 pio_mode = -1;
+        s8 udma_mode = -1;
+        // If bit 2 in word 53 is set, udma information is valid in word 88.
+        if (buffer[53] & 0x04) {
+                udma_mode = 6;
+                while ((udma_mode >= 0) &&
+                        !((buffer[88] & 0x7f) & ( 1 << udma_mode ))) {
+                        udma_mode--;
+                }
+        }
+        // If bit 1 in word 53 is set, multiword-dma and advanced pio modes
+        // are available in words 63 and 64.
+        if (buffer[53] & 0x02) {
+                pio_mode = 4;
+                multi_dma = 3;
+                while ((multi_dma >= 0) &&
+                       !((buffer[63] & 0x7) & ( 1 << multi_dma ))) {
+                        multi_dma--;
+                }
+                while ((pio_mode >= 3) &&
+                       !((buffer[64] & 0x3) & ( 1 << ( pio_mode - 3 ) ))) {
+                        pio_mode--;
+                }
+
+        }
+        dprintf(2, "AHCI/%d: supported modes: udma %d, multi-dma %d, pio %d\n",
+                port->pnr, udma_mode, multi_dma, pio_mode);
+
+        sata_prep_simple(&port->cmd->fis, ATA_CMD_SET_FEATURES);
+        port->cmd->fis.feature = ATA_SET_FEATRUE_TRANSFER_MODE;
+        // Select used mode. UDMA first, then Multi-DMA followed by
+        // advanced PIO modes 3 or 4. If non, set default PIO.
+        if (udma_mode >= 0) {
+            dprintf(1, "AHCI/%d: Set transfer mode to UDMA-%d\n",
+                        port->pnr, udma_mode);
+            port->cmd->fis.sector_count = ATA_TRANSFER_MODE_ULTRA_DMA
+                                          | udma_mode;
+        } else if (multi_dma >= 0) {
+                dprintf(1, "AHCI/%d: Set transfer mode to Multi-DMA-%d\n",
+                            port->pnr, multi_dma);
+                port->cmd->fis.sector_count = ATA_TRANSFER_MODE_MULTIWORD_DMA
+                                              | multi_dma;
+        } else if (pio_mode >= 3) {
+                dprintf(1, "AHCI/%d: Set transfer mode to PIO-%d\n",
+                            port->pnr, pio_mode);
+                port->cmd->fis.sector_count = ATA_TRANSFER_MODE_PIO_FLOW_CTRL
+                                              | pio_mode;
+        } else {
+                dprintf(1, "AHCI/%d: Set transfer mode to default PIO\n",
+                            port->pnr);
+                port->cmd->fis.sector_count = ATA_TRANSFER_MODE_DEFAULT_PIO;
+        }
+        rc = ahci_command(port, 1, 0, 0, 0);
+        if (rc < 0) {
+                dprintf(1, "AHCI/%d: Set transfer mode failed.\n", port->pnr);
+        }
     } else {
         // found cdrom (atapi)
         port->drive.type = DTYPE_AHCI_ATAPI;
diff --git a/src/hw/ata.h b/src/hw/ata.h
index cd14e59..9de2490 100644
--- a/src/hw/ata.h
+++ b/src/hw/ata.h
@@ -155,4 +155,9 @@ void ata_setup(void);
 #define ATA_CMD_READ_NATIVE_MAX_ADDRESS      0xF8
 #define ATA_CMD_SET_MAX                      0xF9

+#define ATA_SET_FEATRUE_TRANSFER_MODE        0x03
+#define ATA_TRANSFER_MODE_ULTRA_DMA          0x40
+#define ATA_TRANSFER_MODE_MULTIWORD_DMA      0x20
+#define ATA_TRANSFER_MODE_PIO_FLOW_CTRL      0x08
+#define ATA_TRANSFER_MODE_DEFAULT_PIO        0x00
 #endif // ata.h