diskio.c

00001 /*-----------------------------------------------------------------------*/
00002 /* Low level disk I/O module skeleton for FatFs     (C)ChaN, 2007        */
00003 /*-----------------------------------------------------------------------*/
00004 /* This is a stub disk I/O module that acts as front end of the existing */
00005 /* disk I/O modules and attach it to FatFs module with common interface. */
00006 /*-----------------------------------------------------------------------*/
00007 
00008 #include "diskio.h"
00009 #include "sowb.h"
00010 #include "user.h"
00011 #include "gpio.h"
00012 #include "gps.h"
00013 #include "ssp0.h"
00014 
00015 #define R1_IDLE_STATE           (1 << 0)
00016 #define R1_ERASE_RESET          (1 << 1)
00017 #define R1_ILLEGAL_COMMAND      (1 << 2)
00018 #define R1_COM_CRC_ERROR        (1 << 3)
00019 #define R1_ERASE_SEQUENCE_ERROR (1 << 4)
00020 #define R1_ADDRESS_ERROR        (1 << 5)
00021 #define R1_PARAMETER_ERROR      (1 << 6)
00022 
00023 //******************************************************************************************************************
00024 // MBED SPI/CS Select functions.... Modify for your layout.
00025 //**************************************************************************************
00026 
00027 //SPI _spi(p5, p6, p7); // mosi, miso, sclk
00028 //DigitalOut _cs(p8);
00029 //DigitalOut P20(p20);
00030 //SPI * _spi;
00031 
00032 
00033 //******************************************************************************************************************
00034 // Low Level Sector Access Function Prototypes('C' Castrated versions of Simon Ford's C++ MBED SDFileSystem class
00035 //******************************************************************************************************************
00036 int _cmd(int cmd, int arg);
00037 int _cmd8(void);
00038 int _cmdR2(int cmd, int arg);
00039 int _read(BYTE *buffer, int length);
00040 int _write(BYTE *buffer, int length);
00041 int ext_bits(BYTE *data, int msb, int lsb);
00042 int _sd_sectors();
00043 int _sectors;
00044 
00045 #define SD_COMMAND_TIMEOUT 5000
00046 
00047 void deassert_cs(void) {
00048     SDCARD_CS_DEASSERT; //_cs = 1;
00049 }
00050 
00051 //******************************************************************************************************************
00052 // Sector Access functions for CHAN FatFs 
00053 //******************************************************************************************************************
00054 
00055 DRESULT disk_ioctl (
00056     BYTE drv,        /* Physical drive nmuber (0..) */
00057     BYTE ctrl,        /* Control code */
00058     void *buff        /* Buffer to send/receive control data */
00059 )
00060 {
00061     DRESULT res;
00062 
00063     switch(ctrl)
00064     {
00065         case CTRL_SYNC:
00066              res = RES_OK;
00067         break;
00068     
00069         case GET_SECTOR_SIZE:
00070               res = RES_OK;
00071             *(WORD *)buff = 512;
00072         break;
00073         
00074         case GET_SECTOR_COUNT:
00075             res = RES_OK;
00076            *(DWORD *)buff = (WORD)_sd_sectors();
00077         break;
00078         
00079         case GET_BLOCK_SIZE:
00080          res = RES_OK;
00081           *(DWORD *)buff = 1;
00082         break;
00083         
00084         default:
00085         res = RES_OK;
00086         break;
00087     }
00088     return res;
00089 }
00090 
00091 int _cmd58() {
00092     SDCARD_CS_ASSERT; //_cs = 0; 
00093     int arg = 0;
00094     
00095     /* Request use of SSP0. */
00096     while(!SSP0_request()) WHILE_WAITING_DO_PROCESS_FUNCTIONS;
00097     
00098     // send a command
00099     SSP0_FLUSH_RX_FIFO;
00100     SSP0_WRITE_BYTE(0x40 | 58);
00101     SSP0_WRITE_BYTE(arg >> 24);
00102     SSP0_WRITE_BYTE(arg >> 16);
00103     SSP0_WRITE_BYTE(arg >> 8);
00104     SSP0_WRITE_BYTE(arg >> 0);
00105     SSP0_WRITE_BYTE(0x95);
00106     while(SSP0_IS_BUSY);
00107     SSP0_FLUSH_RX_FIFO;
00108     
00109     //_spi->write(0x40 | 58);
00110     //_spi->write(arg >> 24);
00111     //_spi->write(arg >> 16);
00112     //_spi->write(arg >> 8);
00113     //_spi->write(arg >> 0);
00114     //_spi->write(0x95);
00115 
00116     // wait for the repsonse (response[7] == 0)
00117     SSP0_WRITE_BYTE(0xFF);
00118     while(SSP0_IS_BUSY);
00119     for(int i = 0; i < SD_COMMAND_TIMEOUT; i++) {
00120         int response = LPC_SSP0->DR;
00121         if(!(response & 0x80)) {
00122             SSP0_WRITE_BYTE(0xFF);
00123             while(SSP0_IS_BUSY);
00124             int ocr = LPC_SSP0->DR << 24;
00125             SSP0_WRITE_BYTE(0xFF);
00126             while(SSP0_IS_BUSY);
00127             ocr |= LPC_SSP0->DR  << 16;
00128             SSP0_WRITE_BYTE(0xFF);
00129             while(SSP0_IS_BUSY);
00130             ocr |= LPC_SSP0->DR  << 8;
00131             SSP0_WRITE_BYTE(0xFF);
00132             while(SSP0_IS_BUSY);
00133             ocr |= LPC_SSP0->DR << 0;
00134 //            printf("OCR = 0x%08X\n", ocr);
00135             SDCARD_CS_DEASSERT; //_cs = 1;
00136             SSP0_WRITE_BYTE(0xFF);
00137             while(SSP0_IS_BUSY);
00138             SSP0_FLUSH_RX_FIFO;
00139             // _spi->write(0xFF);
00140             SSP0_release();
00141             return response;
00142         }
00143         SSP0_WRITE_BYTE(0xFF);
00144         while(SSP0_IS_BUSY);
00145     }
00146     SDCARD_CS_DEASSERT;
00147     SSP0_WRITE_BYTE(0xFF);
00148     while(SSP0_IS_BUSY);
00149     SSP0_release();
00150     return -1; // timeout
00151 }
00152 
00153 int initialise_card_v1() {
00154     
00155     /* Request use of SSP0. */
00156     while(!SSP0_request()) WHILE_WAITING_DO_PROCESS_FUNCTIONS;
00157     
00158     for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
00159         _cmd(55, 0); 
00160         if(_cmd(41, 0) == 0) { 
00161             SSP0_release();
00162             return 0; //SDCARD_V1;
00163         }
00164     }
00165 
00166     //fprintf(stderr, "Timeout waiting for v1.x card\n");
00167     SSP0_release();
00168     return STA_NOINIT;
00169 }
00170 
00171 int initialise_card_v2() {
00172     int c41, c58, r;
00173     
00174     /* Request use of SSP0. */
00175     while(!SSP0_request()) WHILE_WAITING_DO_PROCESS_FUNCTIONS;
00176     
00177     for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
00178         _cmd(55, 0);
00179         c41 = _cmd(41, 0);
00180         if(c41 == 0) { 
00181             SSP0_release();
00182             c58 = _cmd58();
00183             //fprintf(stderr, "C41 returned %02x and C58 returned %02x\r\n", c41, c58);
00184             return 0; //SDCARD_V2;
00185         }        
00186     }
00187     
00188     
00189     //fprintf(stderr, "Timeout waiting for v2.x card response=0x%04x\r\n", c41);
00190     SSP0_release();
00191     return STA_NOINIT;
00192 }
00193 
00194 DSTATUS disk_initialize(BYTE Drive) {
00195     int i, cmd8_r;
00196     uint32_t cpsr = LPC_SSP0->CPSR;
00197     
00198     //_spi = new SPI(p5, p6, p7);
00199     
00200     /* Request use of SSP0. */
00201     while(!SSP0_request()) WHILE_WAITING_DO_PROCESS_FUNCTIONS;
00202     
00203     //_spi->frequency(100000); // Set to 100kHz for initialisation
00204     LPC_SSP0->CPSR = 0x64;
00205     SDCARD_CS_ASSERT; //_cs = 1;
00206     
00207     // Initialise the card by clocking it a bit (cs = 1)
00208     for(int i=0; i < 16; i++) {   
00209         SSP0_WRITE_BYTE(0xFF);
00210     }
00211     while(SSP0_IS_BUSY);
00212     SSP0_FLUSH_RX_FIFO;
00213     
00214 
00215     
00216     // send CMD0, should return with all zeros except IDLE STATE set (bit 0)
00217     if(_cmd(0, 0) != 0x01) { 
00218         //fprintf(stderr, "Not in idle state\n");
00219         SSP0_release();
00220         return STA_NOINIT;
00221     }
00222     
00223     for(i = 0; i < SD_COMMAND_TIMEOUT; i++) {
00224         cmd8_r = _cmd8();
00225         if (cmd8_r == 0 || cmd8_r == 1 || cmd8_r == R1_ILLEGAL_COMMAND) break;
00226         if (cmd8_r == 0 || cmd8_r == R1_ILLEGAL_COMMAND) break;
00227     }
00228     
00229     if ( (cmd8_r & R1_ILLEGAL_COMMAND) != 0) {
00230         //fprintf(stderr, "V1 %d\r\n", cmd8_r);
00231         SSP0_release();
00232         return initialise_card_v1();
00233     }
00234     
00235     if (cmd8_r == 0 || cmd8_r == 1) {
00236         //fprintf(stderr, "V2 %d\r\n", cmd8_r);
00237         SSP0_release();
00238         return initialise_card_v2();
00239     }
00240     
00241     
00242     //fprintf(stderr, "Not in idle state after sending CMD8 (not an SD card?) response = %d\r\n", cmd8_r);
00243     SSP0_release();
00244     return STA_NOINIT;
00245     
00246     // ACMD41 to give host capacity support (repeat until not busy)
00247     // ACMD41 is application specific command, so we send APP_CMD (CMD55) beforehand
00248     for(int i=0;; i++) {
00249         _cmd(55, 0); 
00250         int response = _cmd(41, 0);
00251         if(response == 0) { 
00252             break;
00253         } else if(i > SD_COMMAND_TIMEOUT) {
00254             //fprintf(stderr, "Timeout waiting for card\n");
00255             SSP0_release();
00256             return STA_NOINIT;
00257         }    
00258     }
00259 
00260     _sectors = _sd_sectors();
00261 
00262     // Set block length to 512 (CMD16)
00263     if(_cmd(16, 512) != 0) {
00264         //fprintf(stderr, "Set block timeout\n");
00265         SSP0_release();
00266         return STA_NOINIT;
00267     }
00268     
00269     LPC_SSP0->CPSR = cpsr;    
00270     SSP0_release();
00271     //_spi->frequency(10000000); // Set to 10MHz for data transfer
00272     return 0;
00273 }
00274 
00275 DRESULT disk_write(BYTE Drive,const BYTE * Buffer, DWORD SectorNumber, BYTE SectorCount)
00276 {
00277     BYTE i;
00278     
00279     BYTE * MyBufOut = (BYTE *)Buffer;
00280     
00281     /* Request use of SSP0. */
00282     while(!SSP0_request()) WHILE_WAITING_DO_PROCESS_FUNCTIONS;
00283     
00284     for(i=0;i<SectorCount;i++)
00285     {
00286         // set write address for single block (CMD24)
00287         if(_cmd(24, (SectorNumber + i) * 512 ) != 0) {
00288             SSP0_release();
00289             return RES_ERROR;
00290         }
00291 
00292         // send the data block
00293         _write(MyBufOut, 512);    
00294         
00295         MyBufOut+=512;
00296     }
00297     
00298     SSP0_release();
00299     return RES_OK;    
00300 }
00301 
00302 DRESULT disk_read(BYTE Drive, BYTE * Buffer,DWORD SectorNumber, BYTE SectorCount)
00303 {        
00304     BYTE i;
00305     
00306     /* Request use of SSP0. */
00307     while(!SSP0_request()) WHILE_WAITING_DO_PROCESS_FUNCTIONS;
00308     
00309     for(i=0;i<SectorCount;i++)
00310     {
00311         // set read address for single block (CMD17)
00312         if(_cmd(17, (SectorNumber+i) * 512) != 0)
00313         {
00314             SSP0_release();
00315             return RES_ERROR;
00316         }
00317         // receive the data
00318         _read(Buffer, 512);
00319        
00320         Buffer+=512;
00321     }
00322     
00323     SSP0_release(); 
00324     return RES_OK;
00325 }
00326 
00327 
00328 extern "C" DWORD get_fattime(void) {
00329     GPS_TIME the_time;
00330     
00331     gps_get_time(&the_time);
00332     
00333     uint32_t year   = (the_time.year - 1980) << 25;
00334     uint32_t month  = the_time.month << 21;
00335     uint32_t day    = the_time.day << 16;
00336     uint32_t hour   = the_time.hour << 11;
00337     uint32_t minute = the_time.minute << 5;
00338     uint32_t second = (the_time.second / 2) &0xF;
00339     return (DWORD)(year | month | day | hour | minute | second);
00340 }
00341 
00342 DSTATUS disk_status(BYTE Drive)
00343 {
00344     return 0;
00345 }
00346 
00347 //**************************************************************************************
00348 // Low Level Sector Access Functions (Castrated versions of Simon Fords C++ MBED class
00349 //**************************************************************************************
00350 
00351 int _cmd(int cmd, int arg) {
00352     volatile int delay;
00353     
00354     for (delay = 500; delay; delay--);
00355     SDCARD_CS_ASSERT; //_cs = 0; 
00356     for (delay = 500; delay; delay--);
00357     
00358     // send a command
00359     SSP0_FLUSH_RX_FIFO;
00360     SSP0_WRITE_BYTE(0x40 | cmd);
00361     SSP0_WRITE_BYTE(arg >> 24);
00362     SSP0_WRITE_BYTE(arg >> 16);
00363     SSP0_WRITE_BYTE(arg >> 8);
00364     SSP0_WRITE_BYTE(arg >> 0);
00365     SSP0_WRITE_BYTE(0x95);
00366     while(SSP0_IS_BUSY);
00367     SSP0_FLUSH_RX_FIFO;
00368     
00369     //_spi->write(0x40 | cmd);
00370     //_spi->write(arg >> 24);
00371     //_spi->write(arg >> 16);
00372     //_spi->write(arg >> 8);
00373     //_spi->write(arg >> 0);
00374     //_spi->write(0x95);
00375 
00376     // wait for the repsonse (response[7] == 0)
00377     SSP0_WRITE_BYTE(0xFF);
00378     while(SSP0_IS_BUSY);
00379     for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
00380         int response = LPC_SSP0->DR;
00381         if(!(response & 0x80)) {
00382             SDCARD_CS_DEASSERT; //_cs = 1;
00383             return response;
00384         }
00385         SSP0_WRITE_BYTE(0xFF);
00386         while(SSP0_IS_BUSY);
00387     }
00388     
00389     SDCARD_CS_ASSERT; //_cs = 1;
00390     return -1; // timeout
00391 }
00392 
00393 int _cmdR2(int cmd, int arg) {
00394     int response;
00395     
00396     SDCARD_CS_ASSERT; //_cs = 0; 
00397     
00398     // send a command
00399     SSP0_FLUSH_RX_FIFO;
00400     SSP0_WRITE_BYTE(0x40 | cmd);
00401     SSP0_WRITE_BYTE(arg >> 24);
00402     SSP0_WRITE_BYTE(arg >> 16);
00403     SSP0_WRITE_BYTE(arg >> 8);
00404     SSP0_WRITE_BYTE(arg >> 0);
00405     SSP0_WRITE_BYTE(0x95);
00406     SSP0_WRITE_BYTE(0xFF);
00407     while(SSP0_IS_BUSY);
00408     SSP0_FLUSH_RX_FIFO;
00409     
00410     //_spi->write(0x40 | cmd);
00411     //_spi->write(arg >> 24);
00412     //_spi->write(arg >> 16);
00413     //_spi->write(arg >> 8);
00414     //_spi->write(arg >> 0);
00415     //_spi->write(0x95);    
00416     //_spi->write(0xFF);
00417     
00418     // wait for the repsonse (response[7] == 0)
00419     //for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
00420         response = 0;
00421         SSP0_WRITE_BYTE(0x00);
00422         while(SSP0_IS_BUSY);
00423         response = (LPC_SSP0->DR & 0xFF);
00424         response = (response << 8) & 0xFF00;
00425         SSP0_WRITE_BYTE(0x00);
00426         while(SSP0_IS_BUSY);
00427         response |= (LPC_SSP0->DR & 0xFF);
00428         SDCARD_CS_DEASSERT; //_cs = 1;
00429         return response;
00430     //}
00431     SDCARD_CS_DEASSERT; //_cs = 1;
00432     return -1; // timeout
00433 }
00434 
00435 int _cmd8(void) {
00436     SDCARD_CS_ASSERT; //_cs = 0; 
00437     
00438     // send a command
00439     SSP0_FLUSH_RX_FIFO;
00440     SSP0_WRITE_BYTE(0x40 | 8);
00441     SSP0_WRITE_BYTE(00);
00442     SSP0_WRITE_BYTE(00);
00443     SSP0_WRITE_BYTE(01);
00444     SSP0_WRITE_BYTE(0xAA);
00445     SSP0_WRITE_BYTE(0x87);
00446     while(SSP0_IS_BUSY);
00447     SSP0_FLUSH_RX_FIFO;
00448     
00449     //_spi->write(0x40 | 8); // CMD8
00450     //_spi->write(0x00);     // reserved
00451     //_spi->write(0x00);     // reserved
00452     //_spi->write(0x01);     // 3.3v
00453     //_spi->write(0xAA);     // check pattern
00454     //_spi->write(0x87);     // crc
00455 
00456     // wait for the repsonse (response[7] == 0)
00457     SSP0_WRITE_BYTE(0xFF);
00458     while(SSP0_IS_BUSY);
00459     for(int i=0; i<SD_COMMAND_TIMEOUT * 1000; i++) {
00460         char response[5];
00461         response[0] = LPC_SSP0->DR;
00462         SSP0_WRITE_BYTE(0xFF);
00463         while(SSP0_IS_BUSY);
00464         for(int j=1; j<5; j++) {
00465             response[i] = LPC_SSP0->DR;
00466             SSP0_WRITE_BYTE(0xFF);
00467             while(SSP0_IS_BUSY);
00468         }
00469         SDCARD_CS_DEASSERT; //_cs = 1;
00470         SSP0_WRITE_BYTE(0xFF);
00471         while(SSP0_IS_BUSY);
00472         SSP0_FLUSH_RX_FIFO;
00473         return response[0];
00474     }
00475     
00476     SDCARD_CS_DEASSERT; //_cs = 1;
00477     SSP0_WRITE_BYTE(0xFF);
00478     while(SSP0_IS_BUSY);
00479     SSP0_FLUSH_RX_FIFO;
00480     //_spi->write(0xFF);
00481     return -1; // timeout
00482 }
00483 
00484 int _cmd8original(void) {
00485     SDCARD_CS_ASSERT; //_cs = 0; 
00486     
00487     // send a command
00488     SSP0_FLUSH_RX_FIFO;
00489     SSP0_WRITE_BYTE(0x40 | 8);
00490     SSP0_WRITE_BYTE(00);
00491     SSP0_WRITE_BYTE(00);
00492     SSP0_WRITE_BYTE(01);
00493     SSP0_WRITE_BYTE(0xAA);
00494     SSP0_WRITE_BYTE(0x87);
00495     while(SSP0_IS_BUSY);
00496     SSP0_FLUSH_RX_FIFO;
00497     
00498     //_spi->write(0x40 | 8); // CMD8
00499     //_spi->write(0x00);     // reserved
00500     //_spi->write(0x00);     // reserved
00501     //_spi->write(0x01);     // 3.3v
00502     //_spi->write(0xAA);     // check pattern
00503     //_spi->write(0x87);     // crc
00504 
00505     // wait for the repsonse (response[7] == 0)
00506     SSP0_WRITE_BYTE(0xFF);
00507     while(SSP0_IS_BUSY);
00508     for(int i=0; i<SD_COMMAND_TIMEOUT * 1000; i++) {
00509         char response[5];
00510         response[0] = LPC_SSP0->DR;
00511         if(!(response[0] & 0x80)) {
00512             //fprintf(stderr, "RS = %d\r\n", response[0]);
00513             SSP0_WRITE_BYTE(0xFF);
00514             while(SSP0_IS_BUSY);
00515             for(int j=1; j<5; j++) {
00516                 response[i] = LPC_SSP0->DR;
00517                 SSP0_WRITE_BYTE(0xFF);
00518                 while(SSP0_IS_BUSY);
00519             }
00520             SDCARD_CS_DEASSERT; //_cs = 1;
00521             SSP0_WRITE_BYTE(0xFF);
00522             while(SSP0_IS_BUSY);
00523             //_spi->write(0xFF);
00524             return response[0];
00525         }
00526     }
00527     
00528     SDCARD_CS_DEASSERT; //_cs = 1;
00529     SSP0_WRITE_BYTE(0xFF);
00530     while(SSP0_IS_BUSY);
00531     SSP0_FLUSH_RX_FIFO;
00532     //_spi->write(0xFF);
00533     return -1; // timeout
00534 }
00535 
00536 int _read(BYTE *buffer, int length) {
00537     SDCARD_CS_ASSERT; //_cs = 0;
00538 
00539     // read until start byte (0xFF)
00540     SSP0_FLUSH_RX_FIFO;
00541     SSP0_WRITE_BYTE(0xFF);
00542     while(SSP0_IS_BUSY);
00543     while(LPC_SSP0->DR != 0xFE) {
00544         SSP0_WRITE_BYTE(0xFF);
00545         while(SSP0_IS_BUSY);
00546     }
00547 
00548     // read data
00549     SSP0_FLUSH_RX_FIFO;
00550     SSP0_WRITE_BYTE(0xFF);
00551     while(SSP0_IS_BUSY);
00552     for(int i=0; i<length; i++) {
00553         buffer[i] = LPC_SSP0->DR;
00554         SSP0_WRITE_BYTE(0xFF);
00555         while(SSP0_IS_BUSY);
00556     }
00557     SSP0_WRITE_BYTE(0xFF);
00558     SSP0_WRITE_BYTE(0xFF);
00559     while(SSP0_IS_BUSY);
00560     SSP0_FLUSH_RX_FIFO;
00561     //_spi->write(0xFF); // checksum
00562     //_spi->write(0xFF);
00563 
00564     SDCARD_CS_DEASSERT; //_cs = 1;    
00565     return 0;
00566 }
00567 
00568 int _write(BYTE *buffer, int length) {
00569     SDCARD_CS_ASSERT; //_cs = 0;
00570     
00571     // indicate start of block
00572     SSP0_WRITE_BYTE(0xFE);
00573     while(SSP0_IS_BUSY);
00574     //_spi->write(0xFE);
00575     
00576     // write the data
00577     for(int i=0; i<length; i++) {
00578         SSP0_WRITE_BYTE(buffer[i]);
00579         while(SSP0_IS_BUSY);
00580         //_spi->write(buffer[i]);
00581     }
00582     
00583     // write the checksum
00584     SSP0_WRITE_BYTE(0xFF);
00585     SSP0_WRITE_BYTE(0xFF);
00586     while(SSP0_IS_BUSY);
00587     //_spi->write(0xFF); 
00588     //_spi->write(0xFF);
00589 
00590     // check the repsonse token
00591     SSP0_FLUSH_RX_FIFO;
00592     SSP0_WRITE_BYTE(0xFF);
00593     while(SSP0_IS_BUSY);
00594     if((LPC_SSP0->DR & 0x1F) != 0x05) {
00595         SDCARD_CS_DEASSERT; //_cs = 1; 
00596         return 1;
00597     }
00598 
00599     // wait for write to finish
00600     SSP0_FLUSH_RX_FIFO;
00601     SSP0_WRITE_BYTE(0xFF);
00602     while(SSP0_IS_BUSY);
00603     while(LPC_SSP0->DR == 0) {
00604         SSP0_WRITE_BYTE(0xFF);
00605         while(SSP0_IS_BUSY);
00606     }
00607 
00608     SDCARD_CS_DEASSERT; //_cs = 1; 
00609     return 0;
00610 }
00611 
00612 int ext_bits(BYTE *data, int msb, int lsb) {
00613     int bits = 0;
00614     int size = 1 + msb - lsb; 
00615     for(int i=0; i<size; i++) {
00616         int position = lsb + i;
00617         int byte = 15 - (position >> 3);
00618         int bit = position & 0x7;
00619         int value = (data[byte] >> bit) & 1;
00620         bits |= value << i;
00621     }
00622     return bits;
00623 }
00624 
00625 int _sd_sectors() {
00626 
00627     // CMD9, Response R2 (R1 byte + 16-byte block read)
00628     if(_cmd(9, 0) != 0) {
00629         //fprintf(stderr, "Didn't get a response from the disk\n");
00630         return 0;
00631     }
00632     
00633     BYTE csd[16];    
00634     if(_read(csd, 16) != 0) {
00635         //fprintf(stderr, "Couldn't read csd response from disk\n");
00636         return 0;
00637     }
00638 
00639     // csd_structure : csd[127:126]
00640     // c_size        : csd[73:62]
00641     // c_size_mult   : csd[49:47]
00642     // read_bl_len   : csd[83:80] 
00643 
00644     int csd_structure = ext_bits(csd, 127, 126);
00645     int c_size = ext_bits(csd, 73, 62);
00646     int c_size_mult = ext_bits(csd, 49, 47);
00647     int read_bl_len = ext_bits(csd, 83, 80);
00648     
00649     if(csd_structure != 0) {
00650         //fprintf(stderr, "This disk tastes funny! I only know about type 0 CSD structures");
00651         return 0;
00652     }
00653                             
00654     int blocks = (c_size + 1) * (1 << (c_size_mult + 2));
00655     int block_size = 1 << read_bl_len;
00656 
00657     if(block_size != 512) {
00658         //fprintf(stderr, "This disk tastes funny! I only like 512 byte blocks (%d)\r\n",block_size);
00659         return 0;
00660     }
00661     
00662     return blocks;
00663 }
00664