[USBMSD] Implement MODE SENSE command

Author: sandeepmistry

libraries/USBMSD/PluggableUSBMSD.h

@@ -304,6 +304,7 @@ class USBMSD: public internal::PluggableUSBModule {
     bool infoTransfer(void);
     void memoryRead(void);
     bool modeSense6(void);
+    bool modeSense10(void);
     void testUnitReady(void);
     bool requestSense(void);
     void memoryVerify(uint8_t *buf, uint16_t size);

libraries/USBMSD/USBMSD.cpp

@@ -722,6 +722,15 @@ bool USBMSD::modeSense6(void)
     return true;
 }
 
+bool USBMSD::modeSense10(void)
+{
+    uint8_t sense10[] = { 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+    if (!write(sense10, sizeof(sense10))) {
+        return false;
+    }
+    return true;
+}
+
 void USBMSD::sendCSW()
 {
     _csw.Signature = CSW_Signature;
@@ -842,6 +851,9 @@ void USBMSD::CBWDecode(uint8_t *buf, uint16_t size)
                         sendCSW();
                         _media_removed = true;
                         break;
+                    case MODE_SENSE10:
+                        modeSense10();
+                        break;
                     default:
                         fail();
                         break;