diff options
Diffstat (limited to 'class/msc')
| -rw-r--r-- | class/msc/usbd_msc.c | 41 | ||||
| -rw-r--r-- | class/msc/usbh_msc.c | 8 |
2 files changed, 46 insertions, 3 deletions
diff --git a/class/msc/usbd_msc.c b/class/msc/usbd_msc.c index 6523ffb9..58e78c2c 100644 --- a/class/msc/usbd_msc.c +++ b/class/msc/usbd_msc.c @@ -103,11 +103,12 @@ void msc_storage_notify_handler(uint8_t busid, uint8_t event, void *arg) #if defined(CONFIG_USBDEV_MSC_THREAD) g_usbd_msc[busid].usbd_msc_mq = usb_osal_mq_create(1); if (g_usbd_msc[busid].usbd_msc_mq == NULL) { - USB_LOG_ERR("No memory to alloc for g_usbd_msc[busid].usbd_msc_mq\r\n"); + return; } g_usbd_msc[busid].usbd_msc_thread = usb_osal_thread_create("usbd_msc", CONFIG_USBDEV_MSC_STACKSIZE, CONFIG_USBDEV_MSC_PRIO, usbdev_msc_thread, (void *)(uint32_t)busid); if (g_usbd_msc[busid].usbd_msc_thread == NULL) { - USB_LOG_ERR("No memory to alloc for g_usbd_msc[busid].usbd_msc_thread\r\n"); + usb_osal_mq_delete(g_usbd_msc[busid].usbd_msc_mq); + return; } #elif defined(CONFIG_USBDEV_MSC_POLLING) g_usbd_msc[busid].event = 0; @@ -504,6 +505,39 @@ static bool SCSI_readCapacity10(uint8_t busid, uint8_t **data, uint32_t *len) return true; } +static bool SCSI_readCapacity16(uint8_t busid, uint8_t **data, uint32_t *len) +{ + uint8_t capacity16[32]; + uint32_t last_lba; + uint32_t blk_size; + + /* service action 0x10 = READ CAPACITY(16); other service actions unsupported */ + if ((g_usbd_msc[busid].cbw.dDataLength == 0U) || + ((g_usbd_msc[busid].cbw.CB[1] & 0x1fU) != 0x10U)) { + SCSI_SetSenseData(busid, SCSI_KCQIR_INVALIDCOMMAND); + return false; + } + + last_lba = g_usbd_msc[busid].scsi_blk_nbr[g_usbd_msc[busid].cbw.bLUN] - 1U; + blk_size = g_usbd_msc[busid].scsi_blk_size[g_usbd_msc[busid].cbw.bLUN]; + + memset(capacity16, 0, sizeof(capacity16)); + /* 8-byte LBA (project volume < 4G, upper 4 bytes stay 0) */ + capacity16[4] = (uint8_t)((last_lba >> 24) & 0xff); + capacity16[5] = (uint8_t)((last_lba >> 16) & 0xff); + capacity16[6] = (uint8_t)((last_lba >> 8) & 0xff); + capacity16[7] = (uint8_t)(last_lba & 0xff); + /* 4-byte block length */ + capacity16[8] = (uint8_t)((blk_size >> 24) & 0xff); + capacity16[9] = (uint8_t)((blk_size >> 16) & 0xff); + capacity16[10] = (uint8_t)((blk_size >> 8) & 0xff); + capacity16[11] = (uint8_t)(blk_size & 0xff); + + memcpy(*data, (uint8_t *)capacity16, sizeof(capacity16)); + *len = sizeof(capacity16); + return true; +} + static bool SCSI_read10(uint8_t busid, uint8_t **data, uint32_t *len) { (void)data; @@ -727,6 +761,9 @@ static bool SCSI_CBWDecode(uint8_t busid, uint32_t nbytes) case SCSI_CMD_READCAPACITY10: ret = SCSI_readCapacity10(busid, &buf2send, &len2send); break; + case SCSI_CMD_READCAPACITY16: + ret = SCSI_readCapacity16(busid, &buf2send, &len2send); + break; case SCSI_CMD_READ10: ret = SCSI_read10(busid, NULL, 0); break; diff --git a/class/msc/usbh_msc.c b/class/msc/usbh_msc.c index cec9eb0c..35db3447 100644 --- a/class/msc/usbh_msc.c +++ b/class/msc/usbh_msc.c @@ -52,6 +52,7 @@ static void usbh_msc_class_free(struct usbh_msc *msc_class) static int usbh_msc_get_maxlun(struct usbh_msc *msc_class, uint8_t *buffer) { struct usb_setup_packet *setup; + int ret; if (!msc_class || !msc_class->hport) { return -USB_ERR_INVAL; @@ -64,7 +65,12 @@ static int usbh_msc_get_maxlun(struct usbh_msc *msc_class, uint8_t *buffer) setup->wIndex = msc_class->intf; setup->wLength = 1; - return usbh_control_transfer(msc_class->hport, setup, buffer); + ret = usbh_control_transfer(msc_class->hport, setup, buffer); + if (ret < 1) { + return ret; + } + + return ret; } static void usbh_msc_cbw_dump(struct CBW *cbw) |