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