From fa10615077fd341522aa7a143c93a2ac0194856e Mon Sep 17 00:00:00 2001
From: bsongis <bsongis@8db61ce9-5e8f-94c8-fa6b-1d1c8bcca7d0>
Date: Tue, 25 Sep 2012 14:03:00 +0000
Subject: [PATCH] [sky9x] SD Format working?

---
 src/ersky9x/board.h                              |  7 +++++++
 src/ersky9x/diskio.c                             | 16 ++--------------
 src/ersky9x/libraries/memories/MEDSdcard.c       |  2 --
 src/ersky9x/libraries/usb/device/core/USBD.h     |  2 +-
 .../libraries/usb/device/massstorage/MSDDriver.h |  2 +-
 5 files changed, 11 insertions(+), 18 deletions(-)

diff --git a/src/ersky9x/board.h b/src/ersky9x/board.h
index 014ac7cc0..f84b0bfc1 100644
--- a/src/ersky9x/board.h
+++ b/src/ersky9x/board.h
@@ -513,6 +513,13 @@
 
 extern int32_t Card_state;
 extern volatile uint32_t Card_initialized;
+extern uint32_t Card_CSD[4];
+
+#define SD_CSD(csd, bitfield, bits)   ((csd[3-(bitfield)/32] >> ((bitfield)%32)) & ((1 << (bits)) - 1))
+
+#define SD_CSD_C_SIZE_HC(csd)          ((SD_CSD(csd, 64,  6) << 16) + \
+                                        (SD_CSD(csd, 56,  8) << 8)  + \
+                                        SD_CSD(csd, 48,  8)) ///< Device size v2.0 High Capacity
 
 /*
 #define BOARD_PSRAM_PINS            PIN_EBI_DATA_BUS, PIN_EBI_NCS0, PIN_EBI_NRD, PIN_EBI_NWE, \
diff --git a/src/ersky9x/diskio.c b/src/ersky9x/diskio.c
index 95173b064..e6c447b45 100644
--- a/src/ersky9x/diskio.c
+++ b/src/ersky9x/diskio.c
@@ -1290,9 +1290,6 @@ DRESULT disk_ioctl (
                         )
 {
         DRESULT res;
-        BYTE *csd = (BYTE*)&Card_CSD[0];
-        BYTE n; // csd[16], *ptr = buff;
-        WORD csize;
 
         if (drv) return RES_PARERR;
 
@@ -1324,17 +1321,8 @@ DRESULT disk_ioctl (
                                 break;
 
                         case GET_SECTOR_COUNT : /* Get number of sectors on the disk (DWORD) */
-                                // if ((send_cmd(CMD9, 0) == 0) && rcvr_datablock(csd, 16)) {
-                                        if ((csd[0] >> 6) == 1) {       /* SDC ver 2.00 */
-                                                csize = csd[9] + ((WORD)csd[8] << 8) + 1;
-                                                *(DWORD*)buff = (DWORD)csize << 10;
-                                        } else {                                        /* SDC ver 1.XX or MMC*/
-                                                n = (csd[5] & 15) + ((csd[10] & 128) >> 7) + ((csd[9] & 3) << 1) + 2;
-                                                csize = (csd[8] >> 6) + ((WORD)csd[7] << 2) + ((WORD)(csd[6] & 3) << 10) + 1;
-                                                *(DWORD*)buff = (DWORD)csize << (n - 9);
-                                        }
-                                        res = RES_OK;
-                                // }
+                                *(DWORD*)buff = ((SD_CSD_C_SIZE_HC(Card_CSD) + 1) * 1024);
+                                res = RES_OK;
                                 break;
 
                         case GET_SECTOR_SIZE :  /* Get R/W sector size (WORD) */
diff --git a/src/ersky9x/libraries/memories/MEDSdcard.c b/src/ersky9x/libraries/memories/MEDSdcard.c
index e612db53e..a1c70b362 100644
--- a/src/ersky9x/libraries/memories/MEDSdcard.c
+++ b/src/ersky9x/libraries/memories/MEDSdcard.c
@@ -214,8 +214,6 @@ static unsigned char MEDSdcard_Write(Media         *media,
 /// \return 1 if success.
 //------------------------------------------------------------------------------
 
-extern uint32_t Card_CSD[4];
-
 unsigned char MEDSdcard_Initialize(Media *media, unsigned char mciID)
 {
     TRACE_INFO("MEDSdcard init\n\r");
diff --git a/src/ersky9x/libraries/usb/device/core/USBD.h b/src/ersky9x/libraries/usb/device/core/USBD.h
index 32e83d0c5..588254bf8 100644
--- a/src/ersky9x/libraries/usb/device/core/USBD.h
+++ b/src/ersky9x/libraries/usb/device/core/USBD.h
@@ -49,7 +49,7 @@
 //         Headers
 //------------------------------------------------------------------------------
 
-#include <board.h>
+// #include <board.h>
 #include <usb/common/core/USBEndpointDescriptor.h>
 #include <usb/common/core/USBGenericRequest.h>
 
diff --git a/src/ersky9x/libraries/usb/device/massstorage/MSDDriver.h b/src/ersky9x/libraries/usb/device/massstorage/MSDDriver.h
index 0a8738a6d..388f0adcb 100644
--- a/src/ersky9x/libraries/usb/device/massstorage/MSDDriver.h
+++ b/src/ersky9x/libraries/usb/device/massstorage/MSDDriver.h
@@ -52,7 +52,7 @@
 
 #include "MSD.h"
 #include "MSDLun.h"
-#include <utility/trace.h>
+// #include <utility/trace.h>
 
 //------------------------------------------------------------------------------
 //      Global functions