diff --git a/demos/device/device_os_none/mscd_app.c b/demos/device/device_os_none/mscd_app.c
index 64681f20d..cf42d0582 100644
--- a/demos/device/device_os_none/mscd_app.c
+++ b/demos/device/device_os_none/mscd_app.c
@@ -46,6 +46,12 @@
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
+enum
+{
+  DISK_CAPACITY   = 16 * 1024 * 1024,
+  DISK_BLOCK_SIZE = 512
+};
+
 static scsi_inquiry_data_t mscd_inquiry_data TUSB_CFG_ATTR_USBRAM =
 {
     .is_removable         = 1,
@@ -54,12 +60,27 @@ static scsi_inquiry_data_t mscd_inquiry_data TUSB_CFG_ATTR_USBRAM =
     .vendor_id            = "tinyusb",
     .product_id           = "MSC Example",
     .product_revision     = "0.01"
-} ;
+};
 
 static scsi_read_capacity10_data_t mscd_read_capacity10_data TUSB_CFG_ATTR_USBRAM =
 {
-    .last_lba   = (16*1024*1024)/512,
-    .block_size = 512
+    .last_lba   = DISK_CAPACITY / DISK_BLOCK_SIZE, // read capacity
+    .block_size = DISK_BLOCK_SIZE
+};
+
+static scsi_sense_fixed_data_t mscd_sense_data TUSB_CFG_ATTR_USBRAM =
+{
+    .response_code        = 0x70,
+    .sense_key            = 0, // no errors
+    .additional_sense_len = sizeof(scsi_sense_fixed_data_t) - 8
+};
+
+static scsi_read_format_capacity_data_t mscd_format_capacity_data TUSB_CFG_ATTR_USBRAM =
+{
+    .list_length     = 8,
+    .block_num       = DISK_CAPACITY / DISK_BLOCK_SIZE, // write capacity
+    .descriptor_type = 2, // TODO formatted media, refractor to const
+    .block_size      = DISK_BLOCK_SIZE
 };
 
 //--------------------------------------------------------------------+
@@ -69,16 +90,28 @@ static scsi_read_capacity10_data_t mscd_read_capacity10_data TUSB_CFG_ATTR_USBRA
 //--------------------------------------------------------------------+
 // tinyusb callback (ISR context)
 //--------------------------------------------------------------------+
-msc_csw_status_t tusbd_msc_scsi_received_isr(uint8_t coreid, uint8_t lun, uint8_t scsi_cmd[16], void ** pp_buffer, uint16_t expected_length)
+msc_csw_status_t tusbd_msc_scsi_received_isr (uint8_t coreid, uint8_t lun, uint8_t scsi_cmd[16], void ** pp_buffer, uint16_t* p_length)
 {
-  switch(scsi_cmd[0])
+  switch (scsi_cmd[0])
   {
     case SCSI_CMD_INQUIRY:
       (*pp_buffer) = &mscd_inquiry_data;
+      (*p_length)  = sizeof(scsi_inquiry_data_t);
     break;
 
     case SCSI_CMD_READ_CAPACITY_10:
       (*pp_buffer) = &mscd_read_capacity10_data;
+      (*p_length)  = sizeof(scsi_read_capacity10_data_t);
+    break;
+
+    case SCSI_CMD_REQUEST_SENSE:
+      (*pp_buffer) = &mscd_sense_data;
+      (*p_length)  = sizeof(scsi_sense_fixed_data_t);
+    break;
+
+    case  SCSI_CMD_READ_FORMAT_CAPACITY:
+      (*pp_buffer) = &mscd_format_capacity_data;
+      (*p_length)  = sizeof(scsi_read_format_capacity_data_t);
     break;
 
     default: return MSC_CSW_STATUS_FAILED;
diff --git a/tinyusb/class/msc.h b/tinyusb/class/msc.h
index 69ca0851d..d8295cf7c 100644
--- a/tinyusb/class/msc.h
+++ b/tinyusb/class/msc.h
@@ -121,12 +121,13 @@ STATIC_ASSERT(sizeof(msc_cmd_status_wrapper_t) == 13, "size is not correct");
 
 /// SCSI Command Operation Code
 typedef enum {
-  SCSI_CMD_TEST_UNIT_READY  = 0x00, ///< The SCSI Test Unit Ready command is used to determine if a device is ready to transfer data (read/write), i.e. if a disk has spun up, if a tape is loaded and ready etc. The device does not perform a self-test operation.
-  SCSI_CMD_INQUIRY          = 0x12, ///< The SCSI Inquiry command is used to obtain basic information from a target device.
-  SCSI_CMD_READ_CAPACITY_10 = 0x25, ///< The SCSI Read Capacity command is used to obtain data capacity information from a target device.
-  SCSI_CMD_REQUEST_SENSE    = 0x03, ///< The SCSI Request Sense command is part of the SCSI computer protocol standard. This command is used to obtain sense data -- status/error information -- from a target device.
-  SCSI_CMD_READ_10          = 0x28, ///< The READ (10) command requests that the device server read the specified logical block(s) and transfer them to the data-in buffer.
-  SCSI_CMD_WRITE_10         = 0x2A, ///< The WRITE (10) command requests thatthe device server transfer the specified logical block(s) from the data-out buffer and write them.
+  SCSI_CMD_TEST_UNIT_READY      = 0x00, ///< The SCSI Test Unit Ready command is used to determine if a device is ready to transfer data (read/write), i.e. if a disk has spun up, if a tape is loaded and ready etc. The device does not perform a self-test operation.
+  SCSI_CMD_INQUIRY              = 0x12, ///< The SCSI Inquiry command is used to obtain basic information from a target device.
+  SCSI_CMD_READ_CAPACITY_10     = 0x25, ///< The SCSI Read Capacity command is used to obtain data capacity information from a target device.
+  SCSI_CMD_REQUEST_SENSE        = 0x03, ///< The SCSI Request Sense command is part of the SCSI computer protocol standard. This command is used to obtain sense data -- status/error information -- from a target device.
+  SCSI_CMD_READ_FORMAT_CAPACITY = 0x23, ///< The command allows the Host to request a list of the possible format capacities for an installed writable media. This command also has the capability to report the writable capacity for a media when it is installed
+  SCSI_CMD_READ_10              = 0x28, ///< The READ (10) command requests that the device server read the specified logical block(s) and transfer them to the data-in buffer.
+  SCSI_CMD_WRITE_10             = 0x2A, ///< The WRITE (10) command requests thatthe device server transfer the specified logical block(s) from the data-out buffer and write them.
 }scsi_cmd_type_t;
 
 //--------------------------------------------------------------------+
@@ -201,12 +202,65 @@ typedef ATTR_PACKED_STRUCT(struct)
 
 STATIC_ASSERT(sizeof(scsi_inquiry_data_t) == 36, "size is not correct");
 
+
+typedef ATTR_PACKED_STRUCT(struct) {
+  uint8_t response_code           : 7; ///< 70h - current errors, Fixed Format 71h - deferred errors, Fixed Format
+  uint8_t valid                   : 1;
+
+  uint8_t reserved; ///< Obsolete
+
+  uint8_t sense_key               : 4;
+  uint8_t                         : 1;
+  uint8_t incorrect_len_idicatior : 1;
+  uint8_t end_of_medium           : 1;
+  uint8_t filemark                : 1;
+
+  uint32_t information;
+  uint8_t  additional_sense_len;
+  uint32_t command_specific_info;
+  uint8_t additional_sense_code;
+  uint8_t additional_sense_qualifier;
+  uint8_t field_replaceable_unit_code;
+
+  uint8_t sense_key_specific[3]; ///< sense key specific valid bit is bit 7 of key[0], aka MSB in Big Endian layout
+
+} scsi_sense_fixed_data_t;
+
+STATIC_ASSERT(sizeof(scsi_sense_fixed_data_t) == 18, "size is not correct");
+
+//--------------------------------------------------------------------+
+// SCSI MMC
+//--------------------------------------------------------------------+
+/// SCSI Read Format Capacity: Write Capacity
+typedef ATTR_PACKED_STRUCT(struct) {
+  uint8_t cmd_code;
+  uint8_t reserved[6];
+  uint16_t alloc_length;
+  uint8_t control;
+} scsi_read_format_capacity_t;
+
+STATIC_ASSERT( sizeof(scsi_read_format_capacity_t) == 10, "size is not correct");
+
+typedef ATTR_PACKED_STRUCT(struct){
+  uint8_t reserved[3];
+  uint8_t list_length; /// must be 8*n, length in bytes of formattable capacity descriptor followed it.
+
+  uint32_t block_num; /// Number of Logical Blocks
+  uint8_t  descriptor_type; // 00: reserved, 01 unformatted media , 10 Formatted media, 11 No media present
+
+  uint8_t  reserved2;
+  uint16_t block_size;
+
+} scsi_read_format_capacity_data_t;
+
+STATIC_ASSERT( sizeof(scsi_read_format_capacity_data_t) == 12, "size is not correct");
+
 //--------------------------------------------------------------------+
 // SCSI Block Command (SBC-3)
 // NOTE: All data in SCSI command are in Big Endian
 //--------------------------------------------------------------------+
 
-/// SCSI Read Capacity 10 Command
+/// SCSI Read Capacity 10 Command: Read Capacity
 typedef ATTR_PACKED_STRUCT(struct) {
   uint8_t  cmd_code                 ; ///< SCSI OpCode for \ref SCSI_CMD_READ_CAPACITY_10
   uint8_t  reserved1                ;
diff --git a/tinyusb/class/msc_device.c b/tinyusb/class/msc_device.c
index f02a1584c..d1c225242 100644
--- a/tinyusb/class/msc_device.c
+++ b/tinyusb/class/msc_device.c
@@ -141,13 +141,15 @@ void mscd_isr(endpoint_handle_t edpt_hdl, tusb_event_t event, uint32_t xferred_b
           p_msc->cbw.signature == MSC_CBW_SIGNATURE, VOID_RETURN );
 
   void *p_buffer = NULL;
+  uint16_t actual_length = p_msc->cbw.xfer_bytes;
 
-  p_msc->csw.signature = MSC_CSW_SIGNATURE;
-  p_msc->csw.tag = p_msc->cbw.tag;
-
-  p_msc->csw.status = tusbd_msc_scsi_received_isr(edpt_hdl.coreid, p_msc->cbw.lun, p_msc->cbw.command, &p_buffer, p_msc->cbw.xfer_bytes);
+  p_msc->csw.signature    = MSC_CSW_SIGNATURE;
+  p_msc->csw.tag          = p_msc->cbw.tag;
+  p_msc->csw.status       = tusbd_msc_scsi_received_isr(edpt_hdl.coreid, p_msc->cbw.lun, p_msc->cbw.command, &p_buffer, &actual_length);
   p_msc->csw.data_residue = 0; // TODO expected length, response length
 
+  ASSERT( p_msc->cbw.xfer_bytes >= actual_length, VOID_RETURN );
+
   //------------- Data Phase -------------//
   if ( BIT_TEST_(p_msc->cbw.dir, 7) && p_buffer == NULL )
   { // application does not provide data to response --> possibly unsupported SCSI command
@@ -156,7 +158,7 @@ void mscd_isr(endpoint_handle_t edpt_hdl, tusb_event_t event, uint32_t xferred_b
   }else
   {
     ASSERT( dcd_pipe_queue_xfer( BIT_TEST_(p_msc->cbw.dir, 7) ? p_msc->edpt_in : p_msc->edpt_out,
-                                 p_buffer, p_msc->cbw.xfer_bytes) == TUSB_ERROR_NONE, VOID_RETURN);
+                                 p_buffer, actual_length) == TUSB_ERROR_NONE, VOID_RETURN);
   }
 
   //------------- Status Phase -------------//
diff --git a/tinyusb/class/msc_device.h b/tinyusb/class/msc_device.h
index b267001fd..a2d73945e 100644
--- a/tinyusb/class/msc_device.h
+++ b/tinyusb/class/msc_device.h
@@ -50,7 +50,8 @@
 //--------------------------------------------------------------------+
 // APPLICATION API
 //--------------------------------------------------------------------+
-msc_csw_status_t tusbd_msc_scsi_received_isr(uint8_t coreid, uint8_t lun, uint8_t scsi_cmd[16], void ** pp_buffer, uint16_t expected_length);
+// p_length [in,out] allocated/maximum length, application update with actual length
+msc_csw_status_t tusbd_msc_scsi_received_isr (uint8_t coreid, uint8_t lun, uint8_t scsi_cmd[16], void ** pp_buffer, uint16_t* p_length);
 
 //--------------------------------------------------------------------+
 // USBD-CLASS DRIVER API
diff --git a/tinyusb/device/dcd.h b/tinyusb/device/dcd.h
index 744c77bb7..b4205d542 100644
--- a/tinyusb/device/dcd.h
+++ b/tinyusb/device/dcd.h
@@ -93,7 +93,9 @@ endpoint_handle_t dcd_pipe_open(uint8_t coreid, tusb_descriptor_endpoint_t const
 tusb_error_t dcd_pipe_queue_xfer(endpoint_handle_t edpt_hdl, void * buffer, uint16_t total_bytes) ATTR_WARN_UNUSED_RESULT; // only queue, not transferring yet
 tusb_error_t dcd_pipe_xfer(endpoint_handle_t edpt_hdl, void * buffer, uint16_t total_bytes, bool int_on_complete)  ATTR_WARN_UNUSED_RESULT;
 tusb_error_t dcd_pipe_stall(endpoint_handle_t edpt_hdl) ATTR_WARN_UNUSED_RESULT;
-tusb_error_t dcd_pipe_clear_stall(uint8_t coreid, uint8_t edpt_addr); // TODO coreid + endpoint address are part of endpoint handle, not endpoint handle
+
+// TODO coreid + endpoint address are part of endpoint handle, not endpoint handle, data toggle also need to be reset
+tusb_error_t dcd_pipe_clear_stall(uint8_t coreid, uint8_t edpt_addr);
 
 #ifdef __cplusplus
  }
diff --git a/tinyusb/device/dcd_lpc43xx.c b/tinyusb/device/dcd_lpc43xx.c
index f7a219ad8..c6231b1c0 100644
--- a/tinyusb/device/dcd_lpc43xx.c
+++ b/tinyusb/device/dcd_lpc43xx.c
@@ -360,6 +360,8 @@ tusb_error_t dcd_pipe_clear_stall(uint8_t coreid, uint8_t edpt_addr)
 {
   volatile uint32_t * reg_control = (&LPC_USB0->ENDPTCTRL0) + edpt_phy2log( edpt_addr2phy(edpt_addr) );
 
+  // data toggle also need to be reset
+  (*reg_control) |= ENDPTCTRL_MASK_TOGGLE_RESET << ((edpt_addr & TUSB_DIR_DEV_TO_HOST_MASK) ? 16 : 0);
   (*reg_control) &= ~(ENDPTCTRL_MASK_STALL << ((edpt_addr & TUSB_DIR_DEV_TO_HOST_MASK) ? 16 : 0));
 
   return TUSB_ERROR_NONE;
@@ -482,6 +484,7 @@ void xfer_complete_isr(uint8_t coreid, uint32_t reg_complete)
         //------------- Free QTD and shift array list -------------//
         p_qtd->used = 0; // free QTD
         memmove(p_qhd->list_qtd_idx, p_qhd->list_qtd_idx+1, DCD_QTD_PER_QHD_MAX-1);
+        p_qhd->list_qtd_idx[DCD_QTD_PER_QHD_MAX-1]=0;
 
         if (p_qtd->int_on_complete)
         {