ahci: handle unaligned buffers.

[seabios.git] / src / ahci.c

diff --git a/src/ahci.c b/src/ahci.c
index 9ff13243ed2c2c276be628011d43113ba2b7c423..e2ec07a02215bde6cc05cdd03f92c280dcfbf881 100644 (file)
--- a/src/ahci.c
+++ b/src/ahci.c
@@ -22,6 +22,7 @@
 /****************************************************************
  * these bits must run in both 16bit and 32bit modes
  ****************************************************************/
+u8 *ahci_buf_fl VAR16VISIBLE;
 
 // prepare sata command fis
 static void sata_prep_simple(struct sata_cmd_fis *fis, u8 command)
@@ -105,7 +106,7 @@ static void ahci_port_writel(struct ahci_ctrl_s *ctrl, u32 pnr, u32 reg, u32 val
 static int ahci_command(struct ahci_port_s *port, int iswrite, int isatapi,
                         void *buffer, u32 bsize)
 {
-    u32 val, status, success, flags, intbits;
+    u32 val, status, success, flags, intbits, error;
     struct ahci_ctrl_s *ctrl = GET_GLOBAL(port->ctrl);
     struct ahci_cmd_s  *cmd  = GET_GLOBAL(port->cmd);
     struct ahci_fis_s  *fis  = GET_GLOBAL(port->fis);
@@ -141,10 +142,12 @@ static int ahci_command(struct ahci_port_s *port, int iswrite, int isatapi,
                 ahci_port_writel(ctrl, pnr, PORT_IRQ_STAT, intbits);
                 if (intbits & 0x02) {
                     status = GET_FLATPTR(fis->psfis[2]);
+                    error  = GET_FLATPTR(fis->psfis[3]);
                     break;
                 }
                 if (intbits & 0x01) {
                     status = GET_FLATPTR(fis->rfis[2]);
+                    error  = GET_FLATPTR(fis->rfis[3]);
                     break;
                 }
             }
@@ -157,8 +160,50 @@ static int ahci_command(struct ahci_port_s *port, int iswrite, int isatapi,
     success = (0x00 == (status & (ATA_CB_STAT_BSY | ATA_CB_STAT_DF |
                                   ATA_CB_STAT_DRQ | ATA_CB_STAT_ERR)) &&
                ATA_CB_STAT_RDY == (status & (ATA_CB_STAT_RDY)));
-    dprintf(2, "AHCI/%d: ... finished, status 0x%x, %s\n", pnr,
-            status, success ? "OK" : "ERROR");
+    if (success) {
+        dprintf(2, "AHCI/%d: ... finished, status 0x%x, OK\n", pnr,
+                status);
+    } else {
+        dprintf(2, "AHCI/%d: ... finished, status 0x%x, ERROR 0x%x\n", pnr,
+                status, error);
+
+        // non-queued error recovery (AHCI 1.3 section 6.2.2.1)
+        // Clears PxCMD.ST to 0 to reset the PxCI register
+        val = ahci_port_readl(ctrl, pnr, PORT_CMD);
+        ahci_port_writel(ctrl, pnr, PORT_CMD, val & ~PORT_CMD_START);
+
+        // waits for PxCMD.CR to clear to 0
+        while (1) {
+            val = ahci_port_readl(ctrl, pnr, PORT_CMD);
+            if ((val & PORT_CMD_LIST_ON) == 0)
+                break;
+            yield();
+        }
+
+        // Clears any error bits in PxSERR to enable capturing new errors
+        val = ahci_port_readl(ctrl, pnr, PORT_SCR_ERR);
+        ahci_port_writel(ctrl, pnr, PORT_SCR_ERR, val);
+
+        // Clears status bits in PxIS as appropriate
+        val = ahci_port_readl(ctrl, pnr, PORT_IRQ_STAT);
+        ahci_port_writel(ctrl, pnr, PORT_IRQ_STAT, val);
+
+        // If PxTFD.STS.BSY or PxTFD.STS.DRQ is set to 1, issue
+        // a COMRESET to the device to put it in an idle state
+        val = ahci_port_readl(ctrl, pnr, PORT_TFDATA);
+        if (val & (ATA_CB_STAT_BSY | ATA_CB_STAT_DRQ)) {
+            dprintf(2, "AHCI/%d: issue comreset\n", pnr);
+            val = ahci_port_readl(ctrl, pnr, PORT_SCR_CTL);
+            // set Device Detection Initialization (DET) to 1 for 1 ms for comreset
+            ahci_port_writel(ctrl, pnr, PORT_SCR_CTL, val | 1);
+            mdelay (1);
+            ahci_port_writel(ctrl, pnr, PORT_SCR_CTL, val);
+        }
+
+        // Sets PxCMD.ST to 1 to enable issuing new commands
+        val = ahci_port_readl(ctrl, pnr, PORT_CMD);
+        ahci_port_writel(ctrl, pnr, PORT_CMD, val | PORT_CMD_START);
+    }
     return success ? 0 : -1;
 }
 
@@ -186,9 +231,9 @@ int ahci_cmd_data(struct disk_op_s *op, void *cdbcmd, u16 blocksize)
     return DISK_RET_SUCCESS;
 }
 
-// read/write count blocks from a harddrive.
+// read/write count blocks from a harddrive, op->buf_fl must be word aligned
 static int
-ahci_disk_readwrite(struct disk_op_s *op, int iswrite)
+ahci_disk_readwrite_aligned(struct disk_op_s *op, int iswrite)
 {
     struct ahci_port_s *port = container_of(
         op->drive_g, struct ahci_port_s, drive);
@@ -205,6 +250,47 @@ ahci_disk_readwrite(struct disk_op_s *op, int iswrite)
     return DISK_RET_SUCCESS;
 }
 
+// read/write count blocks from a harddrive.
+static int
+ahci_disk_readwrite(struct disk_op_s *op, int iswrite)
+{
+    // if caller's buffer is word aligned, use it directly
+    if (((u32) op->buf_fl & 1) == 0)
+        return ahci_disk_readwrite_aligned(op, iswrite);
+
+    // Use a word aligned buffer for AHCI I/O
+    int rc;
+    struct disk_op_s localop = *op;
+    u8 *alignedbuf_fl = GET_GLOBAL(ahci_buf_fl);
+    u8 *position = op->buf_fl;
+
+    localop.buf_fl = alignedbuf_fl;
+    localop.count = 1;
+
+    if (iswrite) {
+        u16 block;
+        for (block = 0; block < op->count; block++) {
+            memcpy_fl (alignedbuf_fl, position, DISK_SECTOR_SIZE);
+            rc = ahci_disk_readwrite_aligned (&localop, 1);
+            if (rc)
+                return rc;
+            position += DISK_SECTOR_SIZE;
+            localop.lba++;
+        }
+    } else { // read
+        u16 block;
+        for (block = 0; block < op->count; block++) {
+            rc = ahci_disk_readwrite_aligned (&localop, 0);
+            if (rc)
+                return rc;
+            memcpy_fl (position, alignedbuf_fl, DISK_SECTOR_SIZE);
+            position += DISK_SECTOR_SIZE;
+            localop.lba++;
+        }
+    }
+    return DISK_RET_SUCCESS;
+}
+
 // command demuxer
 int process_ahci_op(struct disk_op_s *op)
 {
@@ -449,6 +535,13 @@ ahci_init_controller(int bdf)
         warn_noalloc();
         return;
     }
+
+    ahci_buf_fl = malloc_low(DISK_SECTOR_SIZE);
+    if (!ahci_buf_fl) {
+        warn_noalloc();
+        return;
+    }
+
     ctrl->pci_bdf = bdf;
     ctrl->iobase = pci_config_readl(bdf, PCI_BASE_ADDRESS_5);
     ctrl->irq = pci_config_readb(bdf, PCI_INTERRUPT_LINE);