USBMSD.cpp

00001 /*
00002  * Copyright (c) 2018-2019, Arm Limited and affiliates.
00003  * SPDX-License-Identifier: Apache-2.0
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License");
00006  * you may not use this file except in compliance with the License.
00007  * You may obtain a copy of the License at
00008  *
00009  *     http://www.apache.org/licenses/LICENSE-2.0
00010  *
00011  * Unless required by applicable law or agreed to in writing, software
00012  * distributed under the License is distributed on an "AS IS" BASIS,
00013  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014  * See the License for the specific language governing permissions and
00015  * limitations under the License.
00016  */
00017 
00018 #include <stdint.h>
00019 #include <stdlib.h>
00020 #include "USBMSD.h"
00021 #include "EndpointResolver.h"
00022 #include "usb_phy_api.h"
00023 
00024 #define DISK_OK         0x00
00025 #define NO_INIT         0x01
00026 #define NO_DISK         0x02
00027 #define WRITE_PROTECT   0x04
00028 
00029 #define CBW_Signature   0x43425355
00030 #define CSW_Signature   0x53425355
00031 
00032 // SCSI Commands
00033 #define TEST_UNIT_READY            0x00
00034 #define REQUEST_SENSE              0x03
00035 #define FORMAT_UNIT                0x04
00036 #define INQUIRY                    0x12
00037 #define MODE_SELECT6               0x15
00038 #define MODE_SENSE6                0x1A
00039 #define START_STOP_UNIT            0x1B
00040 #define MEDIA_REMOVAL              0x1E
00041 #define READ_FORMAT_CAPACITIES     0x23
00042 #define READ_CAPACITY              0x25
00043 #define READ10                     0x28
00044 #define WRITE10                    0x2A
00045 #define VERIFY10                   0x2F
00046 #define READ12                     0xA8
00047 #define WRITE12                    0xAA
00048 #define MODE_SELECT10              0x55
00049 #define MODE_SENSE10               0x5A
00050 
00051 // MSC class specific requests
00052 #define MSC_REQUEST_RESET          0xFF
00053 #define MSC_REQUEST_GET_MAX_LUN    0xFE
00054 
00055 #define DEFAULT_CONFIGURATION (1)
00056 
00057 // max packet size
00058 #define MAX_PACKET  64
00059 
00060 // CSW Status
00061 enum Status {
00062     CSW_PASSED,
00063     CSW_FAILED,
00064     CSW_ERROR,
00065 };
00066 
00067 USBMSD::USBMSD(BlockDevice *bd, bool connect_blocking, uint16_t vendor_id, uint16_t product_id, uint16_t product_release)
00068     : USBDevice(get_usb_phy(), vendor_id, product_id, product_release),
00069       _initialized(false), _media_removed(false), _in_task(&_queue), _out_task(&_queue), _reset_task(&_queue), _control_task(&_queue), _configure_task(&_queue), _bd(bd),
00070       _addr(0), _length(0), _mem_ok(false), _block_size(0), _memory_size(0), _block_count(0), _out_ready(false), _in_ready(false), _bulk_out_size(0)
00071 {
00072     _init();
00073     if (connect_blocking) {
00074         connect();
00075     } else {
00076         init();
00077     }
00078 }
00079 
00080 USBMSD::USBMSD(USBPhy *phy, BlockDevice *bd, uint16_t vendor_id, uint16_t product_id, uint16_t product_release)
00081     : USBDevice(phy, vendor_id, product_id, product_release),
00082       _initialized(false), _media_removed(false), _in_task(&_queue), _out_task(&_queue), _reset_task(&_queue), _control_task(&_queue), _configure_task(&_queue), _bd(bd),
00083       _addr(0), _length(0), _mem_ok(false), _block_size(0), _memory_size(0), _block_count(0), _out_ready(false), _in_ready(false), _bulk_out_size(0)
00084 {
00085     _init();
00086 }
00087 
00088 
00089 void USBMSD::_init()
00090 {
00091     _bd->init();
00092 
00093     _in_task = mbed::callback(this, &USBMSD::_in);
00094     _out_task = mbed::callback(this, &USBMSD::_out);
00095     _reset_task = mbed::callback(this, &USBMSD::_reset);
00096     _control_task = mbed::callback(this, &USBMSD::_control);
00097     _configure_task = mbed::callback(this, &USBMSD::_configure);
00098 
00099     EndpointResolver resolver(endpoint_table());
00100 
00101     resolver.endpoint_ctrl(64);
00102     _bulk_in = resolver.endpoint_in(USB_EP_TYPE_BULK, MAX_PACKET);
00103     _bulk_out = resolver.endpoint_out(USB_EP_TYPE_BULK, MAX_PACKET);
00104     MBED_ASSERT(resolver.valid());
00105 
00106     _stage = READ_CBW;
00107     memset((void *)&_cbw, 0, sizeof(CBW));
00108     memset((void *)&_csw, 0, sizeof(CSW));
00109     _page = NULL;
00110 }
00111 
00112 USBMSD::~USBMSD()
00113 {
00114     disconnect();
00115     _bd->deinit();
00116     deinit();
00117 }
00118 
00119 bool USBMSD::connect()
00120 {
00121     _mutex_init.lock();
00122     _mutex.lock();
00123 
00124     // already initialized
00125     if (_initialized) {
00126         _mutex.unlock();
00127         _mutex_init.unlock();
00128         return false;
00129     }
00130 
00131     //disk initialization
00132     if (disk_status() & NO_INIT) {
00133         if (disk_initialize()) {
00134             _mutex.unlock();
00135             _mutex_init.unlock();
00136             return false;
00137         }
00138     }
00139 
00140     // get number of blocks
00141     _block_count = disk_sectors();
00142 
00143     // get memory size
00144     _memory_size = disk_size();
00145 
00146     if (_block_count > 0) {
00147         _block_size = _memory_size / _block_count;
00148         if (_block_size != 0) {
00149             free(_page);
00150             _page = (uint8_t *)malloc(_block_size * sizeof(uint8_t));
00151             if (_page == NULL) {
00152                 _mutex.unlock();
00153                 _mutex_init.unlock();
00154                 return false;
00155             }
00156         }
00157     } else {
00158         _mutex.unlock();
00159         _mutex_init.unlock();
00160         return false;
00161     }
00162 
00163     //connect the device
00164     USBDevice::connect();
00165     _initialized = true;
00166     _media_removed = false;
00167     _mutex.unlock();
00168     _mutex_init.unlock();
00169     return true;
00170 }
00171 
00172 void USBMSD::disconnect()
00173 {
00174     _mutex_init.lock();
00175     _mutex.lock();
00176 
00177     USBDevice::disconnect();
00178     _initialized = false;
00179 
00180     _in_task.cancel();
00181     _out_task.cancel();
00182     _reset_task.cancel();
00183     _control_task.cancel();
00184     _configure_task.cancel();
00185 
00186     _mutex.unlock();
00187 
00188     // object mutex must be unlocked for waiting
00189     _in_task.wait();
00190     _out_task.wait();
00191     _reset_task.wait();
00192     _control_task.wait();
00193     _configure_task.wait();
00194 
00195     _mutex.lock();
00196 
00197     //De-allocate MSD page size:
00198     free(_page);
00199     _page = NULL;
00200 
00201     _mutex.unlock();
00202     _mutex_init.unlock();
00203 }
00204 
00205 void USBMSD::process()
00206 {
00207     _queue.dispatch();
00208 }
00209 
00210 void USBMSD::attach(mbed::Callback<void()> cb)
00211 {
00212     lock();
00213 
00214     _queue.attach(cb);
00215 
00216     unlock();
00217 }
00218 
00219 bool USBMSD::media_removed()
00220 {
00221     return _media_removed;
00222 }
00223 
00224 int USBMSD::disk_read(uint8_t *data, uint64_t block, uint8_t count)
00225 {
00226     bd_addr_t addr =  block * _bd->get_erase_size();
00227     bd_size_t size = count * _bd->get_erase_size();
00228     return _bd->read(data, addr, size);
00229 }
00230 
00231 int USBMSD::disk_write(const uint8_t *data, uint64_t block, uint8_t count)
00232 {
00233     bd_addr_t addr =  block * _bd->get_erase_size();
00234     bd_size_t size = count * _bd->get_erase_size();
00235     int ret = _bd->erase(addr, size);
00236     if (ret != 0) {
00237         return ret;
00238     }
00239 
00240     return _bd->program(data, addr, size);
00241 }
00242 
00243 int USBMSD::disk_initialize()
00244 {
00245     return 0;
00246 }
00247 
00248 uint64_t USBMSD::disk_sectors()
00249 {
00250     return _bd->size() / _bd->get_erase_size();
00251 }
00252 
00253 uint64_t USBMSD::disk_size()
00254 {
00255     return _bd->size();
00256 }
00257 
00258 
00259 int USBMSD::disk_status()
00260 {
00261     return 0;
00262 }
00263 
00264 void USBMSD::_isr_out()
00265 {
00266     _out_task.call();
00267 }
00268 
00269 void USBMSD::_isr_in()
00270 {
00271     _in_task.call();
00272 }
00273 
00274 void USBMSD::callback_state_change(DeviceState new_state)
00275 {
00276     // called in ISR context
00277 
00278     if (new_state != Configured) {
00279         _reset_task.cancel();
00280         _reset_task.call();
00281     }
00282 }
00283 
00284 void USBMSD::callback_request(const setup_packet_t *setup)
00285 {
00286     // called in ISR context
00287 
00288     if (setup->bmRequestType.Type == CLASS_TYPE) {
00289         _control_task.call(setup);
00290     } else {
00291         complete_request(PassThrough, NULL, 0);
00292     }
00293 }
00294 
00295 void USBMSD::callback_request_xfer_done(const setup_packet_t *setup, bool aborted)
00296 {
00297     // called in ISR context
00298 
00299     bool success = setup->bRequest == MSC_REQUEST_GET_MAX_LUN;
00300     complete_request_xfer_done(success);
00301 }
00302 
00303 void USBMSD::callback_set_configuration(uint8_t configuration)
00304 {
00305     // called in ISR context
00306 
00307     if (configuration != DEFAULT_CONFIGURATION) {
00308         complete_set_configuration(false);
00309         return;
00310     }
00311     _configure_task.call();
00312 }
00313 
00314 void USBMSD::callback_set_interface(uint16_t interface, uint8_t alternate)
00315 {
00316     // called in ISR context
00317 
00318     bool success = (interface == 0) && (alternate == 0);
00319     complete_set_interface(success);
00320 }
00321 
00322 
00323 const uint8_t *USBMSD::string_iinterface_desc()
00324 {
00325     static const uint8_t string_iinterface_descriptor[] = {
00326         0x08,                           //bLength
00327         STRING_DESCRIPTOR,              //bDescriptorType 0x03
00328         'M', 0, 'S', 0, 'D', 0          //bString iInterface - MSD
00329     };
00330     return string_iinterface_descriptor;
00331 }
00332 
00333 const uint8_t *USBMSD::string_iproduct_desc()
00334 {
00335     static const uint8_t string_iproduct_descriptor[] = {
00336         0x12,                                           //bLength
00337         STRING_DESCRIPTOR,                              //bDescriptorType 0x03
00338         'M', 0, 'b', 0, 'e', 0, 'd', 0, ' ', 0, 'M', 0, 'S', 0, 'D', 0 //bString iProduct - Mbed Audio
00339     };
00340     return string_iproduct_descriptor;
00341 }
00342 
00343 
00344 const uint8_t *USBMSD::configuration_desc(uint8_t index)
00345 {
00346     if (index != 0) {
00347         return NULL;
00348     }
00349 
00350     uint8_t config_descriptor_temp[] = {
00351 
00352         // Configuration 1
00353         9,      // bLength
00354         2,      // bDescriptorType
00355         LSB(9 + 9 + 7 + 7), // wTotalLength
00356         MSB(9 + 9 + 7 + 7),
00357         0x01,   // bNumInterfaces
00358         0x01,   // bConfigurationValue: 0x01 is used to select this configuration
00359         0x00,   // iConfiguration: no string to describe this configuration
00360         0xC0,   // bmAttributes
00361         100,    // bMaxPower, device power consumption is 100 mA
00362 
00363         // Interface 0, Alternate Setting 0, MSC Class
00364         9,      // bLength
00365         4,      // bDescriptorType
00366         0x00,   // bInterfaceNumber
00367         0x00,   // bAlternateSetting
00368         0x02,   // bNumEndpoints
00369         0x08,   // bInterfaceClass
00370         0x06,   // bInterfaceSubClass
00371         0x50,   // bInterfaceProtocol
00372         0x04,   // iInterface
00373 
00374         // endpoint descriptor, USB spec 9.6.6, page 269-271, Table 9-13
00375         7,                          // bLength
00376         5,                          // bDescriptorType
00377         _bulk_in,                   // bEndpointAddress
00378         0x02,                       // bmAttributes (0x02=bulk)
00379         LSB(MAX_PACKET),            // wMaxPacketSize (LSB)
00380         MSB(MAX_PACKET),            // wMaxPacketSize (MSB)
00381         0,                          // bInterval
00382 
00383         // endpoint descriptor, USB spec 9.6.6, page 269-271, Table 9-13
00384         7,                          // bLength
00385         5,                          // bDescriptorType
00386         _bulk_out,                  // bEndpointAddress
00387         0x02,                       // bmAttributes (0x02=bulk)
00388         LSB(MAX_PACKET),            // wMaxPacketSize (LSB)
00389         MSB(MAX_PACKET),            // wMaxPacketSize (MSB)
00390         0                           // bInterval
00391     };
00392     MBED_ASSERT(sizeof(config_descriptor_temp) == sizeof(_configuration_descriptor));
00393     memcpy(_configuration_descriptor, config_descriptor_temp, sizeof(_configuration_descriptor));
00394     return _configuration_descriptor;
00395 }
00396 
00397 void USBMSD::_out()
00398 {
00399     _mutex.lock();
00400 
00401     _bulk_out_size = read_finish(_bulk_out);
00402     _out_ready = true;
00403     _process();
00404 
00405     _mutex.unlock();
00406 }
00407 
00408 void USBMSD::_in()
00409 {
00410     _mutex.lock();
00411 
00412     write_finish(_bulk_in);
00413     _in_ready = true;
00414     _process();
00415 
00416     _mutex.unlock();
00417 }
00418 
00419 void USBMSD::_reset()
00420 {
00421     _mutex.lock();
00422 
00423     msd_reset();
00424 
00425     _mutex.unlock();
00426 }
00427 
00428 void USBMSD::_control(const setup_packet_t *setup)
00429 {
00430     _mutex.lock();
00431 
00432     static const uint8_t maxLUN[1] = {0};
00433 
00434     RequestResult result = PassThrough;
00435     uint8_t *data = NULL;
00436     uint32_t size = 0;
00437 
00438     if (setup->bmRequestType.Type == CLASS_TYPE) {
00439         switch (setup->bRequest) {
00440             case MSC_REQUEST_RESET:
00441                 result = Success;
00442                 msd_reset();
00443                 break;
00444             case MSC_REQUEST_GET_MAX_LUN:
00445                 result = Send;
00446                 data = (uint8_t *)maxLUN;
00447                 size = 1;
00448                 break;
00449             default:
00450                 break;
00451         }
00452     }
00453 
00454     complete_request(result, data, size);
00455 
00456     _mutex.unlock();
00457 }
00458 
00459 void USBMSD::_configure()
00460 {
00461     _mutex.lock();
00462 
00463     // Configure endpoints > 0
00464     endpoint_add(_bulk_in, MAX_PACKET, USB_EP_TYPE_BULK, &USBMSD::_isr_in);
00465     endpoint_add(_bulk_out, MAX_PACKET, USB_EP_TYPE_BULK, &USBMSD::_isr_out);
00466     MBED_ASSERT(sizeof(_bulk_out_buf) == MAX_PACKET);
00467     MBED_ASSERT(sizeof(_bulk_in_buf) == MAX_PACKET);
00468 
00469     _out_ready = false;
00470     _in_ready = true;
00471 
00472     //activate readings
00473     read_start(_bulk_out, _bulk_out_buf, sizeof(_bulk_out_buf));
00474     complete_set_configuration(true);
00475 
00476     _mutex.unlock();
00477 }
00478 
00479 void USBMSD::_process()
00480 {
00481     // Mutex must be locked by caller
00482 
00483     switch (_stage) {
00484         // the device has to decode the CBW received
00485         case READ_CBW:
00486             if (!_out_ready) {
00487                 break;
00488             }
00489             CBWDecode(_bulk_out_buf, _bulk_out_size);
00490             _read_next();
00491             break;
00492 
00493 
00494         case PROCESS_CBW:
00495             switch (_cbw.CB[0]) {
00496                 // the device has to receive data from the host
00497                 case WRITE10:
00498                 case WRITE12:
00499                     if (!_out_ready) {
00500                         break;
00501                     }
00502                     memoryWrite(_bulk_out_buf, _bulk_out_size);
00503                     _read_next();
00504                     break;
00505                 case VERIFY10:
00506                     if (!_out_ready) {
00507                         break;
00508                     }
00509                     memoryVerify(_bulk_out_buf, _bulk_out_size);
00510                     _read_next();
00511                     break;
00512                 // the device has to send data to the host
00513                 case READ10:
00514                 case READ12:
00515                     if (!_in_ready) {
00516                         break;
00517                     }
00518                     memoryRead();
00519                     break;
00520             }
00521             break;
00522 
00523         //the device has to send a CSW
00524         case SEND_CSW:
00525             if (!_in_ready) {
00526                 break;
00527             }
00528             sendCSW();
00529             break;
00530 
00531         // an error has occurred: stall endpoint and send CSW
00532         default:
00533             endpoint_stall(_bulk_out);
00534             endpoint_stall(_bulk_in);
00535             _csw.Status = CSW_ERROR;
00536             sendCSW();
00537             break;
00538     }
00539 }
00540 
00541 void USBMSD::_write_next(uint8_t *data, uint32_t size)
00542 {
00543     lock();
00544 
00545     MBED_ASSERT(size <= MAX_PACKET);
00546     MBED_ASSERT(_in_ready);
00547     uint32_t send_size = MAX_PACKET > size ? size : MAX_PACKET;
00548     memcpy(_bulk_in_buf, data, send_size);
00549     write_start(_bulk_in, _bulk_in_buf, send_size);
00550     _in_ready = false;
00551 
00552     unlock();
00553 }
00554 
00555 void USBMSD::_read_next()
00556 {
00557     lock();
00558 
00559     MBED_ASSERT(_out_ready);
00560     read_start(_bulk_out, _bulk_out_buf, sizeof(_bulk_out_buf));
00561     _out_ready = false;
00562 
00563     unlock();
00564 }
00565 
00566 void USBMSD::memoryWrite(uint8_t *buf, uint16_t size)
00567 {
00568     if ((_addr + size) > _memory_size) {
00569         size = _memory_size - _addr;
00570         _stage = ERROR;
00571         endpoint_stall(_bulk_out);
00572     }
00573 
00574     // we fill an array in RAM of 1 block before writing it in memory
00575     for (int i = 0; i < size; i++) {
00576         _page[_addr % _block_size + i] = buf[i];
00577     }
00578 
00579     // if the array is filled, write it in memory
00580     if (!((_addr + size) % _block_size)) {
00581         if (!(disk_status() & WRITE_PROTECT)) {
00582             disk_write(_page, _addr / _block_size, 1);
00583         }
00584     }
00585 
00586     _addr += size;
00587     _length -= size;
00588     _csw.DataResidue -= size;
00589 
00590     if ((!_length) || (_stage != PROCESS_CBW)) {
00591         _csw.Status = (_stage == ERROR) ? CSW_FAILED : CSW_PASSED;
00592         sendCSW();
00593     }
00594 }
00595 
00596 void USBMSD::memoryVerify(uint8_t *buf, uint16_t size)
00597 {
00598     uint32_t n;
00599 
00600     if ((_addr + size) > _memory_size) {
00601         size = _memory_size - _addr;
00602         _stage = ERROR;
00603         endpoint_stall(_bulk_out);
00604     }
00605 
00606     // beginning of a new block -> load a whole block in RAM
00607     if (!(_addr % _block_size)) {
00608         disk_read(_page, _addr / _block_size, 1);
00609     }
00610 
00611     // info are in RAM -> no need to re-read memory
00612     for (n = 0; n < size; n++) {
00613         if (_page[_addr % _block_size + n] != buf[n]) {
00614             _mem_ok = false;
00615             break;
00616         }
00617     }
00618 
00619     _addr += size;
00620     _length -= size;
00621     _csw.DataResidue -= size;
00622 
00623     if (!_length || (_stage != PROCESS_CBW)) {
00624         _csw.Status = (_mem_ok && (_stage == PROCESS_CBW)) ? CSW_PASSED : CSW_FAILED;
00625         sendCSW();
00626     }
00627 }
00628 
00629 
00630 bool USBMSD::inquiryRequest(void)
00631 {
00632     uint8_t inquiry[] = { 0x00, 0x80, 0x00, 0x01,
00633                           36 - 4, 0x80, 0x00, 0x00,
00634                           'M', 'B', 'E', 'D', '.', 'O', 'R', 'G',
00635                           'M', 'B', 'E', 'D', ' ', 'U', 'S', 'B', ' ', 'D', 'I', 'S', 'K', ' ', ' ', ' ',
00636                           '1', '.', '0', ' ',
00637                         };
00638     if (!write(inquiry, sizeof(inquiry))) {
00639         return false;
00640     }
00641     return true;
00642 }
00643 
00644 
00645 bool USBMSD::readFormatCapacity()
00646 {
00647     uint8_t capacity[] = { 0x00, 0x00, 0x00, 0x08,
00648                            (uint8_t)((_block_count >> 24) & 0xff),
00649                            (uint8_t)((_block_count >> 16) & 0xff),
00650                            (uint8_t)((_block_count >> 8) & 0xff),
00651                            (uint8_t)((_block_count >> 0) & 0xff),
00652 
00653                            0x02,
00654                            (uint8_t)((_block_size >> 16) & 0xff),
00655                            (uint8_t)((_block_size >> 8) & 0xff),
00656                            (uint8_t)((_block_size >> 0) & 0xff),
00657                          };
00658     if (!write(capacity, sizeof(capacity))) {
00659         return false;
00660     }
00661     return true;
00662 }
00663 
00664 
00665 bool USBMSD::readCapacity(void)
00666 {
00667     uint8_t capacity[] = {
00668         (uint8_t)(((_block_count - 1) >> 24) & 0xff),
00669         (uint8_t)(((_block_count - 1) >> 16) & 0xff),
00670         (uint8_t)(((_block_count - 1) >> 8) & 0xff),
00671         (uint8_t)(((_block_count - 1) >> 0) & 0xff),
00672 
00673         (uint8_t)((_block_size >> 24) & 0xff),
00674         (uint8_t)((_block_size >> 16) & 0xff),
00675         (uint8_t)((_block_size >> 8) & 0xff),
00676         (uint8_t)((_block_size >> 0) & 0xff),
00677     };
00678     if (!write(capacity, sizeof(capacity))) {
00679         return false;
00680     }
00681     return true;
00682 }
00683 
00684 bool USBMSD::write(uint8_t *buf, uint16_t size)
00685 {
00686 
00687     if (size >= _cbw.DataLength) {
00688         size = _cbw.DataLength;
00689     }
00690     _stage = SEND_CSW;
00691 
00692     _write_next(buf, size);
00693 
00694     _csw.DataResidue -= size;
00695     _csw.Status = CSW_PASSED;
00696     return true;
00697 }
00698 
00699 
00700 bool USBMSD::modeSense6(void)
00701 {
00702     uint8_t sense6[] = { 0x03, 0x00, 0x00, 0x00 };
00703     if (!write(sense6, sizeof(sense6))) {
00704         return false;
00705     }
00706     return true;
00707 }
00708 
00709 bool USBMSD::modeSense10(void)
00710 {
00711     uint8_t sense10[] = { 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
00712     if (!write(sense10, sizeof(sense10))) {
00713         return false;
00714     }
00715     return true;
00716 }
00717 
00718 void USBMSD::sendCSW()
00719 {
00720     _csw.Signature = CSW_Signature;
00721     _write_next((uint8_t *)&_csw, sizeof(CSW));
00722     _stage = READ_CBW;
00723 }
00724 
00725 bool USBMSD::requestSense(void)
00726 {
00727     uint8_t request_sense[] = {
00728         0x70,
00729         0x00,
00730         0x05,   // Sense Key: illegal request
00731         0x00,
00732         0x00,
00733         0x00,
00734         0x00,
00735         0x0A,
00736         0x00,
00737         0x00,
00738         0x00,
00739         0x00,
00740         0x30,
00741         0x01,
00742         0x00,
00743         0x00,
00744         0x00,
00745         0x00,
00746     };
00747 
00748     if (!write(request_sense, sizeof(request_sense))) {
00749         return false;
00750     }
00751 
00752     return true;
00753 }
00754 
00755 void USBMSD::fail()
00756 {
00757     _csw.Status = CSW_FAILED;
00758     sendCSW();
00759 }
00760 
00761 
00762 void USBMSD::CBWDecode(uint8_t *buf, uint16_t size)
00763 {
00764     if (size == sizeof(_cbw)) {
00765         memcpy((uint8_t *)&_cbw, buf, size);
00766         if (_cbw.Signature == CBW_Signature) {
00767             _csw.Tag = _cbw.Tag;
00768             _csw.DataResidue = _cbw.DataLength;
00769             if ((_cbw.CBLength <  1) || (_cbw.CBLength > 16)) {
00770                 fail();
00771             } else {
00772                 switch (_cbw.CB[0]) {
00773                     case TEST_UNIT_READY:
00774                         testUnitReady();
00775                         break;
00776                     case REQUEST_SENSE:
00777                         requestSense();
00778                         break;
00779                     case INQUIRY:
00780                         inquiryRequest();
00781                         break;
00782                     case MODE_SENSE6:
00783                         modeSense6();
00784                         break;
00785                     case READ_FORMAT_CAPACITIES:
00786                         readFormatCapacity();
00787                         break;
00788                     case READ_CAPACITY:
00789                         readCapacity();
00790                         break;
00791                     case READ10:
00792                     case READ12:
00793                         if (infoTransfer()) {
00794                             if ((_cbw.Flags & 0x80)) {
00795                                 _stage = PROCESS_CBW;
00796                                 memoryRead();
00797                             } else {
00798                                 endpoint_stall(_bulk_out);
00799                                 _csw.Status = CSW_ERROR;
00800                                 sendCSW();
00801                             }
00802                         }
00803                         break;
00804                     case WRITE10:
00805                     case WRITE12:
00806                         if (infoTransfer()) {
00807                             if (!(_cbw.Flags & 0x80)) {
00808                                 _stage = PROCESS_CBW;
00809                             } else {
00810                                 endpoint_stall(_bulk_in);
00811                                 _csw.Status = CSW_ERROR;
00812                                 sendCSW();
00813                             }
00814                         }
00815                         break;
00816                     case VERIFY10:
00817                         if (!(_cbw.CB[1] & 0x02)) {
00818                             _csw.Status = CSW_PASSED;
00819                             sendCSW();
00820                             break;
00821                         }
00822                         if (infoTransfer()) {
00823                             if (!(_cbw.Flags & 0x80)) {
00824                                 _stage = PROCESS_CBW;
00825                                 _mem_ok = true;
00826                             } else {
00827                                 endpoint_stall(_bulk_in);
00828                                 _csw.Status = CSW_ERROR;
00829                                 sendCSW();
00830                             }
00831                         }
00832                         break;
00833                     case MEDIA_REMOVAL:
00834                         _csw.Status = CSW_PASSED;
00835                         sendCSW();
00836                         _media_removed = true;
00837                         break;
00838                     case MODE_SENSE10:
00839                         modeSense10();
00840                         break;
00841                     default:
00842                         fail();
00843                         break;
00844                 }
00845             }
00846         }
00847     }
00848 }
00849 
00850 void USBMSD::testUnitReady(void)
00851 {
00852 
00853     if (_cbw.DataLength != 0) {
00854         if ((_cbw.Flags & 0x80) != 0) {
00855             endpoint_stall(_bulk_in);
00856         } else {
00857             endpoint_stall(_bulk_out);
00858         }
00859     }
00860 
00861     _csw.Status = CSW_PASSED;
00862     sendCSW();
00863 }
00864 
00865 
00866 void USBMSD::memoryRead(void)
00867 {
00868     uint32_t n;
00869 
00870     n = (_length > MAX_PACKET) ? MAX_PACKET : _length;
00871 
00872     if ((_addr + n) > _memory_size) {
00873         n = _memory_size - _addr;
00874         _stage = ERROR;
00875     }
00876 
00877     // we read an entire block
00878     if (!(_addr % _block_size)) {
00879         disk_read(_page, _addr / _block_size, 1);
00880     }
00881 
00882     // write data which are in RAM
00883     _write_next(&_page[_addr % _block_size], MAX_PACKET);
00884 
00885     _addr += n;
00886     _length -= n;
00887 
00888     _csw.DataResidue -= n;
00889 
00890     if (!_length || (_stage != PROCESS_CBW)) {
00891         _csw.Status = (_stage == PROCESS_CBW) ? CSW_PASSED : CSW_FAILED;
00892         _stage = (_stage == PROCESS_CBW) ? SEND_CSW : _stage;
00893     }
00894 }
00895 
00896 
00897 bool USBMSD::infoTransfer(void)
00898 {
00899     uint32_t n;
00900 
00901     // Logical Block Address of First Block
00902     n = (_cbw.CB[2] << 24) | (_cbw.CB[3] << 16) | (_cbw.CB[4] <<  8) | (_cbw.CB[5] <<  0);
00903 
00904     _addr = n * _block_size;
00905 
00906     // Number of Blocks to transfer
00907     switch (_cbw.CB[0]) {
00908         case READ10:
00909         case WRITE10:
00910         case VERIFY10:
00911             n = (_cbw.CB[7] <<  8) | (_cbw.CB[8] <<  0);
00912             break;
00913 
00914         case READ12:
00915         case WRITE12:
00916             n = (_cbw.CB[6] << 24) | (_cbw.CB[7] << 16) | (_cbw.CB[8] <<  8) | (_cbw.CB[9] <<  0);
00917             break;
00918     }
00919 
00920     _length = n * _block_size;
00921 
00922     if (!_cbw.DataLength) {              // host requests no data
00923         _csw.Status = CSW_FAILED;
00924         sendCSW();
00925         return false;
00926     }
00927 
00928     if (_cbw.DataLength != _length) {
00929         if ((_cbw.Flags & 0x80) != 0) {
00930             endpoint_stall(_bulk_in);
00931         } else {
00932             endpoint_stall(_bulk_out);
00933         }
00934 
00935         _csw.Status = CSW_FAILED;
00936         sendCSW();
00937         return false;
00938     }
00939 
00940     return true;
00941 }
00942 
00943 void USBMSD::msd_reset()
00944 {
00945     _stage = READ_CBW;
00946 }