[edk2,8/9] MmcDxe: set iospeed and bus width in SD stack

diff --git a/EmbeddedPkg/Include/Protocol/MmcHost.h b/EmbeddedPkg/Include/Protocol/MmcHost.h
index 5e3a2b7..ce0a024 100644
--- a/EmbeddedPkg/Include/Protocol/MmcHost.h
+++ b/EmbeddedPkg/Include/Protocol/MmcHost.h
@@ -62,6 +62,7 @@  typedef UINT32 MMC_CMD;
 #define MMC_CMD20             (MMC_INDX(20) | MMC_CMD_WAIT_RESPONSE)
 #define MMC_CMD23             (MMC_INDX(23) | MMC_CMD_WAIT_RESPONSE)
 #define MMC_CMD24             (MMC_INDX(24) | MMC_CMD_WAIT_RESPONSE)
+#define MMC_CMD51             (MMC_INDX(51) | MMC_CMD_WAIT_RESPONSE)
 #define MMC_CMD55             (MMC_INDX(55) | MMC_CMD_WAIT_RESPONSE)
 #define MMC_ACMD41            (MMC_INDX(41) | MMC_CMD_WAIT_RESPONSE | MMC_CMD_NO_CRC_RESPONSE)
 
diff --git a/EmbeddedPkg/Universal/MmcDxe/Mmc.h b/EmbeddedPkg/Universal/MmcDxe/Mmc.h
index 0ccbc80..a62ba32 100644
--- a/EmbeddedPkg/Universal/MmcDxe/Mmc.h
+++ b/EmbeddedPkg/Universal/MmcDxe/Mmc.h
@@ -75,6 +75,23 @@  typedef struct {
   UINT32  PowerUp:     1; // This bit is set to LOW if the card has not finished the power up routine
 } OCR;
 
+/* For little endian CPU */
+typedef struct {
+  UINT8   SD_SPEC:               4; // SD Memory Card - Spec. Version [59:56]
+  UINT8   SCR_STRUCTURE:         4; // SCR Structure [63:60]
+  UINT8   SD_BUS_WIDTHS:         4; // DAT Bus widths supported [51:48]
+  UINT8   DATA_STAT_AFTER_ERASE: 1; // Data Status after erases [55]
+  UINT8   SD_SECURITY:           3; // CPRM Security Support [54:52]
+  UINT8   EX_SECURITY_1:         1; // Extended Security Support [43]
+  UINT8   SD_SPEC4:              1; // Spec. Version 4.00 or higher [42]
+  UINT8   RESERVED_1:            2; // Reserved [41:40]
+  UINT8   SD_SPEC3:              1; // Spec. Version 3.00 or higher [47]
+  UINT8   EX_SECURITY_2:         3; // Extended Security Support [46:44]
+  UINT8   CMD_SUPPORT:           4; // Command Support bits [35:32]
+  UINT8   RESERVED_2:            4; // Reserved [39:36]
+  UINT32  RESERVED_3;               // Manufacturer Usage [31:0]
+} SCR;
+
 typedef struct {
   UINT32  NOT_USED;   // 1 [0:0]
   UINT32  CRC;        // CRC7 checksum [7:1]
diff --git a/EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c b/EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c
index f806bfc..9daa662 100644
--- a/EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c
+++ b/EmbeddedPkg/Universal/MmcDxe/MmcIdentification.c
@@ -12,6 +12,9 @@ 
 *
 **/
 
+#include <Library/BaseMemoryLib.h>
+#include <Library/TimerLib.h>
+
 #include "Mmc.h"
 
 typedef union {
@@ -41,6 +44,11 @@  typedef union {
 
 #define EMMC_SWITCH_ERROR       (1 << 7)
 
+#define SD_BUS_WIDTH_1BIT       (1 << 0)
+#define SD_BUS_WIDTH_4BIT       (1 << 2)
+
+#define SD_CCC_SWITCH           (1 << 10)
+
 #define DEVICE_STATE(x)         (((x) >> 9) & 0xf)
 typedef enum _EMMC_DEVICE_STATE {
   EMMC_IDLE_STATE = 0,
@@ -69,28 +77,30 @@  EmmcGetDeviceState (
 {
   EFI_MMC_HOST_PROTOCOL *Host;
   EFI_STATUS Status;
-  UINT32     Data, RCA;
+  UINT32     Rsp[4], RCA;
 
   if (State == NULL)
     return EFI_INVALID_PARAMETER;
 
   Host  = MmcHostInstance->MmcHost;
   RCA = MmcHostInstance->CardInfo.RCA << RCA_SHIFT_OFFSET;
-  Status = Host->SendCommand (Host, MMC_CMD13, RCA);
-  if (EFI_ERROR (Status)) {
-    DEBUG ((EFI_D_ERROR, "EmmcGetDeviceState(): Failed to get card status, Status=%r.\n", Status));
-    return Status;
-  }
-  Status = Host->ReceiveResponse (Host, MMC_RESPONSE_TYPE_R1, &Data);
-  if (EFI_ERROR (Status)) {
-    DEBUG ((EFI_D_ERROR, "EmmcGetDeviceState(): Failed to get response of CMD13, Status=%r.\n", Status));
-    return Status;
-  }
-  if (Data & EMMC_SWITCH_ERROR) {
-    DEBUG ((EFI_D_ERROR, "EmmcGetDeviceState(): Failed to switch expected mode, Status=%r.\n", Status));
-    return EFI_DEVICE_ERROR;
-  }
-  *State = DEVICE_STATE(Data);
+  do {
+    Status = Host->SendCommand (Host, MMC_CMD13, RCA);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "EmmcGetDeviceState(): Failed to get card status, Status=%r.\n", Status));
+      return Status;
+    }
+    Status = Host->ReceiveResponse (Host, MMC_RESPONSE_TYPE_R1, Rsp);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "EmmcGetDeviceState(): Failed to get response of CMD13, Status=%r.\n", Status));
+      return Status;
+    }
+    if (Rsp[0] & EMMC_SWITCH_ERROR) {
+      DEBUG ((EFI_D_ERROR, "EmmcGetDeviceState(): Failed to switch expected mode, Status=%r.\n", Status));
+      return EFI_DEVICE_ERROR;
+    }
+  } while (!(Rsp[0] & MMC_R0_READY_FOR_DATA));
+  *State = MMC_R0_CURRENTSTATE(Rsp);
   return EFI_SUCCESS;
 }
 
@@ -305,9 +315,12 @@  InitializeSdMmcDevice (
 {
   UINT32        CmdArg;
   UINT32        Response[4];
+  UINT32        Buffer[128];
   UINTN         BlockSize;
   UINTN         CardSize;
   UINTN         NumBlocks;
+  BOOLEAN       CccSwitch;
+  SCR           Scr;
   EFI_STATUS    Status;
   EFI_MMC_HOST_PROTOCOL     *MmcHost;
 
@@ -328,6 +341,10 @@  InitializeSdMmcDevice (
     return Status;
   }
   PrintCSD (Response);
+  if (MMC_CSD_GET_CCC(Response) & SD_CCC_SWITCH)
+    CccSwitch = TRUE;
+  else
+    CccSwitch = FALSE;
 
   if (MmcHostInstance->CardInfo.CardType == SD_CARD_2_HIGH) {
     CardSize = HC_MMC_CSD_GET_DEVICESIZE (Response);
@@ -358,6 +375,87 @@  InitializeSdMmcDevice (
     return Status;
   }
 
+  Status = MmcHost->SendCommand (MmcHost, MMC_CMD55, CmdArg);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "%a(MMC_CMD55): Error and Status = %r\n", Status));
+    return Status;
+  }
+  /* SCR */
+  Status = MmcHost->SendCommand (MmcHost, MMC_CMD51, 0);
+  if (EFI_ERROR (Status)) {
+    DEBUG ((EFI_D_ERROR, "%a(MMC_CMD51): Error and Status = %r\n", Status));
+    return Status;
+  } else {
+    Status = MmcHost->ReadBlockData (MmcHost, 0, 8, Buffer);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "%a(MMC_CMD51): ReadBlockData Error and Status = %r\n", Status));
+      return Status;
+    }
+    CopyMem (&Scr, Buffer, 8);
+    if (Scr.SD_SPEC == 2) {
+      if (Scr.SD_SPEC3 == 1) {
+	if (Scr.SD_SPEC4 == 1) {
+          DEBUG ((EFI_D_INFO, "Found SD Card for Spec Version 4.xx\n"));
+	} else {
+          DEBUG ((EFI_D_INFO, "Found SD Card for Spec Version 3.0x\n"));
+	}
+      } else {
+	if (Scr.SD_SPEC4 == 0) {
+          DEBUG ((EFI_D_INFO, "Found SD Card for Spec Version 2.0\n"));
+	} else {
+	  DEBUG ((EFI_D_ERROR, "Found invalid SD Card\n"));
+	}
+      }
+    } else {
+      if ((Scr.SD_SPEC3 == 0) && (Scr.SD_SPEC4 == 0)) {
+        if (Scr.SD_SPEC == 1) {
+	  DEBUG ((EFI_D_INFO, "Found SD Card for Spec Version 1.10\n"));
+	} else {
+	  DEBUG ((EFI_D_INFO, "Found SD Card for Spec Version 1.0\n"));
+	}
+      } else {
+        DEBUG ((EFI_D_ERROR, "Found invalid SD Card\n"));
+      }
+    }
+  }
+  if (CccSwitch) {
+    /* SD Switch, Mode:1, Group:0, Value:1 */
+    CmdArg = 1 << 31 | 0x00FFFFFF;
+    CmdArg &= ~(0xF << (0 * 4));
+    CmdArg |= 1 << (0 * 4);
+    Status = MmcHost->SendCommand (MmcHost, MMC_CMD6, CmdArg);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "%a(MMC_CMD6): Error and Status = %r\n", 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));
+        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));
+      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));
+      return Status;
+    }
+  }
+  if (MmcHost->SetIos) {
+    Status = MmcHost->SetIos (MmcHost, 24 * 1000 * 1000, 4, EMMCBACKWARD);
+    if (EFI_ERROR (Status)) {
+      DEBUG ((EFI_D_ERROR, "%a(SetIos): Error and Status = %r\n", Status));
+      return Status;
+    }
+  }
   return EFI_SUCCESS;
 }