From 021e35ff098ea0f022ebd56f3373fa17b0877dfe Mon Sep 17 00:00:00 2001
From: Vladimir 'phcoder' Serbinenko <phcoder@gmail.com>
Date: Sat, 27 Apr 2013 17:18:02 +0200
Subject: [PATCH 362/482] 	Improve AHCI detection and command issuing.

---
 ChangeLog             |   4 +
 grub-core/disk/ahci.c | 701 +++++++++++++++++++++++++++++++++++++++-----------
 2 files changed, 549 insertions(+), 156 deletions(-)

diff --git a/ChangeLog b/ChangeLog
index 8c34cc0..0d4329c 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,3 +1,7 @@
+2013-04-27  Vladimir Serbinenko  <phcoder@gmail.com>
+
+	Improve AHCI detection and command issuing.
+
 2013-04-26  Vladimir Serbinenko  <phcoder@gmail.com>
 
 	Fix pseries test.
diff --git a/grub-core/disk/ahci.c b/grub-core/disk/ahci.c
index 143ab97..29fe246 100644
--- a/grub-core/disk/ahci.c
+++ b/grub-core/disk/ahci.c
@@ -75,6 +75,8 @@ struct grub_ahci_hba_port
 enum grub_ahci_hba_port_command
   {
     GRUB_AHCI_HBA_PORT_CMD_ST  = 0x01,
+    GRUB_AHCI_HBA_PORT_CMD_SPIN_UP = 0x02,
+    GRUB_AHCI_HBA_PORT_CMD_POWER_ON = 0x04,
     GRUB_AHCI_HBA_PORT_CMD_FRE = 0x10,
     GRUB_AHCI_HBA_PORT_CMD_CR = 0x8000,
     GRUB_AHCI_HBA_PORT_CMD_FR = 0x4000,
@@ -136,7 +138,7 @@ struct grub_ahci_device
 static grub_err_t 
 grub_ahci_readwrite_real (struct grub_ahci_device *dev,
 			  struct grub_disk_ata_pass_through_parms *parms,
-			  int spinup);
+			  int spinup, int reset);
 
 
 enum
@@ -162,98 +164,6 @@ enum
 static struct grub_ahci_device *grub_ahci_devices;
 static int numdevs;
 
-static int 
-init_port (struct grub_ahci_device *dev)
-{
-  struct grub_pci_dma_chunk *command_list;
-  struct grub_pci_dma_chunk *command_table;
-  grub_uint64_t endtime;
-
-  command_list = grub_memalign_dma32 (1024, sizeof (struct grub_ahci_cmd_head));
-  if (!command_list)
-    return 1;
-
-  command_table = grub_memalign_dma32 (1024,
-				       sizeof (struct grub_ahci_cmd_table));
-  if (!command_table)
-    {
-      grub_dma_free (command_list);
-      return 1;
-    }
-
-  grub_dprintf ("ahci", "found device ahci%d (port %d)\n", dev->num, dev->port);
-
-  dev->hba->ports[dev->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_FRE;
-  endtime = grub_get_time_ms () + 1000;
-  while ((dev->hba->ports[dev->port].command & GRUB_AHCI_HBA_PORT_CMD_FR))
-    if (grub_get_time_ms () > endtime)
-      {
-	grub_dprintf ("ahci", "couldn't stop FR\n");
-	goto out;
-      }
-
-  dev->hba->ports[dev->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_ST;
-  endtime = grub_get_time_ms () + 1000;
-  while ((dev->hba->ports[dev->port].command & GRUB_AHCI_HBA_PORT_CMD_CR))
-    if (grub_get_time_ms () > endtime)
-      {
-	grub_dprintf ("ahci", "couldn't stop CR\n");
-	goto out;
-      }
-
-  dev->hba->ports[dev->port].fbs = 2;
-
-  dev->rfis = grub_memalign_dma32 (4096, 
-				   sizeof (struct grub_ahci_received_fis));
-  grub_memset ((char *) grub_dma_get_virt (dev->rfis), 0,
-	       sizeof (struct grub_ahci_received_fis));
-  dev->hba->ports[dev->port].fis_base = grub_dma_get_phys (dev->rfis);
-  dev->hba->ports[dev->port].command_list_base
-    = grub_dma_get_phys (command_list);
-  dev->hba->ports[dev->port].command |= GRUB_AHCI_HBA_PORT_CMD_FRE;
-  while (!(dev->hba->ports[dev->port].command & GRUB_AHCI_HBA_PORT_CMD_FR))
-    if (grub_get_time_ms () > endtime)
-      {
-	grub_dprintf ("ahci", "couldn't start FR\n");
-	dev->hba->ports[dev->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_FRE;
-	goto out;
-      }
-  dev->hba->ports[dev->port].command |= GRUB_AHCI_HBA_PORT_CMD_ST;
-  while (!(dev->hba->ports[dev->port].command & GRUB_AHCI_HBA_PORT_CMD_CR))
-    if (grub_get_time_ms () > endtime)
-      {
-	grub_dprintf ("ahci", "couldn't start CR\n");
-	dev->hba->ports[dev->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_CR;
-	goto out_stop_fr;
-      }
-
-  dev->hba->ports[dev->port].command
-    = (dev->hba->ports[dev->port].command & 0x0fffffff) | (1 << 28) | 2 | 4;
-
-  dev->command_list_chunk = command_list;
-  dev->command_list = grub_dma_get_virt (command_list);
-  dev->command_table_chunk = command_table;
-  dev->command_table = grub_dma_get_virt (command_table);
-  dev->command_list->command_table_base
-    = grub_dma_get_phys (command_table);
-
-  return 0;
- out_stop_fr:
-  dev->hba->ports[dev->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_FRE;
-  endtime = grub_get_time_ms () + 1000;
-  while ((dev->hba->ports[dev->port].command & GRUB_AHCI_HBA_PORT_CMD_FR))
-    if (grub_get_time_ms () > endtime)
-      {
-	grub_dprintf ("ahci", "couldn't stop FR\n");
-	break;
-      }
- out:
-  grub_dma_free (command_list);
-  grub_dma_free (command_table);
-  grub_dma_free (dev->rfis);
-  return 1;
-}
-
 static int
 grub_ahci_pciinit (grub_pci_device_t dev,
 		   grub_pci_id_t pciid __attribute__ ((unused)),
@@ -273,7 +183,16 @@ grub_ahci_pciinit (grub_pci_device_t dev,
   if (class >> 8 != 0x010601)
     return 0;
 
+#ifdef GRUB_MACHINE_QEMU
+  addr = grub_pci_make_address (dev, GRUB_PCI_REG_COMMAND);
+  grub_pci_write_word (addr, 0x107);
+#endif
+
   addr = grub_pci_make_address (dev, GRUB_PCI_REG_ADDRESS_REG5);
+
+#ifdef GRUB_MACHINE_QEMU
+  grub_pci_write (addr, 0xf4000000);
+#endif
   bar = grub_pci_read (addr);
 
   if ((bar & (GRUB_PCI_ADDR_SPACE_MASK | GRUB_PCI_ADDR_MEM_TYPE_MASK
@@ -281,8 +200,40 @@ grub_ahci_pciinit (grub_pci_device_t dev,
       != (GRUB_PCI_ADDR_SPACE_MEMORY | GRUB_PCI_ADDR_MEM_TYPE_32))
     return 0;
 
+  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);
+
   hba = grub_pci_device_map_range (dev, bar & GRUB_PCI_ADDR_MEM_MASK,
 				   sizeof (hba));
+  grub_dprintf ("ahci", "dev: %x:%x.%x\n", dev.bus, dev.device, dev.function);
+
+  grub_dprintf ("ahci", "tfd[0]: %x\n",
+		hba->ports[0].task_file_data);
+  grub_dprintf ("ahci", "cmd[0]: %x\n",
+		hba->ports[0].command);
+  grub_dprintf ("ahci", "st[0]: %x\n",
+		hba->ports[0].status);
+  grub_dprintf ("ahci", "err[0]: %x\n",
+		hba->ports[0].sata_error);
+
+  grub_dprintf ("ahci", "tfd[1]: %x\n",
+		hba->ports[1].task_file_data);
+  grub_dprintf ("ahci", "cmd[1]: %x\n",
+		hba->ports[1].command);
+  grub_dprintf ("ahci", "st[1]: %x\n",
+		hba->ports[1].status);
+  grub_dprintf ("ahci", "err[1]: %x\n",
+		hba->ports[1].sata_error);
+
+  hba->ports[1].sata_error = hba->ports[1].sata_error;
+
+  grub_dprintf ("ahci", "err[1]: %x\n",
+		hba->ports[1].sata_error);
+
+  grub_dprintf ("ahci", "BH:%x\n", hba->bios_handoff);
 
   if (! (hba->bios_handoff & GRUB_AHCI_BIOS_HANDOFF_OS_OWNED))
     {
@@ -307,11 +258,38 @@ grub_ahci_pciinit (grub_pci_device_t dev,
   else
     grub_dprintf ("ahci", "AHCI is already in OS mode\n");
 
+  grub_dprintf ("ahci", "GLC:%x\n", hba->global_control);
+
+  grub_dprintf ("ahci", "err[1]: %x\n",
+		hba->ports[1].sata_error);
+
   if (!(hba->global_control & GRUB_AHCI_HBA_GLOBAL_CONTROL_AHCI_EN))
     grub_dprintf ("ahci", "AHCI is in compat mode. Switching\n");
   else
     grub_dprintf ("ahci", "AHCI is in AHCI mode.\n");
 
+  grub_dprintf ("ahci", "err[1]: %x\n",
+		hba->ports[1].sata_error);
+
+  grub_dprintf ("ahci", "GLC:%x\n", hba->global_control);
+
+  /*  {
+      grub_uint64_t endtime;
+      hba->global_control |= 1;
+      endtime = grub_get_time_ms () + 1000;
+      while (hba->global_control & 1)
+	if (grub_get_time_ms () > endtime)
+	  {
+	    grub_dprintf ("ahci", "couldn't reset AHCI\n");
+	    return 0;
+	  }
+	  }*/
+
+  grub_dprintf ("ahci", "GLC:%x\n", hba->global_control);
+
+  grub_dprintf ("ahci", "err[1]: %x\n",
+		hba->ports[1].sata_error);
+
   for (i = 0; i < 5; i++)
     {
       hba->global_control |= GRUB_AHCI_HBA_GLOBAL_CONTROL_AHCI_EN;
@@ -325,18 +303,15 @@ grub_ahci_pciinit (grub_pci_device_t dev,
       return 0;
     }
 
-  /*
-  {
-      grub_uint64_t endtime;
-      hba->global_control |= 1;
-      endtime = grub_get_time_ms () + 1000;
-      while (hba->global_control & 1)
-	if (grub_get_time_ms () > endtime)
-	  {
-	    grub_dprintf ("ahci", "couldn't reset AHCI\n");
-	    return 0;
-	  }
-  }
+  grub_dprintf ("ahci", "GLC:%x\n", hba->global_control);
+
+  grub_dprintf ("ahci", "err[1]: %x\n",
+		hba->ports[1].sata_error);
+
+  grub_dprintf ("ahci", "err[1]: %x\n",
+		hba->ports[1].sata_error);
+
+  grub_dprintf ("ahci", "GLC:%x\n", hba->global_control);
 
   for (i = 0; i < 5; i++)
     {
@@ -350,45 +325,312 @@ grub_ahci_pciinit (grub_pci_device_t dev,
       grub_dprintf ("ahci", "Couldn't put AHCI in AHCI mode\n");
       return 0;
     }
-  */
+
+  grub_dprintf ("ahci", "err[1]: %x\n",
+		hba->ports[1].sata_error);
+
+  grub_dprintf ("ahci", "GLC:%x\n", hba->global_control);
 
   nports = (hba->cap & GRUB_AHCI_HBA_CAP_NPORTS_MASK) + 1;
 
-  grub_dprintf ("ahci", "%d AHCI ports\n", nports);
+  grub_dprintf ("ahci", "%d AHCI ports, PI = 0x%x\n", nports,
+		hba->ports_implemented);
+
+  struct grub_ahci_device *adevs[GRUB_AHCI_HBA_CAP_NPORTS_MASK + 1];
+  struct grub_ahci_device *failed_adevs[GRUB_AHCI_HBA_CAP_NPORTS_MASK + 1];
+  grub_uint32_t fr_running = 0;
 
   for (i = 0; i < nports; i++)
+    failed_adevs[i] = 0;
+  for (i = 0; i < nports; i++)
     {
-      struct grub_ahci_device *adev;
-      grub_uint32_t st;
-
       if (!(hba->ports_implemented & (1 << i)))
-	continue;
-
-      grub_dprintf ("ahci", "status %d:%x\n", i, hba->ports[i].status);
-      /* FIXME: support hotplugging.  */
-      st = hba->ports[i].status;
-      if ((st & 0xf) != 0x3 && (st & 0xf) != 0x1)
-	continue;
+	{
+	  adevs[i] = 0;
+	  continue;
+	}
 
-      adev = grub_malloc (sizeof (*adev));
-      if (!adev)
+      adevs[i] = grub_malloc (sizeof (*adevs[i]));
+      if (!adevs[i])
 	return 1;
 
-      adev->hba = hba;
-      adev->port = i;
-      adev->present = 1;
-      adev->num = numdevs++;
+      adevs[i]->hba = hba;
+      adevs[i]->port = i;
+      adevs[i]->present = 1;
+      adevs[i]->num = numdevs++;
+    }
 
-      if (init_port (adev))
-	{
-	  grub_free (adev);
-	  return 1;
-	}
+  for (i = 0; i < nports; i++)
+    if (adevs[i])
+      {
+	adevs[i]->hba->ports[adevs[i]->port].sata_error = adevs[i]->hba->ports[adevs[i]->port].sata_error;
+	grub_dprintf ("ahci", "err: %x\n",
+		      adevs[i]->hba->ports[adevs[i]->port].sata_error);
+
+	adevs[i]->command_list_chunk = grub_memalign_dma32 (1024, sizeof (struct grub_ahci_cmd_head));
+	if (!adevs[i]->command_list_chunk)
+	  {
+	    adevs[i] = 0;
+	    continue;
+	  }
 
-      grub_list_push (GRUB_AS_LIST_P (&grub_ahci_devices),
-		      GRUB_AS_LIST (adev));
+	adevs[i]->command_table_chunk = grub_memalign_dma32 (1024,
+							    sizeof (struct grub_ahci_cmd_table));
+	if (!adevs[i]->command_table_chunk)
+	  {
+	    grub_dma_free (adevs[i]->command_list_chunk);
+	    adevs[i] = 0;
+	    continue;
+	  }
+
+	adevs[i]->command_list = grub_dma_get_virt (adevs[i]->command_list_chunk);
+	adevs[i]->command_table = grub_dma_get_virt (adevs[i]->command_table_chunk);
+	adevs[i]->command_list->command_table_base
+	  = grub_dma_get_phys (adevs[i]->command_table_chunk);
+
+	grub_dprintf ("ahci", "found device ahci%d (port %d), command_table = %p, command_list = %p\n",
+		      adevs[i]->num, adevs[i]->port, grub_dma_get_virt (adevs[i]->command_table_chunk),
+		      grub_dma_get_virt (adevs[i]->command_list_chunk));
+
+	adevs[i]->hba->ports[adevs[i]->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_FRE;
+      }
+
+  grub_uint64_t endtime;
+  endtime = grub_get_time_ms () + 1000;
+
+  while (grub_get_time_ms () < endtime)
+    {
+      for (i = 0; i < nports; i++)
+	if (adevs[i] && (adevs[i]->hba->ports[adevs[i]->port].command & GRUB_AHCI_HBA_PORT_CMD_FR))
+	  break;
+      if (i == nports)
+	break;
     }
 
+  for (i = 0; i < nports; i++)
+    if (adevs[i] && (adevs[i]->hba->ports[adevs[i]->port].command & GRUB_AHCI_HBA_PORT_CMD_FR))
+      {
+	grub_dprintf ("ahci", "couldn't stop FR on port %d\n", i);
+	failed_adevs[i] = adevs[i];
+	adevs[i] = 0;
+      }
+
+  for (i = 0; i < nports; i++)
+    if (adevs[i])
+      adevs[i]->hba->ports[adevs[i]->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_ST;
+  endtime = grub_get_time_ms () + 1000;
+
+  while (grub_get_time_ms () < endtime)
+    {
+      for (i = 0; i < nports; i++)
+	if (adevs[i] && (adevs[i]->hba->ports[adevs[i]->port].command & GRUB_AHCI_HBA_PORT_CMD_CR))
+	  break;
+      if (i == nports)
+	break;
+    }
+
+  for (i = 0; i < nports; i++)
+    if (adevs[i] && (adevs[i]->hba->ports[adevs[i]->port].command & GRUB_AHCI_HBA_PORT_CMD_CR))
+      {
+	grub_dprintf ("ahci", "couldn't stop CR on port %d\n", i);
+	failed_adevs[i] = adevs[i];
+	adevs[i] = 0;
+      }
+  for (i = 0; i < nports; i++)
+    if (adevs[i])
+      {
+	adevs[i]->hba->ports[adevs[i]->port].inten = 0;
+	adevs[i]->hba->ports[adevs[i]->port].intstatus = ~0;
+	//  adevs[i]->hba->ports[adevs[i]->port].fbs = 0;
+
+	grub_dprintf ("ahci", "err: %x\n",
+		      adevs[i]->hba->ports[adevs[i]->port].sata_error);
+
+	adevs[i]->rfis = grub_memalign_dma32 (4096, 
+					     sizeof (struct grub_ahci_received_fis));
+	grub_memset ((char *) grub_dma_get_virt (adevs[i]->rfis), 0,
+		     sizeof (struct grub_ahci_received_fis));
+	grub_memset ((char *) grub_dma_get_virt (adevs[i]->command_list_chunk), 0,
+		     sizeof (struct grub_ahci_cmd_head));
+	grub_memset ((char *) grub_dma_get_virt (adevs[i]->command_table_chunk), 0,
+		     sizeof (struct grub_ahci_cmd_table));
+	adevs[i]->hba->ports[adevs[i]->port].fis_base = grub_dma_get_phys (adevs[i]->rfis);
+	adevs[i]->hba->ports[adevs[i]->port].command_list_base
+	  = grub_dma_get_phys (adevs[i]->command_list_chunk);
+	adevs[i]->hba->ports[adevs[i]->port].command |= GRUB_AHCI_HBA_PORT_CMD_FRE;
+      }
+
+  endtime = grub_get_time_ms () + 1000;
+
+  while (grub_get_time_ms () < endtime)
+    {
+      for (i = 0; i < nports; i++)
+	if (adevs[i] && !(adevs[i]->hba->ports[adevs[i]->port].command & GRUB_AHCI_HBA_PORT_CMD_FR))
+	  break;
+      if (i == nports)
+	break;
+    }
+
+  for (i = 0; i < nports; i++)
+    if (adevs[i] && !(adevs[i]->hba->ports[adevs[i]->port].command & GRUB_AHCI_HBA_PORT_CMD_FR))
+      {
+	grub_dprintf ("ahci", "couldn't start FR on port %d\n", i);
+	failed_adevs[i] = adevs[i];
+	adevs[i] = 0;
+      }
+
+  for (i = 0; i < nports; i++)
+    if (adevs[i])
+      {
+	grub_dprintf ("ahci", "err: %x\n",
+		      adevs[i]->hba->ports[adevs[i]->port].sata_error);
+	fr_running |= (1 << i);
+
+	adevs[i]->hba->ports[adevs[i]->port].command |= GRUB_AHCI_HBA_PORT_CMD_SPIN_UP;
+	adevs[i]->hba->ports[adevs[i]->port].command |= GRUB_AHCI_HBA_PORT_CMD_POWER_ON;
+	adevs[i]->hba->ports[adevs[i]->port].command |= 1 << 28;
+
+	grub_dprintf ("ahci", "err: %x\n",
+		      adevs[i]->hba->ports[adevs[i]->port].sata_error);
+      }
+
+  /* 10ms should actually be enough.  */
+  endtime = grub_get_time_ms () + 100;
+
+  while (grub_get_time_ms () < endtime)
+    {
+      for (i = 0; i < nports; i++)
+	if (adevs[i] && (adevs[i]->hba->ports[adevs[i]->port].status & 7) != 3)
+	  break;
+      if (i == nports)
+	break;
+    }
+
+  for (i = 0; i < nports; i++)
+    if (adevs[i] && (adevs[i]->hba->ports[adevs[i]->port].status & 7) != 3)
+      {
+	grub_dprintf ("ahci", "couldn't detect device on port %d\n", i);
+	failed_adevs[i] = adevs[i];
+	adevs[i] = 0;
+      }
+
+  for (i = 0; i < nports; i++)
+    if (adevs[i])
+      {
+	grub_dprintf ("ahci", "err: %x\n",
+		      adevs[i]->hba->ports[adevs[i]->port].sata_error);
+
+	adevs[i]->hba->ports[adevs[i]->port].command |= GRUB_AHCI_HBA_PORT_CMD_POWER_ON;
+	adevs[i]->hba->ports[adevs[i]->port].command |= GRUB_AHCI_HBA_PORT_CMD_SPIN_UP;
+
+	grub_dprintf ("ahci", "err: %x\n",
+		      adevs[i]->hba->ports[adevs[i]->port].sata_error);
+
+	adevs[i]->hba->ports[adevs[i]->port].sata_error = ~0;
+	grub_dprintf ("ahci", "err: %x\n",
+		      adevs[i]->hba->ports[adevs[i]->port].sata_error);
+
+	grub_dprintf ("ahci", "offset: %x, tfd:%x, CMD: %x\n",
+		      (char *) &adevs[i]->hba->ports[adevs[i]->port].task_file_data - 
+		      (char *) adevs[i]->hba,
+		      adevs[i]->hba->ports[adevs[i]->port].task_file_data,
+		      adevs[i]->hba->ports[adevs[i]->port].command);
+
+	grub_dprintf ("ahci", "err: %x\n",
+		      adevs[i]->hba->ports[adevs[i]->port].sata_error);
+      }
+
+
+  for (i = 0; i < nports; i++)
+    if (adevs[i])
+      {
+	grub_dprintf ("ahci", "offset: %x, tfd:%x, CMD: %x\n",
+		      (char *) &adevs[i]->hba->ports[adevs[i]->port].task_file_data - 
+		      (char *) adevs[i]->hba,
+		      adevs[i]->hba->ports[adevs[i]->port].task_file_data,
+		      adevs[i]->hba->ports[adevs[i]->port].command);
+
+	grub_dprintf ("ahci", "err: %x\n",
+		      adevs[i]->hba->ports[adevs[i]->port].sata_error);
+
+	adevs[i]->hba->ports[adevs[i]->port].command
+	  = (adevs[i]->hba->ports[adevs[i]->port].command & 0x0fffffff) | (1 << 28) | 2 | 4;
+
+	/*  struct grub_disk_ata_pass_through_parms parms2;
+	    grub_memset (&parms2, 0, sizeof (parms2));
+	    parms2.taskfile.cmd = 8;
+	    grub_ahci_readwrite_real (dev, &parms2, 1, 1);*/
+      }
+
+  endtime = grub_get_time_ms () + 10000;
+
+  while (grub_get_time_ms () < endtime)
+    {
+      for (i = 0; i < nports; i++)
+	if (adevs[i] && (adevs[i]->hba->ports[adevs[i]->port].task_file_data & 0x88))
+	  break;
+      if (i == nports)
+	break;
+    }
+
+  for (i = 0; i < nports; i++)
+    if (adevs[i] && (adevs[i]->hba->ports[adevs[i]->port].task_file_data & 0x88))
+      {
+	grub_dprintf ("ahci", "port %d is busy\n", i);
+	failed_adevs[i] = adevs[i];
+	adevs[i] = 0;
+      }
+
+  for (i = 0; i < nports; i++)
+    if (adevs[i])
+      adevs[i]->hba->ports[adevs[i]->port].command |= GRUB_AHCI_HBA_PORT_CMD_ST;
+
+  endtime = grub_get_time_ms () + 1000;
+
+  while (grub_get_time_ms () < endtime)
+    {
+      for (i = 0; i < nports; i++)
+	if (adevs[i] && !(adevs[i]->hba->ports[adevs[i]->port].command & GRUB_AHCI_HBA_PORT_CMD_CR))
+	  break;
+      if (i == nports)
+	break;
+    }
+
+  for (i = 0; i < nports; i++)
+    if (adevs[i] && !(adevs[i]->hba->ports[adevs[i]->port].command & GRUB_AHCI_HBA_PORT_CMD_CR))
+      {
+	grub_dprintf ("ahci", "couldn't start CR on port %d\n", i);
+	failed_adevs[i] = adevs[i];
+	adevs[i] = 0;
+      }
+  for (i = 0; i < nports; i++)
+    if (failed_adevs[i] && (fr_running & (1 << i)))
+      failed_adevs[i]->hba->ports[failed_adevs[i]->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_FRE;
+
+  endtime = grub_get_time_ms () + 1000;
+  while (grub_get_time_ms () < endtime)
+    {
+      for (i = 0; i < nports; i++)
+	if (failed_adevs[i] && (fr_running & (1 << i)) && (failed_adevs[i]->hba->ports[failed_adevs[i]->port].command & GRUB_AHCI_HBA_PORT_CMD_FR))
+	  break;
+      if (i == nports)
+	break;
+    }
+  for (i = 0; i < nports; i++)
+    if (failed_adevs[i])
+      {
+	grub_dma_free (adevs[i]->command_list_chunk);
+	grub_dma_free (adevs[i]->command_table_chunk);
+	grub_dma_free (adevs[i]->rfis);
+      }
+
+  for (i = 0; i < nports; i++)
+    if (adevs[i])
+      {
+	grub_list_push (GRUB_AS_LIST_P (&grub_ahci_devices),
+			GRUB_AS_LIST (adevs[i]));
+      }
+
   return 0;
 }
 
@@ -435,13 +677,105 @@ grub_ahci_fini_hw (int noreturn __attribute__ ((unused)))
   return GRUB_ERR_NONE;
 }
 
+static int 
+reinit_port (struct grub_ahci_device *dev)
+{
+  struct grub_pci_dma_chunk *command_list;
+  struct grub_pci_dma_chunk *command_table;
+  grub_uint64_t endtime;
+
+  command_list = grub_memalign_dma32 (1024, sizeof (struct grub_ahci_cmd_head));
+  if (!command_list)
+    return 1;
+
+  command_table = grub_memalign_dma32 (1024,
+				       sizeof (struct grub_ahci_cmd_table));
+  if (!command_table)
+    {
+      grub_dma_free (command_list);
+      return 1;
+    }
+
+  grub_dprintf ("ahci", "found device ahci%d (port %d)\n", dev->num, dev->port);
+
+  dev->hba->ports[dev->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_FRE;
+  endtime = grub_get_time_ms () + 1000;
+  while ((dev->hba->ports[dev->port].command & GRUB_AHCI_HBA_PORT_CMD_FR))
+    if (grub_get_time_ms () > endtime)
+      {
+	grub_dprintf ("ahci", "couldn't stop FR\n");
+	goto out;
+      }
+
+  dev->hba->ports[dev->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_ST;
+  endtime = grub_get_time_ms () + 1000;
+  while ((dev->hba->ports[dev->port].command & GRUB_AHCI_HBA_PORT_CMD_CR))
+    if (grub_get_time_ms () > endtime)
+      {
+	grub_dprintf ("ahci", "couldn't stop CR\n");
+	goto out;
+      }
+
+  dev->hba->ports[dev->port].fbs = 2;
+
+  dev->rfis = grub_memalign_dma32 (4096, 
+				   sizeof (struct grub_ahci_received_fis));
+  grub_memset ((char *) grub_dma_get_virt (dev->rfis), 0,
+	       sizeof (struct grub_ahci_received_fis));
+  dev->hba->ports[dev->port].fis_base = grub_dma_get_phys (dev->rfis);
+  dev->hba->ports[dev->port].command_list_base
+    = grub_dma_get_phys (command_list);
+  dev->hba->ports[dev->port].command |= GRUB_AHCI_HBA_PORT_CMD_FRE;
+  while (!(dev->hba->ports[dev->port].command & GRUB_AHCI_HBA_PORT_CMD_FR))
+    if (grub_get_time_ms () > endtime)
+      {
+	grub_dprintf ("ahci", "couldn't start FR\n");
+	dev->hba->ports[dev->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_FRE;
+	goto out;
+      }
+  dev->hba->ports[dev->port].command |= GRUB_AHCI_HBA_PORT_CMD_ST;
+  while (!(dev->hba->ports[dev->port].command & GRUB_AHCI_HBA_PORT_CMD_CR))
+    if (grub_get_time_ms () > endtime)
+      {
+	grub_dprintf ("ahci", "couldn't start CR\n");
+	dev->hba->ports[dev->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_CR;
+	goto out_stop_fr;
+      }
+
+  dev->hba->ports[dev->port].command
+    = (dev->hba->ports[dev->port].command & 0x0fffffff) | (1 << 28) | 2 | 4;
+
+  dev->command_list_chunk = command_list;
+  dev->command_list = grub_dma_get_virt (command_list);
+  dev->command_table_chunk = command_table;
+  dev->command_table = grub_dma_get_virt (command_table);
+  dev->command_list->command_table_base
+    = grub_dma_get_phys (command_table);
+
+  return 0;
+ out_stop_fr:
+  dev->hba->ports[dev->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_FRE;
+  endtime = grub_get_time_ms () + 1000;
+  while ((dev->hba->ports[dev->port].command & GRUB_AHCI_HBA_PORT_CMD_FR))
+    if (grub_get_time_ms () > endtime)
+      {
+	grub_dprintf ("ahci", "couldn't stop FR\n");
+	break;
+      }
+ out:
+  grub_dma_free (command_list);
+  grub_dma_free (command_table);
+  grub_dma_free (dev->rfis);
+  return 1;
+}
+
 static grub_err_t
 grub_ahci_restore_hw (void)
 {
   struct grub_ahci_device **pdev;
 
   for (pdev = &grub_ahci_devices; *pdev; pdev = &((*pdev)->next))
-    if (init_port (*pdev))
+    if (reinit_port (*pdev))
       {
 	struct grub_ahci_device *odev;
 	odev = *pdev;
@@ -504,38 +838,72 @@ static const int register_map[11] = { 3 /* Features */,
 				      9 /* LBA48 mid */,
 				      10 /* LBA48 high */ }; 
 
-static grub_err_t 
-grub_ahci_readwrite_real (struct grub_ahci_device *dev,
-			  struct grub_disk_ata_pass_through_parms *parms,
-			  int spinup)
+static grub_err_t
+grub_ahci_reset_port (struct grub_ahci_device *dev, int force)
 {
-  struct grub_pci_dma_chunk *bufc;
   grub_uint64_t endtime;
-  unsigned i;
-  grub_err_t err = GRUB_ERR_NONE;
-
-  grub_dprintf ("ahci", "AHCI tfd = %x\n",
-		dev->hba->ports[dev->port].task_file_data);
+  
+  dev->hba->ports[dev->port].sata_error = dev->hba->ports[dev->port].sata_error;
 
-  if ((dev->hba->ports[dev->port].task_file_data & 0x80))
+  if (force || (dev->hba->ports[dev->port].command_issue & 1)
+      || (dev->hba->ports[dev->port].task_file_data & 0x80))
     {
+      struct grub_disk_ata_pass_through_parms parms2;
       dev->hba->ports[dev->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_ST;
       endtime = grub_get_time_ms () + 1000;
       while ((dev->hba->ports[dev->port].command & GRUB_AHCI_HBA_PORT_CMD_CR))
 	if (grub_get_time_ms () > endtime)
 	  {
-	    grub_dprintf ("ahci", "couldn't stop CR\n");
-	    break;
+	    grub_dprintf ("ahci", "couldn't stop CR");
+	    return grub_error (GRUB_ERR_IO, "couldn't stop CR");
+	  }
+      dev->hba->ports[dev->port].command |= 8;
+      while (dev->hba->ports[dev->port].command & 8)
+	if (grub_get_time_ms () > endtime)
+	  {
+	    grub_dprintf ("ahci", "couldn't set CLO\n");
+	    dev->hba->ports[dev->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_FRE;
+	    return grub_error (GRUB_ERR_IO, "couldn't set CLO");
 	  }
+
       dev->hba->ports[dev->port].command |= GRUB_AHCI_HBA_PORT_CMD_ST;
-      endtime = grub_get_time_ms () + (spinup ? 10000 : 1000);
       while (!(dev->hba->ports[dev->port].command & GRUB_AHCI_HBA_PORT_CMD_CR))
 	if (grub_get_time_ms () > endtime)
 	  {
-	    grub_dprintf ("ahci", "couldn't start CR\n");
-	    break;
+	    grub_dprintf ("ahci", "couldn't stop CR");
+	    dev->hba->ports[dev->port].command &= ~GRUB_AHCI_HBA_PORT_CMD_ST;
+	    return grub_error (GRUB_ERR_IO, "couldn't stop CR");
 	  }
+      dev->hba->ports[dev->port].sata_error = dev->hba->ports[dev->port].sata_error;
+      grub_memset (&parms2, 0, sizeof (parms2));
+      parms2.taskfile.cmd = 8;
+      return grub_ahci_readwrite_real (dev, &parms2, 1, 1);
     }
+  return GRUB_ERR_NONE;
+}
+
+static grub_err_t 
+grub_ahci_readwrite_real (struct grub_ahci_device *dev,
+			  struct grub_disk_ata_pass_through_parms *parms,
+			  int spinup, int reset)
+{
+  struct grub_pci_dma_chunk *bufc;
+  grub_uint64_t endtime;
+  unsigned i;
+  grub_err_t err = GRUB_ERR_NONE;
+
+  grub_dprintf ("ahci", "AHCI tfd = %x\n",
+		dev->hba->ports[dev->port].task_file_data);
+
+  if (!reset)
+    grub_ahci_reset_port (dev, 0);
+ 
+  grub_dprintf ("ahci", "AHCI tfd = %x\n",
+		dev->hba->ports[dev->port].task_file_data);
+  dev->hba->ports[dev->port].task_file_data = 0;
+  dev->hba->ports[dev->port].command_issue = 0;
+  grub_dprintf ("ahci", "AHCI tfd = %x\n",
+		dev->hba->ports[dev->port].task_file_data);
 
   dev->hba->ports[dev->port].sata_error = dev->hba->ports[dev->port].sata_error;
 
@@ -551,10 +919,9 @@ grub_ahci_readwrite_real (struct grub_ahci_device *dev,
 
   bufc = grub_memalign_dma32 (1024, parms->size + (parms->size & 1));
 
-  dev->hba->ports[dev->port].command |= 8;
-
-  grub_dprintf ("ahci", "AHCI tfd = %x\n",
-		dev->hba->ports[dev->port].task_file_data);
+  grub_dprintf ("ahci", "AHCI tfd = %x, CL=%p\n",
+		dev->hba->ports[dev->port].task_file_data,
+		dev->command_list);
   /* FIXME: support port multipliers.  */
   dev->command_list[0].config
     = (5 << GRUB_AHCI_CONFIG_CFIS_LENGTH_SHIFT)
@@ -564,17 +931,28 @@ grub_ahci_readwrite_real (struct grub_ahci_device *dev,
     | (parms->cmdsize ? GRUB_AHCI_CONFIG_ATAPI : 0)
     | (parms->write ? GRUB_AHCI_CONFIG_WRITE : GRUB_AHCI_CONFIG_READ)
     | (parms->taskfile.cmd == 8 ? (1 << 8) : 0);
+  grub_dprintf ("ahci", "AHCI tfd = %x\n",
+		dev->hba->ports[dev->port].task_file_data);
+
   dev->command_list[0].transfered = 0;
   dev->command_list[0].command_table_base
     = grub_dma_get_phys (dev->command_table_chunk);
+
   grub_memset ((char *) dev->command_list[0].unused, 0,
 	       sizeof (dev->command_list[0].unused));
+
   grub_memset ((char *) &dev->command_table[0], 0,
 	       sizeof (dev->command_table[0]));
+  grub_dprintf ("ahci", "AHCI tfd = %x\n",
+		dev->hba->ports[dev->port].task_file_data);
+
   if (parms->cmdsize)
     grub_memcpy ((char *) dev->command_table[0].command, parms->cmd,
 		 parms->cmdsize);
 
+  grub_dprintf ("ahci", "AHCI tfd = %x\n",
+		dev->hba->ports[dev->port].task_file_data);
+
   dev->command_table[0].cfis[0] = GRUB_AHCI_FIS_REG_H2D;
   dev->command_table[0].cfis[1] = 0x80;
   for (i = 0; i < sizeof (parms->taskfile.raw); i++)
@@ -593,8 +971,7 @@ grub_ahci_readwrite_real (struct grub_ahci_device *dev,
 
   dev->command_table[0].prdt[0].data_base = grub_dma_get_phys (bufc);
   dev->command_table[0].prdt[0].unused = 0;
-  dev->command_table[0].prdt[0].size = (parms->size + (parms->size & 1) - 1)
-    | GRUB_AHCI_INTERRUPT_ON_COMPLETE;
+  dev->command_table[0].prdt[0].size = (parms->size - 1);
 
   grub_dprintf ("ahci", "PRDT = %" PRIxGRUB_UINT64_T ", %x, %x (%"
 		PRIuGRUB_SIZE ")\n",
@@ -610,11 +987,19 @@ grub_ahci_readwrite_real (struct grub_ahci_device *dev,
   grub_dprintf ("ahci", "AHCI command schedulded\n");
   grub_dprintf ("ahci", "AHCI tfd = %x\n",
 		dev->hba->ports[dev->port].task_file_data);
+  grub_dprintf ("ahci", "AHCI inten = %x\n",
+		dev->hba->ports[dev->port].inten);
+  grub_dprintf ("ahci", "AHCI intstatus = %x\n",
+		dev->hba->ports[dev->port].intstatus);
+
   dev->hba->ports[dev->port].inten = 0xffffffff;//(1 << 2) | (1 << 5);
   dev->hba->ports[dev->port].intstatus = 0xffffffff;//(1 << 2) | (1 << 5);
+  grub_dprintf ("ahci", "AHCI inten = %x\n",
+		dev->hba->ports[dev->port].inten);
   grub_dprintf ("ahci", "AHCI tfd = %x\n",
 		dev->hba->ports[dev->port].task_file_data);
-  dev->hba->ports[dev->port].command_issue |= 1;
+  dev->hba->ports[dev->port].sata_active = 1;
+  dev->hba->ports[dev->port].command_issue = 1;
   grub_dprintf ("ahci", "AHCI sig = %x\n", dev->hba->ports[dev->port].sig);
   grub_dprintf ("ahci", "AHCI tfd = %x\n",
 		dev->hba->ports[dev->port].task_file_data);
@@ -623,11 +1008,15 @@ grub_ahci_readwrite_real (struct grub_ahci_device *dev,
   while ((dev->hba->ports[dev->port].command_issue & 1))
     if (grub_get_time_ms () > endtime)
       {
-	grub_dprintf ("ahci", "AHCI status <%x %x %x>\n",
+	grub_dprintf ("ahci", "AHCI status <%x %x %x %x>\n",
 		      dev->hba->ports[dev->port].command_issue,
+		      dev->hba->ports[dev->port].sata_active,
 		      dev->hba->ports[dev->port].intstatus,
 		      dev->hba->ports[dev->port].task_file_data);
+	dev->hba->ports[dev->port].command_issue = 0;
 	err = grub_error (GRUB_ERR_IO, "AHCI transfer timed out");
+	if (!reset)
+	  grub_ahci_reset_port (dev, 1);
 	break;
       }
 
@@ -672,7 +1061,7 @@ grub_ahci_readwrite (grub_ata_t disk,
 		     struct grub_disk_ata_pass_through_parms *parms,
 		     int spinup)
 {
-  return grub_ahci_readwrite_real (disk->data, parms, spinup);
+  return grub_ahci_readwrite_real (disk->data, parms, spinup, 0);
 }
 
 static grub_err_t
-- 
1.8.2.1