]> git.proxmox.com Git - mirror_edk2.git/blobdiff - MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHci.c
UefiCpuPkg: Move AsmRelocateApLoopStart from Mpfuncs.nasm to AmdSev.nasm
[mirror_edk2.git] / MdeModulePkg / Bus / Pci / SdMmcPciHcDxe / SdMmcPciHci.c
index baa12f44eeacea52453f5c01ea687f665ad72d2d..dd45167a009ee8d686aa03a3bdce6a2f02dcc0a0 100644 (file)
@@ -1,17 +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) 2015 - 2016, 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
 **/\r
 VOID\r
 DumpCapabilityReg (\r
-  IN UINT8                Slot,\r
-  IN SD_MMC_HC_SLOT_CAP   *Capability\r
+  IN UINT8               Slot,\r
+  IN SD_MMC_HC_SLOT_CAP  *Capability\r
   )\r
 {\r
   //\r
   // Dump Capability Data\r
   //\r
-  DEBUG ((EFI_D_INFO, " == Slot [%d] Capability is 0x%x ==\n", Slot, Capability));\r
-  DEBUG ((EFI_D_INFO, "   Timeout Clk Freq  %d%a\n", Capability->TimeoutFreq, (Capability->TimeoutUnit) ? "MHz" : "KHz"));\r
-  DEBUG ((EFI_D_INFO, "   Base Clk Freq     %dMHz\n", Capability->BaseClkFreq));\r
-  DEBUG ((EFI_D_INFO, "   Max Blk Len       %dbytes\n", 512 * (1 << Capability->MaxBlkLen)));\r
-  DEBUG ((EFI_D_INFO, "   8-bit Support     %a\n", Capability->BusWidth8 ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   ADMA2 Support     %a\n", Capability->Adma2 ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   HighSpeed Support %a\n", Capability->HighSpeed ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   SDMA Support      %a\n", Capability->Sdma ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   Suspend/Resume    %a\n", Capability->SuspRes ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   Voltage 3.3       %a\n", Capability->Voltage33 ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   Voltage 3.0       %a\n", Capability->Voltage30 ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   Voltage 1.8       %a\n", Capability->Voltage18 ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   64-bit Sys Bus    %a\n", Capability->SysBus64 ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   Async Interrupt   %a\n", Capability->AsyncInt ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   SlotType          "));\r
+  DEBUG ((DEBUG_INFO, " == Slot [%d] Capability is 0x%x ==\n", Slot, Capability));\r
+  DEBUG ((DEBUG_INFO, "   Timeout Clk Freq  %d%a\n", Capability->TimeoutFreq, (Capability->TimeoutUnit) ? "MHz" : "KHz"));\r
+  DEBUG ((DEBUG_INFO, "   Base Clk Freq     %dMHz\n", Capability->BaseClkFreq));\r
+  DEBUG ((DEBUG_INFO, "   Max Blk Len       %dbytes\n", 512 * (1 << Capability->MaxBlkLen)));\r
+  DEBUG ((DEBUG_INFO, "   8-bit Support     %a\n", Capability->BusWidth8 ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   ADMA2 Support     %a\n", Capability->Adma2 ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   HighSpeed Support %a\n", Capability->HighSpeed ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   SDMA Support      %a\n", Capability->Sdma ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   Suspend/Resume    %a\n", Capability->SuspRes ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   Voltage 3.3       %a\n", Capability->Voltage33 ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   Voltage 3.0       %a\n", Capability->Voltage30 ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   Voltage 1.8       %a\n", Capability->Voltage18 ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   V4 64-bit Sys Bus %a\n", Capability->SysBus64V4 ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   V3 64-bit Sys Bus %a\n", Capability->SysBus64V3 ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   Async Interrupt   %a\n", Capability->AsyncInt ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   SlotType          "));\r
   if (Capability->SlotType == 0x00) {\r
-    DEBUG ((EFI_D_INFO, "%a\n", "Removable Slot"));\r
+    DEBUG ((DEBUG_INFO, "%a\n", "Removable Slot"));\r
   } else if (Capability->SlotType == 0x01) {\r
-    DEBUG ((EFI_D_INFO, "%a\n", "Embedded Slot"));\r
+    DEBUG ((DEBUG_INFO, "%a\n", "Embedded Slot"));\r
   } else if (Capability->SlotType == 0x02) {\r
-    DEBUG ((EFI_D_INFO, "%a\n", "Shared Bus Slot"));\r
+    DEBUG ((DEBUG_INFO, "%a\n", "Shared Bus Slot"));\r
   } else {\r
-    DEBUG ((EFI_D_INFO, "%a\n", "Reserved"));\r
-  }\r
-  DEBUG ((EFI_D_INFO, "   SDR50  Support    %a\n", Capability->Sdr50 ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   SDR104 Support    %a\n", Capability->Sdr104 ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   DDR50  Support    %a\n", Capability->Ddr50 ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   Driver Type A     %a\n", Capability->DriverTypeA ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   Driver Type C     %a\n", Capability->DriverTypeC ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   Driver Type D     %a\n", Capability->DriverTypeD ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   Driver Type 4     %a\n", Capability->DriverType4 ? "TRUE" : "FALSE"));\r
+    DEBUG ((DEBUG_INFO, "%a\n", "Reserved"));\r
+  }\r
+\r
+  DEBUG ((DEBUG_INFO, "   SDR50  Support    %a\n", Capability->Sdr50 ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   SDR104 Support    %a\n", Capability->Sdr104 ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   DDR50  Support    %a\n", Capability->Ddr50 ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   Driver Type A     %a\n", Capability->DriverTypeA ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   Driver Type C     %a\n", Capability->DriverTypeC ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   Driver Type D     %a\n", Capability->DriverTypeD ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   Driver Type 4     %a\n", Capability->DriverType4 ? "TRUE" : "FALSE"));\r
   if (Capability->TimerCount == 0) {\r
-    DEBUG ((EFI_D_INFO, "   Retuning TimerCnt Disabled\n", 2 * (Capability->TimerCount - 1)));\r
+    DEBUG ((DEBUG_INFO, "   Retuning TimerCnt Disabled\n"));\r
   } else {\r
-    DEBUG ((EFI_D_INFO, "   Retuning TimerCnt %dseconds\n", 2 * (Capability->TimerCount - 1)));\r
+    DEBUG ((DEBUG_INFO, "   Retuning TimerCnt %dseconds\n", 2 * (Capability->TimerCount - 1)));\r
   }\r
-  DEBUG ((EFI_D_INFO, "   SDR50 Tuning      %a\n", Capability->TuningSDR50 ? "TRUE" : "FALSE"));\r
-  DEBUG ((EFI_D_INFO, "   Retuning Mode     Mode %d\n", Capability->RetuningMod + 1));\r
-  DEBUG ((EFI_D_INFO, "   Clock Multiplier  M = %d\n", Capability->ClkMultiplier + 1));\r
-  DEBUG ((EFI_D_INFO, "   HS 400            %a\n", Capability->Hs400 ? "TRUE" : "FALSE"));\r
+\r
+  DEBUG ((DEBUG_INFO, "   SDR50 Tuning      %a\n", Capability->TuningSDR50 ? "TRUE" : "FALSE"));\r
+  DEBUG ((DEBUG_INFO, "   Retuning Mode     Mode %d\n", Capability->RetuningMod + 1));\r
+  DEBUG ((DEBUG_INFO, "   Clock Multiplier  M = %d\n", Capability->ClkMultiplier + 1));\r
+  DEBUG ((DEBUG_INFO, "   HS 400            %a\n", Capability->Hs400 ? "TRUE" : "FALSE"));\r
   return;\r
 }\r
 \r
@@ -90,13 +90,13 @@ DumpCapabilityReg (
 EFI_STATUS\r
 EFIAPI\r
 SdMmcHcGetSlotInfo (\r
-  IN     EFI_PCI_IO_PROTOCOL   *PciIo,\r
-     OUT UINT8                 *FirstBar,\r
-     OUT UINT8                 *SlotNum\r
+  IN     EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  OUT UINT8                   *FirstBar,\r
+  OUT UINT8                   *SlotNum\r
   )\r
 {\r
-  EFI_STATUS                   Status;\r
-  SD_MMC_HC_SLOT_INFO          SlotInfo;\r
+  EFI_STATUS           Status;\r
+  SD_MMC_HC_SLOT_INFO  SlotInfo;\r
 \r
   Status = PciIo->Pci.Read (\r
                         PciIo,\r
@@ -141,39 +141,56 @@ SdMmcHcGetSlotInfo (
 EFI_STATUS\r
 EFIAPI\r
 SdMmcHcRwMmio (\r
-  IN     EFI_PCI_IO_PROTOCOL   *PciIo,\r
-  IN     UINT8                 BarIndex,\r
-  IN     UINT32                Offset,\r
-  IN     BOOLEAN               Read,\r
-  IN     UINT8                 Count,\r
-  IN OUT VOID                  *Data\r
+  IN     EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN     UINT8                BarIndex,\r
+  IN     UINT32               Offset,\r
+  IN     BOOLEAN              Read,\r
+  IN     UINT8                Count,\r
+  IN OUT VOID                 *Data\r
   )\r
 {\r
-  EFI_STATUS                   Status;\r
+  EFI_STATUS                 Status;\r
+  EFI_PCI_IO_PROTOCOL_WIDTH  Width;\r
 \r
-  if ((PciIo == NULL) || (Data == NULL))  {\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
+                          (UINT64)Offset,\r
                           Count,\r
                           Data\r
                           );\r
   } else {\r
     Status = PciIo->Mem.Write (\r
                           PciIo,\r
-                          EfiPciIoWidthUint8,\r
+                          Width,\r
                           BarIndex,\r
-                          (UINT64) Offset,\r
+                          (UINT64)Offset,\r
                           Count,\r
                           Data\r
                           );\r
@@ -206,16 +223,16 @@ SdMmcHcRwMmio (
 EFI_STATUS\r
 EFIAPI\r
 SdMmcHcOrMmio (\r
-  IN  EFI_PCI_IO_PROTOCOL      *PciIo,\r
-  IN  UINT8                    BarIndex,\r
-  IN  UINT32                   Offset,\r
-  IN  UINT8                    Count,\r
-  IN  VOID                     *OrData\r
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN  UINT8                BarIndex,\r
+  IN  UINT32               Offset,\r
+  IN  UINT8                Count,\r
+  IN  VOID                 *OrData\r
   )\r
 {\r
-  EFI_STATUS                   Status;\r
-  UINT64                       Data;\r
-  UINT64                       Or;\r
+  EFI_STATUS  Status;\r
+  UINT64      Data;\r
+  UINT64      Or;\r
 \r
   Status = SdMmcHcRwMmio (PciIo, BarIndex, Offset, TRUE, Count, &Data);\r
   if (EFI_ERROR (Status)) {\r
@@ -223,13 +240,13 @@ SdMmcHcOrMmio (
   }\r
 \r
   if (Count == 1) {\r
-    Or = *(UINT8*) OrData;\r
+    Or = *(UINT8 *)OrData;\r
   } else if (Count == 2) {\r
-    Or = *(UINT16*) OrData;\r
+    Or = *(UINT16 *)OrData;\r
   } else if (Count == 4) {\r
-    Or = *(UINT32*) OrData;\r
+    Or = *(UINT32 *)OrData;\r
   } else if (Count == 8) {\r
-    Or = *(UINT64*) OrData;\r
+    Or = *(UINT64 *)OrData;\r
   } else {\r
     return EFI_INVALID_PARAMETER;\r
   }\r
@@ -264,16 +281,16 @@ SdMmcHcOrMmio (
 EFI_STATUS\r
 EFIAPI\r
 SdMmcHcAndMmio (\r
-  IN  EFI_PCI_IO_PROTOCOL      *PciIo,\r
-  IN  UINT8                    BarIndex,\r
-  IN  UINT32                   Offset,\r
-  IN  UINT8                    Count,\r
-  IN  VOID                     *AndData\r
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN  UINT8                BarIndex,\r
+  IN  UINT32               Offset,\r
+  IN  UINT8                Count,\r
+  IN  VOID                 *AndData\r
   )\r
 {\r
-  EFI_STATUS                   Status;\r
-  UINT64                       Data;\r
-  UINT64                       And;\r
+  EFI_STATUS  Status;\r
+  UINT64      Data;\r
+  UINT64      And;\r
 \r
   Status = SdMmcHcRwMmio (PciIo, BarIndex, Offset, TRUE, Count, &Data);\r
   if (EFI_ERROR (Status)) {\r
@@ -281,13 +298,13 @@ SdMmcHcAndMmio (
   }\r
 \r
   if (Count == 1) {\r
-    And = *(UINT8*) AndData;\r
+    And = *(UINT8 *)AndData;\r
   } else if (Count == 2) {\r
-    And = *(UINT16*) AndData;\r
+    And = *(UINT16 *)AndData;\r
   } else if (Count == 4) {\r
-    And = *(UINT32*) AndData;\r
+    And = *(UINT32 *)AndData;\r
   } else if (Count == 8) {\r
-    And = *(UINT64*) AndData;\r
+    And = *(UINT64 *)AndData;\r
   } else {\r
     return EFI_INVALID_PARAMETER;\r
   }\r
@@ -320,16 +337,16 @@ SdMmcHcAndMmio (
 EFI_STATUS\r
 EFIAPI\r
 SdMmcHcCheckMmioSet (\r
-  IN  EFI_PCI_IO_PROTOCOL       *PciIo,\r
-  IN  UINT8                     BarIndex,\r
-  IN  UINT32                    Offset,\r
-  IN  UINT8                     Count,\r
-  IN  UINT64                    MaskValue,\r
-  IN  UINT64                    TestValue\r
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN  UINT8                BarIndex,\r
+  IN  UINT32               Offset,\r
+  IN  UINT8                Count,\r
+  IN  UINT64               MaskValue,\r
+  IN  UINT64               TestValue\r
   )\r
 {\r
-  EFI_STATUS            Status;\r
-  UINT64                Value;\r
+  EFI_STATUS  Status;\r
+  UINT64      Value;\r
 \r
   //\r
   // Access PCI MMIO space to see if the value is the tested one.\r
@@ -374,17 +391,17 @@ SdMmcHcCheckMmioSet (
 EFI_STATUS\r
 EFIAPI\r
 SdMmcHcWaitMmioSet (\r
-  IN  EFI_PCI_IO_PROTOCOL       *PciIo,\r
-  IN  UINT8                     BarIndex,\r
-  IN  UINT32                    Offset,\r
-  IN  UINT8                     Count,\r
-  IN  UINT64                    MaskValue,\r
-  IN  UINT64                    TestValue,\r
-  IN  UINT64                    Timeout\r
+  IN  EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN  UINT8                BarIndex,\r
+  IN  UINT32               Offset,\r
+  IN  UINT8                Count,\r
+  IN  UINT64               MaskValue,\r
+  IN  UINT64               TestValue,\r
+  IN  UINT64               Timeout\r
   )\r
 {\r
-  EFI_STATUS            Status;\r
-  BOOLEAN               InfiniteWait;\r
+  EFI_STATUS  Status;\r
+  BOOLEAN     InfiniteWait;\r
 \r
   if (Timeout == 0) {\r
     InfiniteWait = TRUE;\r
@@ -416,10 +433,40 @@ SdMmcHcWaitMmioSet (
   return EFI_TIMEOUT;\r
 }\r
 \r
+/**\r
+  Get the controller version information from the specified slot.\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[out] Version         The buffer to store the version information.\r
+\r
+  @retval EFI_SUCCESS         The operation executes successfully.\r
+  @retval Others              The operation fails.\r
+\r
+**/\r
+EFI_STATUS\r
+SdMmcHcGetControllerVersion (\r
+  IN     EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN     UINT8                Slot,\r
+  OUT    UINT16               *Version\r
+  )\r
+{\r
+  EFI_STATUS  Status;\r
+\r
+  Status = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_CTRL_VER, TRUE, sizeof (UINT16), Version);\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
+  *Version &= 0xFF;\r
+\r
+  return EFI_SUCCESS;\r
+}\r
+\r
 /**\r
   Software reset the specified SD/MMC host controller and enable all interrupts.\r
 \r
-  @param[in] PciIo          The PCI IO protocol instance.\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
 \r
   @retval EFI_SUCCESS       The software reset executes successfully.\r
@@ -428,18 +475,42 @@ SdMmcHcWaitMmioSet (
 **/\r
 EFI_STATUS\r
 SdMmcHcReset (\r
-  IN EFI_PCI_IO_PROTOCOL    *PciIo,\r
-  IN UINT8                  Slot\r
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,\r
+  IN UINT8                   Slot\r
   )\r
 {\r
-  EFI_STATUS                Status;\r
-  UINT8                     SwReset;\r
+  EFI_STATUS           Status;\r
+  UINT8                SwReset;\r
+  EFI_PCI_IO_PROTOCOL  *PciIo;\r
+\r
+  //\r
+  // Notify the SD/MMC override protocol that we are about to reset\r
+  // the SD/MMC host controller.\r
+  //\r
+  if ((mOverride != NULL) && (mOverride->NotifyPhase != NULL)) {\r
+    Status = mOverride->NotifyPhase (\r
+                          Private->ControllerHandle,\r
+                          Slot,\r
+                          EdkiiSdMmcResetPre,\r
+                          NULL\r
+                          );\r
+    if (EFI_ERROR (Status)) {\r
+      DEBUG ((\r
+        DEBUG_WARN,\r
+        "%a: SD/MMC pre reset notifier callback failed - %r\n",\r
+        __FUNCTION__,\r
+        Status\r
+        ));\r
+      return Status;\r
+    }\r
+  }\r
 \r
-  SwReset = 0xFF;\r
-  Status  = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_SW_RST, FALSE, sizeof (SwReset), &SwReset);\r
+  PciIo   = Private->PciIo;\r
+  SwReset = BIT0;\r
+  Status  = SdMmcHcOrMmio (PciIo, Slot, SD_MMC_HC_SW_RST, sizeof (SwReset), &SwReset);\r
 \r
   if (EFI_ERROR (Status)) {\r
-    DEBUG ((EFI_D_ERROR, "SdMmcHcReset: write full 1 fails: %r\n", Status));\r
+    DEBUG ((DEBUG_ERROR, "SdMmcHcReset: write SW Reset for All fails: %r\n", Status));\r
     return Status;\r
   }\r
 \r
@@ -448,18 +519,48 @@ SdMmcHcReset (
              Slot,\r
              SD_MMC_HC_SW_RST,\r
              sizeof (SwReset),\r
-             0xFF,\r
+             BIT0,\r
              0x00,\r
              SD_MMC_HC_GENERIC_TIMEOUT\r
              );\r
   if (EFI_ERROR (Status)) {\r
-    DEBUG ((EFI_D_INFO, "SdMmcHcReset: reset done with %r\n", Status));\r
+    DEBUG ((DEBUG_INFO, "SdMmcHcReset: reset done with %r\n", Status));\r
     return Status;\r
   }\r
+\r
   //\r
   // Enable all interrupt after reset all.\r
   //\r
   Status = SdMmcHcEnableInterrupt (PciIo, Slot);\r
+  if (EFI_ERROR (Status)) {\r
+    DEBUG ((\r
+      DEBUG_INFO,\r
+      "SdMmcHcReset: SdMmcHcEnableInterrupt done with %r\n",\r
+      Status\r
+      ));\r
+    return Status;\r
+  }\r
+\r
+  //\r
+  // Notify the SD/MMC override protocol that we have just reset\r
+  // the SD/MMC host controller.\r
+  //\r
+  if ((mOverride != NULL) && (mOverride->NotifyPhase != NULL)) {\r
+    Status = mOverride->NotifyPhase (\r
+                          Private->ControllerHandle,\r
+                          Slot,\r
+                          EdkiiSdMmcResetPost,\r
+                          NULL\r
+                          );\r
+    if (EFI_ERROR (Status)) {\r
+      DEBUG ((\r
+        DEBUG_WARN,\r
+        "%a: SD/MMC post reset notifier callback failed - %r\n",\r
+        __FUNCTION__,\r
+        Status\r
+        ));\r
+    }\r
+  }\r
 \r
   return Status;\r
 }\r
@@ -477,26 +578,27 @@ SdMmcHcReset (
 **/\r
 EFI_STATUS\r
 SdMmcHcEnableInterrupt (\r
-  IN EFI_PCI_IO_PROTOCOL    *PciIo,\r
-  IN UINT8                  Slot\r
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN UINT8                Slot\r
   )\r
 {\r
-  EFI_STATUS                Status;\r
-  UINT16                    IntStatus;\r
+  EFI_STATUS  Status;\r
+  UINT16      IntStatus;\r
 \r
   //\r
   // Enable all bits in Error Interrupt Status Enable Register\r
   //\r
   IntStatus = 0xFFFF;\r
-  Status = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_ERR_INT_STS_EN, FALSE, sizeof (IntStatus), &IntStatus);\r
+  Status    = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_ERR_INT_STS_EN, FALSE, sizeof (IntStatus), &IntStatus);\r
   if (EFI_ERROR (Status)) {\r
     return Status;\r
   }\r
+\r
   //\r
   // Enable all bits in Normal Interrupt Status Enable Register\r
   //\r
   IntStatus = 0xFFFF;\r
-  Status = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_NOR_INT_STS_EN, FALSE, sizeof (IntStatus), &IntStatus);\r
+  Status    = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_NOR_INT_STS_EN, FALSE, sizeof (IntStatus), &IntStatus);\r
 \r
   return Status;\r
 }\r
@@ -516,11 +618,11 @@ EFI_STATUS
 SdMmcHcGetCapability (\r
   IN     EFI_PCI_IO_PROTOCOL  *PciIo,\r
   IN     UINT8                Slot,\r
-     OUT SD_MMC_HC_SLOT_CAP   *Capability\r
+  OUT SD_MMC_HC_SLOT_CAP      *Capability\r
   )\r
 {\r
-  EFI_STATUS                Status;\r
-  UINT64                    Cap;\r
+  EFI_STATUS  Status;\r
+  UINT64      Cap;\r
 \r
   Status = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_CAP, TRUE, sizeof (Cap), &Cap);\r
   if (EFI_ERROR (Status)) {\r
@@ -547,10 +649,10 @@ EFI_STATUS
 SdMmcHcGetMaxCurrent (\r
   IN     EFI_PCI_IO_PROTOCOL  *PciIo,\r
   IN     UINT8                Slot,\r
-     OUT UINT64               *MaxCurrent\r
+  OUT UINT64                  *MaxCurrent\r
   )\r
 {\r
-  EFI_STATUS          Status;\r
+  EFI_STATUS  Status;\r
 \r
   Status = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_MAX_CURRENT_CAP, TRUE, sizeof (UINT64), MaxCurrent);\r
 \r
@@ -574,14 +676,14 @@ SdMmcHcGetMaxCurrent (
 **/\r
 EFI_STATUS\r
 SdMmcHcCardDetect (\r
-  IN EFI_PCI_IO_PROTOCOL    *PciIo,\r
-  IN UINT8                  Slot,\r
-     OUT BOOLEAN            *MediaPresent\r
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN UINT8                Slot,\r
+  OUT BOOLEAN             *MediaPresent\r
   )\r
 {\r
-  EFI_STATUS                Status;\r
-  UINT16                    Data;\r
-  UINT32                    PresentState;\r
+  EFI_STATUS  Status;\r
+  UINT16      Data;\r
+  UINT32      PresentState;\r
 \r
   //\r
   // Check Present State Register to see if there is a card presented.\r
@@ -635,13 +737,13 @@ SdMmcHcCardDetect (
 **/\r
 EFI_STATUS\r
 SdMmcHcStopClock (\r
-  IN EFI_PCI_IO_PROTOCOL    *PciIo,\r
-  IN UINT8                  Slot\r
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN UINT8                Slot\r
   )\r
 {\r
-  EFI_STATUS                Status;\r
-  UINT32                    PresentState;\r
-  UINT16                    ClockCtrl;\r
+  EFI_STATUS  Status;\r
+  UINT32      PresentState;\r
+  UINT16      ClockCtrl;\r
 \r
   //\r
   // Ensure no SD transactions are occurring on the SD Bus by\r
@@ -664,21 +766,46 @@ SdMmcHcStopClock (
   //\r
   // Set SD Clock Enable in the Clock Control register to 0\r
   //\r
-  ClockCtrl = (UINT16)~BIT2;\r
-  Status = SdMmcHcAndMmio (PciIo, Slot, SD_MMC_HC_CLOCK_CTRL, sizeof (ClockCtrl), &ClockCtrl);\r
+  ClockCtrl = (UINT16) ~BIT2;\r
+  Status    = SdMmcHcAndMmio (PciIo, Slot, SD_MMC_HC_CLOCK_CTRL, sizeof (ClockCtrl), &ClockCtrl);\r
 \r
   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] Capability     The capability of the slot.\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
@@ -686,29 +813,34 @@ SdMmcHcStopClock (
 **/\r
 EFI_STATUS\r
 SdMmcHcClockSupply (\r
-  IN EFI_PCI_IO_PROTOCOL    *PciIo,\r
-  IN UINT8                  Slot,\r
-  IN UINT64                 ClockFreq,\r
-  IN SD_MMC_HC_SLOT_CAP     Capability\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
-  UINT32                    BaseClkFreq;\r
-  UINT32                    SettingFreq;\r
-  UINT32                    Divisor;\r
-  UINT32                    Remainder;\r
-  UINT16                    ControllerVer;\r
-  UINT16                    ClockCtrl;\r
-\r
-  //\r
-  // Calculate a divisor for SD clock frequency\r
-  //\r
-  ASSERT (Capability.BaseClkFreq != 0);\r
-\r
-  BaseClkFreq = Capability.BaseClkFreq;\r
-  if ((ClockFreq > (BaseClkFreq * 1000)) || (ClockFreq == 0)) {\r
+  EFI_STATUS           Status;\r
+  UINT32               SettingFreq;\r
+  UINT32               Divisor;\r
+  UINT32               Remainder;\r
+  UINT16               ClockCtrl;\r
+  UINT32               BaseClkFreq;\r
+  UINT16               ControllerVer;\r
+  EFI_PCI_IO_PROTOCOL  *PciIo;\r
+\r
+  PciIo         = Private->PciIo;\r
+  BaseClkFreq   = Private->BaseClkFreq[Slot];\r
+  ControllerVer = Private->ControllerVersion[Slot];\r
+\r
+  if ((BaseClkFreq == 0) || (ClockFreq == 0)) {\r
     return EFI_INVALID_PARAMETER;\r
   }\r
+\r
+  if (ClockFreq > (BaseClkFreq * 1000)) {\r
+    ClockFreq = BaseClkFreq * 1000;\r
+  }\r
+\r
   //\r
   // Calculate the divisor of base frequency.\r
   //\r
@@ -722,34 +854,36 @@ SdMmcHcClockSupply (
     if ((ClockFreq == SettingFreq) && (Remainder == 0)) {\r
       break;\r
     }\r
+\r
     if ((ClockFreq == SettingFreq) && (Remainder != 0)) {\r
-      SettingFreq ++;\r
+      SettingFreq++;\r
     }\r
   }\r
 \r
-  DEBUG ((EFI_D_INFO, "BaseClkFreq %dMHz Divisor %d ClockFreq %dKhz\n", BaseClkFreq, Divisor, ClockFreq));\r
+  DEBUG ((DEBUG_INFO, "BaseClkFreq %dMHz Divisor %d ClockFreq %dKhz\n", BaseClkFreq, Divisor, ClockFreq));\r
 \r
-  Status = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_CTRL_VER, TRUE, sizeof (ControllerVer), &ControllerVer);\r
-  if (EFI_ERROR (Status)) {\r
-    return Status;\r
-  }\r
   //\r
   // Set SDCLK Frequency Select and Internal Clock Enable fields in Clock Control register.\r
   //\r
-  if ((ControllerVer & 0xFF) == 2) {\r
+  if ((ControllerVer >= SD_MMC_HC_CTRL_VER_300) &&\r
+      (ControllerVer <= SD_MMC_HC_CTRL_VER_420))\r
+  {\r
     ASSERT (Divisor <= 0x3FF);\r
     ClockCtrl = ((Divisor & 0xFF) << 8) | ((Divisor & 0x300) >> 2);\r
-  } else if (((ControllerVer & 0xFF) == 0) || ((ControllerVer & 0xFF) == 1)) {\r
+  } else if ((ControllerVer == SD_MMC_HC_CTRL_VER_100) ||\r
+             (ControllerVer == SD_MMC_HC_CTRL_VER_200))\r
+  {\r
     //\r
     // Only the most significant bit can be used as divisor.\r
     //\r
     if (((Divisor - 1) & Divisor) != 0) {\r
       Divisor = 1 << (HighBitSet32 (Divisor) + 1);\r
     }\r
+\r
     ASSERT (Divisor <= 0x80);\r
     ClockCtrl = (Divisor & 0xFF) << 8;\r
   } else {\r
-    DEBUG ((EFI_D_ERROR, "Unknown SD Host Controller Spec version [0x%x]!!!\n", ControllerVer));\r
+    DEBUG ((DEBUG_ERROR, "Unknown SD Host Controller Spec version [0x%x]!!!\n", ControllerVer));\r
     return EFI_UNSUPPORTED;\r
   }\r
 \r
@@ -765,9 +899,9 @@ SdMmcHcClockSupply (
   // Supply clock frequency with specified divisor\r
   //\r
   ClockCtrl |= BIT0;\r
-  Status = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_CLOCK_CTRL, FALSE, sizeof (ClockCtrl), &ClockCtrl);\r
+  Status     = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_CLOCK_CTRL, FALSE, sizeof (ClockCtrl), &ClockCtrl);\r
   if (EFI_ERROR (Status)) {\r
-    DEBUG ((EFI_D_ERROR, "Set SDCLK Frequency Select and Internal Clock Enable fields fails\n"));\r
+    DEBUG ((DEBUG_ERROR, "Set SDCLK Frequency Select and Internal Clock Enable fields fails\n"));\r
     return Status;\r
   }\r
 \r
@@ -787,11 +921,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
@@ -811,18 +969,18 @@ SdMmcHcClockSupply (
 **/\r
 EFI_STATUS\r
 SdMmcHcPowerControl (\r
-  IN EFI_PCI_IO_PROTOCOL    *PciIo,\r
-  IN UINT8                  Slot,\r
-  IN UINT8                  PowerCtrl\r
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN UINT8                Slot,\r
+  IN UINT8                PowerCtrl\r
   )\r
 {\r
-  EFI_STATUS                Status;\r
+  EFI_STATUS  Status;\r
 \r
   //\r
   // Clr SD Bus Power\r
   //\r
-  PowerCtrl &= (UINT8)~BIT0;\r
-  Status = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_POWER_CTRL, FALSE, sizeof (PowerCtrl), &PowerCtrl);\r
+  PowerCtrl &= (UINT8) ~BIT0;\r
+  Status     = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_POWER_CTRL, FALSE, sizeof (PowerCtrl), &PowerCtrl);\r
   if (EFI_ERROR (Status)) {\r
     return Status;\r
   }\r
@@ -831,7 +989,7 @@ SdMmcHcPowerControl (
   // Set SD Bus Voltage Select and SD Bus Power fields in Power Control Register\r
   //\r
   PowerCtrl |= BIT0;\r
-  Status = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_POWER_CTRL, FALSE, sizeof (PowerCtrl), &PowerCtrl);\r
+  Status     = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_POWER_CTRL, FALSE, sizeof (PowerCtrl), &PowerCtrl);\r
 \r
   return Status;\r
 }\r
@@ -851,33 +1009,35 @@ SdMmcHcPowerControl (
 **/\r
 EFI_STATUS\r
 SdMmcHcSetBusWidth (\r
-  IN EFI_PCI_IO_PROTOCOL    *PciIo,\r
-  IN UINT8                  Slot,\r
-  IN UINT16                 BusWidth\r
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN UINT8                Slot,\r
+  IN UINT16               BusWidth\r
   )\r
 {\r
-  EFI_STATUS                Status;\r
-  UINT8                     HostCtrl1;\r
+  EFI_STATUS  Status;\r
+  UINT8       HostCtrl1;\r
 \r
   if (BusWidth == 1) {\r
-    HostCtrl1 = (UINT8)~(BIT5 | BIT1);\r
-    Status = SdMmcHcAndMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL1, sizeof (HostCtrl1), &HostCtrl1);\r
+    HostCtrl1 = (UINT8) ~(BIT5 | BIT1);\r
+    Status    = SdMmcHcAndMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL1, sizeof (HostCtrl1), &HostCtrl1);\r
   } else if (BusWidth == 4) {\r
     Status = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL1, TRUE, sizeof (HostCtrl1), &HostCtrl1);\r
     if (EFI_ERROR (Status)) {\r
       return Status;\r
     }\r
+\r
     HostCtrl1 |= BIT1;\r
-    HostCtrl1 &= (UINT8)~BIT5;\r
-    Status = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL1, FALSE, sizeof (HostCtrl1), &HostCtrl1);\r
+    HostCtrl1 &= (UINT8) ~BIT5;\r
+    Status     = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL1, FALSE, sizeof (HostCtrl1), &HostCtrl1);\r
   } else if (BusWidth == 8) {\r
     Status = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL1, TRUE, sizeof (HostCtrl1), &HostCtrl1);\r
     if (EFI_ERROR (Status)) {\r
       return Status;\r
     }\r
-    HostCtrl1 &= (UINT8)~BIT1;\r
+\r
+    HostCtrl1 &= (UINT8) ~BIT1;\r
     HostCtrl1 |= BIT5;\r
-    Status = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL1, FALSE, sizeof (HostCtrl1), &HostCtrl1);\r
+    Status     = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL1, FALSE, sizeof (HostCtrl1), &HostCtrl1);\r
   } else {\r
     ASSERT (FALSE);\r
     return EFI_INVALID_PARAMETER;\r
@@ -887,41 +1047,67 @@ SdMmcHcSetBusWidth (
 }\r
 \r
 /**\r
-  Supply SD/MMC card with lowest clock frequency at initialization.\r
+  Configure V4 controller enhancements 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] Capability     The capability of the slot.\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 SD_MMC_HC_SLOT_CAP     Capability\r
+SdMmcHcInitV4Enhancements (\r
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN UINT8                Slot,\r
+  IN SD_MMC_HC_SLOT_CAP   Capability,\r
+  IN UINT16               ControllerVer\r
   )\r
 {\r
-  EFI_STATUS                Status;\r
-  UINT32                    InitFreq;\r
+  EFI_STATUS  Status;\r
+  UINT16      HostCtrl2;\r
 \r
   //\r
-  // Calculate a divisor for SD clock frequency\r
+  // Check if controller version V4 or higher\r
   //\r
-  if (Capability.BaseClkFreq == 0) {\r
+  if (ControllerVer >= SD_MMC_HC_CTRL_VER_400) {\r
+    HostCtrl2 = SD_MMC_HC_V4_EN;\r
     //\r
-    // Don't support get Base Clock Frequency information via another method\r
+    // Check if controller version V4.0\r
     //\r
-    return EFI_UNSUPPORTED;\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
+    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
+\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
+\r
+    Status = SdMmcHcOrMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL2, sizeof (HostCtrl2), &HostCtrl2);\r
+    if (EFI_ERROR (Status)) {\r
+      return Status;\r
+    }\r
   }\r
-  //\r
-  // Supply 400KHz clock frequency at initialization phase.\r
-  //\r
-  InitFreq = 400;\r
-  Status = SdMmcHcClockSupply (PciIo, Slot, InitFreq, Capability);\r
-  return Status;\r
+\r
+  return EFI_SUCCESS;\r
 }\r
 \r
 /**\r
@@ -939,14 +1125,14 @@ SdMmcHcInitClockFreq (
 **/\r
 EFI_STATUS\r
 SdMmcHcInitPowerVoltage (\r
-  IN EFI_PCI_IO_PROTOCOL    *PciIo,\r
-  IN UINT8                  Slot,\r
-  IN SD_MMC_HC_SLOT_CAP     Capability\r
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN UINT8                Slot,\r
+  IN SD_MMC_HC_SLOT_CAP   Capability\r
   )\r
 {\r
-  EFI_STATUS                Status;\r
-  UINT8                     MaxVoltage;\r
-  UINT8                     HostCtrl2;\r
+  EFI_STATUS  Status;\r
+  UINT8       MaxVoltage;\r
+  UINT8       HostCtrl2;\r
 \r
   //\r
   // Calculate supported maximum voltage according to SD Bus Voltage Select\r
@@ -967,7 +1153,7 @@ SdMmcHcInitPowerVoltage (
     //\r
     MaxVoltage = 0x0A;\r
     HostCtrl2  = BIT3;\r
-    Status = SdMmcHcOrMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL2, sizeof (HostCtrl2), &HostCtrl2);\r
+    Status     = SdMmcHcOrMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL2, sizeof (HostCtrl2), &HostCtrl2);\r
     gBS->Stall (5000);\r
     if (EFI_ERROR (Status)) {\r
       return Status;\r
@@ -999,12 +1185,12 @@ SdMmcHcInitPowerVoltage (
 **/\r
 EFI_STATUS\r
 SdMmcHcInitTimeoutCtrl (\r
-  IN EFI_PCI_IO_PROTOCOL    *PciIo,\r
-  IN UINT8                  Slot\r
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN UINT8                Slot\r
   )\r
 {\r
-  EFI_STATUS                Status;\r
-  UINT8                     Timeout;\r
+  EFI_STATUS  Status;\r
+  UINT8       Timeout;\r
 \r
   Timeout = 0x0E;\r
   Status  = SdMmcHcRwMmio (PciIo, Slot, SD_MMC_HC_TIMEOUT_CTRL, FALSE, sizeof (Timeout), &Timeout);\r
@@ -1016,9 +1202,8 @@ SdMmcHcInitTimeoutCtrl (
   Initial SD/MMC host controller with lowest clock frequency, max power and max timeout value\r
   at initialization.\r
 \r
-  @param[in] PciIo          The PCI IO protocol instance.\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] Capability     The capability of the slot.\r
 \r
   @retval EFI_SUCCESS       The host controller is initialized successfully.\r
   @retval Others            The host controller isn't initialized successfully.\r
@@ -1026,14 +1211,52 @@ SdMmcHcInitTimeoutCtrl (
 **/\r
 EFI_STATUS\r
 SdMmcHcInitHost (\r
-  IN EFI_PCI_IO_PROTOCOL    *PciIo,\r
-  IN UINT8                  Slot,\r
-  IN SD_MMC_HC_SLOT_CAP     Capability\r
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,\r
+  IN UINT8                   Slot\r
   )\r
 {\r
-  EFI_STATUS       Status;\r
+  EFI_STATUS           Status;\r
+  EFI_PCI_IO_PROTOCOL  *PciIo;\r
+  SD_MMC_HC_SLOT_CAP   Capability;\r
+\r
+  //\r
+  // Notify the SD/MMC override protocol that we are about to initialize\r
+  // the SD/MMC host controller.\r
+  //\r
+  if ((mOverride != NULL) && (mOverride->NotifyPhase != NULL)) {\r
+    Status = mOverride->NotifyPhase (\r
+                          Private->ControllerHandle,\r
+                          Slot,\r
+                          EdkiiSdMmcInitHostPre,\r
+                          NULL\r
+                          );\r
+    if (EFI_ERROR (Status)) {\r
+      DEBUG ((\r
+        DEBUG_WARN,\r
+        "%a: SD/MMC pre init notifier callback failed - %r\n",\r
+        __FUNCTION__,\r
+        Status\r
+        ));\r
+      return Status;\r
+    }\r
+  }\r
+\r
+  PciIo      = Private->PciIo;\r
+  Capability = Private->Capability[Slot];\r
+\r
+  Status = SdMmcHcInitV4Enhancements (PciIo, Slot, Capability, Private->ControllerVersion[Slot]);\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
 \r
-  Status = SdMmcHcInitClockFreq (PciIo, Slot, Capability);\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
@@ -1044,9 +1267,158 @@ SdMmcHcInitHost (
   }\r
 \r
   Status = SdMmcHcInitTimeoutCtrl (PciIo, Slot);\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
+  //\r
+  // Notify the SD/MMC override protocol that we are have just initialized\r
+  // the SD/MMC host controller.\r
+  //\r
+  if ((mOverride != NULL) && (mOverride->NotifyPhase != NULL)) {\r
+    Status = mOverride->NotifyPhase (\r
+                          Private->ControllerHandle,\r
+                          Slot,\r
+                          EdkiiSdMmcInitHostPost,\r
+                          NULL\r
+                          );\r
+    if (EFI_ERROR (Status)) {\r
+      DEBUG ((\r
+        DEBUG_WARN,\r
+        "%a: SD/MMC post init notifier callback failed - %r\n",\r
+        __FUNCTION__,\r
+        Status\r
+        ));\r
+    }\r
+  }\r
+\r
   return Status;\r
 }\r
 \r
+/**\r
+  Set SD Host Controler control 2 registry according to selected speed.\r
+\r
+  @param[in] ControllerHandle The handle of the controller.\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] Timing           The timing to select.\r
+\r
+  @retval EFI_SUCCESS         The timing is set successfully.\r
+  @retval Others              The timing isn't set successfully.\r
+**/\r
+EFI_STATUS\r
+SdMmcHcUhsSignaling (\r
+  IN EFI_HANDLE           ControllerHandle,\r
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN UINT8                Slot,\r
+  IN SD_MMC_BUS_MODE      Timing\r
+  )\r
+{\r
+  EFI_STATUS  Status;\r
+  UINT8       HostCtrl2;\r
+\r
+  HostCtrl2 = (UINT8) ~SD_MMC_HC_CTRL_UHS_MASK;\r
+  Status    = SdMmcHcAndMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL2, sizeof (HostCtrl2), &HostCtrl2);\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
+  switch (Timing) {\r
+    case SdMmcUhsSdr12:\r
+      HostCtrl2 = SD_MMC_HC_CTRL_UHS_SDR12;\r
+      break;\r
+    case SdMmcUhsSdr25:\r
+      HostCtrl2 = SD_MMC_HC_CTRL_UHS_SDR25;\r
+      break;\r
+    case SdMmcUhsSdr50:\r
+      HostCtrl2 = SD_MMC_HC_CTRL_UHS_SDR50;\r
+      break;\r
+    case SdMmcUhsSdr104:\r
+      HostCtrl2 = SD_MMC_HC_CTRL_UHS_SDR104;\r
+      break;\r
+    case SdMmcUhsDdr50:\r
+      HostCtrl2 = SD_MMC_HC_CTRL_UHS_DDR50;\r
+      break;\r
+    case SdMmcMmcLegacy:\r
+      HostCtrl2 = SD_MMC_HC_CTRL_MMC_LEGACY;\r
+      break;\r
+    case SdMmcMmcHsSdr:\r
+      HostCtrl2 = SD_MMC_HC_CTRL_MMC_HS_SDR;\r
+      break;\r
+    case SdMmcMmcHsDdr:\r
+      HostCtrl2 = SD_MMC_HC_CTRL_MMC_HS_DDR;\r
+      break;\r
+    case SdMmcMmcHs200:\r
+      HostCtrl2 = SD_MMC_HC_CTRL_MMC_HS200;\r
+      break;\r
+    case SdMmcMmcHs400:\r
+      HostCtrl2 = SD_MMC_HC_CTRL_MMC_HS400;\r
+      break;\r
+    default:\r
+      HostCtrl2 = 0;\r
+      break;\r
+  }\r
+\r
+  Status = SdMmcHcOrMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL2, sizeof (HostCtrl2), &HostCtrl2);\r
+  if (EFI_ERROR (Status)) {\r
+    return Status;\r
+  }\r
+\r
+  if ((mOverride != NULL) && (mOverride->NotifyPhase != NULL)) {\r
+    Status = mOverride->NotifyPhase (\r
+                          ControllerHandle,\r
+                          Slot,\r
+                          EdkiiSdMmcUhsSignaling,\r
+                          &Timing\r
+                          );\r
+    if (EFI_ERROR (Status)) {\r
+      DEBUG ((\r
+        DEBUG_ERROR,\r
+        "%a: SD/MMC uhs signaling notifier callback failed - %r\n",\r
+        __FUNCTION__,\r
+        Status\r
+        ));\r
+      return Status;\r
+    }\r
+  }\r
+\r
+  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
@@ -1060,19 +1432,19 @@ SdMmcHcInitHost (
 **/\r
 EFI_STATUS\r
 SdMmcHcLedOnOff (\r
-  IN EFI_PCI_IO_PROTOCOL    *PciIo,\r
-  IN UINT8                  Slot,\r
-  IN BOOLEAN                On\r
+  IN EFI_PCI_IO_PROTOCOL  *PciIo,\r
+  IN UINT8                Slot,\r
+  IN BOOLEAN              On\r
   )\r
 {\r
-  EFI_STATUS                Status;\r
-  UINT8                     HostCtrl1;\r
+  EFI_STATUS  Status;\r
+  UINT8       HostCtrl1;\r
 \r
   if (On) {\r
     HostCtrl1 = BIT0;\r
     Status    = SdMmcHcOrMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL1, sizeof (HostCtrl1), &HostCtrl1);\r
   } else {\r
-    HostCtrl1 = (UINT8)~BIT0;\r
+    HostCtrl1 = (UINT8) ~BIT0;\r
     Status    = SdMmcHcAndMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL1, sizeof (HostCtrl1), &HostCtrl1);\r
   }\r
 \r
@@ -1082,9 +1454,10 @@ SdMmcHcLedOnOff (
 /**\r
   Build ADMA descriptor table for transfer.\r
 \r
-  Refer to SD Host Controller Simplified spec 3.0 Section 1.13 for details.\r
+  Refer to SD Host Controller Simplified spec 4.2 Section 1.13 for details.\r
 \r
   @param[in] Trb            The pointer to the SD_MMC_HC_TRB instance.\r
+  @param[in] ControllerVer  The version of host controller.\r
 \r
   @retval EFI_SUCCESS       The ADMA descriptor table is created successfully.\r
   @retval Others            The ADMA descriptor table isn't created successfully.\r
@@ -1092,57 +1465,97 @@ SdMmcHcLedOnOff (
 **/\r
 EFI_STATUS\r
 BuildAdmaDescTable (\r
-  IN SD_MMC_HC_TRB          *Trb\r
+  IN SD_MMC_HC_TRB  *Trb,\r
+  IN UINT16         ControllerVer\r
   )\r
 {\r
-  EFI_PHYSICAL_ADDRESS      Data;\r
-  UINT64                    DataLen;\r
-  UINT64                    Entries;\r
-  UINT32                    Index;\r
-  UINT64                    Remaining;\r
-  UINT32                    Address;\r
-  UINTN                     TableSize;\r
-  EFI_PCI_IO_PROTOCOL       *PciIo;\r
-  EFI_STATUS                Status;\r
-  UINTN                     Bytes;\r
+  EFI_PHYSICAL_ADDRESS  Data;\r
+  UINT64                DataLen;\r
+  UINT64                Entries;\r
+  UINT32                Index;\r
+  UINT64                Remaining;\r
+  UINT64                Address;\r
+  UINTN                 TableSize;\r
+  EFI_PCI_IO_PROTOCOL   *PciIo;\r
+  EFI_STATUS            Status;\r
+  UINTN                 Bytes;\r
+  UINT32                AdmaMaxDataPerLine;\r
+  UINT32                DescSize;\r
+  VOID                  *AdmaDesc;\r
+\r
+  AdmaMaxDataPerLine = ADMA_MAX_DATA_PER_LINE_16B;\r
+  DescSize           = sizeof (SD_MMC_HC_ADMA_32_DESC_LINE);\r
+  AdmaDesc           = NULL;\r
 \r
   Data    = Trb->DataPhy;\r
   DataLen = Trb->DataLen;\r
   PciIo   = Trb->Private->PciIo;\r
+\r
   //\r
-  // Only support 32bit ADMA Descriptor Table\r
+  // Check for valid ranges in 32bit ADMA Descriptor Table\r
   //\r
-  if ((Data >= 0x100000000ul) || ((Data + DataLen) > 0x100000000ul)) {\r
+  if ((Trb->Mode == SdMmcAdma32bMode) &&\r
+      ((Data >= 0x100000000ul) || ((Data + DataLen) > 0x100000000ul)))\r
+  {\r
     return EFI_INVALID_PARAMETER;\r
   }\r
+\r
+  //\r
+  // Check address field alignment\r
+  //\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
+    if ((Data & (BIT0 | BIT1 | BIT2)) != 0) {\r
+      DEBUG ((DEBUG_INFO, "The buffer [0x%x] to construct ADMA desc is not aligned to 8 bytes boundary!\n", Data));\r
+    }\r
+  } else {\r
+    //\r
+    // Address field shall be set on 32-bit boundary (Lower 2-bit is always set to 0)\r
+    //\r
+    if ((Data & (BIT0 | BIT1)) != 0) {\r
+      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
   //\r
-  // Address field shall be set on 32-bit boundary (Lower 2-bit is always set to 0)\r
-  // for 32-bit address descriptor table.\r
+  // Configure 26b data length.\r
   //\r
-  if ((Data & (BIT0 | BIT1)) != 0) {\r
-    DEBUG ((EFI_D_INFO, "The buffer [0x%x] to construct ADMA desc is not aligned to 4 bytes boundary!\n", Data));\r
+  if (Trb->AdmaLengthMode == SdMmcAdmaLen26b) {\r
+    AdmaMaxDataPerLine = ADMA_MAX_DATA_PER_LINE_26B;\r
   }\r
 \r
-  Entries   = DivU64x32 ((DataLen + ADMA_MAX_DATA_PER_LINE - 1), ADMA_MAX_DATA_PER_LINE);\r
-  TableSize = (UINTN)MultU64x32 (Entries, sizeof (SD_MMC_HC_ADMA_DESC_LINE));\r
+  Entries        = DivU64x32 ((DataLen + AdmaMaxDataPerLine - 1), AdmaMaxDataPerLine);\r
+  TableSize      = (UINTN)MultU64x32 (Entries, DescSize);\r
   Trb->AdmaPages = (UINT32)EFI_SIZE_TO_PAGES (TableSize);\r
-  Status = PciIo->AllocateBuffer (\r
-                    PciIo,\r
-                    AllocateAnyPages,\r
-                    EfiBootServicesData,\r
-                    EFI_SIZE_TO_PAGES (TableSize),\r
-                    (VOID **)&Trb->AdmaDesc,\r
-                    0\r
-                    );\r
+  Status         = PciIo->AllocateBuffer (\r
+                            PciIo,\r
+                            AllocateAnyPages,\r
+                            EfiBootServicesData,\r
+                            EFI_SIZE_TO_PAGES (TableSize),\r
+                            (VOID **)&AdmaDesc,\r
+                            0\r
+                            );\r
   if (EFI_ERROR (Status)) {\r
     return EFI_OUT_OF_RESOURCES;\r
   }\r
-  ZeroMem (Trb->AdmaDesc, TableSize);\r
+\r
+  ZeroMem (AdmaDesc, TableSize);\r
   Bytes  = TableSize;\r
   Status = PciIo->Map (\r
                     PciIo,\r
                     EfiPciIoOperationBusMasterCommonBuffer,\r
-                    Trb->AdmaDesc,\r
+                    AdmaDesc,\r
                     &Bytes,\r
                     &Trb->AdmaDescPhy,\r
                     &Trb->AdmaMap\r
@@ -1155,51 +1568,266 @@ BuildAdmaDescTable (
     PciIo->FreeBuffer (\r
              PciIo,\r
              EFI_SIZE_TO_PAGES (TableSize),\r
-             Trb->AdmaDesc\r
+             AdmaDesc\r
              );\r
     return EFI_OUT_OF_RESOURCES;\r
   }\r
 \r
-  if ((UINT64)(UINTN)Trb->AdmaDescPhy > 0x100000000ul) {\r
+  if ((Trb->Mode == SdMmcAdma32bMode) &&\r
+      ((UINT64)(UINTN)Trb->AdmaDescPhy > 0x100000000ul))\r
+  {\r
     //\r
     // The ADMA doesn't support 64bit addressing.\r
     //\r
     PciIo->Unmap (\r
-      PciIo,\r
-      Trb->AdmaMap\r
-    );\r
+             PciIo,\r
+             Trb->AdmaMap\r
+             );\r
+    Trb->AdmaMap = NULL;\r
+\r
     PciIo->FreeBuffer (\r
-      PciIo,\r
-      EFI_SIZE_TO_PAGES (TableSize),\r
-      Trb->AdmaDesc\r
-    );\r
+             PciIo,\r
+             EFI_SIZE_TO_PAGES (TableSize),\r
+             AdmaDesc\r
+             );\r
     return EFI_DEVICE_ERROR;\r
   }\r
 \r
   Remaining = DataLen;\r
-  Address   = (UINT32)Data;\r
-  for (Index = 0; Index < Entries; Index++) {\r
-    if (Remaining <= ADMA_MAX_DATA_PER_LINE) {\r
-      Trb->AdmaDesc[Index].Valid = 1;\r
-      Trb->AdmaDesc[Index].Act   = 2;\r
-      Trb->AdmaDesc[Index].Length  = (UINT16)Remaining;\r
-      Trb->AdmaDesc[Index].Address = Address;\r
-      break;\r
-    } else {\r
-      Trb->AdmaDesc[Index].Valid = 1;\r
-      Trb->AdmaDesc[Index].Act   = 2;\r
-      Trb->AdmaDesc[Index].Length  = 0;\r
-      Trb->AdmaDesc[Index].Address = Address;\r
-    }\r
-\r
-    Remaining -= ADMA_MAX_DATA_PER_LINE;\r
-    Address   += ADMA_MAX_DATA_PER_LINE;\r
+  Address   = Data;\r
+  if (Trb->Mode == SdMmcAdma32bMode) {\r
+    Trb->Adma32Desc = AdmaDesc;\r
+  } else if (Trb->Mode == SdMmcAdma64bV3Mode) {\r
+    Trb->Adma64V3Desc = AdmaDesc;\r
+  } else {\r
+    Trb->Adma64V4Desc = AdmaDesc;\r
   }\r
 \r
+  for (Index = 0; Index < Entries; Index++) {\r
+    if (Trb->Mode == SdMmcAdma32bMode) {\r
+      if (Remaining <= AdmaMaxDataPerLine) {\r
+        Trb->Adma32Desc[Index].Valid = 1;\r
+        Trb->Adma32Desc[Index].Act   = 2;\r
+        if (Trb->AdmaLengthMode == SdMmcAdmaLen26b) {\r
+          Trb->Adma32Desc[Index].UpperLength = (UINT16)RShiftU64 (Remaining, 16);\r
+        }\r
+\r
+        Trb->Adma32Desc[Index].LowerLength = (UINT16)(Remaining & MAX_UINT16);\r
+        Trb->Adma32Desc[Index].Address     = (UINT32)Address;\r
+        break;\r
+      } else {\r
+        Trb->Adma32Desc[Index].Valid = 1;\r
+        Trb->Adma32Desc[Index].Act   = 2;\r
+        if (Trb->AdmaLengthMode == SdMmcAdmaLen26b) {\r
+          Trb->Adma32Desc[Index].UpperLength = 0;\r
+        }\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
+\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
+\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->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
+\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->Adma64V4Desc[Index].Valid = 1;\r
+        Trb->Adma64V4Desc[Index].Act   = 2;\r
+        if (Trb->AdmaLengthMode == SdMmcAdmaLen26b) {\r
+          Trb->Adma64V4Desc[Index].UpperLength = 0;\r
+        }\r
+\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
+    Remaining -= AdmaMaxDataPerLine;\r
+    Address   += AdmaMaxDataPerLine;\r
+  }\r
+\r
+  //\r
+  // Set the last descriptor line as end of descriptor table\r
   //\r
-  // Set the last descriptor line as end of descriptor table\r
-  //\r
-  Trb->AdmaDesc[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
+\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
+\r
+  if (Packet->SdMmcStatusBlk != NULL) {\r
+    DEBUG ((\r
+      DebugLevel,\r
+      "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
+\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
+  {\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
@@ -1218,32 +1846,33 @@ BuildAdmaDescTable (
 **/\r
 SD_MMC_HC_TRB *\r
 SdMmcCreateTrb (\r
-  IN SD_MMC_HC_PRIVATE_DATA              *Private,\r
-  IN UINT8                               Slot,\r
-  IN EFI_SD_MMC_PASS_THRU_COMMAND_PACKET *Packet,\r
-  IN EFI_EVENT                           Event\r
+  IN SD_MMC_HC_PRIVATE_DATA               *Private,\r
+  IN UINT8                                Slot,\r
+  IN EFI_SD_MMC_PASS_THRU_COMMAND_PACKET  *Packet,\r
+  IN EFI_EVENT                            Event\r
   )\r
 {\r
-  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
+  SD_MMC_HC_TRB  *Trb;\r
+  EFI_STATUS     Status;\r
+  EFI_TPL        OldTpl;\r
 \r
   Trb = AllocateZeroPool (sizeof (SD_MMC_HC_TRB));\r
   if (Trb == NULL) {\r
     return NULL;\r
   }\r
 \r
-  Trb->Signature = SD_MMC_HC_TRB_SIG;\r
-  Trb->Slot      = Slot;\r
-  Trb->BlockSize = 0x200;\r
-  Trb->Packet    = Packet;\r
-  Trb->Event     = Event;\r
-  Trb->Started   = FALSE;\r
-  Trb->Timeout   = Packet->Timeout;\r
-  Trb->Private   = Private;\r
+  Trb->Signature                = SD_MMC_HC_TRB_SIG;\r
+  Trb->Slot                     = Slot;\r
+  Trb->BlockSize                = 0x200;\r
+  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
     Trb->Data    = Packet->InDataBuffer;\r
@@ -1260,52 +1889,55 @@ SdMmcCreateTrb (
     goto Error;\r
   }\r
 \r
-  if (Trb->Read) {\r
-    Flag = EfiPciIoOperationBusMasterWrite;\r
-  } else {\r
-    Flag = EfiPciIoOperationBusMasterRead;\r
+  if ((Trb->DataLen != 0) && (Trb->DataLen < Trb->BlockSize)) {\r
+    Trb->BlockSize = (UINT16)Trb->DataLen;\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
+  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
+    Trb->Mode = SdMmcPioMode;\r
+  } else {\r
+    if (Trb->DataLen == 0) {\r
+      Trb->Mode = SdMmcNoData;\r
+    } else if (Private->Capability[Slot].Adma2 != 0) {\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
+      {\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
+      {\r
+        Trb->Mode = SdMmcAdma64bV4Mode;\r
+      }\r
 \r
-  if ((Trb->DataLen % Trb->BlockSize) != 0) {\r
-    if (Trb->DataLen < Trb->BlockSize) {\r
-      Trb->BlockSize = (UINT16)Trb->DataLen;\r
-    }\r
-  }\r
+      if (Private->ControllerVersion[Slot] >= SD_MMC_HC_CTRL_VER_410) {\r
+        Trb->AdmaLengthMode = SdMmcAdmaLen26b;\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);\r
-    if (EFI_ERROR (Status)) {\r
-      PciIo->Unmap (PciIo, Trb->DataMap);\r
-      goto Error;\r
+      Status = SdMmcSetupMemoryForDmaTransfer (Private, Slot, Trb);\r
+      if (EFI_ERROR (Status)) {\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
-  } else if (Private->Capability[Slot].Sdma != 0) {\r
-    Trb->Mode = SdMmcSdmaMode;\r
-  } else {\r
-    Trb->Mode = SdMmcPioMode;\r
   }\r
 \r
   if (Event != NULL) {\r
-    OldTpl = gBS->RaiseTPL (TPL_CALLBACK);\r
+    OldTpl = gBS->RaiseTPL (TPL_NOTIFY);\r
     InsertTailList (&Private->Queue, &Trb->TrbList);\r
     gBS->RestoreTPL (OldTpl);\r
   }\r
@@ -1325,32 +1957,51 @@ Error:
 **/\r
 VOID\r
 SdMmcFreeTrb (\r
-  IN SD_MMC_HC_TRB           *Trb\r
+  IN SD_MMC_HC_TRB  *Trb\r
   )\r
 {\r
-  EFI_PCI_IO_PROTOCOL        *PciIo;\r
+  EFI_PCI_IO_PROTOCOL  *PciIo;\r
 \r
   PciIo = Trb->Private->PciIo;\r
 \r
   if (Trb->AdmaMap != NULL) {\r
     PciIo->Unmap (\r
-      PciIo,\r
-      Trb->AdmaMap\r
-    );\r
+             PciIo,\r
+             Trb->AdmaMap\r
+             );\r
+  }\r
+\r
+  if (Trb->Adma32Desc != NULL) {\r
+    PciIo->FreeBuffer (\r
+             PciIo,\r
+             Trb->AdmaPages,\r
+             Trb->Adma32Desc\r
+             );\r
   }\r
-  if (Trb->AdmaDesc != NULL) {\r
+\r
+  if (Trb->Adma64V3Desc != NULL) {\r
+    PciIo->FreeBuffer (\r
+             PciIo,\r
+             Trb->AdmaPages,\r
+             Trb->Adma64V3Desc\r
+             );\r
+  }\r
+\r
+  if (Trb->Adma64V4Desc != NULL) {\r
     PciIo->FreeBuffer (\r
-      PciIo,\r
-      Trb->AdmaPages,\r
-      Trb->AdmaDesc\r
-    );\r
+             PciIo,\r
+             Trb->AdmaPages,\r
+             Trb->Adma64V4Desc\r
+             );\r
   }\r
+\r
   if (Trb->DataMap != NULL) {\r
     PciIo->Unmap (\r
-      PciIo,\r
-      Trb->DataMap\r
-    );\r
+             PciIo,\r
+             Trb->DataMap\r
+             );\r
   }\r
+\r
   FreePool (Trb);\r
   return;\r
 }\r
@@ -1368,34 +2019,26 @@ SdMmcFreeTrb (
 **/\r
 EFI_STATUS\r
 SdMmcCheckTrbEnv (\r
-  IN SD_MMC_HC_PRIVATE_DATA           *Private,\r
-  IN SD_MMC_HC_TRB                    *Trb\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
-  EFI_PCI_IO_PROTOCOL                 *PciIo;\r
-  UINT32                              PresentState;\r
+  EFI_STATUS                           Status;\r
+  EFI_SD_MMC_PASS_THRU_COMMAND_PACKET  *Packet;\r
+  EFI_PCI_IO_PROTOCOL                  *PciIo;\r
+  UINT32                               PresentState;\r
 \r
   Packet = Trb->Packet;\r
 \r
   if ((Packet->SdMmcCmdBlk->CommandType == SdMmcCommandTypeAdtc) ||\r
       (Packet->SdMmcCmdBlk->ResponseType == SdMmcResponseTypeR1b) ||\r
-      (Packet->SdMmcCmdBlk->ResponseType == SdMmcResponseTypeR5b)) {\r
+      (Packet->SdMmcCmdBlk->ResponseType == SdMmcResponseTypeR5b))\r
+  {\r
     //\r
     // Wait Command Inhibit (CMD) and Command Inhibit (DAT) in\r
     // the Present State register to be 0\r
     //\r
     PresentState = BIT0 | BIT1;\r
-    //\r
-    // For Send Tuning Block cmd, just wait for Command Inhibit (CMD) to be 0\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
-      PresentState = BIT0;\r
-    }\r
   } else {\r
     //\r
     // Wait Command Inhibit (CMD) in the Present State register\r
@@ -1430,14 +2073,14 @@ SdMmcCheckTrbEnv (
 **/\r
 EFI_STATUS\r
 SdMmcWaitTrbEnv (\r
-  IN SD_MMC_HC_PRIVATE_DATA           *Private,\r
-  IN SD_MMC_HC_TRB                    *Trb\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
-  UINT64                              Timeout;\r
-  BOOLEAN                             InfiniteWait;\r
+  EFI_STATUS                           Status;\r
+  EFI_SD_MMC_PASS_THRU_COMMAND_PACKET  *Packet;\r
+  UINT64                               Timeout;\r
+  BOOLEAN                              InfiniteWait;\r
 \r
   //\r
   // Wait Command Complete Interrupt Status bit in Normal Interrupt Status Register\r
@@ -1458,6 +2101,7 @@ SdMmcWaitTrbEnv (
     if (Status != EFI_NOT_READY) {\r
       return Status;\r
     }\r
+\r
     //\r
     // Stall for 1 microsecond.\r
     //\r
@@ -1481,22 +2125,25 @@ SdMmcWaitTrbEnv (
 **/\r
 EFI_STATUS\r
 SdMmcExecTrb (\r
-  IN SD_MMC_HC_PRIVATE_DATA           *Private,\r
-  IN SD_MMC_HC_TRB                    *Trb\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
-  EFI_PCI_IO_PROTOCOL                 *PciIo;\r
-  UINT16                              Cmd;\r
-  UINT16                              IntStatus;\r
-  UINT32                              Argument;\r
-  UINT16                              BlkCount;\r
-  UINT16                              BlkSize;\r
-  UINT16                              TransMode;\r
-  UINT8                               HostCtrl1;\r
-  UINT32                              SdmaAddr;\r
-  UINT64                              AdmaAddr;\r
+  EFI_STATUS                           Status;\r
+  EFI_SD_MMC_PASS_THRU_COMMAND_PACKET  *Packet;\r
+  EFI_PCI_IO_PROTOCOL                  *PciIo;\r
+  UINT16                               Cmd;\r
+  UINT16                               IntStatus;\r
+  UINT32                               Argument;\r
+  UINT32                               BlkCount;\r
+  UINT16                               BlkSize;\r
+  UINT16                               TransMode;\r
+  UINT8                                HostCtrl1;\r
+  UINT64                               SdmaAddr;\r
+  UINT64                               AdmaAddr;\r
+  BOOLEAN                              AddressingMode64;\r
+\r
+  AddressingMode64 = FALSE;\r
 \r
   Packet = Trb->Packet;\r
   PciIo  = Trb->Private->PciIo;\r
@@ -1508,6 +2155,7 @@ SdMmcExecTrb (
   if (EFI_ERROR (Status)) {\r
     return Status;\r
   }\r
+\r
   //\r
   // Clear all bits in Normal Interrupt Status Register excepts for Card Removal & Card Insertion bits.\r
   //\r
@@ -1516,12 +2164,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 (\r
+               PciIo,\r
+               Trb->Slot,\r
+               SD_MMC_HC_HOST_CTRL2,\r
+               sizeof (UINT16),\r
+               SD_MMC_HC_64_ADDR_EN,\r
+               SD_MMC_HC_64_ADDR_EN\r
+               );\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
+  {\r
     HostCtrl1 = BIT4;\r
-    Status = SdMmcHcOrMmio (PciIo, Trb->Slot, SD_MMC_HC_HOST_CTRL1, sizeof (HostCtrl1), &HostCtrl1);\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
@@ -1530,16 +2201,27 @@ SdMmcExecTrb (
   SdMmcHcLedOnOff (PciIo, Trb->Slot, TRUE);\r
 \r
   if (Trb->Mode == SdMmcSdmaMode) {\r
-    if ((UINT64)(UINTN)Trb->DataPhy >= 0x100000000ul) {\r
+    if ((!AddressingMode64) &&\r
+        ((UINT64)(UINTN)Trb->DataPhy >= 0x100000000ul))\r
+    {\r
       return EFI_INVALID_PARAMETER;\r
     }\r
 \r
-    SdmaAddr = (UINT32)(UINTN)Trb->DataPhy;\r
-    Status   = SdMmcHcRwMmio (PciIo, Trb->Slot, SD_MMC_HC_SDMA_ADDR, FALSE, sizeof (SdmaAddr), &SdmaAddr);\r
+    SdmaAddr = (UINT64)(UINTN)Trb->DataPhy;\r
+\r
+    if (Private->ControllerVersion[Trb->Slot] >= SD_MMC_HC_CTRL_VER_400) {\r
+      Status = SdMmcHcRwMmio (PciIo, Trb->Slot, SD_MMC_HC_ADMA_SYS_ADDR, FALSE, sizeof (UINT64), &SdmaAddr);\r
+    } else {\r
+      Status = SdMmcHcRwMmio (PciIo, Trb->Slot, SD_MMC_HC_SDMA_ADDR, FALSE, sizeof (UINT32), &SdmaAddr);\r
+    }\r
+\r
     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
+  {\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
@@ -1560,8 +2242,20 @@ SdMmcExecTrb (
     return Status;\r
   }\r
 \r
-  BlkCount = (UINT16)(Trb->DataLen / Trb->BlockSize);\r
-  Status   = SdMmcHcRwMmio (PciIo, Trb->Slot, SD_MMC_HC_BLK_COUNT, FALSE, sizeof (BlkCount), &BlkCount);\r
+  BlkCount = 0;\r
+  if (Trb->Mode != SdMmcNoData) {\r
+    //\r
+    // Calcuate Block Count.\r
+    //\r
+    BlkCount = (Trb->DataLen / Trb->BlockSize);\r
+  }\r
+\r
+  if (Private->ControllerVersion[Trb->Slot] >= SD_MMC_HC_CTRL_VER_410) {\r
+    Status = SdMmcHcRwMmio (PciIo, Trb->Slot, SD_MMC_HC_SDMA_ADDR, FALSE, sizeof (UINT32), &BlkCount);\r
+  } else {\r
+    Status = SdMmcHcRwMmio (PciIo, Trb->Slot, SD_MMC_HC_BLK_COUNT, FALSE, sizeof (UINT16), &BlkCount);\r
+  }\r
+\r
   if (EFI_ERROR (Status)) {\r
     return Status;\r
   }\r
@@ -1577,12 +2271,15 @@ SdMmcExecTrb (
     if (Trb->Mode != SdMmcPioMode) {\r
       TransMode |= BIT0;\r
     }\r
+\r
     if (Trb->Read) {\r
       TransMode |= BIT4;\r
     }\r
-    if (BlkCount != 0) {\r
+\r
+    if (BlkCount > 1) {\r
       TransMode |= BIT5 | BIT1;\r
     }\r
+\r
     //\r
     // Only SD memory card needs to use AUTO CMD12 feature.\r
     //\r
@@ -1598,10 +2295,11 @@ SdMmcExecTrb (
     return Status;\r
   }\r
 \r
-  Cmd = (UINT16)LShiftU64(Packet->SdMmcCmdBlk->CommandIndex, 8);\r
+  Cmd = (UINT16)LShiftU64 (Packet->SdMmcCmdBlk->CommandIndex, 8);\r
   if (Packet->SdMmcCmdBlk->CommandType == SdMmcCommandTypeAdtc) {\r
     Cmd |= BIT5;\r
   }\r
+\r
   //\r
   // Convert ResponseType to value\r
   //\r
@@ -1615,7 +2313,7 @@ SdMmcExecTrb (
         break;\r
       case SdMmcResponseTypeR2:\r
         Cmd |= (BIT0 | BIT3);\r
-       break;\r
+        break;\r
       case SdMmcResponseTypeR3:\r
       case SdMmcResponseTypeR4:\r
         Cmd |= BIT1;\r
@@ -1629,6 +2327,7 @@ SdMmcExecTrb (
         break;\r
     }\r
   }\r
+\r
   //\r
   // Execute cmd\r
   //\r
@@ -1637,213 +2336,539 @@ SdMmcExecTrb (
 }\r
 \r
 /**\r
-  Check the TRB execution result.\r
+  Performs SW reset based on passed error status mask.\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
+  @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
-  UINT32                              SdmaAddr;\r
-  UINT8                               Index;\r
-  UINT8                               SwReset;\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
+\r
+  if ((ErrIntStatus & 0x70) != 0) {\r
+    SwReset |= BIT2;\r
+  }\r
+\r
   Status = SdMmcHcRwMmio (\r
              Private->PciIo,\r
-             Trb->Slot,\r
-             SD_MMC_HC_NOR_INT_STS,\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
+             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
-      goto Done;\r
+      return Status;\r
     }\r
-    if ((IntStatus & 0x0F) != 0) {\r
-      SwReset |= BIT1;\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
+      return Status;\r
     }\r
-    if ((IntStatus & 0xF0) != 0) {\r
-      SwReset |= BIT2;\r
+\r
+    Status = SdMmcGetResponse (Private, Trb);\r
+    if (EFI_ERROR (Status)) {\r
+      return Status;\r
     }\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
+    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
+\r
+    Data16 = BIT5;\r
+    SdMmcHcRwMmio (Private->PciIo, Trb->Slot, SD_MMC_HC_NOR_INT_STS, FALSE, sizeof (Data16), &Data16);\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
-    Status = SdMmcHcWaitMmioSet (\r
+\r
+    Trb->PioBlockIndex++;\r
+  } else {\r
+    if ((IntStatus & BIT4) == 0) {\r
+      return EFI_NOT_READY;\r
+    }\r
+\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_BUF_DAT_PORT,\r
+                                   Count,\r
+                                   (VOID *)((UINT8 *)Trb->Data + (Trb->BlockSize * Trb->PioBlockIndex))\r
+                                   );\r
+    if (EFI_ERROR (Status)) {\r
+      return Status;\r
+    }\r
+\r
+    Trb->PioBlockIndex++;\r
+  }\r
+\r
+  if (Trb->PioBlockIndex >= BlockCount) {\r
+    Trb->PioModeTransferCompleted = TRUE;\r
+    return EFI_SUCCESS;\r
+  } else {\r
+    return EFI_NOT_READY;\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_SW_RST,\r
-               sizeof (SwReset),\r
-               0xFF,\r
-               0,\r
-               SD_MMC_HC_GENERIC_TIMEOUT\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
-    Status = EFI_DEVICE_ERROR;\r
-    goto Done;\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
+  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
-      goto Done;\r
+      return Status;\r
     }\r
-    //\r
-    // Update SDMA Address register.\r
-    //\r
-    SdmaAddr = SD_MMC_SDMA_ROUND_UP ((UINT32)(UINTN)Trb->DataPhy, SD_MMC_SDMA_BOUNDARY);\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
+    Status = SdMmcUpdateSdmaAddress (Private, Trb);\r
     if (EFI_ERROR (Status)) {\r
-      goto Done;\r
+      return Status;\r
     }\r
-    Trb->DataPhy = (UINT32)(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
-    // While performing tuning procedure (Execute Tuning is set to 1),\r
-    // Transfer Completeis not set to 1\r
-    // Refer to SD Host Controller Simplified Specification 3.0 table 2-23 for details.\r
-    //\r
-    Status = EFI_SUCCESS;\r
+       (Packet->SdMmcCmdBlk->CommandIndex == SD_SEND_TUNING_BLOCK)))\r
+  {\r
+    Status = SdMmcTransferDataWithPio (Private, Trb, IntStatus);\r
     goto Done;\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
+  if (!Trb->CommandComplete) {\r
+    Status = SdMmcCheckCommandComplete (Private, Trb, IntStatus);\r
+    if (EFI_ERROR (Status)) {\r
+      goto Done;\r
     }\r
   }\r
 \r
+  if ((Packet->SdMmcCmdBlk->CommandType == SdMmcCommandTypeAdtc) ||\r
+      (Packet->SdMmcCmdBlk->ResponseType == SdMmcResponseTypeR1b) ||\r
+      (Packet->SdMmcCmdBlk->ResponseType == SdMmcResponseTypeR5b))\r
+  {\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
@@ -1861,14 +2886,14 @@ Done:
 **/\r
 EFI_STATUS\r
 SdMmcWaitTrbResult (\r
-  IN SD_MMC_HC_PRIVATE_DATA           *Private,\r
-  IN SD_MMC_HC_TRB                    *Trb\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
-  UINT64                              Timeout;\r
-  BOOLEAN                             InfiniteWait;\r
+  EFI_STATUS                           Status;\r
+  EFI_SD_MMC_PASS_THRU_COMMAND_PACKET  *Packet;\r
+  UINT64                               Timeout;\r
+  BOOLEAN                              InfiniteWait;\r
 \r
   Packet = Trb->Packet;\r
   //\r
@@ -1889,6 +2914,7 @@ SdMmcWaitTrbResult (
     if (Status != EFI_NOT_READY) {\r
       return Status;\r
     }\r
+\r
     //\r
     // Stall for 1 microsecond.\r
     //\r
@@ -1899,4 +2925,3 @@ SdMmcWaitTrbResult (
 \r
   return EFI_TIMEOUT;\r
 }\r
-\r