Merge branch 'master' into vendor_class_zero_length_transfer

5 files changed, 691 insertions, 411 deletions

diff --git a/src/class/msc/msc.h b/src/class/msc/msc.h
index 84b6e4d79..bbfd35a43 100644
--- a/src/class/msc/msc.h
+++ b/src/class/msc/msc.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -53,7 +53,7 @@ enum {
 };
 
 /// \brief MassStorage Protocol.
-/// \details CBI only approved to use with full-speed floopy disk & should not used with highspeed or device other than floopy
+/// \details CBI only approved to use with full-speed floppy disk & should not used with highspeed or device other than floppy
 typedef enum
 {
   MSC_PROTOCOL_CBI              = 0 ,  ///< Control/Bulk/Interrupt protocol (with command completion interrupt)
@@ -97,7 +97,7 @@ typedef struct TU_ATTR_PACKED
 {
   uint32_t signature    ; ///< Signature that helps identify this data packet as a CSW. The signature field shall contain the value 53425355h (little endian), indicating CSW.
   uint32_t tag          ; ///< The device shall set this field to the value received in the dCBWTag of the associated CBW.
-  uint32_t data_residue ; ///< For Data-Out the device shall report in the dCSWDataResiduethe difference between the amount of data expected as stated in the dCBWDataTransferLength, and the actual amount of data processed by the device. For Data-In the device shall report in the dCSWDataResiduethe difference between the amount of data expected as stated in the dCBWDataTransferLengthand the actual amount of relevant data sent by the device
+  uint32_t data_residue ; ///< For Data-Out the device shall report in the dCSWDataResidue the difference between the amount of data expected as stated in the dCBWDataTransferLength, and the actual amount of data processed by the device. For Data-In the device shall report in the dCSWDataResiduethe difference between the amount of data expected as stated in the dCBWDataTransferLengthand the actual amount of relevant data sent by the device
   uint8_t  status       ; ///< indicates the success or failure of the command. Values from \ref msc_csw_status_t
 }msc_csw_t;
 
@@ -120,14 +120,14 @@ typedef enum
   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_WRITE_10                     = 0x2A, ///< The WRITE (10) command requests that the device server transfer the specified logical block(s) from the data-out buffer and write them.
 }scsi_cmd_type_t;
 
 /// SCSI Sense Key
 typedef enum
 {
   SCSI_SENSE_NONE            = 0x00, ///< no specific Sense Key. This would be the case for a successful command
-  SCSI_SENSE_RECOVERED_ERROR = 0x01, ///< ndicates the last command completed successfully with some recovery action performed by the disc drive.
+  SCSI_SENSE_RECOVERED_ERROR = 0x01, ///< Indicates the last command completed successfully with some recovery action performed by the disc drive.
   SCSI_SENSE_NOT_READY       = 0x02, ///< Indicates the logical unit addressed cannot be accessed.
   SCSI_SENSE_MEDIUM_ERROR    = 0x03, ///< Indicates the command terminated with a non-recovered error condition.
   SCSI_SENSE_HARDWARE_ERROR  = 0x04, ///< Indicates the disc drive detected a nonrecoverable hardware failure while performing the command or during a self test.
@@ -138,7 +138,7 @@ typedef enum
   SCSI_SENSE_ABORTED_COMMAND = 0x0b, ///< Indicates the disc drive aborted the command.
   SCSI_SENSE_EQUAL           = 0x0c, ///< Indicates a SEARCH DATA command has satisfied an equal comparison.
   SCSI_SENSE_VOLUME_OVERFLOW = 0x0d, ///< Indicates a buffered peripheral device has reached the end of medium partition and data remains in the buffer that has not been written to the medium.
-  SCSI_SENSE_MISCOMPARE      = 0x0e  ///< ndicates that the source data did not match the data read from the medium.
+  SCSI_SENSE_MISCOMPARE      = 0x0e  ///< Indicates that the source data did not match the data read from the medium.
 }scsi_sense_key_type_t;
 
 //--------------------------------------------------------------------+
diff --git a/src/class/msc/msc_device.c b/src/class/msc/msc_device.c
index 7f13b4891..588fcaaca 100644
--- a/src/class/msc/msc_device.c
+++ b/src/class/msc/msc_device.c
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -26,14 +26,21 @@
 
 #include "tusb_option.h"
 
-#if (TUSB_OPT_DEVICE_ENABLED && CFG_TUD_MSC)
+#if (CFG_TUD_ENABLED && CFG_TUD_MSC)
 
+#include "device/dcd.h"         // for faking dcd_event_xfer_complete
 #include "device/usbd.h"
 #include "device/usbd_pvt.h"
-#include "device/dcd.h"         // for faking dcd_event_xfer_complete
 
 #include "msc_device.h"
 
+// Level where CFG_TUSB_DEBUG must be at least for this driver is logged
+#ifndef CFG_TUD_MSC_LOG_LEVEL
+  #define CFG_TUD_MSC_LOG_LEVEL   CFG_TUD_LOG_LEVEL
+#endif
+
+#define TU_LOG_DRV(...)   TU_LOG(CFG_TUD_MSC_LOG_LEVEL, __VA_ARGS__)
+
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
@@ -42,7 +49,8 @@ enum
   MSC_STAGE_CMD  = 0,
   MSC_STAGE_DATA,
   MSC_STAGE_STATUS,
-  MSC_STAGE_STATUS_SENT
+  MSC_STAGE_STATUS_SENT,
+  MSC_STAGE_NEED_RESET,
 };
 
 typedef struct
@@ -57,7 +65,7 @@ typedef struct
 
   // Bulk Only Transfer (BOT) Protocol
   uint8_t  stage;
-  uint32_t total_len;
+  uint32_t total_len;   // byte to be transferred, can be smaller than total_bytes in cbw
   uint32_t xferred_len; // numbered of bytes transferred so far in the Data Stage
 
   // Sense Response Data
@@ -66,15 +74,63 @@ typedef struct
   uint8_t add_sense_qualifier;
 }mscd_interface_t;
 
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static mscd_interface_t _mscd_itf;
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t _mscd_buf[CFG_TUD_MSC_EP_BUFSIZE];
+CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN tu_static mscd_interface_t _mscd_itf;
+CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN tu_static uint8_t _mscd_buf[CFG_TUD_MSC_EP_BUFSIZE];
 
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
 //--------------------------------------------------------------------+
 static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_t* buffer, uint32_t bufsize);
 static void proc_read10_cmd(uint8_t rhport, mscd_interface_t* p_msc);
+
 static void proc_write10_cmd(uint8_t rhport, mscd_interface_t* p_msc);
+static void proc_write10_new_data(uint8_t rhport, mscd_interface_t* p_msc, uint32_t xferred_bytes);
+
+TU_ATTR_ALWAYS_INLINE static inline bool is_data_in(uint8_t dir)
+{
+  return tu_bit_test(dir, 7);
+}
+
+static inline bool send_csw(uint8_t rhport, mscd_interface_t* p_msc)
+{
+  // Data residue is always = host expect - actual transferred
+  p_msc->csw.data_residue = p_msc->cbw.total_bytes - p_msc->xferred_len;
+
+  p_msc->stage = MSC_STAGE_STATUS_SENT;
+  return usbd_edpt_xfer(rhport, p_msc->ep_in , (uint8_t*) &p_msc->csw, sizeof(msc_csw_t));
+}
+
+static inline bool prepare_cbw(uint8_t rhport, mscd_interface_t* p_msc)
+{
+  p_msc->stage = MSC_STAGE_CMD;
+  return usbd_edpt_xfer(rhport, p_msc->ep_out, (uint8_t*) &p_msc->cbw, sizeof(msc_cbw_t));
+}
+
+static void fail_scsi_op(uint8_t rhport, mscd_interface_t* p_msc, uint8_t status)
+{
+  msc_cbw_t const * p_cbw = &p_msc->cbw;
+  msc_csw_t       * p_csw = &p_msc->csw;
+
+  p_csw->status       = status;
+  p_csw->data_residue = p_msc->cbw.total_bytes - p_msc->xferred_len;
+  p_msc->stage        = MSC_STAGE_STATUS;
+
+  // failed but sense key is not set: default to Illegal Request
+  if ( p_msc->sense_key == 0 ) tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_ILLEGAL_REQUEST, 0x20, 0x00);
+
+  // If there is data stage and not yet complete, stall it
+  if ( p_cbw->total_bytes && p_csw->data_residue )
+  {
+    if ( is_data_in(p_cbw->dir) )
+    {
+      usbd_edpt_stall(rhport, p_msc->ep_in);
+    }
+    else
+    {
+      usbd_edpt_stall(rhport, p_msc->ep_out);
+    }
+  }
+}
 
 static inline uint32_t rdwr10_get_lba(uint8_t const command[])
 {
@@ -85,28 +141,78 @@ static inline uint32_t rdwr10_get_lba(uint8_t const command[])
   return tu_ntohl(lba);
 }
 
-static inline uint16_t rdwr10_get_blockcount(uint8_t const command[])
+static inline uint16_t rdwr10_get_blockcount(msc_cbw_t const* cbw)
 {
-  // use offsetof to avoid pointer to the odd/misaligned address
-  uint16_t const block_count = tu_unaligned_read16(command + offsetof(scsi_write10_t, block_count));
-
-  // block count is in Big Endian
+  uint16_t const block_count = tu_unaligned_read16(cbw->command + offsetof(scsi_write10_t, block_count));
   return tu_ntohs(block_count);
 }
 
+static inline uint16_t rdwr10_get_blocksize(msc_cbw_t const* cbw)
+{
+  // first extract block count in the command
+  uint16_t const block_count = rdwr10_get_blockcount(cbw);
+
+  // invalid block count
+  if (block_count == 0) return 0;
+
+  return (uint16_t) (cbw->total_bytes / block_count);
+}
+
+uint8_t rdwr10_validate_cmd(msc_cbw_t const* cbw)
+{
+  uint8_t status = MSC_CSW_STATUS_PASSED;
+  uint16_t const block_count = rdwr10_get_blockcount(cbw);
+
+  if ( cbw->total_bytes == 0 )
+  {
+    if ( block_count )
+    {
+      TU_LOG_DRV("  SCSI case 2 (Hn < Di) or case 3 (Hn < Do) \r\n");
+      status = MSC_CSW_STATUS_PHASE_ERROR;
+    }else
+    {
+      // no data transfer, only exist in complaint test suite
+    }
+  }else
+  {
+    if ( SCSI_CMD_READ_10 == cbw->command[0] && !is_data_in(cbw->dir) )
+    {
+      TU_LOG_DRV("  SCSI case 10 (Ho <> Di)\r\n");
+      status = MSC_CSW_STATUS_PHASE_ERROR;
+    }
+    else if ( SCSI_CMD_WRITE_10 == cbw->command[0] && is_data_in(cbw->dir) )
+    {
+      TU_LOG_DRV("  SCSI case 8 (Hi <> Do)\r\n");
+      status = MSC_CSW_STATUS_PHASE_ERROR;
+    }
+    else if ( 0 == block_count )
+    {
+      TU_LOG_DRV("  SCSI case 4 Hi > Dn (READ10) or case 9 Ho > Dn (WRITE10) \r\n");
+      status =  MSC_CSW_STATUS_FAILED;
+    }
+    else if ( cbw->total_bytes / block_count == 0 )
+    {
+      TU_LOG_DRV(" Computed block size = 0. SCSI case 7 Hi < Di (READ10) or case 13 Ho < Do (WRIT10)\r\n");
+      status = MSC_CSW_STATUS_PHASE_ERROR;
+    }
+  }
+
+  return status;
+}
+
 //--------------------------------------------------------------------+
 // Debug
 //--------------------------------------------------------------------+
-#if CFG_TUSB_DEBUG >= 2
+#if CFG_TUSB_DEBUG >= CFG_TUD_MSC_LOG_LEVEL
 
-static tu_lookup_entry_t const _msc_scsi_cmd_lookup[] =
+TU_ATTR_UNUSED tu_static tu_lookup_entry_t const _msc_scsi_cmd_lookup[] =
 {
   { .key = SCSI_CMD_TEST_UNIT_READY              , .data = "Test Unit Ready" },
   { .key = SCSI_CMD_INQUIRY                      , .data = "Inquiry" },
   { .key = SCSI_CMD_MODE_SELECT_6                , .data = "Mode_Select 6" },
   { .key = SCSI_CMD_MODE_SENSE_6                 , .data = "Mode_Sense 6" },
   { .key = SCSI_CMD_START_STOP_UNIT              , .data = "Start Stop Unit" },
-  { .key = SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL , .data = "Prevent Allow Medium Removal" },
+  { .key = SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL , .data = "Prevent/Allow Medium Removal" },
   { .key = SCSI_CMD_READ_CAPACITY_10             , .data = "Read Capacity10" },
   { .key = SCSI_CMD_REQUEST_SENSE                , .data = "Request Sense" },
   { .key = SCSI_CMD_READ_FORMAT_CAPACITY         , .data = "Read Format Capacity" },
@@ -114,7 +220,7 @@ static tu_lookup_entry_t const _msc_scsi_cmd_lookup[] =
   { .key = SCSI_CMD_WRITE_10                     , .data = "Write10" }
 };
 
-static tu_lookup_table_t const _msc_scsi_cmd_table =
+TU_ATTR_UNUSED tu_static tu_lookup_table_t const _msc_scsi_cmd_table =
 {
   .count = TU_ARRAY_SIZE(_msc_scsi_cmd_lookup),
   .items = _msc_scsi_cmd_lookup
@@ -136,14 +242,24 @@ bool tud_msc_set_sense(uint8_t lun, uint8_t sense_key, uint8_t add_sense_code, u
   return true;
 }
 
+static inline void set_sense_medium_not_present(uint8_t lun)
+{
+  // default sense is NOT READY, MEDIUM NOT PRESENT
+  tud_msc_set_sense(lun, SCSI_SENSE_NOT_READY, 0x3A, 0x00);
+}
+
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void mscd_init(void)
-{
+void mscd_init(void) {
   tu_memclr(&_mscd_itf, sizeof(mscd_interface_t));
 }
 
+bool mscd_deinit(void) {
+  // nothing to do
+  return true;
+}
+
 void mscd_reset(uint8_t rhport)
 {
   (void) rhport;
@@ -160,7 +276,7 @@ uint16_t mscd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint1
   // msc driver length is fixed
   uint16_t const drv_len = sizeof(tusb_desc_interface_t) + 2*sizeof(tusb_desc_endpoint_t);
 
-  // Max length mus be at least 1 interface + 2 endpoints
+  // Max length must be at least 1 interface + 2 endpoints
   TU_ASSERT(max_len >= drv_len, 0);
 
   mscd_interface_t * p_msc = &_mscd_itf;
@@ -170,35 +286,93 @@ uint16_t mscd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint1
   TU_ASSERT( usbd_open_edpt_pair(rhport, tu_desc_next(itf_desc), 2, TUSB_XFER_BULK, &p_msc->ep_out, &p_msc->ep_in), 0 );
 
   // Prepare for Command Block Wrapper
-  if ( !usbd_edpt_xfer(rhport, p_msc->ep_out, (uint8_t*) &p_msc->cbw, sizeof(msc_cbw_t)) )
-  {
-    TU_LOG_FAILED();
-    TU_BREAKPOINT();
-  }
+  TU_ASSERT( prepare_cbw(rhport, p_msc), drv_len);
 
   return drv_len;
 }
 
+static void proc_bot_reset(mscd_interface_t* p_msc)
+{
+  p_msc->stage       = MSC_STAGE_CMD;
+  p_msc->total_len   = 0;
+  p_msc->xferred_len = 0;
+
+  p_msc->sense_key           = 0;
+  p_msc->add_sense_code      = 0;
+  p_msc->add_sense_qualifier = 0;
+}
+
 // Invoked when a control transfer occurred on an interface of this class
 // Driver response accordingly to the request and the transfer stage (setup/data/ack)
 // return false to stall control endpoint (e.g unsupported request)
-bool mscd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * p_request)
+bool mscd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
 {
   // nothing to do with DATA & ACK stage
   if (stage != CONTROL_STAGE_SETUP) return true;
 
-  // Handle class request only
-  TU_VERIFY(p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
+  mscd_interface_t* p_msc = &_mscd_itf;
+
+  // Clear Endpoint Feature (stall) for recovery
+  if ( TUSB_REQ_TYPE_STANDARD     == request->bmRequestType_bit.type      &&
+       TUSB_REQ_RCPT_ENDPOINT     == request->bmRequestType_bit.recipient &&
+       TUSB_REQ_CLEAR_FEATURE     == request->bRequest                    &&
+       TUSB_REQ_FEATURE_EDPT_HALT == request->wValue )
+  {
+    uint8_t const ep_addr = tu_u16_low(request->wIndex);
+
+    if ( p_msc->stage == MSC_STAGE_NEED_RESET )
+    {
+      // reset recovery is required to recover from this stage
+      // Clear Stall request cannot resolve this -> continue to stall endpoint
+      usbd_edpt_stall(rhport, ep_addr);
+    }
+    else
+    {
+      if ( ep_addr == p_msc->ep_in )
+      {
+        if ( p_msc->stage == MSC_STAGE_STATUS )
+        {
+          // resume sending SCSI status if we are in this stage previously before stalled
+          TU_ASSERT( send_csw(rhport, p_msc) );
+        }
+      }
+      else if ( ep_addr == p_msc->ep_out )
+      {
+        if ( p_msc->stage == MSC_STAGE_CMD )
+        {
+          // part of reset recovery (probably due to invalid CBW) -> prepare for new command
+          // Note: skip if already queued previously
+          if ( usbd_edpt_ready(rhport, p_msc->ep_out) )
+          {
+            TU_ASSERT( prepare_cbw(rhport, p_msc) );
+          }
+        }
+      }
+    }
+
+    return true;
+  }
+
+  // From this point only handle class request only
+  TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
 
-  switch ( p_request->bRequest )
+  switch ( request->bRequest )
   {
     case MSC_REQ_RESET:
-      // TODO: Actually reset interface.
-      tud_control_status(rhport, p_request);
+      TU_LOG_DRV("  MSC BOT Reset\r\n");
+      TU_VERIFY(request->wValue == 0 && request->wLength == 0);
+
+      // driver state reset
+      proc_bot_reset(p_msc);
+
+      tud_control_status(rhport, request);
     break;
 
     case MSC_REQ_GET_MAX_LUN:
     {
+      TU_LOG_DRV("  MSC Get Max Lun\r\n");
+      TU_VERIFY(request->wValue == 0 && request->wLength == 1);
+
       uint8_t maxlun = 1;
       if (tud_msc_get_maxlun_cb) maxlun = tud_msc_get_maxlun_cb();
       TU_VERIFY(maxlun);
@@ -206,7 +380,7 @@ bool mscd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t
       // MAX LUN is minus 1 by specs
       maxlun--;
 
-      tud_control_xfer(rhport, p_request, &maxlun, 1);
+      tud_control_xfer(rhport, request, &maxlun, 1);
     }
     break;
 
@@ -218,6 +392,8 @@ bool mscd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t
 
 bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
 {
+  (void) event;
+
   mscd_interface_t* p_msc = &_mscd_itf;
   msc_cbw_t const * p_cbw = &p_msc->cbw;
   msc_csw_t       * p_csw = &p_msc->csw;
@@ -229,166 +405,170 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
       // Complete IN while waiting for CMD is usually Status of previous SCSI op, ignore it
       if(ep_addr != p_msc->ep_out) return true;
 
-      TU_ASSERT( event == XFER_RESULT_SUCCESS &&
-                 xferred_bytes == sizeof(msc_cbw_t) && p_cbw->signature == MSC_CBW_SIGNATURE );
+      if ( !(xferred_bytes == sizeof(msc_cbw_t) && p_cbw->signature == MSC_CBW_SIGNATURE) )
+      {
+        TU_LOG_DRV("  SCSI CBW is not valid\r\n");
 
-      TU_LOG2("  SCSI Command: %s\r\n", tu_lookup_find(&_msc_scsi_cmd_table, p_cbw->command[0]));
-      // TU_LOG2_MEM(p_cbw, xferred_bytes, 2);
+        // BOT 6.6.1 If CBW is not valid stall both endpoints until reset recovery
+        p_msc->stage = MSC_STAGE_NEED_RESET;
+
+        // invalid CBW stall both endpoints
+        usbd_edpt_stall(rhport, p_msc->ep_in);
+        usbd_edpt_stall(rhport, p_msc->ep_out);
+
+        return false;
+      }
+
+      TU_LOG_DRV("  SCSI Command [Lun%u]: %s\r\n", p_cbw->lun, tu_lookup_find(&_msc_scsi_cmd_table, p_cbw->command[0]));
+      //TU_LOG_MEM(MSC_DEBUG, p_cbw, xferred_bytes, 2);
 
       p_csw->signature    = MSC_CSW_SIGNATURE;
       p_csw->tag          = p_cbw->tag;
       p_csw->data_residue = 0;
+      p_csw->status       = MSC_CSW_STATUS_PASSED;
 
       /*------------- Parse command and prepare DATA -------------*/
       p_msc->stage = MSC_STAGE_DATA;
       p_msc->total_len = p_cbw->total_bytes;
       p_msc->xferred_len = 0;
 
-      if (SCSI_CMD_READ_10 == p_cbw->command[0])
+      // Read10 or Write10
+      if ( (SCSI_CMD_READ_10 == p_cbw->command[0]) || (SCSI_CMD_WRITE_10 == p_cbw->command[0]) )
       {
-        proc_read10_cmd(rhport, p_msc);
-      }
-      else if (SCSI_CMD_WRITE_10 == p_cbw->command[0])
-      {
-        proc_write10_cmd(rhport, p_msc);
+        uint8_t const status = rdwr10_validate_cmd(p_cbw);
+
+        if ( status != MSC_CSW_STATUS_PASSED)
+        {
+          fail_scsi_op(rhport, p_msc, status);
+        }else if ( p_cbw->total_bytes )
+        {
+          if (SCSI_CMD_READ_10 == p_cbw->command[0])
+          {
+            proc_read10_cmd(rhport, p_msc);
+          }else
+          {
+            proc_write10_cmd(rhport, p_msc);
+          }
+        }else
+        {
+          // no data transfer, only exist in complaint test suite
+          p_msc->stage = MSC_STAGE_STATUS;
+        }
       }
       else
       {
         // For other SCSI commands
         // 1. OUT : queue transfer (invoke app callback after done)
         // 2. IN & Zero: Process if is built-in, else Invoke app callback. Skip DATA if zero length
-        if ( (p_cbw->total_bytes > 0 ) && !tu_bit_test(p_cbw->dir, 7) )
+        if ( (p_cbw->total_bytes > 0 ) && !is_data_in(p_cbw->dir) )
         {
-          // queue transfer
-          TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_out, _mscd_buf, p_msc->total_len) );
+          if (p_cbw->total_bytes > sizeof(_mscd_buf))
+          {
+            TU_LOG_DRV("  SCSI reject non READ10/WRITE10 with large data\r\n");
+            fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
+          }else
+          {
+            // Didn't check for case 9 (Ho > Dn), which requires examining scsi command first
+            // but it is OK to just receive data then responded with failed status
+            TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_out, _mscd_buf, (uint16_t) p_msc->total_len) );
+          }
         }else
         {
-          int32_t resplen;
-
           // First process if it is a built-in commands
-          resplen = proc_builtin_scsi(p_cbw->lun, p_cbw->command, _mscd_buf, sizeof(_mscd_buf));
+          int32_t resplen = proc_builtin_scsi(p_cbw->lun, p_cbw->command, _mscd_buf, sizeof(_mscd_buf));
 
-          // Not built-in, invoke user callback
+          // Invoke user callback if not built-in
           if ( (resplen < 0) && (p_msc->sense_key == 0) )
           {
-            resplen = tud_msc_scsi_cb(p_cbw->lun, p_cbw->command, _mscd_buf, p_msc->total_len);
+            resplen = tud_msc_scsi_cb(p_cbw->lun, p_cbw->command, _mscd_buf, (uint16_t) p_msc->total_len);
           }
 
           if ( resplen < 0 )
           {
-            p_msc->total_len = 0;
-            p_csw->status = MSC_CSW_STATUS_FAILED;
-            p_msc->stage = MSC_STAGE_STATUS;
-
-            // failed but senskey is not set: default to Illegal Request
-            if ( p_msc->sense_key == 0 ) tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_ILLEGAL_REQUEST, 0x20, 0x00);
-
-            // Stall bulk In if needed
-            if (p_cbw->total_bytes) usbd_edpt_stall(rhport, p_msc->ep_in);
+            // unsupported command
+            TU_LOG_DRV("  SCSI unsupported or failed command\r\n");
+            fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
           }
-          else
+          else if (resplen == 0)
           {
-            p_msc->total_len = (uint32_t) resplen;
-            p_csw->status = MSC_CSW_STATUS_PASSED;
-
-            if (p_msc->total_len)
+            if (p_cbw->total_bytes)
             {
-              TU_ASSERT( p_cbw->total_bytes >= p_msc->total_len ); // cannot return more than host expect
-              TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_in, _mscd_buf, p_msc->total_len) );
+              // 6.7 The 13 Cases: case 4 (Hi > Dn)
+              // TU_LOG(MSC_DEBUG, "  SCSI case 4 (Hi > Dn): %lu\r\n", p_cbw->total_bytes);
+              fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
             }else
             {
+              // case 1 Hn = Dn: all good
               p_msc->stage = MSC_STAGE_STATUS;
             }
           }
+          else
+          {
+            if ( p_cbw->total_bytes == 0 )
+            {
+              // 6.7 The 13 Cases: case 2 (Hn < Di)
+              // TU_LOG(MSC_DEBUG, "  SCSI case 2 (Hn < Di): %lu\r\n", p_cbw->total_bytes);
+              fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
+            }else
+            {
+              // cannot return more than host expect
+              p_msc->total_len = tu_min32((uint32_t) resplen, p_cbw->total_bytes);
+              TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_in, _mscd_buf, (uint16_t) p_msc->total_len) );
+            }
+          }
         }
       }
     break;
 
     case MSC_STAGE_DATA:
-      TU_LOG2("  SCSI Data\r\n");
-      //TU_LOG2_MEM(_mscd_buf, xferred_bytes, 2);
+      TU_LOG_DRV("  SCSI Data [Lun%u]\r\n", p_cbw->lun);
+      //TU_LOG_MEM(MSC_DEBUG, _mscd_buf, xferred_bytes, 2);
 
-      // OUT transfer, invoke callback if needed
-      if ( !tu_bit_test(p_cbw->dir, 7) )
+      if (SCSI_CMD_READ_10 == p_cbw->command[0])
       {
-        if ( SCSI_CMD_WRITE_10 != p_cbw->command[0] )
-        {
-          int32_t cb_result = tud_msc_scsi_cb(p_cbw->lun, p_cbw->command, _mscd_buf, p_msc->total_len);
+        p_msc->xferred_len += xferred_bytes;
 
-          if ( cb_result < 0 )
-          {
-            p_csw->status = MSC_CSW_STATUS_FAILED;
-            tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_ILLEGAL_REQUEST, 0x20, 0x00); // Sense = Invalid Command Operation
-          }else
-          {
-            p_csw->status = MSC_CSW_STATUS_PASSED;
-          }
-        }
-        else
+        if ( p_msc->xferred_len >= p_msc->total_len )
         {
-          uint16_t const block_sz = p_cbw->total_bytes / rdwr10_get_blockcount(p_cbw->command);
-
-          // Adjust lba with transferred bytes
-          uint32_t const lba = rdwr10_get_lba(p_cbw->command) + (p_msc->xferred_len / block_sz);
+          // Data Stage is complete
+          p_msc->stage = MSC_STAGE_STATUS;
+        }else
+        {
+          proc_read10_cmd(rhport, p_msc);
+        }
+      }
+      else if (SCSI_CMD_WRITE_10 == p_cbw->command[0])
+      {
+        proc_write10_new_data(rhport, p_msc, xferred_bytes);
+      }
+      else
+      {
+        p_msc->xferred_len += xferred_bytes;
 
-          // Application can consume smaller bytes
-          int32_t nbytes = tud_msc_write10_cb(p_cbw->lun, lba, p_msc->xferred_len % block_sz, _mscd_buf, xferred_bytes);
+        // OUT transfer, invoke callback if needed
+        if ( !is_data_in(p_cbw->dir) )
+        {
+          int32_t cb_result = tud_msc_scsi_cb(p_cbw->lun, p_cbw->command, _mscd_buf, (uint16_t) p_msc->total_len);
 
-          if ( nbytes < 0 )
+          if ( cb_result < 0 )
           {
-            // negative means error -> skip to status phase, status in CSW set to failed
-            p_csw->data_residue = p_cbw->total_bytes - p_msc->xferred_len;
-            p_csw->status       = MSC_CSW_STATUS_FAILED;
-            p_msc->stage        = MSC_STAGE_STATUS;
-
-            tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_ILLEGAL_REQUEST, 0x20, 0x00); // Sense = Invalid Command Operation
-            break;
+            // unsupported command
+            TU_LOG_DRV("  SCSI unsupported command\r\n");
+            fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
           }else
           {
-            // Application consume less than what we got (including zero)
-            if ( nbytes < (int32_t) xferred_bytes )
-            {
-              if ( nbytes > 0 )
-              {
-                p_msc->xferred_len += nbytes;
-                memmove(_mscd_buf, _mscd_buf+nbytes, xferred_bytes-nbytes);
-              }
-
-              // simulate an transfer complete with adjusted parameters --> this driver callback will fired again
-              dcd_event_xfer_complete(rhport, p_msc->ep_out, xferred_bytes-nbytes, XFER_RESULT_SUCCESS, false);
-
-              return true; // skip the rest
-            }
-            else
-            {
-              // Application consume all bytes in our buffer. Nothing to do, process with normal flow
-            }
+            // TODO haven't implement this scenario any further yet
           }
         }
-      }
 
-      // Accumulate data so far
-      p_msc->xferred_len += xferred_bytes;
-
-      if ( p_msc->xferred_len >= p_msc->total_len )
-      {
-        // Data Stage is complete
-        p_msc->stage = MSC_STAGE_STATUS;
-      }
-      else
-      {
-        // READ10 & WRITE10 Can be executed with large bulk of data e.g write 8K bytes (several flash write)
-        // We break it into multiple smaller command whose data size is up to CFG_TUD_MSC_EP_BUFSIZE
-        if (SCSI_CMD_READ_10 == p_cbw->command[0])
+        if ( p_msc->xferred_len >= p_msc->total_len )
         {
-          proc_read10_cmd(rhport, p_msc);
+          // Data Stage is complete
+          p_msc->stage = MSC_STAGE_STATUS;
         }
-        else if (SCSI_CMD_WRITE_10 == p_cbw->command[0])
-        {
-          proc_write10_cmd(rhport, p_msc);
-        }else
+        else
         {
-          // No other command take more than one transfer yet -> unlikely error
+          // This scenario with command that take more than one transfer is already rejected at Command stage
           TU_BREAKPOINT();
         }
       }
@@ -402,8 +582,8 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
       // Wait for the Status phase to complete
       if( (ep_addr == p_msc->ep_in) && (xferred_bytes == sizeof(msc_csw_t)) )
       {
-        TU_LOG2("  SCSI Status: %u\r\n", p_csw->status);
-        // TU_LOG2_MEM(p_csw, xferred_bytes, 2);
+        TU_LOG_DRV("  SCSI Status [Lun%u] = %u\r\n", p_cbw->lun, p_csw->status);
+        // TU_LOG_MEM(MSC_DEBUG, p_csw, xferred_bytes, 2);
 
         // Invoke complete callback if defined
         // Note: There is racing issue with samd51 + qspi flash testing with arduino
@@ -423,11 +603,11 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
           break;
         }
 
-        // Move to default CMD stage
-        p_msc->stage = MSC_STAGE_CMD;
-
-        // Queue for the next CBW
-        TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_out, (uint8_t*) &p_msc->cbw, sizeof(msc_cbw_t)) );
+        TU_ASSERT( prepare_cbw(rhport, p_msc) );
+      }else
+      {
+        // Any xfer ended here is consider unknown error, ignore it
+        TU_LOG1("  Warning expect SCSI Status but received unknown data\r\n");
       }
     break;
 
@@ -436,22 +616,30 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
 
   if ( p_msc->stage == MSC_STAGE_STATUS )
   {
-    // Either endpoints is stalled, need to wait until it is cleared by host
-    if ( usbd_edpt_stalled(rhport,  p_msc->ep_in) || usbd_edpt_stalled(rhport,  p_msc->ep_out) )
+    // skip status if epin is currently stalled, will do it when received Clear Stall request
+    if ( !usbd_edpt_stalled(rhport,  p_msc->ep_in) )
     {
-      // simulate an transfer complete with adjusted parameters --> this driver callback will fired again
-      // and response with status phase after halted endpoints are cleared.
-      // note: use ep_out to prevent confusing with STATUS complete
-      dcd_event_xfer_complete(rhport, p_msc->ep_out, 0, XFER_RESULT_SUCCESS, false);
+      if ( (p_cbw->total_bytes > p_msc->xferred_len) && is_data_in(p_cbw->dir) )
+      {
+        // 6.7 The 13 Cases: case 5 (Hi > Di): STALL before status
+        // TU_LOG(MSC_DEBUG, "  SCSI case 5 (Hi > Di): %lu > %lu\r\n", p_cbw->total_bytes, p_msc->xferred_len);
+        usbd_edpt_stall(rhport, p_msc->ep_in);
+      }else
+      {
+        TU_ASSERT( send_csw(rhport, p_msc) );
+      }
     }
-    else
-    {
-      // Move to Status Sent stage
-      p_msc->stage = MSC_STAGE_STATUS_SENT;
 
-      // Send SCSI Status
-      TU_ASSERT(usbd_edpt_xfer(rhport, p_msc->ep_in , (uint8_t*) &p_msc->csw, sizeof(msc_csw_t)));
+    #if TU_CHECK_MCU(OPT_MCU_CXD56)
+    // WORKAROUND: cxd56 has its own nuttx usb stack which does not forward Set/ClearFeature(Endpoint) to DCD.
+    // There is no way for us to know when EP is un-stall, therefore we will unconditionally un-stall here and
+    // hope everything will work
+    if ( usbd_edpt_stalled(rhport, p_msc->ep_in) )
+    {
+      usbd_edpt_clear_stall(rhport, p_msc->ep_in);
+      send_csw(rhport, p_msc);
     }
+    #endif
   }
 
   return true;
@@ -468,6 +656,8 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
   (void) bufsize; // TODO refractor later
   int32_t resplen;
 
+  mscd_interface_t* p_msc = &_mscd_itf;
+
   switch ( scsi_cmd[0] )
   {
     case SCSI_CMD_TEST_UNIT_READY:
@@ -477,8 +667,8 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
         // Failed status response
         resplen = - 1;
 
-        // If sense key is not set by callback, default to Logical Unit Not Ready, Cause Not Reportable
-        if ( _mscd_itf.sense_key == 0 ) tud_msc_set_sense(lun, SCSI_SENSE_NOT_READY, 0x04, 0x00);
+        // set default sense if not set by callback
+        if ( p_msc->sense_key == 0 ) set_sense_medium_not_present(lun);
       }
     break;
 
@@ -493,8 +683,8 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
           // Failed status response
           resplen = - 1;
 
-          // If sense key is not set by callback, default to Logical Unit Not Ready, Cause Not Reportable
-          if ( _mscd_itf.sense_key == 0 ) tud_msc_set_sense(lun, SCSI_SENSE_NOT_READY, 0x04, 0x00);
+          // set default sense if not set by callback
+          if ( p_msc->sense_key == 0 ) set_sense_medium_not_present(lun);
         }
       }
     break;
@@ -514,17 +704,17 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
       {
         resplen = -1;
 
-        // If sense key is not set by callback, default to Logical Unit Not Ready, Cause Not Reportable
-        if ( _mscd_itf.sense_key == 0 ) tud_msc_set_sense(lun, SCSI_SENSE_NOT_READY, 0x04, 0x00);
+        // set default sense if not set by callback
+        if ( p_msc->sense_key == 0 ) set_sense_medium_not_present(lun);
       }else
       {
         scsi_read_capacity10_resp_t read_capa10;
 
-        read_capa10.last_lba = tu_htonl(block_count-1);
+        read_capa10.last_lba   = tu_htonl(block_count-1);
         read_capa10.block_size = tu_htonl(block_size);
 
         resplen = sizeof(read_capa10);
-        memcpy(buffer, &read_capa10, resplen);
+        TU_VERIFY(0 == tu_memcpy_s(buffer, bufsize, &read_capa10, (size_t) resplen));
       }
     }
     break;
@@ -550,15 +740,15 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
       {
         resplen = -1;
 
-        // If sense key is not set by callback, default to Logical Unit Not Ready, Cause Not Reportable
-        if ( _mscd_itf.sense_key == 0 ) tud_msc_set_sense(lun, SCSI_SENSE_NOT_READY, 0x04, 0x00);
+        // set default sense if not set by callback
+        if ( p_msc->sense_key == 0 ) set_sense_medium_not_present(lun);
       }else
       {
         read_fmt_capa.block_num = tu_htonl(block_count);
         read_fmt_capa.block_size_u16 = tu_htons(block_size);
 
         resplen = sizeof(read_fmt_capa);
-        memcpy(buffer, &read_fmt_capa, resplen);
+        TU_VERIFY(0 == tu_memcpy_s(buffer, bufsize, &read_fmt_capa, (size_t) resplen));
       }
     }
     break;
@@ -570,6 +760,7 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
           .is_removable         = 1,
           .version              = 2,
           .response_data_format = 2,
+          .additional_length    = sizeof(scsi_inquiry_resp_t) - 5,
       };
 
       // vendor_id, product_id, product_rev is space padded string
@@ -580,7 +771,7 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
       tud_msc_inquiry_cb(lun, inquiry_rsp.vendor_id, inquiry_rsp.product_id, inquiry_rsp.product_rev);
 
       resplen = sizeof(inquiry_rsp);
-      memcpy(buffer, &inquiry_rsp, resplen);
+      TU_VERIFY(0 == tu_memcpy_s(buffer, bufsize, &inquiry_rsp, (size_t) resplen));
     }
     break;
 
@@ -588,21 +779,23 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
     {
       scsi_mode_sense6_resp_t mode_resp =
       {
-          .data_len = 3,
-          .medium_type = 0,
-          .write_protected = false,
-          .reserved = 0,
+          .data_len             = 3,
+          .medium_type          = 0,
+          .write_protected      = false,
+          .reserved             = 0,
           .block_descriptor_len = 0  // no block descriptor are included
       };
 
       bool writable = true;
-      if (tud_msc_is_writable_cb) {
-          writable = tud_msc_is_writable_cb(lun);
+      if ( tud_msc_is_writable_cb )
+      {
+        writable = tud_msc_is_writable_cb(lun);
       }
+
       mode_resp.write_protected = !writable;
 
       resplen = sizeof(mode_resp);
-      memcpy(buffer, &mode_resp, resplen);
+      TU_VERIFY(0 == tu_memcpy_s(buffer, bufsize, &mode_resp, (size_t) resplen));
     }
     break;
 
@@ -610,18 +803,23 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
     {
       scsi_sense_fixed_resp_t sense_rsp =
       {
-          .response_code = 0x70,
+          .response_code = 0x70, // current, fixed format
           .valid         = 1
       };
 
-      sense_rsp.add_sense_len = sizeof(scsi_sense_fixed_resp_t) - 8;
-
-      sense_rsp.sense_key           = _mscd_itf.sense_key;
-      sense_rsp.add_sense_code      = _mscd_itf.add_sense_code;
-      sense_rsp.add_sense_qualifier = _mscd_itf.add_sense_qualifier;
+      sense_rsp.add_sense_len       = sizeof(scsi_sense_fixed_resp_t) - 8;
+      sense_rsp.sense_key           = (uint8_t) (p_msc->sense_key & 0x0F);
+      sense_rsp.add_sense_code      = p_msc->add_sense_code;
+      sense_rsp.add_sense_qualifier = p_msc->add_sense_qualifier;
 
       resplen = sizeof(sense_rsp);
-      memcpy(buffer, &sense_rsp, resplen);
+      TU_VERIFY(0 == tu_memcpy_s(buffer, bufsize, &sense_rsp, (size_t) resplen));
+
+      // request sense callback could overwrite the sense data
+      if (tud_msc_request_sense_cb)
+      {
+        resplen = tud_msc_request_sense_cb(lun, buffer, (uint16_t) bufsize);
+      }
 
       // Clear sense data after copy
       tud_msc_set_sense(lun, 0, 0, 0);
@@ -637,13 +835,9 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
 static void proc_read10_cmd(uint8_t rhport, mscd_interface_t* p_msc)
 {
   msc_cbw_t const * p_cbw = &p_msc->cbw;
-  msc_csw_t       * p_csw = &p_msc->csw;
 
-  uint16_t const block_cnt = rdwr10_get_blockcount(p_cbw->command);
-  TU_ASSERT(block_cnt, ); // prevent div by zero
-
-  uint16_t const block_sz = p_cbw->total_bytes / block_cnt;
-  TU_ASSERT(block_sz, ); // prevent div by zero
+  // block size already verified not zero
+  uint16_t const block_sz = rdwr10_get_blocksize(p_cbw);
 
   // Adjust lba with transferred bytes
   uint32_t const lba = rdwr10_get_lba(p_cbw->command) + (p_msc->xferred_len / block_sz);
@@ -652,16 +846,18 @@ static void proc_read10_cmd(uint8_t rhport, mscd_interface_t* p_msc)
   int32_t nbytes = (int32_t) tu_min32(sizeof(_mscd_buf), p_cbw->total_bytes-p_msc->xferred_len);
 
   // Application can consume smaller bytes
-  nbytes = tud_msc_read10_cb(p_cbw->lun, lba, p_msc->xferred_len % block_sz, _mscd_buf, (uint32_t) nbytes);
+  uint32_t const offset = p_msc->xferred_len % block_sz;
+  nbytes = tud_msc_read10_cb(p_cbw->lun, lba, offset, _mscd_buf, (uint32_t) nbytes);
 
   if ( nbytes < 0 )
   {
-    // negative means error -> pipe is stalled & status in CSW set to failed
-    p_csw->data_residue = p_cbw->total_bytes - p_msc->xferred_len;
-    p_csw->status       = MSC_CSW_STATUS_FAILED;
+    // negative means error -> endpoint is stalled & status in CSW set to failed
+    TU_LOG_DRV("  tud_msc_read10_cb() return -1\r\n");
+
+    // set sense
+    set_sense_medium_not_present(p_cbw->lun);
 
-    tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_ILLEGAL_REQUEST, 0x20, 0x00); // Sense = Invalid Command Operation
-    usbd_edpt_stall(rhport, p_msc->ep_in);
+    fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
   }
   else if ( nbytes == 0 )
   {
@@ -670,7 +866,7 @@ static void proc_read10_cmd(uint8_t rhport, mscd_interface_t* p_msc)
   }
   else
   {
-    TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_in, _mscd_buf, nbytes), );
+    TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_in, _mscd_buf, (uint16_t) nbytes), );
   }
 }
 
@@ -678,24 +874,86 @@ static void proc_write10_cmd(uint8_t rhport, mscd_interface_t* p_msc)
 {
   msc_cbw_t const * p_cbw = &p_msc->cbw;
   bool writable = true;
-  if (tud_msc_is_writable_cb) {
+
+  if ( tud_msc_is_writable_cb )
+  {
     writable = tud_msc_is_writable_cb(p_cbw->lun);
   }
-  if (!writable) {
-    msc_csw_t* p_csw = &p_msc->csw;
-    p_csw->data_residue = p_cbw->total_bytes;
-    p_csw->status       = MSC_CSW_STATUS_FAILED;
 
-    tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_DATA_PROTECT, 0x27, 0x00); // Sense = Write protected
-    usbd_edpt_stall(rhport, p_msc->ep_out);
+  if ( !writable )
+  {
+    // Not writable, complete this SCSI op with error
+    // Sense = Write protected
+    tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_DATA_PROTECT, 0x27, 0x00);
+    fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
     return;
   }
 
   // remaining bytes capped at class buffer
-  int32_t nbytes = (int32_t) tu_min32(sizeof(_mscd_buf), p_cbw->total_bytes-p_msc->xferred_len);
+  uint16_t nbytes = (uint16_t) tu_min32(sizeof(_mscd_buf), p_cbw->total_bytes-p_msc->xferred_len);
 
   // Write10 callback will be called later when usb transfer complete
   TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_out, _mscd_buf, nbytes), );
 }
 
+// process new data arrived from WRITE10
+static void proc_write10_new_data(uint8_t rhport, mscd_interface_t* p_msc, uint32_t xferred_bytes)
+{
+  msc_cbw_t const * p_cbw = &p_msc->cbw;
+
+  // block size already verified not zero
+  uint16_t const block_sz = rdwr10_get_blocksize(p_cbw);
+
+  // Adjust lba with transferred bytes
+  uint32_t const lba = rdwr10_get_lba(p_cbw->command) + (p_msc->xferred_len / block_sz);
+
+  // Invoke callback to consume new data
+  uint32_t const offset = p_msc->xferred_len % block_sz;
+  int32_t nbytes = tud_msc_write10_cb(p_cbw->lun, lba, offset, _mscd_buf, xferred_bytes);
+
+  if ( nbytes < 0 )
+  {
+    // negative means error -> failed this scsi op
+    TU_LOG_DRV("  tud_msc_write10_cb() return -1\r\n");
+
+    // update actual byte before failed
+    p_msc->xferred_len += xferred_bytes;
+
+    // Set sense
+    set_sense_medium_not_present(p_cbw->lun);
+
+    fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
+  }else
+  {
+    // Application consume less than what we got (including zero)
+    if ( (uint32_t) nbytes < xferred_bytes )
+    {
+      uint32_t const left_over = xferred_bytes - (uint32_t) nbytes;
+      if ( nbytes > 0 )
+      {
+        p_msc->xferred_len += (uint16_t) nbytes;
+        memmove(_mscd_buf, _mscd_buf+nbytes, left_over);
+      }
+
+      // simulate an transfer complete with adjusted parameters --> callback will be invoked with adjusted parameter
+      dcd_event_xfer_complete(rhport, p_msc->ep_out, left_over, XFER_RESULT_SUCCESS, false);
+    }
+    else
+    {
+      // Application consume all bytes in our buffer
+      p_msc->xferred_len += xferred_bytes;
+
+      if ( p_msc->xferred_len >= p_msc->total_len )
+      {
+        // Data Stage is complete
+        p_msc->stage = MSC_STAGE_STATUS;
+      }else
+      {
+        // prepare to receive more data from host
+        proc_write10_cmd(rhport, p_msc);
+      }
+    }
+  }
+}
+
 #endif
diff --git a/src/class/msc/msc_device.h b/src/class/msc/msc_device.h
index 8f90ef4ad..4247a40bd 100644
--- a/src/class/msc/msc_device.h
+++ b/src/class/msc/msc_device.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -131,6 +131,9 @@ TU_ATTR_WEAK uint8_t tud_msc_get_maxlun_cb(void);
 // - Start = 1 : active mode, if load_eject = 1 : load disk storage
 TU_ATTR_WEAK bool tud_msc_start_stop_cb(uint8_t lun, uint8_t power_condition, bool start, bool load_eject);
 
+// Invoked when received REQUEST_SENSE
+TU_ATTR_WEAK int32_t tud_msc_request_sense_cb(uint8_t lun, void* buffer, uint16_t bufsize);
+
 // Invoked when Read10 command is complete
 TU_ATTR_WEAK void tud_msc_read10_complete_cb(uint8_t lun);
 
@@ -140,13 +143,14 @@ TU_ATTR_WEAK void tud_msc_write10_complete_cb(uint8_t lun);
 // Invoked when command in tud_msc_scsi_cb is complete
 TU_ATTR_WEAK void tud_msc_scsi_complete_cb(uint8_t lun, uint8_t const scsi_cmd[16]);
 
-// Hook to make a mass storage device read-only. TODO remove
+// Invoked to check if device is writable as part of SCSI WRITE10
 TU_ATTR_WEAK bool tud_msc_is_writable_cb(uint8_t lun);
 
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     mscd_init            (void);
+bool     mscd_deinit          (void);
 void     mscd_reset           (uint8_t rhport);
 uint16_t mscd_open            (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     mscd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * p_request);
diff --git a/src/class/msc/msc_host.c b/src/class/msc/msc_host.c
index 205f0fd2d..ce6e7fb2d 100644
--- a/src/class/msc/msc_host.c
+++ b/src/class/msc/msc_host.c
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -26,24 +26,31 @@
 
 #include "tusb_option.h"
 
-#if TUSB_OPT_HOST_ENABLED & CFG_TUH_MSC
+#if CFG_TUH_ENABLED && CFG_TUH_MSC
 
 #include "host/usbh.h"
+#include "host/usbh_pvt.h"
+
 #include "msc_host.h"
 
+// Level where CFG_TUSB_DEBUG must be at least for this driver is logged
+#ifndef CFG_TUH_MSC_LOG_LEVEL
+  #define CFG_TUH_MSC_LOG_LEVEL   CFG_TUH_LOG_LEVEL
+#endif
+
+#define TU_LOG_DRV(...)   TU_LOG(CFG_TUH_MSC_LOG_LEVEL, __VA_ARGS__)
+
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-enum
-{
+enum {
   MSC_STAGE_IDLE = 0,
   MSC_STAGE_CMD,
   MSC_STAGE_DATA,
   MSC_STAGE_STATUS,
 };
 
-typedef struct
-{
+typedef struct {
   uint8_t itf_num;
   uint8_t ep_in;
   uint8_t ep_out;
@@ -60,90 +67,91 @@ typedef struct
 
   //------------- SCSI -------------//
   uint8_t stage;
-  void*   buffer;
+  void* buffer;
   tuh_msc_complete_cb_t complete_cb;
+  uintptr_t complete_arg;
 
-  msc_cbw_t cbw;
-  msc_csw_t csw;
-}msch_interface_t;
+  CFG_TUH_MEM_ALIGN msc_cbw_t cbw;
+  CFG_TUH_MEM_ALIGN msc_csw_t csw;
+} msch_interface_t;
 
-CFG_TUSB_MEM_SECTION static msch_interface_t _msch_itf[CFG_TUSB_HOST_DEVICE_MAX];
+CFG_TUH_MEM_SECTION static msch_interface_t _msch_itf[CFG_TUH_DEVICE_MAX];
 
 // buffer used to read scsi information when mounted
 // largest response data currently is inquiry TODO Inquiry is not part of enum anymore
-CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(4)
+CFG_TUH_MEM_SECTION CFG_TUH_MEM_ALIGN
 static uint8_t _msch_buffer[sizeof(scsi_inquiry_resp_t)];
 
-static inline msch_interface_t* get_itf(uint8_t dev_addr)
-{
-  return &_msch_itf[dev_addr-1];
+// FIXME potential nul reference
+TU_ATTR_ALWAYS_INLINE
+static inline msch_interface_t* get_itf(uint8_t dev_addr) {
+  return &_msch_itf[dev_addr - 1];
 }
 
 //--------------------------------------------------------------------+
 // PUBLIC API
 //--------------------------------------------------------------------+
-uint8_t tuh_msc_get_maxlun(uint8_t dev_addr)
-{
+uint8_t tuh_msc_get_maxlun(uint8_t dev_addr) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   return p_msc->max_lun;
 }
 
-uint32_t tuh_msc_get_block_count(uint8_t dev_addr, uint8_t lun)
-{
+uint32_t tuh_msc_get_block_count(uint8_t dev_addr, uint8_t lun) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   return p_msc->capacity[lun].block_count;
 }
 
-uint32_t tuh_msc_get_block_size(uint8_t dev_addr, uint8_t lun)
-{
+uint32_t tuh_msc_get_block_size(uint8_t dev_addr, uint8_t lun) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   return p_msc->capacity[lun].block_size;
 }
 
-bool tuh_msc_mounted(uint8_t dev_addr)
-{
+bool tuh_msc_mounted(uint8_t dev_addr) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   return p_msc->mounted;
 }
 
-bool tuh_msc_ready(uint8_t dev_addr)
-{
+bool tuh_msc_ready(uint8_t dev_addr) {
   msch_interface_t* p_msc = get_itf(dev_addr);
-  return p_msc->mounted && !hcd_edpt_busy(dev_addr, p_msc->ep_in);
+  return p_msc->mounted && !usbh_edpt_busy(dev_addr, p_msc->ep_in) && !usbh_edpt_busy(dev_addr, p_msc->ep_out);
 }
 
 //--------------------------------------------------------------------+
 // PUBLIC API: SCSI COMMAND
 //--------------------------------------------------------------------+
-static inline void cbw_init(msc_cbw_t *cbw, uint8_t lun)
-{
+static inline void cbw_init(msc_cbw_t* cbw, uint8_t lun) {
   tu_memclr(cbw, sizeof(msc_cbw_t));
   cbw->signature = MSC_CBW_SIGNATURE;
   cbw->tag       = 0x54555342; // TUSB
   cbw->lun       = lun;
 }
 
-bool tuh_msc_scsi_command(uint8_t dev_addr, msc_cbw_t const* cbw, void* data, tuh_msc_complete_cb_t complete_cb)
-{
-  msch_interface_t* p_msc = get_itf(dev_addr);
+bool tuh_msc_scsi_command(uint8_t daddr, msc_cbw_t const* cbw, void* data,
+                          tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
+  msch_interface_t* p_msc = get_itf(daddr);
   TU_VERIFY(p_msc->configured);
 
-  // TODO claim endpoint
+  // claim endpoint
+  TU_VERIFY(usbh_edpt_claim(daddr, p_msc->ep_out));
 
   p_msc->cbw = *cbw;
   p_msc->stage = MSC_STAGE_CMD;
   p_msc->buffer = data;
   p_msc->complete_cb = complete_cb;
+  p_msc->complete_arg = arg;
 
-  TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_out, (uint8_t*) &p_msc->cbw, sizeof(msc_cbw_t)));
+  if (!usbh_edpt_xfer(daddr, p_msc->ep_out, (uint8_t*) &p_msc->cbw, sizeof(msc_cbw_t))) {
+    usbh_edpt_release(daddr, p_msc->ep_out);
+    return false;
+  }
 
   return true;
 }
 
-bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_resp_t* response, tuh_msc_complete_cb_t complete_cb)
-{
-   msch_interface_t* p_msc = get_itf(dev_addr);
-   TU_VERIFY(p_msc->configured);
+bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_resp_t* response,
+                           tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
+  msch_interface_t* p_msc = get_itf(dev_addr);
+  TU_VERIFY(p_msc->configured);
 
   msc_cbw_t cbw;
   cbw_init(&cbw, lun);
@@ -153,11 +161,11 @@ bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_r
   cbw.cmd_len    = sizeof(scsi_read_capacity10_t);
   cbw.command[0] = SCSI_CMD_READ_CAPACITY_10;
 
-  return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb);
+  return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb, arg);
 }
 
-bool tuh_msc_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* response, tuh_msc_complete_cb_t complete_cb)
-{
+bool tuh_msc_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* response,
+                     tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   TU_VERIFY(p_msc->mounted);
 
@@ -168,18 +176,16 @@ bool tuh_msc_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* respons
   cbw.dir         = TUSB_DIR_IN_MASK;
   cbw.cmd_len     = sizeof(scsi_inquiry_t);
 
-  scsi_inquiry_t const cmd_inquiry =
-  {
-    .cmd_code     = SCSI_CMD_INQUIRY,
-    .alloc_length = sizeof(scsi_inquiry_resp_t)
+  scsi_inquiry_t const cmd_inquiry = {
+      .cmd_code     = SCSI_CMD_INQUIRY,
+      .alloc_length = sizeof(scsi_inquiry_resp_t)
   };
   memcpy(cbw.command, &cmd_inquiry, cbw.cmd_len);
 
-  return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb);
+  return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb, arg);
 }
 
-bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_t complete_cb)
-{
+bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   TU_VERIFY(p_msc->configured);
 
@@ -187,16 +193,16 @@ bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_
   cbw_init(&cbw, lun);
 
   cbw.total_bytes = 0;
-  cbw.dir         = TUSB_DIR_OUT;
-  cbw.cmd_len     = sizeof(scsi_test_unit_ready_t);
-  cbw.command[0]  = SCSI_CMD_TEST_UNIT_READY;
-  cbw.command[1]  = lun; // according to wiki TODO need verification
+  cbw.dir        = TUSB_DIR_OUT;
+  cbw.cmd_len    = sizeof(scsi_test_unit_ready_t);
+  cbw.command[0] = SCSI_CMD_TEST_UNIT_READY;
+  cbw.command[1] = lun; // according to wiki TODO need verification
 
-  return tuh_msc_scsi_command(dev_addr, &cbw, NULL, complete_cb);
+  return tuh_msc_scsi_command(dev_addr, &cbw, NULL, complete_cb, arg);
 }
 
-bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void *resposne, tuh_msc_complete_cb_t complete_cb)
-{
+bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void* response,
+                           tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
   msc_cbw_t cbw;
   cbw_init(&cbw, lun);
 
@@ -204,73 +210,64 @@ bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void *resposne, tuh_ms
   cbw.dir         = TUSB_DIR_IN_MASK;
   cbw.cmd_len     = sizeof(scsi_request_sense_t);
 
-  scsi_request_sense_t const cmd_request_sense =
-  {
-    .cmd_code     = SCSI_CMD_REQUEST_SENSE,
-    .alloc_length = 18
+  scsi_request_sense_t const cmd_request_sense = {
+      .cmd_code     = SCSI_CMD_REQUEST_SENSE,
+      .alloc_length = 18
   };
-
   memcpy(cbw.command, &cmd_request_sense, cbw.cmd_len);
 
-  return tuh_msc_scsi_command(dev_addr, &cbw, resposne, complete_cb);
+  return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb, arg);
 }
 
-bool tuh_msc_read10(uint8_t dev_addr, uint8_t lun, void * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb)
-{
+bool tuh_msc_read10(uint8_t dev_addr, uint8_t lun, void* buffer, uint32_t lba, uint16_t block_count,
+                    tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   TU_VERIFY(p_msc->mounted);
 
   msc_cbw_t cbw;
   cbw_init(&cbw, lun);
- 
-  cbw.total_bytes = block_count*p_msc->capacity[lun].block_size;
-  cbw.dir         = TUSB_DIR_IN_MASK;
-  cbw.cmd_len     = sizeof(scsi_read10_t);
- 
-  scsi_read10_t const cmd_read10 =
-  {
-    .cmd_code    = SCSI_CMD_READ_10,
-    .lba         = tu_htonl(lba),
-    .block_count = tu_htons(block_count)
+
+  cbw.total_bytes = block_count * p_msc->capacity[lun].block_size;
+  cbw.dir = TUSB_DIR_IN_MASK;
+  cbw.cmd_len = sizeof(scsi_read10_t);
+
+  scsi_read10_t const cmd_read10 = {
+      .cmd_code    = SCSI_CMD_READ_10,
+      .lba         = tu_htonl(lba),
+      .block_count = tu_htons(block_count)
   };
- 
   memcpy(cbw.command, &cmd_read10, cbw.cmd_len);
- 
-  return tuh_msc_scsi_command(dev_addr, &cbw, buffer, complete_cb);
+
+  return tuh_msc_scsi_command(dev_addr, &cbw, buffer, complete_cb, arg);
 }
- 
-bool tuh_msc_write10(uint8_t dev_addr, uint8_t lun, void const * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb)
-{
+
+bool tuh_msc_write10(uint8_t dev_addr, uint8_t lun, void const* buffer, uint32_t lba, uint16_t block_count,
+                     tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   TU_VERIFY(p_msc->mounted);
 
   msc_cbw_t cbw;
   cbw_init(&cbw, lun);
 
-  cbw.total_bytes = block_count*p_msc->capacity[lun].block_size;
+  cbw.total_bytes = block_count * p_msc->capacity[lun].block_size;
   cbw.dir         = TUSB_DIR_OUT;
   cbw.cmd_len     = sizeof(scsi_write10_t);
 
-  scsi_write10_t const cmd_write10 =
-  {
-    .cmd_code    = SCSI_CMD_WRITE_10,
-    .lba         = tu_htonl(lba),
-    .block_count = tu_htons(block_count)
+  scsi_write10_t const cmd_write10 = {
+      .cmd_code    = SCSI_CMD_WRITE_10,
+      .lba         = tu_htonl(lba),
+      .block_count = tu_htons(block_count)
   };
-
   memcpy(cbw.command, &cmd_write10, cbw.cmd_len);
 
-  return tuh_msc_scsi_command(dev_addr, &cbw, (void*) buffer, complete_cb);
+  return tuh_msc_scsi_command(dev_addr, &cbw, (void*) (uintptr_t) buffer, complete_cb, arg);
 }
 
 #if 0
 // MSC interface Reset (not used now)
-bool tuh_msc_reset(uint8_t dev_addr)
-{
-  tusb_control_request_t const new_request =
-  {
-    .bmRequestType_bit =
-    {
+bool tuh_msc_reset(uint8_t dev_addr) {
+  tusb_control_request_t const new_request = {
+    .bmRequestType_bit = {
       .recipient = TUSB_REQ_RCPT_INTERFACE,
       .type      = TUSB_REQ_TYPE_CLASS,
       .direction = TUSB_DIR_OUT
@@ -287,63 +284,77 @@ bool tuh_msc_reset(uint8_t dev_addr)
 //--------------------------------------------------------------------+
 // CLASS-USBH API
 //--------------------------------------------------------------------+
-void msch_init(void)
-{
-  tu_memclr(_msch_itf, sizeof(msch_interface_t)*CFG_TUSB_HOST_DEVICE_MAX);
+bool msch_init(void) {
+  TU_LOG_DRV("sizeof(msch_interface_t) = %u\r\n", sizeof(msch_interface_t));
+  tu_memclr(_msch_itf, sizeof(_msch_itf));
+  return true;
+}
+
+bool msch_deinit(void) {
+  return true;
 }
 
-void msch_close(uint8_t dev_addr)
-{
+void msch_close(uint8_t dev_addr) {
+  TU_VERIFY(dev_addr <= CFG_TUH_DEVICE_MAX,);
   msch_interface_t* p_msc = get_itf(dev_addr);
+  TU_VERIFY(p_msc->configured,);
+
+  TU_LOG_DRV("  MSCh close addr = %d\r\n", dev_addr);
 
   // invoke Application Callback
-  if (p_msc->mounted && tuh_msc_umount_cb) tuh_msc_umount_cb(dev_addr);
+  if (p_msc->mounted) {
+    if (tuh_msc_umount_cb) tuh_msc_umount_cb(dev_addr);
+  }
 
   tu_memclr(p_msc, sizeof(msch_interface_t));
 }
 
-bool msch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
-{
+bool msch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   msc_cbw_t const * cbw = &p_msc->cbw;
   msc_csw_t       * csw = &p_msc->csw;
 
-  switch (p_msc->stage)
-  {
+  switch (p_msc->stage) {
     case MSC_STAGE_CMD:
       // Must be Command Block
-      TU_ASSERT(ep_addr == p_msc->ep_out &&  event == XFER_RESULT_SUCCESS && xferred_bytes == sizeof(msc_cbw_t));
+      TU_ASSERT(ep_addr == p_msc->ep_out && event == XFER_RESULT_SUCCESS && xferred_bytes == sizeof(msc_cbw_t));
 
-      if ( cbw->total_bytes && p_msc->buffer )
-      {
+      if (cbw->total_bytes && p_msc->buffer) {
         // Data stage if any
         p_msc->stage = MSC_STAGE_DATA;
-
         uint8_t const ep_data = (cbw->dir & TUSB_DIR_IN_MASK) ? p_msc->ep_in : p_msc->ep_out;
-        TU_ASSERT(usbh_edpt_xfer(dev_addr, ep_data, p_msc->buffer, cbw->total_bytes));
-      }else
-      {
+        TU_ASSERT(usbh_edpt_xfer(dev_addr, ep_data, p_msc->buffer, (uint16_t) cbw->total_bytes));
+      } else {
         // Status stage
         p_msc->stage = MSC_STAGE_STATUS;
-        TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_in, (uint8_t*) &p_msc->csw, sizeof(msc_csw_t)));
+        TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_in, (uint8_t*) &p_msc->csw, (uint16_t) sizeof(msc_csw_t)));
       }
-    break;
+      break;
 
     case MSC_STAGE_DATA:
       // Status stage
       p_msc->stage = MSC_STAGE_STATUS;
-      TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_in, (uint8_t*) &p_msc->csw, sizeof(msc_csw_t)));
-    break;
+      TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_in, (uint8_t*) &p_msc->csw, (uint16_t) sizeof(msc_csw_t)));
+      break;
 
     case MSC_STAGE_STATUS:
       // SCSI op is complete
       p_msc->stage = MSC_STAGE_IDLE;
 
-      if (p_msc->complete_cb) p_msc->complete_cb(dev_addr, cbw, csw);
-    break;
+      if (p_msc->complete_cb) {
+        tuh_msc_complete_data_t const cb_data = {
+            .cbw = cbw,
+            .csw = csw,
+            .scsi_data = p_msc->buffer,
+            .user_arg = p_msc->complete_arg
+        };
+        p_msc->complete_cb(dev_addr, &cb_data);
+      }
+      break;
 
-    // unknown state
-    default: break;
+      // unknown state
+    default:
+      break;
   }
 
   return true;
@@ -352,115 +363,124 @@ bool msch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32
 //--------------------------------------------------------------------+
 // MSC Enumeration
 //--------------------------------------------------------------------+
+static void config_get_maxlun_complete(tuh_xfer_t* xfer);
+static bool config_test_unit_ready_complete(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data);
+static bool config_request_sense_complete(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data);
+static bool config_read_capacity_complete(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data);
 
-static bool config_get_maxlun_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result);
-static bool config_test_unit_ready_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw);
-static bool config_request_sense_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw);
-static bool config_read_capacity_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw);
-
-bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t *p_length)
-{
+bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const* desc_itf, uint16_t max_len) {
+  (void) rhport;
   TU_VERIFY (MSC_SUBCLASS_SCSI == desc_itf->bInterfaceSubClass &&
-             MSC_PROTOCOL_BOT  == desc_itf->bInterfaceProtocol);
+             MSC_PROTOCOL_BOT == desc_itf->bInterfaceProtocol);
+
+  // msc driver length is fixed
+  uint16_t const drv_len = (uint16_t) (sizeof(tusb_desc_interface_t) +
+                                       desc_itf->bNumEndpoints * sizeof(tusb_desc_endpoint_t));
+  TU_ASSERT(drv_len <= max_len);
 
   msch_interface_t* p_msc = get_itf(dev_addr);
-  tusb_desc_endpoint_t const * ep_desc = (tusb_desc_endpoint_t const *) tu_desc_next(desc_itf);
+  tusb_desc_endpoint_t const* ep_desc = (tusb_desc_endpoint_t const*) tu_desc_next(desc_itf);
 
-  for(uint32_t i=0; i<2; i++)
-  {
+  for (uint32_t i = 0; i < 2; i++) {
     TU_ASSERT(TUSB_DESC_ENDPOINT == ep_desc->bDescriptorType && TUSB_XFER_BULK == ep_desc->bmAttributes.xfer);
-    TU_ASSERT(usbh_edpt_open(rhport, dev_addr, ep_desc));
+    TU_ASSERT(tuh_edpt_open(dev_addr, ep_desc));
 
-    if ( tu_edpt_dir(ep_desc->bEndpointAddress) == TUSB_DIR_IN )
-    {
+    if (TUSB_DIR_IN == tu_edpt_dir(ep_desc->bEndpointAddress)) {
       p_msc->ep_in = ep_desc->bEndpointAddress;
-    }else
-    {
+    } else {
       p_msc->ep_out = ep_desc->bEndpointAddress;
     }
 
-    ep_desc = (tusb_desc_endpoint_t const *) tu_desc_next(ep_desc);
+    ep_desc = (tusb_desc_endpoint_t const*) tu_desc_next(ep_desc);
   }
 
   p_msc->itf_num = desc_itf->bInterfaceNumber;
-  (*p_length) += sizeof(tusb_desc_interface_t) + 2*sizeof(tusb_desc_endpoint_t);
 
   return true;
 }
 
-bool msch_set_config(uint8_t dev_addr, uint8_t itf_num)
-{
+bool msch_set_config(uint8_t dev_addr, uint8_t itf_num) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   TU_ASSERT(p_msc->itf_num == itf_num);
 
   p_msc->configured = true;
 
   //------------- Get Max Lun -------------//
-  TU_LOG2("MSC Get Max Lun\r\n");
-  tusb_control_request_t request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_INTERFACE,
-      .type      = TUSB_REQ_TYPE_CLASS,
-      .direction = TUSB_DIR_IN
-    },
-    .bRequest = MSC_REQ_GET_MAX_LUN,
-    .wValue   = 0,
-    .wIndex   = itf_num,
-    .wLength  = 1
+  TU_LOG_DRV("MSC Get Max Lun\r\n");
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_INTERFACE,
+          .type      = TUSB_REQ_TYPE_CLASS,
+          .direction = TUSB_DIR_IN
+      },
+      .bRequest = MSC_REQ_GET_MAX_LUN,
+      .wValue   = 0,
+      .wIndex   = itf_num,
+      .wLength  = 1
   };
-  TU_ASSERT(tuh_control_xfer(dev_addr, &request, &p_msc->max_lun, config_get_maxlun_complete));
+
+  tuh_xfer_t xfer = {
+      .daddr       = dev_addr,
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = _msch_buffer,
+      .complete_cb = config_get_maxlun_complete,
+      .user_data    = 0
+  };
+  TU_ASSERT(tuh_control_xfer(&xfer));
 
   return true;
 }
 
-static bool config_get_maxlun_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result)
-{
-  (void) request;
-
-  msch_interface_t* p_msc = get_itf(dev_addr);
+static void config_get_maxlun_complete(tuh_xfer_t* xfer) {
+  uint8_t const daddr = xfer->daddr;
+  msch_interface_t* p_msc = get_itf(daddr);
 
   // STALL means zero
-  p_msc->max_lun = (XFER_RESULT_SUCCESS == result) ? _msch_buffer[0] : 0;
+  p_msc->max_lun = (XFER_RESULT_SUCCESS == xfer->result) ? _msch_buffer[0] : 0;
   p_msc->max_lun++; // MAX LUN is minus 1 by specs
 
+  TU_LOG_DRV("  Max LUN = %u\r\n", p_msc->max_lun);
+
   // TODO multiple LUN support
-  TU_LOG2("SCSI Test Unit Ready\r\n");
+  TU_LOG_DRV("SCSI Test Unit Ready\r\n");
   uint8_t const lun = 0;
-  tuh_msc_test_unit_ready(dev_addr, lun, config_test_unit_ready_complete);
-
-  return true;
+  tuh_msc_test_unit_ready(daddr, lun, config_test_unit_ready_complete, 0);
 }
 
-static bool config_test_unit_ready_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw)
-{
-  if (csw->status == 0)
-  {
+static bool config_test_unit_ready_complete(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data) {
+  msc_cbw_t const* cbw = cb_data->cbw;
+  msc_csw_t const* csw = cb_data->csw;
+
+  if (csw->status == 0) {
     // Unit is ready, read its capacity
-    TU_LOG2("SCSI Read Capacity\r\n");
-    tuh_msc_read_capacity(dev_addr, cbw->lun, (scsi_read_capacity10_resp_t*) ((void*) _msch_buffer), config_read_capacity_complete);
-  }else
-  {
+    TU_LOG_DRV("SCSI Read Capacity\r\n");
+    tuh_msc_read_capacity(dev_addr, cbw->lun, (scsi_read_capacity10_resp_t*) ((void*) _msch_buffer),
+                          config_read_capacity_complete, 0);
+  } else {
     // Note: During enumeration, some device fails Test Unit Ready and require a few retries
     // with Request Sense to start working !!
     // TODO limit number of retries
-    TU_LOG2("SCSI Request Sense\r\n");
-    TU_ASSERT(tuh_msc_request_sense(dev_addr, cbw->lun, _msch_buffer, config_request_sense_complete));
+    TU_LOG_DRV("SCSI Request Sense\r\n");
+    TU_ASSERT(tuh_msc_request_sense(dev_addr, cbw->lun, _msch_buffer, config_request_sense_complete, 0));
   }
 
   return true;
 }
 
-static bool config_request_sense_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw)
-{
+static bool config_request_sense_complete(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data) {
+  msc_cbw_t const* cbw = cb_data->cbw;
+  msc_csw_t const* csw = cb_data->csw;
+
   TU_ASSERT(csw->status == 0);
-  TU_ASSERT(tuh_msc_test_unit_ready(dev_addr, cbw->lun, config_test_unit_ready_complete));
+  TU_ASSERT(tuh_msc_test_unit_ready(dev_addr, cbw->lun, config_test_unit_ready_complete, 0));
   return true;
 }
 
-static bool config_read_capacity_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw)
-{
+static bool config_read_capacity_complete(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data) {
+  msc_cbw_t const* cbw = cb_data->cbw;
+  msc_csw_t const* csw = cb_data->csw;
+
   TU_ASSERT(csw->status == 0);
 
   msch_interface_t* p_msc = get_itf(dev_addr);
@@ -468,7 +488,7 @@ static bool config_read_capacity_complete(uint8_t dev_addr, msc_cbw_t const* cbw
   // Capacity response field: Block size and Last LBA are both Big-Endian
   scsi_read_capacity10_resp_t* resp = (scsi_read_capacity10_resp_t*) ((void*) _msch_buffer);
   p_msc->capacity[cbw->lun].block_count = tu_ntohl(resp->last_lba) + 1;
-  p_msc->capacity[cbw->lun].block_size = tu_ntohl(resp->block_size);
+  p_msc->capacity[cbw->lun].block_size  = tu_ntohl(resp->block_size);
 
   // Mark enumeration is complete
   p_msc->mounted = true;
diff --git a/src/class/msc/msc_host.h b/src/class/msc/msc_host.h
index ce4fe64dc..9fda566d8 100644
--- a/src/class/msc/msc_host.h
+++ b/src/class/msc/msc_host.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -24,8 +24,8 @@
  * This file is part of the TinyUSB stack.
  */
 
-#ifndef _TUSB_MSC_HOST_H_
-#define _TUSB_MSC_HOST_H_
+#ifndef TUSB_MSC_HOST_H_
+#define TUSB_MSC_HOST_H_
 
 #include "msc.h"
 
@@ -41,14 +41,14 @@
 #define CFG_TUH_MSC_MAXLUN  4
 #endif
 
+typedef struct {
+  msc_cbw_t const* cbw; // SCSI command
+  msc_csw_t const* csw; // SCSI status
+  void* scsi_data;      // SCSI Data
+  uintptr_t user_arg;   // user argument
+}tuh_msc_complete_data_t;
 
-/** \addtogroup ClassDriver_MSC
- *  @{
- * \defgroup MSC_Host Host
- *  The interface API includes status checking function, data transferring function and callback functions
- *  @{ */
-
-typedef bool (*tuh_msc_complete_cb_t)(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw);
+typedef bool (*tuh_msc_complete_cb_t)(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data);
 
 //--------------------------------------------------------------------+
 // Application API
@@ -73,33 +73,33 @@ uint32_t tuh_msc_get_block_size(uint8_t dev_addr, uint8_t lun);
 // Perform a full SCSI command (cbw, data, csw) in non-blocking manner.
 // Complete callback is invoked when SCSI op is complete.
 // return true if success, false if there is already pending operation.
-bool tuh_msc_scsi_command(uint8_t dev_addr, msc_cbw_t const* cbw, void* data, tuh_msc_complete_cb_t complete_cb);
+bool tuh_msc_scsi_command(uint8_t daddr, msc_cbw_t const* cbw, void* data, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 // Perform SCSI Inquiry command
 // Complete callback is invoked when SCSI op is complete.
-bool tuh_msc_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* response, tuh_msc_complete_cb_t complete_cb);
+bool tuh_msc_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* response, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 // Perform SCSI Test Unit Ready command
 // Complete callback is invoked when SCSI op is complete.
-bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_t complete_cb);
+bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 // Perform SCSI Request Sense 10 command
 // Complete callback is invoked when SCSI op is complete.
-bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void *resposne, tuh_msc_complete_cb_t complete_cb);
+bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void *response, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 // Perform SCSI Read 10 command. Read n blocks starting from LBA to buffer
 // Complete callback is invoked when SCSI op is complete.
-bool  tuh_msc_read10(uint8_t dev_addr, uint8_t lun, void * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb);
+bool tuh_msc_read10(uint8_t dev_addr, uint8_t lun, void * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 // Perform SCSI Write 10 command. Write n blocks starting from LBA to device
 // Complete callback is invoked when SCSI op is complete.
-bool tuh_msc_write10(uint8_t dev_addr, uint8_t lun, void const * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb);
+bool tuh_msc_write10(uint8_t dev_addr, uint8_t lun, void const * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 // Perform SCSI Read Capacity 10 command
 // Complete callback is invoked when SCSI op is complete.
 // Note: during enumeration, host stack already carried out this request. Application can retrieve capacity by
 // simply call tuh_msc_get_block_count() and tuh_msc_get_block_size()
-bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_resp_t* response, tuh_msc_complete_cb_t complete_cb);
+bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_resp_t* response, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 //------------- Application Callback -------------//
 
@@ -113,17 +113,15 @@ TU_ATTR_WEAK void tuh_msc_umount_cb(uint8_t dev_addr);
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 
-void msch_init(void);
-bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t *p_length);
-bool msch_set_config(uint8_t dev_addr, uint8_t itf_num);
-bool msch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
-void msch_close(uint8_t dev_addr);
+bool msch_init       (void);
+bool msch_deinit     (void);
+bool msch_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len);
+bool msch_set_config (uint8_t dev_addr, uint8_t itf_num);
+void msch_close      (uint8_t dev_addr);
+bool msch_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
 
 #ifdef __cplusplus
  }
 #endif
 
-#endif /* _TUSB_MSC_HOST_H_ */
-
-/// @}
-/// @}
+#endif