USB_MSD. Added READ_FORMAT_CAPACITIES handler

1 files changed, 30 insertions, 5 deletions

diff --git a/os/hal/src/hal_usb_msd.c b/os/hal/src/hal_usb_msd.c
index 9c49192..068d698 100644
--- a/os/hal/src/hal_usb_msd.c
+++ b/os/hal/src/hal_usb_msd.c
@@ -262,22 +262,47 @@ bool msd_request_hook(USBDriver *usbp) {
       /* check that it is a HOST2DEV request */
       if (((usbp->setup[0] & USB_RTYPE_DIR_MASK) != USB_RTYPE_DIR_HOST2DEV) ||
          (MSD_SETUP_LENGTH(usbp->setup) != 0) ||
-         (MSD_SETUP_VALUE(usbp->setup) != 0))
+         (MSD_SETUP_VALUE(usbp->setup) != 0)) {
         return false;
+      }
 
-      /* reset all endpoints */
-      /* TODO!*/
+      /*
+      As required by the BOT specification, the Bulk-only mass storage reset request (classspecific
+      request) is implemented. This request is used to reset the mass storage device and
+      its associated interface. This class-specific request should prepare the device for the next
+      CBW from the host.
+      To generate the BOT Mass Storage Reset, the host must send a device request on the
+      default pipe of:
+      • bmRequestType: Class, interface, host to device
+      • bRequest field set to 255 (FFh)
+      • wValue field set to ‘0’
+      • wIndex field set to the interface number
+      • wLength field set to ‘0’
+      */
+      chSysLockFromISR();
+
+      /* release and abandon current transmission */
+      usbStallReceiveI(usbp, 1);
+      usbStallTransmitI(usbp, 1);
       /* The device shall NAK the status stage of the device request until
        * the Bulk-Only Mass Storage Reset is complete.
-       */
+       * NAK EP1 in and out */
+      usbp->otg->ie[1].DIEPCTL = DIEPCTL_SNAK;
+      usbp->otg->oe[1].DOEPCTL = DOEPCTL_SNAK;
+
+      chSysUnlockFromISR();
+
+      /* response to this request using EP0 */
+      usbSetupTransfer(usbp, 0, 0, NULL);
       return true;
 
     case MSD_GET_MAX_LUN:
       /* check that it is a DEV2HOST request */
       if (((usbp->setup[0] & USB_RTYPE_DIR_MASK) != USB_RTYPE_DIR_DEV2HOST) ||
          (MSD_SETUP_LENGTH(usbp->setup) != 1) ||
-         (MSD_SETUP_VALUE(usbp->setup) != 0))
+         (MSD_SETUP_VALUE(usbp->setup) != 0)) {
         return false;
+      }
 
       /* stall to indicate that we don't support LUN */
       osalSysLockFromISR();