Merge branch 'master' into cdc_without_dtr

24 files changed, 715 insertions, 713 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 2d0aed238..bd1d9b0d4 100644
--- a/src/class/cdc/cdc_device.c
+++ b/src/class/cdc/cdc_device.c
@@ -322,38 +322,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);
@@ -372,41 +344,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);
-
-      // TODO if terminal supports DTR we can check for an connection event here and
-      // clear the fifo as well as ongoing transfers with new usbd_edpt_xfer_abort api.
-      // Until then user can self clear the buffer with tud_cdc_n_write_clear in tud_cdc_line_state_cb
-
-      p_cdc->line_state = (uint8_t) request->wValue;
-
-      // Disable fifo overwriting if DTR bit is set
-      tu_fifo_set_mode(&p_cdc->tx_ff, !dtr);
+      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_mode(&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 342025961..62dcd3c0a 100644
--- a/src/class/cdc/cdc_device.h
+++ b/src/class/cdc/cdc_device.h
@@ -245,12 +245,11 @@ static inline bool tud_cdc_write_clear(void)
 //--------------------------------------------------------------------+
 // 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/cdc/cdc_host.c b/src/class/cdc/cdc_host.c
index e62f47b72..a897fb58a 100644
--- a/src/class/cdc/cdc_host.c
+++ b/src/class/cdc/cdc_host.c
@@ -51,9 +51,14 @@ typedef struct {
 //--------------------------------------------------------------------+
 static cdch_data_t cdch_data[CFG_TUSB_HOST_DEVICE_MAX];
 
+static inline cdch_data_t* get_itf(uint8_t dev_addr)
+{
+  return &cdch_data[dev_addr-1];
+}
+
 bool tuh_cdc_mounted(uint8_t dev_addr)
 {
-  cdch_data_t* cdc = &cdch_data[dev_addr-1];
+  cdch_data_t* cdc = get_itf(dev_addr);
   return cdc->ep_in && cdc->ep_out;
 }
 
@@ -61,7 +66,7 @@ bool tuh_cdc_is_busy(uint8_t dev_addr, cdc_pipeid_t pipeid)
 {
   if ( !tuh_cdc_mounted(dev_addr) ) return false;
 
-  cdch_data_t const * p_cdc = &cdch_data[dev_addr-1];
+  cdch_data_t const * p_cdc = get_itf(dev_addr);
 
   switch (pipeid)
   {
@@ -111,6 +116,27 @@ bool tuh_cdc_receive(uint8_t dev_addr, void * p_buffer, uint32_t length, bool is
   return hcd_pipe_xfer(dev_addr, ep_in, p_buffer, length, is_notify);
 }
 
+bool tuh_cdc_set_control_line_state(uint8_t dev_addr, bool dtr, bool rts, tuh_control_complete_cb_t complete_cb)
+{
+  cdch_data_t const * p_cdc = get_itf(dev_addr);
+  tusb_control_request_t const request =
+  {
+    .bmRequestType_bit =
+    {
+      .recipient = TUSB_REQ_RCPT_INTERFACE,
+      .type      = TUSB_REQ_TYPE_CLASS,
+      .direction = TUSB_DIR_OUT
+    },
+    .bRequest = CDC_REQUEST_SET_CONTROL_LINE_STATE,
+    .wValue   = (rts ? 2 : 0) | (dtr ? 1 : 0),
+    .wIndex   = p_cdc->itf_num,
+    .wLength  = 0
+  };
+
+  TU_ASSERT( tuh_control_xfer(dev_addr, &request, NULL, complete_cb) );
+  return true;
+}
+
 //--------------------------------------------------------------------+
 // USBH-CLASS DRIVER API
 //--------------------------------------------------------------------+
@@ -132,7 +158,7 @@ bool cdch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *it
   cdch_data_t * p_cdc;
 
   p_desc = tu_desc_next(itf_desc);
-  p_cdc  = &cdch_data[dev_addr-1];
+  p_cdc  = get_itf(dev_addr);
 
   p_cdc->itf_num   = itf_desc->bInterfaceNumber;
   p_cdc->itf_protocol = itf_desc->bInterfaceProtocol; // TODO 0xff is consider as rndis candidate, other is virtual Com
@@ -194,30 +220,25 @@ bool cdch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *it
     }
   }
 
-  // FIXME move to seperate API : connect
-  tusb_control_request_t request =
-  {
-    .bmRequestType_bit = { .recipient = TUSB_REQ_RCPT_INTERFACE, .type = TUSB_REQ_TYPE_CLASS, .direction = TUSB_DIR_OUT },
-    .bRequest = CDC_REQUEST_SET_CONTROL_LINE_STATE,
-    .wValue = 0x03, // dtr on, cst on
-    .wIndex = p_cdc->itf_num,
-    .wLength = 0
-  };
-
-  TU_ASSERT( usbh_control_xfer(dev_addr, &request, NULL) );
+  return true;
+}
 
+bool cdch_set_config(uint8_t dev_addr, uint8_t itf_num)
+{
+  (void) dev_addr; (void) itf_num;
   return true;
 }
 
-void cdch_isr(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
+bool cdch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
 {
   (void) ep_addr;
   tuh_cdc_xfer_isr( dev_addr, event, 0, xferred_bytes );
+  return true;
 }
 
 void cdch_close(uint8_t dev_addr)
 {
-  cdch_data_t * p_cdc = &cdch_data[dev_addr-1];
+  cdch_data_t * p_cdc = get_itf(dev_addr);
   tu_memclr(p_cdc, sizeof(cdch_data_t));
 }
 
diff --git a/src/class/cdc/cdc_host.h b/src/class/cdc/cdc_host.h
index 306420ce4..66c2f0727 100644
--- a/src/class/cdc/cdc_host.h
+++ b/src/class/cdc/cdc_host.h
@@ -44,6 +44,18 @@
  * \defgroup   CDC_Serial_Host Host
  * @{ */
 
+bool tuh_cdc_set_control_line_state(uint8_t dev_addr, bool dtr, bool rts, tuh_control_complete_cb_t complete_cb);
+
+static inline bool tuh_cdc_connect(uint8_t dev_addr, tuh_control_complete_cb_t complete_cb)
+{
+  return tuh_cdc_set_control_line_state(dev_addr, true, true, complete_cb);
+}
+
+static inline bool tuh_cdc_disconnect(uint8_t dev_addr, tuh_control_complete_cb_t complete_cb)
+{
+  return tuh_cdc_set_control_line_state(dev_addr, false, false, complete_cb);
+}
+
 /** \brief 			Check if device support CDC Serial interface or not
  * \param[in]		dev_addr	device address
  * \retval      true if device supports
@@ -113,7 +125,8 @@ void tuh_cdc_xfer_isr(uint8_t dev_addr, xfer_result_t event, cdc_pipeid_t pipe_i
 //--------------------------------------------------------------------+
 void cdch_init(void);
 bool cdch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t *p_length);
-void cdch_isr(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
+bool cdch_set_config(uint8_t dev_addr, uint8_t itf_num);
+bool cdch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
 void cdch_close(uint8_t dev_addr);
 
 #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..2d4c59d8f 100644
--- a/src/class/hid/hid_device.h
+++ b/src/class/hid/hid_device.h
@@ -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/hid/hid_host.c b/src/class/hid/hid_host.c
index 378278cc3..a96eb6276 100644
--- a/src/class/hid/hid_host.c
+++ b/src/class/hid/hid_host.c
@@ -71,7 +71,7 @@ tusb_error_t hidh_interface_get_report(uint8_t dev_addr, void * report, hidh_int
   TU_VERIFY(report, TUSB_ERROR_INVALID_PARA);
   TU_ASSERT(!hcd_edpt_busy(dev_addr, p_hid->ep_in), TUSB_ERROR_INTERFACE_IS_BUSY);
 
-  TU_ASSERT( hcd_pipe_xfer(dev_addr, p_hid->ep_in, report, p_hid->report_size, true) ) ;
+  TU_ASSERT( usbh_edpt_xfer(dev_addr, p_hid->ep_in, report, p_hid->report_size) ) ;
 
   return TUSB_ERROR_NONE;
 }
@@ -173,30 +173,6 @@ bool hidh_open_subtask(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t c
   tusb_desc_endpoint_t const * p_endpoint_desc = (tusb_desc_endpoint_t const *) p_desc;
   TU_ASSERT(TUSB_DESC_ENDPOINT == p_endpoint_desc->bDescriptorType, TUSB_ERROR_INVALID_PARA);
 
-  //------------- SET IDLE (0) request -------------//
-  tusb_control_request_t request = {
-        .bmRequestType_bit = { .recipient = TUSB_REQ_RCPT_INTERFACE, .type = TUSB_REQ_TYPE_CLASS, .direction = TUSB_DIR_OUT },
-        .bRequest = HID_REQ_CONTROL_SET_IDLE,
-        .wValue = 0, // idle_rate = 0
-        .wIndex = p_interface_desc->bInterfaceNumber,
-        .wLength = 0
-  };
-  TU_ASSERT( usbh_control_xfer( dev_addr, &request, NULL ) );
-
-#if 0
-  //------------- Get Report Descriptor TODO HID parser -------------//
-  if ( p_desc_hid->bNumDescriptors )
-  {
-    STASK_INVOKE(
-        usbh_control_xfer_subtask( dev_addr, bm_request_type(TUSB_DIR_IN, TUSB_REQ_TYPE_STANDARD, TUSB_REQ_RCPT_INTERFACE),
-                                   TUSB_REQ_GET_DESCRIPTOR, (p_desc_hid->bReportType << 8), 0,
-                                   p_desc_hid->wReportLength, report_descriptor ),
-        error
-    );
-    (void) error; // if error in getting report descriptor --> treating like there is none
-  }
-#endif
-
   if ( HID_SUBCLASS_BOOT == p_interface_desc->bInterfaceSubClass )
   {
     #if CFG_TUH_HID_KEYBOARD
@@ -204,7 +180,6 @@ bool hidh_open_subtask(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t c
     {
       TU_ASSERT( hidh_interface_open(rhport, dev_addr, p_interface_desc->bInterfaceNumber, p_endpoint_desc, &keyboardh_data[dev_addr-1]) );
       TU_LOG2_HEX(keyboardh_data[dev_addr-1].ep_in);
-      tuh_hid_keyboard_mounted_cb(dev_addr);
     } else
     #endif
 
@@ -213,7 +188,6 @@ bool hidh_open_subtask(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t c
     {
       TU_ASSERT ( hidh_interface_open(rhport, dev_addr, p_interface_desc->bInterfaceNumber, p_endpoint_desc, &mouseh_data[dev_addr-1]) );
       TU_LOG2_HEX(mouseh_data[dev_addr-1].ep_in);
-      tuh_hid_mouse_mounted_cb(dev_addr);
     } else
     #endif
 
@@ -232,7 +206,63 @@ bool hidh_open_subtask(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t c
   return true;
 }
 
-void hidh_isr(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
+bool hidh_set_config(uint8_t dev_addr, uint8_t itf_num)
+{
+#if 0
+  //------------- Get Report Descriptor TODO HID parser -------------//
+  if ( p_desc_hid->bNumDescriptors )
+  {
+    STASK_INVOKE(
+        usbh_control_xfer_subtask( dev_addr, bm_request_type(TUSB_DIR_IN, TUSB_REQ_TYPE_STANDARD, TUSB_REQ_RCPT_INTERFACE),
+                                   TUSB_REQ_GET_DESCRIPTOR, (p_desc_hid->bReportType << 8), 0,
+                                   p_desc_hid->wReportLength, report_descriptor ),
+        error
+    );
+    (void) error; // if error in getting report descriptor --> treating like there is none
+  }
+#endif
+
+#if 0
+  // SET IDLE = 0 request
+  // Device can stall if not support this request
+  tusb_control_request_t const request =
+  {
+    .bmRequestType_bit =
+    {
+      .recipient = TUSB_REQ_RCPT_INTERFACE,
+      .type      = TUSB_REQ_TYPE_CLASS,
+      .direction = TUSB_DIR_OUT
+    },
+    .bRequest = HID_REQ_CONTROL_SET_IDLE,
+    .wValue   = 0, // idle_rate = 0
+    .wIndex   = p_interface_desc->bInterfaceNumber,
+    .wLength  = 0
+  };
+
+  // stall is a valid response for SET_IDLE, therefore we could ignore result of this request
+  tuh_control_xfer(dev_addr, &request, NULL, NULL);
+#endif
+
+  usbh_driver_set_config_complete(dev_addr, itf_num);
+
+#if CFG_TUH_HID_KEYBOARD
+  if ( keyboardh_data[dev_addr-1].itf_num == itf_num)
+  {
+    tuh_hid_keyboard_mounted_cb(dev_addr);
+  }
+#endif
+
+#if CFG_TUH_HID_MOUSE
+  if ( mouseh_data[dev_addr-1].ep_in == itf_num )
+  {
+    tuh_hid_mouse_mounted_cb(dev_addr);
+  }
+#endif
+
+  return true;
+}
+
+bool hidh_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
 {
   (void) xferred_bytes; // TODO may need to use this para later
 
@@ -240,7 +270,7 @@ void hidh_isr(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t x
   if ( ep_addr == keyboardh_data[dev_addr-1].ep_in )
   {
     tuh_hid_keyboard_isr(dev_addr, event);
-    return;
+    return true;
   }
 #endif
 
@@ -248,13 +278,15 @@ void hidh_isr(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t x
   if ( ep_addr == mouseh_data[dev_addr-1].ep_in )
   {
     tuh_hid_mouse_isr(dev_addr, event);
-    return;
+    return true;
   }
 #endif
 
 #if CFG_TUSB_HOST_HID_GENERIC
 
 #endif
+
+  return true;
 }
 
 void hidh_close(uint8_t dev_addr)
diff --git a/src/class/hid/hid_host.h b/src/class/hid/hid_host.h
index 2e7f52674..5c77398f8 100644
--- a/src/class/hid/hid_host.h
+++ b/src/class/hid/hid_host.h
@@ -197,7 +197,8 @@ void tuh_hid_generic_isr(uint8_t dev_addr, xfer_result_t event);
 //--------------------------------------------------------------------+
 void hidh_init(void);
 bool hidh_open_subtask(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *p_interface_desc, uint16_t *p_length);
-void hidh_isr(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
+bool hidh_set_config(uint8_t dev_addr, uint8_t itf_num);
+bool hidh_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
 void hidh_close(uint8_t dev_addr);
 
 #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_device.c b/src/class/msc/msc_device.c
index f3f2e536e..cfb8a43a3 100644
--- a/src/class/msc/msc_device.c
+++ b/src/class/msc/msc_device.c
@@ -186,10 +186,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,17 +223,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)
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/msc/msc_host.c b/src/class/msc/msc_host.c
index 539e98376..02ca81e77 100644
--- a/src/class/msc/msc_host.c
+++ b/src/class/msc/msc_host.c
@@ -37,53 +37,63 @@
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-CFG_TUSB_MEM_SECTION static msch_interface_t msch_data[CFG_TUSB_HOST_DEVICE_MAX];
+enum
+{
+  MSC_STAGE_IDLE = 0,
+  MSC_STAGE_CMD,
+  MSC_STAGE_DATA,
+  MSC_STAGE_STATUS,
+};
+
+typedef struct
+{
+  uint8_t  itf_num;
+  uint8_t  ep_in;
+  uint8_t  ep_out;
+
+  uint8_t  max_lun;
 
-//------------- Initalization Data -------------//
-static osal_semaphore_def_t msch_sem_def;
-static osal_semaphore_t msch_sem_hdl;
+  volatile bool mounted;
+
+  uint8_t stage;
+  void*   buffer;
+  tuh_msc_complete_cb_t complete_cb;
+
+  msc_cbw_t cbw;
+  msc_csw_t csw;
+}msch_interface_t;
+
+CFG_TUSB_MEM_SECTION static msch_interface_t msch_data[CFG_TUSB_HOST_DEVICE_MAX];
 
 // buffer used to read scsi information when mounted, largest response data currently is inquiry
 CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(4) static uint8_t msch_buffer[sizeof(scsi_inquiry_resp_t)];
 
-//--------------------------------------------------------------------+
-// INTERNAL OBJECT & FUNCTION DECLARATION
-//--------------------------------------------------------------------+
+static inline msch_interface_t* get_itf(uint8_t dev_addr)
+{
+  return &msch_data[dev_addr-1];
+}
 
 //--------------------------------------------------------------------+
 // PUBLIC API
 //--------------------------------------------------------------------+
-bool tuh_msc_is_mounted(uint8_t dev_addr)
+uint8_t tuh_msc_get_maxlun(uint8_t dev_addr)
 {
-  return  tuh_device_is_configured(dev_addr) && // is configured can be omitted
-          msch_data[dev_addr-1].is_initialized;
+  msch_interface_t* p_msc = get_itf(dev_addr);
+  return p_msc->max_lun;
 }
 
-bool tuh_msc_is_busy(uint8_t dev_addr)
+bool tuh_msc_mounted(uint8_t dev_addr)
 {
-  return  msch_data[dev_addr-1].is_initialized &&
-          hcd_edpt_busy(dev_addr, msch_data[dev_addr-1].ep_in);
-}
-
-uint8_t const* tuh_msc_get_vendor_name(uint8_t dev_addr)
-{
-  return msch_data[dev_addr-1].is_initialized ? msch_data[dev_addr-1].vendor_id : NULL;
-}
+  msch_interface_t* p_msc = get_itf(dev_addr);
 
-uint8_t const* tuh_msc_get_product_name(uint8_t dev_addr)
-{
-  return msch_data[dev_addr-1].is_initialized ? msch_data[dev_addr-1].product_id : NULL;
+  // is configured can be omitted
+  return tuh_device_is_configured(dev_addr) && p_msc->mounted;
 }
 
-tusb_error_t tuh_msc_get_capacity(uint8_t dev_addr, uint32_t* p_last_lba, uint32_t* p_block_size)
+bool tuh_msc_is_busy(uint8_t dev_addr)
 {
-  if ( !msch_data[dev_addr-1].is_initialized )   return TUSB_ERROR_MSCH_DEVICE_NOT_MOUNTED;
-  TU_ASSERT(p_last_lba != NULL && p_block_size != NULL, TUSB_ERROR_INVALID_PARA);
-
-  (*p_last_lba)   = msch_data[dev_addr-1].last_lba;
-  (*p_block_size) = (uint32_t) msch_data[dev_addr-1].block_size;
-
-  return TUSB_ERROR_NONE;
+  msch_interface_t* p_msc = get_itf(dev_addr);
+  return p_msc->mounted && hcd_edpt_busy(dev_addr, p_msc->ep_in);
 }
 
 //--------------------------------------------------------------------+
@@ -92,131 +102,98 @@ tusb_error_t tuh_msc_get_capacity(uint8_t dev_addr, uint32_t* p_last_lba, uint32
 static inline void msc_cbw_add_signature(msc_cbw_t *p_cbw, uint8_t lun)
 {
   p_cbw->signature  = MSC_CBW_SIGNATURE;
-  p_cbw->tag        = 0xCAFECAFE;
+  p_cbw->tag        = 0x54555342; // TUSB
   p_cbw->lun        = lun;
 }
 
-static tusb_error_t msch_command_xfer(uint8_t dev_addr, msch_interface_t * p_msch, void* p_buffer)
+bool tuh_msc_scsi_command(uint8_t dev_addr, msc_cbw_t const* cbw, void* data, tuh_msc_complete_cb_t complete_cb)
 {
-  if ( NULL != p_buffer)
-  { // there is data phase
-    if (p_msch->cbw.dir & TUSB_DIR_IN_MASK)
-    {
-      TU_ASSERT( hcd_pipe_xfer(dev_addr, p_msch->ep_out, (uint8_t*) &p_msch->cbw, sizeof(msc_cbw_t), false), TUSB_ERROR_FAILED );
-      TU_ASSERT( hcd_pipe_queue_xfer(dev_addr, p_msch->ep_in , p_buffer, p_msch->cbw.total_bytes), TUSB_ERROR_FAILED );
-    }else
-    {
-      TU_ASSERT( hcd_pipe_queue_xfer(dev_addr, p_msch->ep_out, (uint8_t*) &p_msch->cbw, sizeof(msc_cbw_t)), TUSB_ERROR_FAILED );
-      TU_ASSERT( hcd_pipe_xfer(dev_addr, p_msch->ep_out , p_buffer, p_msch->cbw.total_bytes, false), TUSB_ERROR_FAILED );
-    }
-  }
+  msch_interface_t* p_msc = get_itf(dev_addr);
+  // TU_VERIFY(p_msc->mounted); // TODO part of the enumeration also use scsi command
 
-  TU_ASSERT( hcd_pipe_xfer(dev_addr, p_msch->ep_in , (uint8_t*) &p_msch->csw, sizeof(msc_csw_t), true), TUSB_ERROR_FAILED);
+  // TODO claim endpoint
 
-  return TUSB_ERROR_NONE;
-}
+  p_msc->cbw = *cbw;
+  p_msc->stage = MSC_STAGE_CMD;
+  p_msc->buffer = data;
+  p_msc->complete_cb = complete_cb;
 
-tusb_error_t tusbh_msc_inquiry(uint8_t dev_addr, uint8_t lun, uint8_t *p_data)
-{
-  msch_interface_t* p_msch = &msch_data[dev_addr-1];
+  TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_out, (uint8_t*) &p_msc->cbw, sizeof(msc_cbw_t)));
 
-  //------------- Command Block Wrapper -------------//
-  msc_cbw_add_signature(&p_msch->cbw, lun);
-  p_msch->cbw.total_bytes = sizeof(scsi_inquiry_resp_t);
-  p_msch->cbw.dir        = TUSB_DIR_IN_MASK;
-  p_msch->cbw.cmd_len    = sizeof(scsi_inquiry_t);
+  return true;
+}
 
-  //------------- SCSI command -------------//
-  scsi_inquiry_t cmd_inquiry =
-  {
-      .cmd_code     = SCSI_CMD_INQUIRY,
-      .alloc_length = sizeof(scsi_inquiry_resp_t)
-  };
+bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_resp_t* response, tuh_msc_complete_cb_t complete_cb)
+{
+  msch_interface_t* p_msc = get_itf(dev_addr);
+  if ( !p_msc->mounted ) return false;
 
-  memcpy(p_msch->cbw.command, &cmd_inquiry, p_msch->cbw.cmd_len);
+  msc_cbw_t cbw = { 0 };
 
-  TU_ASSERT_ERR ( msch_command_xfer(dev_addr, p_msch, p_data) );
+  msc_cbw_add_signature(&cbw, lun);
+  cbw.total_bytes = sizeof(scsi_read_capacity10_resp_t);
+  cbw.dir        = TUSB_DIR_IN_MASK;
+  cbw.cmd_len    = sizeof(scsi_read_capacity10_t);
+  cbw.command[0] = SCSI_CMD_READ_CAPACITY_10;
 
-  return TUSB_ERROR_NONE;
+  return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb);
 }
 
-tusb_error_t tusbh_msc_read_capacity10(uint8_t dev_addr, uint8_t lun, uint8_t *p_data)
+bool tuh_msc_scsi_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* response, tuh_msc_complete_cb_t complete_cb)
 {
-  msch_interface_t* p_msch = &msch_data[dev_addr-1];
+  msc_cbw_t cbw = { 0 };
 
-  //------------- Command Block Wrapper -------------//
-  msc_cbw_add_signature(&p_msch->cbw, lun);
-  p_msch->cbw.total_bytes = sizeof(scsi_read_capacity10_resp_t);
-  p_msch->cbw.dir        = TUSB_DIR_IN_MASK;
-  p_msch->cbw.cmd_len    = sizeof(scsi_read_capacity10_t);
+  msc_cbw_add_signature(&cbw, lun);
+  cbw.total_bytes = sizeof(scsi_inquiry_resp_t);
+  cbw.dir         = TUSB_DIR_IN_MASK;
+  cbw.cmd_len     = sizeof(scsi_inquiry_t);
 
-  //------------- SCSI command -------------//
-  scsi_read_capacity10_t cmd_read_capacity10 =
+  scsi_inquiry_t const cmd_inquiry =
   {
-      .cmd_code                 = SCSI_CMD_READ_CAPACITY_10,
-      .lba                      = 0,
-      .partial_medium_indicator = 0
+    .cmd_code     = SCSI_CMD_INQUIRY,
+    .alloc_length = sizeof(scsi_inquiry_resp_t)
   };
+  memcpy(cbw.command, &cmd_inquiry, cbw.cmd_len);
 
-  memcpy(p_msch->cbw.command, &cmd_read_capacity10, p_msch->cbw.cmd_len);
-
-  TU_ASSERT_ERR ( msch_command_xfer(dev_addr, p_msch, p_data) );
-
-  return TUSB_ERROR_NONE;
+  return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb);
 }
 
-tusb_error_t tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, uint8_t *p_data)
+bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_t complete_cb)
 {
-  (void) lun; // TODO [MSCH] multiple lun support
+  msc_cbw_t cbw = { 0 };
+  msc_cbw_add_signature(&cbw, lun);
 
-  msch_interface_t* p_msch = &msch_data[dev_addr-1];
-
-  //------------- Command Block Wrapper -------------//
-  p_msch->cbw.total_bytes = 18;
-  p_msch->cbw.dir        = TUSB_DIR_IN_MASK;
-  p_msch->cbw.cmd_len    = sizeof(scsi_request_sense_t);
+  cbw.total_bytes = 0; // Number of bytes
+  cbw.dir        = TUSB_DIR_OUT;
+  cbw.cmd_len    = sizeof(scsi_test_unit_ready_t);
+  cbw.command[0] = SCSI_CMD_TEST_UNIT_READY;
+  cbw.command[1] = lun; // according to wiki TODO need verification
 
-  //------------- SCSI command -------------//
-  scsi_request_sense_t cmd_request_sense =
-  {
-      .cmd_code     = SCSI_CMD_REQUEST_SENSE,
-      .alloc_length = 18
-  };
-
-  memcpy(p_msch->cbw.command, &cmd_request_sense, p_msch->cbw.cmd_len);
-
-  TU_ASSERT_ERR ( msch_command_xfer(dev_addr, p_msch, p_data) );
-
-  return TUSB_ERROR_NONE;
+  return tuh_msc_scsi_command(dev_addr, &cbw, NULL, complete_cb);
 }
 
-tusb_error_t tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun,  msc_csw_t * p_csw)
+bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void *resposne, tuh_msc_complete_cb_t complete_cb)
 {
-  msch_interface_t* p_msch = &msch_data[dev_addr-1];
+  msc_cbw_t cbw = { 0 };
+  msc_cbw_add_signature(&cbw, lun);
 
-  //------------- Command Block Wrapper -------------//
-  msc_cbw_add_signature(&p_msch->cbw, lun);
-
-  p_msch->cbw.total_bytes = 0; // Number of bytes
-  p_msch->cbw.dir        = TUSB_DIR_OUT;
-  p_msch->cbw.cmd_len    = sizeof(scsi_test_unit_ready_t);
+  cbw.total_bytes = 18; // TODO sense response
+  cbw.dir        = TUSB_DIR_IN_MASK;
+  cbw.cmd_len    = sizeof(scsi_request_sense_t);
 
-  //------------- SCSI command -------------//
-  scsi_test_unit_ready_t cmd_test_unit_ready =
+  scsi_request_sense_t const cmd_request_sense =
   {
-      .cmd_code = SCSI_CMD_TEST_UNIT_READY,
-      .lun      = lun // according to wiki
+    .cmd_code     = SCSI_CMD_REQUEST_SENSE,
+    .alloc_length = 18
   };
 
-  memcpy(p_msch->cbw.command, &cmd_test_unit_ready, p_msch->cbw.cmd_len);
+  memcpy(cbw.command, &cmd_request_sense, cbw.cmd_len);
 
-  // TODO MSCH refractor test uinit ready
-  TU_ASSERT( hcd_pipe_xfer(dev_addr, p_msch->ep_out, (uint8_t*) &p_msch->cbw, sizeof(msc_cbw_t), false), TUSB_ERROR_FAILED );
-  TU_ASSERT( hcd_pipe_xfer(dev_addr, p_msch->ep_in , (uint8_t*) p_csw, sizeof(msc_csw_t), true), TUSB_ERROR_FAILED );
-
-  return TUSB_ERROR_NONE;
+  return tuh_msc_scsi_command(dev_addr, &cbw, resposne, complete_cb);
 }
 
+#if 0
+
 tusb_error_t  tuh_msc_read10(uint8_t dev_addr, uint8_t lun, void * p_buffer, uint32_t lba, uint16_t block_count)
 {
   msch_interface_t* p_msch = &msch_data[dev_addr-1];
@@ -229,7 +206,7 @@ tusb_error_t  tuh_msc_read10(uint8_t dev_addr, uint8_t lun, void * p_buffer, uin
   p_msch->cbw.cmd_len    = sizeof(scsi_read10_t);
 
   //------------- SCSI command -------------//
-  scsi_read10_t cmd_read10 =
+  scsi_read10_t cmd_read10 =msch_sem_hdl
   {
       .cmd_code    = SCSI_CMD_READ_10,
       .lba         = tu_htonl(lba),
@@ -238,7 +215,7 @@ tusb_error_t  tuh_msc_read10(uint8_t dev_addr, uint8_t lun, void * p_buffer, uin
 
   memcpy(p_msch->cbw.command, &cmd_read10, p_msch->cbw.cmd_len);
 
-  TU_ASSERT_ERR ( msch_command_xfer(dev_addr, p_msch, p_buffer));
+  TU_ASSERT_ERR ( send_cbw(dev_addr, p_msch, p_buffer));
 
   return TUSB_ERROR_NONE;
 }
@@ -264,10 +241,32 @@ tusb_error_t tuh_msc_write10(uint8_t dev_addr, uint8_t lun, void const * p_buffe
 
   memcpy(p_msch->cbw.command, &cmd_write10, p_msch->cbw.cmd_len);
 
-  TU_ASSERT_ERR ( msch_command_xfer(dev_addr, p_msch, (void*) p_buffer));
+  TU_ASSERT_ERR ( send_cbw(dev_addr, p_msch, (void*) p_buffer));
 
   return TUSB_ERROR_NONE;
 }
+#endif
+
+#if 0
+// MSC interface Reset (not used now)
+bool tuh_msc_reset(uint8_t dev_addr)
+{
+  tusb_control_request_t const new_request =
+  {
+    .bmRequestType_bit =
+    {
+      .recipient = TUSB_REQ_RCPT_INTERFACE,
+      .type      = TUSB_REQ_TYPE_CLASS,
+      .direction = TUSB_DIR_OUT
+    },
+    .bRequest = MSC_REQ_RESET,
+    .wValue   = 0,
+    .wIndex   = p_msc->itf_num,
+    .wLength  = 0
+  };
+  TU_ASSERT( usbh_control_xfer( dev_addr, &new_request, NULL ) );
+}
+#endif
 
 //--------------------------------------------------------------------+
 // CLASS-USBH API (don't require to verify parameters)
@@ -275,24 +274,83 @@ tusb_error_t tuh_msc_write10(uint8_t dev_addr, uint8_t lun, void const * p_buffe
 void msch_init(void)
 {
   tu_memclr(msch_data, sizeof(msch_interface_t)*CFG_TUSB_HOST_DEVICE_MAX);
-  msch_sem_hdl = osal_semaphore_create(&msch_sem_def);
 }
 
+void msch_close(uint8_t dev_addr)
+{
+  msch_interface_t* p_msc = get_itf(dev_addr);
+  tu_memclr(p_msc, sizeof(msch_interface_t));
+  tuh_msc_unmounted_cb(dev_addr); // invoke Application Callback
+}
+
+bool msch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
+{
+  msch_interface_t* p_msc = get_itf(dev_addr);
+  msc_cbw_t const * cbw = &p_msc->cbw;
+  msc_csw_t       * csw = &p_msc->csw;
+
+  switch (p_msc->stage)
+  {
+    case MSC_STAGE_CMD:
+      // Must be Command Block
+      TU_ASSERT(ep_addr == p_msc->ep_out &&  event == XFER_RESULT_SUCCESS && xferred_bytes == sizeof(msc_cbw_t));
+
+      if ( cbw->total_bytes && p_msc->buffer )
+      {
+        // Data stage if any
+        p_msc->stage = MSC_STAGE_DATA;
+
+        uint8_t const ep_data = (cbw->dir & TUSB_DIR_IN_MASK) ? p_msc->ep_in : p_msc->ep_out;
+        TU_ASSERT(usbh_edpt_xfer(dev_addr, ep_data, p_msc->buffer, cbw->total_bytes));
+      }else
+      {
+        // Status stage
+        p_msc->stage = MSC_STAGE_STATUS;
+        TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_in, (uint8_t*) &p_msc->csw, sizeof(msc_csw_t)));
+      }
+    break;
+
+    case MSC_STAGE_DATA:
+      // Status stage
+      p_msc->stage = MSC_STAGE_STATUS;
+      TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_in, (uint8_t*) &p_msc->csw, sizeof(msc_csw_t)));
+    break;
+
+    case MSC_STAGE_STATUS:
+      // SCSI op is complete
+      p_msc->stage = MSC_STAGE_IDLE;
+
+      if (p_msc->complete_cb) p_msc->complete_cb(dev_addr, cbw, csw);
+    break;
+
+    // unknown state
+    default: break;
+  }
+
+  return true;
+}
+
+//--------------------------------------------------------------------+
+// MSC Enumeration
+//--------------------------------------------------------------------+
+
+static bool config_get_maxlun_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result);
+static bool config_test_unit_ready_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw);
+static bool config_request_sense_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw);
+
 bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t *p_length)
 {
   TU_VERIFY (MSC_SUBCLASS_SCSI == itf_desc->bInterfaceSubClass &&
              MSC_PROTOCOL_BOT  == itf_desc->bInterfaceProtocol);
 
-  msch_interface_t* p_msc = &msch_data[dev_addr-1];
+  msch_interface_t* p_msc = get_itf(dev_addr);
 
   //------------- Open Data Pipe -------------//
   tusb_desc_endpoint_t const * ep_desc = (tusb_desc_endpoint_t const *) tu_desc_next(itf_desc);
 
   for(uint32_t i=0; i<2; i++)
   {
-    TU_ASSERT(TUSB_DESC_ENDPOINT == ep_desc->bDescriptorType);
-    TU_ASSERT(TUSB_XFER_BULK == ep_desc->bmAttributes.xfer);
-
+    TU_ASSERT(TUSB_DESC_ENDPOINT == ep_desc->bDescriptorType && TUSB_XFER_BULK == ep_desc->bmAttributes.xfer);
     TU_ASSERT(usbh_edpt_open(rhport, dev_addr, ep_desc));
 
     if ( tu_edpt_dir(ep_desc->bEndpointAddress) == TUSB_DIR_IN )
@@ -309,106 +367,78 @@ bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *it
   p_msc->itf_num = itf_desc->bInterfaceNumber;
   (*p_length) += sizeof(tusb_desc_interface_t) + 2*sizeof(tusb_desc_endpoint_t);
 
-  //------------- Get Max Lun -------------//
-  TU_LOG2("MSC Get Max Lun\r\n");
-  tusb_control_request_t request = {
-        .bmRequestType_bit = { .recipient = TUSB_REQ_RCPT_INTERFACE, .type = TUSB_REQ_TYPE_CLASS, .direction = TUSB_DIR_IN },
-        .bRequest = MSC_REQ_GET_MAX_LUN,
-        .wValue = 0,
-        .wIndex = p_msc->itf_num,
-        .wLength = 1
-  };
-  // TODO STALL means zero
-  TU_ASSERT( usbh_control_xfer( dev_addr, &request, msch_buffer ) );
-  p_msc->max_lun = msch_buffer[0];
-
-#if 0
-  //------------- Reset -------------//
-  request = (tusb_control_request_t) {
-        .bmRequestType_bit = { .recipient = TUSB_REQ_RCPT_INTERFACE, .type = TUSB_REQ_TYPE_CLASS, .direction = TUSB_DIR_OUT },
-        .bRequest = MSC_REQ_RESET,
-        .wValue = 0,
-        .wIndex = p_msc->itf_num,
-        .wLength = 0
-  };
-  TU_ASSERT( usbh_control_xfer( dev_addr, &request, NULL ) );
-#endif
-
-  enum { SCSI_XFER_TIMEOUT = 2000 };
-  //------------- SCSI Inquiry -------------//
-  tusbh_msc_inquiry(dev_addr, 0, msch_buffer);
-  TU_ASSERT( osal_semaphore_wait(msch_sem_hdl, SCSI_XFER_TIMEOUT) );
-
-  memcpy(p_msc->vendor_id , ((scsi_inquiry_resp_t*) msch_buffer)->vendor_id , 8);
-  memcpy(p_msc->product_id, ((scsi_inquiry_resp_t*) msch_buffer)->product_id, 16);
+  return true;
+}
 
-  //------------- SCSI Read Capacity 10 -------------//
-  tusbh_msc_read_capacity10(dev_addr, 0, msch_buffer);
-  TU_ASSERT( osal_semaphore_wait(msch_sem_hdl, SCSI_XFER_TIMEOUT));
+bool msch_set_config(uint8_t dev_addr, uint8_t itf_num)
+{
+  msch_interface_t* p_msc = get_itf(dev_addr);
+  TU_ASSERT(p_msc->itf_num == itf_num);
 
-  // NOTE: my toshiba thumb-drive stall the first Read Capacity and require the sequence
-  // Read Capacity --> Stalled --> Clear Stall --> Request Sense --> Read Capacity (2) to work
-  if ( hcd_edpt_stalled(dev_addr, p_msc->ep_in) )
+  //------------- Get Max Lun -------------//
+  TU_LOG2("MSC Get Max Lun\r\n");
+  tusb_control_request_t request =
   {
-    // clear stall TODO abstract clear stall function
-    request = (tusb_control_request_t) {
-      .bmRequestType_bit = { .recipient = TUSB_REQ_RCPT_ENDPOINT, .type = TUSB_REQ_TYPE_STANDARD, .direction = TUSB_DIR_OUT },
-          .bRequest = TUSB_REQ_CLEAR_FEATURE,
-          .wValue = 0,
-          .wIndex = p_msc->ep_in,
-          .wLength = 0
-    };
-
-    TU_ASSERT(usbh_control_xfer( dev_addr, &request, NULL ));
-
-    hcd_edpt_clear_stall(dev_addr, p_msc->ep_in);
-    TU_ASSERT( osal_semaphore_wait(msch_sem_hdl, SCSI_XFER_TIMEOUT) ); // wait for SCSI status
+    .bmRequestType_bit =
+    {
+      .recipient = TUSB_REQ_RCPT_INTERFACE,
+      .type      = TUSB_REQ_TYPE_CLASS,
+      .direction = TUSB_DIR_IN
+    },
+    .bRequest = MSC_REQ_GET_MAX_LUN,
+    .wValue   = 0,
+    .wIndex   = itf_num,
+    .wLength  = 1
+  };
+  TU_ASSERT(tuh_control_xfer(dev_addr, &request, &p_msc->max_lun, config_get_maxlun_complete));
 
-    //------------- SCSI Request Sense -------------//
-    (void) tuh_msc_request_sense(dev_addr, 0, msch_buffer);
-    TU_ASSERT(osal_semaphore_wait(msch_sem_hdl, SCSI_XFER_TIMEOUT));
+  return true;
+}
 
-    //------------- Re-read SCSI Read Capactity -------------//
-    tusbh_msc_read_capacity10(dev_addr, 0, msch_buffer);
-    TU_ASSERT(osal_semaphore_wait(msch_sem_hdl, SCSI_XFER_TIMEOUT));
-  }
+static bool config_get_maxlun_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result)
+{
+  (void) request;
 
-  p_msc->last_lba   = tu_ntohl( ((scsi_read_capacity10_resp_t*)msch_buffer)->last_lba );
-  p_msc->block_size = (uint16_t) tu_ntohl( ((scsi_read_capacity10_resp_t*)msch_buffer)->block_size );
+  msch_interface_t* p_msc = get_itf(dev_addr);
 
-  p_msc->is_initialized = true;
+  // STALL means zero
+  p_msc->max_lun = (XFER_RESULT_SUCCESS == result) ? msch_buffer[0] : 0;
+  p_msc->max_lun++; // MAX LUN is minus 1 by specs
 
-  tuh_msc_mounted_cb(dev_addr);
+  // TODO multiple LUN support
+  TU_LOG2("SCSI Test Unit Ready\r\n");
+  tuh_msc_test_unit_ready(dev_addr, 0, config_test_unit_ready_complete);
 
   return true;
 }
 
-void msch_isr(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
+static bool config_test_unit_ready_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw)
 {
-  msch_interface_t* p_msc = &msch_data[dev_addr-1];
-  if ( ep_addr == p_msc->ep_in )
+  if (csw->status == 0)
   {
-    if (p_msc->is_initialized)
-    {
-      tuh_msc_isr(dev_addr, event, xferred_bytes);
-    }else
-    { // still initializing under open subtask
-      osal_semaphore_post(msch_sem_hdl, true);
-    }
+    msch_interface_t* p_msc = get_itf(dev_addr);
+
+    usbh_driver_set_config_complete(dev_addr, p_msc->itf_num);
+
+    // Unit is ready, Enumeration is complete
+    p_msc->mounted = true;
+    tuh_msc_mounted_cb(dev_addr);
+  }else
+  {
+    // Note: During enumeration, some device fails Test Unit Ready and require a few retries
+    // with Request Sense to start working !!
+    // TODO limit number of retries
+    TU_ASSERT(tuh_msc_request_sense(dev_addr, cbw->lun, msch_buffer, config_request_sense_complete));
   }
+
+  return true;
 }
 
-void msch_close(uint8_t dev_addr)
+static bool config_request_sense_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw)
 {
-  tu_memclr(&msch_data[dev_addr-1], sizeof(msch_interface_t));
-  osal_semaphore_reset(msch_sem_hdl);
-
-  tuh_msc_unmounted_cb(dev_addr); // invoke Application Callback
+  TU_ASSERT(csw->status == 0);
+  TU_ASSERT(tuh_msc_test_unit_ready(dev_addr, cbw->lun, config_test_unit_ready_complete));
+  return true;
 }
 
-//--------------------------------------------------------------------+
-// INTERNAL & HELPER
-//--------------------------------------------------------------------+
-
-
 #endif
diff --git a/src/class/msc/msc_host.h b/src/class/msc/msc_host.h
index 959294adf..5913350b8 100644
--- a/src/class/msc/msc_host.h
+++ b/src/class/msc/msc_host.h
@@ -40,15 +40,16 @@
  * \defgroup MSC_Host Host
  *  The interface API includes status checking function, data transferring function and callback functions
  *  @{ */
+
+typedef bool (*tuh_msc_complete_cb_t)(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw);
+
 //--------------------------------------------------------------------+
-// MASS STORAGE Application API
+// Application API
 //--------------------------------------------------------------------+
-/** \brief      Check if device supports MassStorage interface or not
- * \param[in]   dev_addr    device address
- * \retval      true if device supports
- * \retval      false if device does not support or is not mounted
- */
-bool          tuh_msc_is_mounted(uint8_t dev_addr);
+
+// Check if device supports MassStorage interface.
+// This function true after tuh_msc_mounted_cb() and false after tuh_msc_unmounted_cb()
+bool tuh_msc_mounted(uint8_t dev_addr);
 
 /** \brief      Check if the interface is currently busy or not
  * \param[in]   dev_addr device address
@@ -60,35 +61,27 @@ bool          tuh_msc_is_mounted(uint8_t dev_addr);
  */
 bool          tuh_msc_is_busy(uint8_t dev_addr);
 
-/** \brief      Get SCSI vendor's name of MassStorage device
- * \param[in]   dev_addr device address
- * \return      pointer to vendor's name or NULL if specified device does not support MassStorage
- * \note        SCSI vendor's name is 8-byte length field in \ref scsi_inquiry_data_t. During enumeration, the stack has already
- *              retrieved (via SCSI INQUIRY) and store this information internally. There is no need for application to re-send SCSI INQUIRY
- *              command or allocate buffer for this.
- */
-uint8_t const* tuh_msc_get_vendor_name(uint8_t dev_addr);
+// Get Max Lun
+uint8_t tuh_msc_get_maxlun(uint8_t dev_addr);
 
-/** \brief      Get SCSI product's name of MassStorage device
- * \param[in]   dev_addr device address
- * \return      pointer to product's name or NULL if specified device does not support MassStorage
- * \note        SCSI product's name is 16-byte length field in \ref scsi_inquiry_data_t. During enumeration, the stack has already
- *              retrieved (via SCSI INQUIRY) and store this information internally. There is no need for application to re-send SCSI INQUIRY
- *              command or allocate buffer for this.
- */
-uint8_t const* tuh_msc_get_product_name(uint8_t dev_addr);
+// Carry out a full SCSI command (cbw, data, csw) in non-blocking manner.
+// `complete_cb` callback is invoked when SCSI op is complete.
+// return true if success, false if there is already pending operation.
+bool tuh_msc_scsi_command(uint8_t dev_addr, msc_cbw_t const* cbw, void* data, tuh_msc_complete_cb_t complete_cb);
 
-/** \brief      Get SCSI Capacity of MassStorage device
- * \param[in]   dev_addr device address
- * \param[out]  p_last_lba Last Logical Block Address of device
- * \param[out]  p_block_size Block Size of device in bytes
- * \retval      pointer to product's name or NULL if specified device does not support MassStorage
- * \note        MassStorage's capacity can be computed by last LBA x block size (in bytes). During enumeration, the stack has already
- *              retrieved (via SCSI READ CAPACITY 10) and store this information internally. There is no need for application
- *              to re-send SCSI READ CAPACITY 10 command
- */
-tusb_error_t tuh_msc_get_capacity(uint8_t dev_addr, uint32_t* p_last_lba, uint32_t* p_block_size);
+// Carry out SCSI INQUIRY command in non-blocking manner.
+bool tuh_msc_scsi_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* response, tuh_msc_complete_cb_t complete_cb);
+
+// Carry out SCSI REQUEST SENSE (10) command in non-blocking manner.
+bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_t complete_cb);
 
+// Carry out SCSI REQUEST SENSE (10) command in non-blocking manner.
+bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void *resposne, tuh_msc_complete_cb_t complete_cb);
+
+// Carry out SCSI READ CAPACITY (10) command in non-blocking manner.
+bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_resp_t* response, tuh_msc_complete_cb_t complete_cb);
+
+#if 0
 /** \brief 			Perform SCSI READ 10 command to read data from MassStorage device
  * \param[in]		dev_addr	device address
  * \param[in]		lun       Targeted Logical Unit
@@ -116,84 +109,24 @@ tusb_error_t tuh_msc_read10 (uint8_t dev_addr, uint8_t lun, void * p_buffer, uin
  * \note        This function is non-blocking and returns immediately. The result of USB transfer will be reported by the interface's callback function
  */
 tusb_error_t tuh_msc_write10(uint8_t dev_addr, uint8_t lun, void const * p_buffer, uint32_t lba, uint16_t block_count);
-
-/** \brief 			Perform SCSI REQUEST SENSE command, used to retrieve sense data from MassStorage device
- * \param[in]		dev_addr	device address
- * \param[in]		lun       Targeted Logical Unit
- * \param[in]	  p_data    Buffer to store response's data from device. Must be accessible by USB controller (see \ref CFG_TUSB_MEM_SECTION)
- * \retval      TUSB_ERROR_NONE on success
- * \retval      TUSB_ERROR_INTERFACE_IS_BUSY if the interface is already transferring data with device
- * \retval      TUSB_ERROR_DEVICE_NOT_READY if device is not yet configured (by SET CONFIGURED request)
- * \retval      TUSB_ERROR_INVALID_PARA if input parameters are not correct
- * \note        This function is non-blocking and returns immediately. The result of USB transfer will be reported by the interface's callback function
- */
-tusb_error_t tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, uint8_t *p_data);
-
-/** \brief 			Perform SCSI TEST UNIT READY command to test if MassStorage device is ready
- * \param[in]		dev_addr	device address
- * \param[in]		lun       Targeted Logical Unit
- * \retval      TUSB_ERROR_NONE on success
- * \retval      TUSB_ERROR_INTERFACE_IS_BUSY if the interface is already transferring data with device
- * \retval      TUSB_ERROR_DEVICE_NOT_READY if device is not yet configured (by SET CONFIGURED request)
- * \retval      TUSB_ERROR_INVALID_PARA if input parameters are not correct
- * \note        This function is non-blocking and returns immediately. The result of USB transfer will be reported by the interface's callback function
- */
-tusb_error_t tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, msc_csw_t * p_csw); // TODO to be refractor
-
-//tusb_error_t  tusbh_msc_scsi_send(uint8_t dev_addr, uint8_t lun, bool is_direction_in,
-//                                  uint8_t const * p_command, uint8_t cmd_len,
-//                                  uint8_t * p_response, uint32_t resp_len);
+#endif
 
 //------------- Application Callback -------------//
-/** \brief 			Callback function that will be invoked when a device with MassStorage interface is mounted
- * \param[in]	  dev_addr Address of newly mounted device
- * \note        This callback should be used by Application to set-up interface-related data
- */
+
+// Invoked when a device with MassStorage interface is mounted
 void tuh_msc_mounted_cb(uint8_t dev_addr);
 
-/** \brief 			Callback function that will be invoked when a device with MassStorage interface is unmounted
- * \param[in] 	dev_addr Address of newly unmounted device
- * \note        This callback should be used by Application to tear-down interface-related data
- */
+// Invoked when a device with MassStorage interface is unmounted
 void tuh_msc_unmounted_cb(uint8_t dev_addr);
 
-/** \brief      Callback function that is invoked when an transferring event occurred
- * \param[in]		dev_addr	Address of device
- * \param[in]   event an value from \ref xfer_result_t
- * \param[in]   xferred_bytes Number of bytes transferred via USB bus
- * \note        event can be one of following
- *              - XFER_RESULT_SUCCESS : previously scheduled transfer completes successfully.
- *              - XFER_RESULT_FAILED   : previously scheduled transfer encountered a transaction error.
- *              - XFER_RESULT_STALLED : previously scheduled transfer is stalled by device.
- * \note
- */
-void tuh_msc_isr(uint8_t dev_addr, xfer_result_t event, uint32_t xferred_bytes);
-
-
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-typedef struct
-{
-  uint8_t  itf_num;
-  uint8_t  ep_in;
-  uint8_t  ep_out;
-
-  uint8_t  max_lun;
-  uint16_t block_size;
-  uint32_t last_lba; // last logical block address
-
-  volatile bool is_initialized;
-  uint8_t vendor_id[8];
-  uint8_t product_id[16];
-
-  msc_cbw_t cbw;
-  msc_csw_t csw;
-}msch_interface_t;
 
 void msch_init(void);
 bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t *p_length);
-void msch_isr(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
+bool msch_set_config(uint8_t dev_addr, uint8_t itf_num);
+bool msch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
 void msch_close(uint8_t dev_addr);
 
 #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);
 
 /************************************************************