From a73da6555e9cec2a8be74e6df4c65b91a0beccf1 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 | 135 ++++++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 119 insertions(+), 16 deletions(-)

diff --git a/grub-core/disk/pata.c b/grub-core/disk/pata.c
index 23eef2b..7730a21 100644
--- a/grub-core/disk/pata.c
+++ b/grub-core/disk/pata.c
@@ -32,9 +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 };
+static const grub_port_t grub_pata_bmaddress[] = { 0xc040, 0xc048 };
+
+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
 {
@@ -45,6 +65,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 +75,9 @@ struct grub_pata_device
 
   int present;
 
+  void *dma_buffer;
+  struct grub_pata_prdt_entry *prdt_entries;
+
   struct grub_pata_device *next;
 };
 
@@ -182,9 +208,30 @@ grub_pata_readwrite (struct grub_ata *disk,
   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]);
 
+  if (parms->dma)
+    {
+      unsigned char 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");
+      if (parms->write)
+        grub_memcpy (dev->dma_buffer, parms->buffer, parms->size);
+      val = grub_inb (dev->bmaddress + GRUB_PATA_BM_COMMAND);
+      val = (val & ~9) | (parms->write ? 0 : 8);
+      grub_outb (val, dev->bmaddress + GRUB_PATA_BM_COMMAND);
+      val = grub_inb (dev->bmaddress + GRUB_PATA_BM_STATUS);
+      grub_outb (val & ~6, dev->bmaddress + GRUB_PATA_BM_STATUS);
+    }
+
   /* Start command. */
   grub_pata_regset (dev, GRUB_ATA_REG_CMD, parms->taskfile.cmd);
 
+  if (parms->dma)
+    {
+      unsigned char val;
+      val = grub_inb (dev->bmaddress + GRUB_PATA_BM_COMMAND);
+      grub_outb (val | 1, dev->bmaddress + GRUB_PATA_BM_COMMAND);
+    }
+
   /* Wait for !BSY.  */
   if (grub_pata_wait_not_busy (dev, GRUB_ATA_TOUT_DATA))
     return grub_errno;
@@ -210,7 +257,7 @@ grub_pata_readwrite (struct grub_ata *disk,
     }
 
   /* Transfer data.  */
-  while (nread < parms->size
+  while (!parms->dma && nread < parms->size
 	 && (sts & (GRUB_ATA_STATUS_DRQ | GRUB_ATA_STATUS_ERR))
 	 == GRUB_ATA_STATUS_DRQ)
     {
@@ -248,6 +295,12 @@ grub_pata_readwrite (struct grub_ata *disk,
 
       nread += cnt;
     }
+  if (parms->dma)
+    {
+      if (!parms->write)
+        grub_memcpy (parms->buffer, dev->dma_buffer, parms->size);
+      nread = parms->size;
+    }
   if (parms->write)
     {
       /* Check for write error.  */
@@ -306,14 +359,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 +375,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 + GRUB_MACHINE_PCI_IO_BASE;
   dev->present = 1;
   dev->next = NULL;
 
@@ -348,7 +402,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,18 +436,27 @@ 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.  */
       if (compat == 0 && !compat_use[i])
 	{
 	  rega = grub_pata_ioaddress[i];
+	  regb = grub_pata_bmaddress[i];
 	  compat_use[i] = 1;
+
+	  /* Make sure I/O, memory and bus master are enabled. */
+	  addr = grub_pci_make_address (dev, GRUB_PCI_REG_COMMAND);
+	  grub_pci_write_word (addr, grub_pci_read_word (addr)
+				| GRUB_PCI_COMMAND_IO_ENABLED
+				| GRUB_PCI_COMMAND_MEM_ENABLED
+				| GRUB_PCI_COMMAND_BUS_MASTER);
 	}
       else if (compat)
 	{
 	  /* Read the BARs, which either contain a mmapped IO address
-	     or the IO port address.  */
+	     or the IO port address and bus master address.  */
 	  addr = grub_pci_make_address (dev, GRUB_PCI_REG_ADDRESSES
 					+ sizeof (grub_uint64_t) * i);
 	  bar1 = grub_pci_read (addr);
@@ -400,9 +464,12 @@ 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);
+	  regb = (bar4 & GRUB_PCI_ADDR_IO_MASK) + i * 8;
 
 	  /* Check if the BARs describe an IO region.  */
-	  if ((bar1 & 1) && (bar2 & 1) && (bar1 & ~3))
+	  if ((bar1 & 1) && (bar2 & 1) && (bar1 & ~3) && (bar4 & 1))
 	    {
 	      rega = bar1 & ~3;
 	      addr = grub_pci_make_address (dev, GRUB_PCI_REG_COMMAND);
@@ -415,14 +482,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 +501,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 +530,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], grub_pata_bmaddress[i]);
+      grub_pata_device_initialize (i, 1, grub_pata_ioaddress[i], grub_pata_bmaddress[i]);
     }
   return 0;
 }
@@ -499,10 +566,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.1

