Merge pull request #1058 from hathach/usbcv-compliant-test

nrf5x USB Compliance Verification Test suite

3 files changed, 391 insertions, 177 deletions

diff --git a/src/class/hid/hid_device.h b/src/class/hid/hid_device.h
index e2c950dd1..5cf4b3060 100644
--- a/src/class/hid/hid_device.h
+++ b/src/class/hid/hid_device.h
@@ -195,16 +195,7 @@ static inline bool  tud_hid_gamepad_report(uint8_t report_id, int8_t x, int8_t y
       HID_REPORT_COUNT ( 1                                      )  ,\
       HID_REPORT_SIZE  ( 8                                      )  ,\
       HID_INPUT        ( HID_CONSTANT                           )  ,\
-    /* 6-byte Keycodes */ \
-    HID_USAGE_PAGE ( HID_USAGE_PAGE_KEYBOARD )                     ,\
-      HID_USAGE_MIN    ( 0                                   )     ,\
-      HID_USAGE_MAX_N  ( 255, 2                              )     ,\
-      HID_LOGICAL_MIN  ( 0                                   )     ,\
-      HID_LOGICAL_MAX_N( 255, 2                              )     ,\
-      HID_REPORT_COUNT ( 6                                   )     ,\
-      HID_REPORT_SIZE  ( 8                                   )     ,\
-      HID_INPUT        ( HID_DATA | HID_ARRAY | HID_ABSOLUTE )     ,\
-    /* 5-bit LED Indicator Kana | Compose | ScrollLock | CapsLock | NumLock */ \
+    /* Output 5-bit LED Indicator Kana | Compose | ScrollLock | CapsLock | NumLock */ \
     HID_USAGE_PAGE  ( HID_USAGE_PAGE_LED                   )       ,\
       HID_USAGE_MIN    ( 1                                       ) ,\
       HID_USAGE_MAX    ( 5                                       ) ,\
@@ -215,6 +206,15 @@ static inline bool  tud_hid_gamepad_report(uint8_t report_id, int8_t x, int8_t y
       HID_REPORT_COUNT ( 1                                       ) ,\
       HID_REPORT_SIZE  ( 3                                       ) ,\
       HID_OUTPUT       ( HID_CONSTANT                            ) ,\
+    /* 6-byte Keycodes */ \
+    HID_USAGE_PAGE ( HID_USAGE_PAGE_KEYBOARD )                     ,\
+      HID_USAGE_MIN    ( 0                                   )     ,\
+      HID_USAGE_MAX_N  ( 255, 2                              )     ,\
+      HID_LOGICAL_MIN  ( 0                                   )     ,\
+      HID_LOGICAL_MAX_N( 255, 2                              )     ,\
+      HID_REPORT_COUNT ( 6                                   )     ,\
+      HID_REPORT_SIZE  ( 8                                   )     ,\
+      HID_INPUT        ( HID_DATA | HID_ARRAY | HID_ABSOLUTE )     ,\
   HID_COLLECTION_END \
 
 // Mouse Report Descriptor Template
diff --git a/src/class/msc/msc_device.c b/src/class/msc/msc_device.c
index 014e8b69e..7dcd4983e 100644
--- a/src/class/msc/msc_device.c
+++ b/src/class/msc/msc_device.c
@@ -46,7 +46,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
@@ -61,7 +62,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
@@ -78,7 +79,55 @@ CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t _mscd_buf[CFG_TUD_MSC_EP_
 //--------------------------------------------------------------------+
 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[])
 {
@@ -89,15 +138,60 @@ 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 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(MSC_DEBUG, "  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(MSC_DEBUG, "  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(MSC_DEBUG, "  SCSI case 8 (Hi <> Do)\r\n");
+      status = MSC_CSW_STATUS_PHASE_ERROR;
+    }
+    else if ( !block_count )
+    {
+      TU_LOG(MSC_DEBUG, "  SCSI case 4 Hi > Dn\r\n");
+      status =  MSC_CSW_STATUS_FAILED;
+    }
+  }
+
+  return status;
+}
+
 //--------------------------------------------------------------------+
 // Debug
 //--------------------------------------------------------------------+
@@ -174,35 +268,89 @@ 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;
 
-  switch ( p_request->bRequest )
+  // 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
+          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 ( request->bRequest )
   {
     case MSC_REQ_RESET:
-      // TODO: Actually reset interface.
-      tud_control_status(rhport, p_request);
+      TU_LOG(MSC_DEBUG, "  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(MSC_DEBUG, "  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);
@@ -210,7 +358,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;
 
@@ -222,6 +370,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;
@@ -233,46 +383,79 @@ 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(MSC_DEBUG, "  SCSI CBW is not valid\r\n");
+
+        // 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(MSC_DEBUG, "  SCSI Command: %s\r\n", tu_lookup_find(&_msc_scsi_cmd_table, p_cbw->command[0]));
-      // TU_LOG_MEM(MSC_DEBUG, p_cbw, xferred_bytes, 2);
+      //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(MSC_DEBUG, "  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, 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);
@@ -280,30 +463,37 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
 
           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(MSC_DEBUG, "  SCSI unsupported 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, p_msc->total_len) );
+            }
+          }
         }
       }
     break;
@@ -312,87 +502,51 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
       TU_LOG(MSC_DEBUG, "  SCSI Data\r\n");
       //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, 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(MSC_DEBUG, "  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();
         }
       }
@@ -406,7 +560,7 @@ 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_LOG(MSC_DEBUG, "  SCSI Status: %u\r\n", p_csw->status);
+        TU_LOG(MSC_DEBUG, "  SCSI Status = %u\r\n", p_csw->status);
         // TU_LOG_MEM(MSC_DEBUG, p_csw, xferred_bytes, 2);
 
         // Invoke complete callback if defined
@@ -427,11 +581,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;
 
@@ -440,21 +594,18 @@ 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) )
-    {
-      // 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);
-    }
-    else
+    // skip status if epin is currently stalled, will do it when received Clear Stall request
+    if ( !usbd_edpt_stalled(rhport,  p_msc->ep_in) )
     {
-      // 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 ( (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) );
+      }
     }
   }
 
@@ -472,6 +623,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:
@@ -482,7 +635,7 @@ 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);
+        if ( p_msc->sense_key == 0 ) tud_msc_set_sense(lun, SCSI_SENSE_NOT_READY, 0x04, 0x00);
       }
     break;
 
@@ -498,7 +651,7 @@ 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);
+          if ( p_msc->sense_key == 0 ) tud_msc_set_sense(lun, SCSI_SENSE_NOT_READY, 0x04, 0x00);
         }
       }
     break;
@@ -519,7 +672,7 @@ 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);
+        if ( p_msc->sense_key == 0 ) tud_msc_set_sense(lun, SCSI_SENSE_NOT_READY, 0x04, 0x00);
       }else
       {
         scsi_read_capacity10_resp_t read_capa10;
@@ -555,7 +708,7 @@ 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);
+        if ( p_msc->sense_key == 0 ) tud_msc_set_sense(lun, SCSI_SENSE_NOT_READY, 0x04, 0x00);
       }else
       {
         read_fmt_capa.block_num = tu_htonl(block_count);
@@ -600,9 +753,11 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
       };
 
       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);
@@ -620,9 +775,9 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
 
       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.sense_key           = p_msc->sense_key;
+      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);
@@ -641,13 +796,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);
@@ -656,16 +807,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(MSC_DEBUG, "  tud_msc_read10_cb() return -1\r\n");
+
+    // Sense = Flash not ready for access
+    tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_MEDIUM_ERROR, 0x33, 0x00);
 
-    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 )
   {
@@ -682,16 +835,18 @@ 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;
   }
 
@@ -702,4 +857,63 @@ static void proc_write10_cmd(uint8_t rhport, mscd_interface_t* p_msc)
   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(MSC_DEBUG, "  tud_msc_write10_cb() return -1\r\n");
+
+    // update actual byte before failed
+    p_msc->xferred_len += xferred_bytes;
+
+    // Sense = Flash not ready for access
+    tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_MEDIUM_ERROR, 0x33, 0x00);
+
+    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 )
+    {
+      if ( nbytes > 0 )
+      {
+        p_msc->xferred_len += nbytes;
+        memmove(_mscd_buf, _mscd_buf+nbytes, xferred_bytes-nbytes);
+      }
+
+      // simulate an transfer complete with adjusted parameters --> callback will be invoked with adjusted parameter
+      dcd_event_xfer_complete(rhport, p_msc->ep_out, xferred_bytes-nbytes, 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..d32694340 100644
--- a/src/class/msc/msc_device.h
+++ b/src/class/msc/msc_device.h
@@ -140,7 +140,7 @@ 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);
 
 //--------------------------------------------------------------------+