From bbfce57ef4ce5b458beac64cefbc0b6cbb9f08b2 Mon Sep 17 00:00:00 2001
From: Heiher <r@hev.cc>
Date: Wed, 27 May 2015 00:19:03 +0800
Subject: [PATCH] PATA: Add data transfer by DMA.

---
 grub-core/disk/pata.c | 255 +++++++++++++++++++++++++++++++++++++++++---------
 1 file changed, 211 insertions(+), 44 deletions(-)

diff --git a/grub-core/disk/pata.c b/grub-core/disk/pata.c
index 23eef2b..d8c046c 100644
--- a/grub-core/disk/pata.c
+++ b/grub-core/disk/pata.c
@@ -32,10 +32,29 @@
 
 GRUB_MOD_LICENSE ("GPLv3+");
 
+#define GRUB_PATA_PRDT_MAX_COUNT 16
+#define GRUB_PATA_PRDT_MAX_CHUNK_LENGTH 0x10000
+
 /* At the moment, only two IDE ports are supported.  */
 static const grub_port_t grub_pata_ioaddress[] = { GRUB_ATA_CH0_PORT1,
 						   GRUB_ATA_CH1_PORT1 };
 
+enum
+{
+  GRUB_PATA_BM_COMMAND = 0,
+  GRUB_PATA_BM_STATUS  = 2,
+  GRUB_PATA_BM_PRDT    = 4,
+  GRUB_PATA_BM_SIZE    = 8,
+};
+
+struct grub_pata_prdt_entry
+{
+  grub_uint32_t paddr;
+  grub_uint16_t size;
+  grub_uint16_t _reserved : 15;
+  grub_uint16_t last : 1;
+} __attribute__ ((packed));
+
 struct grub_pata_device
 {
   /* IDE port to use.  */
@@ -45,6 +64,9 @@ struct grub_pata_device
      found.  */
   grub_port_t ioaddress;
 
+  /* Bus master DMA. */
+  grub_port_t bmaddress;
+
   /* Two devices can be connected to a single cable.  Use this field
      to select device 0 (commonly known as "master") or device 1
      (commonly known as "slave").  */
@@ -52,6 +74,9 @@ struct grub_pata_device
 
   int present;
 
+  void *dma_buffer;
+  struct grub_pata_prdt_entry *prdt_entries;
+
   struct grub_pata_device *next;
 };
 
@@ -69,6 +94,18 @@ grub_pata_regget (struct grub_pata_device *dev, int reg)
   return grub_inb (dev->ioaddress + reg);
 }
 
+static inline void
+grub_pata_dma_regset (struct grub_pata_device *dev, int reg, int val)
+{
+  grub_outb (val, dev->bmaddress + reg);
+}
+
+static inline grub_uint8_t
+grub_pata_dma_regget (struct grub_pata_device *dev, int reg)
+{
+  return grub_inb (dev->bmaddress + reg);
+}
+
 /* Wait for !BSY.  */
 static grub_err_t
 grub_pata_wait_not_busy (struct grub_pata_device *dev, int milliseconds)
@@ -97,6 +134,30 @@ grub_pata_wait_not_busy (struct grub_pata_device *dev, int milliseconds)
   return GRUB_ERR_NONE;
 }
 
+/* Wait for finish. */
+static grub_err_t
+grub_pata_dma_wait_finish (struct grub_pata_device *dev, int milliseconds)
+{
+  int i = 1;
+  grub_uint8_t sts;
+
+  grub_millisleep (1);
+  while ((sts = grub_pata_dma_regget (dev, GRUB_PATA_BM_STATUS)) & 1)
+    {
+      if (i >= milliseconds)
+        {
+	  grub_dprintf ("pata", "dma timeout: %dms, status=0x%x\n",
+			milliseconds, sts);
+	  return grub_error (GRUB_ERR_TIMEOUT, "PATA dma timeout");
+	}
+
+      grub_millisleep (1);
+      i++;
+    }
+
+  return GRUB_ERR_NONE;
+}
+
 static inline grub_err_t
 grub_pata_check_ready (struct grub_pata_device *dev, int spinup)
 {
@@ -146,41 +207,12 @@ grub_pata_pio_write (struct grub_pata_device *dev, char *buf, grub_size_t size)
     grub_outw(grub_cpu_to_ata16 (grub_get_unaligned16 (buf + 2 * i)), dev->ioaddress + GRUB_ATA_REG_DATA);
 }
 
-/* ATA pass through support, used by hdparm.mod.  */
 static grub_err_t
-grub_pata_readwrite (struct grub_ata *disk,
-		     struct grub_disk_ata_pass_through_parms *parms,
-		     int spinup)
+grub_pata_readwrite_pio (struct grub_ata *disk,
+		     struct grub_disk_ata_pass_through_parms *parms)
 {
   struct grub_pata_device *dev = (struct grub_pata_device *) disk->data;
   grub_size_t nread = 0;
-  int i;
-
-  if (! (parms->cmdsize == 0 || parms->cmdsize == 12))
-    return grub_error (GRUB_ERR_NOT_IMPLEMENTED_YET,
-		       "ATAPI non-12 byte commands not supported");
-
-  grub_dprintf ("pata", "pata_pass_through: cmd=0x%x, features=0x%x, sectors=0x%x\n",
-		parms->taskfile.cmd,
-		parms->taskfile.features,
-		parms->taskfile.sectors);
-  grub_dprintf ("pata", "lba_high=0x%x, lba_mid=0x%x, lba_low=0x%x, size=%"
-		PRIuGRUB_SIZE "\n",
-	        parms->taskfile.lba_high,
-	        parms->taskfile.lba_mid,
-	        parms->taskfile.lba_low, parms->size);
-
-  /* Set registers.  */
-  grub_pata_regset (dev, GRUB_ATA_REG_DISK, (dev->device << 4)
-		    | (parms->taskfile.disk & 0xef));
-  if (grub_pata_check_ready (dev, spinup))
-    return grub_errno;
-
-  for (i = GRUB_ATA_REG_SECTORS; i <= GRUB_ATA_REG_LBAHIGH; i++)
-    grub_pata_regset (dev, i,
-		     parms->taskfile.raw[7 + (i - GRUB_ATA_REG_SECTORS)]);
-  for (i = GRUB_ATA_REG_FEATURES; i <= GRUB_ATA_REG_LBAHIGH; i++)
-    grub_pata_regset (dev, i, parms->taskfile.raw[i - GRUB_ATA_REG_FEATURES]);
 
   /* Start command. */
   grub_pata_regset (dev, GRUB_ATA_REG_CMD, parms->taskfile.cmd);
@@ -264,6 +296,98 @@ grub_pata_readwrite (struct grub_ata *disk,
   if (grub_pata_wait_not_busy (dev, GRUB_ATA_TOUT_DATA))
     return grub_errno;
 
+  return GRUB_ERR_NONE;
+}
+
+static grub_err_t
+grub_pata_readwrite_dma (struct grub_ata *disk,
+		     struct grub_disk_ata_pass_through_parms *parms)
+{
+  struct grub_pata_device *dev = (struct grub_pata_device *) disk->data;
+  grub_uint8_t val;
+
+  if (parms->size > GRUB_PATA_PRDT_MAX_CHUNK_LENGTH * GRUB_PATA_PRDT_MAX_COUNT)
+    return grub_error (GRUB_ERR_BUG, "too big data buffer");
+
+  /* Copy data for write. */
+  if (parms->write)
+    grub_memcpy (dev->dma_buffer, parms->buffer, parms->size);
+
+  /* Clear bus master command and status. */
+  val = grub_pata_dma_regget (dev, GRUB_PATA_BM_COMMAND);
+  val = (val & ~9) | (parms->write ? 0 : 8);
+  grub_pata_dma_regset (dev, GRUB_PATA_BM_COMMAND, val);
+  val = grub_pata_dma_regget (dev, GRUB_PATA_BM_STATUS);
+  grub_pata_dma_regset (dev, GRUB_PATA_BM_STATUS, val & ~6);
+
+  /* Start command. */
+  grub_pata_regset (dev, GRUB_ATA_REG_CMD, parms->taskfile.cmd);
+  val = grub_pata_dma_regget (dev, GRUB_PATA_BM_COMMAND);
+  grub_pata_dma_regset (dev, GRUB_PATA_BM_COMMAND, val | 1);
+
+  /* Wait for finish. */
+  if (grub_pata_dma_wait_finish (dev, GRUB_ATA_TOUT_DATA))
+    return grub_errno;
+
+  /* Reset bus master start. */
+  val = grub_pata_dma_regget (dev, GRUB_PATA_BM_COMMAND);
+  grub_pata_dma_regset (dev, GRUB_PATA_BM_COMMAND, val & ~1);
+
+  /* Check status. */
+  grub_int8_t sts = grub_pata_regget (dev, GRUB_ATA_REG_STATUS);
+  grub_dprintf ("pata", "status=0x%x\n", sts);
+
+  /* Copy data for read. */
+  if (!parms->write)
+    grub_memcpy (parms->buffer, dev->dma_buffer, parms->size);
+
+  return GRUB_ERR_NONE;
+}
+
+/* ATA pass through support, used by hdparm.mod.  */
+static grub_err_t
+grub_pata_readwrite (struct grub_ata *disk,
+		     struct grub_disk_ata_pass_through_parms *parms,
+		     int spinup)
+{
+  struct grub_pata_device *dev = (struct grub_pata_device *) disk->data;
+  grub_err_t err;
+  int i;
+
+  if (! (parms->cmdsize == 0 || parms->cmdsize == 12))
+    return grub_error (GRUB_ERR_NOT_IMPLEMENTED_YET,
+		       "ATAPI non-12 byte commands not supported");
+
+  grub_dprintf ("pata", "pata_pass_through: cmd=0x%x, features=0x%x, sectors=0x%x\n",
+		parms->taskfile.cmd,
+		parms->taskfile.features,
+		parms->taskfile.sectors);
+  grub_dprintf ("pata", "lba_high=0x%x, lba_mid=0x%x, lba_low=0x%x, size=%"
+		PRIuGRUB_SIZE "\n",
+	        parms->taskfile.lba_high,
+	        parms->taskfile.lba_mid,
+	        parms->taskfile.lba_low, parms->size);
+
+  /* Set registers.  */
+  grub_pata_regset (dev, GRUB_ATA_REG_DISK, (dev->device << 4)
+		    | (parms->taskfile.disk & 0xef));
+  if (grub_pata_check_ready (dev, spinup))
+    return grub_errno;
+
+  for (i = GRUB_ATA_REG_SECTORS; i <= GRUB_ATA_REG_LBAHIGH; i++)
+    grub_pata_regset (dev, i,
+		     parms->taskfile.raw[7 + (i - GRUB_ATA_REG_SECTORS)]);
+  for (i = GRUB_ATA_REG_FEATURES; i <= GRUB_ATA_REG_LBAHIGH; i++)
+    grub_pata_regset (dev, i, parms->taskfile.raw[i - GRUB_ATA_REG_FEATURES]);
+
+  /* Call real read write. */
+  if (parms->dma)
+    err = grub_pata_readwrite_dma (disk, parms);
+  else
+    err = grub_pata_readwrite_pio (disk, parms);
+  if (GRUB_ERR_NONE != err)
+    return err;
+
   /* Return registers.  */
   for (i = GRUB_ATA_REG_ERROR; i <= GRUB_ATA_REG_STATUS; i++)
     parms->taskfile.raw[i - GRUB_ATA_REG_FEATURES] = grub_pata_regget (dev, i);
@@ -306,14 +430,14 @@ check_device (struct grub_pata_device *dev)
 }
 
 static grub_err_t
-grub_pata_device_initialize (int port, int device, int addr)
+grub_pata_device_initialize (int port, int device, int ioaddr, int bmaddr)
 {
   struct grub_pata_device *dev;
   struct grub_pata_device **devp;
   grub_err_t err;
 
-  grub_dprintf ("pata", "detecting device %d,%d (0x%x)\n",
-		port, device, addr);
+  grub_dprintf ("pata", "detecting device %d,%d (0x%x, 0x%x)\n",
+		port, device, ioaddr, bmaddr);
 
   dev = grub_malloc (sizeof(*dev));
   if (! dev)
@@ -322,7 +446,8 @@ grub_pata_device_initialize (int port, int device, int addr)
   /* Setup the device information.  */
   dev->port = port;
   dev->device = device;
-  dev->ioaddress = addr + GRUB_MACHINE_PCI_IO_BASE;
+  dev->ioaddress = ioaddr + GRUB_MACHINE_PCI_IO_BASE;
+  dev->bmaddress = bmaddr ? bmaddr + GRUB_MACHINE_PCI_IO_BASE : 0;
   dev->present = 1;
   dev->next = NULL;
 
@@ -348,7 +473,8 @@ grub_pata_pciinit (grub_pci_device_t dev,
   grub_uint32_t class;
   grub_uint32_t bar1;
   grub_uint32_t bar2;
-  int rega;
+  grub_uint32_t bar4;
+  int rega, regb;
   int i;
   static int controller = 0;
   int cs5536 = 0;
@@ -381,6 +507,7 @@ grub_pata_pciinit (grub_pci_device_t dev,
 	compat = (class >> (8 + 2 * i)) & 1;
 
       rega = 0;
+      regb = 0;
 
       /* If the channel is in compatibility mode, just assign the
 	 default registers.  */
@@ -400,6 +527,10 @@ grub_pata_pciinit (grub_pci_device_t dev,
 					+ sizeof (grub_uint64_t) * i
 					+ sizeof (grub_uint32_t));
 	  bar2 = grub_pci_read (addr);
+	  addr = grub_pci_make_address (dev, GRUB_PCI_REG_ADDRESS_REG4);
+	  bar4 = grub_pci_read (addr);
+	  if ((bar4 & 1) && (bar4 & GRUB_PCI_ADDR_IO_MASK))
+	    regb = (bar4 & GRUB_PCI_ADDR_IO_MASK) + i * 8;
 
 	  /* Check if the BARs describe an IO region.  */
 	  if ((bar1 & 1) && (bar2 & 1) && (bar1 & ~3))
@@ -415,14 +546,14 @@ grub_pata_pciinit (grub_pci_device_t dev,
 	}
 
       grub_dprintf ("pata",
-		    "PCI dev (%d,%d,%d) compat=%d rega=0x%x\n",
+		    "PCI dev (%d,%d,%d) compat=%d rega=0x%x regb=%x\n",
 		    grub_pci_get_bus (dev), grub_pci_get_device (dev),
-		    grub_pci_get_function (dev), compat, rega);
+		    grub_pci_get_function (dev), compat, rega, regb);
 
       if (rega)
 	{
 	  grub_errno = GRUB_ERR_NONE;
-	  grub_pata_device_initialize (controller * 2 + i, 0, rega);
+	  grub_pata_device_initialize (controller * 2 + i, 0, rega, regb);
 
 	  /* Most errors raised by grub_ata_device_initialize() are harmless.
 	     They just indicate this particular drive is not responding, most
@@ -434,7 +565,7 @@ grub_pata_pciinit (grub_pci_device_t dev,
 	      grub_errno = GRUB_ERR_NONE;
 	    }
 
-	  grub_pata_device_initialize (controller * 2 + i, 1, rega);
+	  grub_pata_device_initialize (controller * 2 + i, 1, rega, regb);
 
 	  /* Likewise.  */
 	  if (grub_errno)
@@ -463,8 +594,8 @@ grub_pata_initialize (void)
   int i;
   for (i = 0; i < 2; i++)
     {
-      grub_pata_device_initialize (i, 0, grub_pata_ioaddress[i]);
-      grub_pata_device_initialize (i, 1, grub_pata_ioaddress[i]);
+      grub_pata_device_initialize (i, 0, grub_pata_ioaddress[i], 0);
+      grub_pata_device_initialize (i, 1, grub_pata_ioaddress[i], 0);
     }
   return 0;
 }
@@ -499,10 +630,46 @@ grub_pata_open (int id, int devnum, struct grub_ata *ata)
     return err;
 
   ata->data = devfnd;
-  ata->dma = 0;
-  ata->maxbuffer = 256 * 512;
+  ata->dma = devfnd->bmaddress ? 1 : 0;
+  ata->maxbuffer = ata->dma ? GRUB_PATA_PRDT_MAX_CHUNK_LENGTH * GRUB_PATA_PRDT_MAX_COUNT :
+                                256 * 512;
   ata->present = &devfnd->present;
 
+  if (ata->dma)
+    {
+      int i;
+      struct grub_pci_dma_chunk *prdt_entries, *dma_buffer;
+
+      prdt_entries = grub_memalign_dma32 (1024, sizeof (struct grub_pata_prdt_entry) *
+                                          GRUB_PATA_PRDT_MAX_COUNT);
+      if (!prdt_entries)
+        grub_error (GRUB_ERR_OUT_OF_MEMORY, "Failed alloc PRDT entries");
+      grub_outl (grub_dma_get_phys (prdt_entries), devfnd->bmaddress + GRUB_PATA_BM_PRDT);
+      devfnd->prdt_entries = (void *) grub_dma_get_virt (prdt_entries);
+
+      dma_buffer = grub_memalign_dma32 (1024, ata->maxbuffer);
+      if (!dma_buffer)
+        grub_error (GRUB_ERR_OUT_OF_MEMORY, "Failed alloc dma buffer");
+      devfnd->dma_buffer = (void *) grub_dma_get_virt (dma_buffer);
+      for (i = 0; i < GRUB_PATA_PRDT_MAX_COUNT; i ++)
+        {
+          devfnd->prdt_entries[i].paddr = grub_dma_get_phys (dma_buffer) +
+                                       i * GRUB_PATA_PRDT_MAX_CHUNK_LENGTH;
+          devfnd->prdt_entries[i].size = 0;
+          devfnd->prdt_entries[i]._reserved = 0;
+          devfnd->prdt_entries[i].last = 0;
+        }
+      devfnd->prdt_entries[GRUB_PATA_PRDT_MAX_COUNT - 1].last = 1;
+
+      /* Select DMA mode: UDMA 2. */
+      grub_pata_regset (devfnd, GRUB_ATA_REG_DISK, devfnd->device << 4);
+      grub_pata_regset (devfnd, GRUB_ATA_REG_FEATURES, 0x3);
+      grub_pata_regset (devfnd, GRUB_ATAPI_REG_IREASON, 0x42);
+      grub_pata_regset (devfnd, GRUB_ATA_REG_CMD, GRUB_ATA_CMD_SET_FEATURES);
+      if (grub_pata_wait_not_busy (devfnd, GRUB_ATA_TOUT_STD))
+        grub_error (GRUB_ERR_UNKNOWN_DEVICE, "Failed select DMA mode");
+    }
+
   return GRUB_ERR_NONE;
 }
 
-- 
2.4.2

