API tools faq
paste
Guest User

chibios mass storage

a guest
Sep 18th, 2012
152
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 21.03 KB | None | 0 0
raw download clone embed print report
  1. #include "ch.h"
  2. #include "hal.h"
  3. #include "usb_msd.h"
  4.  
  5. static WORKING_AREA(waMassStorage, 1024);
  6. static msg_t MassStorageThd(void *arg);
  7.  
  8. static Thread *msdThd = NULL;
  9.  
  10. /* TODO: need a way of specifying the size of this */
  11. static uint8_t rw_buf[2][512];
  12.  
  13. inline uint32_t swap_uint32( uint32_t val ) {
  14.     val = ((val << 8) & 0xFF00FF00 ) | ((val >> 8) & 0xFF00FF );
  15.     return ((val << 16) & 0xFFFF0000) | ((val >> 16) & 0x0000FFFF);
  16. }
  17.  
  18. #define swap_uint16(x) ((((x) >> 8) & 0xff) | (((x) & 0xff) << 8))
  19.  
  20. #if !defined(MSD_RW_LED_ON)
  21. #define MSD_RW_LED_ON()
  22. #endif
  23.  
  24. #if !defined(MSD_RW_LED_OFF)
  25. #define MSD_RW_LED_OFF()
  26. #endif
  27.  
  28. /*===========================================================================*/
  29. /* USB related stuff.                                                        */
  30. /*===========================================================================*/
  31.  
  32. /*
  33.  * USB Driver structure.
  34.  */
  35. static USBMassStorageDriver UMSD1;
  36. /*
  37.  * USB Device Descriptor.
  38.  */
  39. static const uint8_t msd_device_descriptor_data[18] = {
  40.   USB_DESC_DEVICE       (0x0200,        /* bcdUSB (2.0).                    */
  41.                          0x00,          /* bDeviceClass (None).             */
  42.                          0x00,          /* bDeviceSubClass.                 */
  43.                          0x00,          /* bDeviceProtocol.                 */
  44.                          0x40,          /* Control Endpoint Size.           */
  45.                          0x0483,        /* idVendor (ST).                   */
  46.                          0x5742,        /* idProduct.                       */
  47.                          0x0100,        /* bcdDevice.                       */
  48.                          1,             /* iManufacturer.                   */
  49.                          2,             /* iProduct.                        */
  50.                          3,             /* iSerialNumber.                   */
  51.                          1)             /* bNumConfigurations.              */
  52. };
  53.  
  54. /*
  55.  * Device Descriptor wrapper.
  56.  */
  57. static const USBDescriptor msd_device_descriptor = {
  58.   sizeof msd_device_descriptor_data,
  59.   msd_device_descriptor_data
  60. };
  61.  
  62. /* Configuration Descriptor tree for a CDC.*/
  63. static const uint8_t msd_configuration_descriptor_data[] = {
  64.   /* Configuration Descriptor.*/
  65.   USB_DESC_CONFIGURATION(0x0020,            /* wTotalLength.                    */
  66.                          0x01,          /* bNumInterfaces.                  */
  67.                          0x01,          /* bConfigurationValue.             */
  68.                          0,             /* iConfiguration.                  */
  69.                          0xC0,          /* bmAttributes (self powered).     */
  70.                          0x32),           /* bMaxPower (100mA).               */
  71.   /* Interface Descriptor.*/
  72.   USB_DESC_INTERFACE    (0x00,          /* bInterfaceNumber.                */
  73.                          0x00,          /* bAlternateSetting.               */
  74.                          0x02,          /* bNumEndpoints.                   */
  75.                          0x00,          /* bInterfaceClass (Mass Storage) 0x08   */
  76.                          0x06,          /* bInterfaceSubClass (SCSI
  77.                                            Transparent storage class)       */
  78.                          0x50,          /* bInterfaceProtocol (Bulk Only)   */
  79.                          0),            /* iInterface. (none)               */
  80.   /* Mass Storage Data In Endpoint Descriptor.*/
  81.   USB_DESC_ENDPOINT     (USB_MS_DATA_EP|0x80,
  82.                          0x02,          /* bmAttributes (Bulk).             */
  83.                          USB_MS_EP_SIZE,/* wMaxPacketSize.                  */
  84.                          0x05),         /* bInterval. 1ms                   */
  85.   /* Mass Storage Data In Endpoint Descriptor.*/
  86.   USB_DESC_ENDPOINT     (USB_MS_DATA_EP,
  87.                          0x02,          /* bmAttributes (Bulk).             */
  88.                          USB_MS_EP_SIZE,/* wMaxPacketSize.                  */
  89.                          0x05)          /* bInterval. 1ms                   */
  90. };
  91.  
  92. /*
  93.  * Configuration Descriptor wrapper.
  94.  */
  95. static const USBDescriptor msd_configuration_descriptor = {
  96.   sizeof msd_configuration_descriptor_data,
  97.   msd_configuration_descriptor_data
  98. };
  99.  
  100. /*
  101.  * U.S. English language identifier.
  102.  */
  103. static const uint8_t msd_string0[] = {
  104.   USB_DESC_BYTE(4),                     /* bLength.                         */
  105.   USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType.                 */
  106.   USB_DESC_WORD(0x0409)                 /* wLANGID (U.S. English).          */
  107. };
  108.  
  109. /*
  110.  * Vendor string.
  111.  */
  112. static const uint8_t msd_string1[] = {
  113.   USB_DESC_BYTE(38),                    /* bLength.                         */
  114.   USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType.                 */
  115.   'S', 0, 'T', 0, 'M', 0, 'i', 0, 'c', 0, 'r', 0, 'o', 0, 'e', 0,
  116.   'l', 0, 'e', 0, 'c', 0, 't', 0, 'r', 0, 'o', 0, 'n', 0, 'i', 0,
  117.   'c', 0, 's', 0
  118. };
  119.  
  120. /*
  121.  * Device Description string.
  122.  */
  123. static const uint8_t msd_string2[] = {
  124.   USB_DESC_BYTE(62),                    /* bLength.                         */
  125.   USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType.                 */
  126.   'C', 0, 'h', 0, 'i', 0, 'b', 0, 'i', 0, 'O', 0, 'S', 0, '/', 0,
  127.   'R', 0, 'T', 0, ' ', 0, 'M', 0, 'a', 0, 's', 0, 's', 0, ' ', 0,
  128.   'S', 0, 't', 0, 'o', 0, 'r', 0, 'a', 0, 'g', 0, 'e', 0, ' ', 0,
  129.   'D', 0, 'e', 0, 'v', 0, 'i', 0, 'c', 0, 'e'
  130. };
  131.  
  132. static const uint8_t msd_string3[] = {
  133.   USB_DESC_BYTE(26),                     /* bLength.                         */
  134.   USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType.                 */
  135.   'A', 0, 'E', 0, 'C', 0, 'C', 0, 'E', 0, 'C', 0, 'C', 0, 'C', 0, 'C', 0,
  136.   '0' + CH_KERNEL_MAJOR, 0,
  137.   '0' + CH_KERNEL_MINOR, 0,
  138.   '0' + CH_KERNEL_PATCH, 0
  139. };
  140.  
  141. /*
  142.  * Strings wrappers array.
  143.  */
  144. static const USBDescriptor msd_strings[] = {
  145.   {sizeof msd_string0, msd_string0},
  146.   {sizeof msd_string1, msd_string1},
  147.   {sizeof msd_string2, msd_string2},
  148.   {sizeof msd_string3, msd_string3}
  149. };
  150.  
  151. /*
  152.  * Handles the GET_DESCRIPTOR callback. All required descriptors must be
  153.  * handled here.
  154.  */
  155. static const USBDescriptor *get_descriptor(USBDriver *usbp,
  156.                                            uint8_t dtype,
  157.                                            uint8_t dindex,
  158.                                            uint16_t lang) {
  159.  
  160.   (void)usbp;
  161.   (void)lang;
  162.   switch (dtype) {
  163.   case USB_DESCRIPTOR_DEVICE:
  164.     return &msd_device_descriptor;
  165.   case USB_DESCRIPTOR_CONFIGURATION:
  166.     return &msd_configuration_descriptor;
  167.   case USB_DESCRIPTOR_STRING:
  168.     if (dindex < 4)
  169.       return &msd_strings[dindex];
  170.   }
  171.   return NULL;
  172. }
  173.  
  174. /**
  175.  * @brief   Default requests hook.
  176.  *
  177.  * @param[in] usbp      pointer to the @p USBDriver object
  178.  * @return              The hook status.
  179.  * @retval TRUE         Message handled internally.
  180.  * @retval FALSE        Message not handled.
  181.  */
  182. bool_t msdRequestsHook(USBDriver *usbp) {
  183.     if (((usbp->setup[0] & USB_RTYPE_TYPE_MASK) == USB_RTYPE_TYPE_CLASS) &&
  184.         ((usbp->setup[0] & USB_RTYPE_RECIPIENT_MASK) == USB_RTYPE_RECIPIENT_INTERFACE)) {
  185.         /* check that the request is for interface 0.*/
  186.         if(MSD_SETUP_INDEX(usbp->setup) != 0)
  187.             return FALSE;
  188.  
  189.         /* act depending on bRequest = setup[1] */
  190.         switch(usbp->setup[1]) {
  191.         case MSD_REQ_RESET:
  192.             /* check that it is a HOST2DEV request */
  193.             if(((usbp->setup[0] & USB_RTYPE_DIR_MASK) != USB_RTYPE_DIR_HOST2DEV) ||
  194.                (MSD_SETUP_LENGTH(usbp->setup) != 0) ||
  195.                (MSD_SETUP_VALUE(usbp->setup) != 0))
  196.                 return FALSE;
  197.  
  198.             /* reset all endpoints */
  199.             /* TODO!*/
  200.             /* The device shall NAK the status stage of the device request until
  201.              * the Bulk-Only Mass Storage Reset is complete.
  202.              */
  203.             return TRUE;
  204.         case MSD_GET_MAX_LUN:
  205.             /* check that it is a DEV2HOST request */
  206.             if(((usbp->setup[0] & USB_RTYPE_DIR_MASK) != USB_RTYPE_DIR_DEV2HOST) ||
  207.                (MSD_SETUP_LENGTH(usbp->setup) != 1) ||
  208.                (MSD_SETUP_VALUE(usbp->setup) != 0))
  209.                 return FALSE;
  210.  
  211.             static uint8_t len_buf[1] = {0};
  212.             /* stall to indicate that we don't support LUN */
  213.             usbSetupTransfer(usbp, len_buf, 1, NULL);
  214.             return TRUE;
  215.         default:
  216.             return FALSE;
  217.             break;
  218.         }
  219.     }
  220.   return FALSE;
  221. }
  222.  
  223. static void WaitForISR(USBMassStorageDriver *msdp) {
  224.     /* sleep until it completes */
  225.     chSysLock();
  226.     chBSemWaitS(&msdp->bsem);
  227.     chSysUnlock();
  228. }
  229.  
  230. void msdUsbEvent(USBDriver *usbp, usbep_t ep) {
  231.     (void)usbp;
  232.     (void)ep;
  233.  
  234.     chSysLockFromIsr();
  235.     chBSemSignalI(&((USBMassStorageDriver *)usbp->param)->bsem);
  236.     chSysUnlockFromIsr();
  237. }
  238.  
  239. /**
  240.  * @brief   IN EP1 state.
  241.  */
  242. static USBInEndpointState ep1InState, ep1OutState;
  243.  
  244. /**
  245.  * @brief   EP1 initialization structure (IN only).
  246.  */
  247. static const USBEndpointConfig epDataConfig = {
  248.   USB_EP_MODE_TYPE_BULK,
  249.   NULL,
  250.   msdUsbEvent,
  251.   msdUsbEvent,
  252.   USB_MS_EP_SIZE,
  253.   USB_MS_EP_SIZE,
  254.   &ep1InState,
  255.   &ep1OutState,
  256.   1,
  257.   NULL
  258. };
  259.  
  260. /*
  261.  * Handles the USB driver global events.
  262.  */
  263. static void usb_event(USBDriver *usbp, usbevent_t event) {
  264.     USBMassStorageDriver *msdp = (USBMassStorageDriver *)usbp->param;
  265.   switch (event) {
  266.   case USB_EVENT_RESET:
  267.     return;
  268.   case USB_EVENT_ADDRESS:
  269.     return;
  270.   case USB_EVENT_CONFIGURED:
  271.     chSysLockFromIsr();
  272.     usbInitEndpointI(usbp, USB_MS_DATA_EP, &epDataConfig);
  273.     chSysUnlockFromIsr();
  274.  
  275.     /* initialise the thread */
  276.     chSysLock();
  277.     chSchReadyI(msdThd);
  278.     chSysUnlock();
  279.     return;
  280.   case USB_EVENT_SUSPEND:
  281.     return;
  282.   case USB_EVENT_WAKEUP:
  283.     return;
  284.   case USB_EVENT_STALLED:
  285.     return;
  286.   }
  287.   return;
  288. }
  289.  
  290. static const USBConfig msd_usb_config = {
  291.     usb_event,
  292.     get_descriptor,
  293.     msdRequestsHook,
  294.     NULL
  295. };
  296.  
  297. static inline void SCSISetSense(USBMassStorageDriver *msdp, uint8_t key, uint8_t acode, uint8_t aqual) {
  298.     msdp->sense.byte[2] = key;
  299.     msdp->sense.byte[12] = acode;
  300.     msdp->sense.byte[13] = aqual;
  301. }
  302.  
  303. bool_t SCSICommandInquiry(USBMassStorageDriver *msdp) {
  304.     msd_cbw_t *cbw = &(msdp->cbw);
  305.  
  306.     static const scsi_inquiry_response_t inquiry = {
  307.         0x00,                       // direct access block device
  308.         0x80,                       // removable
  309.         0x04,                       // SPC-2
  310.         0x02,                       // response data format
  311.         0x20,                       // response has 0x20 + 4 bytes
  312.         0x00,
  313.         0x00,
  314.         0x00,
  315.         "Chibios",
  316.         "Mass Storage",
  317.         {'v',CH_KERNEL_MAJOR+'0','.',CH_KERNEL_MINOR+'0'},
  318.     };
  319.  
  320.     if((cbw->scsi_cmd_data[1] & ((1 << 0) | (1 << 1))) ||
  321.             cbw->scsi_cmd_data[2]) {
  322.         /* Optional but unsupported bits set - update the SENSE key and fail the request */
  323.         SCSISetSense(   msdp,
  324.                         SCSI_SENSE_KEY_ILLEGAL_REQUEST,
  325.                         SCSI_ASENSE_INVALID_FIELD_IN_CDB,
  326.                         SCSI_ASENSEQ_NO_QUALIFIER);
  327.  
  328.         return FALSE;
  329.     }
  330.  
  331.     usbPrepareTransmit(msdp->usbp, USB_MS_DATA_EP, (uint8_t *)&inquiry,
  332.             sizeof(scsi_inquiry_response_t));
  333.  
  334.     chSysLock();
  335.     usbStartTransmitI(msdp->usbp, USB_MS_DATA_EP);
  336.     chSysUnlock();
  337.  
  338.     msdp->result = TRUE;
  339.  
  340.     /* wait for ISR */
  341.     return TRUE;
  342. }
  343.  
  344. bool_t SCSICommandRequestSense(USBMassStorageDriver *msdp) {
  345.     usbPrepareTransmit(msdp->usbp, USB_MS_DATA_EP, (uint8_t *)&msdp->sense,
  346.             sizeof(scsi_sense_response_t));
  347.  
  348.     chSysLock();
  349.     usbStartTransmitI(msdp->usbp, USB_MS_DATA_EP);
  350.     chSysUnlock();
  351.  
  352.     msdp->result = TRUE;
  353.  
  354.     /* wait for ISR */
  355.     return TRUE;
  356. }
  357.  
  358. bool_t SCSICommandReadCapacity10(USBMassStorageDriver *msdp) {
  359.     static SCSIReadCapacity10Response_t response;
  360.  
  361.     response.block_size = swap_uint32(msdp->block_dev_info.blk_size);
  362.     response.last_block_addr = swap_uint32(msdp->block_dev_info.blk_num-1);
  363.  
  364.     usbPrepareTransmit(msdp->usbp, USB_MS_DATA_EP, (uint8_t *)&response,
  365.             sizeof(SCSIReadCapacity10Response_t));
  366.  
  367.     chSysLock();
  368.     usbStartTransmitI(msdp->usbp, USB_MS_DATA_EP);
  369.     chSysUnlock();
  370.  
  371.     msdp->result = TRUE;
  372.  
  373.     /* wait for ISR */
  374.     return TRUE;
  375. }
  376.  
  377. bool_t SCSICommandSendDiagnostic(USBMassStorageDriver *msdp) {
  378.     msd_cbw_t *cbw = &(msdp->cbw);
  379.  
  380.     if(!cbw->scsi_cmd_data[1] & (1 << 2)) {
  381.         /* Only self-test supported - update SENSE key and fail the command */
  382.         SCSISetSense(   msdp,
  383.                         SCSI_SENSE_KEY_ILLEGAL_REQUEST,
  384.                         SCSI_ASENSE_INVALID_FIELD_IN_CDB,
  385.                         SCSI_ASENSEQ_NO_QUALIFIER);
  386.  
  387.         return FALSE;
  388.     }
  389.  
  390.     /* TODO: actually perform the test */
  391.     msdp->result = TRUE;
  392.  
  393.     /* don't wait for ISR */
  394.     return FALSE;
  395. }
  396.  
  397. bool_t SCSICommandStartReadWrite10(USBMassStorageDriver *msdp) {
  398.     msd_cbw_t *cbw = &(msdp->cbw);
  399.     bool_t data_cached = FALSE;
  400.  
  401.     if((cbw->scsi_cmd_data[0] == SCSI_CMD_WRITE_10) &&
  402.             blkIsWriteProtected(msdp->bbdp)) {
  403.         /* device is write protected and a write has been issued */
  404.         /* Block address is invalid, update SENSE key and return command fail */
  405.         SCSISetSense(   msdp,
  406.                         SCSI_SENSE_KEY_DATA_PROTECT,
  407.                         SCSI_ASENSE_WRITE_PROTECTED,
  408.                         SCSI_ASENSEQ_NO_QUALIFIER);
  409.         msdp->result = FALSE;
  410.         return FALSE;
  411.     }
  412.  
  413.     uint32_t rw_block_address = swap_uint32(*(uint32_t *)&cbw->scsi_cmd_data[2]);
  414.     uint16_t total = swap_uint16(*(uint16_t *)&cbw->scsi_cmd_data[7]);
  415.     uint16_t i = 0;
  416.  
  417.     if(rw_block_address >= msdp->block_dev_info.blk_num) {
  418.         /* Block address is invalid, update SENSE key and return command fail */
  419.         SCSISetSense(   msdp,
  420.                         SCSI_SENSE_KEY_DATA_PROTECT,
  421.                         SCSI_ASENSE_WRITE_PROTECTED,
  422.                         SCSI_ASENSEQ_NO_QUALIFIER);
  423.         msdp->result = FALSE;
  424.  
  425.         /* don't wait for ISR */
  426.         return FALSE;
  427.     }
  428.  
  429.     /* set state according to read / write */
  430.     msdp->state = (cbw->scsi_cmd_data[0] == SCSI_CMD_WRITE_10) ? writing : reading;
  431.  
  432.     if(msdp->state == writing) {
  433. #if 1
  434.         i = 0;
  435.         /* get the first packet */
  436.         usbPrepareReceive(msdp->usbp, USB_MS_DATA_EP, rw_buf[i % 2],
  437.             msdp->block_dev_info.blk_size);
  438.  
  439.         chSysLock();
  440.         usbStartReceiveI(msdp->usbp, USB_MS_DATA_EP);
  441.         chSysUnlock();
  442.  
  443.         WaitForISR(msdp);
  444.         /* loop over each block */
  445.         for(i = 0; i < total; i++) {
  446.             if(i < (total - 1)) {
  447.                 /* There is at least one block of data left to be read over USB */
  448.                 /* queue this read before issuing the blocking write */
  449.                 usbPrepareReceive(msdp->usbp, USB_MS_DATA_EP, rw_buf[(i+1) % 2],
  450.                     msdp->block_dev_info.blk_size);
  451.  
  452.                 chSysLock();
  453.                 usbStartReceiveI(msdp->usbp, USB_MS_DATA_EP);
  454.                 chSysUnlock();
  455.             }
  456.  
  457.             /* now write the block to the block device */
  458.             if(blkWrite(msdp->bbdp, rw_block_address++, rw_buf[i % 2], 1) == CH_FAILED) {
  459.                 /* TODO: handle this */
  460.                 chSysHalt();
  461.             }
  462.  
  463.             if(i < (total -1 )) {
  464.                 /* now wait for the USB event to complete */
  465.                 WaitForISR(msdp);
  466.             }
  467.         }
  468. #else
  469.         /* loop over each block */
  470.         for(i = 0; i < total; i++) {
  471.             /* request a usb read into rw_buf */
  472.             usbPrepareReceive(msdp->usbp, USB_MS_DATA_EP, rw_buf,
  473.                 msdp->block_dev_info.blk_size);
  474.  
  475.             chSysLock();
  476.             usbStartReceiveI(msdp->usbp, USB_MS_DATA_EP);
  477.             chSysUnlock();
  478.  
  479.             WaitForISR(msdp);
  480.  
  481.             /* now write the block to the block device */
  482.             if(blkWrite(msdp->bbdp, rw_block_address++, rw_buf, 1) == CH_FAILED) {
  483.                 /* TODO: handle this */
  484.                 chSysHalt();
  485.             }
  486.         }
  487. #endif
  488.     } else {
  489.         i = 0;
  490.         /* read the first block from block device */
  491.         if(blkRead(msdp->bbdp, rw_block_address++, rw_buf[i % 2], 1) == CH_FAILED) {
  492.             /* TODO: handle this */
  493.             chSysHalt();
  494.         }
  495.  
  496.         /* loop over each block */
  497.         for(i = 0; i < total; i++) {
  498.             /* transmit the block */
  499.             usbPrepareTransmit(msdp->usbp, USB_MS_DATA_EP, rw_buf[i % 2],
  500.                     msdp->block_dev_info.blk_size);
  501.  
  502.             chSysLock();
  503.             usbStartTransmitI(msdp->usbp, USB_MS_DATA_EP);
  504.             chSysUnlock();
  505.  
  506.             if(i < (total - 1)) {
  507.                 /* there is at least one more block to be read from device */
  508.                 /* so read that whilst the USB transfer takes place */
  509.  
  510.                 if(blkRead(msdp->bbdp, rw_block_address++, rw_buf[(i+1) % 2], 1) == CH_FAILED) {
  511.                     /* TODO: handle this */
  512.                     chSysHalt();
  513.                 }
  514.             }
  515.  
  516.             WaitForISR(msdp);
  517.         }
  518.     }
  519.  
  520.     msdp->result = TRUE;
  521.  
  522.     /* don't wait for ISR */
  523.     return FALSE;
  524. }
  525.  
  526. bool_t SCSICommandModeSense6(USBMassStorageDriver *msdp) {
  527.     /* Send an empty header response with the Write Protect flag status */
  528.     /* TODO set byte3 to 0x80 if disk is read only */
  529.     static uint8_t response[4] = {0x00, 0x00, 0x00, 0x00};
  530.  
  531.     usbPrepareTransmit(msdp->usbp, USB_MS_DATA_EP, response, 4);
  532.  
  533.     chSysLock();
  534.     usbStartTransmitI(msdp->usbp, USB_MS_DATA_EP);
  535.     chSysUnlock();
  536.  
  537.     msdp->result = TRUE;
  538.  
  539.     /* wait for ISR */
  540.     return TRUE;
  541. }
  542.  
  543. bool_t msdWaitForCommandBlock(USBMassStorageDriver *msdp) {
  544.     usbPrepareReceive(msdp->usbp, USB_MS_DATA_EP,
  545.             (uint8_t *)&msdp->cbw, sizeof(msd_cbw_t));
  546.  
  547.     chSysLock();
  548.     usbStartReceiveI(msdp->usbp, USB_MS_DATA_EP);
  549.     chSysUnlock();
  550.  
  551.     msdp->state = read_cmd_block;
  552.  
  553.     /* wait for ISR */
  554.     return TRUE;
  555. }
  556.  
  557.  
  558.  
  559. /* A command block has been received */
  560. bool_t msdReadCommandBlock(USBMassStorageDriver *msdp) {
  561.     msd_cbw_t *cbw = &(msdp->cbw);
  562.  
  563.     /* check the command */
  564.     if((cbw->signature != MSD_CBW_SIGNATURE) ||
  565.         (cbw->lun > 0) ||
  566.         ((cbw->data_len > 0) && (cbw->flags & 0x1F)) ||
  567.         (cbw->scsi_cmd_len == 0) ||
  568.         (cbw->scsi_cmd_len > 16)) {
  569.  
  570.         /* stall both IN and OUT endpoints */
  571.         chSysLockFromIsr();
  572.         usbStallReceiveI(msdp->usbp, USB_MS_DATA_EP);
  573.         chSysUnlockFromIsr();
  574.  
  575.         /* don't wait for ISR */
  576.         return FALSE;
  577.     }
  578.  
  579.     bool_t sleep = FALSE;
  580.     switch(cbw->scsi_cmd_data[0]) {
  581.     case SCSI_CMD_INQUIRY:
  582.         sleep = SCSICommandInquiry(msdp);
  583.         break;
  584.     case SCSI_CMD_REQUEST_SENSE:
  585.         sleep = SCSICommandRequestSense(msdp);
  586.         break;
  587.     case SCSI_CMD_READ_CAPACITY_10:
  588.         sleep = SCSICommandReadCapacity10(msdp);
  589.         break;
  590.     case SCSI_CMD_READ_10:
  591.     case SCSI_CMD_WRITE_10:
  592.         MSD_RW_LED_ON();
  593.         sleep = SCSICommandStartReadWrite10(msdp);
  594.         MSD_RW_LED_OFF();
  595.         break;
  596.     case SCSI_CMD_SEND_DIAGNOSTIC:
  597.         sleep = SCSICommandSendDiagnostic(msdp);
  598.         break;
  599.     case SCSI_CMD_TEST_UNIT_READY:
  600.     case SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL:
  601.     case SCSI_CMD_VERIFY_10:
  602.         /* don't handle */
  603.         msdp->result = TRUE;
  604.         break;
  605.     case SCSI_CMD_MODE_SENSE_6:
  606.         sleep = SCSICommandModeSense6(msdp);
  607.         break;
  608.     default:
  609.         SCSISetSense(   msdp,
  610.                         SCSI_SENSE_KEY_ILLEGAL_REQUEST,
  611.                         SCSI_ASENSE_INVALID_COMMAND,
  612.                         SCSI_ASENSEQ_NO_QUALIFIER);
  613.  
  614.         /* stall IN endpoint */
  615.         chSysLockFromIsr();
  616.         usbStallTransmitI(msdp->usbp, USB_MS_DATA_EP);
  617.         chSysUnlockFromIsr();
  618.  
  619.         msdp->state = idle;
  620.         cbw->data_len = 0;
  621.         return FALSE;
  622.     }
  623.  
  624.     cbw->data_len = 0;
  625.     msdp->state = send_csw;
  626.  
  627.     if(msdp->result) {
  628.         /* update sense with success status */
  629.         SCSISetSense(   msdp,
  630.                         SCSI_SENSE_KEY_GOOD,
  631.                         SCSI_ASENSE_NO_ADDITIONAL_INFORMATION,
  632.                         SCSI_ASENSEQ_NO_QUALIFIER);
  633.     } else {
  634.         /* stall IN endpoint */
  635.         chSysLockFromIsr();
  636.         usbStallTransmitI(msdp->usbp, USB_MS_DATA_EP);
  637.         chSysUnlockFromIsr();
  638.  
  639.         msdp->state = idle;
  640.         cbw->data_len = 0;
  641.         return FALSE;
  642.     }
  643.  
  644.     return sleep;
  645. }
  646.  
  647. bool_t msdSendCSW(USBMassStorageDriver *msdp) {
  648.     msd_cbw_t *cbw = &(msdp->cbw);
  649.     msd_csw_t *csw = &(msdp->csw);
  650.  
  651.     /* transition to idle state - waiting for a command block */
  652.     msdp->state = idle;
  653.  
  654.     if(!msdp->result && cbw->data_len) {
  655.         /* still bytes left to send, this is too early to send CSW? */
  656.         chSysLockFromIsr();
  657.         usbStallReceiveI(msdp->usbp, USB_MS_DATA_EP);
  658.         usbStallTransmitI(msdp->usbp, USB_MS_DATA_EP);
  659.         chSysUnlockFromIsr();
  660.  
  661.         return TRUE;
  662.     }
  663.  
  664.     csw->status = (msdp->result) ? MSD_COMMAND_PASSED : MSD_COMMAND_FAILED;
  665.     csw->signature = MSD_CSW_SIGNATURE;
  666.     csw->data_residue = cbw->data_len;
  667.     csw->tag = cbw->tag;
  668.  
  669.     usbPrepareTransmit(msdp->usbp, USB_MS_DATA_EP, (uint8_t *)csw,
  670.             sizeof(msd_csw_t));
  671.  
  672.     chSysLock();
  673.     usbStartTransmitI(msdp->usbp, USB_MS_DATA_EP);
  674.     chSysUnlock();
  675.  
  676.     return TRUE;
  677. }
  678.  
  679. static msg_t MassStorageThd(void *arg) {
  680.     USBMassStorageDriver *msdp = (USBMassStorageDriver *)arg;
  681.  
  682.     chRegSetThreadName("USB-MSD");
  683.  
  684.     bool_t wait_for_isr = FALSE;
  685.  
  686.     WaitForISR(msdp);
  687.  
  688.     while (TRUE) {
  689.         wait_for_isr = FALSE;
  690.  
  691.         /* wait on data depending on the current state */
  692.         switch(msdp->state) {
  693.         case idle:
  694.             wait_for_isr = msdWaitForCommandBlock(msdp);
  695.             break;
  696.         case read_cmd_block:
  697.             wait_for_isr = msdReadCommandBlock(msdp);
  698.             break;
  699.         case send_csw:
  700.             wait_for_isr = msdSendCSW(msdp);
  701.             break;
  702.         }
  703.  
  704.         if(!wait_for_isr)
  705.             continue;
  706.  
  707.         /* wait until the ISR wakes thread */
  708.         WaitForISR(msdp);
  709.     }
  710.     return 0;
  711. }
  712.  
  713. void msdInit(USBDriver *usbp, BaseBlockDevice *bbdp) {
  714.     uint8_t i;
  715.     UMSD1.usbp = usbp;
  716.     UMSD1.state = idle;
  717.     UMSD1.bbdp = bbdp;
  718.  
  719.     /* initialise binary semaphore as taken */
  720.     chBSemInit(&UMSD1.bsem, TRUE);
  721.  
  722.     /* initialise sense values to zero */
  723.     for(i = 0; i < sizeof(scsi_sense_response_t); i++)
  724.         UMSD1.sense.byte[i] = 0x00;
  725.     /* Response code = 0x70, additional sense length = 0x0A */
  726.     UMSD1.sense.byte[0] = 0x70;
  727.     UMSD1.sense.byte[7] = 0x0A;
  728.  
  729.     /* make sure block device is working and get info */
  730.     while(TRUE) {
  731.         blkstate_t state = blkGetDriverState(bbdp);
  732.         if(state == BLK_READY)
  733.             break;
  734.  
  735.         chThdSleepMilliseconds(50);
  736.     }
  737.  
  738.     blkGetInfo(bbdp, &UMSD1.block_dev_info);
  739.  
  740.     usbDisconnectBus(UMSD1.usbp);
  741.     chThdSleepMilliseconds(1000);
  742.     UMSD1.usbp->param = &UMSD1;
  743.  
  744.     usbStart(UMSD1.usbp, &msd_usb_config);
  745.     usbConnectBus(UMSD1.usbp);
  746.  
  747.     msdThd = chThdCreateStatic(waMassStorage, sizeof(waMassStorage), NORMALPRIO, MassStorageThd, &UMSD1);
  748. }
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!