[PATCH v3 2/2] MdeModulePkg/SdMmcHcDxe: Implement revision 3 of SdMmcOverrideProtocol

["Albecki, Mateusz" <mateusz.albecki@intel.com>]

From: "Albecki, Mateusz" <mateusz.albecki@intel.com>

https://bugzilla.tianocore.org/show_bug.cgi?id=1882

Implement support for GetOperatingParamters notify phase
in SdMmcHcDxe driver. GetOperatingParameters notify phase
is signaled before we start card detection and initialization.
Code has been updated for both eMMC and SD card controllers to
take into consideration those new parameters. Initialization process
has been divided into 2 steps. In the first step we bring the link
up to the point where we can get card identification data(Extended
CSD in eMMC case and SWITCH command response in SD card case). This
data is later used along with controller capabilities and operating
parameters passed in GetOperatingParameters phase to choose prefered
bus settings in GetTargetBusSettings function. Those settings are later
on to start bus training to high speeds. If user passes incompatible
setting with selected bus timing driver will assume it's standard behavior
with respect to that setting. For instance if HS400 has been selected as a
target bus timing due to card and controller support bus width setting of
4 and 1 bit won't be respected and 8 bit setting will be choosen instead.

Cc: Hao A Wu <hao.a.wu@intel.com>
Signed-off-by: Mateusz Albecki <mateusz.albecki@intel.com>
---
 MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/EmmcDevice.c    | 512 +++++++++++++++------
 MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdDevice.c      | 409 +++++++++++++---
 MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHcDxe.c |  52 ++-
 MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHcDxe.h |  18 +-
 MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHci.c   |  34 ++
 MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHci.h   |  19 +
 6 files changed, 813 insertions(+), 231 deletions(-)

diff --git a/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/EmmcDevice.c b/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/EmmcDevice.c
index deaf4468c9..5645a32906 100644
--- a/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/EmmcDevice.c
+++ b/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/EmmcDevice.c
@@ -641,13 +641,13 @@ EmmcSwitchBusWidth (
   Refer to EMMC Electrical Standard Spec 5.1 Section 6.6 and SD Host Controller
   Simplified Spec 3.0 Figure 3-3 for details.
 
-  @param[in] PciIo          A pointer to the EFI_PCI_IO_PROTOCOL instance.
-  @param[in] PassThru       A pointer to the EFI_SD_MMC_PASS_THRU_PROTOCOL instance.
-  @param[in] Slot           The slot number of the SD card to send the command to.
-  @param[in] Rca            The relative device address to be assigned.
-  @param[in] HsTiming       The value to be written to HS_TIMING field of EXT_CSD register.
-  @param[in] Timing         The bus mode timing indicator.
-  @param[in] ClockFreq      The max clock frequency to be set, the unit is MHz.
+  @param[in] PciIo           A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param[in] PassThru        A pointer to the EFI_SD_MMC_PASS_THRU_PROTOCOL instance.
+  @param[in] Slot            The slot number of the SD card to send the command to.
+  @param[in] Rca             The relative device address to be assigned.
+  @param[in] DriverStrength  Driver strength to set for speed modes that support it.
+  @param[in] BusTiming       The bus mode timing indicator.
+  @param[in] ClockFreq       The max clock frequency to be set, the unit is MHz.
 
   @retval EFI_SUCCESS       The operation is done correctly.
   @retval Others            The operation fails.
@@ -659,8 +659,8 @@ EmmcSwitchBusTiming (
   IN EFI_SD_MMC_PASS_THRU_PROTOCOL      *PassThru,
   IN UINT8                              Slot,
   IN UINT16                             Rca,
-  IN UINT8                              HsTiming,
-  IN SD_MMC_BUS_MODE                    Timing,
+  IN EDKII_SD_MMC_DRIVER_STRENGTH       DriverStrength,
+  IN SD_MMC_BUS_MODE                    BusTiming,
   IN UINT32                             ClockFreq
   )
 {
@@ -678,12 +678,29 @@ EmmcSwitchBusTiming (
   //
   Access = 0x03;
   Index  = OFFSET_OF (EMMC_EXT_CSD, HsTiming);
-  Value  = HsTiming;
   CmdSet = 0;
+  switch (BusTiming) {
+    case SdMmcMmcHs400:
+      Value = (UINT8)((DriverStrength.Emmc << 4) | 3);
+      break;
+    case SdMmcMmcHs200:
+      Value = (UINT8)((DriverStrength.Emmc << 4) | 2);
+      break;
+    case SdMmcMmcHsSdr:
+    case SdMmcMmcHsDdr:
+      Value = 1;
+      break;
+    case SdMmcMmcLegacy:
+      Value = 0;
+      break;
+    default:
+      DEBUG ((DEBUG_ERROR, "EmmcSwitchBusTiming: Unsupported BusTiming(%d\n)", BusTiming));
+      return EFI_INVALID_PARAMETER;
+  }
 
   Status = EmmcSwitch (PassThru, Slot, Access, Index, Value, CmdSet);
   if (EFI_ERROR (Status)) {
-    DEBUG ((DEBUG_ERROR, "EmmcSwitchBusTiming: Switch to hstiming %d fails with %r\n", HsTiming, Status));
+    DEBUG ((DEBUG_ERROR, "EmmcSwitchBusTiming: Switch to bus timing %d fails with %r\n", BusTiming, Status));
     return Status;
   }
 
@@ -713,7 +730,7 @@ EmmcSwitchBusTiming (
                           Private->ControllerHandle,
                           Slot,
                           EdkiiSdMmcSwitchClockFreqPost,
-                          &Timing
+                          &BusTiming
                           );
     if (EFI_ERROR (Status)) {
       DEBUG ((
@@ -739,10 +756,7 @@ EmmcSwitchBusTiming (
   @param[in] PassThru       A pointer to the EFI_SD_MMC_PASS_THRU_PROTOCOL instance.
   @param[in] Slot           The slot number of the SD card to send the command to.
   @param[in] Rca            The relative device address to be assigned.
-  @param[in] ClockFreq      The max clock frequency to be set.
-  @param[in] IsDdr          If TRUE, use dual data rate data simpling method. Otherwise
-                            use single data rate data simpling method.
-  @param[in] BusWidth       The bus width to be set, it could be 4 or 8.
+  @param[in] BusMode        Pointer to SD_MMC_BUS_SETTINGS structure containing bus settings.
 
   @retval EFI_SUCCESS       The operation is done correctly.
   @retval Others            The operation fails.
@@ -754,25 +768,34 @@ EmmcSwitchToHighSpeed (
   IN EFI_SD_MMC_PASS_THRU_PROTOCOL      *PassThru,
   IN UINT8                              Slot,
   IN UINT16                             Rca,
-  IN UINT32                             ClockFreq,
-  IN BOOLEAN                            IsDdr,
-  IN UINT8                              BusWidth
+  IN SD_MMC_BUS_SETTINGS                *BusMode
   )
 {
   EFI_STATUS              Status;
-  UINT8                   HsTiming;
   UINT8                   HostCtrl1;
-  SD_MMC_BUS_MODE         Timing;
   SD_MMC_HC_PRIVATE_DATA  *Private;
+  BOOLEAN                 IsDdr;
 
   Private = SD_MMC_HC_PRIVATE_FROM_THIS (PassThru);
 
-  Status = EmmcSwitchBusWidth (PciIo, PassThru, Slot, Rca, IsDdr, BusWidth);
+  if ((BusMode->BusTiming != SdMmcMmcHsSdr && BusMode->BusTiming != SdMmcMmcHsDdr) ||
+      BusMode->ClockFreq > 52) {
+    return EFI_INVALID_PARAMETER;
+  }
+
+  if (BusMode->BusTiming == SdMmcMmcHsDdr) {
+    IsDdr = TRUE;
+  } else {
+    IsDdr = FALSE;
+  }
+
+  Status = EmmcSwitchBusWidth (PciIo, PassThru, Slot, Rca, IsDdr, BusMode->BusWidth);
   if (EFI_ERROR (Status)) {
     return Status;
   }
+
   //
-  // Set to Hight Speed timing
+  // Set to High Speed timing
   //
   HostCtrl1 = BIT2;
   Status = SdMmcHcOrMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL1, sizeof (HostCtrl1), &HostCtrl1);
@@ -780,37 +803,25 @@ EmmcSwitchToHighSpeed (
     return Status;
   }
 
-  if (IsDdr) {
-    Timing = SdMmcMmcHsDdr;
-  } else if (ClockFreq == 52) {
-    Timing = SdMmcMmcHsSdr;
-  } else {
-    Timing = SdMmcMmcLegacy;
-  }
-
-  Status = SdMmcHcUhsSignaling (Private->ControllerHandle, PciIo, Slot, Timing);
+  Status = SdMmcHcUhsSignaling (Private->ControllerHandle, PciIo, Slot, BusMode->BusTiming);
   if (EFI_ERROR (Status)) {
     return Status;
   }
 
-  HsTiming = 1;
-  Status = EmmcSwitchBusTiming (PciIo, PassThru, Slot, Rca, HsTiming, Timing, ClockFreq);
-
-  return Status;
+  return EmmcSwitchBusTiming (PciIo, PassThru, Slot, Rca, BusMode->DriverStrength, BusMode->BusTiming, BusMode->ClockFreq);
 }
 
 /**
-  Switch to the HS200 timing according to request.
+  Switch to the HS200 timing. This function assumes that eMMC bus is still in legacy mode.
 
   Refer to EMMC Electrical Standard Spec 5.1 Section 6.6.8 and SD Host Controller
   Simplified Spec 3.0 Figure 2-29 for details.
 
-  @param[in] PciIo          A pointer to the EFI_PCI_IO_PROTOCOL instance.
-  @param[in] PassThru       A pointer to the EFI_SD_MMC_PASS_THRU_PROTOCOL instance.
-  @param[in] Slot           The slot number of the SD card to send the command to.
-  @param[in] Rca            The relative device address to be assigned.
-  @param[in] ClockFreq      The max clock frequency to be set.
-  @param[in] BusWidth       The bus width to be set, it could be 4 or 8.
+  @param[in] PciIo           A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param[in] PassThru        A pointer to the EFI_SD_MMC_PASS_THRU_PROTOCOL instance.
+  @param[in] Slot            The slot number of the SD card to send the command to.
+  @param[in] Rca             The relative device address to be assigned.
+  @param[in] BusMode         Pointer to SD_MMC_BUS_SETTINGS structure containing bus settings.
 
   @retval EFI_SUCCESS       The operation is done correctly.
   @retval Others            The operation fails.
@@ -822,30 +833,25 @@ EmmcSwitchToHS200 (
   IN EFI_SD_MMC_PASS_THRU_PROTOCOL      *PassThru,
   IN UINT8                              Slot,
   IN UINT16                             Rca,
-  IN UINT32                             ClockFreq,
-  IN UINT8                              BusWidth
+  IN SD_MMC_BUS_SETTINGS                *BusMode
   )
 {
   EFI_STATUS               Status;
-  UINT8                    HsTiming;
   UINT16                   ClockCtrl;
-  SD_MMC_BUS_MODE          Timing;
   SD_MMC_HC_PRIVATE_DATA  *Private;
 
   Private = SD_MMC_HC_PRIVATE_FROM_THIS (PassThru);
 
-  if ((BusWidth != 4) && (BusWidth != 8)) {
+  if (BusMode->BusTiming != SdMmcMmcHs200 ||
+      (BusMode->BusWidth != 4 && BusMode->BusWidth != 8)) {
     return EFI_INVALID_PARAMETER;
   }
 
-  Status = EmmcSwitchBusWidth (PciIo, PassThru, Slot, Rca, FALSE, BusWidth);
+  Status = EmmcSwitchBusWidth (PciIo, PassThru, Slot, Rca, FALSE, BusMode->BusWidth);
   if (EFI_ERROR (Status)) {
     return Status;
   }
   //
-  // Set to HS200/SDR104 timing
-  //
-  //
   // Stop bus clock at first
   //
   Status = SdMmcHcStopClock (PciIo, Slot);
@@ -853,9 +859,7 @@ EmmcSwitchToHS200 (
     return Status;
   }
 
-  Timing = SdMmcMmcHs200;
-
-  Status = SdMmcHcUhsSignaling (Private->ControllerHandle, PciIo, Slot, Timing);
+  Status = SdMmcHcUhsSignaling (Private->ControllerHandle, PciIo, Slot, BusMode->BusTiming);
   if (EFI_ERROR (Status)) {
     return Status;
   }
@@ -881,28 +885,27 @@ EmmcSwitchToHS200 (
   ClockCtrl = BIT2;
   Status = SdMmcHcOrMmio (PciIo, Slot, SD_MMC_HC_CLOCK_CTRL, sizeof (ClockCtrl), &ClockCtrl);
 
-  HsTiming = 2;
-  Status = EmmcSwitchBusTiming (PciIo, PassThru, Slot, Rca, HsTiming, Timing, ClockFreq);
+  Status = EmmcSwitchBusTiming (PciIo, PassThru, Slot, Rca, BusMode->DriverStrength, BusMode->BusTiming, BusMode->ClockFreq);
   if (EFI_ERROR (Status)) {
     return Status;
   }
 
-  Status = EmmcTuningClkForHs200 (PciIo, PassThru, Slot, BusWidth);
+  Status = EmmcTuningClkForHs200 (PciIo, PassThru, Slot, BusMode->BusWidth);
 
   return Status;
 }
 
 /**
-  Switch to the HS400 timing according to request.
+  Switch to the HS400 timing. This function assumes that eMMC bus is still in legacy mode.
 
   Refer to EMMC Electrical Standard Spec 5.1 Section 6.6.8 and SD Host Controller
   Simplified Spec 3.0 Figure 2-29 for details.
 
-  @param[in] PciIo          A pointer to the EFI_PCI_IO_PROTOCOL instance.
-  @param[in] PassThru       A pointer to the EFI_SD_MMC_PASS_THRU_PROTOCOL instance.
-  @param[in] Slot           The slot number of the SD card to send the command to.
-  @param[in] Rca            The relative device address to be assigned.
-  @param[in] ClockFreq      The max clock frequency to be set.
+  @param[in] PciIo           A pointer to the EFI_PCI_IO_PROTOCOL instance.
+  @param[in] PassThru        A pointer to the EFI_SD_MMC_PASS_THRU_PROTOCOL instance.
+  @param[in] Slot            The slot number of the SD card to send the command to.
+  @param[in] Rca             The relative device address to be assigned.
+  @param[in] BusMode         Pointer to SD_MMC_BUS_SETTINGS structure containing bus settings.
 
   @retval EFI_SUCCESS       The operation is done correctly.
   @retval Others            The operation fails.
@@ -914,47 +917,314 @@ EmmcSwitchToHS400 (
   IN EFI_SD_MMC_PASS_THRU_PROTOCOL      *PassThru,
   IN UINT8                              Slot,
   IN UINT16                             Rca,
-  IN UINT32                             ClockFreq
+  IN SD_MMC_BUS_SETTINGS                *BusMode
   )
 {
   EFI_STATUS                 Status;
-  UINT8                      HsTiming;
-  SD_MMC_BUS_MODE            Timing;
   SD_MMC_HC_PRIVATE_DATA     *Private;
+  SD_MMC_BUS_SETTINGS        Hs200BusMode;
+  UINT32                     HsFreq;
+
+  if (BusMode->BusTiming != SdMmcMmcHs400 ||
+      BusMode->BusWidth != 8) {
+    return EFI_INVALID_PARAMETER;
+  }
 
   Private = SD_MMC_HC_PRIVATE_FROM_THIS (PassThru);
+  Hs200BusMode.BusTiming = SdMmcMmcHs200;
+  Hs200BusMode.BusWidth = BusMode->BusWidth;
+  Hs200BusMode.ClockFreq = BusMode->ClockFreq;
+  Hs200BusMode.DriverStrength = BusMode->DriverStrength;
 
-  Status = EmmcSwitchToHS200 (PciIo, PassThru, Slot, Rca, ClockFreq, 8);
+  Status = EmmcSwitchToHS200 (PciIo, PassThru, Slot, Rca, &Hs200BusMode);
   if (EFI_ERROR (Status)) {
     return Status;
   }
+
   //
-  // Set to Hight Speed timing and set the clock frequency to a value less than 52MHz.
+  // Set to High Speed timing and set the clock frequency to a value less than or equal to 52MHz.
+  // This step is necessary to be able to switch Bus into 8 bit DDR mode which is unsupported in HS200.
   //
-  HsTiming = 1;
-  Status = EmmcSwitchBusTiming (PciIo, PassThru, Slot, Rca, HsTiming, SdMmcMmcHsSdr, 52);
+  Status = SdMmcHcUhsSignaling (Private->ControllerHandle, PciIo, Slot, SdMmcMmcHsSdr);
   if (EFI_ERROR (Status)) {
     return Status;
   }
-  //
-  // HS400 mode must use 8 data lines.
-  //
-  Status = EmmcSwitchBusWidth (PciIo, PassThru, Slot, Rca, TRUE, 8);
+
+  HsFreq = BusMode->ClockFreq < 52 ? BusMode->ClockFreq : 52;
+  Status = EmmcSwitchBusTiming (PciIo, PassThru, Slot, Rca, BusMode->DriverStrength, SdMmcMmcHsSdr, HsFreq);
   if (EFI_ERROR (Status)) {
     return Status;
   }
 
-  Timing = SdMmcMmcHs400;
+  Status = EmmcSwitchBusWidth (PciIo, PassThru, Slot, Rca, TRUE, BusMode->BusWidth);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
 
-  Status = SdMmcHcUhsSignaling (Private->ControllerHandle, PciIo, Slot, Timing);
+  Status = SdMmcHcUhsSignaling (Private->ControllerHandle, PciIo, Slot, BusMode->BusTiming);
   if (EFI_ERROR (Status)) {
     return Status;
   }
 
-  HsTiming = 3;
-  Status = EmmcSwitchBusTiming (PciIo, PassThru, Slot, Rca, HsTiming, Timing, ClockFreq);
+  return EmmcSwitchBusTiming (PciIo, PassThru, Slot, Rca, BusMode->DriverStrength, BusMode->BusTiming, BusMode->ClockFreq);
+}
 
-  return Status;
+/**
+  Check if passed BusTiming is supported in both controller and card.
+
+  @param[in] Private    Pointer to controller private data
+  @param[in] SlotIndex  Index of the slot in the controller
+  @param[in] ExtCsd     Pointer to the card's extended CSD
+  @param[in] BusTiming  Bus timing to check
+
+  @retval TRUE  Both card and controller support given BusTiming
+  @retval FALSE Card or controller doesn't support given BusTiming
+**/
+BOOLEAN
+EmmcIsBusTimingSupported (
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,
+  IN UINT8                   SlotIndex,
+  IN EMMC_EXT_CSD            *ExtCsd,
+  IN SD_MMC_BUS_MODE         BusTiming
+  )
+{
+  BOOLEAN             Supported;
+  SD_MMC_HC_SLOT_CAP  *Capabilities;
+
+  Capabilities = &Private->Capability[SlotIndex];
+
+  Supported = FALSE;
+  switch (BusTiming) {
+    case SdMmcMmcHs400:
+      if ((((ExtCsd->DeviceType & (BIT6 | BIT7))  != 0) && (Capabilities->Hs400 != 0)) && Capabilities->BusWidth8) {
+        Supported = TRUE;
+      }
+      break;
+    case SdMmcMmcHs200:
+      if ((((ExtCsd->DeviceType & (BIT4 | BIT5))  != 0) && (Capabilities->Sdr104 != 0))) {
+        Supported = TRUE;
+      }
+      break;
+    case SdMmcMmcHsDdr:
+      if ((((ExtCsd->DeviceType & (BIT2 | BIT3))  != 0) && (Capabilities->Ddr50 != 0))) {
+        Supported = TRUE;
+      }
+      break;
+    case SdMmcMmcHsSdr:
+      if ((((ExtCsd->DeviceType & BIT1)  != 0) && (Capabilities->HighSpeed != 0))) {
+        Supported = TRUE;
+      }
+      break;
+    case SdMmcMmcLegacy:
+      if ((ExtCsd->DeviceType & BIT0) != 0) {
+        Supported = TRUE;
+      }
+      break;
+    default:
+      ASSERT (FALSE);
+  }
+
+  return Supported;
+}
+
+/**
+  Get the target bus timing to set on the link. This function
+  will try to select highest bus timing supported by card, controller
+  and the driver.
+
+  @param[in] Private    Pointer to controller private data
+  @param[in] SlotIndex  Index of the slot in the controller
+  @param[in] ExtCsd     Pointer to the card's extended CSD
+
+  @return  Bus timing value that should be set on link
+**/
+SD_MMC_BUS_MODE
+EmmcGetTargetBusTiming (
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,
+  IN UINT8                    SlotIndex,
+  IN EMMC_EXT_CSD             *ExtCsd
+  )
+{
+  SD_MMC_BUS_MODE  BusTiming;
+
+  //
+  // We start with highest bus timing that this driver currently supports and
+  // return as soon as we find supported timing.
+  //
+  BusTiming = SdMmcMmcHs400;
+  while (BusTiming > SdMmcMmcLegacy) {
+    if (EmmcIsBusTimingSupported (Private, SlotIndex, ExtCsd, BusTiming)) {
+      break;
+    }
+    BusTiming--;
+  }
+
+  return BusTiming;
+}
+
+/**
+  Check if the passed bus width is supported by controller and card.
+
+  @param[in] Private    Pointer to controller private data
+  @param[in] SlotIndex  Index of the slot in the controller
+  @param[in] BusTiming  Bus timing set on the link
+  @param[in] BusWidth   Bus width to check
+
+  @retval TRUE   Passed bus width is supported in current bus configuration
+  @retval FALSE  Passed bus width is not supported in current bus configuration
+**/
+BOOLEAN
+EmmcIsBusWidthSupported (
+  IN SD_MMC_HC_PRIVATE_DATA   *Private,
+  IN UINT8                    SlotIndex,
+  IN SD_MMC_BUS_MODE          BusTiming,
+  IN UINT16                   BusWidth
+  )
+{
+  if (BusWidth == 8 && Private->Capability[SlotIndex].BusWidth8) {
+    return TRUE;
+  } else if (BusWidth == 4 && BusTiming != SdMmcMmcHs400) {
+    return TRUE;
+  } else if (BusWidth == 1 && (BusTiming == SdMmcMmcHsSdr || BusTiming == SdMmcMmcLegacy)) {
+    return TRUE;
+  }
+
+  return FALSE;
+}
+
+/**
+  Get the target bus width to be set on the bus.
+
+  @param[in] Private    Pointer to controller private data
+  @param[in] SlotIndex  Index of the slot in the controller
+  @param[in] ExtCsd     Pointer to card's extended CSD
+  @param[in] BusTiming  Bus timing set on the bus
+
+  @return Bus width to be set on the bus
+**/
+UINT8
+EmmcGetTargetBusWidth (
+  IN SD_MMC_HC_PRIVATE_DATA   *Private,
+  IN UINT8                    SlotIndex,
+  IN EMMC_EXT_CSD             *ExtCsd,
+  IN SD_MMC_BUS_MODE          BusTiming
+  )
+{
+  UINT8  BusWidth;
+  UINT8  PreferredBusWidth;
+
+  PreferredBusWidth = Private->Slot[SlotIndex].OperatingParameters.BusWidth;
+
+  if (PreferredBusWidth != EDKII_SD_MMC_BUS_WIDTH_IGNORE &&
+      EmmcIsBusWidthSupported (Private, SlotIndex, BusTiming, PreferredBusWidth)) {
+    BusWidth = PreferredBusWidth;
+  } else if (EmmcIsBusWidthSupported (Private, SlotIndex, BusTiming, 8)) {
+    BusWidth = 8;
+  } else if (EmmcIsBusWidthSupported (Private, SlotIndex, BusTiming, 4)) {
+    BusWidth = 4;
+  } else if (EmmcIsBusWidthSupported (Private, SlotIndex, BusTiming, 1)) {
+    BusWidth = 1;
+  }
+
+  return BusWidth;
+}
+
+/**
+  Get the target clock frequency to be set on the bus.
+
+  @param[in] Private    Pointer to controller private data
+  @param[in] SlotIndex  Index of the slot in the controller
+  @param[in] ExtCsd     Pointer to card's extended CSD
+  @param[in] BusTiming  Bus timing to be set on the bus
+
+  @return Value of the clock frequency to be set on bus in MHz
+**/
+UINT32
+EmmcGetTargetClockFreq (
+  IN SD_MMC_HC_PRIVATE_DATA   *Private,
+  IN UINT8                    SlotIndex,
+  IN EMMC_EXT_CSD             *ExtCsd,
+  IN SD_MMC_BUS_MODE          BusTiming
+  )
+{
+  UINT32 PreferredClockFreq;
+  UINT32 MaxClockFreq;
+
+  PreferredClockFreq = Private->Slot[SlotIndex].OperatingParameters.ClockFreq;
+
+  switch (BusTiming) {
+    case SdMmcMmcHs400:
+    case SdMmcMmcHs200:
+      MaxClockFreq = 200;
+      break;
+    case SdMmcMmcHsSdr:
+    case SdMmcMmcHsDdr:
+      MaxClockFreq = 52;
+      break;
+    default:
+      MaxClockFreq = 26;
+      break;
+  }
+
+  if (PreferredClockFreq != EDKII_SD_MMC_CLOCK_FREQ_IGNORE && PreferredClockFreq < MaxClockFreq) {
+    return PreferredClockFreq;
+  } else {
+    return MaxClockFreq;
+  }
+}
+
+/**
+  Get the driver strength to be set on bus.
+
+  @param[in] Private    Pointer to controller private data
+  @param[in] SlotIndex  Index of the slot in the controller
+  @param[in] ExtCsd     Pointer to card's extended CSD
+  @param[in] BusTiming  Bus timing set on the bus
+
+  @return Value of the driver strength to be set on the bus
+**/
+EDKII_SD_MMC_DRIVER_STRENGTH
+EmmcGetTargetDriverStrength (
+  IN SD_MMC_HC_PRIVATE_DATA   *Private,
+  IN UINT8                    SlotIndex,
+  IN EMMC_EXT_CSD             *ExtCsd,
+  IN SD_MMC_BUS_MODE          BusTiming
+  )
+{
+  EDKII_SD_MMC_DRIVER_STRENGTH  PreferredDriverStrength;
+  EDKII_SD_MMC_DRIVER_STRENGTH  DriverStrength;
+
+  PreferredDriverStrength = Private->Slot[SlotIndex].OperatingParameters.DriverStrength;
+  DriverStrength.Emmc = EmmcDriverStrengthType0;
+
+  if (PreferredDriverStrength.Emmc != EDKII_SD_MMC_DRIVER_STRENGTH_IGNORE &&
+      (ExtCsd->DriverStrength & (BIT0 << PreferredDriverStrength.Emmc))) {
+    DriverStrength.Emmc = PreferredDriverStrength.Emmc;
+  }
+
+  return DriverStrength;
+}
+
+/**
+  Get the target settings for the bus mode.
+
+  @param[in]  Private    Pointer to controller private data
+  @param[in]  SlotIndex  Index of the slot in the controller
+  @param[in]  ExtCsd     Pointer to card's extended CSD
+  @param[out] BusMode    Target configuration of the bus
+**/
+VOID
+EmmcGetTargetBusMode (
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,
+  IN UINT8                   SlotIndex,
+  IN EMMC_EXT_CSD            *ExtCsd,
+  OUT SD_MMC_BUS_SETTINGS    *BusMode
+  )
+{
+  BusMode->BusTiming = EmmcGetTargetBusTiming (Private, SlotIndex, ExtCsd);
+  BusMode->BusWidth = EmmcGetTargetBusWidth (Private, SlotIndex, ExtCsd, BusMode->BusTiming);
+  BusMode->ClockFreq = EmmcGetTargetClockFreq (Private, SlotIndex, ExtCsd, BusMode->BusTiming);
+  BusMode->DriverStrength = EmmcGetTargetDriverStrength (Private, SlotIndex, ExtCsd, BusMode->BusTiming);
 }
 
 /**
@@ -983,10 +1253,7 @@ EmmcSetBusMode (
   EFI_STATUS                    Status;
   EMMC_CSD                      Csd;
   EMMC_EXT_CSD                  ExtCsd;
-  UINT8                         HsTiming;
-  BOOLEAN                       IsDdr;
-  UINT32                        ClockFreq;
-  UINT8                         BusWidth;
+  SD_MMC_BUS_SETTINGS           BusMode;
   SD_MMC_HC_PRIVATE_DATA        *Private;
 
   Private = SD_MMC_HC_PRIVATE_FROM_THIS (PassThru);
@@ -1004,85 +1271,30 @@ EmmcSetBusMode (
   }
 
   ASSERT (Private->BaseClkFreq[Slot] != 0);
+
   //
-  // Check if the Host Controller support 8bits bus width.
-  //
-  if (Private->Capability[Slot].BusWidth8 != 0) {
-    BusWidth = 8;
-  } else {
-    BusWidth = 4;
-  }
-  //
-  // Get Deivce_Type from EXT_CSD register.
+  // Get Device_Type from EXT_CSD register.
   //
   Status = EmmcGetExtCsd (PassThru, Slot, &ExtCsd);
   if (EFI_ERROR (Status)) {
     DEBUG ((DEBUG_ERROR, "EmmcSetBusMode: GetExtCsd fails with %r\n", Status));
     return Status;
   }
-  //
-  // Calculate supported bus speed/bus width/clock frequency.
-  //
-  HsTiming  = 0;
-  IsDdr     = FALSE;
-  ClockFreq = 0;
-  if (((ExtCsd.DeviceType & (BIT4 | BIT5))  != 0) && (Private->Capability[Slot].Sdr104 != 0)) {
-    HsTiming  = 2;
-    IsDdr     = FALSE;
-    ClockFreq = 200;
-  } else if (((ExtCsd.DeviceType & (BIT2 | BIT3))  != 0) && (Private->Capability[Slot].Ddr50 != 0)) {
-    HsTiming  = 1;
-    IsDdr     = TRUE;
-    ClockFreq = 52;
-  } else if (((ExtCsd.DeviceType & BIT1)  != 0) && (Private->Capability[Slot].HighSpeed != 0)) {
-    HsTiming  = 1;
-    IsDdr     = FALSE;
-    ClockFreq = 52;
-  } else if (((ExtCsd.DeviceType & BIT0)  != 0) && (Private->Capability[Slot].HighSpeed != 0)) {
-    HsTiming  = 1;
-    IsDdr     = FALSE;
-    ClockFreq = 26;
-  }
-  //
-  // Check if both of the device and the host controller support HS400 DDR mode.
-  //
-  if (((ExtCsd.DeviceType & (BIT6 | BIT7))  != 0) && (Private->Capability[Slot].Hs400 != 0)) {
-    //
-    // The host controller supports 8bits bus.
-    //
-    ASSERT (BusWidth == 8);
-    HsTiming  = 3;
-    IsDdr     = TRUE;
-    ClockFreq = 200;
-  }
 
-  if ((ClockFreq == 0) || (HsTiming == 0)) {
-    //
-    // Continue using default setting.
-    //
-    return EFI_SUCCESS;
-  }
+  EmmcGetTargetBusMode (Private, Slot, &ExtCsd, &BusMode);
 
-  DEBUG ((DEBUG_INFO, "EmmcSetBusMode: HsTiming %d ClockFreq %d BusWidth %d Ddr %a\n", HsTiming, ClockFreq, BusWidth, IsDdr ? "TRUE":"FALSE"));
+  DEBUG ((DEBUG_INFO, "EmmcSetBusMode: Target bus mode: timing = %d, width = %d, clock freq = %d, driver strength = %d\n",
+                          BusMode.BusTiming, BusMode.BusWidth, BusMode.ClockFreq, BusMode.DriverStrength.Emmc));
 
-  if (HsTiming == 3) {
-    //
-    // Execute HS400 timing switch procedure
-    //
-    Status = EmmcSwitchToHS400 (PciIo, PassThru, Slot, Rca, ClockFreq);
-  } else if (HsTiming == 2) {
-    //
-    // Execute HS200 timing switch procedure
-    //
-    Status = EmmcSwitchToHS200 (PciIo, PassThru, Slot, Rca, ClockFreq, BusWidth);
+  if (BusMode.BusTiming == SdMmcMmcHs400) {
+    Status = EmmcSwitchToHS400 (PciIo, PassThru, Slot, Rca, &BusMode);
+  } else if (BusMode.BusTiming == SdMmcMmcHs200) {
+    Status = EmmcSwitchToHS200 (PciIo, PassThru, Slot, Rca, &BusMode);
   } else {
-    //
-    // Execute High Speed timing switch procedure
-    //
-    Status = EmmcSwitchToHighSpeed (PciIo, PassThru, Slot, Rca, ClockFreq, IsDdr, BusWidth);
+    Status = EmmcSwitchToHighSpeed (PciIo, PassThru, Slot, Rca, &BusMode);
   }
 
-  DEBUG ((DEBUG_INFO, "EmmcSetBusMode: Switch to %a %r\n", (HsTiming == 3) ? "HS400" : ((HsTiming == 2) ? "HS200" : "HighSpeed"), Status));
+  DEBUG ((DEBUG_INFO, "EmmcSetBusMode: Switch to %a %r\n", (BusMode.BusTiming == SdMmcMmcHs400) ? "HS400" : ((BusMode.BusTiming == SdMmcMmcHs200) ? "HS200" : "HighSpeed"), Status));
 
   return Status;
 }
diff --git a/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdDevice.c b/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdDevice.c
index 17936a5492..27a13e1008 100644
--- a/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdDevice.c
+++ b/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdDevice.c
@@ -313,10 +313,6 @@ SdCardSetRca (
   return Status;
 }
 
-
-
-
-
 /**
   Send command SELECT_DESELECT_CARD to the SD device to select/deselect it.
 
@@ -472,14 +468,14 @@ SdCardSetBusWidth (
 
   Refer to SD Physical Layer Simplified Spec 4.1 Section 4.7 for details.
 
-  @param[in]  PassThru      A pointer to the EFI_SD_MMC_PASS_THRU_PROTOCOL instance.
-  @param[in]  Slot          The slot number of the SD card to send the command to.
-  @param[in]  AccessMode    The value for access mode group.
-  @param[in]  CommandSystem The value for command set group.
-  @param[in]  DriveStrength The value for drive length group.
-  @param[in]  PowerLimit    The value for power limit group.
-  @param[in]  Mode          Switch or check function.
-  @param[out] SwitchResp    The return switch function status.
+  @param[in]  PassThru       A pointer to the EFI_SD_MMC_PASS_THRU_PROTOCOL instance.
+  @param[in]  Slot           The slot number of the SD card to send the command to.
+  @param[in]  BusTiming      Target bus timing based on which access group value will be set.
+  @param[in]  CommandSystem  The value for command set group.
+  @param[in]  DriverStrength The value for driver strength group.
+  @param[in]  PowerLimit     The value for power limit group.
+  @param[in]  Mode           Switch or check function.
+  @param[out] SwitchResp     The return switch function status.
 
   @retval EFI_SUCCESS       The operation is done correctly.
   @retval Others            The operation fails.
@@ -489,9 +485,9 @@ EFI_STATUS
 SdCardSwitch (
   IN     EFI_SD_MMC_PASS_THRU_PROTOCOL  *PassThru,
   IN     UINT8                          Slot,
-  IN     UINT8                          AccessMode,
+  IN     SD_MMC_BUS_MODE                BusTiming,
   IN     UINT8                          CommandSystem,
-  IN     UINT8                          DriveStrength,
+  IN     SD_DRIVER_STRENGTH_TYPE        DriverStrength,
   IN     UINT8                          PowerLimit,
   IN     BOOLEAN                        Mode,
      OUT UINT8                          *SwitchResp
@@ -502,6 +498,7 @@ SdCardSwitch (
   EFI_SD_MMC_PASS_THRU_COMMAND_PACKET   Packet;
   EFI_STATUS                            Status;
   UINT32                                ModeValue;
+  UINT8                                 AccessMode;
 
   ZeroMem (&SdMmcCmdBlk, sizeof (SdMmcCmdBlk));
   ZeroMem (&SdMmcStatusBlk, sizeof (SdMmcStatusBlk));
@@ -516,14 +513,48 @@ SdCardSwitch (
   SdMmcCmdBlk.ResponseType = SdMmcResponseTypeR1;
 
   ModeValue = Mode ? BIT31 : 0;
-  SdMmcCmdBlk.CommandArgument = (AccessMode & 0xF) | ((PowerLimit & 0xF) << 4) | \
-                                ((DriveStrength & 0xF) << 8) | ((DriveStrength & 0xF) << 12) | \
+
+  switch (BusTiming) {
+    case SdMmcUhsDdr50:
+      AccessMode = 0x4;
+      break;
+    case SdMmcUhsSdr104:
+      AccessMode = 0x3;
+      break;
+    case SdMmcUhsSdr50:
+      AccessMode = 0x2;
+      break;
+    case SdMmcUhsSdr25:
+    case SdMmcSdHs:
+      AccessMode = 0x1;
+      break;
+    case SdMmcUhsSdr12:
+    case SdMmcSdDs:
+      AccessMode = 0;
+      break;
+    default:
+      AccessMode = 0xF;
+  }
+
+  SdMmcCmdBlk.CommandArgument = (AccessMode & 0xF) | ((CommandSystem & 0xF) << 4) | \
+                                ((DriverStrength & 0xF) << 8) | ((PowerLimit & 0xF) << 12) | \
                                 ModeValue;
 
   Packet.InDataBuffer     = SwitchResp;
   Packet.InTransferLength = 64;
 
   Status = SdMmcPassThruPassThru (PassThru, Slot, &Packet, NULL);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (!Mode) {
+    if ((((AccessMode & 0xF) != 0xF) && (SwitchResp[16] == 0xF)) ||
+        (((CommandSystem & 0xF) != 0xF) && (SwitchResp[15] == 0xF)) ||
+        (((DriverStrength & 0xF) != 0xF) && (SwitchResp[14] == 0xF)) ||
+        (((PowerLimit & 0xF) != 0xF) && (SwitchResp[13] == 0xF)))
+      return EFI_DEVICE_ERROR;
+  }
 
   return Status;
 }
@@ -748,6 +779,273 @@ SdCardSwitchBusWidth (
   return Status;
 }
 
+/**
+  Check if passed BusTiming is supported in both controller and card.
+
+  @param[in] Private                  Pointer to controller private data
+  @param[in] SlotIndex                Index of the slot in the controller
+  @param[in] CardSupportedBusTimings  Bitmask indicating which bus timings are supported by card
+  @param[in] IsInUhsI                 Flag indicating if link is in UHS-I
+
+  @retval TRUE  Both card and controller support given BusTiming
+  @retval FALSE Card or controller doesn't support given BusTiming
+**/
+BOOLEAN
+SdIsBusTimingSupported (
+  IN SD_MMC_HC_PRIVATE_DATA   *Private,
+  IN UINT8                    SlotIndex,
+  IN UINT8                    CardSupportedBusTimings,
+  IN BOOLEAN                  IsInUhsI,
+  IN SD_MMC_BUS_MODE          BusTiming
+  )
+{
+  SD_MMC_HC_SLOT_CAP           *Capability;
+
+  Capability = &Private->Capability[SlotIndex];
+
+  if (IsInUhsI) {
+    switch (BusTiming) {
+      case SdMmcUhsSdr104:
+        if (Capability->Sdr104 && ((CardSupportedBusTimings & BIT3) != 0)) {
+          return TRUE;
+        }
+        break;
+      case SdMmcUhsDdr50:
+        if (Capability->Ddr50 && ((CardSupportedBusTimings & BIT4) != 0)) {
+          return TRUE;
+        }
+        break;
+      case SdMmcUhsSdr50:
+        if (Capability->Sdr50 && ((CardSupportedBusTimings & BIT2) != 0)) {
+          return TRUE;
+        }
+        break;
+      case SdMmcUhsSdr25:
+        if ((CardSupportedBusTimings & BIT1) != 0) {
+          return TRUE;
+        }
+        break;
+      case SdMmcUhsSdr12:
+        if ((CardSupportedBusTimings & BIT0) != 0) {
+          return TRUE;
+        }
+        break;
+      default:
+        break;
+    }
+  } else {
+    switch (BusTiming) {
+      case SdMmcSdHs:
+        if (Capability->HighSpeed && (CardSupportedBusTimings & BIT1) != 0) {
+          return TRUE;
+        }
+        break;
+      case SdMmcSdDs:
+        if ((CardSupportedBusTimings & BIT0) != 0) {
+          return TRUE;
+        }
+        break;
+      default:
+        break;
+    }
+  }
+
+  return FALSE;
+}
+
+/**
+  Get the target bus timing to set on the link. This function
+  will try to select highest bus timing supported by card, controller
+  and the driver.
+
+  @param[in] Private                  Pointer to controller private data
+  @param[in] SlotIndex                Index of the slot in the controller
+  @param[in] CardSupportedBusTimings  Bitmask indicating which bus timings are supported by card
+  @param[in] IsInUhsI                 Flag indicating if link is in UHS-I
+
+  @return  Bus timing value that should be set on link
+**/
+SD_MMC_BUS_MODE
+SdGetTargetBusTiming (
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,
+  IN UINT8                   SlotIndex,
+  IN UINT8                   CardSupportedBusTimings,
+  IN BOOLEAN                 IsInUhsI
+  )
+{
+  SD_MMC_BUS_MODE  BusTiming;
+
+  if (IsInUhsI) {
+    BusTiming = SdMmcUhsSdr104;
+  } else {
+    BusTiming = SdMmcSdHs;
+  }
+
+  while (BusTiming > SdMmcSdDs) {
+    if (SdIsBusTimingSupported (Private, SlotIndex, CardSupportedBusTimings, IsInUhsI, BusTiming)) {
+      break;
+    }
+    BusTiming--;
+  }
+
+  return BusTiming;
+}
+
+/**
+  Get the target bus width to be set on the bus.
+
+  @param[in] Private    Pointer to controller private data
+  @param[in] SlotIndex  Index of the slot in the controller
+  @param[in] BusTiming  Bus timing set on the bus
+
+  @return Bus width to be set on the bus
+**/
+UINT8
+SdGetTargetBusWidth (
+  IN SD_MMC_HC_PRIVATE_DATA   *Private,
+  IN UINT8                    SlotIndex,
+  IN SD_MMC_BUS_MODE          BusTiming
+  )
+{
+  UINT8  BusWidth;
+  UINT8  PreferredBusWidth;
+
+  PreferredBusWidth = Private->Slot[SlotIndex].OperatingParameters.BusWidth;
+
+  if (BusTiming == SdMmcSdDs || BusTiming == SdMmcSdHs) {
+    if (PreferredBusWidth != EDKII_SD_MMC_BUS_WIDTH_IGNORE &&
+        (PreferredBusWidth == 1 || PreferredBusWidth == 4)) {
+      BusWidth = PreferredBusWidth;
+    } else {
+      BusWidth = 4;
+    }
+  } else {
+    //
+    // UHS-I modes support only 4-bit width.
+    // Switch to 4-bit has been done before calling this function anyway so
+    // this is purely informational.
+    //
+    BusWidth = 4;
+  }
+
+  return BusWidth;
+}
+
+/**
+  Get the target clock frequency to be set on the bus.
+
+  @param[in] Private    Pointer to controller private data
+  @param[in] SlotIndex  Index of the slot in the controller
+  @param[in] BusTiming  Bus timing to be set on the bus
+
+  @return Value of the clock frequency to be set on bus in MHz
+**/
+UINT32
+SdGetTargetBusClockFreq (
+  IN SD_MMC_HC_PRIVATE_DATA   *Private,
+  IN UINT8                    SlotIndex,
+  IN SD_MMC_BUS_MODE          BusTiming
+  )
+{
+  UINT32 PreferredClockFreq;
+  UINT32 MaxClockFreq;
+
+  PreferredClockFreq = Private->Slot[SlotIndex].OperatingParameters.ClockFreq;
+
+  switch (BusTiming) {
+    case SdMmcUhsSdr104:
+      MaxClockFreq = 208;
+      break;
+    case SdMmcUhsSdr50:
+      MaxClockFreq = 100;
+      break;
+    case SdMmcUhsDdr50:
+    case SdMmcUhsSdr25:
+    case SdMmcSdHs:
+      MaxClockFreq = 50;
+      break;
+    case SdMmcUhsSdr12:
+    case SdMmcSdDs:
+    default:
+      MaxClockFreq = 25;
+  }
+
+  if (PreferredClockFreq != EDKII_SD_MMC_CLOCK_FREQ_IGNORE && PreferredClockFreq < MaxClockFreq) {
+    return PreferredClockFreq;
+  } else {
+    return MaxClockFreq;
+  }
+}
+
+/**
+  Get the driver strength to be set on bus.
+
+  @param[in] Private                       Pointer to controller private data
+  @param[in] SlotIndex                     Index of the slot in the controller
+  @param[in] CardSupportedDriverStrengths  Bitmask indicating which driver strengths are supported on the card
+  @param[in] BusTiming                     Bus timing set on the bus
+
+  @return Value of the driver strength to be set on the bus
+**/
+EDKII_SD_MMC_DRIVER_STRENGTH
+SdGetTargetDriverStrength (
+  IN SD_MMC_HC_PRIVATE_DATA   *Private,
+  IN UINT8                    SlotIndex,
+  IN UINT8                    CardSupportedDriverStrengths,
+  IN SD_MMC_BUS_MODE          BusTiming
+  )
+{
+  EDKII_SD_MMC_DRIVER_STRENGTH  PreferredDriverStrength;
+  EDKII_SD_MMC_DRIVER_STRENGTH  DriverStrength;
+
+  if (BusTiming == SdMmcSdDs || BusTiming == SdMmcSdHs) {
+    DriverStrength.Sd = SdDriverStrengthIgnore;
+    return DriverStrength;
+  }
+
+  PreferredDriverStrength = Private->Slot[SlotIndex].OperatingParameters.DriverStrength;
+  DriverStrength.Sd = SdDriverStrengthTypeB;
+
+  if (PreferredDriverStrength.Sd != EDKII_SD_MMC_DRIVER_STRENGTH_IGNORE &&
+      (CardSupportedDriverStrengths & (BIT0 << PreferredDriverStrength.Sd))) {
+
+    if ((PreferredDriverStrength.Sd == SdDriverStrengthTypeA &&
+        Private->Capability[SlotIndex].DriverTypeA) ||
+        (PreferredDriverStrength.Sd == SdDriverStrengthTypeC &&
+        Private->Capability[SlotIndex].DriverTypeC) ||
+        (PreferredDriverStrength.Sd == SdDriverStrengthTypeD &&
+        Private->Capability[SlotIndex].DriverTypeD)) {
+      DriverStrength.Sd = PreferredDriverStrength.Sd;
+    }
+  }
+
+  return DriverStrength;
+}
+
+/**
+  Get the target settings for the bus mode.
+
+  @param[in]  Private          Pointer to controller private data
+  @param[in]  SlotIndex        Index of the slot in the controller
+  @param[in]  SwitchQueryResp  Pointer to switch query response
+  @param[in]  IsInUhsI         Flag indicating if link is in UHS-I mode
+  @param[out] BusMode          Target configuration of the bus
+**/
+VOID
+SdGetTargetBusMode (
+  IN SD_MMC_HC_PRIVATE_DATA  *Private,
+  IN UINT8                   SlotIndex,
+  IN UINT8                   *SwitchQueryResp,
+  IN BOOLEAN                 IsInUhsI,
+  OUT SD_MMC_BUS_SETTINGS    *BusMode
+  )
+{
+  BusMode->BusTiming = SdGetTargetBusTiming (Private, SlotIndex, SwitchQueryResp[13], IsInUhsI);
+  BusMode->BusWidth = SdGetTargetBusWidth (Private, SlotIndex, BusMode->BusTiming);
+  BusMode->ClockFreq = SdGetTargetBusClockFreq (Private, SlotIndex, BusMode->BusTiming);
+  BusMode->DriverStrength = SdGetTargetDriverStrength (Private, SlotIndex, SwitchQueryResp[9], BusMode->BusTiming);
+}
+
 /**
   Switch the high speed timing according to request.
 
@@ -775,13 +1073,10 @@ SdCardSetBusMode (
 {
   EFI_STATUS                   Status;
   SD_MMC_HC_SLOT_CAP           *Capability;
-  UINT32                       ClockFreq;
-  UINT8                        BusWidth;
-  UINT8                        AccessMode;
   UINT8                        HostCtrl1;
   UINT8                        SwitchResp[64];
-  SD_MMC_BUS_MODE              Timing;
   SD_MMC_HC_PRIVATE_DATA       *Private;
+  SD_MMC_BUS_SETTINGS          BusMode;
 
   Private = SD_MMC_HC_PRIVATE_FROM_THIS (PassThru);
 
@@ -792,61 +1087,51 @@ SdCardSetBusMode (
     return Status;
   }
 
-  BusWidth = 4;
-
-  Status = SdCardSwitchBusWidth (PciIo, PassThru, Slot, Rca, BusWidth);
-  if (EFI_ERROR (Status)) {
-    return Status;
+  if (S18A) {
+    //
+    // For UHS-I speed modes 4-bit data bus is requiered so we
+    // switch here irrespective of platform preference.
+    //
+    Status = SdCardSwitchBusWidth (PciIo, PassThru, Slot, Rca, 4);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
   }
+
   //
   // Get the supported bus speed from SWITCH cmd return data group #1.
   //
-  Status = SdCardSwitch (PassThru, Slot, 0xF, 0xF, 0xF, 0xF, FALSE, SwitchResp);
+  Status = SdCardSwitch (PassThru, Slot, 0xFF, 0xF, SdDriverStrengthIgnore, 0xF, FALSE, SwitchResp);
   if (EFI_ERROR (Status)) {
     return Status;
   }
-  //
-  // Calculate supported bus speed/bus width/clock frequency by host and device capability.
-  //
-  ClockFreq = 0;
-  if (S18A && (Capability->Sdr104 != 0) && ((SwitchResp[13] & BIT3) != 0)) {
-    ClockFreq = 208;
-    AccessMode = 3;
-    Timing = SdMmcUhsSdr104;
-  } else if (S18A && (Capability->Sdr50 != 0) && ((SwitchResp[13] & BIT2) != 0)) {
-    ClockFreq = 100;
-    AccessMode = 2;
-    Timing = SdMmcUhsSdr50;
-  } else if (S18A && (Capability->Ddr50 != 0) && ((SwitchResp[13] & BIT4) != 0)) {
-    ClockFreq = 50;
-    AccessMode = 4;
-    Timing = SdMmcUhsDdr50;
-  } else if ((SwitchResp[13] & BIT1) != 0) {
-    ClockFreq = 50;
-    AccessMode = 1;
-    Timing = SdMmcUhsSdr25;
-  } else {
-    ClockFreq = 25;
-    AccessMode = 0;
-    Timing = SdMmcUhsSdr12;
+
+  SdGetTargetBusMode (Private, Slot, SwitchResp, S18A, &BusMode);
+
+  DEBUG ((DEBUG_INFO, "SdCardSetBusMode: Target bus mode: bus timing = %d, bus width = %d, clock freq[MHz] = %d, driver strength = %d\n",
+                         BusMode.BusTiming, BusMode.BusWidth, BusMode.ClockFreq, BusMode.DriverStrength.Sd));
+
+  if (!S18A) {
+    Status = SdCardSwitchBusWidth (PciIo, PassThru, Slot, Rca, BusMode.BusWidth);
+    if (EFI_ERROR (Status)) {
+      return Status;
+    }
   }
 
-  Status = SdCardSwitch (PassThru, Slot, AccessMode, 0xF, 0xF, 0xF, TRUE, SwitchResp);
+  Status = SdCardSwitch (PassThru, Slot, BusMode.BusTiming, 0xF, BusMode.DriverStrength.Sd, 0xF, TRUE, SwitchResp);
   if (EFI_ERROR (Status)) {
     return Status;
   }
 
-  if ((SwitchResp[16] & 0xF) != AccessMode) {
-    DEBUG ((DEBUG_ERROR, "SdCardSetBusMode: Switch to AccessMode %d ClockFreq %d BusWidth %d fails! The Switch response is 0x%1x\n", AccessMode, ClockFreq, BusWidth, SwitchResp[16] & 0xF));
-    return EFI_DEVICE_ERROR;
+  Status = SdMmcSetDriverStrength (Private->PciIo, Slot, BusMode.DriverStrength.Sd);
+  if (EFI_ERROR (Status)) {
+    return Status;
   }
 
-  DEBUG ((DEBUG_INFO, "SdCardSetBusMode: Switch to AccessMode %d ClockFreq %d BusWidth %d\n", AccessMode, ClockFreq, BusWidth));
-
   //
-  // Set to Hight Speed timing
+  // Set to High Speed timing
   //
-  if (AccessMode == 1) {
+  if (BusMode.BusTiming == SdMmcSdHs) {
     HostCtrl1 = BIT2;
     Status = SdMmcHcOrMmio (PciIo, Slot, SD_MMC_HC_HOST_CTRL1, sizeof (HostCtrl1), &HostCtrl1);
     if (EFI_ERROR (Status)) {
@@ -854,12 +1139,12 @@ SdCardSetBusMode (
     }
   }
 
-  Status = SdMmcHcUhsSignaling (Private->ControllerHandle, PciIo, Slot, Timing);
+  Status = SdMmcHcUhsSignaling (Private->ControllerHandle, PciIo, Slot, BusMode.BusTiming);
   if (EFI_ERROR (Status)) {
     return Status;
   }
 
-  Status = SdMmcHcClockSupply (PciIo, Slot, ClockFreq * 1000, Private->BaseClkFreq[Slot], Private->ControllerVersion[Slot]);
+  Status = SdMmcHcClockSupply (PciIo, Slot, BusMode.ClockFreq * 1000, Private->BaseClkFreq[Slot], Private->ControllerVersion[Slot]);
   if (EFI_ERROR (Status)) {
     return Status;
   }
@@ -869,7 +1154,7 @@ SdCardSetBusMode (
                           Private->ControllerHandle,
                           Slot,
                           EdkiiSdMmcSwitchClockFreqPost,
-                          &Timing
+                          &BusMode.BusTiming
                           );
     if (EFI_ERROR (Status)) {
       DEBUG ((
@@ -882,7 +1167,7 @@ SdCardSetBusMode (
     }
   }
 
-  if ((AccessMode == 3) || ((AccessMode == 2) && (Capability->TuningSDR50 != 0))) {
+  if ((BusMode.BusTiming == SdMmcUhsSdr104) || ((BusMode.BusTiming == SdMmcUhsSdr50) && (Capability->TuningSDR50 != 0))) {
     Status = SdCardTuningClock (PciIo, PassThru, Slot);
     if (EFI_ERROR (Status)) {
       return Status;
diff --git a/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHcDxe.c b/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHcDxe.c
index 4881ee44cc..373f1bed45 100644
--- a/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHcDxe.c
+++ b/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHcDxe.c
@@ -28,6 +28,11 @@ EFI_DRIVER_BINDING_PROTOCOL gSdMmcPciHcDriverBinding = {
   NULL
 };
 
+#define SLOT_INIT_TEMPLATE {0, UnknownSlot, 0, 0, 0, \
+                               {EDKII_SD_MMC_BUS_WIDTH_IGNORE,\
+                               EDKII_SD_MMC_CLOCK_FREQ_IGNORE,\
+                               {EDKII_SD_MMC_DRIVER_STRENGTH_IGNORE}}}
+
 //
 // Template for SD/MMC host controller private data.
 //
@@ -50,8 +55,12 @@ SD_MMC_HC_PRIVATE_DATA gSdMmcPciHcTemplate = {
                                     // Queue
   INITIALIZE_LIST_HEAD_VARIABLE (gSdMmcPciHcTemplate.Queue),
   {                                 // Slot
-    {0, UnknownSlot, 0, 0, 0}, {0, UnknownSlot, 0, 0, 0}, {0, UnknownSlot, 0, 0, 0},
-    {0, UnknownSlot, 0, 0, 0}, {0, UnknownSlot, 0, 0, 0}, {0, UnknownSlot, 0, 0, 0}
+    SLOT_INIT_TEMPLATE,
+    SLOT_INIT_TEMPLATE,
+    SLOT_INIT_TEMPLATE,
+    SLOT_INIT_TEMPLATE,
+    SLOT_INIT_TEMPLATE,
+    SLOT_INIT_TEMPLATE
   },
   {                                 // Capability
     {0},
@@ -328,6 +337,7 @@ SdMmcPciHcEnumerateDevice (
 
   return;
 }
+
 /**
   Tests to see if this driver supports a given controller. If a child device is provided,
   it further tests to see if this driver supports creating a handle for the specified child device.
@@ -619,7 +629,6 @@ SdMmcPciHcDriverBindingStart (
   Support64BitDma = TRUE;
   for (Slot = FirstBar; Slot < (FirstBar + SlotNum); Slot++) {
     Private->Slot[Slot].Enable = TRUE;
-
     //
     // Get SD/MMC Pci Host Controller Version
     //
@@ -635,19 +644,34 @@ SdMmcPciHcDriverBindingStart (
 
     Private->BaseClkFreq[Slot] = Private->Capability[Slot].BaseClkFreq;
 
-    if (mOverride != NULL && mOverride->Capability != NULL) {
-      Status = mOverride->Capability (
-                            Controller,
-                            Slot,
-                            &Private->Capability[Slot],
-                            &Private->BaseClkFreq[Slot]
-                            );
-      if (EFI_ERROR (Status)) {
-        DEBUG ((DEBUG_WARN, "%a: Failed to override capability - %r\n",
-          __FUNCTION__, Status));
-        continue;
+    if (mOverride != NULL) {
+      if (mOverride->Capability != NULL) {
+        Status = mOverride->Capability (
+                              Controller,
+                              Slot,
+                              &Private->Capability[Slot],
+                              &Private->BaseClkFreq[Slot]
+                              );
+        if (EFI_ERROR (Status)) {
+          DEBUG ((DEBUG_WARN, "%a: Failed to override capability - %r\n",
+            __FUNCTION__, Status));
+          continue;
+        }
+      }
+
+      if (mOverride->NotifyPhase != NULL) {
+        Status = mOverride->NotifyPhase (
+                              Controller,
+                              Slot,
+                              EdkiiSdMmcGetOperatingParam,
+                              (VOID*)&Private->Slot[Slot].OperatingParameters
+                              );
+        if (EFI_ERROR (Status)) {
+          DEBUG ((DEBUG_WARN, "%a: Failed to get operating parameters, using defaults\n", __FUNCTION__));
+        }
       }
     }
+
     DumpCapabilityReg (Slot, &Private->Capability[Slot]);
     DEBUG ((
       DEBUG_INFO,
diff --git a/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHcDxe.h b/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHcDxe.h
index 77bbf83b76..c29e48767e 100644
--- a/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHcDxe.h
+++ b/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHcDxe.h
@@ -78,11 +78,12 @@ typedef enum {
 } EFI_SD_MMC_SLOT_TYPE;
 
 typedef struct {
-  BOOLEAN                           Enable;
-  EFI_SD_MMC_SLOT_TYPE              SlotType;
-  BOOLEAN                           MediaPresent;
-  BOOLEAN                           Initialized;
-  SD_MMC_CARD_TYPE                  CardType;
+  BOOLEAN                            Enable;
+  EFI_SD_MMC_SLOT_TYPE               SlotType;
+  BOOLEAN                            MediaPresent;
+  BOOLEAN                            Initialized;
+  SD_MMC_CARD_TYPE                   CardType;
+  EDKII_SD_MMC_OPERATING_PARAMETERS  OperatingParameters;
 } SD_MMC_HC_SLOT;
 
 typedef struct {
@@ -120,6 +121,13 @@ typedef struct {
   UINT32                              BaseClkFreq[SD_MMC_HC_MAX_SLOT];
 } SD_MMC_HC_PRIVATE_DATA;
 
+typedef struct {
+  SD_MMC_BUS_MODE               BusTiming;
+  UINT8                         BusWidth;
+  UINT32                        ClockFreq;
+  EDKII_SD_MMC_DRIVER_STRENGTH  DriverStrength;
+} SD_MMC_BUS_SETTINGS;
+
 #define SD_MMC_HC_TRB_SIG             SIGNATURE_32 ('T', 'R', 'B', 'T')
 
 //
diff --git a/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHci.c b/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHci.c
index 5d1f977e55..75deb79ae4 100644
--- a/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHci.c
+++ b/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHci.c
@@ -1339,6 +1339,40 @@ SdMmcHcUhsSignaling (
   return EFI_SUCCESS;
 }
 
+/**
+  Set driver strength in host controller.
+
+  @param[in] Private         A pointer to the SD_MMC_HC_PRIVATE_DATA instance.
+  @param[in] SlotIndex       The slot index of the card.
+  @param[in] DriverStrength  DriverStrength to set in the controller.
+
+  @retval EFI_SUCCESS  Driver strength programmed successfully.
+  @retval Others       Failed to set driver strength.
+**/
+EFI_STATUS
+SdMmcSetDriverStrength (
+  IN EFI_PCI_IO_PROTOCOL      *PciIo,
+  IN UINT8                    SlotIndex,
+  IN SD_DRIVER_STRENGTH_TYPE  DriverStrength
+  )
+{
+  EFI_STATUS  Status;
+  UINT16      HostCtrl2;
+
+  HostCtrl2 = (UINT16)~SD_MMC_HC_CTRL_DRIVER_STRENGTH_MASK;
+  Status = SdMmcHcAndMmio (PciIo, SlotIndex, SD_MMC_HC_HOST_CTRL2, sizeof (HostCtrl2), &HostCtrl2);
+  if (EFI_ERROR (Status)) {
+    return Status;
+  }
+
+  if (DriverStrength == SdDriverStrengthIgnore) {
+    return EFI_SUCCESS;
+  }
+
+  HostCtrl2 = (DriverStrength << 4) & SD_MMC_HC_CTRL_DRIVER_STRENGTH_MASK;
+  return SdMmcHcOrMmio (PciIo, SlotIndex, SD_MMC_HC_HOST_CTRL2, sizeof (HostCtrl2), &HostCtrl2);
+}
+
 /**
   Turn on/off LED.
 
diff --git a/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHci.h b/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHci.h
index 0b0d415256..bf2add1748 100644
--- a/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHci.h
+++ b/MdeModulePkg/Bus/Pci/SdMmcPciHcDxe/SdMmcPciHci.h
@@ -72,6 +72,8 @@ SPDX-License-Identifier: BSD-2-Clause-Patent
 #define SD_MMC_HC_CTRL_MMC_HS200      0x0003
 #define SD_MMC_HC_CTRL_MMC_HS400      0x0005
 
+#define SD_MMC_HC_CTRL_DRIVER_STRENGTH_MASK  0x0030
+
 //
 // The transfer modes supported by SD Host Controller
 //
@@ -617,4 +619,21 @@ SdMmcHcUhsSignaling (
   IN SD_MMC_BUS_MODE        Timing
   );
 
+/**
+  Set driver strength in host controller.
+
+  @param[in] Private         A pointer to the SD_MMC_HC_PRIVATE_DATA instance.
+  @param[in] SlotIndex       The slot index of the card.
+  @param[in] DriverStrength  DriverStrength to set in the controller.
+
+  @retval EFI_SUCCESS  Driver strength programmed successfully.
+  @retval Others       Failed to set driver strength.
+**/
+EFI_STATUS
+SdMmcSetDriverStrength (
+  IN EFI_PCI_IO_PROTOCOL      *PciIo,
+  IN UINT8                    SlotIndex,
+  IN SD_DRIVER_STRENGTH_TYPE  DriverStrength
+  );
+
 #endif
-- 
2.14.1.windows.1

--------------------------------------------------------------------

Intel Technology Poland sp. z o.o.
ul. Slowackiego 173 | 80-298 Gdansk | Sad Rejonowy Gdansk Polnoc | VII Wydzial Gospodarczy Krajowego Rejestru Sadowego - KRS 101882 | NIP 957-07-52-316 | Kapital zakladowy 200.000 PLN.

Ta wiadomosc wraz z zalacznikami jest przeznaczona dla okreslonego adresata i moze zawierac informacje poufne. W razie przypadkowego otrzymania tej wiadomosci, prosimy o powiadomienie nadawcy oraz trwale jej usuniecie; jakiekolwiek
przegladanie lub rozpowszechnianie jest zabronione.
This e-mail and any attachments may contain confidential material for the sole use of the intended recipient(s). If you are not the intended recipient, please contact the sender and delete all copies; any review or distribution by
others is strictly prohibited.