Index: AhciMode.c
===================================================================
--- AhciMode.c	(revision 15896)
+++ AhciMode.c	(working copy)
@@ -704,6 +704,8 @@
   UINT32                        PortTfd;
   UINT32                        PrdCount;
   BOOLEAN                       InfiniteWait;
+  BOOLEAN                       PioFisReceived;
+  BOOLEAN                       D2hFisReceived;
 
   if (Timeout == 0) {
     InfiniteWait = TRUE;
@@ -780,10 +782,21 @@
     Status = EFI_TIMEOUT;
     Delay  = DivU64x32 (Timeout, 1000) + 1;
     do {
+      PioFisReceived = FALSE;
+      D2hFisReceived = FALSE;
       Offset = FisBaseAddr + EFI_AHCI_PIO_FIS_OFFSET;
-
       Status = AhciCheckMemSet (Offset, EFI_AHCI_FIS_TYPE_MASK, EFI_AHCI_FIS_PIO_SETUP, NULL);
       if (!EFI_ERROR (Status)) {
+        PioFisReceived = TRUE;
+      }
+
+      Offset = FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET;
+      Status = AhciCheckMemSet (Offset, EFI_AHCI_FIS_TYPE_MASK, EFI_AHCI_FIS_REGISTER_D2H, NULL);
+      if (!EFI_ERROR (Status)) {
+        D2hFisReceived = TRUE;
+      }
+
+      if (PioFisReceived || D2hFisReceived) {
         Offset = EFI_AHCI_PORT_START + Port * EFI_AHCI_PORT_REG_WIDTH + EFI_AHCI_PORT_TFD;
         PortTfd = AhciReadReg (PciIo, (UINT32) Offset);
         //
@@ -801,13 +814,6 @@
         }
       }
 
-      Offset = FisBaseAddr + EFI_AHCI_D2H_FIS_OFFSET;
-      Status = AhciCheckMemSet (Offset, EFI_AHCI_FIS_TYPE_MASK, EFI_AHCI_FIS_REGISTER_D2H, NULL);
-      if (!EFI_ERROR (Status)) {
-        Status = EFI_DEVICE_ERROR;
-        break;
-      }
-
       //
       // Stall for 100 microseconds.
       //