stm32/usbd_msc: Provide custom irquiry processing by MSC interface.

So the MSC interface can customise the inquiry response based on the
attached logical units.

3 files changed, 64 insertions, 25 deletions

diff --git a/ports/stm32/usbd_msc_interface.c b/ports/stm32/usbd_msc_interface.c
index 493dece1f..6a81d14f6 100644
--- a/ports/stm32/usbd_msc_interface.c
+++ b/ports/stm32/usbd_msc_interface.c
@@ -57,6 +57,21 @@ static inline bool lu_flag_is_set(uint8_t lun, uint8_t flag) {
     return usbd_msc_lu_flags & (flag << (lun * 2));
 }
 
+STATIC const uint8_t usbd_msc_vpd00[6] = {
+    0x00, // peripheral qualifier; peripheral device type
+    0x00, // page code
+    0x00, // reserved
+    2, // page length (additional bytes beyond this entry)
+    0x00, // page 0x00 supported
+    0x83, // page 0x83 supported
+};
+
+STATIC const uint8_t usbd_msc_vpd83[4] = {
+    0x00, // peripheral qualifier; peripheral device type
+    0x83, // page code
+    0x00, 0x00, // page length (additional bytes beyond this entry)
+};
+
 STATIC const int8_t usbd_msc_inquiry_data[36] = {
     0x00, // peripheral qualifier; peripheral device type
     0x80, // 0x00 for a fixed drive, 0x80 for a removable drive
@@ -152,6 +167,48 @@ STATIC int8_t usbd_msc_Init(uint8_t lun_in) {
     return 0;
 }
 
+// Process SCSI INQUIRY command for the logical unit
+STATIC int usbd_msc_Inquiry(uint8_t lun, const uint8_t *params, uint8_t *data_out) {
+    if (params[1] & 1) {
+        // EVPD set - return vital product data parameters
+        uint8_t page_code = params[2];
+        switch (page_code) {
+            case 0x00: // Supported VPD pages
+                memcpy(data_out, usbd_msc_vpd00, sizeof(usbd_msc_vpd00));
+                return sizeof(usbd_msc_vpd00);
+            case 0x83: // Device identification
+                memcpy(data_out, usbd_msc_vpd83, sizeof(usbd_msc_vpd83));
+                return sizeof(usbd_msc_vpd83);
+            default: // Unsupported
+                return -1;
+        }
+    }
+
+    // A standard inquiry
+
+    if (lun >= usbd_msc_lu_num) {
+        return -1;
+    }
+    const void *lu = usbd_msc_lu_data[lun];
+
+    uint8_t alloc_len = params[3] << 8 | params[4];
+    int len = MIN(sizeof(usbd_msc_inquiry_data), alloc_len);
+    memcpy(data_out, usbd_msc_inquiry_data, len);
+
+    if (len == sizeof(usbd_msc_inquiry_data)) {
+        if (lu == &pyb_sdcard_type) {
+            memcpy(data_out + 24, "SDCard", sizeof("SDCard") - 1);
+        }
+        #if MICROPY_HW_ENABLE_MMCARD
+        else if (lu == &pyb_mmcard_type) {
+            memcpy(data_out + 24, "MMCard", sizeof("MMCard") - 1);
+        }
+        #endif
+    }
+
+    return len;
+}
+
 // Get storage capacity of a logical unit
 STATIC int8_t usbd_msc_GetCapacity(uint8_t lun, uint32_t *block_num, uint16_t *block_size) {
     uint32_t block_size_u32 = 0;
@@ -251,6 +308,7 @@ STATIC int8_t usbd_msc_GetMaxLun(void) {
 // Table of operations for the SCSI layer to call
 const USBD_StorageTypeDef usbd_msc_fops = {
     usbd_msc_Init,
+    usbd_msc_Inquiry,
     usbd_msc_GetCapacity,
     usbd_msc_IsReady,
     usbd_msc_IsWriteProtected,
@@ -259,5 +317,4 @@ const USBD_StorageTypeDef usbd_msc_fops = {
     usbd_msc_Read,
     usbd_msc_Write,
     usbd_msc_GetMaxLun,
-    (int8_t *)usbd_msc_inquiry_data,
 };
diff --git a/ports/stm32/usbdev/class/inc/usbd_cdc_msc_hid.h b/ports/stm32/usbdev/class/inc/usbd_cdc_msc_hid.h
index d46f763d1..eaa39f187 100644
--- a/ports/stm32/usbdev/class/inc/usbd_cdc_msc_hid.h
+++ b/ports/stm32/usbdev/class/inc/usbd_cdc_msc_hid.h
@@ -61,6 +61,7 @@ typedef struct {
 
 typedef struct _USBD_STORAGE {
   int8_t (* Init) (uint8_t lun);
+  int    (* Inquiry) (uint8_t lun, const uint8_t *params, uint8_t *data_out);
   int8_t (* GetCapacity) (uint8_t lun, uint32_t *block_num, uint16_t *block_size);
   int8_t (* IsReady) (uint8_t lun);
   int8_t (* IsWriteProtected) (uint8_t lun);
@@ -69,7 +70,6 @@ typedef struct _USBD_STORAGE {
   int8_t (* Read) (uint8_t lun, uint8_t *buf, uint32_t blk_addr, uint16_t blk_len);
   int8_t (* Write)(uint8_t lun, uint8_t *buf, uint32_t blk_addr, uint16_t blk_len);
   int8_t (* GetMaxLun)(void);
-  int8_t *pInquiry;
 } USBD_StorageTypeDef;
 
 typedef struct {
diff --git a/ports/stm32/usbdev/class/src/usbd_msc_scsi.c b/ports/stm32/usbdev/class/src/usbd_msc_scsi.c
index 9da903377..8651b3ca8 100644
--- a/ports/stm32/usbdev/class/src/usbd_msc_scsi.c
+++ b/ports/stm32/usbdev/class/src/usbd_msc_scsi.c
@@ -225,33 +225,15 @@ static int8_t SCSI_TestUnitReady(USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t
 */
 static int8_t  SCSI_Inquiry(USBD_HandleTypeDef  *pdev, uint8_t lun, uint8_t *params)
 {
-  uint8_t* pPage;
-  uint16_t len;
   USBD_MSC_BOT_HandleTypeDef *hmsc = &((usbd_cdc_msc_hid_state_t*)pdev->pClassData)->MSC_BOT_ClassData;
 
-  if (params[1] & 0x01)/*Evpd is set*/
+  int res = hmsc->bdev_ops->Inquiry(lun, params, hmsc->bot_data);
+  if (res < 0)
   {
-    pPage = (uint8_t *)MSC_Page00_Inquiry_Data;
-    len = LENGTH_INQUIRY_PAGE00;
-  }
-  else
-  {
-
-    pPage = (uint8_t *)&hmsc->bdev_ops->pInquiry[lun * STANDARD_INQUIRY_DATA_LEN];
-    len = pPage[4] + 5;
-
-    if (params[4] <= len)
-    {
-      len = params[4];
-    }
-  }
-  hmsc->bot_data_length = len;
-
-  while (len)
-  {
-    len--;
-    hmsc->bot_data[len] = pPage[len];
+    SCSI_SenseCode(pdev, lun, ILLEGAL_REQUEST, INVALID_CDB);
+    return -1;
   }
+  hmsc->bot_data_length = res;
   return 0;
 }