From eba4b7056128ae85402984fb03882a5a9eb88213 Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Thu, 10 May 2012 15:07:40 +0000
Subject: [PATCH] git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@4182
 35acf78f-673a-0410-8e92-d51de3d6d3f4

---
 os/hal/include/mmc_spi.h         |  4 +++
 os/hal/platforms/STM32/sdc_lld.c |  2 +-
 os/hal/src/mmc_spi.c             | 59 ++++++++++++++++++++++++++++++--
 os/hal/src/mmcsd.c               |  6 ++--
 4 files changed, 65 insertions(+), 6 deletions(-)

diff --git a/os/hal/include/mmc_spi.h b/os/hal/include/mmc_spi.h
index a5812e399..a1a33c72d 100644
--- a/os/hal/include/mmc_spi.h
+++ b/os/hal/include/mmc_spi.h
@@ -157,6 +157,10 @@ typedef struct {
    * @brief Addresses use blocks instead of bytes.
    */
   bool_t                block_addresses;
+  /**
+   * @brief Total number of blocks in card.
+   */
+  uint32_t              capacity;
 } MMCDriver;
 
 /*===========================================================================*/
diff --git a/os/hal/platforms/STM32/sdc_lld.c b/os/hal/platforms/STM32/sdc_lld.c
index 14752c049..f618d7776 100644
--- a/os/hal/platforms/STM32/sdc_lld.c
+++ b/os/hal/platforms/STM32/sdc_lld.c
@@ -578,7 +578,7 @@ bool_t sdc_lld_send_cmd_long_crc(SDCDriver *sdcp, uint8_t cmd, uint32_t arg,
     sdc_lld_collect_errors(sdcp);
     return CH_FAILED;
   }
-  /* save bytes in reverse order because MSB in response comes first */
+  /* Save bytes in reverse order because MSB in response comes first.*/
   *resp++ = SDIO->RESP4;
   *resp++ = SDIO->RESP3;
   *resp++ = SDIO->RESP2;
diff --git a/os/hal/src/mmc_spi.c b/os/hal/src/mmc_spi.c
index dad837613..a4ce608e3 100644
--- a/os/hal/src/mmc_spi.c
+++ b/os/hal/src/mmc_spi.c
@@ -310,7 +310,7 @@ static uint8_t send_command_R1(MMCDriver *mmcp, uint8_t cmd, uint32_t arg) {
 static uint8_t send_command_R3(MMCDriver *mmcp, uint8_t cmd, uint32_t arg,
                                uint8_t *response) {
   uint8_t r1;
-  
+
   spiSelect(mmcp->config->spip);
   send_hdr(mmcp, cmd, arg);
   r1 = recvr3(mmcp, response);
@@ -318,6 +318,53 @@ static uint8_t send_command_R3(MMCDriver *mmcp, uint8_t cmd, uint32_t arg,
   return r1;
 }
 
+/**
+ * @brief   Reads the CSD.
+ *
+ * @param[in] mmcp      pointer to the @p MMCDriver object
+ * @param[out] csd       pointer to the CSD buffer
+ *
+ * @return              The operation status.
+ * @retval FALSE        the operation succeeded.
+ * @retval TRUE         the operation failed.
+ *
+ * @notapi
+ */
+static bool_t read_CSD(MMCDriver *mmcp, uint32_t csd[4]) {
+  unsigned i;
+  uint8_t *bp, buf[16];
+
+  spiSelect(mmcp->config->spip);
+  send_hdr(mmcp, MMCSD_CMD_SEND_CSD, 0);
+  if (recvr1(mmcp) != 0x00) {
+    spiUnselect(mmcp->config->spip);
+    return TRUE;
+  }
+
+  /* Wait for data availability.*/
+  for (i = 0; i < MMC_WAIT_DATA; i++) {
+    spiReceive(mmcp->config->spip, 1, buf);
+    if (buf[0] == 0xFE) {
+      uint32_t *wp;
+
+      spiReceive(mmcp->config->spip, 16, buf);
+      bp = buf;
+      for (wp = &csd[3]; wp >= csd; wp--) {
+        *wp = ((uint32_t)bp[0] << 24) | ((uint32_t)bp[1] << 16) |
+              ((uint32_t)bp[2] << 8)  | (uint32_t)bp[3];
+        bp += 4;
+      }
+
+      /* CRC ignored then end of transaction. */
+      spiIgnore(mmcp->config->spip, 2);
+      spiUnselect(mmcp->config->spip);
+
+      return FALSE;
+    }
+  }
+  return TRUE;
+}
+
 /**
  * @brief   Waits that the card reaches an idle state.
  *
@@ -444,6 +491,7 @@ void mmcStop(MMCDriver *mmcp) {
 bool_t mmcConnect(MMCDriver *mmcp) {
   unsigned i;
   bool_t result;
+  uint32_t csd[4];
 
   chDbgCheck(mmcp != NULL, "mmcConnect");
 
@@ -515,6 +563,13 @@ bool_t mmcConnect(MMCDriver *mmcp) {
                         MMCSD_BLOCK_SIZE) != 0x00)
       return TRUE;
 
+    /* Determine capacity.*/
+    if (read_CSD(mmcp, csd))
+      return TRUE;
+    mmcp->capacity = mmcsdGetCapacity(csd);
+    if (mmcp->capacity == 0)
+      return TRUE;
+
     /* Transition to MMC_READY state (if not extracted).*/
     chSysLock();
     if (mmcp->state == MMC_INSERTED) {
@@ -866,7 +921,7 @@ bool_t mmcGetInfo(MMCDriver *mmcp, BlockDeviceInfo *bdip) {
   }
   chSysUnlock();
 
-  bdip->blk_num = 0; /* NOTE: To be implemented.*/
+  bdip->blk_num  = mmcp->capacity;
   bdip->blk_size = MMCSD_BLOCK_SIZE;
 
   return FALSE;
diff --git a/os/hal/src/mmcsd.c b/os/hal/src/mmcsd.c
index 88f8a5505..44a552cbd 100644
--- a/os/hal/src/mmcsd.c
+++ b/os/hal/src/mmcsd.c
@@ -58,7 +58,7 @@ static uint32_t mmcsd_get_slice(uint32_t *data, uint32_t end, uint32_t start) {
 
   chDbgCheck(end >= start, "sdc_get_slice");
 
-  while ((start - 32 * word) > 31){
+  while ((start - 32 * word) > 31) {
     word++;
     data++;
   }
@@ -66,12 +66,12 @@ static uint32_t mmcsd_get_slice(uint32_t *data, uint32_t end, uint32_t start) {
   end   -= 32 * word;
   start -= 32 * word;
 
-  if (end < 31){
+  if (end < 31) {
     /* Value lays in one word.*/
     mask = (1 << (end - start + 1)) - 1;
     return (*data >> start) & mask;
   }
-  else{
+  else {
     /* Value spread on separate words.*/
     uint32_t lsb, msb;
     lsb = *data >> start;