]> git.proxmox.com Git - mirror_edk2.git/blobdiff - MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHci.c
MdeModulePkg/SdMmcPciHcDxe: Fix PIO transfer mode
[mirror_edk2.git] / MdeModulePkg / Bus / Pci / SdMmcPciHcDxe / SdMmcPciHci.c
index 5aec8c69927abb5e051bafaf189895d10db292cf..6548ef71dee1d7797bde8bcd06744e92f77d9918 100644 (file)
@@ -1,18 +1,14 @@
 /** @file\r
   This driver is used to manage SD/MMC PCI host controllers which are compliance\r
-  with SD Host Controller Simplified Specification version 3.00.\r
+  with SD Host Controller Simplified Specification version 3.00 plus the 64-bit\r
+  System Addressing support in SD Host Controller Simplified Specification version\r
+  4.20.\r
 \r
   It would expose EFI_SD_MMC_PASS_THRU_PROTOCOL for upper layer use.\r
 \r
-  Copyright (c) 2018, NVIDIA CORPORATION. All rights reserved.\r
-  Copyright (c) 2015 - 2017, Intel Corporation. All rights reserved.<BR>\r
-  This program and the accompanying materials\r
-  are licensed and made available under the terms and conditions of the BSD License\r
-  which accompanies this distribution.  The full text of the license may be found at\r
-  http://opensource.org/licenses/bsd-license.php\r
-\r
-  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
-  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
+  Copyright (c) 2018-2019, NVIDIA CORPORATION. All rights reserved.\r
+  Copyright (c) 2015 - 2020, Intel Corporation. All rights reserved.<BR>\r
+  SPDX-License-Identifier: BSD-2-Clause-Patent\r
 \r
 **/\r
 \r
@@ -152,19 +148,36 @@ SdMmcHcRwMmio (
   )\r
 {\r
   EFI_STATUS                   Status;\r
+  EFI_PCI_IO_PROTOCOL_WIDTH    Width;\r
 \r
   if ((PciIo == NULL) || (Data == NULL))  {\r
     return EFI_INVALID_PARAMETER;\r
   }\r
 \r
-  if ((Count != 1) && (Count != 2) && (Count != 4) && (Count != 8)) {\r
-    return EFI_INVALID_PARAMETER;\r
+  switch (Count) {\r
+    case 1:\r
+      Width = EfiPciIoWidthUint8;\r
+      break;\r
+    case 2:\r
+      Width = EfiPciIoWidthUint16;\r
+      Count = 1;\r
+      break;\r
+    case 4:\r
+      Width = EfiPciIoWidthUint32;\r
+      Count = 1;\r
+      break;\r
+    case 8:\r
+      Width = EfiPciIoWidthUint32;\r
+      Count = 2;\r
+      break;\r
+    default:\r
+      return EFI_INVALID_PARAMETER;\r
   }\r
 \r
   if (Read) {\r
     Status = PciIo->Mem.Read (\r
                           PciIo,\r
-                          EfiPciIoWidthUint8,\r
+                          Width,\r
                           BarIndex,\r
                           (UINT64) Offset,\r
                           Count,\r
@@ -173,7 +186,7 @@ SdMmcHcRwMmio (
   } else {\r
     Status = PciIo->Mem.Write (\r
                           PciIo,\r
-                          EfiPciIoWidthUint8,\r
+                          Width,\r
                           BarIndex,\r
                           (UINT64) Offset,\r
                           Count,\r
@@ -745,16 +758,40 @@ SdMmcHcStopClock (
   return Status;\r
 }\r
 \r
+/**\r
+  Start the SD clock.\r
+\r
+  @param[in] PciIo  The PCI IO protocol instance.\r
+  @param[in] Slot   The slot number.\r
+\r
+  @retval EFI_SUCCESS  Succeeded to start the SD clock.\r
+  @retval Others       Failed to start the SD clock.\r
+**/\r
+EFI_STATUS\r
+SdMmcHcStartSdClock (\r
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN UINT8                Slot\r
+  )\r
+{\r
+  UINT16                    ClockCtrl;\r
+\r
+  //\r
+  // Set SD Clock Enable in the Clock Control register to 1\r
+  //\r
+  ClockCtrl = BIT2;\r
+  return SdMmcHcOrMmio (PciIo, Slot, SD_MMC_HC_CLOCK_CTRL, sizeof (ClockCtrl), &ClockCtrl);\r
+}\r
+\r
 /**\r
   SD/MMC card clock supply.\r
 \r
   Refer to SD Host Controller Simplified spec 3.0 Section 3.2.1 for details.\r
 \r
-  @param[in] PciIo          The PCI IO protocol instance.\r
-  @param[in] Slot           The slot number of the SD card to send the command to.\r
-  @param[in] ClockFreq      The max clock frequency to be set. The unit is KHz.\r
-  @param[in] BaseClkFreq    The base clock frequency of host controller in MHz.\r
-  @param[in] ControllerVer  The version of host controller.\r
+  @param[in] Private         A pointer to the SD_MMC_HC_PRIVATE_DATA instance.\r
+  @param[in] Slot            The slot number of the SD card to send the command to.\r
+  @param[in] BusTiming       BusTiming at which the frequency change is done.\r
+  @param[in] FirstTimeSetup  Flag to indicate whether the clock is being setup for the first time.\r
+  @param[in] ClockFreq       The max clock frequency to be set. The unit is KHz.\r
 \r
   @retval EFI_SUCCESS       The clock is supplied successfully.\r
   @retval Others            The clock isn't supplied successfully.\r
@@ -762,11 +799,11 @@ SdMmcHcStopClock (
 **/\r
 EFI_STATUS\r
 SdMmcHcClockSupply (\r
-  IN EFI_PCI_IO_PROTOCOL    *PciIo,\r
-  IN UINT8                  Slot,\r
-  IN UINT64                 ClockFreq,\r
-  IN UINT32                 BaseClkFreq,\r
-  IN UINT16                 ControllerVer\r
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,\r
+  IN UINT8                   Slot,\r
+  IN SD_MMC_BUS_MODE         BusTiming,\r
+  IN BOOLEAN                 FirstTimeSetup,\r
+  IN UINT64                  ClockFreq\r
   )\r
 {\r
   EFI_STATUS                Status;\r
@@ -774,13 +811,15 @@ SdMmcHcClockSupply (
   UINT32                    Divisor;\r
   UINT32                    Remainder;\r
   UINT16                    ClockCtrl;\r
+  UINT32                    BaseClkFreq;\r
+  UINT16                    ControllerVer;\r
+  EFI_PCI_IO_PROTOCOL       *PciIo;\r
 \r
-  //\r
-  // Calculate a divisor for SD clock frequency\r
-  //\r
-  ASSERT (BaseClkFreq != 0);\r
+  PciIo = Private->PciIo;\r
+  BaseClkFreq = Private->BaseClkFreq[Slot];\r
+  ControllerVer = Private->ControllerVersion[Slot];\r
 \r
-  if (ClockFreq == 0) {\r
+  if (BaseClkFreq == 0 || ClockFreq == 0) {\r
     return EFI_INVALID_PARAMETER;\r
   }\r
 \r
@@ -864,11 +903,35 @@ SdMmcHcClockSupply (
     return Status;\r
   }\r
 \r
+  Status = SdMmcHcStartSdClock (PciIo, Slot);\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
   //\r
-  // Set SD Clock Enable in the Clock Control register to 1\r
+  // We don't notify the platform on first time setup to avoid changing\r
+  // legacy behavior. During first time setup we also don't know what type\r
+  // of the card slot it is and which enum value of BusTiming applies.\r
   //\r
-  ClockCtrl = BIT2;\r
-  Status = SdMmcHcOrMmio (PciIo, Slot, SD_MMC_HC_CLOCK_CTRL, sizeof (ClockCtrl), &ClockCtrl);\r
+  if (!FirstTimeSetup && mOverride != NULL && mOverride->NotifyPhase != NULL) {\r
+    Status = mOverride->NotifyPhase (\r
+                          Private->ControllerHandle,\r
+                          Slot,\r
+                          EdkiiSdMmcSwitchClockFreqPost,\r
+                          &BusTiming\r
+                          );\r
+    if (EFI_ERROR (Status)) {\r
+      DEBUG ((\r
+        DEBUG_ERROR,\r
+        "%a: SD/MMC switch clock freq post notifier callback failed - %r\n",\r
+        __FUNCTION__,\r
+        Status\r
+        ));\r
+      return Status;\r
+    }\r
+  }\r
+\r
+  Private->Slot[Slot].CurrentFreq = ClockFreq;\r
 \r
   return Status;\r
 }\r
@@ -991,16 +1054,28 @@ SdMmcHcInitV4Enhancements (
   if (ControllerVer >= SD_MMC_HC_CTRL_VER_400) {\r
     HostCtrl2 = SD_MMC_HC_V4_EN;\r
     //\r
-    // Check if V4 64bit support is available\r
+    // Check if controller version V4.0\r
     //\r
-    if (Capability.SysBus64V4 != 0) {\r
-      HostCtrl2 |= SD_MMC_HC_64_ADDR_EN;\r
-      DEBUG ((DEBUG_INFO, "Enabled V4 64 bit system bus support\n"));\r
+    if (ControllerVer == SD_MMC_HC_CTRL_VER_400) {\r
+      //\r
+      // Check if 64bit support is available\r
+      //\r
+      if (Capability.SysBus64V3 != 0) {\r
+        HostCtrl2 |= SD_MMC_HC_64_ADDR_EN;\r
+        DEBUG ((DEBUG_INFO, "Enabled V4 64 bit system bus support\n"));\r
+      }\r
     }\r
     //\r
     // Check if controller version V4.10 or higher\r
     //\r
-    if (ControllerVer >= SD_MMC_HC_CTRL_VER_410) {\r
+    else if (ControllerVer >= SD_MMC_HC_CTRL_VER_410) {\r
+      //\r
+      // Check if 64bit support is available\r
+      //\r
+      if (Capability.SysBus64V4 != 0) {\r
+        HostCtrl2 |= SD_MMC_HC_64_ADDR_EN;\r
+        DEBUG ((DEBUG_INFO, "Enabled V4 64 bit system bus support\n"));\r
+      }\r
       HostCtrl2 |= SD_MMC_HC_26_DATA_LEN_ADMA_EN;\r
       DEBUG ((DEBUG_INFO, "Enabled V4 26 bit data length ADMA support\n"));\r
     }\r
@@ -1013,49 +1088,6 @@ SdMmcHcInitV4Enhancements (
   return EFI_SUCCESS;\r
 }\r
 \r
-/**\r
-  Supply SD/MMC card with lowest clock frequency at initialization.\r
-\r
-  @param[in] PciIo          The PCI IO protocol instance.\r
-  @param[in] Slot           The slot number of the SD card to send the command to.\r
-  @param[in] BaseClkFreq    The base clock frequency of host controller in MHz.\r
-  @param[in] ControllerVer  The version of host controller.\r
-\r
-  @retval EFI_SUCCESS       The clock is supplied successfully.\r
-  @retval Others            The clock isn't supplied successfully.\r
-\r
-**/\r
-EFI_STATUS\r
-SdMmcHcInitClockFreq (\r
-  IN EFI_PCI_IO_PROTOCOL    *PciIo,\r
-  IN UINT8                  Slot,\r
-  IN UINT32                 BaseClkFreq,\r
-  IN UINT16                 ControllerVer\r
-  )\r
-{\r
-  EFI_STATUS                Status;\r
-  UINT32                    InitFreq;\r
-\r
-  //\r
-  // According to SDHCI specification ver. 4.2, BaseClkFreq field value of\r
-  // the Capability Register 1 can be zero, which means a need for obtaining\r
-  // the clock frequency via another method. Fail in case it is not updated\r
-  // by SW at this point.\r
-  //\r
-  if (BaseClkFreq == 0) {\r
-    //\r
-    // Don't support get Base Clock Frequency information via another method\r
-    //\r
-    return EFI_UNSUPPORTED;\r
-  }\r
-  //\r
-  // Supply 400KHz clock frequency at initialization phase.\r
-  //\r
-  InitFreq = 400;\r
-  Status = SdMmcHcClockSupply (PciIo, Slot, InitFreq, BaseClkFreq, ControllerVer);\r
-  return Status;\r
-}\r
-\r
 /**\r
   Supply SD/MMC card with maximum voltage at initialization.\r
 \r
@@ -1191,7 +1223,14 @@ SdMmcHcInitHost (
     return Status;\r
   }\r
 \r
-  Status = SdMmcHcInitClockFreq (PciIo, Slot, Private->BaseClkFreq[Slot], Private->ControllerVersion[Slot]);\r
+  //\r
+  // Perform first time clock setup with 400 KHz frequency.\r
+  // We send the 0 as the BusTiming value because at this time\r
+  // we still do not know the slot type and which enum value will apply.\r
+  // Since it is a first time setup SdMmcHcClockSupply won't notify\r
+  // the platofrm driver anyway so it doesn't matter.\r
+  //\r
+  Status = SdMmcHcClockSupply (Private, Slot, 0, TRUE, 400);\r
   if (EFI_ERROR (Status)) {\r
     return Status;\r
   }\r
@@ -1314,6 +1353,40 @@ SdMmcHcUhsSignaling (
   return EFI_SUCCESS;\r
 }\r
 \r
+/**\r
+  Set driver strength in host controller.\r
+\r
+  @param[in] PciIo           The PCI IO protocol instance.\r
+  @param[in] SlotIndex       The slot index of the card.\r
+  @param[in] DriverStrength  DriverStrength to set in the controller.\r
+\r
+  @retval EFI_SUCCESS  Driver strength programmed successfully.\r
+  @retval Others       Failed to set driver strength.\r
+**/\r
+EFI_STATUS\r
+SdMmcSetDriverStrength (\r
+  IN EFI_PCI_IO_PROTOCOL      *PciIo,\r
+  IN UINT8                    SlotIndex,\r
+  IN SD_DRIVER_STRENGTH_TYPE  DriverStrength\r
+  )\r
+{\r
+  EFI_STATUS  Status;\r
+  UINT16      HostCtrl2;\r
+\r
+  if (DriverStrength == SdDriverStrengthIgnore) {\r
+    return EFI_SUCCESS;\r
+  }\r
+\r
+  HostCtrl2 = (UINT16)~SD_MMC_HC_CTRL_DRIVER_STRENGTH_MASK;\r
+  Status = SdMmcHcAndMmio (PciIo, SlotIndex, SD_MMC_HC_HOST_CTRL2, sizeof (HostCtrl2), &HostCtrl2);\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
+  HostCtrl2 = (DriverStrength << 4) & SD_MMC_HC_CTRL_DRIVER_STRENGTH_MASK;\r
+  return SdMmcHcOrMmio (PciIo, SlotIndex, SD_MMC_HC_HOST_CTRL2, sizeof (HostCtrl2), &HostCtrl2);\r
+}\r
+\r
 /**\r
   Turn on/off LED.\r
 \r
@@ -1374,14 +1447,10 @@ BuildAdmaDescTable (
   EFI_PCI_IO_PROTOCOL       *PciIo;\r
   EFI_STATUS                Status;\r
   UINTN                     Bytes;\r
-  BOOLEAN                   AddressingMode64;\r
-  BOOLEAN                   DataLength26;\r
   UINT32                    AdmaMaxDataPerLine;\r
   UINT32                    DescSize;\r
   VOID                      *AdmaDesc;\r
 \r
-  AddressingMode64   = FALSE;\r
-  DataLength26       = FALSE;\r
   AdmaMaxDataPerLine = ADMA_MAX_DATA_PER_LINE_16B;\r
   DescSize           = sizeof (SD_MMC_HC_ADMA_32_DESC_LINE);\r
   AdmaDesc           = NULL;\r
@@ -1390,28 +1459,17 @@ BuildAdmaDescTable (
   DataLen = Trb->DataLen;\r
   PciIo   = Trb->Private->PciIo;\r
 \r
-  //\r
-  // Detect whether 64bit addressing is supported.\r
-  //\r
-  if (ControllerVer >= SD_MMC_HC_CTRL_VER_400) {\r
-    Status = SdMmcHcCheckMmioSet(PciIo, Trb->Slot, SD_MMC_HC_HOST_CTRL2, sizeof(UINT16),\r
-                                 SD_MMC_HC_V4_EN|SD_MMC_HC_64_ADDR_EN, SD_MMC_HC_V4_EN|SD_MMC_HC_64_ADDR_EN);\r
-    if (!EFI_ERROR (Status)) {\r
-      AddressingMode64 = TRUE;\r
-      DescSize = sizeof (SD_MMC_HC_ADMA_64_DESC_LINE);\r
-    }\r
-  }\r
   //\r
   // Check for valid ranges in 32bit ADMA Descriptor Table\r
   //\r
-  if (!AddressingMode64 &&\r
+  if ((Trb->Mode == SdMmcAdma32bMode) &&\r
       ((Data >= 0x100000000ul) || ((Data + DataLen) > 0x100000000ul))) {\r
     return EFI_INVALID_PARAMETER;\r
   }\r
   //\r
   // Check address field alignment\r
   //\r
-  if (AddressingMode64) {\r
+  if (Trb->Mode != SdMmcAdma32bMode) {\r
     //\r
     // Address field shall be set on 64-bit boundary (Lower 3-bit is always set to 0)\r
     //\r
@@ -1426,13 +1484,19 @@ BuildAdmaDescTable (
       DEBUG ((DEBUG_INFO, "The buffer [0x%x] to construct ADMA desc is not aligned to 4 bytes boundary!\n", Data));\r
     }\r
   }\r
+\r
+  //\r
+  // Configure 64b ADMA.\r
+  //\r
+  if (Trb->Mode == SdMmcAdma64bV3Mode) {\r
+    DescSize = sizeof (SD_MMC_HC_ADMA_64_V3_DESC_LINE);\r
+  }else if (Trb->Mode == SdMmcAdma64bV4Mode) {\r
+    DescSize = sizeof (SD_MMC_HC_ADMA_64_V4_DESC_LINE);\r
+  }\r
   //\r
-  // Detect whether 26bit data length is supported.\r
+  // Configure 26b data length.\r
   //\r
-  Status = SdMmcHcCheckMmioSet(PciIo, Trb->Slot, SD_MMC_HC_HOST_CTRL2, sizeof(UINT16),\r
-                               SD_MMC_HC_26_DATA_LEN_ADMA_EN, SD_MMC_HC_26_DATA_LEN_ADMA_EN);\r
-  if (!EFI_ERROR (Status)) {\r
-    DataLength26 = TRUE;\r
+  if (Trb->AdmaLengthMode == SdMmcAdmaLen26b) {\r
     AdmaMaxDataPerLine = ADMA_MAX_DATA_PER_LINE_26B;\r
   }\r
 \r
@@ -1473,7 +1537,7 @@ BuildAdmaDescTable (
     return EFI_OUT_OF_RESOURCES;\r
   }\r
 \r
-  if ((!AddressingMode64) &&\r
+  if ((Trb->Mode == SdMmcAdma32bMode) &&\r
       (UINT64)(UINTN)Trb->AdmaDescPhy > 0x100000000ul) {\r
     //\r
     // The ADMA doesn't support 64bit addressing.\r
@@ -1482,6 +1546,8 @@ BuildAdmaDescTable (
       PciIo,\r
       Trb->AdmaMap\r
     );\r
+    Trb->AdmaMap = NULL;\r
+\r
     PciIo->FreeBuffer (\r
       PciIo,\r
       EFI_SIZE_TO_PAGES (TableSize),\r
@@ -1492,19 +1558,20 @@ BuildAdmaDescTable (
 \r
   Remaining = DataLen;\r
   Address   = Data;\r
-  if (!AddressingMode64) {\r
+  if (Trb->Mode == SdMmcAdma32bMode) {\r
     Trb->Adma32Desc = AdmaDesc;\r
-    Trb->Adma64Desc = NULL;\r
+  } else if (Trb->Mode == SdMmcAdma64bV3Mode) {\r
+    Trb->Adma64V3Desc = AdmaDesc;\r
   } else {\r
-    Trb->Adma64Desc = AdmaDesc;\r
-    Trb->Adma32Desc = NULL;\r
+    Trb->Adma64V4Desc = AdmaDesc;\r
   }\r
+\r
   for (Index = 0; Index < Entries; Index++) {\r
-    if (!AddressingMode64) {\r
+    if (Trb->Mode == SdMmcAdma32bMode) {\r
       if (Remaining <= AdmaMaxDataPerLine) {\r
         Trb->Adma32Desc[Index].Valid = 1;\r
         Trb->Adma32Desc[Index].Act   = 2;\r
-        if (DataLength26) {\r
+        if (Trb->AdmaLengthMode == SdMmcAdmaLen26b) {\r
           Trb->Adma32Desc[Index].UpperLength = (UINT16)RShiftU64 (Remaining, 16);\r
         }\r
         Trb->Adma32Desc[Index].LowerLength = (UINT16)(Remaining & MAX_UINT16);\r
@@ -1513,32 +1580,53 @@ BuildAdmaDescTable (
       } else {\r
         Trb->Adma32Desc[Index].Valid = 1;\r
         Trb->Adma32Desc[Index].Act   = 2;\r
-        if (DataLength26) {\r
+        if (Trb->AdmaLengthMode == SdMmcAdmaLen26b) {\r
           Trb->Adma32Desc[Index].UpperLength  = 0;\r
         }\r
         Trb->Adma32Desc[Index].LowerLength  = 0;\r
         Trb->Adma32Desc[Index].Address = (UINT32)Address;\r
       }\r
+    } else if (Trb->Mode == SdMmcAdma64bV3Mode) {\r
+      if (Remaining <= AdmaMaxDataPerLine) {\r
+        Trb->Adma64V3Desc[Index].Valid = 1;\r
+        Trb->Adma64V3Desc[Index].Act   = 2;\r
+        if (Trb->AdmaLengthMode == SdMmcAdmaLen26b) {\r
+          Trb->Adma64V3Desc[Index].UpperLength  = (UINT16)RShiftU64 (Remaining, 16);\r
+        }\r
+        Trb->Adma64V3Desc[Index].LowerLength  = (UINT16)(Remaining & MAX_UINT16);\r
+        Trb->Adma64V3Desc[Index].LowerAddress = (UINT32)Address;\r
+        Trb->Adma64V3Desc[Index].UpperAddress = (UINT32)RShiftU64 (Address, 32);\r
+        break;\r
+      } else {\r
+        Trb->Adma64V3Desc[Index].Valid = 1;\r
+        Trb->Adma64V3Desc[Index].Act   = 2;\r
+        if (Trb->AdmaLengthMode == SdMmcAdmaLen26b) {\r
+          Trb->Adma64V3Desc[Index].UpperLength  = 0;\r
+        }\r
+        Trb->Adma64V3Desc[Index].LowerLength  = 0;\r
+        Trb->Adma64V3Desc[Index].LowerAddress = (UINT32)Address;\r
+        Trb->Adma64V3Desc[Index].UpperAddress = (UINT32)RShiftU64 (Address, 32);\r
+      }\r
     } else {\r
       if (Remaining <= AdmaMaxDataPerLine) {\r
-        Trb->Adma64Desc[Index].Valid = 1;\r
-        Trb->Adma64Desc[Index].Act   = 2;\r
-        if (DataLength26) {\r
-          Trb->Adma64Desc[Index].UpperLength  = (UINT16)RShiftU64 (Remaining, 16);\r
+        Trb->Adma64V4Desc[Index].Valid = 1;\r
+        Trb->Adma64V4Desc[Index].Act   = 2;\r
+        if (Trb->AdmaLengthMode == SdMmcAdmaLen26b) {\r
+          Trb->Adma64V4Desc[Index].UpperLength  = (UINT16)RShiftU64 (Remaining, 16);\r
         }\r
-        Trb->Adma64Desc[Index].LowerLength  = (UINT16)(Remaining & MAX_UINT16);\r
-        Trb->Adma64Desc[Index].LowerAddress = (UINT32)Address;\r
-        Trb->Adma64Desc[Index].UpperAddress = (UINT32)RShiftU64 (Address, 32);\r
+        Trb->Adma64V4Desc[Index].LowerLength  = (UINT16)(Remaining & MAX_UINT16);\r
+        Trb->Adma64V4Desc[Index].LowerAddress = (UINT32)Address;\r
+        Trb->Adma64V4Desc[Index].UpperAddress = (UINT32)RShiftU64 (Address, 32);\r
         break;\r
       } else {\r
-        Trb->Adma64Desc[Index].Valid = 1;\r
-        Trb->Adma64Desc[Index].Act   = 2;\r
-        if (DataLength26) {\r
-          Trb->Adma64Desc[Index].UpperLength  = 0;\r
+        Trb->Adma64V4Desc[Index].Valid = 1;\r
+        Trb->Adma64V4Desc[Index].Act   = 2;\r
+        if (Trb->AdmaLengthMode == SdMmcAdmaLen26b) {\r
+          Trb->Adma64V4Desc[Index].UpperLength  = 0;\r
         }\r
-        Trb->Adma64Desc[Index].LowerLength  = 0;\r
-        Trb->Adma64Desc[Index].LowerAddress = (UINT32)Address;\r
-        Trb->Adma64Desc[Index].UpperAddress = (UINT32)RShiftU64 (Address, 32);\r
+        Trb->Adma64V4Desc[Index].LowerLength  = 0;\r
+        Trb->Adma64V4Desc[Index].LowerAddress = (UINT32)Address;\r
+        Trb->Adma64V4Desc[Index].UpperAddress = (UINT32)RShiftU64 (Address, 32);\r
       }\r
     }\r
 \r
@@ -1549,7 +1637,148 @@ BuildAdmaDescTable (
   //\r
   // Set the last descriptor line as end of descriptor table\r
   //\r
-  AddressingMode64 ? (Trb->Adma64Desc[Index].End = 1) : (Trb->Adma32Desc[Index].End = 1);\r
+  if (Trb->Mode == SdMmcAdma32bMode) {\r
+    Trb->Adma32Desc[Index].End = 1;\r
+  } else if (Trb->Mode == SdMmcAdma64bV3Mode) {\r
+    Trb->Adma64V3Desc[Index].End = 1;\r
+  } else {\r
+    Trb->Adma64V4Desc[Index].End = 1;\r
+  }\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+/**\r
+  Prints the contents of the command packet to the debug port.\r
+\r
+  @param[in] DebugLevel  Debug level at which the packet should be printed.\r
+  @param[in] Packet      Pointer to packet to print.\r
+**/\r
+VOID\r
+SdMmcPrintPacket (\r
+  IN UINT32                               DebugLevel,\r
+  IN EFI_SD_MMC_PASS_THRU_COMMAND_PACKET  *Packet\r
+  )\r
+{\r
+  if (Packet == NULL) {\r
+    return;\r
+  }\r
+\r
+  DEBUG ((DebugLevel, "Printing EFI_SD_MMC_PASS_THRU_COMMAND_PACKET\n"));\r
+  if (Packet->SdMmcCmdBlk != NULL) {\r
+    DEBUG ((DebugLevel, "Command index: %d, argument: %X\n", Packet->SdMmcCmdBlk->CommandIndex, Packet->SdMmcCmdBlk->CommandArgument));\r
+    DEBUG ((DebugLevel, "Command type: %d, response type: %d\n", Packet->SdMmcCmdBlk->CommandType, Packet->SdMmcCmdBlk->ResponseType));\r
+  }\r
+  if (Packet->SdMmcStatusBlk != NULL) {\r
+    DEBUG ((DebugLevel, "Response 0: %X, 1: %X, 2: %X, 3: %X\n",\r
+                           Packet->SdMmcStatusBlk->Resp0,\r
+                           Packet->SdMmcStatusBlk->Resp1,\r
+                           Packet->SdMmcStatusBlk->Resp2,\r
+                           Packet->SdMmcStatusBlk->Resp3\r
+                           ));\r
+  }\r
+  DEBUG ((DebugLevel, "Timeout: %ld\n", Packet->Timeout));\r
+  DEBUG ((DebugLevel, "InDataBuffer: %p\n", Packet->InDataBuffer));\r
+  DEBUG ((DebugLevel, "OutDataBuffer: %p\n", Packet->OutDataBuffer));\r
+  DEBUG ((DebugLevel, "InTransferLength: %d\n", Packet->InTransferLength));\r
+  DEBUG ((DebugLevel, "OutTransferLength: %d\n", Packet->OutTransferLength));\r
+  DEBUG ((DebugLevel, "TransactionStatus: %r\n", Packet->TransactionStatus));\r
+}\r
+\r
+/**\r
+  Prints the contents of the TRB to the debug port.\r
+\r
+  @param[in] DebugLevel  Debug level at which the TRB should be printed.\r
+  @param[in] Trb         Pointer to the TRB structure.\r
+**/\r
+VOID\r
+SdMmcPrintTrb (\r
+  IN UINT32         DebugLevel,\r
+  IN SD_MMC_HC_TRB  *Trb\r
+  )\r
+{\r
+  if (Trb == NULL) {\r
+    return;\r
+  }\r
+\r
+  DEBUG ((DebugLevel, "Printing SD_MMC_HC_TRB\n"));\r
+  DEBUG ((DebugLevel, "Slot: %d\n", Trb->Slot));\r
+  DEBUG ((DebugLevel, "BlockSize: %d\n", Trb->BlockSize));\r
+  DEBUG ((DebugLevel, "Data: %p\n", Trb->Data));\r
+  DEBUG ((DebugLevel, "DataLen: %d\n", Trb->DataLen));\r
+  DEBUG ((DebugLevel, "Read: %d\n", Trb->Read));\r
+  DEBUG ((DebugLevel, "DataPhy: %lX\n", Trb->DataPhy));\r
+  DEBUG ((DebugLevel, "DataMap: %p\n", Trb->DataMap));\r
+  DEBUG ((DebugLevel, "Mode: %d\n", Trb->Mode));\r
+  DEBUG ((DebugLevel, "AdmaLengthMode: %d\n", Trb->AdmaLengthMode));\r
+  DEBUG ((DebugLevel, "Event: %p\n", Trb->Event));\r
+  DEBUG ((DebugLevel, "Started: %d\n", Trb->Started));\r
+  DEBUG ((DebugLevel, "CommandComplete: %d\n", Trb->CommandComplete));\r
+  DEBUG ((DebugLevel, "Timeout: %ld\n", Trb->Timeout));\r
+  DEBUG ((DebugLevel, "Retries: %d\n", Trb->Retries));\r
+  DEBUG ((DebugLevel, "PioModeTransferCompleted: %d\n", Trb->PioModeTransferCompleted));\r
+  DEBUG ((DebugLevel, "PioBlockIndex: %d\n", Trb->PioBlockIndex));\r
+  DEBUG ((DebugLevel, "Adma32Desc: %p\n", Trb->Adma32Desc));\r
+  DEBUG ((DebugLevel, "Adma64V3Desc: %p\n", Trb->Adma64V3Desc));\r
+  DEBUG ((DebugLevel, "Adma64V4Desc: %p\n", Trb->Adma64V4Desc));\r
+  DEBUG ((DebugLevel, "AdmaMap: %p\n", Trb->AdmaMap));\r
+  DEBUG ((DebugLevel, "AdmaPages: %X\n", Trb->AdmaPages));\r
+\r
+  SdMmcPrintPacket (DebugLevel, Trb->Packet);\r
+}\r
+\r
+/**\r
+  Sets up host memory to allow DMA transfer.\r
+\r
+  @param[in] Private  A pointer to the SD_MMC_HC_PRIVATE_DATA instance.\r
+  @param[in] Slot     The slot number of the SD card to send the command to.\r
+  @param[in] Packet   A pointer to the SD command data structure.\r
+\r
+  @retval EFI_SUCCESS  Memory has been mapped for DMA transfer.\r
+  @retval Others       Memory has not been mapped.\r
+**/\r
+EFI_STATUS\r
+SdMmcSetupMemoryForDmaTransfer (\r
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,\r
+  IN UINT8                   Slot,\r
+  IN SD_MMC_HC_TRB           *Trb\r
+  )\r
+{\r
+  EFI_PCI_IO_PROTOCOL_OPERATION Flag;\r
+  EFI_PCI_IO_PROTOCOL           *PciIo;\r
+  UINTN                         MapLength;\r
+  EFI_STATUS                    Status;\r
+\r
+  if (Trb->Read) {\r
+    Flag = EfiPciIoOperationBusMasterWrite;\r
+  } else {\r
+    Flag = EfiPciIoOperationBusMasterRead;\r
+  }\r
+\r
+  PciIo = Private->PciIo;\r
+  if (Trb->Data != NULL && Trb->DataLen != 0) {\r
+    MapLength = Trb->DataLen;\r
+    Status = PciIo->Map (\r
+                      PciIo,\r
+                      Flag,\r
+                      Trb->Data,\r
+                      &MapLength,\r
+                      &Trb->DataPhy,\r
+                      &Trb->DataMap\r
+                      );\r
+    if (EFI_ERROR (Status) || (Trb->DataLen != MapLength)) {\r
+      return EFI_BAD_BUFFER_SIZE;\r
+    }\r
+  }\r
+\r
+  if (Trb->Mode == SdMmcAdma32bMode ||\r
+      Trb->Mode == SdMmcAdma64bV3Mode ||\r
+      Trb->Mode == SdMmcAdma64bV4Mode) {\r
+    Status = BuildAdmaDescTable (Trb, Private->ControllerVersion[Slot]);\r
+    if (EFI_ERROR (Status)) {\r
+      return Status;\r
+    }\r
+  }\r
+\r
   return EFI_SUCCESS;\r
 }\r
 \r
@@ -1577,9 +1806,6 @@ SdMmcCreateTrb (
   SD_MMC_HC_TRB                 *Trb;\r
   EFI_STATUS                    Status;\r
   EFI_TPL                       OldTpl;\r
-  EFI_PCI_IO_PROTOCOL_OPERATION Flag;\r
-  EFI_PCI_IO_PROTOCOL           *PciIo;\r
-  UINTN                         MapLength;\r
 \r
   Trb = AllocateZeroPool (sizeof (SD_MMC_HC_TRB));\r
   if (Trb == NULL) {\r
@@ -1592,7 +1818,11 @@ SdMmcCreateTrb (
   Trb->Packet    = Packet;\r
   Trb->Event     = Event;\r
   Trb->Started   = FALSE;\r
+  Trb->CommandComplete = FALSE;\r
   Trb->Timeout   = Packet->Timeout;\r
+  Trb->Retries   = SD_MMC_TRB_RETRIES;\r
+  Trb->PioModeTransferCompleted = FALSE;\r
+  Trb->PioBlockIndex = 0;\r
   Trb->Private   = Private;\r
 \r
   if ((Packet->InTransferLength != 0) && (Packet->InDataBuffer != NULL)) {\r
@@ -1620,40 +1850,33 @@ SdMmcCreateTrb (
        (Packet->SdMmcCmdBlk->CommandIndex == SD_SEND_TUNING_BLOCK))) {\r
     Trb->Mode = SdMmcPioMode;\r
   } else {\r
-    if (Trb->Read) {\r
-      Flag = EfiPciIoOperationBusMasterWrite;\r
-    } else {\r
-      Flag = EfiPciIoOperationBusMasterRead;\r
-    }\r
-\r
-    PciIo = Private->PciIo;\r
-    if (Trb->DataLen != 0) {\r
-      MapLength = Trb->DataLen;\r
-      Status = PciIo->Map (\r
-                        PciIo,\r
-                        Flag,\r
-                        Trb->Data,\r
-                        &MapLength,\r
-                        &Trb->DataPhy,\r
-                        &Trb->DataMap\r
-                        );\r
-      if (EFI_ERROR (Status) || (Trb->DataLen != MapLength)) {\r
-        Status = EFI_BAD_BUFFER_SIZE;\r
-        goto Error;\r
-      }\r
-    }\r
-\r
     if (Trb->DataLen == 0) {\r
       Trb->Mode = SdMmcNoData;\r
     } else if (Private->Capability[Slot].Adma2 != 0) {\r
-      Trb->Mode = SdMmcAdmaMode;\r
-      Status = BuildAdmaDescTable (Trb, Private->ControllerVersion[Slot]);\r
+      Trb->Mode = SdMmcAdma32bMode;\r
+      Trb->AdmaLengthMode = SdMmcAdmaLen16b;\r
+      if ((Private->ControllerVersion[Slot] == SD_MMC_HC_CTRL_VER_300) &&\r
+          (Private->Capability[Slot].SysBus64V3 == 1)) {\r
+        Trb->Mode = SdMmcAdma64bV3Mode;\r
+      } else if (((Private->ControllerVersion[Slot] == SD_MMC_HC_CTRL_VER_400) &&\r
+                  (Private->Capability[Slot].SysBus64V3 == 1)) ||\r
+                 ((Private->ControllerVersion[Slot] >= SD_MMC_HC_CTRL_VER_410) &&\r
+                  (Private->Capability[Slot].SysBus64V4 == 1))) {\r
+        Trb->Mode = SdMmcAdma64bV4Mode;\r
+      }\r
+      if (Private->ControllerVersion[Slot] >= SD_MMC_HC_CTRL_VER_410) {\r
+        Trb->AdmaLengthMode = SdMmcAdmaLen26b;\r
+      }\r
+      Status = SdMmcSetupMemoryForDmaTransfer (Private, Slot, Trb);\r
       if (EFI_ERROR (Status)) {\r
-        PciIo->Unmap (PciIo, Trb->DataMap);\r
         goto Error;\r
       }\r
     } else if (Private->Capability[Slot].Sdma != 0) {\r
       Trb->Mode = SdMmcSdmaMode;\r
+      Status = SdMmcSetupMemoryForDmaTransfer (Private, Slot, Trb);\r
+      if (EFI_ERROR (Status)) {\r
+        goto Error;\r
+      }\r
     } else {\r
       Trb->Mode = SdMmcPioMode;\r
     }\r
@@ -1700,11 +1923,18 @@ SdMmcFreeTrb (
       Trb->Adma32Desc\r
     );\r
   }\r
-  if (Trb->Adma64Desc != NULL) {\r
+  if (Trb->Adma64V3Desc != NULL) {\r
+    PciIo->FreeBuffer (\r
+      PciIo,\r
+      Trb->AdmaPages,\r
+      Trb->Adma64V3Desc\r
+    );\r
+  }\r
+  if (Trb->Adma64V4Desc != NULL) {\r
     PciIo->FreeBuffer (\r
       PciIo,\r
       Trb->AdmaPages,\r
-      Trb->Adma64Desc\r
+      Trb->Adma64V4Desc\r
     );\r
   }\r
   if (Trb->DataMap != NULL) {\r
@@ -1872,27 +2102,35 @@ SdMmcExecTrb (
   if (EFI_ERROR (Status)) {\r
     return Status;\r
   }\r
+\r
+  if (Private->ControllerVersion[Trb->Slot] >= SD_MMC_HC_CTRL_VER_400) {\r
+    Status = SdMmcHcCheckMmioSet(PciIo, Trb->Slot, SD_MMC_HC_HOST_CTRL2, sizeof(UINT16),\r
+                                 SD_MMC_HC_64_ADDR_EN, SD_MMC_HC_64_ADDR_EN);\r
+    if (!EFI_ERROR (Status)) {\r
+      AddressingMode64 = TRUE;\r
+    }\r
+  }\r
+\r
   //\r
   // Set Host Control 1 register DMA Select field\r
   //\r
-  if (Trb->Mode == SdMmcAdmaMode) {\r
+  if ((Trb->Mode == SdMmcAdma32bMode) ||\r
+      (Trb->Mode == SdMmcAdma64bV4Mode)) {\r
     HostCtrl1 = BIT4;\r
     Status = SdMmcHcOrMmio (PciIo, Trb->Slot, SD_MMC_HC_HOST_CTRL1, sizeof (HostCtrl1), &HostCtrl1);\r
     if (EFI_ERROR (Status)) {\r
       return Status;\r
     }\r
+  } else if (Trb->Mode == SdMmcAdma64bV3Mode) {\r
+    HostCtrl1 = BIT4|BIT3;\r
+    Status = SdMmcHcOrMmio (PciIo, Trb->Slot, SD_MMC_HC_HOST_CTRL1, sizeof (HostCtrl1), &HostCtrl1);\r
+    if (EFI_ERROR (Status)) {\r
+      return Status;\r
+    }\r
   }\r
 \r
   SdMmcHcLedOnOff (PciIo, Trb->Slot, TRUE);\r
 \r
-  if (Private->ControllerVersion[Trb->Slot] >= SD_MMC_HC_CTRL_VER_400) {\r
-    Status = SdMmcHcCheckMmioSet(PciIo, Trb->Slot, SD_MMC_HC_HOST_CTRL2, sizeof(UINT16),\r
-                                 SD_MMC_HC_V4_EN|SD_MMC_HC_64_ADDR_EN, SD_MMC_HC_V4_EN|SD_MMC_HC_64_ADDR_EN);\r
-    if (!EFI_ERROR (Status)) {\r
-      AddressingMode64 = TRUE;\r
-    }\r
-  }\r
-\r
   if (Trb->Mode == SdMmcSdmaMode) {\r
     if ((!AddressingMode64) &&\r
         ((UINT64)(UINTN)Trb->DataPhy >= 0x100000000ul)) {\r
@@ -1910,7 +2148,9 @@ SdMmcExecTrb (
     if (EFI_ERROR (Status)) {\r
       return Status;\r
     }\r
-  } else if (Trb->Mode == SdMmcAdmaMode) {\r
+  } else if ((Trb->Mode == SdMmcAdma32bMode) ||\r
+             (Trb->Mode == SdMmcAdma64bV3Mode) ||\r
+             (Trb->Mode == SdMmcAdma64bV4Mode)) {\r
     AdmaAddr = (UINT64)(UINTN)Trb->AdmaDescPhy;\r
     Status   = SdMmcHcRwMmio (PciIo, Trb->Slot, SD_MMC_HC_ADMA_SYS_ADDR, FALSE, sizeof (AdmaAddr), &AdmaAddr);\r
     if (EFI_ERROR (Status)) {\r
@@ -2018,240 +2258,528 @@ SdMmcExecTrb (
 }\r
 \r
 /**\r
-  Check the TRB execution result.\r
-\r
-  @param[in] Private        A pointer to the SD_MMC_HC_PRIVATE_DATA instance.\r
-  @param[in] Trb            The pointer to the SD_MMC_HC_TRB instance.\r
+  Performs SW reset based on passed error status mask.\r
 \r
-  @retval EFI_SUCCESS       The TRB is executed successfully.\r
-  @retval EFI_NOT_READY     The TRB is not completed for execution.\r
-  @retval Others            Some erros happen when executing this request.\r
+  @param[in]  Private       Pointer to driver private data.\r
+  @param[in]  Slot          Index of the slot to reset.\r
+  @param[in]  ErrIntStatus  Error interrupt status mask.\r
 \r
+  @retval EFI_SUCCESS  Software reset performed successfully.\r
+  @retval Other        Software reset failed.\r
 **/\r
 EFI_STATUS\r
-SdMmcCheckTrbResult (\r
-  IN SD_MMC_HC_PRIVATE_DATA           *Private,\r
-  IN SD_MMC_HC_TRB                    *Trb\r
+SdMmcSoftwareReset (\r
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,\r
+  IN UINT8                   Slot,\r
+  IN UINT16                  ErrIntStatus\r
   )\r
 {\r
-  EFI_STATUS                          Status;\r
-  EFI_SD_MMC_PASS_THRU_COMMAND_PACKET *Packet;\r
-  UINT16                              IntStatus;\r
-  UINT32                              Response[4];\r
-  UINT64                              SdmaAddr;\r
-  UINT8                               Index;\r
-  UINT8                               SwReset;\r
-  UINT32                              PioLength;\r
+  UINT8       SwReset;\r
+  EFI_STATUS  Status;\r
 \r
   SwReset = 0;\r
-  Packet  = Trb->Packet;\r
-  //\r
-  // Check Trb execution result by reading Normal Interrupt Status register.\r
-  //\r
+  if ((ErrIntStatus & 0x0F) != 0) {\r
+    SwReset |= BIT1;\r
+  }\r
+  if ((ErrIntStatus & 0x70) != 0) {\r
+    SwReset |= BIT2;\r
+  }\r
+\r
+  Status  = SdMmcHcRwMmio (\r
+              Private->PciIo,\r
+              Slot,\r
+              SD_MMC_HC_SW_RST,\r
+              FALSE,\r
+              sizeof (SwReset),\r
+              &SwReset\r
+              );\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
+  Status = SdMmcHcWaitMmioSet (\r
+             Private->PciIo,\r
+             Slot,\r
+             SD_MMC_HC_SW_RST,\r
+             sizeof (SwReset),\r
+             0xFF,\r
+             0,\r
+             SD_MMC_HC_GENERIC_TIMEOUT\r
+             );\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+/**\r
+  Checks the error status in error status register\r
+  and issues appropriate software reset as described in\r
+  SD specification section 3.10.\r
+\r
+  @param[in] Private    Pointer to driver private data.\r
+  @param[in] Slot       Index of the slot for device.\r
+  @param[in] IntStatus  Normal interrupt status mask.\r
+\r
+  @retval EFI_CRC_ERROR  CRC error happened during CMD execution.\r
+  @retval EFI_SUCCESS    No error reported.\r
+  @retval Others         Some other error happened.\r
+\r
+**/\r
+EFI_STATUS\r
+SdMmcCheckAndRecoverErrors (\r
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,\r
+  IN UINT8                   Slot,\r
+  IN UINT16                  IntStatus\r
+  )\r
+{\r
+  UINT16      ErrIntStatus;\r
+  EFI_STATUS  Status;\r
+  EFI_STATUS  ErrorStatus;\r
+\r
+  if ((IntStatus & BIT15) == 0) {\r
+    return EFI_SUCCESS;\r
+  }\r
+\r
   Status = SdMmcHcRwMmio (\r
              Private->PciIo,\r
-             Trb->Slot,\r
-             SD_MMC_HC_NOR_INT_STS,\r
+             Slot,\r
+             SD_MMC_HC_ERR_INT_STS,\r
              TRUE,\r
-             sizeof (IntStatus),\r
-             &IntStatus\r
+             sizeof (ErrIntStatus),\r
+             &ErrIntStatus\r
              );\r
   if (EFI_ERROR (Status)) {\r
-    goto Done;\r
+    return Status;\r
   }\r
+\r
+  DEBUG ((DEBUG_ERROR, "Error reported by SDHCI\n"));\r
+  DEBUG ((DEBUG_ERROR, "Interrupt status = %X\n", IntStatus));\r
+  DEBUG ((DEBUG_ERROR, "Error interrupt status = %X\n", ErrIntStatus));\r
+\r
   //\r
-  // Check Transfer Complete bit is set or not.\r
+  // If the data timeout error is reported\r
+  // but data transfer is signaled as completed we\r
+  // have to ignore data timeout. We also assume that no\r
+  // other error is present on the link since data transfer\r
+  // completed successfully. Error interrupt status\r
+  // register is going to be reset when the next command\r
+  // is started.\r
   //\r
-  if ((IntStatus & BIT1) == BIT1) {\r
-    if ((IntStatus & BIT15) == BIT15) {\r
-      //\r
-      // Read Error Interrupt Status register to check if the error is\r
-      // Data Timeout Error.\r
-      // If yes, treat it as success as Transfer Complete has higher\r
-      // priority than Data Timeout Error.\r
-      //\r
-      Status = SdMmcHcRwMmio (\r
-                 Private->PciIo,\r
-                 Trb->Slot,\r
-                 SD_MMC_HC_ERR_INT_STS,\r
-                 TRUE,\r
-                 sizeof (IntStatus),\r
-                 &IntStatus\r
-                 );\r
-      if (!EFI_ERROR (Status)) {\r
-        if ((IntStatus & BIT4) == BIT4) {\r
-          Status = EFI_SUCCESS;\r
-        } else {\r
-          Status = EFI_DEVICE_ERROR;\r
-        }\r
-      }\r
-    }\r
-\r
-    goto Done;\r
+  if (((ErrIntStatus & BIT4) != 0) && ((IntStatus & BIT1) != 0)) {\r
+    return EFI_SUCCESS;\r
   }\r
+\r
   //\r
-  // Check if there is a error happened during cmd execution.\r
-  // If yes, then do error recovery procedure to follow SD Host Controller\r
-  // Simplified Spec 3.0 section 3.10.1.\r
+  // We treat both CMD and DAT CRC errors and\r
+  // end bits errors as EFI_CRC_ERROR. This will\r
+  // let higher layer know that the error possibly\r
+  // happened due to random bus condition and the\r
+  // command can be retried.\r
   //\r
-  if ((IntStatus & BIT15) == BIT15) {\r
+  if ((ErrIntStatus & (BIT1 | BIT2 | BIT5 | BIT6)) != 0) {\r
+    ErrorStatus = EFI_CRC_ERROR;\r
+  } else {\r
+    ErrorStatus = EFI_DEVICE_ERROR;\r
+  }\r
+\r
+  Status = SdMmcSoftwareReset (Private, Slot, ErrIntStatus);\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
+  return ErrorStatus;\r
+}\r
+\r
+/**\r
+  Reads the response data into the TRB buffer.\r
+  This function assumes that caller made sure that\r
+  command has completed.\r
+\r
+  @param[in] Private  A pointer to the SD_MMC_HC_PRIVATE_DATA instance.\r
+  @param[in] Trb      The pointer to the SD_MMC_HC_TRB instance.\r
+\r
+  @retval EFI_SUCCESS  Response read successfully.\r
+  @retval Others       Failed to get response.\r
+**/\r
+EFI_STATUS\r
+SdMmcGetResponse (\r
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,\r
+  IN SD_MMC_HC_TRB           *Trb\r
+  )\r
+{\r
+  EFI_SD_MMC_PASS_THRU_COMMAND_PACKET  *Packet;\r
+  UINT8                                Index;\r
+  UINT32                               Response[4];\r
+  EFI_STATUS                           Status;\r
+\r
+  Packet = Trb->Packet;\r
+\r
+  if (Packet->SdMmcCmdBlk->CommandType == SdMmcCommandTypeBc) {\r
+    return EFI_SUCCESS;\r
+  }\r
+\r
+  for (Index = 0; Index < 4; Index++) {\r
     Status = SdMmcHcRwMmio (\r
                Private->PciIo,\r
                Trb->Slot,\r
-               SD_MMC_HC_ERR_INT_STS,\r
+               SD_MMC_HC_RESPONSE + Index * 4,\r
                TRUE,\r
-               sizeof (IntStatus),\r
-               &IntStatus\r
+               sizeof (UINT32),\r
+               &Response[Index]\r
+               );\r
+      if (EFI_ERROR (Status)) {\r
+        return Status;\r
+      }\r
+    }\r
+  CopyMem (Packet->SdMmcStatusBlk, Response, sizeof (Response));\r
+\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+/**\r
+  Checks if the command completed. If the command\r
+  completed it gets the response and records the\r
+  command completion in the TRB.\r
+\r
+  @param[in] Private    A pointer to the SD_MMC_HC_PRIVATE_DATA instance.\r
+  @param[in] Trb        The pointer to the SD_MMC_HC_TRB instance.\r
+  @param[in] IntStatus  Snapshot of the normal interrupt status register.\r
+\r
+  @retval EFI_SUCCESS   Command completed successfully.\r
+  @retval EFI_NOT_READY Command completion still pending.\r
+  @retval Others        Command failed to complete.\r
+**/\r
+EFI_STATUS\r
+SdMmcCheckCommandComplete (\r
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,\r
+  IN SD_MMC_HC_TRB           *Trb,\r
+  IN UINT16                  IntStatus\r
+  )\r
+{\r
+  UINT16      Data16;\r
+  EFI_STATUS  Status;\r
+\r
+  if ((IntStatus & BIT0) != 0) {\r
+    Data16 = BIT0;\r
+    Status = SdMmcHcRwMmio (\r
+               Private->PciIo,\r
+               Trb->Slot,\r
+               SD_MMC_HC_NOR_INT_STS,\r
+               FALSE,\r
+               sizeof (Data16),\r
+               &Data16\r
                );\r
     if (EFI_ERROR (Status)) {\r
-      goto Done;\r
+      return Status;\r
     }\r
-    if ((IntStatus & 0x0F) != 0) {\r
-      SwReset |= BIT1;\r
+    Status = SdMmcGetResponse (Private, Trb);\r
+    if (EFI_ERROR (Status)) {\r
+      return Status;\r
     }\r
-    if ((IntStatus & 0xF0) != 0) {\r
-      SwReset |= BIT2;\r
+    Trb->CommandComplete = TRUE;\r
+    return EFI_SUCCESS;\r
+  }\r
+\r
+  return EFI_NOT_READY;\r
+}\r
+\r
+/**\r
+  Transfers data from card using PIO method.\r
+\r
+  @param[in] Private    A pointer to the SD_MMC_HC_PRIVATE_DATA instance.\r
+  @param[in] Trb        The pointer to the SD_MMC_HC_TRB instance.\r
+  @param[in] IntStatus  Snapshot of the normal interrupt status register.\r
+\r
+  @retval EFI_SUCCESS   PIO transfer completed successfully.\r
+  @retval EFI_NOT_READY PIO transfer completion still pending.\r
+  @retval Others        PIO transfer failed to complete.\r
+**/\r
+EFI_STATUS\r
+SdMmcTransferDataWithPio (\r
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,\r
+  IN SD_MMC_HC_TRB           *Trb,\r
+  IN UINT16                  IntStatus\r
+  )\r
+{\r
+  EFI_STATUS                  Status;\r
+  UINT16                      Data16;\r
+  UINT32                      BlockCount;\r
+  EFI_PCI_IO_PROTOCOL_WIDTH  Width;\r
+  UINTN                       Count;\r
+\r
+  BlockCount = (Trb->DataLen / Trb->BlockSize);\r
+  if (Trb->DataLen % Trb->BlockSize != 0) {\r
+    BlockCount += 1;\r
+  }\r
+\r
+  if (Trb->PioBlockIndex >= BlockCount) {\r
+    return EFI_SUCCESS;\r
+  }\r
+\r
+  switch (Trb->BlockSize % sizeof (UINT32)) {\r
+    case 0:\r
+      Width = EfiPciIoWidthFifoUint32;\r
+      Count = Trb->BlockSize / sizeof (UINT32);\r
+      break;\r
+    case 2:\r
+      Width = EfiPciIoWidthFifoUint16;\r
+      Count = Trb->BlockSize / sizeof (UINT16);\r
+      break;\r
+    case 1:\r
+    case 3:\r
+    default:\r
+      Width = EfiPciIoWidthFifoUint8;\r
+      Count = Trb->BlockSize;\r
+      break;\r
+    }\r
+\r
+  if (Trb->Read) {\r
+    if ((IntStatus & BIT5) == 0) {\r
+      return EFI_NOT_READY;\r
     }\r
+    Data16 = BIT5;\r
+    SdMmcHcRwMmio (Private->PciIo, Trb->Slot, SD_MMC_HC_NOR_INT_STS, FALSE, sizeof (Data16), &Data16);\r
 \r
-    Status  = SdMmcHcRwMmio (\r
-                Private->PciIo,\r
-                Trb->Slot,\r
-                SD_MMC_HC_SW_RST,\r
-                FALSE,\r
-                sizeof (SwReset),\r
-                &SwReset\r
-                );\r
+    Status = Private->PciIo->Mem.Read (\r
+               Private->PciIo,\r
+               Width,\r
+               Trb->Slot,\r
+               SD_MMC_HC_BUF_DAT_PORT,\r
+               Count,\r
+               (VOID*)((UINT8*)Trb->Data + (Trb->BlockSize * Trb->PioBlockIndex))\r
+               );\r
     if (EFI_ERROR (Status)) {\r
-      goto Done;\r
+      return Status;\r
+    }\r
+    Trb->PioBlockIndex++;\r
+  } else {\r
+    if ((IntStatus & BIT4) == 0) {\r
+      return EFI_NOT_READY;\r
     }\r
-    Status = SdMmcHcWaitMmioSet (\r
+    Data16 = BIT4;\r
+    SdMmcHcRwMmio (Private->PciIo, Trb->Slot, SD_MMC_HC_NOR_INT_STS, FALSE, sizeof (Data16), &Data16);\r
+\r
+    Status = Private->PciIo->Mem.Write (\r
                Private->PciIo,\r
+               Width,\r
                Trb->Slot,\r
-               SD_MMC_HC_SW_RST,\r
-               sizeof (SwReset),\r
-               0xFF,\r
-               0,\r
-               SD_MMC_HC_GENERIC_TIMEOUT\r
+               SD_MMC_HC_BUF_DAT_PORT,\r
+               Count,\r
+               (VOID*)((UINT8*)Trb->Data + (Trb->BlockSize * Trb->PioBlockIndex))\r
                );\r
     if (EFI_ERROR (Status)) {\r
-      goto Done;\r
+      return Status;\r
     }\r
+    Trb->PioBlockIndex++;\r
+  }\r
 \r
-    Status = EFI_DEVICE_ERROR;\r
-    goto Done;\r
+  if (Trb->PioBlockIndex >= BlockCount) {\r
+    Trb->PioModeTransferCompleted = TRUE;\r
+    return EFI_SUCCESS;\r
+  } else {\r
+    return EFI_NOT_READY;\r
   }\r
-  //\r
-  // Check if DMA interrupt is signalled for the SDMA transfer.\r
-  //\r
-  if ((Trb->Mode == SdMmcSdmaMode) && ((IntStatus & BIT3) == BIT3)) {\r
-    //\r
-    // Clear DMA interrupt bit.\r
-    //\r
-    IntStatus = BIT3;\r
-    Status    = SdMmcHcRwMmio (\r
-                  Private->PciIo,\r
-                  Trb->Slot,\r
-                  SD_MMC_HC_NOR_INT_STS,\r
-                  FALSE,\r
-                  sizeof (IntStatus),\r
-                  &IntStatus\r
-                  );\r
+}\r
+\r
+/**\r
+  Update the SDMA address on the SDMA buffer boundary interrupt.\r
+\r
+  @param[in] Private    A pointer to the SD_MMC_HC_PRIVATE_DATA instance.\r
+  @param[in] Trb        The pointer to the SD_MMC_HC_TRB instance.\r
+\r
+  @retval EFI_SUCCESS  Updated SDMA buffer address.\r
+  @retval Others       Failed to update SDMA buffer address.\r
+**/\r
+EFI_STATUS\r
+SdMmcUpdateSdmaAddress (\r
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,\r
+  IN SD_MMC_HC_TRB           *Trb\r
+  )\r
+{\r
+  UINT64      SdmaAddr;\r
+  EFI_STATUS  Status;\r
+\r
+  SdmaAddr = SD_MMC_SDMA_ROUND_UP ((UINTN)Trb->DataPhy, SD_MMC_SDMA_BOUNDARY);\r
+\r
+  if (Private->ControllerVersion[Trb->Slot] >= SD_MMC_HC_CTRL_VER_400) {\r
+    Status = SdMmcHcRwMmio (\r
+               Private->PciIo,\r
+               Trb->Slot,\r
+               SD_MMC_HC_ADMA_SYS_ADDR,\r
+               FALSE,\r
+               sizeof (UINT64),\r
+               &SdmaAddr\r
+               );\r
+  } else {\r
+    Status = SdMmcHcRwMmio (\r
+               Private->PciIo,\r
+               Trb->Slot,\r
+               SD_MMC_HC_SDMA_ADDR,\r
+               FALSE,\r
+               sizeof (UINT32),\r
+               &SdmaAddr\r
+               );\r
+  }\r
+\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
+  Trb->DataPhy = (UINT64)(UINTN)SdmaAddr;\r
+  return EFI_SUCCESS;\r
+}\r
+\r
+/**\r
+  Checks if the data transfer completed and performs any actions\r
+  neccessary to continue the data transfer such as SDMA system\r
+  address fixup or PIO data transfer.\r
+\r
+  @param[in] Private    A pointer to the SD_MMC_HC_PRIVATE_DATA instance.\r
+  @param[in] Trb        The pointer to the SD_MMC_HC_TRB instance.\r
+  @param[in] IntStatus  Snapshot of the normal interrupt status register.\r
+\r
+  @retval EFI_SUCCESS   Data transfer completed successfully.\r
+  @retval EFI_NOT_READY Data transfer completion still pending.\r
+  @retval Others        Data transfer failed to complete.\r
+**/\r
+EFI_STATUS\r
+SdMmcCheckDataTransfer (\r
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,\r
+  IN SD_MMC_HC_TRB           *Trb,\r
+  IN UINT16                  IntStatus\r
+  )\r
+{\r
+  UINT16      Data16;\r
+  EFI_STATUS  Status;\r
+\r
+  if ((IntStatus & BIT1) != 0) {\r
+    Data16 = BIT1;\r
+    Status = SdMmcHcRwMmio (\r
+               Private->PciIo,\r
+               Trb->Slot,\r
+               SD_MMC_HC_NOR_INT_STS,\r
+               FALSE,\r
+               sizeof (Data16),\r
+               &Data16\r
+               );\r
+    return Status;\r
+  }\r
+\r
+  if (Trb->Mode == SdMmcPioMode && !Trb->PioModeTransferCompleted) {\r
+    Status = SdMmcTransferDataWithPio (Private, Trb, IntStatus);\r
     if (EFI_ERROR (Status)) {\r
-      goto Done;\r
+      return Status;\r
     }\r
-    //\r
-    // Update SDMA Address register.\r
-    //\r
-    SdmaAddr = SD_MMC_SDMA_ROUND_UP ((UINTN)Trb->DataPhy, SD_MMC_SDMA_BOUNDARY);\r
+  }\r
 \r
-    if (Private->ControllerVersion[Trb->Slot] >= SD_MMC_HC_CTRL_VER_400) {\r
-      Status = SdMmcHcRwMmio (\r
-                 Private->PciIo,\r
-                 Trb->Slot,\r
-                 SD_MMC_HC_ADMA_SYS_ADDR,\r
-                 FALSE,\r
-                 sizeof (UINT64),\r
-                 &SdmaAddr\r
-                 );\r
-    } else {\r
-      Status = SdMmcHcRwMmio (\r
-                 Private->PciIo,\r
-                 Trb->Slot,\r
-                 SD_MMC_HC_SDMA_ADDR,\r
-                 FALSE,\r
-                 sizeof (UINT32),\r
-                 &SdmaAddr\r
-                 );\r
+  if ((Trb->Mode == SdMmcSdmaMode) && ((IntStatus & BIT3) != 0)) {\r
+    Data16 = BIT3;\r
+    Status = SdMmcHcRwMmio (\r
+               Private->PciIo,\r
+               Trb->Slot,\r
+               SD_MMC_HC_NOR_INT_STS,\r
+               FALSE,\r
+               sizeof (Data16),\r
+               &Data16\r
+               );\r
+    if (EFI_ERROR (Status)) {\r
+      return Status;\r
     }\r
-\r
+    Status = SdMmcUpdateSdmaAddress (Private, Trb);\r
     if (EFI_ERROR (Status)) {\r
-      goto Done;\r
+      return Status;\r
     }\r
-    Trb->DataPhy = (UINT64)(UINTN)SdmaAddr;\r
   }\r
 \r
-  if ((Packet->SdMmcCmdBlk->CommandType != SdMmcCommandTypeAdtc) &&\r
-      (Packet->SdMmcCmdBlk->ResponseType != SdMmcResponseTypeR1b) &&\r
-      (Packet->SdMmcCmdBlk->ResponseType != SdMmcResponseTypeR5b)) {\r
-    if ((IntStatus & BIT0) == BIT0) {\r
-      Status = EFI_SUCCESS;\r
-      goto Done;\r
-    }\r
+  return EFI_NOT_READY;\r
+}\r
+\r
+/**\r
+  Check the TRB execution result.\r
+\r
+  @param[in] Private        A pointer to the SD_MMC_HC_PRIVATE_DATA instance.\r
+  @param[in] Trb            The pointer to the SD_MMC_HC_TRB instance.\r
+\r
+  @retval EFI_SUCCESS       The TRB is executed successfully.\r
+  @retval EFI_NOT_READY     The TRB is not completed for execution.\r
+  @retval Others            Some erros happen when executing this request.\r
+\r
+**/\r
+EFI_STATUS\r
+SdMmcCheckTrbResult (\r
+  IN SD_MMC_HC_PRIVATE_DATA           *Private,\r
+  IN SD_MMC_HC_TRB                    *Trb\r
+  )\r
+{\r
+  EFI_STATUS                          Status;\r
+  EFI_SD_MMC_PASS_THRU_COMMAND_PACKET *Packet;\r
+  UINT16                              IntStatus;\r
+\r
+  Packet  = Trb->Packet;\r
+  //\r
+  // Check Trb execution result by reading Normal Interrupt Status register.\r
+  //\r
+  Status = SdMmcHcRwMmio (\r
+             Private->PciIo,\r
+             Trb->Slot,\r
+             SD_MMC_HC_NOR_INT_STS,\r
+             TRUE,\r
+             sizeof (IntStatus),\r
+             &IntStatus\r
+             );\r
+  if (EFI_ERROR (Status)) {\r
+    goto Done;\r
+  }\r
+\r
+  //\r
+  // Check if there are any errors reported by host controller\r
+  // and if neccessary recover the controller before next command is executed.\r
+  //\r
+  Status = SdMmcCheckAndRecoverErrors (Private, Trb->Slot, IntStatus);\r
+  if (EFI_ERROR (Status)) {\r
+    goto Done;\r
   }\r
 \r
+  //\r
+  // Tuning commands are the only ones that do not generate command\r
+  // complete interrupt. Process them here before entering the code\r
+  // that waits for command completion.\r
+  //\r
   if (((Private->Slot[Trb->Slot].CardType == EmmcCardType) &&\r
        (Packet->SdMmcCmdBlk->CommandIndex == EMMC_SEND_TUNING_BLOCK)) ||\r
       ((Private->Slot[Trb->Slot].CardType == SdCardType) &&\r
        (Packet->SdMmcCmdBlk->CommandIndex == SD_SEND_TUNING_BLOCK))) {\r
-    //\r
-    // When performing tuning procedure (Execute Tuning is set to 1) through PIO mode,\r
-    // wait Buffer Read Ready bit of Normal Interrupt Status Register to be 1.\r
-    // Refer to SD Host Controller Simplified Specification 3.0 figure 2-29 for details.\r
-    //\r
-    if ((IntStatus & BIT5) == BIT5) {\r
-      //\r
-      // Clear Buffer Read Ready interrupt at first.\r
-      //\r
-      IntStatus = BIT5;\r
-      SdMmcHcRwMmio (Private->PciIo, Trb->Slot, SD_MMC_HC_NOR_INT_STS, FALSE, sizeof (IntStatus), &IntStatus);\r
-      //\r
-      // Read data out from Buffer Port register\r
-      //\r
-      for (PioLength = 0; PioLength < Trb->DataLen; PioLength += 4) {\r
-        SdMmcHcRwMmio (Private->PciIo, Trb->Slot, SD_MMC_HC_BUF_DAT_PORT, TRUE, 4, (UINT8*)Trb->Data + PioLength);\r
-      }\r
-      Status = EFI_SUCCESS;\r
+    Status = SdMmcTransferDataWithPio (Private, Trb, IntStatus);\r
+    goto Done;\r
+  }\r
+\r
+  if (!Trb->CommandComplete) {\r
+    Status = SdMmcCheckCommandComplete (Private, Trb, IntStatus);\r
+    if (EFI_ERROR (Status)) {\r
       goto Done;\r
     }\r
   }\r
 \r
-  Status = EFI_NOT_READY;\r
-Done:\r
-  //\r
-  // Get response data when the cmd is executed successfully.\r
-  //\r
-  if (!EFI_ERROR (Status)) {\r
-    if (Packet->SdMmcCmdBlk->CommandType != SdMmcCommandTypeBc) {\r
-      for (Index = 0; Index < 4; Index++) {\r
-        Status = SdMmcHcRwMmio (\r
-                   Private->PciIo,\r
-                   Trb->Slot,\r
-                   SD_MMC_HC_RESPONSE + Index * 4,\r
-                   TRUE,\r
-                   sizeof (UINT32),\r
-                   &Response[Index]\r
-                   );\r
-        if (EFI_ERROR (Status)) {\r
-          SdMmcHcLedOnOff (Private->PciIo, Trb->Slot, FALSE);\r
-          return Status;\r
-        }\r
-      }\r
-      CopyMem (Packet->SdMmcStatusBlk, Response, sizeof (Response));\r
-    }\r
+  if (Packet->SdMmcCmdBlk->CommandType == SdMmcCommandTypeAdtc ||\r
+      Packet->SdMmcCmdBlk->ResponseType == SdMmcResponseTypeR1b ||\r
+      Packet->SdMmcCmdBlk->ResponseType == SdMmcResponseTypeR5b) {\r
+    Status = SdMmcCheckDataTransfer (Private, Trb, IntStatus);\r
+  } else {\r
+    Status = EFI_SUCCESS;\r
   }\r
 \r
+Done:\r
   if (Status != EFI_NOT_READY) {\r
     SdMmcHcLedOnOff (Private->PciIo, Trb->Slot, FALSE);\r
+    if (EFI_ERROR (Status)) {\r
+      DEBUG ((DEBUG_ERROR, "TRB failed with %r\n", Status));\r
+      SdMmcPrintTrb (DEBUG_ERROR, Trb);\r
+    } else {\r
+      DEBUG ((DEBUG_VERBOSE, "TRB success\n"));\r
+      SdMmcPrintTrb (DEBUG_VERBOSE, Trb);\r
+    }\r
   }\r
 \r
   return Status;\r