Merge branch 'master' into pico

19 files changed, 553 insertions, 549 deletions

diff --git a/src/class/audio/audio_device.c b/src/class/audio/audio_device.c
index 2c8ba300d..8581925e4 100644
--- a/src/class/audio/audio_device.c
+++ b/src/class/audio/audio_device.c
@@ -989,7 +989,7 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
 
 // Invoked when class request DATA stage is finished.
 // return false to stall control EP (e.g Host send non-sense DATA)
-bool audiod_control_complete(uint8_t rhport, tusb_control_request_t const * p_request)
+static bool audiod_control_complete(uint8_t rhport, tusb_control_request_t const * p_request)
 {
   // Handle audio class specific set requests
   if(p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS && p_request->bmRequestType_bit.direction == TUSB_DIR_OUT)
@@ -1065,7 +1065,7 @@ bool audiod_control_complete(uint8_t rhport, tusb_control_request_t const * p_re
 
 // Handle class control request
 // return false to stall control endpoint (e.g unsupported request)
-bool audiod_control_request(uint8_t rhport, tusb_control_request_t const * p_request)
+static bool audiod_control_request(uint8_t rhport, tusb_control_request_t const * p_request)
 {
   (void) rhport;
 
@@ -1175,6 +1175,20 @@ bool audiod_control_request(uint8_t rhport, tusb_control_request_t const * p_req
   return false;
 }
 
+bool audiod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
+{
+  if ( stage == CONTROL_STAGE_SETUP )
+  {
+    return audiod_control_request(rhport, request);
+  }
+  else if ( stage == CONTROL_STAGE_DATA )
+  {
+    return audiod_control_complete(rhport, request);
+  }
+
+  return true;
+}
+
 bool audiod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
 {
   (void) result;
diff --git a/src/class/audio/audio_device.h b/src/class/audio/audio_device.h
index d86232720..5061501ce 100644
--- a/src/class/audio/audio_device.h
+++ b/src/class/audio/audio_device.h
@@ -384,11 +384,10 @@ static inline uint16_t tud_audio_int_ctr_write(uint8_t const* buffer, uint16_t b
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-void audiod_init             (void);
-void audiod_reset            (uint8_t rhport);
-uint16_t audiod_open         (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
-bool audiod_control_request  (uint8_t rhport, tusb_control_request_t const * request);
-bool audiod_control_complete (uint8_t rhport, tusb_control_request_t const * request);
+void audiod_init            (void);
+void audiod_reset           (uint8_t rhport);
+uint16_t audiod_open        (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
+bool audiod_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
 bool audiod_xfer_cb          (uint8_t rhport, uint8_t edpt_addr, xfer_result_t result, uint32_t xferred_bytes);
 
 #ifdef __cplusplus
diff --git a/src/class/bth/bth_device.c b/src/class/bth/bth_device.c
index 252e20e37..481dc134e 100755
--- a/src/class/bth/bth_device.c
+++ b/src/class/bth/bth_device.c
@@ -186,43 +186,46 @@ uint16_t btd_open(uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_
   return drv_len;
 }
 
-bool btd_control_complete(uint8_t rhport, tusb_control_request_t const *request)
+// 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 btd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request)
 {
   (void)rhport;
 
-  // Handle class request only
-  TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
-
-  if (tud_bt_hci_cmd_cb) tud_bt_hci_cmd_cb(&_btd_itf.hci_cmd, request->wLength);
-
-  return true;
-}
-
-bool btd_control_request(uint8_t rhport, tusb_control_request_t const *request)
-{
-  (void)rhport;
-
-  if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS &&
-      request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_DEVICE)
-  {
-    // HCI command packet addressing for single function Primary Controllers
-    TU_VERIFY(request->bRequest == 0 && request->wValue == 0 && request->wIndex == 0);
-  }
-  else if (request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE)
+  if ( stage == CONTROL_STAGE_SETUP )
   {
-    if (request->bRequest == TUSB_REQ_SET_INTERFACE && _btd_itf.itf_num + 1 == request->wIndex)
+    if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS &&
+        request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_DEVICE)
     {
-      // TODO: Set interface it would involve changing size of endpoint size
+      // HCI command packet addressing for single function Primary Controllers
+      TU_VERIFY(request->bRequest == 0 && request->wValue == 0 && request->wIndex == 0);
     }
-    else
+    else if (request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE)
     {
-      // HCI command packet for Primary Controller function in a composite device
-      TU_VERIFY(request->bRequest == 0 && request->wValue == 0 && request->wIndex == _btd_itf.itf_num);
+      if (request->bRequest == TUSB_REQ_SET_INTERFACE && _btd_itf.itf_num + 1 == request->wIndex)
+      {
+        // TODO: Set interface it would involve changing size of endpoint size
+      }
+      else
+      {
+        // HCI command packet for Primary Controller function in a composite device
+        TU_VERIFY(request->bRequest == 0 && request->wValue == 0 && request->wIndex == _btd_itf.itf_num);
+      }
     }
+    else return false;
+
+    return tud_control_xfer(rhport, request, &_btd_itf.hci_cmd, request->wLength);
+  }
+  else if ( stage == CONTROL_STAGE_DATA )
+  {
+    // Handle class request only
+    TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
+
+    if (tud_bt_hci_cmd_cb) tud_bt_hci_cmd_cb(&_btd_itf.hci_cmd, request->wLength);
   }
-  else return false;
 
-  return tud_control_xfer(rhport, request, &_btd_itf.hci_cmd, request->wLength);
+  return true;
 }
 
 bool btd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
@@ -246,7 +249,7 @@ bool btd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t
     if (tud_bt_acl_data_sent_cb) tud_bt_acl_data_sent_cb((uint16_t)xferred_bytes);
   }
 
-  return TUSB_ERROR_NONE;
+  return true;
 }
 
 #endif
diff --git a/src/class/bth/bth_device.h b/src/class/bth/bth_device.h
index 5e5468084..1b90d0915 100755
--- a/src/class/bth/bth_device.h
+++ b/src/class/bth/bth_device.h
@@ -96,12 +96,11 @@ bool tud_bt_acl_data_send(void *acl_data, uint16_t data_len);
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-void     btd_init             (void);
-void     btd_reset            (uint8_t rhport);
-uint16_t btd_open             (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
-bool     btd_control_request  (uint8_t rhport, tusb_control_request_t const * request);
-bool     btd_control_complete (uint8_t rhport, tusb_control_request_t const * request);
-bool     btd_xfer_cb          (uint8_t rhport, uint8_t edpt_addr, xfer_result_t result, uint32_t xferred_bytes);
+void     btd_init            (void);
+void     btd_reset           (uint8_t rhport);
+uint16_t btd_open            (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
+bool     btd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const *request);
+bool     btd_xfer_cb         (uint8_t rhport, uint8_t edpt_addr, xfer_result_t result, uint32_t xferred_bytes);
 
 #ifdef __cplusplus
  }
diff --git a/src/class/cdc/cdc_device.c b/src/class/cdc/cdc_device.c
index e54b7d260..58f485a5d 100644
--- a/src/class/cdc/cdc_device.c
+++ b/src/class/cdc/cdc_device.c
@@ -178,6 +178,9 @@ uint32_t tud_cdc_n_write_flush (uint8_t itf)
 {
   cdcd_interface_t* p_cdc = &_cdcd_itf[itf];
 
+  // Skip if usb is not ready yet
+  TU_VERIFY( tud_ready(), 0 );
+
   // No data to send
   if ( !tu_fifo_count(&p_cdc->tx_ff) ) return 0;
 
@@ -189,7 +192,7 @@ uint32_t tud_cdc_n_write_flush (uint8_t itf)
   // Pull data from FIFO
   uint16_t const count = tu_fifo_read_n(&p_cdc->tx_ff, p_cdc->epin_buf, sizeof(p_cdc->epin_buf));
 
-  if ( count && tud_cdc_n_connected(itf) )
+  if ( count )
   {
     TU_ASSERT( usbd_edpt_xfer(rhport, p_cdc->ep_in, p_cdc->epin_buf, count), 0 );
     return count;
@@ -207,6 +210,10 @@ uint32_t tud_cdc_n_write_available (uint8_t itf)
   return tu_fifo_remaining(&_cdcd_itf[itf].tx_ff);
 }
 
+bool tud_cdc_n_write_clear (uint8_t itf)
+{
+  return tu_fifo_clear(&_cdcd_itf[itf].tx_ff);
+}
 
 //--------------------------------------------------------------------+
 // USBD Driver API
@@ -227,9 +234,13 @@ void cdcd_init(void)
     p_cdc->line_coding.parity    = 0;
     p_cdc->line_coding.data_bits = 8;
 
-    // config fifo
+    // Config RX fifo
     tu_fifo_config(&p_cdc->rx_ff, p_cdc->rx_ff_buf, TU_ARRAY_SIZE(p_cdc->rx_ff_buf), 1, false);
-    tu_fifo_config(&p_cdc->tx_ff, p_cdc->tx_ff_buf, TU_ARRAY_SIZE(p_cdc->tx_ff_buf), 1, false);
+
+    // Config TX fifo as overwritable at initialization and will be changed to non-overwritable
+    // if terminal supports DTR bit. Without DTR we do not know if data is actually polled by terminal.
+    // In this way, the most current data is prioritized.
+    tu_fifo_config(&p_cdc->tx_ff, p_cdc->tx_ff_buf, TU_ARRAY_SIZE(p_cdc->tx_ff_buf), 1, true);
 
 #if CFG_FIFO_MUTEX
     tu_fifo_config_mutex(&p_cdc->rx_ff, osal_mutex_create(&p_cdc->rx_ff_mutex));
@@ -244,9 +255,12 @@ void cdcd_reset(uint8_t rhport)
 
   for(uint8_t i=0; i<CFG_TUD_CDC; i++)
   {
-    tu_memclr(&_cdcd_itf[i], ITF_MEM_RESET_SIZE);
-    tu_fifo_clear(&_cdcd_itf[i].rx_ff);
-    tu_fifo_clear(&_cdcd_itf[i].tx_ff);
+    cdcd_interface_t* p_cdc = &_cdcd_itf[i];
+
+    tu_memclr(p_cdc, ITF_MEM_RESET_SIZE);
+    tu_fifo_clear(&p_cdc->rx_ff);
+    tu_fifo_clear(&p_cdc->tx_ff);
+    tu_fifo_set_overwritable(&p_cdc->tx_ff, true);
   }
 }
 
@@ -315,38 +329,10 @@ uint16_t cdcd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint1
   return drv_len;
 }
 
-// Invoked when class request DATA stage is finished.
-// return false to stall control endpoint (e.g Host send non-sense DATA)
-bool cdcd_control_complete(uint8_t rhport, tusb_control_request_t const * request)
-{
-  (void) rhport;
-
-  //------------- Class Specific Request -------------//
-  TU_VERIFY (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
-
-  uint8_t itf = 0;
-  cdcd_interface_t* p_cdc = _cdcd_itf;
-
-  // Identify which interface to use
-  for ( ; ; itf++, p_cdc++)
-  {
-    if (itf >= TU_ARRAY_SIZE(_cdcd_itf)) return false;
-
-    if ( p_cdc->itf_num == request->wIndex ) break;
-  }
-
-  // Invoke callback
-  if ( CDC_REQUEST_SET_LINE_CODING == request->bRequest )
-  {
-    if ( tud_cdc_line_coding_cb ) tud_cdc_line_coding_cb(itf, &p_cdc->line_coding);
-  }
-
-  return true;
-}
-
-// Handle class control request
+// 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 cdcd_control_request(uint8_t rhport, tusb_control_request_t const * request)
+bool cdcd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
 {
   // Handle class request only
   TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
@@ -365,34 +351,50 @@ bool cdcd_control_request(uint8_t rhport, tusb_control_request_t const * request
   switch ( request->bRequest )
   {
     case CDC_REQUEST_SET_LINE_CODING:
-      TU_LOG2("  Set Line Coding\r\n");
-      tud_control_xfer(rhport, request, &p_cdc->line_coding, sizeof(cdc_line_coding_t));
+      if (stage == CONTROL_STAGE_SETUP)
+      {
+        TU_LOG2("  Set Line Coding\r\n");
+        tud_control_xfer(rhport, request, &p_cdc->line_coding, sizeof(cdc_line_coding_t));
+      }
+      else if ( stage == CONTROL_STAGE_ACK)
+      {
+        if ( tud_cdc_line_coding_cb ) tud_cdc_line_coding_cb(itf, &p_cdc->line_coding);
+      }
     break;
 
     case CDC_REQUEST_GET_LINE_CODING:
-      TU_LOG2("  Get Line Coding\r\n");
-      tud_control_xfer(rhport, request, &p_cdc->line_coding, sizeof(cdc_line_coding_t));
+      if (stage == CONTROL_STAGE_SETUP)
+      {
+        TU_LOG2("  Get Line Coding\r\n");
+        tud_control_xfer(rhport, request, &p_cdc->line_coding, sizeof(cdc_line_coding_t));
+      }
     break;
 
     case CDC_REQUEST_SET_CONTROL_LINE_STATE:
-    {
-      // CDC PSTN v1.2 section 6.3.12
-      // Bit 0: Indicates if DTE is present or not.
-      //        This signal corresponds to V.24 signal 108/2 and RS-232 signal DTR (Data Terminal Ready)
-      // Bit 1: Carrier control for half-duplex modems.
-      //        This signal corresponds to V.24 signal 105 and RS-232 signal RTS (Request to Send)
-      bool const dtr = tu_bit_test(request->wValue, 0);
-      bool const rts = tu_bit_test(request->wValue, 1);
-
-      p_cdc->line_state = (uint8_t) request->wValue;
+      if (stage == CONTROL_STAGE_SETUP)
+      {
+        tud_control_status(rhport, request);
+      }
+      else if (stage == CONTROL_STAGE_ACK)
+      {
+        // CDC PSTN v1.2 section 6.3.12
+        // Bit 0: Indicates if DTE is present or not.
+        //        This signal corresponds to V.24 signal 108/2 and RS-232 signal DTR (Data Terminal Ready)
+        // Bit 1: Carrier control for half-duplex modems.
+        //        This signal corresponds to V.24 signal 105 and RS-232 signal RTS (Request to Send)
+        bool const dtr = tu_bit_test(request->wValue, 0);
+        bool const rts = tu_bit_test(request->wValue, 1);
 
-      TU_LOG2("  Set Control Line State: DTR = %d, RTS = %d\r\n", dtr, rts);
+        p_cdc->line_state = (uint8_t) request->wValue;
+        
+        // Disable fifo overwriting if DTR bit is set
+        tu_fifo_set_overwritable(&p_cdc->tx_ff, !dtr);
 
-      tud_control_status(rhport, request);
+        TU_LOG2("  Set Control Line State: DTR = %d, RTS = %d\r\n", dtr, rts);
 
-      // Invoke callback
-      if ( tud_cdc_line_state_cb ) tud_cdc_line_state_cb(itf, dtr, rts);
-    }
+        // Invoke callback
+        if ( tud_cdc_line_state_cb ) tud_cdc_line_state_cb(itf, dtr, rts);
+      }
     break;
 
     default: return false; // stall unsupported request
diff --git a/src/class/cdc/cdc_device.h b/src/class/cdc/cdc_device.h
index 3c679c48e..62dcd3c0a 100644
--- a/src/class/cdc/cdc_device.h
+++ b/src/class/cdc/cdc_device.h
@@ -102,6 +102,9 @@ uint32_t tud_cdc_n_write_flush     (uint8_t itf);
 // Return the number of bytes (characters) available for writing to TX FIFO buffer in a single n_write operation.
 uint32_t tud_cdc_n_write_available (uint8_t itf);
 
+// Clear the transmit FIFO
+bool tud_cdc_n_write_clear (uint8_t itf);
+
 //--------------------------------------------------------------------+
 // Application API (Single Port)
 //--------------------------------------------------------------------+
@@ -121,6 +124,7 @@ static inline uint32_t tud_cdc_write           (void const* buffer, uint32_t buf
 static inline uint32_t tud_cdc_write_str       (char const* str);
 static inline uint32_t tud_cdc_write_flush     (void);
 static inline uint32_t tud_cdc_write_available (void);
+static inline bool     tud_cdc_write_clear     (void);
 
 //--------------------------------------------------------------------+
 // Application Callback API (weak is optional)
@@ -230,18 +234,22 @@ static inline uint32_t tud_cdc_write_available(void)
   return tud_cdc_n_write_available(0);
 }
 
+static inline bool tud_cdc_write_clear(void)
+{
+  return tud_cdc_n_write_clear(0);
+}
+
 /** @} */
 /** @} */
 
 //--------------------------------------------------------------------+
 // INTERNAL USBD-CLASS DRIVER API
 //--------------------------------------------------------------------+
-void     cdcd_init             (void);
-void     cdcd_reset            (uint8_t rhport);
-uint16_t cdcd_open             (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
-bool     cdcd_control_request  (uint8_t rhport, tusb_control_request_t const * request);
-bool     cdcd_control_complete (uint8_t rhport, tusb_control_request_t const * request);
-bool     cdcd_xfer_cb          (uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
+void     cdcd_init            (void);
+void     cdcd_reset           (uint8_t rhport);
+uint16_t cdcd_open            (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
+bool     cdcd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
+bool     cdcd_xfer_cb         (uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
 
 #ifdef __cplusplus
  }
diff --git a/src/class/dfu/dfu_rt_device.c b/src/class/dfu/dfu_rt_device.c
index d4c3ecb62..38644615a 100644
--- a/src/class/dfu/dfu_rt_device.c
+++ b/src/class/dfu/dfu_rt_device.c
@@ -85,17 +85,14 @@ uint16_t dfu_rtd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, ui
   return drv_len;
 }
 
-bool dfu_rtd_control_complete(uint8_t rhport, tusb_control_request_t const * request)
+// 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 dfu_rtd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
 {
-  (void) rhport;
-  (void) request;
+  // nothing to do with DATA and ACK stage
+  if ( stage != CONTROL_STAGE_SETUP ) return true;
 
-  // nothing to do
-  return true;
-}
-
-bool dfu_rtd_control_request(uint8_t rhport, tusb_control_request_t const * request)
-{
   TU_VERIFY(request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE);
 
   // dfu-util will try to claim the interface with SET_INTERFACE request before sending DFU request
diff --git a/src/class/dfu/dfu_rt_device.h b/src/class/dfu/dfu_rt_device.h
index 91ead88c6..643e25d8f 100644
--- a/src/class/dfu/dfu_rt_device.h
+++ b/src/class/dfu/dfu_rt_device.h
@@ -66,8 +66,7 @@ TU_ATTR_WEAK void tud_dfu_rt_reboot_to_dfu(void); // TODO rename to _cb conventi
 void     dfu_rtd_init(void);
 void     dfu_rtd_reset(uint8_t rhport);
 uint16_t dfu_rtd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
-bool     dfu_rtd_control_request(uint8_t rhport, tusb_control_request_t const * request);
-bool     dfu_rtd_control_complete(uint8_t rhport, tusb_control_request_t const * request);
+bool     dfu_rtd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
 bool     dfu_rtd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
 
 #ifdef __cplusplus
diff --git a/src/class/hid/hid_device.c b/src/class/hid/hid_device.c
index 11da5f220..1f03f8862 100644
--- a/src/class/hid/hid_device.c
+++ b/src/class/hid/hid_device.c
@@ -211,9 +211,10 @@ uint16_t hidd_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint1
   return drv_len;
 }
 
-// Handle class control request
+// 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 hidd_control_request(uint8_t rhport, tusb_control_request_t const * request)
+bool hidd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
 {
   TU_VERIFY(request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE);
 
@@ -225,27 +226,29 @@ bool hidd_control_request(uint8_t rhport, tusb_control_request_t const * request
   if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD)
   {
     //------------- STD Request -------------//
-    uint8_t const desc_type  = tu_u16_high(request->wValue);
-    uint8_t const desc_index = tu_u16_low (request->wValue);
-    (void) desc_index;
-
-    if (request->bRequest == TUSB_REQ_GET_DESCRIPTOR && desc_type == HID_DESC_TYPE_HID)
-    {
-      TU_VERIFY(p_hid->hid_descriptor != NULL);
-      TU_VERIFY(tud_control_xfer(rhport, request, (void*) p_hid->hid_descriptor, p_hid->hid_descriptor->bLength));
-    }
-    else if (request->bRequest == TUSB_REQ_GET_DESCRIPTOR && desc_type == HID_DESC_TYPE_REPORT)
-    {
-      uint8_t const * desc_report = tud_hid_descriptor_report_cb(
-          #if CFG_TUD_HID > 1
-          hid_itf // TODO for backward compatible callback, remove later when appropriate
-          #endif
-      );
-      tud_control_xfer(rhport, request, (void*) desc_report, p_hid->report_desc_len);
-    }
-    else
+    if ( stage == CONTROL_STAGE_SETUP )
     {
-      return false; // stall unsupported request
+      uint8_t const desc_type  = tu_u16_high(request->wValue);
+      //uint8_t const desc_index = tu_u16_low (request->wValue);
+
+      if (request->bRequest == TUSB_REQ_GET_DESCRIPTOR && desc_type == HID_DESC_TYPE_HID)
+      {
+        TU_VERIFY(p_hid->hid_descriptor != NULL);
+        TU_VERIFY(tud_control_xfer(rhport, request, (void*) p_hid->hid_descriptor, p_hid->hid_descriptor->bLength));
+      }
+      else if (request->bRequest == TUSB_REQ_GET_DESCRIPTOR && desc_type == HID_DESC_TYPE_REPORT)
+      {
+        uint8_t const * desc_report = tud_hid_descriptor_report_cb(
+            #if CFG_TUD_HID > 1
+            hid_itf // TODO for backward compatible callback, remove later when appropriate
+            #endif
+        );
+        tud_control_xfer(rhport, request, (void*) desc_report, p_hid->report_desc_len);
+      }
+      else
+      {
+        return false; // stall unsupported request
+      }
     }
   }
   else if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS)
@@ -254,70 +257,98 @@ bool hidd_control_request(uint8_t rhport, tusb_control_request_t const * request
     switch( request->bRequest )
     {
       case HID_REQ_CONTROL_GET_REPORT:
-      {
-        // wValue = Report Type | Report ID
-        uint8_t const report_type = tu_u16_high(request->wValue);
-        uint8_t const report_id   = tu_u16_low(request->wValue);
+        if ( stage == CONTROL_STAGE_SETUP )
+        {
+          uint8_t const report_type = tu_u16_high(request->wValue);
+          uint8_t const report_id   = tu_u16_low(request->wValue);
 
-        uint16_t xferlen  = tud_hid_get_report_cb(
-            #if CFG_TUD_HID > 1
-            hid_itf, // TODO for backward compatible callback, remove later when appropriate
-            #endif
-            report_id, (hid_report_type_t) report_type, p_hid->epin_buf, request->wLength
-        );
-        TU_ASSERT( xferlen > 0 );
+          uint16_t xferlen  = tud_hid_get_report_cb(
+              #if CFG_TUD_HID > 1
+              hid_itf, // TODO for backward compatible callback, remove later when appropriate
+              #endif
+              report_id, (hid_report_type_t) report_type, p_hid->epin_buf, request->wLength
+          );
+          TU_ASSERT( xferlen > 0 );
 
-        tud_control_xfer(rhport, request, p_hid->epin_buf, xferlen);
-      }
+          tud_control_xfer(rhport, request, p_hid->epin_buf, xferlen);
+        }
       break;
 
       case  HID_REQ_CONTROL_SET_REPORT:
-        TU_VERIFY(request->wLength <= sizeof(p_hid->epout_buf));
-        tud_control_xfer(rhport, request, p_hid->epout_buf, request->wLength);
+        if ( stage == CONTROL_STAGE_SETUP )
+        {
+          TU_VERIFY(request->wLength <= sizeof(p_hid->epout_buf));
+          tud_control_xfer(rhport, request, p_hid->epout_buf, request->wLength);
+        }
+        else if ( stage == CONTROL_STAGE_ACK )
+        {
+          uint8_t const report_type = tu_u16_high(request->wValue);
+          uint8_t const report_id   = tu_u16_low(request->wValue);
+
+          tud_hid_set_report_cb(
+              #if CFG_TUD_HID > 1
+              hid_itf, // TODO for backward compatible callback, remove later when appropriate
+              #endif
+              report_id, (hid_report_type_t) report_type, p_hid->epout_buf, request->wLength
+          );
+        }
       break;
 
       case HID_REQ_CONTROL_SET_IDLE:
-        p_hid->idle_rate = tu_u16_high(request->wValue);
-        if ( tud_hid_set_idle_cb )
+        if ( stage == CONTROL_STAGE_SETUP )
         {
-          // stall request if callback return false
-          TU_VERIFY( tud_hid_set_idle_cb(
-                          #if CFG_TUD_HID > 1
-                          hid_itf, // TODO for backward compatible callback, remove later when appropriate
-                          #endif
-                          p_hid->idle_rate)
-          );
-        }
+          p_hid->idle_rate = tu_u16_high(request->wValue);
+          if ( tud_hid_set_idle_cb )
+          {
+            // stall request if callback return false
+            TU_VERIFY( tud_hid_set_idle_cb(
+                            #if CFG_TUD_HID > 1
+                            hid_itf, // TODO for backward compatible callback, remove later when appropriate
+                            #endif
+                            p_hid->idle_rate)
+            );
+          }
 
-        tud_control_status(rhport, request);
+          tud_control_status(rhport, request);
+        }
       break;
 
       case HID_REQ_CONTROL_GET_IDLE:
-        // TODO idle rate of report
-        tud_control_xfer(rhport, request, &p_hid->idle_rate, 1);
+        if ( stage == CONTROL_STAGE_SETUP )
+        {
+          // TODO idle rate of report
+          tud_control_xfer(rhport, request, &p_hid->idle_rate, 1);
+        }
       break;
 
       case HID_REQ_CONTROL_GET_PROTOCOL:
-      {
-        uint8_t protocol = (uint8_t)(1-p_hid->boot_mode);   // 0 is Boot, 1 is Report protocol
-        tud_control_xfer(rhport, request, &protocol, 1);
-      }
+        if ( stage == CONTROL_STAGE_SETUP )
+        {
+          // 0 is Boot, 1 is Report protocol
+          uint8_t protocol = (uint8_t)(1-p_hid->boot_mode);
+          tud_control_xfer(rhport, request, &protocol, 1);
+        }
       break;
 
       case HID_REQ_CONTROL_SET_PROTOCOL:
-        p_hid->boot_mode = 1 - request->wValue; // 0 is Boot, 1 is Report protocol
-
-        if (tud_hid_boot_mode_cb)
+        if ( stage == CONTROL_STAGE_SETUP )
         {
-          tud_hid_boot_mode_cb(
-              #if CFG_TUD_HID > 1
-              hid_itf, // TODO for backward compatible callback, remove later when appropriate
-              #endif
-              p_hid->boot_mode
-          );
+          // 0 is Boot, 1 is Report protocol
+          p_hid->boot_mode = 1 - request->wValue;
+          tud_control_status(rhport, request);
+        }
+        else if ( stage == CONTROL_STAGE_ACK )
+        {
+          if (tud_hid_boot_mode_cb)
+          {
+            tud_hid_boot_mode_cb(
+                #if CFG_TUD_HID > 1
+                hid_itf, // TODO for backward compatible callback, remove later when appropriate
+                #endif
+                p_hid->boot_mode
+            );
+          }
         }
-
-        tud_control_status(rhport, request);
       break;
 
       default: return false; // stall unsupported request
@@ -330,35 +361,6 @@ bool hidd_control_request(uint8_t rhport, tusb_control_request_t const * request
   return true;
 }
 
-// Invoked when class request DATA stage is finished.
-// return false to stall control endpoint (e.g Host send non-sense DATA)
-bool hidd_control_complete(uint8_t rhport, tusb_control_request_t const * p_request)
-{
-  (void) rhport;
-
-  uint8_t const hid_itf = get_index_by_itfnum((uint8_t) p_request->wIndex);
-  TU_VERIFY(hid_itf < CFG_TUD_HID);
-
-  hidd_interface_t* p_hid = &_hidd_itf[hid_itf];
-
-  if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS &&
-      p_request->bRequest == HID_REQ_CONTROL_SET_REPORT)
-  {
-    // wValue = Report Type | Report ID
-    uint8_t const report_type = tu_u16_high(p_request->wValue);
-    uint8_t const report_id   = tu_u16_low(p_request->wValue);
-
-    tud_hid_set_report_cb(
-        #if CFG_TUD_HID > 1
-        hid_itf, // TODO for backward compatible callback, remove later when appropriate
-        #endif
-        report_id, (hid_report_type_t) report_type, p_hid->epout_buf, p_request->wLength
-    );
-  }
-
-  return true;
-}
-
 bool hidd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
 {
   (void) result;
diff --git a/src/class/hid/hid_device.h b/src/class/hid/hid_device.h
index d5b5b7296..456b011ba 100644
--- a/src/class/hid/hid_device.h
+++ b/src/class/hid/hid_device.h
@@ -46,7 +46,7 @@
 #endif
 
 #ifndef CFG_TUD_HID_EP_BUFSIZE
-  #define CFG_TUD_HID_EP_BUFSIZE     16
+  #define CFG_TUD_HID_EP_BUFSIZE     64
 #endif
 
 //--------------------------------------------------------------------+
@@ -359,12 +359,11 @@ static inline bool tud_hid_mouse_report(uint8_t report_id, uint8_t buttons, int8
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-void     hidd_init             (void);
-void     hidd_reset            (uint8_t rhport);
-uint16_t hidd_open             (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
-bool     hidd_control_request  (uint8_t rhport, tusb_control_request_t const * request);
-bool     hidd_control_complete (uint8_t rhport, tusb_control_request_t const * request);
-bool     hidd_xfer_cb          (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
+void     hidd_init            (void);
+void     hidd_reset           (uint8_t rhport);
+uint16_t hidd_open            (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
+bool     hidd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
+bool     hidd_xfer_cb         (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
 
 #ifdef __cplusplus
  }
diff --git a/src/class/midi/midi_device.c b/src/class/midi/midi_device.c
index a07acf0b8..ce4c9cafe 100644
--- a/src/class/midi/midi_device.c
+++ b/src/class/midi/midi_device.c
@@ -375,17 +375,14 @@ uint16_t midid_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint
   return drv_len;
 }
 
-bool midid_control_complete(uint8_t rhport, tusb_control_request_t const * p_request)
+// 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 midid_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
 {
   (void) rhport;
-  (void) p_request;
-  return true;
-}
-
-bool midid_control_request(uint8_t rhport, tusb_control_request_t const * p_request)
-{
-  (void) rhport;
-  (void) p_request;
+  (void) stage;
+  (void) request;
 
   // driver doesn't support any request yet
   return false;
diff --git a/src/class/midi/midi_device.h b/src/class/midi/midi_device.h
index b8fb55cc2..9235448f2 100644
--- a/src/class/midi/midi_device.h
+++ b/src/class/midi/midi_device.h
@@ -142,12 +142,11 @@ static inline bool tud_midi_send (uint8_t const packet[4])
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-void     midid_init             (void);
-void     midid_reset            (uint8_t rhport);
-uint16_t midid_open             (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
-bool     midid_control_request  (uint8_t rhport, tusb_control_request_t const * request);
-bool     midid_control_complete (uint8_t rhport, tusb_control_request_t const * request);
-bool     midid_xfer_cb          (uint8_t rhport, uint8_t edpt_addr, xfer_result_t result, uint32_t xferred_bytes);
+void     midid_init            (void);
+void     midid_reset           (uint8_t rhport);
+uint16_t midid_open            (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
+bool     midid_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
+bool     midid_xfer_cb         (uint8_t rhport, uint8_t edpt_addr, xfer_result_t result, uint32_t xferred_bytes);
 
 #ifdef __cplusplus
  }
diff --git a/src/class/msc/msc.h b/src/class/msc/msc.h
index cba8066b7..0bdc00692 100644
--- a/src/class/msc/msc.h
+++ b/src/class/msc/msc.h
@@ -255,7 +255,7 @@ typedef struct TU_ATTR_PACKED
 
   uint8_t : 3;
   uint8_t disable_block_descriptor : 1;
-  uint8_t : 0;
+  uint8_t : 4;
 
   uint8_t page_code : 6;
   uint8_t page_control : 2;
diff --git a/src/class/msc/msc_device.c b/src/class/msc/msc_device.c
index f3f2e536e..194f4d3cb 100644
--- a/src/class/msc/msc_device.c
+++ b/src/class/msc/msc_device.c
@@ -71,6 +71,7 @@ CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t _mscd_buf[CFG_TUD_MSC_EP_
 //--------------------------------------------------------------------+
 // 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);
 
@@ -186,10 +187,14 @@ uint16_t mscd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint1
   return drv_len;
 }
 
-// Handle class control request
+// 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_request(uint8_t rhport, tusb_control_request_t const * p_request)
+bool mscd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * p_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);
 
@@ -219,190 +224,6 @@ bool mscd_control_request(uint8_t rhport, tusb_control_request_t const * p_reque
   return true;
 }
 
-// Invoked when class request DATA stage is finished.
-// return false to stall control endpoint (e.g Host send non-sense DATA)
-bool mscd_control_complete(uint8_t rhport, tusb_control_request_t const * request)
-{
-  (void) rhport;
-  (void) request;
-
-  // nothing to do
-  return true;
-}
-
-// return response's length (copied to buffer). Negative if it is not an built-in command or indicate Failed status (CSW)
-// In case of a failed status, sense key must be set for reason of failure
-int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_t* buffer, uint32_t bufsize)
-{
-  (void) bufsize; // TODO refractor later
-  int32_t resplen;
-
-  switch ( scsi_cmd[0] )
-  {
-    case SCSI_CMD_TEST_UNIT_READY:
-      resplen = 0;
-      if ( !tud_msc_test_unit_ready_cb(lun) )
-      {
-        // 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);
-      }
-    break;
-
-    case SCSI_CMD_START_STOP_UNIT:
-      resplen = 0;
-
-      if (tud_msc_start_stop_cb)
-      {
-        scsi_start_stop_unit_t const * start_stop = (scsi_start_stop_unit_t const *) scsi_cmd;
-        if ( !tud_msc_start_stop_cb(lun, start_stop->power_condition, start_stop->start, start_stop->load_eject) )
-        {
-          // 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);
-        }
-      }
-    break;
-
-    case SCSI_CMD_READ_CAPACITY_10:
-    {
-      uint32_t block_count;
-      uint32_t block_size;
-      uint16_t block_size_u16;
-
-      tud_msc_capacity_cb(lun, &block_count, &block_size_u16);
-      block_size = (uint32_t) block_size_u16;
-
-      // Invalid block size/count from callback, possibly unit is not ready
-      // stall this request, set sense key to NOT READY
-      if (block_count == 0 || block_size == 0)
-      {
-        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);
-      }else
-      {
-        scsi_read_capacity10_resp_t read_capa10;
-
-        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);
-      }
-    }
-    break;
-
-    case SCSI_CMD_READ_FORMAT_CAPACITY:
-    {
-      scsi_read_format_capacity_data_t read_fmt_capa =
-      {
-          .list_length     = 8,
-          .block_num       = 0,
-          .descriptor_type = 2, // formatted media
-          .block_size_u16  = 0
-      };
-
-      uint32_t block_count;
-      uint16_t block_size;
-
-      tud_msc_capacity_cb(lun, &block_count, &block_size);
-
-      // Invalid block size/count from callback, possibly unit is not ready
-      // stall this request, set sense key to NOT READY
-      if (block_count == 0 || block_size == 0)
-      {
-        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);
-      }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);
-      }
-    }
-    break;
-
-    case SCSI_CMD_INQUIRY:
-    {
-      scsi_inquiry_resp_t inquiry_rsp =
-      {
-          .is_removable         = 1,
-          .version              = 2,
-          .response_data_format = 2,
-      };
-
-      // vendor_id, product_id, product_rev is space padded string
-      memset(inquiry_rsp.vendor_id  , ' ', sizeof(inquiry_rsp.vendor_id));
-      memset(inquiry_rsp.product_id , ' ', sizeof(inquiry_rsp.product_id));
-      memset(inquiry_rsp.product_rev, ' ', sizeof(inquiry_rsp.product_rev));
-
-      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);
-    }
-    break;
-
-    case SCSI_CMD_MODE_SENSE_6:
-    {
-      scsi_mode_sense6_resp_t mode_resp =
-      {
-          .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);
-      }
-      mode_resp.write_protected = !writable;
-
-      resplen = sizeof(mode_resp);
-      memcpy(buffer, &mode_resp, resplen);
-    }
-    break;
-
-    case SCSI_CMD_REQUEST_SENSE:
-    {
-      scsi_sense_fixed_resp_t sense_rsp =
-      {
-          .response_code = 0x70,
-          .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;
-
-      resplen = sizeof(sense_rsp);
-      memcpy(buffer, &sense_rsp, resplen);
-
-      // Clear sense data after copy
-      tud_msc_set_sense(lun, 0, 0, 0);
-    }
-    break;
-
-    default: resplen = -1; break;
-  }
-
-  return resplen;
-}
-
 bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
 {
   mscd_interface_t* p_msc = &_mscd_itf;
@@ -592,6 +413,24 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
         TU_LOG2("  SCSI Status: %u\r\n", p_csw->status);
         // TU_LOG2_MEM(p_csw, xferred_bytes, 2);
 
+        // Invoke complete callback if defined
+        // Note: There is racing issue with samd51 + qspi flash testing with arduino
+        // if complete_cb() is invoked after queuing the status.
+        switch(p_cbw->command[0])
+        {
+          case SCSI_CMD_READ_10:
+            if ( tud_msc_read10_complete_cb ) tud_msc_read10_complete_cb(p_cbw->lun);
+          break;
+
+          case SCSI_CMD_WRITE_10:
+            if ( tud_msc_write10_complete_cb ) tud_msc_write10_complete_cb(p_cbw->lun);
+          break;
+
+          default:
+            if ( tud_msc_scsi_complete_cb ) tud_msc_scsi_complete_cb(p_cbw->lun, p_cbw->command);
+          break;
+        }
+
         // Move to default CMD stage
         p_msc->stage = MSC_STAGE_CMD;
 
@@ -615,24 +454,6 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
     }
     else
     {
-      // Invoke complete callback if defined
-      // Note: There is racing issue with samd51 + qspi flash testing with arduino
-      // if complete_cb() is invoked after queuing the status.
-      switch(p_cbw->command[0])
-      {
-        case SCSI_CMD_READ_10:
-          if ( tud_msc_read10_complete_cb ) tud_msc_read10_complete_cb(p_cbw->lun);
-        break;
-
-        case SCSI_CMD_WRITE_10:
-          if ( tud_msc_write10_complete_cb ) tud_msc_write10_complete_cb(p_cbw->lun);
-        break;
-
-        default:
-          if ( tud_msc_scsi_complete_cb ) tud_msc_scsi_complete_cb(p_cbw->lun, p_cbw->command);
-        break;
-      }
-
       // Move to Status Sent stage
       p_msc->stage = MSC_STAGE_STATUS_SENT;
 
@@ -647,6 +468,180 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
 /*------------------------------------------------------------------*/
 /* SCSI Command Process
  *------------------------------------------------------------------*/
+
+// return response's length (copied to buffer). Negative if it is not an built-in command or indicate Failed status (CSW)
+// In case of a failed status, sense key must be set for reason of failure
+static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_t* buffer, uint32_t bufsize)
+{
+  (void) bufsize; // TODO refractor later
+  int32_t resplen;
+
+  switch ( scsi_cmd[0] )
+  {
+    case SCSI_CMD_TEST_UNIT_READY:
+      resplen = 0;
+      if ( !tud_msc_test_unit_ready_cb(lun) )
+      {
+        // 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);
+      }
+    break;
+
+    case SCSI_CMD_START_STOP_UNIT:
+      resplen = 0;
+
+      if (tud_msc_start_stop_cb)
+      {
+        scsi_start_stop_unit_t const * start_stop = (scsi_start_stop_unit_t const *) scsi_cmd;
+        if ( !tud_msc_start_stop_cb(lun, start_stop->power_condition, start_stop->start, start_stop->load_eject) )
+        {
+          // 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);
+        }
+      }
+    break;
+
+    case SCSI_CMD_READ_CAPACITY_10:
+    {
+      uint32_t block_count;
+      uint32_t block_size;
+      uint16_t block_size_u16;
+
+      tud_msc_capacity_cb(lun, &block_count, &block_size_u16);
+      block_size = (uint32_t) block_size_u16;
+
+      // Invalid block size/count from callback, possibly unit is not ready
+      // stall this request, set sense key to NOT READY
+      if (block_count == 0 || block_size == 0)
+      {
+        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);
+      }else
+      {
+        scsi_read_capacity10_resp_t read_capa10;
+
+        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);
+      }
+    }
+    break;
+
+    case SCSI_CMD_READ_FORMAT_CAPACITY:
+    {
+      scsi_read_format_capacity_data_t read_fmt_capa =
+      {
+          .list_length     = 8,
+          .block_num       = 0,
+          .descriptor_type = 2, // formatted media
+          .block_size_u16  = 0
+      };
+
+      uint32_t block_count;
+      uint16_t block_size;
+
+      tud_msc_capacity_cb(lun, &block_count, &block_size);
+
+      // Invalid block size/count from callback, possibly unit is not ready
+      // stall this request, set sense key to NOT READY
+      if (block_count == 0 || block_size == 0)
+      {
+        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);
+      }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);
+      }
+    }
+    break;
+
+    case SCSI_CMD_INQUIRY:
+    {
+      scsi_inquiry_resp_t inquiry_rsp =
+      {
+          .is_removable         = 1,
+          .version              = 2,
+          .response_data_format = 2,
+      };
+
+      // vendor_id, product_id, product_rev is space padded string
+      memset(inquiry_rsp.vendor_id  , ' ', sizeof(inquiry_rsp.vendor_id));
+      memset(inquiry_rsp.product_id , ' ', sizeof(inquiry_rsp.product_id));
+      memset(inquiry_rsp.product_rev, ' ', sizeof(inquiry_rsp.product_rev));
+
+      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);
+    }
+    break;
+
+    case SCSI_CMD_MODE_SENSE_6:
+    {
+      scsi_mode_sense6_resp_t mode_resp =
+      {
+          .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);
+      }
+      mode_resp.write_protected = !writable;
+
+      resplen = sizeof(mode_resp);
+      memcpy(buffer, &mode_resp, resplen);
+    }
+    break;
+
+    case SCSI_CMD_REQUEST_SENSE:
+    {
+      scsi_sense_fixed_resp_t sense_rsp =
+      {
+          .response_code = 0x70,
+          .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;
+
+      resplen = sizeof(sense_rsp);
+      memcpy(buffer, &sense_rsp, resplen);
+
+      // Clear sense data after copy
+      tud_msc_set_sense(lun, 0, 0, 0);
+    }
+    break;
+
+    default: resplen = -1; break;
+  }
+
+  return resplen;
+}
+
 static void proc_read10_cmd(uint8_t rhport, mscd_interface_t* p_msc)
 {
   msc_cbw_t const * p_cbw = &p_msc->cbw;
diff --git a/src/class/msc/msc_device.h b/src/class/msc/msc_device.h
index 3aa93ffd7..469f2f2f7 100644
--- a/src/class/msc/msc_device.h
+++ b/src/class/msc/msc_device.h
@@ -158,12 +158,11 @@ TU_ATTR_WEAK bool tud_msc_is_writable_cb(uint8_t lun);
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-void     mscd_init             (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_request  (uint8_t rhport, tusb_control_request_t const * p_request);
-bool     mscd_control_complete (uint8_t rhport, tusb_control_request_t const * p_request);
-bool     mscd_xfer_cb          (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
+void     mscd_init            (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);
+bool     mscd_xfer_cb         (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
 
 #ifdef __cplusplus
  }
diff --git a/src/class/net/net_device.c b/src/class/net/net_device.c
index 3a45a9b99..ce1131223 100644
--- a/src/class/net/net_device.c
+++ b/src/class/net/net_device.c
@@ -220,26 +220,6 @@ uint16_t netd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint1
   return drv_len;
 }
 
-// Invoked when class request DATA stage is finished.
-// return false to stall control endpoint (e.g Host send nonsense DATA)
-bool netd_control_complete(uint8_t rhport, tusb_control_request_t const * request)
-{
-  (void) rhport;
-
-  // Handle RNDIS class control OUT only
-  if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS &&
-      request->bmRequestType_bit.direction == TUSB_DIR_OUT   &&
-      _netd_itf.itf_num == request->wIndex)
-  {
-    if ( !_netd_itf.ecm_mode )
-    {
-      rndis_class_set_handler(notify.rndis_buf, request->wLength);
-    }
-  }
-
-  return true;
-}
-
 static void ecm_report(bool nc)
 {
   notify.ecm_buf = (nc) ? ecm_notify_nc : ecm_notify_csc;
@@ -247,99 +227,116 @@ static void ecm_report(bool nc)
   netd_report((uint8_t *)&notify.ecm_buf, (nc) ? sizeof(notify.ecm_buf.header) : sizeof(notify.ecm_buf));
 }
 
-// Handle class control request
+// 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 netd_control_request(uint8_t rhport, tusb_control_request_t const * request)
+bool netd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
 {
-  switch ( request->bmRequestType_bit.type )
+  if ( stage == CONTROL_STAGE_SETUP )
   {
-    case TUSB_REQ_TYPE_STANDARD:
-      switch ( request->bRequest )
-      {
-        case TUSB_REQ_GET_INTERFACE:
+    switch ( request->bmRequestType_bit.type )
+    {
+      case TUSB_REQ_TYPE_STANDARD:
+        switch ( request->bRequest )
         {
-          uint8_t const req_itfnum = (uint8_t) request->wIndex;
-          TU_VERIFY(_netd_itf.itf_num+1 == req_itfnum);
+          case TUSB_REQ_GET_INTERFACE:
+          {
+            uint8_t const req_itfnum = (uint8_t) request->wIndex;
+            TU_VERIFY(_netd_itf.itf_num+1 == req_itfnum);
 
-          tud_control_xfer(rhport, request, &_netd_itf.itf_data_alt, 1);
-        }
-        break;
+            tud_control_xfer(rhport, request, &_netd_itf.itf_data_alt, 1);
+          }
+          break;
 
-        case TUSB_REQ_SET_INTERFACE:
-        {
-          uint8_t const req_itfnum = (uint8_t) request->wIndex;
-          uint8_t const req_alt    = (uint8_t) request->wValue;
+          case TUSB_REQ_SET_INTERFACE:
+          {
+            uint8_t const req_itfnum = (uint8_t) request->wIndex;
+            uint8_t const req_alt    = (uint8_t) request->wValue;
 
-          // Only valid for Data Interface with Alternate is either 0 or 1
-          TU_VERIFY(_netd_itf.itf_num+1 == req_itfnum && req_alt < 2);
+            // Only valid for Data Interface with Alternate is either 0 or 1
+            TU_VERIFY(_netd_itf.itf_num+1 == req_itfnum && req_alt < 2);
 
-          // ACM-ECM only: qequest to enable/disable network activities
-          TU_VERIFY(_netd_itf.ecm_mode);
+            // ACM-ECM only: qequest to enable/disable network activities
+            TU_VERIFY(_netd_itf.ecm_mode);
 
-          _netd_itf.itf_data_alt = req_alt;
+            _netd_itf.itf_data_alt = req_alt;
 
-          if ( _netd_itf.itf_data_alt )
-          {
-            // TODO since we don't actually close endpoint
-            // hack here to not re-open it
-            if ( _netd_itf.ep_in == 0 && _netd_itf.ep_out == 0 )
+            if ( _netd_itf.itf_data_alt )
             {
-              TU_ASSERT(_netd_itf.ecm_desc_epdata);
-              TU_ASSERT( usbd_open_edpt_pair(rhport, _netd_itf.ecm_desc_epdata, 2, TUSB_XFER_BULK, &_netd_itf.ep_out, &_netd_itf.ep_in) );
+              // TODO since we don't actually close endpoint
+              // hack here to not re-open it
+              if ( _netd_itf.ep_in == 0 && _netd_itf.ep_out == 0 )
+              {
+                TU_ASSERT(_netd_itf.ecm_desc_epdata);
+                TU_ASSERT( usbd_open_edpt_pair(rhport, _netd_itf.ecm_desc_epdata, 2, TUSB_XFER_BULK, &_netd_itf.ep_out, &_netd_itf.ep_in) );
 
-              // TODO should be merge with RNDIS's after endpoint opened
-              // Also should have opposite callback for application to disable network !!
-              tud_network_init_cb();
-              can_xmit = true; // we are ready to transmit a packet
-              tud_network_recv_renew(); // prepare for incoming packets
+                // TODO should be merge with RNDIS's after endpoint opened
+                // Also should have opposite callback for application to disable network !!
+                tud_network_init_cb();
+                can_xmit = true; // we are ready to transmit a packet
+                tud_network_recv_renew(); // prepare for incoming packets
+              }
+            }else
+            {
+              // TODO close the endpoint pair
+              // For now pretend that we did, this should have no harm since host won't try to
+              // communicate with the endpoints again
+              // _netd_itf.ep_in = _netd_itf.ep_out = 0
             }
-          }else
-          {
-            // TODO close the endpoint pair
-            // For now pretend that we did, this should have no harm since host won't try to
-            // communicate with the endpoints again
-            // _netd_itf.ep_in = _netd_itf.ep_out = 0
+
+            tud_control_status(rhport, request);
           }
+          break;
 
-          tud_control_status(rhport, request);
+          // unsupported request
+          default: return false;
         }
-        break;
+      break;
 
-        // unsupported request
-        default: return false;
-      }
-    break;
-
-    case TUSB_REQ_TYPE_CLASS:
-      TU_VERIFY (_netd_itf.itf_num == request->wIndex);
+      case TUSB_REQ_TYPE_CLASS:
+        TU_VERIFY (_netd_itf.itf_num == request->wIndex);
 
-      if (_netd_itf.ecm_mode)
-      {
-        /* the only required CDC-ECM Management Element Request is SetEthernetPacketFilter */
-        if (0x43 /* SET_ETHERNET_PACKET_FILTER */ == request->bRequest)
+        if (_netd_itf.ecm_mode)
         {
-          tud_control_xfer(rhport, request, NULL, 0);
-          ecm_report(true);
-        }
-      }
-      else
-      {
-        if (request->bmRequestType_bit.direction == TUSB_DIR_IN)
-        {
-          rndis_generic_msg_t *rndis_msg = (rndis_generic_msg_t *) ((void*) notify.rndis_buf);
-          uint32_t msglen = tu_le32toh(rndis_msg->MessageLength);
-          TU_ASSERT(msglen <= sizeof(notify.rndis_buf));
-          tud_control_xfer(rhport, request, notify.rndis_buf, msglen);
+          /* the only required CDC-ECM Management Element Request is SetEthernetPacketFilter */
+          if (0x43 /* SET_ETHERNET_PACKET_FILTER */ == request->bRequest)
+          {
+            tud_control_xfer(rhport, request, NULL, 0);
+            ecm_report(true);
+          }
         }
         else
         {
-          tud_control_xfer(rhport, request, notify.rndis_buf, sizeof(notify.rndis_buf));
+          if (request->bmRequestType_bit.direction == TUSB_DIR_IN)
+          {
+            rndis_generic_msg_t *rndis_msg = (rndis_generic_msg_t *) ((void*) notify.rndis_buf);
+            uint32_t msglen = tu_le32toh(rndis_msg->MessageLength);
+            TU_ASSERT(msglen <= sizeof(notify.rndis_buf));
+            tud_control_xfer(rhport, request, notify.rndis_buf, msglen);
+          }
+          else
+          {
+            tud_control_xfer(rhport, request, notify.rndis_buf, sizeof(notify.rndis_buf));
+          }
         }
-      }
-    break;
+      break;
 
-    // unsupported request
-    default: return false;
+      // unsupported request
+      default: return false;
+    }
+  }
+  else if ( stage == CONTROL_STAGE_DATA )
+  {
+    // Handle RNDIS class control OUT only
+    if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS &&
+        request->bmRequestType_bit.direction == TUSB_DIR_OUT   &&
+        _netd_itf.itf_num == request->wIndex)
+    {
+      if ( !_netd_itf.ecm_mode )
+      {
+        rndis_class_set_handler(notify.rndis_buf, request->wLength);
+      }
+    }
   }
 
   return true;
diff --git a/src/class/net/net_device.h b/src/class/net/net_device.h
index 795b2f9e3..38c47d647 100644
--- a/src/class/net/net_device.h
+++ b/src/class/net/net_device.h
@@ -73,13 +73,12 @@ void tud_network_xmit(void *ref, uint16_t arg);
 //--------------------------------------------------------------------+
 // INTERNAL USBD-CLASS DRIVER API
 //--------------------------------------------------------------------+
-void     netd_init             (void);
-void     netd_reset            (uint8_t rhport);
-uint16_t netd_open             (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
-bool     netd_control_request  (uint8_t rhport, tusb_control_request_t const * request);
-bool     netd_control_complete (uint8_t rhport, tusb_control_request_t const * request);
-bool     netd_xfer_cb          (uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
-void     netd_report           (uint8_t *buf, uint16_t len);
+void     netd_init            (void);
+void     netd_reset           (uint8_t rhport);
+uint16_t netd_open            (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
+bool     netd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
+bool     netd_xfer_cb         (uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
+void     netd_report          (uint8_t *buf, uint16_t len);
 
 #ifdef __cplusplus
  }
diff --git a/src/class/usbtmc/usbtmc_device.c b/src/class/usbtmc/usbtmc_device.c
index bc5f23f42..88776d169 100644
--- a/src/class/usbtmc/usbtmc_device.c
+++ b/src/class/usbtmc/usbtmc_device.c
@@ -575,7 +575,13 @@ bool usbtmcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint
   return false;
 }
 
-bool usbtmcd_control_request_cb(uint8_t rhport, tusb_control_request_t const * request) {
+// 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 usbtmcd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
+{
+  // nothing to do with DATA and ACK stage
+  if ( stage != CONTROL_STAGE_SETUP ) return true;
 
   uint8_t tmcStatusCode = USBTMC_STATUS_FAILED;
 #if (CFG_TUD_USBTMC_ENABLE_488)
@@ -855,13 +861,4 @@ bool usbtmcd_control_request_cb(uint8_t rhport, tusb_control_request_t const * r
   TU_VERIFY(false);
 }
 
-bool usbtmcd_control_complete_cb(uint8_t rhport, tusb_control_request_t const * request)
-{
-  (void)rhport;
-  //------------- Class Specific Request -------------//
-  TU_ASSERT (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
-
-  return true;
-}
-
 #endif /* CFG_TUD_TSMC */
diff --git a/src/class/usbtmc/usbtmc_device.h b/src/class/usbtmc/usbtmc_device.h
index a6b5e4cca..622800315 100644
--- a/src/class/usbtmc/usbtmc_device.h
+++ b/src/class/usbtmc/usbtmc_device.h
@@ -111,8 +111,7 @@ bool tud_usbtmc_start_bus_read(void);
 uint16_t usbtmcd_open_cb(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 void     usbtmcd_reset_cb(uint8_t rhport);
 bool     usbtmcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
-bool     usbtmcd_control_request_cb(uint8_t rhport, tusb_control_request_t const * request);
-bool     usbtmcd_control_complete_cb(uint8_t rhport, tusb_control_request_t const * request);
+bool     usbtmcd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
 void     usbtmcd_init_cb(void);
 
 /************************************************************