1 files changed, 53 insertions, 43 deletions

diff --git a/supervisor/shared/usb/usb_msc_flash.c b/supervisor/shared/usb/usb_msc_flash.c
index f6f0fa9c3..205cb7b50 100644
--- a/supervisor/shared/usb/usb_msc_flash.c
+++ b/supervisor/shared/usb/usb_msc_flash.c
@@ -59,59 +59,18 @@ static fs_user_mount_t* get_vfs(int lun) {
 }
 
 // Callback invoked when received an SCSI command not in built-in list below
-// - READ_CAPACITY10, READ_FORMAT_CAPACITY, INQUIRY, MODE_SENSE6, REQUEST_SENSE
+// - READ_CAPACITY10, READ_FORMAT_CAPACITY, INQUIRY, TEST_UNIT_READY, START_STOP_UNIT, MODE_SENSE6, REQUEST_SENSE
 // - READ10 and WRITE10 have their own callbacks
 int32_t tud_msc_scsi_cb (uint8_t lun, const uint8_t scsi_cmd[16], void* buffer, uint16_t bufsize) {
     const void* response = NULL;
     int32_t resplen = 0;
 
     switch ( scsi_cmd[0] ) {
-        case SCSI_CMD_TEST_UNIT_READY:
-            // Command that host uses to check our readiness before sending other commands
-            resplen = 0;
-            if (lun > 1) {
-                resplen = -1;
-            } else {
-                fs_user_mount_t* current_mount = get_vfs(lun);
-                if (current_mount == NULL) {
-                    resplen = -1;
-                }
-                if (ejected[lun]) {
-                    resplen = -1;
-                }
-            }
-        break;
-
         case SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL:
             // Host is about to read/write etc ... better not to disconnect disk
             resplen = 0;
         break;
 
-        case SCSI_CMD_START_STOP_UNIT:
-        {
-            // Host try to eject/safe remove/poweroff us. We could safely disconnect with disk storage, or go into lower power
-            const scsi_start_stop_unit_t* start_stop = (const scsi_start_stop_unit_t*) scsi_cmd;
-            // Start bit = 0 : low power mode, if load_eject = 1 : unmount disk storage as well
-            // Start bit = 1 : Ready mode, if load_eject = 1 : mount disk storage
-            resplen = 0;
-            if (start_stop->load_eject == 1) {
-                if (lun > 1) {
-                    resplen = -1;
-                } else {
-                    fs_user_mount_t* current_mount = get_vfs(lun);
-                    if (current_mount == NULL) {
-                        resplen = -1;
-                    }
-                    if (disk_ioctl(current_mount, CTRL_SYNC, NULL) != RES_OK) {
-                        resplen = -1;
-                    } else {
-                        ejected[lun] = true;
-                    }
-                }
-            }
-        }
-        break;
-
         default:
           // Set Sense = Invalid Command Operation
           tud_msc_set_sense(lun, SCSI_SENSE_ILLEGAL_REQUEST, 0x20, 0x00);
@@ -127,7 +86,7 @@ int32_t tud_msc_scsi_cb (uint8_t lun, const uint8_t scsi_cmd[16], void* buffer,
     }
 
     // copy response to stack's buffer if any
-    if ( response && resplen ) {
+    if ( response && (resplen > 0) ) {
         memcpy(buffer, response, resplen);
     }
 
@@ -206,3 +165,54 @@ void tud_msc_write10_complete_cb (uint8_t lun) {
     // This write is complete, start the autoreload clock.
     autoreload_start();
 }
+
+// Invoked when received SCSI_CMD_INQUIRY
+// Application fill vendor id, product id and revision with string up to 8, 16, 4 characters respectively
+void tud_msc_inquiry_cb(uint8_t lun, uint8_t vendor_id[8], uint8_t product_id[16], uint8_t product_rev[4]) {
+    (void) lun;
+
+    memcpy(vendor_id  , CFG_TUD_MSC_VENDOR     , strlen(CFG_TUD_MSC_VENDOR));
+    memcpy(product_id , CFG_TUD_MSC_PRODUCT    , strlen(CFG_TUD_MSC_PRODUCT));
+    memcpy(product_rev, CFG_TUD_MSC_PRODUCT_REV, strlen(CFG_TUD_MSC_PRODUCT_REV));
+}
+
+// Invoked when received Test Unit Ready command.
+// return true allowing host to read/write this LUN e.g SD card inserted
+bool tud_msc_test_unit_ready_cb(uint8_t lun) {
+    if (lun > 1) {
+        return false;
+    }
+
+    fs_user_mount_t* current_mount = get_vfs(lun);
+    if (current_mount == NULL) {
+        return false;
+    }
+    if (ejected[lun]) {
+        return false;
+    }
+
+    return true;
+}
+
+// Invoked when received Start Stop Unit command
+// - Start = 0 : stopped power mode, if load_eject = 1 : unload disk storage
+// - Start = 1 : active mode, if load_eject = 1 : load disk storage
+bool tud_msc_start_stop_cb(uint8_t lun, uint8_t power_condition, bool start, bool load_eject) {
+    if (load_eject) {
+        if (lun > 1) {
+            return false;
+        } else {
+            fs_user_mount_t* current_mount = get_vfs(lun);
+            if (current_mount == NULL) {
+                return false;
+            }
+            if (disk_ioctl(current_mount, CTRL_SYNC, NULL) != RES_OK) {
+                return false;
+            } else {
+                ejected[lun] = true;
+            }
+        }
+    }
+
+    return true;
+}