[PATCH v2 2/2] SD : Updated CMD 6 implememtation.

[Meenakshi Aggarwal <meenakshi.aggarwal@nxp.com>]

For setting high speed in SD card,
First CMD 6 (Switch) is send to check if card supports High Speed and
Second command is send to switch card to high speed mode.

In current inplementation, CMD 6 was sent only once to switch the
card into HS mode without checking if card supports HS or not, which is
not as per specification and also we are not setting the HS i.e. 50000000
but directly asking the card to switch to 26000000 which is incorrect as
SD card supports either 25000000 or 50000000.

Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off-by: Meenakshi Aggarwal <meenakshi.aggarwal@nxp.com>
---
 EmbeddedPkg/Universal/MmcDxe/Mmc.h               |  8 ++++
 EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c | 55 +++++++++++++++++++++---
 2 files changed, 57 insertions(+), 6 deletions(-)

diff --git a/EmbeddedPkg/Universal/MmcDxe/Mmc.h b/EmbeddedPkg/Universal/MmcDxe/Mmc.h
index f3e56ff..a77ba41 100644
--- a/EmbeddedPkg/Universal/MmcDxe/Mmc.h
+++ b/EmbeddedPkg/Universal/MmcDxe/Mmc.h
@@ -64,6 +64,14 @@
 #define EMMC_CMD6_ARG_VALUE(x)              (((x) & 0xFF) << 8)
 #define EMMC_CMD6_ARG_CMD_SET(x)            (((x) & 0x7) << 0)
 
+#define SWITCH_CMD_DATA_LENGTH              64
+#define SD_HIGH_SPEED_SUPPORTED             0x20000
+#define SD_DEFAULT_SPEED                    25000000
+#define SD_HIGH_SPEED                       50000000
+#define SWITCH_CMD_SUCCESS_MASK             0x0f000000
+
+#define BUSWIDTH_4                          4
+
 typedef enum {
   UNKNOWN_CARD,
   MMC_CARD,              //MMC card
diff --git a/EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c b/EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c
index 8010dea..e60d8b0 100644
--- a/EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c
+++ b/EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c
@@ -318,6 +318,23 @@ InitializeEmmcDevice (
 }
 
 STATIC
+UINT32
+CreateSwitchCmdArgument (
+  IN  UINT32 Mode,
+  IN  UINT8  Group,
+  IN  UINT8  Value
+  )
+{
+  UINT32 Argument;
+
+  Argument = Mode << 31 | 0x00FFFFFF;
+  Argument &= ~(0xF << (Group * 4));
+  Argument |= Value << (Group * 4);
+
+  return Argument;
+}
+
+STATIC
 EFI_STATUS
 InitializeSdMmcDevice (
   IN  MMC_HOST_INSTANCE   *MmcHostInstance
@@ -326,6 +343,7 @@ InitializeSdMmcDevice (
   UINT32        CmdArg;
   UINT32        Response[4];
   UINT32        Buffer[128];
+  UINT32        Speed;
   UINTN         BlockSize;
   UINTN         CardSize;
   UINTN         NumBlocks;
@@ -334,6 +352,7 @@ InitializeSdMmcDevice (
   EFI_STATUS    Status;
   EFI_MMC_HOST_PROTOCOL     *MmcHost;
 
+  Speed = SD_DEFAULT_SPEED;
   MmcHost = MmcHostInstance->MmcHost;
 
   // Send a command to get Card specific data
@@ -439,21 +458,45 @@ InitializeSdMmcDevice (
     }
   }
   if (CccSwitch) {
-    /* SD Switch, Mode:1, Group:0, Value:1 */
-    CmdArg = 1 << 31 | 0x00FFFFFF;
-    CmdArg &= ~(0xF << (0 * 4));
-    CmdArg |= 1 << (0 * 4);
+    /* SD Switch, Mode:0, Group:0, Value:0 */
+    CmdArg = CreateSwitchCmdArgument(0, 0, 0);
     Status = MmcHost->SendCommand (MmcHost, MMC_CMD6, CmdArg);
     if (EFI_ERROR (Status)) {
       DEBUG ((EFI_D_ERROR, "%a (MMC_CMD6): Error and Status = %r\n", __FUNCTION__, Status));
        return Status;
     } else {
-      Status = MmcHost->ReadBlockData (MmcHost, 0, 64, Buffer);
+      Status = MmcHost->ReadBlockData (MmcHost, 0, SWITCH_CMD_DATA_LENGTH, Buffer);
       if (EFI_ERROR (Status)) {
         DEBUG ((EFI_D_ERROR, "%a (MMC_CMD6): ReadBlockData Error and Status = %r\n", __FUNCTION__, Status));
         return Status;
       }
     }
+
+    if (!(Buffer[3] & SD_HIGH_SPEED_SUPPORTED)) {
+      DEBUG ((EFI_D_ERROR, "%a : High Speed not supported by Card %r\n", __FUNCTION__, Status));
+      return Status;
+    }
+
+    Speed = SD_HIGH_SPEED;
+
+    /* SD Switch, Mode:1, Group:0, Value:1 */
+    CmdArg = CreateSwitchCmdArgument(1, 0, 1);
+    Status = MmcHost->SendCommand (MmcHost, MMC_CMD6, CmdArg);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "%a (MMC_CMD6): Error and Status = %r\n", __FUNCTION__, Status));
+       return Status;
+    } else {
+      Status = MmcHost->ReadBlockData (MmcHost, 0, SWITCH_CMD_DATA_LENGTH, Buffer);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "%a (MMC_CMD6): ReadBlockData Error and Status = %r\n",__FUNCTION__, Status));
+        return Status;
+      }
+
+      if ((Buffer[4] & SWITCH_CMD_SUCCESS_MASK) != 0x01000000) {
+        DEBUG((EFI_D_ERROR, "Problem switching SD card into high-speed mode\n"));
+        return Status;
+      }
+    }
   }
   if (Scr.SD_BUS_WIDTHS & SD_BUS_WIDTH_4BIT) {
     CmdArg = MmcHostInstance->CardInfo.RCA << 16;
@@ -470,7 +513,7 @@ InitializeSdMmcDevice (
     }
   }
   if (MMC_HOST_HAS_SETIOS(MmcHost)) {
-    Status = MmcHost->SetIos (MmcHost, 26 * 1000 * 1000, 4, EMMCBACKWARD);
+    Status = MmcHost->SetIos (MmcHost, Speed, BUSWIDTH_4, EMMCBACKWARD);
     if (EFI_ERROR (Status)) {
       DEBUG ((EFI_D_ERROR, "%a (SetIos): Error and Status = %r\n", __FUNCTION__, Status));
       return Status;
-- 
1.9.1