src/libs/USBCDCMSC/USBCDCMSC.cpp

   1 /* Copyright (c) 2010-2011 mbed.org, MIT License
   2 *
   3 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
   4 * and associated documentation files (the "Software"), to deal in the Software without
   5 * restriction, including without limitation the rights to use, copy, modify, merge, publish,
   6 * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
   7 * Software is furnished to do so, subject to the following conditions:
   8 *
   9 * The above copyright notice and this permission notice shall be included in all copies or
  10 * substantial portions of the Software.
  11 *
  12 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
  13 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  14 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
  15 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  16 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  17 */
  18
  19 #include "stdint.h"
  20 #include "USBCDCMSC.h"
  21 #include "USBBusInterface.h"
  22 #include "libs/SerialMessage.h"
  23
  24 #include "libs/RingBuffer.h"
  25
  26 static uint8_t cdc_line_coding[7]= {0x80, 0x25, 0x00, 0x00, 0x00, 0x00, 0x08};
  27
  28 #define DEFAULT_CONFIGURATION (1)
  29
  30 #define CDC_SET_LINE_CODING        0x20
  31 #define CDC_GET_LINE_CODING        0x21
  32 #define CDC_SET_CONTROL_LINE_STATE 0x22
  33 #define CDC_SEND_BREAK             0x23
  34
  35 #define MAX_CDC_REPORT_SIZE MAX_PACKET_SIZE_EPBULK
  36
  37
  38
  39 #define DISK_OK         0x00
  40 #define NO_INIT         0x01
  41 #define NO_DISK         0x02
  42 #define WRITE_PROTECT   0x04
  43
  44 #define CBW_Signature   0x43425355
  45 #define CSW_Signature   0x53425355
  46
  47 // SCSI Commands
  48 #define TEST_UNIT_READY            0x00
  49 #define REQUEST_SENSE              0x03
  50 #define FORMAT_UNIT                0x04
  51 #define INQUIRY                    0x12
  52 #define MODE_SELECT6               0x15
  53 #define MODE_SENSE6                0x1A
  54 #define START_STOP_UNIT            0x1B
  55 #define MEDIA_REMOVAL              0x1E
  56 #define READ_FORMAT_CAPACITIES     0x23
  57 #define READ_CAPACITY              0x25
  58 #define READ10                     0x28
  59 #define WRITE10                    0x2A
  60 #define VERIFY10                   0x2F
  61 #define READ12                     0xA8
  62 #define WRITE12                    0xAA
  63 #define MODE_SELECT10              0x55
  64 #define MODE_SENSE10               0x5A
  65
  66 // MSC class specific requests
  67 #define MSC_REQUEST_RESET          0xFF
  68 #define MSC_REQUEST_GET_MAX_LUN    0xFE
  69
  70 #define DEFAULT_CONFIGURATION (1)
  71
  72 // max packet size
  73 #define MAX_PACKET  MAX_PACKET_SIZE_EPBULK
  74
  75 // CSW Status
  76 enum Status {
  77     CSW_PASSED,
  78     CSW_FAILED,
  79     CSW_ERROR,
  80 };
  81
  82
  83 USBCDCMSC::USBCDCMSC(SDFileSystem *sd, uint16_t vendor_id, uint16_t product_id, uint16_t product_release): USBDevice(vendor_id, product_id, product_release), cdcbuf(128), _sd(sd) {
  84     cdcbreak = 0;
  85     _status = NO_INIT;
  86     connect();
  87 //    USBDevice::connect();
  88     USBHAL::connect();
  89 }
  90
  91 bool USBCDCMSC::USBCallback_request(void) {
  92     /* Called in ISR context */
  93
  94     bool success = false;
  95     CONTROL_TRANSFER * transfer = getTransferPtr();
  96     static uint8_t maxLUN[1] = {0};
  97
  98     /* Process class-specific requests */
  99
 100     if (transfer->setup.bmRequestType.Type == CLASS_TYPE) {
 101         switch (transfer->setup.bRequest) {
 102             case CDC_GET_LINE_CODING:
 103                 transfer->remaining = 7;
 104                 transfer->ptr = cdc_line_coding;
 105                 transfer->direction = DEVICE_TO_HOST;
 106                 success = true;
 107                 break;
 108             case CDC_SET_LINE_CODING:
 109                 transfer->remaining = 7;
 110                 success = true;
 111                 break;
 112             case CDC_SET_CONTROL_LINE_STATE:
 113                 success = true;
 114                 break;
 115             case CDC_SEND_BREAK:
 116                 cdcbreak = 1;
 117                 success = true;
 118                 break;
 119             case MSC_REQUEST_RESET:
 120                 reset();
 121                 success = true;
 122                 break;
 123             case MSC_REQUEST_GET_MAX_LUN:
 124                 transfer->remaining = 1;
 125                 transfer->ptr = maxLUN;
 126                 transfer->direction = DEVICE_TO_HOST;
 127                 success = true;
 128                 break;
 129             default:
 130                 break;
 131         }
 132     }
 133
 134     return success;
 135 }
 136
 137
 138 // Called in ISR context
 139 // Set configuration. Return false if the
 140 // configuration is not supported.
 141 bool USBCDCMSC::USBCallback_setConfiguration(uint8_t configuration) {
 142     if (configuration != DEFAULT_CONFIGURATION) {
 143         return false;
 144     }
 145
 146     // Configure endpoints > 0
 147     addEndpoint(EPINT_IN, MAX_PACKET_SIZE_EPINT);
 148     addEndpoint(EPBULK_IN, MAX_PACKET_SIZE_EPBULK);
 149     addEndpoint(EPBULK_OUT, MAX_PACKET_SIZE_EPBULK);
 150
 151     // Configure endpoints > 0
 152     addEndpoint(MSDBULK_IN, MAX_PACKET_SIZE_MSDBULK);
 153     addEndpoint(MSDBULK_OUT, MAX_PACKET_SIZE_MSDBULK);
 154
 155     // We activate the endpoint to be able to recceive data
 156     readStart(EPBULK_OUT, MAX_PACKET_SIZE_EPBULK);
 157
 158     //activate readings
 159     readStart(MSDBULK_OUT, MAX_PACKET_SIZE_MSDBULK);
 160     return true;
 161 }
 162
 163 bool USBCDCMSC::send(uint8_t * buffer, uint16_t size) {
 164     return USBDevice::write(EPBULK_IN, buffer, size, MAX_CDC_REPORT_SIZE);
 165 }
 166
 167 bool USBCDCMSC::readEP(uint8_t * buffer, uint16_t * size) {
 168     if (!USBDevice::readEP(EPBULK_OUT, buffer, size, MAX_CDC_REPORT_SIZE))
 169         return false;
 170     if (!readStart(EPBULK_OUT, MAX_CDC_REPORT_SIZE))
 171         return false;
 172     return true;
 173 }
 174
 175 bool USBCDCMSC::readEP_NB(uint8_t * buffer, uint16_t * size) {
 176     if (!USBDevice::readEP_NB(EPBULK_OUT, buffer, size, MAX_CDC_REPORT_SIZE))
 177         return false;
 178     if (!readStart(EPBULK_OUT, MAX_CDC_REPORT_SIZE))
 179         return false;
 180     return true;
 181 }
 182
 183
 184 uint8_t * USBCDCMSC::deviceDesc() {
 185     static uint8_t deviceDescriptor[] = {
 186         18,                   // bLength
 187         1,                    // bDescriptorType
 188         0x10, 0x01,           // bcdUSB
 189         0xef,                    // bDeviceClass
 190         0x02,                    // bDeviceSubClass
 191         0x01,                    // bDeviceProtocol
 192         MAX_PACKET_SIZE_EP0,  // bMaxPacketSize0
 193         LSB(VENDOR_ID), MSB(VENDOR_ID),  // idVendor
 194         LSB(PRODUCT_ID), MSB(PRODUCT_ID),// idProduct
 195         0x00, 0x01,           // bcdDevice
 196         1,                    // iManufacturer
 197         2,                    // iProduct
 198         3,                    // iSerialNumber
 199         1                     // bNumConfigurations
 200     };
 201     return deviceDescriptor;
 202 }
 203
 204 uint8_t * USBCDCMSC::stringIinterfaceDesc() {
 205     static uint8_t stringIinterfaceDescriptor[] = {
 206         0x0e,
 207         STRING_DESCRIPTOR,
 208         'C',0,'D',0,'C',0,'M',0,'S',0,'C',0,
 209     };
 210     return stringIinterfaceDescriptor;
 211 }
 212
 213 uint8_t * USBCDCMSC::stringIproductDesc() {
 214     static uint8_t stringIproductDescriptor[] = {
 215         0x1c,
 216         STRING_DESCRIPTOR,
 217         'C',0,'D',0,'C',0,'M',0,'S',0,'C',0,' ',0,'D',0,'E',0,'V',0,'I',0,'C',0,'E',0
 218     };
 219     return stringIproductDescriptor;
 220 }
 221
 222
 223 uint8_t * USBCDCMSC::configurationDesc() {
 224     static uint8_t configDescriptor[] = {
 225         9,                      // bLength;
 226         2,                      // bDescriptorType;
 227         LSB(0x62),              // wTotalLength
 228         MSB(0x62),
 229         3,                      // bNumInterfaces
 230         1,                      // bConfigurationValue
 231         0,                      // iConfiguration
 232         0xc0,                   // bmAttributes
 233         50,                     // bMaxPower
 234
 235         // IAD
 236 //        0x08, 0x0B, 0x00, 0x02, 0x02, 0x00, 0x00, 0x00,
 237         0x08, 0x0B, 0x00, 0x02, 0x02, 0x02, 0x01, 0x00,
 238
 239         // interface descriptor, USB spec 9.6.5, page 267-269, Table 9-12
 240         9,                      // bLength
 241         4,                      // bDescriptorType
 242         0,                      // bInterfaceNumber
 243         0,                      // bAlternateSetting
 244         1,                      // bNumEndpoints
 245         0x02,                   // bInterfaceClass
 246         0x02,                   // bInterfaceSubClass
 247         0x01,                   // bInterfaceProtocol
 248         0,                      // iInterface
 249
 250         // CDC Header Functional Descriptor, CDC Spec 5.2.3.1, Table 26
 251         5,                      // bFunctionLength
 252         0x24,                   // bDescriptorType
 253         0x00,                   // bDescriptorSubtype
 254         0x10, 0x01,             // bcdCDC
 255
 256         // Call Management Functional Descriptor, CDC Spec 5.2.3.2, Table 27
 257         5,                      // bFunctionLength
 258         0x24,                   // bDescriptorType
 259         0x01,                   // bDescriptorSubtype
 260         0x03,                   // bmCapabilities
 261         1,                      // bDataInterface
 262
 263         // Abstract Control Management Functional Descriptor, CDC Spec 5.2.3.3, Table 28
 264         4,                      // bFunctionLength
 265         0x24,                   // bDescriptorType
 266         0x02,                   // bDescriptorSubtype
 267         0x06,                   // bmCapabilities
 268
 269         // Union Functional Descriptor, CDC Spec 5.2.3.8, Table 33
 270         5,                      // bFunctionLength
 271         0x24,                   // bDescriptorType
 272         0x06,                   // bDescriptorSubtype
 273         0,                      // bMasterInterface
 274         1,                      // bSlaveInterface0
 275
 276         // endpoint descriptor, USB spec 9.6.6, page 269-271, Table 9-13
 277         ENDPOINT_DESCRIPTOR_LENGTH,     // bLength
 278         ENDPOINT_DESCRIPTOR,            // bDescriptorType
 279         PHY_TO_DESC(EPINT_IN),          // bEndpointAddress
 280         E_INTERRUPT,                    // bmAttributes (0x03=intr)
 281         LSB(MAX_PACKET_SIZE_EPINT),     // wMaxPacketSize (LSB)
 282         MSB(MAX_PACKET_SIZE_EPINT),     // wMaxPacketSize (MSB)
 283         16,                             // bInterval
 284
 285
 286
 287
 288         // interface descriptor, USB spec 9.6.5, page 267-269, Table 9-12
 289         9,          // bLength
 290         4,          // bDescriptorType
 291         1,          // bInterfaceNumber
 292         0,          // bAlternateSetting
 293         2,          // bNumEndpoints
 294         0x0A,       // bInterfaceClass
 295         0x00,       // bInterfaceSubClass
 296         0x00,       // bInterfaceProtocol
 297         0,          // iInterface
 298
 299         // endpoint descriptor, USB spec 9.6.6, page 269-271, Table 9-13
 300         7,                      // bLength
 301         5,                      // bDescriptorType
 302         PHY_TO_DESC(EPBULK_IN), // bEndpointAddress
 303         0x02,                   // bmAttributes (0x02=bulk)
 304         LSB(MAX_PACKET_SIZE_EPBULK),    // wMaxPacketSize (LSB)
 305         MSB(MAX_PACKET_SIZE_EPBULK),    // wMaxPacketSize (MSB)
 306         0,                      // bInterval
 307
 308         // endpoint descriptor, USB spec 9.6.6, page 269-271, Table 9-13
 309         7,                      // bLength
 310         5,                      // bDescriptorType
 311         PHY_TO_DESC(EPBULK_OUT),// bEndpointAddress
 312         0x02,                   // bmAttributes (0x02=bulk)
 313         LSB(MAX_PACKET_SIZE_EPBULK),    // wMaxPacketSize (LSB)
 314         MSB(MAX_PACKET_SIZE_EPBULK),     // wMaxPacketSize (MSB)
 315         0,                       // bInterval
 316
 317         // Interface 2, Alternate Setting 0, MSC Class
 318         9,      // bLength
 319         4,      // bDescriptorType
 320         0x02,   // bInterfaceNumber
 321         0x00,   // bAlternateSetting
 322         0x02,   // bNumEndpoints
 323         0x08,   // bInterfaceClass
 324         0x06,   // bInterfaceSubClass
 325         0x50,   // bInterfaceProtocol
 326         0x04,   // iInterface
 327
 328         // endpoint descriptor, USB spec 9.6.6, page 269-271, Table 9-13
 329         7,                          // bLength
 330         5,                          // bDescriptorType
 331         PHY_TO_DESC(MSDBULK_IN),     // bEndpointAddress
 332         0x02,                       // bmAttributes (0x02=bulk)
 333         LSB(MAX_PACKET_SIZE_EPBULK),// wMaxPacketSize (LSB)
 334         MSB(MAX_PACKET_SIZE_EPBULK),// wMaxPacketSize (MSB)
 335         0,                          // bInterval
 336
 337         // endpoint descriptor, USB spec 9.6.6, page 269-271, Table 9-13
 338         7,                          // bLength
 339         5,                          // bDescriptorType
 340         PHY_TO_DESC(MSDBULK_OUT),    // bEndpointAddress
 341         0x02,                       // bmAttributes (0x02=bulk)
 342         LSB(MAX_PACKET_SIZE_EPBULK),// wMaxPacketSize (LSB)
 343         MSB(MAX_PACKET_SIZE_EPBULK),// wMaxPacketSize (MSB)
 344         0                           // bInterval
 345     };
 346     return configDescriptor;
 347 }
 348
 349 int USBCDCMSC::_putc(int c) {
 350     send((uint8_t *)&c, 1);
 351     return 1;
 352 }
 353
 354 int USBCDCMSC::_getc() {
 355     uint8_t c;
 356     while (cdcbuf.isEmpty());
 357     cdcbuf.dequeue(&c);
 358     return c;
 359 }
 360
 361
 362 bool USBCDCMSC::writeBlock(uint8_t * buf, uint16_t size) {
 363     if(size > MAX_PACKET_SIZE_EPBULK) {
 364         return false;
 365     }
 366     if(!send(buf, size)) {
 367         return false;
 368     }
 369     return true;
 370 }
 371
 372
 373
 374 bool USBCDCMSC::EP2_OUT_callback() {
 375     uint8_t c[65];
 376     uint16_t size = 0;
 377
 378     //we read the packet received and put it on the circular buffer
 379     readEP(c, &size);
 380     for (int i = 0; i < size; i++) {
 381         cdcbuf.queue(c[i]);
 382     }
 383
 384     //call a potential handler
 385     rx.call();
 386
 387     // We reactivate the endpoint to receive next characters
 388     readStart(EPBULK_OUT, MAX_PACKET_SIZE_EPBULK);
 389     return true;
 390 }
 391
 392 uint8_t USBCDCMSC::available() {
 393     return cdcbuf.available();
 394 }
 395
 396
 397 bool USBCDCMSC::connect() {
 398
 399     //disk initialization
 400     if (disk_status() & NO_INIT) {
 401         if (disk_initialize()) {
 402             return false;
 403         }
 404     }
 405
 406     // get number of blocks
 407     BlockCount = disk_sectors();
 408
 409     // get memory size
 410     MemorySize = disk_size();
 411
 412     if (BlockCount >= 0) {
 413         BlockSize = MemorySize / BlockCount;
 414         if (BlockSize != 0) {
 415             page = (uint8_t *)malloc(BlockSize * sizeof(uint8_t));
 416             if (page == NULL)
 417                 return false;
 418         }
 419     } else {
 420         return false;
 421     }
 422
 423     //connect the device
 424 //    USBDevice::connect();
 425     return true;
 426 }
 427
 428
 429 void USBCDCMSC::reset() {
 430     stage = READ_CBW;
 431 }
 432
 433
 434 // Called in ISR context called when a data is received
 435 bool USBCDCMSC::EP5_OUT_callback() {
 436     uint16_t size = 0;
 437     uint8_t buf[MAX_PACKET_SIZE_EPBULK];
 438     USBDevice::readEP(MSDBULK_OUT, buf, &size, MAX_PACKET_SIZE_EPBULK);
 439     switch (stage) {
 440             // the device has to decode the CBW received
 441         case READ_CBW:
 442             CBWDecode(buf, size);
 443             break;
 444
 445             // the device has to receive data from the host
 446         case PROCESS_CBW:
 447             switch (cbw.CB[0]) {
 448                 case WRITE10:
 449                 case WRITE12:
 450                     memoryWrite(buf, size);
 451                     break;
 452                 case VERIFY10:
 453                     memoryVerify(buf, size);
 454                     break;
 455             }
 456             break;
 457
 458             // an error has occured: stall endpoint and send CSW
 459         default:
 460             stallEndpoint(MSDBULK_OUT);
 461             csw.Status = CSW_ERROR;
 462             sendCSW();
 463             break;
 464     }
 465
 466     //reactivate readings on the OUT bulk endpoint
 467     readStart(MSDBULK_OUT, MAX_PACKET_SIZE_EPBULK);
 468     return true;
 469 }
 470
 471 // Called in ISR context when a data has been transferred
 472 bool USBCDCMSC::EP5_IN_callback() {
 473     switch (stage) {
 474
 475             // the device has to send data to the host
 476         case PROCESS_CBW:
 477             switch (cbw.CB[0]) {
 478                 case READ10:
 479                 case READ12:
 480                     memoryRead();
 481                     break;
 482             }
 483             break;
 484
 485             //the device has to send a CSW
 486         case SEND_CSW:
 487             sendCSW();
 488             break;
 489
 490             // an error has occured
 491         case ERROR:
 492             stallEndpoint(MSDBULK_IN);
 493             sendCSW();
 494             break;
 495
 496             // the host has received the CSW -> we wait a CBW
 497         case WAIT_CSW:
 498             stage = READ_CBW;
 499             break;
 500     }
 501     return true;
 502 }
 503
 504
 505 void USBCDCMSC::memoryWrite (uint8_t * buf, uint16_t size) {
 506
 507     if ((addr + size) > MemorySize) {
 508         size = MemorySize - addr;
 509         stage = ERROR;
 510         stallEndpoint(MSDBULK_OUT);
 511     }
 512
 513     // we fill an array in RAM of 1 block before writing it in memory
 514     for (int i = 0; i < size; i++)
 515         page[addr%BlockSize + i] = buf[i];
 516
 517     // if the array is filled, write it in memory
 518     if (!((addr + size)%BlockSize)) {
 519         if (!(disk_status() & WRITE_PROTECT)) {
 520             disk_write((const char *)page, addr/BlockSize);
 521         }
 522     }
 523
 524     addr += size;
 525     length -= size;
 526     csw.DataResidue -= size;
 527
 528     if ((!length) || (stage != PROCESS_CBW)) {
 529         csw.Status = (stage == ERROR) ? CSW_FAILED : CSW_PASSED;
 530         sendCSW();
 531     }
 532 }
 533
 534 void USBCDCMSC::memoryVerify (uint8_t * buf, uint16_t size) {
 535     uint32_t n;
 536
 537     if ((addr + size) > MemorySize) {
 538         size = MemorySize - addr;
 539         stage = ERROR;
 540         stallEndpoint(MSDBULK_OUT);
 541     }
 542
 543     // beginning of a new block -> load a whole block in RAM
 544     if (!(addr%BlockSize))
 545         disk_read((char *)page, addr/BlockSize);
 546
 547     // info are in RAM -> no need to re-read memory
 548     for (n = 0; n < size; n++) {
 549         if (page[addr%BlockSize + n] != buf[n]) {
 550             memOK = false;
 551             break;
 552         }
 553     }
 554
 555     addr += size;
 556     length -= size;
 557     csw.DataResidue -= size;
 558
 559     if ( !length || (stage != PROCESS_CBW)) {
 560         csw.Status = (memOK && (stage == PROCESS_CBW)) ? CSW_PASSED : CSW_FAILED;
 561         sendCSW();
 562     }
 563 }
 564
 565
 566 bool USBCDCMSC::inquiryRequest (void) {
 567     uint8_t inquiry[] = { 0x00, 0x80, 0x00, 0x01,
 568                           36 - 4, 0x80, 0x00, 0x00,
 569                           'M', 'B', 'E', 'D', '.', 'O', 'R', 'G',
 570                           'M', 'B', 'E', 'D', ' ', 'U', 'S', 'B', ' ', 'D', 'I', 'S', 'K', ' ', ' ', ' ',
 571                           '1', '.', '0', ' ',
 572                         };
 573     if (!msd_write(inquiry, sizeof(inquiry))) {
 574         return false;
 575     }
 576     return true;
 577 }
 578
 579
 580 bool USBCDCMSC::readFormatCapacity() {
 581     uint8_t capacity[] = { 0x00, 0x00, 0x00, 0x08,
 582                            (BlockCount >> 24) & 0xff,
 583                            (BlockCount >> 16) & 0xff,
 584                            (BlockCount >> 8) & 0xff,
 585                            (BlockCount >> 0) & 0xff,
 586
 587                            0x02,
 588                            (BlockSize >> 16) & 0xff,
 589                            (BlockSize >> 8) & 0xff,
 590                            (BlockSize >> 0) & 0xff,
 591                          };
 592     if (!msd_write(capacity, sizeof(capacity))) {
 593         return false;
 594     }
 595     return true;
 596 }
 597
 598
 599 bool USBCDCMSC::readCapacity (void) {
 600     uint8_t capacity[] = {
 601         ((BlockCount - 1) >> 24) & 0xff,
 602         ((BlockCount - 1) >> 16) & 0xff,
 603         ((BlockCount - 1) >> 8) & 0xff,
 604         ((BlockCount - 1) >> 0) & 0xff,
 605
 606         (BlockSize >> 24) & 0xff,
 607         (BlockSize >> 16) & 0xff,
 608         (BlockSize >> 8) & 0xff,
 609         (BlockSize >> 0) & 0xff,
 610     };
 611     if (!msd_write(capacity, sizeof(capacity))) {
 612         return false;
 613     }
 614     return true;
 615 }
 616
 617 bool USBCDCMSC::msd_write (uint8_t * buf, uint16_t size) {
 618
 619     if (size >= cbw.DataLength) {
 620         size = cbw.DataLength;
 621     }
 622     stage = SEND_CSW;
 623
 624     if (!writeNB(MSDBULK_IN, buf, size, MAX_PACKET_SIZE_EPBULK)) {
 625         return false;
 626     }
 627
 628     csw.DataResidue -= size;
 629     csw.Status = CSW_PASSED;
 630     return true;
 631 }
 632
 633
 634 bool USBCDCMSC::modeSense6 (void) {
 635     uint8_t sense6[] = { 0x03, 0x00, 0x00, 0x00 };
 636     if (!msd_write(sense6, sizeof(sense6))) {
 637         return false;
 638     }
 639     return true;
 640 }
 641
 642 void USBCDCMSC::sendCSW() {
 643     csw.Signature = CSW_Signature;
 644     writeNB(MSDBULK_IN, (uint8_t *)&csw, sizeof(CSW), MAX_PACKET_SIZE_EPBULK);
 645     stage = WAIT_CSW;
 646 }
 647
 648 bool USBCDCMSC::requestSense (void) {
 649     uint8_t request_sense[] = {
 650         0x70,
 651         0x00,
 652         0x05,   // Sense Key: illegal request
 653         0x00,
 654         0x00,
 655         0x00,
 656         0x00,
 657         0x0A,
 658         0x00,
 659         0x00,
 660         0x00,
 661         0x00,
 662         0x30,
 663         0x01,
 664         0x00,
 665         0x00,
 666         0x00,
 667         0x00,
 668     };
 669
 670     if (!msd_write(request_sense, sizeof(request_sense))) {
 671         return false;
 672     }
 673
 674     return true;
 675 }
 676
 677 void USBCDCMSC::fail() {
 678     csw.Status = CSW_FAILED;
 679     sendCSW();
 680 }
 681
 682
 683 void USBCDCMSC::CBWDecode(uint8_t * buf, uint16_t size) {
 684     if (size == sizeof(cbw)) {
 685         memcpy((uint8_t *)&cbw, buf, size);
 686         if (cbw.Signature == CBW_Signature) {
 687             csw.Tag = cbw.Tag;
 688             csw.DataResidue = cbw.DataLength;
 689             if ((cbw.CBLength <  1) || (cbw.CBLength > 16) ) {
 690                 fail();
 691             } else {
 692                 switch (cbw.CB[0]) {
 693                     case TEST_UNIT_READY:
 694                         testUnitReady();
 695                         break;
 696                     case REQUEST_SENSE:
 697                         requestSense();
 698                         break;
 699                     case INQUIRY:
 700                         inquiryRequest();
 701                         break;
 702                     case MODE_SENSE6:
 703                         modeSense6();
 704                         break;
 705                     case READ_FORMAT_CAPACITIES:
 706                         readFormatCapacity();
 707                         break;
 708                     case READ_CAPACITY:
 709                         readCapacity();
 710                         break;
 711                     case READ10:
 712                     case READ12:
 713                         if (infoTransfer()) {
 714                             if ((cbw.Flags & 0x80)) {
 715                                 stage = PROCESS_CBW;
 716                                 memoryRead();
 717                             } else {
 718                                 stallEndpoint(MSDBULK_OUT);
 719                                 csw.Status = CSW_ERROR;
 720                                 sendCSW();
 721                             }
 722                         }
 723                         break;
 724                     case WRITE10:
 725                     case WRITE12:
 726                         if (infoTransfer()) {
 727                             if (!(cbw.Flags & 0x80)) {
 728                                 stage = PROCESS_CBW;
 729                             } else {
 730                                 stallEndpoint(MSDBULK_IN);
 731                                 csw.Status = CSW_ERROR;
 732                                 sendCSW();
 733                             }
 734                         }
 735                         break;
 736                     case VERIFY10:
 737                         if (!(cbw.CB[1] & 0x02)) {
 738                             csw.Status = CSW_PASSED;
 739                             sendCSW();
 740                             break;
 741                         }
 742                         if (infoTransfer()) {
 743                             if (!(cbw.Flags & 0x80)) {
 744                                 stage = PROCESS_CBW;
 745                                 memOK = true;
 746                             } else {
 747                                 stallEndpoint(MSDBULK_IN);
 748                                 csw.Status = CSW_ERROR;
 749                                 sendCSW();
 750                             }
 751                         }
 752                         break;
 753                     default:
 754                         fail();
 755                         break;
 756                 }
 757             }
 758         }
 759     }
 760 }
 761
 762 void USBCDCMSC::testUnitReady (void) {
 763
 764     if (cbw.DataLength != 0) {
 765         if ((cbw.Flags & 0x80) != 0) {
 766             stallEndpoint(MSDBULK_IN);
 767         } else {
 768             stallEndpoint(MSDBULK_OUT);
 769         }
 770     }
 771
 772     csw.Status = CSW_PASSED;
 773     sendCSW();
 774 }
 775
 776
 777 void USBCDCMSC::memoryRead (void) {
 778     uint32_t n;
 779
 780     n = (length > MAX_PACKET) ? MAX_PACKET : length;
 781
 782     if ((addr + n) > MemorySize) {
 783         n = MemorySize - addr;
 784         stage = ERROR;
 785     }
 786
 787     // we read an entire block
 788     if (!(addr%BlockSize))
 789         disk_read((char *)page, addr/BlockSize);
 790
 791     // write data which are in RAM
 792     writeNB(MSDBULK_IN, &page[addr%BlockSize], n, MAX_PACKET_SIZE_EPBULK);
 793
 794     addr += n;
 795     length -= n;
 796
 797     csw.DataResidue -= n;
 798
 799     if ( !length || (stage != PROCESS_CBW)) {
 800         csw.Status = (stage == PROCESS_CBW) ? CSW_PASSED : CSW_FAILED;
 801         stage = (stage == PROCESS_CBW) ? SEND_CSW : stage;
 802     }
 803 }
 804
 805
 806 bool USBCDCMSC::infoTransfer (void) {
 807     uint32_t n;
 808
 809     // Logical Block Address of First Block
 810     n = (cbw.CB[2] << 24) | (cbw.CB[3] << 16) | (cbw.CB[4] <<  8) | (cbw.CB[5] <<  0);
 811
 812     addr = n * BlockSize;
 813
 814     // Number of Blocks to transfer
 815     switch (cbw.CB[0]) {
 816         case READ10:
 817         case WRITE10:
 818         case VERIFY10:
 819             n = (cbw.CB[7] <<  8) | (cbw.CB[8] <<  0);
 820             break;
 821
 822         case READ12:
 823         case WRITE12:
 824             n = (cbw.CB[6] << 24) | (cbw.CB[7] << 16) | (cbw.CB[8] <<  8) | (cbw.CB[9] <<  0);
 825             break;
 826     }
 827
 828     length = n * BlockSize;
 829
 830     if (!cbw.DataLength) {              // host requests no data
 831         csw.Status = CSW_FAILED;
 832         sendCSW();
 833         return false;
 834     }
 835
 836     if (cbw.DataLength != length) {
 837         if ((cbw.Flags & 0x80) != 0) {
 838             stallEndpoint(MSDBULK_IN);
 839         } else {
 840             stallEndpoint(MSDBULK_OUT);
 841         }
 842
 843         csw.Status = CSW_FAILED;
 844         sendCSW();
 845         return false;
 846     }
 847
 848     return true;
 849 }
 850
 851 int USBCDCMSC::isBreak () {
 852     int ret = cdcbreak;
 853     cdcbreak = 0;
 854     return ret;
 855 }
 856
 857 int USBCDCMSC::disk_initialize() {
 858     if (_sd->disk_initialize()) {
 859         _status |= NO_DISK;
 860         return 1;
 861     } else {
 862         _status = DISK_OK;
 863         return 0;
 864     }
 865 }
 866
 867 int USBCDCMSC::disk_write(const char *buffer, int block_number) {
 868     return _sd->disk_write(buffer, block_number);
 869 }
 870
 871 int USBCDCMSC::disk_read(char *buffer, int block_number) {
 872     return _sd->disk_read(buffer, block_number);
 873 }
 874
 875 int USBCDCMSC::disk_status() {
 876     return _status;
 877 }
 878
 879 int USBCDCMSC::disk_sectors() {
 880      return _sd->disk_sectors();
 881 }
 882 int USBCDCMSC::disk_size() {
 883     return _sd->disk_sectors() * 512;
 884 }
 885
 886
 887 void USBCDCMSC::on_module_loaded(){
 888     // We want to be called every time a new char is received
 889     this->attach(this, &USBCDCMSC::on_serial_char_received);
 890
 891     // We only call the command dispatcher in the main loop, nowhere else
 892     this->register_for_event(ON_MAIN_LOOP);
 893 }
 894
 895 void USBCDCMSC::on_main_loop(void* argument){
 896     //if( this->configured() ){
 897     //    this->kernel->serial->printf("a:%d\r\n", this->buffer.size());
 898     //}
 899     if( this->has_char('\n') ){
 900         int index = 0;
 901         string received;
 902         while(1){
 903            char c;
 904            this->buffer.pop_front(c);
 905            if( c == '\n' ){
 906                 struct SerialMessage message;
 907                 message.message = received;
 908                 message.stream = this;
 909                 this->kernel->call_event(ON_CONSOLE_LINE_RECEIVED, &message );
 910                //this->kernel->serial->printf("received: %s \r\n", received.c_str() );
 911                //this->printf("received: %s\r\n", received.c_str() );
 912                return;
 913             }else{
 914                 received += c;
 915             }
 916         }
 917     }
 918
 919
 920 }
 921
 922 void USBCDCMSC::on_serial_char_received(){
 923     if(this->available()){
 924         char received = this->getc();
 925         //On newline, we have received a line, else concatenate in buffer
 926         if( received == '\r' ){ return; }
 927         //if( this->kernel != NULL ){
 928         //    this->kernel->serial->printf("received:%c\r\n", received);
 929         //}
 930         this->buffer.push_back(received);
 931     }
 932
 933
 934
 935 }
 936
 937
 938 bool USBCDCMSC::has_char(char letter){
 939     int index = this->buffer.head;
 940     while( index != this->buffer.tail ){
 941         if( this->buffer.buffer[index] == letter ){
 942             return true;
 943         }
 944         index = this->buffer.next_block_index(index);
 945     }
 946     return false;
 947 }