[PATCH 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.

Signed-off-by: Meenakshi Aggarwal <meenakshi.aggarwal@nxp.com>
---
 EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c | 64 ++++++++++++++++++++----
 1 file changed, 55 insertions(+), 9 deletions(-)

diff --git a/EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c b/EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c
index 7f74c54..3071b3b 100644
--- a/EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c
+++ b/EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c
@@ -317,6 +317,24 @@ InitializeEmmcDevice (
   return Status;
 }
 
+
+STATIC
+UINT32
+CreateSwitchCmdArgument (
+  IN  UINT8  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 (
@@ -326,6 +344,7 @@ InitializeSdMmcDevice (
   UINT32        CmdArg;
   UINT32        Response[4];
   UINT32        Buffer[128];
+  UINT32        Speed;
   UINTN         BlockSize;
   UINTN         CardSize;
   UINTN         NumBlocks;
@@ -334,6 +353,7 @@ InitializeSdMmcDevice (
   EFI_STATUS    Status;
   EFI_MMC_HOST_PROTOCOL     *MmcHost;
 
+  Speed = 25000000;
   MmcHost = MmcHostInstance->MmcHost;
 
   // Send a command to get Card specific data
@@ -439,43 +459,69 @@ InitializeSdMmcDevice (
     }
   }
   if (CccSwitch) {
+    /* 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): Failed with Status = %r\n", __func__, Status));
+       return Status;
+    } else {
+      Status = MmcHost->ReadBlockData (MmcHost, 0, 64, Buffer);
+      if (EFI_ERROR (Status)) {
+        DEBUG ((EFI_D_ERROR, "%a(MMC_CMD6): ReadBlockData Failed with Status = %r\n", __func__, Status));
+        return Status;
+      }
+    }
+
+    if (!(Buffer[3] & 0x20000)) {
+      DEBUG ((EFI_D_ERROR, "%aHigh Speed not supported by Card %r\n", __func__, Status));
+      return Status;
+    }
+
+    Speed = 50000000;       //High Speed for SD card is 50 MHZ
+
     /* SD Switch, Mode:1, Group:0, Value:1 */
-    CmdArg = 1 << 31 | 0x00FFFFFF;
-    CmdArg &= ~(0xF << (0 * 4));
-    CmdArg |= 1 << (0 * 4);
+    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", Status));
+      DEBUG ((EFI_D_ERROR, "%a(MMC_CMD6): Error and Status = %r\n", __func__, Status));
        return Status;
     } else {
       Status = MmcHost->ReadBlockData (MmcHost, 0, 64, Buffer);
       if (EFI_ERROR (Status)) {
-        DEBUG ((EFI_D_ERROR, "%a(MMC_CMD6): ReadBlockData Error and Status = %r\n", Status));
+        DEBUG ((EFI_D_ERROR, "%a(MMC_CMD6): ReadBlockData Error and Status = %r\n",__func__, Status));
+        return Status;
+      }
+
+      if ((Buffer[4] & 0x0f000000) != 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;
     Status = MmcHost->SendCommand (MmcHost, MMC_CMD55, CmdArg);
     if (EFI_ERROR (Status)) {
-      DEBUG ((EFI_D_ERROR, "%a(MMC_CMD55): Error and Status = %r\n", Status));
+      DEBUG ((EFI_D_ERROR, "%a(MMC_CMD55): Error and Status = %r\n", __func__, Status));
       return Status;
     }
     /* Width: 4 */
     Status = MmcHost->SendCommand (MmcHost, MMC_CMD6, 2);
     if (EFI_ERROR (Status)) {
-      DEBUG ((EFI_D_ERROR, "%a(MMC_CMD6): Error and Status = %r\n", Status));
+      DEBUG ((EFI_D_ERROR, "%a(MMC_CMD6): Error and Status = %r\n", __func__, Status));
       return Status;
     }
   }
   if (MMC_HOST_HAS_SETIOS(MmcHost)) {
-    Status = MmcHost->SetIos (MmcHost, 26 * 1000 * 1000, 4, EMMCBACKWARD);
+    Status = MmcHost->SetIos (MmcHost, Speed, 4, EMMCBACKWARD);
     if (EFI_ERROR (Status)) {
-      DEBUG ((EFI_D_ERROR, "%a(SetIos): Error and Status = %r\n", Status));
+      DEBUG ((EFI_D_ERROR, "%a(SetIos): Error and Status = %r\n", __func__, Status));
       return Status;
     }
   }
+
   return EFI_SUCCESS;
 }
 
-- 
1.9.1