cdms_sd.h

00001 //SPI spi(PTE1, PTE3, PTE2);      // MOSI,MISO, CLOCK microcontroller(in order)     
00002 //DigitalOut cs_sd(PTE22);
00003 
00004 //Serial sd1(USBTX,USBRX);
00005 
00006 
00007 #define SD_COMMAND_TIMEOUT 5000
00008 
00009 #define SD_DBG             0
00010 
00011 #define R1_IDLE_STATE           (1 << 0)
00012 #define R1_ERASE_RESET          (1 << 1)
00013 #define R1_ILLEGAL_COMMAND      (1 << 2)
00014 #define R1_COM_CRC_ERROR        (1 << 3)
00015 #define R1_ERASE_SEQUENCE_ERROR (1 << 4)
00016 #define R1_ADDRESS_ERROR        (1 << 5)
00017 #define R1_PARAMETER_ERROR      (1 << 6)
00018 
00019 
00020 
00021 #define SD_MAX_CYCLES 10000
00022 
00023 uint32_t SD_SCP_FIRST=1001;
00024 uint32_t SD_SCP_LAST=2000;
00025 uint32_t SD_SFF_AT_FIRST=2001;
00026 uint32_t SD_SFF_AT_LAST = 3000;
00027 uint32_t SD_SFF_BT_FIRST =3001;
00028 uint32_t SD_SFF_BT_LAST=4000;
00029 uint32_t SD_HK_ARCH_FIRST=4001;
00030 uint32_t SD_HK_ARCH_LAST= 5000;
00031 uint32_t LOG_FIRST =5001;
00032 uint32_t LOG_LAST=6000;
00033 uint32_t SD_MNG_SECT=7000;
00034 uint16_t SD_LIB_WRITES = 0;
00035 
00036 extern uint8_t SD_INIT_FLAGS;
00037 
00038 int initialise_card();
00039 int initialise_card_v1();
00040 int initialise_card_v2();
00041 int disk_write(const uint8_t *, uint64_t);
00042 int disk_read(uint8_t *, uint64_t);
00043 int disk_erase(int,int);
00044 int disk_read_statusbits(uint8_t *);
00045 
00046 void FCTN_SD_MNG();
00047 int INCREMENT_SD_LIB(uint8_t);
00048 
00049 
00050 int cmd(int, int);
00051 int cmd58();
00052 int cmdx(int, int);
00053 int cmd8();
00054 int read(uint8_t*, uint32_t );
00055 int write(const uint8_t*, uint32_t );
00056 static uint32_t ext_bits(unsigned char *, int , int );
00057 int SD_WRITE(uint8_t*,uint32_t,uint8_t);
00058 int FCTN_CDMS_SD_INIT();
00059 uint8_t SD_READ(uint8_t*,uint32_t,uint8_t);
00060 #define SDCARD_FAIL 0
00061 #define SDCARD_V1   1
00062 #define SDCARD_V2   2
00063 #define SDCARD_V2HC 3
00064 
00065 int cdv;
00066 uint64_t sd_sectors();
00067 uint64_t sectors;
00068 
00069 int FCTN_CDMS_SD_INIT()
00070 {   
00071     int i = initialise_card();
00072     debug_if(SD_DBG, "init card = %d\n", i);
00073     sectors = sd_sectors();
00074 
00075     // Set block length to 512 (CMD16)
00076     if (cmd(16, 512) != 0) {
00077         debug("\rSet 512-byte block timed out\r\n");
00078         return 1;
00079     } else {
00080     //printf("\rDisk initialization successfull\r\n");
00081     }
00082 
00083     spi.frequency(1000000); // Set to 1MHz for data transfer
00084     return 0;
00085 }
00086 
00087 void  FCTN_SD_MNGR()
00088 {
00089     uint32_t fsc;
00090     uint32_t start_fsc;
00091     uint8_t buffer[512];
00092     int b=disk_read(buffer, SD_MNG_SECT);
00093     
00094     fsc=(uint32_t)(buffer[0]<<24)+(uint32_t)(buffer[1]<<16)+(uint32_t)(buffer[2]<<8)+(uint32_t)buffer[3];
00095     start_fsc=(uint32_t)(buffer[4]<<24)+(uint32_t)(buffer[5]<<16)+(uint32_t)(buffer[6]<<8)+(uint32_t)buffer[7];
00096     FSC_CURRENT[1] = fsc;
00097     FSC_LAST[1] = start_fsc;
00098 
00099     fsc=(uint32_t)(buffer[8]<<24)+(uint32_t)(buffer[9]<<16)+(uint32_t)(buffer[10]<<8)+(uint32_t)buffer[11];
00100     start_fsc=(uint32_t)(buffer[12]<<24)+(uint32_t)(buffer[13]<<16)+(uint32_t)(buffer[14]<<8)+(uint32_t)buffer[15];
00101     FSC_CURRENT[2] = fsc;
00102     FSC_LAST[2] = start_fsc;
00103 
00104     fsc=(uint32_t)(buffer[16]<<24)+(uint32_t)(buffer[17]<<16)+(uint32_t)(buffer[18]<<8)+(uint32_t)buffer[19];
00105     start_fsc=(uint32_t)(buffer[20]<<24)+(uint32_t)(buffer[21]<<16)+(uint32_t)(buffer[22]<<8)+(uint32_t)buffer[23];
00106     FSC_CURRENT[3] = fsc;
00107     FSC_LAST[3] = start_fsc;
00108 
00109     fsc=(uint32_t)(buffer[24]<<24)+(uint32_t)(buffer[25]<<16)+(uint32_t)(buffer[26]<<8)+(uint32_t)buffer[27];
00110     start_fsc=(uint32_t)(buffer[28]<<24)+(uint32_t)(buffer[29]<<16)+(uint32_t)(buffer[30]<<8)+(uint32_t)buffer[31];
00111     FSC_CURRENT[4] = fsc;
00112     FSC_LAST[4] = start_fsc;
00113 
00114     fsc=(uint32_t)(buffer[32]<<24)+(uint32_t)(buffer[33]<<16)+(uint32_t)(buffer[34]<<8)+(uint32_t)buffer[35];
00115     start_fsc=(uint32_t)(buffer[36]<<24)+(uint32_t)(buffer[37]<<16)+(uint32_t)(buffer[38]<<8)+(uint32_t)buffer[39];
00116     FSC_CURRENT[5] = fsc;
00117     FSC_LAST[5] = start_fsc;
00118 }
00119 
00120 int INCREMENT_SD_LIB(uint8_t sid)
00121 {
00122     uint32_t fsc;
00123     uint32_t start_fsc;
00124     int i;
00125     uint8_t buffer[512];
00126     SD_MNG_SECT += SD_LIB_WRITES/(int)0xFFFF;
00127     SD_LIB_WRITES = SD_LIB_WRITES%(int)0xFFFF;
00128     disk_read(buffer,SD_MNG_SECT);
00129     if(sid==0x01)
00130     {
00131         fsc=(uint32_t)(buffer[0]<<24)+(uint32_t)(buffer[1]<<16)+(uint32_t)(buffer[2]<<8)+(uint32_t)buffer[3];
00132         start_fsc=(uint32_t)(buffer[4]<<24)+(uint32_t)(buffer[5]<<16)+(uint32_t)(buffer[6]<<8)+(uint32_t)buffer[7];
00133         fsc++;
00134         buffer[0]=(uint8_t) (fsc>>24 & 0xFF);
00135         buffer[1]=(uint8_t) (fsc>>16 & 0xFF);
00136         buffer[2]=(uint8_t) (fsc>>8 & 0xFF);
00137         buffer[3]=(uint8_t) (fsc & 0xFF);
00138         if(fsc > SD_SCP_LAST-SD_SCP_FIRST+1)
00139         {
00140             start_fsc = start_fsc+1;
00141             buffer[4]=(uint8_t) (start_fsc>>24 & 0xFF);
00142             buffer[5]=(uint8_t) (start_fsc>>16 & 0xFF);
00143             buffer[6]=(uint8_t) (start_fsc>>8 & 0xFF);
00144             buffer[7]=(uint8_t) (start_fsc & 0xFF);
00145         }
00146         
00147         i = disk_write(buffer,SD_MNG_SECT);
00148         if(i == 0)
00149         {
00150             FSC_CURRENT[1] = fsc;
00151             FSC_LAST[1] = start_fsc;
00152             return i;
00153         } 
00154     }
00155     if(sid==0x02)
00156     {
00157         fsc=(uint32_t)(buffer[8]<<24)+(uint32_t)(buffer[9]<<16)+(uint32_t)(buffer[10]<<8)+(uint32_t)buffer[11];
00158         start_fsc=(uint32_t)(buffer[12]<<24)+(uint32_t)(buffer[13]<<16)+(uint32_t)(buffer[14]<<8)+(uint32_t)buffer[15];
00159         fsc++;
00160         buffer[8]=(uint8_t) (fsc>>24 & 0xFF);
00161         buffer[9]=(uint8_t) (fsc>>16 & 0xFF);
00162         buffer[10]=(uint8_t) (fsc>>8 & 0xFF);
00163         buffer[11]=(uint8_t) (fsc & 0xFF);
00164         if(fsc > SD_SFF_AT_LAST-SD_SFF_AT_FIRST+1)
00165         {
00166             start_fsc = start_fsc+1;
00167             buffer[12]=(uint8_t) (start_fsc>>24 & 0xFF);
00168             buffer[13]=(uint8_t) (start_fsc>>16 & 0xFF);
00169             buffer[14]=(uint8_t) (start_fsc>>8 & 0xFF);
00170             buffer[15]=(uint8_t) (start_fsc & 0xFF);
00171         }
00172         i = disk_write(buffer,SD_MNG_SECT);
00173         if(i == 0)
00174         {
00175             FSC_CURRENT[2] = fsc;
00176             FSC_LAST[2] = start_fsc;
00177             return i;
00178         }
00179     }
00180     if(sid==0x03)
00181     {
00182         fsc=(uint32_t)(buffer[16]<<24)+(uint32_t)(buffer[17]<<16)+(uint32_t)(buffer[18]<<8)+(uint32_t)buffer[19];
00183         start_fsc=(uint32_t)(buffer[20]<<24)+(uint32_t)(buffer[21]<<16)+(uint32_t)(buffer[22]<<8)+(uint32_t)buffer[23];
00184         fsc++;
00185         buffer[16]=(uint8_t) (fsc>>24 & 0xFF);
00186         buffer[17]=(uint8_t) (fsc>>16 & 0xFF);
00187         buffer[18]=(uint8_t) (fsc>>8 & 0xFF);
00188         buffer[19]=(uint8_t) (fsc & 0xFF);
00189         if(fsc > SD_SFF_BT_LAST-SD_SFF_BT_FIRST+1)
00190         {
00191             start_fsc = start_fsc+1;
00192             buffer[20]=(uint8_t) (start_fsc>>24 & 0xFF);
00193             buffer[21]=(uint8_t) (start_fsc>>16 & 0xFF);
00194             buffer[22]=(uint8_t) (start_fsc>>8 & 0xFF);
00195             buffer[23]=(uint8_t) (start_fsc & 0xFF);
00196         }
00197         i = disk_write(buffer,SD_MNG_SECT);
00198         if(i == 0)
00199         {
00200             FSC_CURRENT[3] = fsc;
00201             FSC_LAST[3] = start_fsc;
00202             return i;
00203         }
00204     }
00205      if(sid==0x04)
00206     {
00207         fsc=(uint32_t)(buffer[24]<<24)+(uint32_t)(buffer[25]<<16)+(uint32_t)(buffer[26]<<8)+(uint32_t)buffer[27];
00208         start_fsc=(uint32_t)(buffer[28]<<24)+(uint32_t)(buffer[29]<<16)+(uint32_t)(buffer[30]<<8)+(uint32_t)buffer[31];
00209         fsc++;
00210         buffer[24]=(uint8_t) (fsc>>24 & 0xFF);
00211         buffer[25]=(uint8_t) (fsc>>16 & 0xFF);
00212         buffer[26]=(uint8_t) (fsc>>8 & 0xFF);
00213         buffer[27]=(uint8_t) (fsc & 0xFF);
00214         if(fsc > SD_HK_ARCH_LAST-SD_HK_ARCH_FIRST+1)
00215         {
00216             start_fsc = start_fsc+1;
00217             buffer[28]=(uint8_t) (start_fsc>>24 & 0xFF);
00218             buffer[29]=(uint8_t) (start_fsc>>16 & 0xFF);
00219             buffer[30]=(uint8_t) (start_fsc>>8 & 0xFF);
00220             buffer[31]=(uint8_t) (start_fsc & 0xFF);
00221         }
00222         i = disk_write(buffer,SD_MNG_SECT);
00223         if(i == 0)
00224         {
00225             FSC_CURRENT[4] = fsc;
00226             FSC_LAST[4] = start_fsc;
00227             return i;
00228         }
00229     }
00230      if(sid==0x05)
00231     {
00232         fsc=(uint32_t)(buffer[32]<<24)+(uint32_t)(buffer[33]<<16)+(uint32_t)(buffer[34]<<8)+(uint32_t)buffer[35];
00233         start_fsc=(uint32_t)(buffer[36]<<24)+(uint32_t)(buffer[37]<<16)+(uint32_t)(buffer[38]<<8)+(uint32_t)buffer[39];
00234         fsc++;
00235         buffer[32]=(uint8_t) (fsc>>24 & 0xFF);
00236         buffer[33]=(uint8_t) (fsc>>16 & 0xFF);
00237         buffer[34]=(uint8_t) (fsc>>8 & 0xFF);
00238         buffer[35]=(uint8_t) (fsc & 0xFF);
00239         if(fsc > LOG_LAST-LOG_FIRST+1)
00240         {
00241             start_fsc = start_fsc+1;
00242             buffer[36]=(uint8_t) (start_fsc>>24 & 0xFF);
00243             buffer[37]=(uint8_t) (start_fsc>>16 & 0xFF);
00244             buffer[38]=(uint8_t) (start_fsc>>8 & 0xFF);
00245             buffer[39]=(uint8_t) (start_fsc & 0xFF);
00246         }
00247         i = disk_write(buffer,SD_MNG_SECT);
00248         if(i == 0)
00249         {
00250             FSC_CURRENT[5] = fsc;
00251             FSC_LAST[5] = start_fsc;
00252             return i;
00253         }
00254     }
00255     return -1;
00256 }
00257 
00258 
00259 int SD_WRITE(uint8_t* buffer,uint32_t fsc,uint8_t sid)
00260 {
00261     uint32_t block_number;
00262     int result = 10;
00263     if(sid==0x01)
00264     {
00265         //block_number=SD_SCP_FIRST+(fsc%(SD_SCP_LAST-SD_SCP_FIRST+1))-1;
00266         block_number=SD_SCP_FIRST+fsc;
00267         result= disk_write(buffer,block_number);
00268         if(result == 0)
00269         {
00270             if(INCREMENT_SD_LIB(sid) == 0)
00271                 SD_LIB_WRITES++;
00272         }
00273         return result;
00274     }
00275    if(sid==0x02)
00276     {
00277         //block_number= SD_SFF_AT_FIRST+(fsc%(SD_SFF_AT_LAST - SD_SFF_AT_FIRST+1))-1;
00278         block_number= SD_SFF_AT_FIRST+fsc;
00279         result= disk_write(buffer,block_number);
00280         if(result == 0)
00281          {
00282             if(INCREMENT_SD_LIB(sid) == 0)
00283                 SD_LIB_WRITES++;
00284         }
00285         return result;
00286     }
00287      if(sid==0x03)
00288     {
00289         //block_number= SD_SFF_BT_FIRST +(fsc%(SD_SFF_BT_LAST - SD_SFF_BT_FIRST +1))-1;
00290         block_number= SD_SFF_BT_FIRST +fsc;
00291         result= disk_write(buffer,block_number);
00292         if(result == 0)
00293         {
00294             if(INCREMENT_SD_LIB(sid) == 0)
00295                 SD_LIB_WRITES++;
00296         }
00297         return result;
00298     }
00299      if(sid==0x04)
00300     {
00301         //block_number=SD_HK_ARCH_FIRST +(fsc%(SD_HK_ARCH_LAST - SD_HK_ARCH_FIRST +1))-1;
00302         block_number=SD_HK_ARCH_FIRST +fsc;
00303         result= disk_write(buffer,block_number);
00304         if(result == 0)
00305         {
00306             if(INCREMENT_SD_LIB(sid) == 0)
00307                 SD_LIB_WRITES++;
00308         }
00309         return result;
00310     }
00311      if(sid==0x05)
00312     {
00313         //block_number= LOG_FIRST +(fsc%(LOG_FIRST - LOG_FIRST +1))-1;
00314         block_number= LOG_FIRST +fsc;
00315         result= disk_write(buffer,block_number);
00316         if(result == 0)
00317         {
00318             if(INCREMENT_SD_LIB(sid) == 0)
00319                 SD_LIB_WRITES++;
00320         }
00321         return result;
00322     }
00323     return 1;
00324 }
00325 
00326 uint8_t SD_READ(uint8_t* buffer,uint32_t fsc,uint8_t sid)
00327 {
00328     FCTN_SD_MNGR();
00329     uint32_t block_number;
00330     int result;
00331     if(SD_SW_EN_DS == 1)
00332         return 0x89;
00333     if(sid==0x01)
00334     {
00335         if(!(FSC_LAST[1]<=fsc && fsc<=FSC_CURRENT[1])){
00336             return 0x86;
00337         }
00338         block_number=SD_SCP_FIRST + fsc;
00339         result= disk_read(buffer,block_number);
00340     }
00341    else if(sid==0x02)
00342     {
00343         if(!(FSC_LAST[2]<=fsc && fsc<=FSC_CURRENT[2])){
00344             return 0x86;
00345         }
00346         block_number=SD_SFF_AT_FIRST + fsc;
00347         result= disk_read(buffer,block_number);
00348     }
00349     else if(sid==0x03)
00350     {
00351         if(!(FSC_LAST[3]<=fsc && fsc<=FSC_CURRENT[3])){
00352             return 0x86;
00353         }
00354         block_number=SD_SFF_BT_FIRST + fsc;
00355         result= disk_read(buffer,block_number);
00356     }
00357     else if(sid==0x04)
00358     {
00359         if(!(FSC_LAST[4]<=fsc && fsc<=FSC_CURRENT[4])){
00360             return 0x86;
00361         }
00362         block_number=SD_HK_ARCH_FIRST + fsc;
00363         result= disk_read(buffer,block_number);
00364     }
00365     else if(sid==0x05)
00366     {
00367         if(!(FSC_LAST[5]<=fsc && fsc<=FSC_CURRENT[5])){
00368             return 0x86;
00369         }
00370         block_number=LOG_FIRST +fsc;
00371         result= disk_read(buffer,block_number);
00372     }
00373     else
00374     {
00375         return 0x02;
00376     }
00377     if(result == 0)
00378         return 0xA0;
00379     else
00380         return 0x88;
00381 }
00382 
00383 
00384 int initialise_card()
00385 {
00386     // Set to 100kHz for initialisation, and clock card with cs_sd = 1
00387     spi.frequency(100000);           // changed on 31 12 2015 to 1 MHz 
00388     cs_sd = 1;
00389     for (int i = 0; i < 16; i++) {
00390         spi.write(0xFF);
00391     }
00392     uint8_t R1_response = cmd(0,0);
00393     gPC.printf("0x%02X",R1_response);
00394     // send CMD0, should return with all zeros except IDLE STATE set (bit 0)
00395     if (R1_response != R1_IDLE_STATE) {
00396         debug("No disk, or could not put SD card in to spi idle state\r\n");
00397         return SDCARD_FAIL;
00398     }
00399     else
00400         gPC.puts("SD Card is in IDLE state\n\r");    
00401 
00402     // send CMD8 to determine whther it is ver 2.x
00403     int r = cmd8();
00404     if (r == R1_IDLE_STATE) {
00405         gPC.puts("Entering V2\r");
00406         int q =  initialise_card_v2();
00407         return q;
00408 
00409     } else if (r == (R1_IDLE_STATE | R1_ILLEGAL_COMMAND)) {
00410         gPC.puts("Entering V1");
00411         return initialise_card_v1();
00412 
00413     } else {
00414         debug("\rNot in idle state after sending CMD8 (not an SD card?)\r\n");
00415         return SDCARD_FAIL;
00416     }
00417 }
00418 
00419 int initialise_card_v1()
00420 {
00421     for (int i = 0; i < SD_COMMAND_TIMEOUT; i++) {
00422         cmd(55, 0);
00423         if (cmd(41, 0) == 0) {
00424             gPC.puts("\rv1 initialization successfull\r\n");
00425             cdv = 512;
00426             debug_if(SD_DBG, "\n\rInit: SEDCARD_V1\n\r");
00427 
00428             return SDCARD_V1;
00429         }
00430     }
00431 
00432     debug("\rTimeout waiting for v1.x card\r\n");
00433     return SDCARD_FAIL;
00434 }
00435 
00436 
00437 int initialise_card_v2()
00438 {
00439     for (int i = 0; i < SD_COMMAND_TIMEOUT; i++) {
00440         wait_ms(50);
00441         cmd58();
00442         cmd(55, 0);
00443         if (cmd(41, 0x40000000) == 0) {
00444             if (DEBUG)
00445                 gPC.puts("\rv2 initialization successfull\r\n");
00446             cmd58();
00447             debug_if(SD_DBG, "\n\rInit: SDCARD_V2\n\r");
00448             cdv = 1;
00449             return SDCARD_V2;
00450         }
00451     }
00452 
00453     debug("\rTimeout waiting for v2.x card\r\n");
00454     return SDCARD_FAIL;
00455 }
00456 
00457 int cmd(int cmd, int arg)
00458 {
00459     cs_sd = 0;
00460 
00461     // send a command
00462     spi.write(0x40 | cmd);
00463     spi.write(arg >> 24);
00464     spi.write(arg >> 16);
00465     spi.write(arg >> 8);
00466     spi.write(arg >> 0);
00467     spi.write(0x95);
00468 
00469     // wait for the repsonse (response[7] == 0)
00470     for (int i = 0; i < SD_COMMAND_TIMEOUT; i++) {
00471         int response = spi.write(0xFF);
00472         if (!(response & 0x80)) {
00473             cs_sd = 1;
00474             spi.write(0xFF);
00475             return response;
00476         }
00477     }
00478     cs_sd = 1;
00479     spi.write(0xFF);
00480     return -1; // timeout
00481 }
00482 
00483 
00484 int cmd58()
00485 {
00486     cs_sd = 0;
00487     int arg = 0;
00488 
00489     // send a command
00490     spi.write(0x40 | 58);
00491     spi.write(arg >> 24);
00492     spi.write(arg >> 16);
00493     spi.write(arg >> 8);
00494     spi.write(arg >> 0);
00495     spi.write(0x95);
00496 
00497     // wait for the repsonse (response[7] == 0)
00498     for (int i = 0; i < SD_COMMAND_TIMEOUT; i++) {
00499         int response = spi.write(0xFF);
00500         if (!(response & 0x80)) {
00501             int ocr = spi.write(0xFF) << 24;
00502             ocr |= spi.write(0xFF) << 16;
00503             ocr |= spi.write(0xFF) << 8;
00504             ocr |= spi.write(0xFF) << 0;
00505             cs_sd = 1;
00506             spi.write(0xFF);
00507             return response;
00508         }
00509     }
00510     cs_sd = 1;
00511     spi.write(0xFF);
00512     return -1; // timeout
00513 }
00514 
00515 
00516 int cmd8()
00517 {
00518     cs_sd = 0;
00519 
00520     // send a command
00521     spi.write(0x40 | 8); // CMD8
00522     spi.write(0x00);     // reserved
00523     spi.write(0x00);     // reserved
00524     spi.write(0x01);     // 3.3v
00525     spi.write(0xAA);     // check pattern
00526     spi.write(0x87);     // crc
00527 
00528     // wait for the repsonse (response[7] == 0)
00529     for (int i = 0; i < SD_COMMAND_TIMEOUT * 1000; i++) {
00530         char response[5];
00531         response[0] = spi.write(0xFF);
00532         if (!(response[0] & 0x80)) {
00533             for (int j = 1; j < 5; j++) {
00534                 response[i] = spi.write(0xFF);
00535             }
00536             cs_sd = 1;
00537             spi.write(0xFF);
00538             return response[0];
00539         }
00540     }
00541     cs_sd = 1;
00542     spi.write(0xFF);
00543     return -1; // timeout
00544 }
00545 
00546 uint64_t sd_sectors()
00547 {
00548     uint32_t c_size, c_size_mult, read_bl_len;
00549     uint32_t block_len, mult, blocknr, capacity;
00550     uint32_t hc_c_size;
00551     uint64_t blocks;
00552 
00553     // CMD9, Response R2 (R1 byte + 16-byte block read)
00554     if (cmdx(9, 0) != 0) {
00555         debug("\rDidn't get a response from the disk\n");
00556         return 0;
00557     }
00558 
00559     uint8_t cs_sdd[16];
00560     if (read(cs_sdd, 16) != 0) {
00561         debug("\rCouldn't read cs_sdd response from disk\n");
00562         return 0;
00563     }
00564 
00565     // cs_sdd_structure : cs_sdd[127:126]
00566     // c_size        : cs_sdd[73:62]
00567     // c_size_mult   : cs_sdd[49:47]
00568     // read_bl_len   : cs_sdd[83:80] - the *maximum* read block length
00569 
00570     int cs_sdd_structure = ext_bits(cs_sdd, 127, 126);
00571 
00572     switch (cs_sdd_structure) {
00573         case 0:
00574             cdv = 512;
00575             c_size = ext_bits(cs_sdd, 73, 62);
00576             c_size_mult = ext_bits(cs_sdd, 49, 47);
00577             read_bl_len = ext_bits(cs_sdd, 83, 80);
00578 
00579             block_len = 1 << read_bl_len;
00580             mult = 1 << (c_size_mult + 2);
00581             blocknr = (c_size + 1) * mult;
00582             capacity = blocknr * block_len;
00583             blocks = capacity / 512;
00584             debug_if(SD_DBG, "\n\rSDCard\n\rc_size: %d \n\rcapacity: %ld \n\rsectors: %lld\n\r", c_size, capacity, blocks);
00585             break;
00586 
00587         case 1:
00588             cdv = 1;
00589             hc_c_size = ext_bits(cs_sdd, 63, 48);
00590             blocks = (hc_c_size+1)*1024;
00591             debug_if(SD_DBG, "\n\rSDHC Card \n\rhc_c_size: %d\n\rcapacity: %lld \n\rsectors: %lld\n\r", hc_c_size, blocks*512, blocks);
00592             break;
00593 
00594         default:
00595             debug("cs_sdD struct unsupported\r\n");
00596             return 0;
00597     };
00598     return blocks;
00599 }
00600 
00601 int cmdx(int cmd, int arg)
00602 {
00603     cs_sd = 0;
00604 
00605     // send a command
00606     spi.write(0x40 | cmd);
00607     spi.write(arg >> 24);
00608     spi.write(arg >> 16);
00609     spi.write(arg >> 8);
00610     spi.write(arg >> 0);
00611     spi.write(0x95);
00612 
00613     // wait for the repsonse (response[7] == 0)
00614     for (int i = 0; i < SD_COMMAND_TIMEOUT; i++) {
00615         int response = spi.write(0xFF);
00616         if (!(response & 0x80)) {
00617             return response;
00618         }
00619     }
00620     cs_sd = 1;
00621     spi.write(0xFF);
00622     return 1; // timeout
00623 }
00624 
00625 static uint32_t ext_bits(unsigned char *data, int msb, int lsb)
00626 {
00627     uint32_t bits = 0;
00628     uint32_t size = 1 + msb - lsb;
00629     for (int i = 0; i < size; i++) {
00630         uint32_t position = lsb + i;
00631         uint32_t byte = 15 - (position >> 3);
00632         uint32_t bit = position & 0x7;
00633         uint32_t value = (data[byte] >> bit) & 1;
00634         bits |= value << i;
00635     }
00636     return bits;
00637 }
00638 
00639 int disk_write(const uint8_t *buffer, uint64_t block_number)
00640 
00641 {
00642     // set write address for single block (CMD24)
00643     if (cmd(24, block_number * cdv) != 0) {
00644         return 1;
00645     }
00646     
00647     uint64_t temp;
00648     int r = write(buffer, 512);
00649     if(r == 0 ){
00650         temp = FCTN_CDMS_RD_RTC();
00651         TIME_LATEST_SD_RD = temp >> 7;
00652     }
00653     return  r;
00654 }
00655 
00656 int write(const uint8_t*buffer, uint32_t length)
00657 {
00658     cs_sd = 0;
00659 
00660     // indicate start of block
00661     spi.write(0xFE);
00662 
00663     // write the data
00664     for (int i = 0; i < length; i++) {
00665         spi.write(buffer[i]);
00666     }
00667 
00668     // write the checksum
00669     spi.write(0xFF);
00670     spi.write(0xFF);
00671 
00672     // check the response token
00673     if ((spi.write(0xFF) & 0x1F) != 0x05) {
00674         cs_sd = 1;
00675         spi.write(0xFF);
00676         return 1;
00677     }
00678 
00679     // wait for write to finish
00680     while (spi.write(0xFF) == 0);
00681 
00682     cs_sd = 1;
00683     spi.write(0xFF);
00684     return 0;
00685 }
00686 
00687 int disk_read(uint8_t *buffer, uint64_t block_number)
00688 {
00689     // set read address for single block (CMD17)
00690     if (cmd(17, block_number * cdv) != 0) {
00691         SD_RD_ERROR = 1;
00692         return 1;
00693     }
00694 
00695     // receive the data
00696     read(buffer, 512);
00697     uint64_t temp = FCTN_CDMS_RD_RTC();
00698     TIME_LATEST_SD_RD = temp >> 7;
00699     return 0;
00700 }
00701 
00702 int read(uint8_t *buffer, uint32_t length)
00703 {
00704     cs_sd = 0;
00705 
00706     // read until start byte (0xFF)
00707     while (spi.write(0xFF) != 0xFE);
00708 
00709     // read data
00710     for (int i = 0; i < length; i++) {
00711         buffer[i] = spi.write(0xFF);
00712     }
00713     spi.write(0xFF); // checksum
00714     spi.write(0xFF);
00715 
00716     cs_sd = 1;
00717     spi.write(0xFF);
00718     return 0;
00719 }
00720 
00721 int disk_erase(int startBlock, int totalBlocks)
00722 {
00723     if(cmd(32, startBlock * cdv) != 0) {
00724         return 1;
00725     }
00726     if (cmd(33, (startBlock+totalBlocks-1) * cdv) != 0) {
00727         return 1;
00728     }
00729     if (cmd(38,0) != 0) {
00730         return 1;
00731     }
00732     
00733     return 0; //normal return
00734 }
00735 
00736 int disk_read_statusbits(uint8_t *buffer)
00737 {
00738     if (cmd(17, 0) != 0) {
00739           SD_RD_ERROR = 1;
00740         return -1;
00741     }
00742 
00743     // receive the data
00744     return read(buffer,64);
00745 }