Merge branch 'master' into pico

45 files changed, 1387 insertions, 1647 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);
 
 /************************************************************
diff --git a/src/common/sys_queue.h b/src/common/sys_queue.h
deleted file mode 100644
index 443f01b22..000000000
--- a/src/common/sys_queue.h
+++ /dev/null
@@ -1,871 +0,0 @@
-/*-
- * SPDX-License-Identifier: BSD-3-Clause
- *
- * Copyright (c) 1991, 1993
- *	The Regents of the University of California.  All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in the
- *    documentation and/or other materials provided with the distribution.
- * 3. Neither the name of the University nor the names of its contributors
- *    may be used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
- * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
- * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
- * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
- * SUCH DAMAGE.
- *
- *	@(#)queue.h	8.5 (Berkeley) 8/20/94
- * $FreeBSD$
- */
-
-#ifndef _SYS_QUEUE_H_
-#define	_SYS_QUEUE_H_
-
-#include <sys/cdefs.h>
-
-/*
- * This file defines four types of data structures: singly-linked lists,
- * singly-linked tail queues, lists and tail queues.
- *
- * A singly-linked list is headed by a single forward pointer. The elements
- * are singly linked for minimum space and pointer manipulation overhead at
- * the expense of O(n) removal for arbitrary elements. New elements can be
- * added to the list after an existing element or at the head of the list.
- * Elements being removed from the head of the list should use the explicit
- * macro for this purpose for optimum efficiency. A singly-linked list may
- * only be traversed in the forward direction.  Singly-linked lists are ideal
- * for applications with large datasets and few or no removals or for
- * implementing a LIFO queue.
- *
- * A singly-linked tail queue is headed by a pair of pointers, one to the
- * head of the list and the other to the tail of the list. The elements are
- * singly linked for minimum space and pointer manipulation overhead at the
- * expense of O(n) removal for arbitrary elements. New elements can be added
- * to the list after an existing element, at the head of the list, or at the
- * end of the list. Elements being removed from the head of the tail queue
- * should use the explicit macro for this purpose for optimum efficiency.
- * A singly-linked tail queue may only be traversed in the forward direction.
- * Singly-linked tail queues are ideal for applications with large datasets
- * and few or no removals or for implementing a FIFO queue.
- *
- * A list is headed by a single forward pointer (or an array of forward
- * pointers for a hash table header). The elements are doubly linked
- * so that an arbitrary element can be removed without a need to
- * traverse the list. New elements can be added to the list before
- * or after an existing element or at the head of the list. A list
- * may be traversed in either direction.
- *
- * A tail queue is headed by a pair of pointers, one to the head of the
- * list and the other to the tail of the list. The elements are doubly
- * linked so that an arbitrary element can be removed without a need to
- * traverse the list. New elements can be added to the list before or
- * after an existing element, at the head of the list, or at the end of
- * the list. A tail queue may be traversed in either direction.
- *
- * For details on the use of these macros, see the queue(3) manual page.
- *
- * Below is a summary of implemented functions where:
- *  +  means the macro is available
- *  -  means the macro is not available
- *  s  means the macro is available but is slow (runs in O(n) time)
- *
- *                              SLIST    LIST     STAILQ   TAILQ
- * _HEAD                        +        +        +        +
- * _CLASS_HEAD                  +        +        +        +
- * _HEAD_INITIALIZER            +        +        +        +
- * _ENTRY                       +        +        +        +
- * _CLASS_ENTRY                 +        +        +        +
- * _INIT                        +        +        +        +
- * _EMPTY                       +        +        +        +
- * _FIRST                       +        +        +        +
- * _NEXT                        +        +        +        +
- * _PREV                        -        +        -        +
- * _LAST                        -        -        +        +
- * _LAST_FAST                   -        -        -        +
- * _FOREACH                     +        +        +        +
- * _FOREACH_FROM                +        +        +        +
- * _FOREACH_SAFE                +        +        +        +
- * _FOREACH_FROM_SAFE           +        +        +        +
- * _FOREACH_REVERSE             -        -        -        +
- * _FOREACH_REVERSE_FROM        -        -        -        +
- * _FOREACH_REVERSE_SAFE        -        -        -        +
- * _FOREACH_REVERSE_FROM_SAFE   -        -        -        +
- * _INSERT_HEAD                 +        +        +        +
- * _INSERT_BEFORE               -        +        -        +
- * _INSERT_AFTER                +        +        +        +
- * _INSERT_TAIL                 -        -        +        +
- * _CONCAT                      s        s        +        +
- * _REMOVE_AFTER                +        -        +        -
- * _REMOVE_HEAD                 +        -        +        -
- * _REMOVE                      s        +        s        +
- * _SWAP                        +        +        +        +
- *
- */
-#ifdef QUEUE_MACRO_DEBUG
-#warn Use QUEUE_MACRO_DEBUG_TRACE and/or QUEUE_MACRO_DEBUG_TRASH
-#define	QUEUE_MACRO_DEBUG_TRACE
-#define	QUEUE_MACRO_DEBUG_TRASH
-#endif
-
-#ifdef QUEUE_MACRO_DEBUG_TRACE
-/* Store the last 2 places the queue element or head was altered */
-struct qm_trace {
-	unsigned long	 lastline;
-	unsigned long	 prevline;
-	const char	*lastfile;
-	const char	*prevfile;
-};
-
-#define	TRACEBUF	struct qm_trace trace;
-#define	TRACEBUF_INITIALIZER	{ __LINE__, 0, __FILE__, NULL } ,
-
-#define	QMD_TRACE_HEAD(head) do {					\
-	(head)->trace.prevline = (head)->trace.lastline;		\
-	(head)->trace.prevfile = (head)->trace.lastfile;		\
-	(head)->trace.lastline = __LINE__;				\
-	(head)->trace.lastfile = __FILE__;				\
-} while (0)
-
-#define	QMD_TRACE_ELEM(elem) do {					\
-	(elem)->trace.prevline = (elem)->trace.lastline;		\
-	(elem)->trace.prevfile = (elem)->trace.lastfile;		\
-	(elem)->trace.lastline = __LINE__;				\
-	(elem)->trace.lastfile = __FILE__;				\
-} while (0)
-
-#else	/* !QUEUE_MACRO_DEBUG_TRACE */
-#define	QMD_TRACE_ELEM(elem)
-#define	QMD_TRACE_HEAD(head)
-#define	TRACEBUF
-#define	TRACEBUF_INITIALIZER
-#endif	/* QUEUE_MACRO_DEBUG_TRACE */
-
-#ifdef QUEUE_MACRO_DEBUG_TRASH
-#define	TRASHIT(x)		do {(x) = (void *)-1;} while (0)
-#define	QMD_IS_TRASHED(x)	((x) == (void *)(intptr_t)-1)
-#else	/* !QUEUE_MACRO_DEBUG_TRASH */
-#define	TRASHIT(x)
-#define	QMD_IS_TRASHED(x)	0
-#endif	/* QUEUE_MACRO_DEBUG_TRASH */
-
-#if defined(QUEUE_MACRO_DEBUG_TRACE) || defined(QUEUE_MACRO_DEBUG_TRASH)
-#define	QMD_SAVELINK(name, link)	void **name = (void *)&(link)
-#else	/* !QUEUE_MACRO_DEBUG_TRACE && !QUEUE_MACRO_DEBUG_TRASH */
-#define	QMD_SAVELINK(name, link)
-#endif	/* QUEUE_MACRO_DEBUG_TRACE || QUEUE_MACRO_DEBUG_TRASH */
-
-#ifdef __cplusplus
-/*
- * In C++ there can be structure lists and class lists:
- */
-#define	QUEUE_TYPEOF(type) type
-#else
-#define	QUEUE_TYPEOF(type) struct type
-#endif
-
-/*
- * Singly-linked List declarations.
- */
-#define	SLIST_HEAD(name, type)						\
-struct name {								\
-	struct type *slh_first;	/* first element */			\
-}
-
-#define	SLIST_CLASS_HEAD(name, type)					\
-struct name {								\
-	class type *slh_first;	/* first element */			\
-}
-
-#define	SLIST_HEAD_INITIALIZER(head)					\
-	{ NULL }
-
-#define	SLIST_ENTRY(type)						\
-struct {								\
-	struct type *sle_next;	/* next element */			\
-}
-
-#define	SLIST_CLASS_ENTRY(type)						\
-struct {								\
-	class type *sle_next;		/* next element */		\
-}
-
-/*
- * Singly-linked List functions.
- */
-#if (defined(_KERNEL) && defined(INVARIANTS))
-#define	QMD_SLIST_CHECK_PREVPTR(prevp, elm) do {			\
-	if (*(prevp) != (elm))						\
-		panic("Bad prevptr *(%p) == %p != %p",			\
-		    (prevp), *(prevp), (elm));				\
-} while (0)
-#else
-#define	QMD_SLIST_CHECK_PREVPTR(prevp, elm)
-#endif
-
-#define SLIST_CONCAT(head1, head2, type, field) do {			\
-	QUEUE_TYPEOF(type) *curelm = SLIST_FIRST(head1);		\
-	if (curelm == NULL) {						\
-		if ((SLIST_FIRST(head1) = SLIST_FIRST(head2)) != NULL)	\
-			SLIST_INIT(head2);				\
-	} else if (SLIST_FIRST(head2) != NULL) {			\
-		while (SLIST_NEXT(curelm, field) != NULL)		\
-			curelm = SLIST_NEXT(curelm, field);		\
-		SLIST_NEXT(curelm, field) = SLIST_FIRST(head2);		\
-		SLIST_INIT(head2);					\
-	}								\
-} while (0)
-
-#define	SLIST_EMPTY(head)	((head)->slh_first == NULL)
-
-#define	SLIST_FIRST(head)	((head)->slh_first)
-
-#define	SLIST_FOREACH(var, head, field)					\
-	for ((var) = SLIST_FIRST((head));				\
-	    (var);							\
-	    (var) = SLIST_NEXT((var), field))
-
-#define	SLIST_FOREACH_FROM(var, head, field)				\
-	for ((var) = ((var) ? (var) : SLIST_FIRST((head)));		\
-	    (var);							\
-	    (var) = SLIST_NEXT((var), field))
-
-#define	SLIST_FOREACH_SAFE(var, head, field, tvar)			\
-	for ((var) = SLIST_FIRST((head));				\
-	    (var) && ((tvar) = SLIST_NEXT((var), field), 1);		\
-	    (var) = (tvar))
-
-#define	SLIST_FOREACH_FROM_SAFE(var, head, field, tvar)			\
-	for ((var) = ((var) ? (var) : SLIST_FIRST((head)));		\
-	    (var) && ((tvar) = SLIST_NEXT((var), field), 1);		\
-	    (var) = (tvar))
-
-#define	SLIST_FOREACH_PREVPTR(var, varp, head, field)			\
-	for ((varp) = &SLIST_FIRST((head));				\
-	    ((var) = *(varp)) != NULL;					\
-	    (varp) = &SLIST_NEXT((var), field))
-
-#define	SLIST_INIT(head) do {						\
-	SLIST_FIRST((head)) = NULL;					\
-} while (0)
-
-#define	SLIST_INSERT_AFTER(slistelm, elm, field) do {			\
-	SLIST_NEXT((elm), field) = SLIST_NEXT((slistelm), field);	\
-	SLIST_NEXT((slistelm), field) = (elm);				\
-} while (0)
-
-#define	SLIST_INSERT_HEAD(head, elm, field) do {			\
-	SLIST_NEXT((elm), field) = SLIST_FIRST((head));			\
-	SLIST_FIRST((head)) = (elm);					\
-} while (0)
-
-#define	SLIST_NEXT(elm, field)	((elm)->field.sle_next)
-
-#define	SLIST_REMOVE(head, elm, type, field) do {			\
-	QMD_SAVELINK(oldnext, (elm)->field.sle_next);			\
-	if (SLIST_FIRST((head)) == (elm)) {				\
-		SLIST_REMOVE_HEAD((head), field);			\
-	}								\
-	else {								\
-		QUEUE_TYPEOF(type) *curelm = SLIST_FIRST(head);		\
-		while (SLIST_NEXT(curelm, field) != (elm))		\
-			curelm = SLIST_NEXT(curelm, field);		\
-		SLIST_REMOVE_AFTER(curelm, field);			\
-	}								\
-	TRASHIT(*oldnext);						\
-} while (0)
-
-#define SLIST_REMOVE_AFTER(elm, field) do {				\
-	SLIST_NEXT(elm, field) =					\
-	    SLIST_NEXT(SLIST_NEXT(elm, field), field);			\
-} while (0)
-
-#define	SLIST_REMOVE_HEAD(head, field) do {				\
-	SLIST_FIRST((head)) = SLIST_NEXT(SLIST_FIRST((head)), field);	\
-} while (0)
-
-#define	SLIST_REMOVE_PREVPTR(prevp, elm, field) do {			\
-	QMD_SLIST_CHECK_PREVPTR(prevp, elm);				\
-	*(prevp) = SLIST_NEXT(elm, field);				\
-	TRASHIT((elm)->field.sle_next);					\
-} while (0)
-
-#define SLIST_SWAP(head1, head2, type) do {				\
-	QUEUE_TYPEOF(type) *swap_first = SLIST_FIRST(head1);		\
-	SLIST_FIRST(head1) = SLIST_FIRST(head2);			\
-	SLIST_FIRST(head2) = swap_first;				\
-} while (0)
-
-/*
- * Singly-linked Tail queue declarations.
- */
-#define	STAILQ_HEAD(name, type)						\
-struct name {								\
-	struct type *stqh_first;/* first element */			\
-	struct type **stqh_last;/* addr of last next element */		\
-}
-
-#define	STAILQ_CLASS_HEAD(name, type)					\
-struct name {								\
-	class type *stqh_first;	/* first element */			\
-	class type **stqh_last;	/* addr of last next element */		\
-}
-
-#define	STAILQ_HEAD_INITIALIZER(head)					\
-	{ NULL, &(head).stqh_first }
-
-#define	STAILQ_ENTRY(type)						\
-struct {								\
-	struct type *stqe_next;	/* next element */			\
-}
-
-#define	STAILQ_CLASS_ENTRY(type)					\
-struct {								\
-	class type *stqe_next;	/* next element */			\
-}
-
-/*
- * Singly-linked Tail queue functions.
- */
-#define	STAILQ_CONCAT(head1, head2) do {				\
-	if (!STAILQ_EMPTY((head2))) {					\
-		*(head1)->stqh_last = (head2)->stqh_first;		\
-		(head1)->stqh_last = (head2)->stqh_last;		\
-		STAILQ_INIT((head2));					\
-	}								\
-} while (0)
-
-#define	STAILQ_EMPTY(head)	((head)->stqh_first == NULL)
-
-#define	STAILQ_FIRST(head)	((head)->stqh_first)
-
-#define	STAILQ_FOREACH(var, head, field)				\
-	for((var) = STAILQ_FIRST((head));				\
-	   (var);							\
-	   (var) = STAILQ_NEXT((var), field))
-
-#define	STAILQ_FOREACH_FROM(var, head, field)				\
-	for ((var) = ((var) ? (var) : STAILQ_FIRST((head)));		\
-	   (var);							\
-	   (var) = STAILQ_NEXT((var), field))
-
-#define	STAILQ_FOREACH_SAFE(var, head, field, tvar)			\
-	for ((var) = STAILQ_FIRST((head));				\
-	    (var) && ((tvar) = STAILQ_NEXT((var), field), 1);		\
-	    (var) = (tvar))
-
-#define	STAILQ_FOREACH_FROM_SAFE(var, head, field, tvar)		\
-	for ((var) = ((var) ? (var) : STAILQ_FIRST((head)));		\
-	    (var) && ((tvar) = STAILQ_NEXT((var), field), 1);		\
-	    (var) = (tvar))
-
-#define	STAILQ_INIT(head) do {						\
-	STAILQ_FIRST((head)) = NULL;					\
-	(head)->stqh_last = &STAILQ_FIRST((head));			\
-} while (0)
-
-#define	STAILQ_INSERT_AFTER(head, tqelm, elm, field) do {		\
-	if ((STAILQ_NEXT((elm), field) = STAILQ_NEXT((tqelm), field)) == NULL)\
-		(head)->stqh_last = &STAILQ_NEXT((elm), field);		\
-	STAILQ_NEXT((tqelm), field) = (elm);				\
-} while (0)
-
-#define	STAILQ_INSERT_HEAD(head, elm, field) do {			\
-	if ((STAILQ_NEXT((elm), field) = STAILQ_FIRST((head))) == NULL)	\
-		(head)->stqh_last = &STAILQ_NEXT((elm), field);		\
-	STAILQ_FIRST((head)) = (elm);					\
-} while (0)
-
-#define	STAILQ_INSERT_TAIL(head, elm, field) do {			\
-	STAILQ_NEXT((elm), field) = NULL;				\
-	*(head)->stqh_last = (elm);					\
-	(head)->stqh_last = &STAILQ_NEXT((elm), field);			\
-} while (0)
-
-#define	STAILQ_LAST(head, type, field)				\
-	(STAILQ_EMPTY((head)) ? NULL :				\
-	    __containerof((head)->stqh_last,			\
-	    QUEUE_TYPEOF(type), field.stqe_next))
-
-#define	STAILQ_NEXT(elm, field)	((elm)->field.stqe_next)
-
-#define	STAILQ_REMOVE(head, elm, type, field) do {			\
-	QMD_SAVELINK(oldnext, (elm)->field.stqe_next);			\
-	if (STAILQ_FIRST((head)) == (elm)) {				\
-		STAILQ_REMOVE_HEAD((head), field);			\
-	}								\
-	else {								\
-		QUEUE_TYPEOF(type) *curelm = STAILQ_FIRST(head);	\
-		while (STAILQ_NEXT(curelm, field) != (elm))		\
-			curelm = STAILQ_NEXT(curelm, field);		\
-		STAILQ_REMOVE_AFTER(head, curelm, field);		\
-	}								\
-	TRASHIT(*oldnext);						\
-} while (0)
-
-#define STAILQ_REMOVE_AFTER(head, elm, field) do {			\
-	if ((STAILQ_NEXT(elm, field) =					\
-	     STAILQ_NEXT(STAILQ_NEXT(elm, field), field)) == NULL)	\
-		(head)->stqh_last = &STAILQ_NEXT((elm), field);		\
-} while (0)
-
-#define	STAILQ_REMOVE_HEAD(head, field) do {				\
-	if ((STAILQ_FIRST((head)) =					\
-	     STAILQ_NEXT(STAILQ_FIRST((head)), field)) == NULL)		\
-		(head)->stqh_last = &STAILQ_FIRST((head));		\
-} while (0)
-
-#define STAILQ_SWAP(head1, head2, type) do {				\
-	QUEUE_TYPEOF(type) *swap_first = STAILQ_FIRST(head1);		\
-	QUEUE_TYPEOF(type) **swap_last = (head1)->stqh_last;		\
-	STAILQ_FIRST(head1) = STAILQ_FIRST(head2);			\
-	(head1)->stqh_last = (head2)->stqh_last;			\
-	STAILQ_FIRST(head2) = swap_first;				\
-	(head2)->stqh_last = swap_last;					\
-	if (STAILQ_EMPTY(head1))					\
-		(head1)->stqh_last = &STAILQ_FIRST(head1);		\
-	if (STAILQ_EMPTY(head2))					\
-		(head2)->stqh_last = &STAILQ_FIRST(head2);		\
-} while (0)
-
-
-/*
- * List declarations.
- */
-#define	LIST_HEAD(name, type)						\
-struct name {								\
-	struct type *lh_first;	/* first element */			\
-}
-
-#define	LIST_CLASS_HEAD(name, type)					\
-struct name {								\
-	class type *lh_first;	/* first element */			\
-}
-
-#define	LIST_HEAD_INITIALIZER(head)					\
-	{ NULL }
-
-#define	LIST_ENTRY(type)						\
-struct {								\
-	struct type *le_next;	/* next element */			\
-	struct type **le_prev;	/* address of previous next element */	\
-}
-
-#define	LIST_CLASS_ENTRY(type)						\
-struct {								\
-	class type *le_next;	/* next element */			\
-	class type **le_prev;	/* address of previous next element */	\
-}
-
-/*
- * List functions.
- */
-
-#if (defined(_KERNEL) && defined(INVARIANTS))
-/*
- * QMD_LIST_CHECK_HEAD(LIST_HEAD *head, LIST_ENTRY NAME)
- *
- * If the list is non-empty, validates that the first element of the list
- * points back at 'head.'
- */
-#define	QMD_LIST_CHECK_HEAD(head, field) do {				\
-	if (LIST_FIRST((head)) != NULL &&				\
-	    LIST_FIRST((head))->field.le_prev !=			\
-	     &LIST_FIRST((head)))					\
-		panic("Bad list head %p first->prev != head", (head));	\
-} while (0)
-
-/*
- * QMD_LIST_CHECK_NEXT(TYPE *elm, LIST_ENTRY NAME)
- *
- * If an element follows 'elm' in the list, validates that the next element
- * points back at 'elm.'
- */
-#define	QMD_LIST_CHECK_NEXT(elm, field) do {				\
-	if (LIST_NEXT((elm), field) != NULL &&				\
-	    LIST_NEXT((elm), field)->field.le_prev !=			\
-	     &((elm)->field.le_next))					\
-	     	panic("Bad link elm %p next->prev != elm", (elm));	\
-} while (0)
-
-/*
- * QMD_LIST_CHECK_PREV(TYPE *elm, LIST_ENTRY NAME)
- *
- * Validates that the previous element (or head of the list) points to 'elm.'
- */
-#define	QMD_LIST_CHECK_PREV(elm, field) do {				\
-	if (*(elm)->field.le_prev != (elm))				\
-		panic("Bad link elm %p prev->next != elm", (elm));	\
-} while (0)
-#else
-#define	QMD_LIST_CHECK_HEAD(head, field)
-#define	QMD_LIST_CHECK_NEXT(elm, field)
-#define	QMD_LIST_CHECK_PREV(elm, field)
-#endif /* (_KERNEL && INVARIANTS) */
-
-#define LIST_CONCAT(head1, head2, type, field) do {			      \
-	QUEUE_TYPEOF(type) *curelm = LIST_FIRST(head1);			      \
-	if (curelm == NULL) {						      \
-		if ((LIST_FIRST(head1) = LIST_FIRST(head2)) != NULL) {	      \
-			LIST_FIRST(head2)->field.le_prev =		      \
-			    &LIST_FIRST((head1));			      \
-			LIST_INIT(head2);				      \
-		}							      \
-	} else if (LIST_FIRST(head2) != NULL) {				      \
-		while (LIST_NEXT(curelm, field) != NULL)		      \
-			curelm = LIST_NEXT(curelm, field);		      \
-		LIST_NEXT(curelm, field) = LIST_FIRST(head2);		      \
-		LIST_FIRST(head2)->field.le_prev = &LIST_NEXT(curelm, field); \
-		LIST_INIT(head2);					      \
-	}								      \
-} while (0)
-
-#define	LIST_EMPTY(head)	((head)->lh_first == NULL)
-
-#define	LIST_FIRST(head)	((head)->lh_first)
-
-#define	LIST_FOREACH(var, head, field)					\
-	for ((var) = LIST_FIRST((head));				\
-	    (var);							\
-	    (var) = LIST_NEXT((var), field))
-
-#define	LIST_FOREACH_FROM(var, head, field)				\
-	for ((var) = ((var) ? (var) : LIST_FIRST((head)));		\
-	    (var);							\
-	    (var) = LIST_NEXT((var), field))
-
-#define	LIST_FOREACH_SAFE(var, head, field, tvar)			\
-	for ((var) = LIST_FIRST((head));				\
-	    (var) && ((tvar) = LIST_NEXT((var), field), 1);		\
-	    (var) = (tvar))
-
-#define	LIST_FOREACH_FROM_SAFE(var, head, field, tvar)			\
-	for ((var) = ((var) ? (var) : LIST_FIRST((head)));		\
-	    (var) && ((tvar) = LIST_NEXT((var), field), 1);		\
-	    (var) = (tvar))
-
-#define	LIST_INIT(head) do {						\
-	LIST_FIRST((head)) = NULL;					\
-} while (0)
-
-#define	LIST_INSERT_AFTER(listelm, elm, field) do {			\
-	QMD_LIST_CHECK_NEXT(listelm, field);				\
-	if ((LIST_NEXT((elm), field) = LIST_NEXT((listelm), field)) != NULL)\
-		LIST_NEXT((listelm), field)->field.le_prev =		\
-		    &LIST_NEXT((elm), field);				\
-	LIST_NEXT((listelm), field) = (elm);				\
-	(elm)->field.le_prev = &LIST_NEXT((listelm), field);		\
-} while (0)
-
-#define	LIST_INSERT_BEFORE(listelm, elm, field) do {			\
-	QMD_LIST_CHECK_PREV(listelm, field);				\
-	(elm)->field.le_prev = (listelm)->field.le_prev;		\
-	LIST_NEXT((elm), field) = (listelm);				\
-	*(listelm)->field.le_prev = (elm);				\
-	(listelm)->field.le_prev = &LIST_NEXT((elm), field);		\
-} while (0)
-
-#define	LIST_INSERT_HEAD(head, elm, field) do {				\
-	QMD_LIST_CHECK_HEAD((head), field);				\
-	if ((LIST_NEXT((elm), field) = LIST_FIRST((head))) != NULL)	\
-		LIST_FIRST((head))->field.le_prev = &LIST_NEXT((elm), field);\
-	LIST_FIRST((head)) = (elm);					\
-	(elm)->field.le_prev = &LIST_FIRST((head));			\
-} while (0)
-
-#define	LIST_NEXT(elm, field)	((elm)->field.le_next)
-
-#define	LIST_PREV(elm, head, type, field)			\
-	((elm)->field.le_prev == &LIST_FIRST((head)) ? NULL :	\
-	    __containerof((elm)->field.le_prev,			\
-	    QUEUE_TYPEOF(type), field.le_next))
-
-#define	LIST_REMOVE(elm, field) do {					\
-	QMD_SAVELINK(oldnext, (elm)->field.le_next);			\
-	QMD_SAVELINK(oldprev, (elm)->field.le_prev);			\
-	QMD_LIST_CHECK_NEXT(elm, field);				\
-	QMD_LIST_CHECK_PREV(elm, field);				\
-	if (LIST_NEXT((elm), field) != NULL)				\
-		LIST_NEXT((elm), field)->field.le_prev = 		\
-		    (elm)->field.le_prev;				\
-	*(elm)->field.le_prev = LIST_NEXT((elm), field);		\
-	TRASHIT(*oldnext);						\
-	TRASHIT(*oldprev);						\
-} while (0)
-
-#define LIST_SWAP(head1, head2, type, field) do {			\
-	QUEUE_TYPEOF(type) *swap_tmp = LIST_FIRST(head1);		\
-	LIST_FIRST((head1)) = LIST_FIRST((head2));			\
-	LIST_FIRST((head2)) = swap_tmp;					\
-	if ((swap_tmp = LIST_FIRST((head1))) != NULL)			\
-		swap_tmp->field.le_prev = &LIST_FIRST((head1));		\
-	if ((swap_tmp = LIST_FIRST((head2))) != NULL)			\
-		swap_tmp->field.le_prev = &LIST_FIRST((head2));		\
-} while (0)
-
-/*
- * Tail queue declarations.
- */
-#define	TAILQ_HEAD(name, type)						\
-struct name {								\
-	struct type *tqh_first;	/* first element */			\
-	struct type **tqh_last;	/* addr of last next element */		\
-	TRACEBUF							\
-}
-
-#define	TAILQ_CLASS_HEAD(name, type)					\
-struct name {								\
-	class type *tqh_first;	/* first element */			\
-	class type **tqh_last;	/* addr of last next element */		\
-	TRACEBUF							\
-}
-
-#define	TAILQ_HEAD_INITIALIZER(head)					\
-	{ NULL, &(head).tqh_first, TRACEBUF_INITIALIZER }
-
-#define	TAILQ_ENTRY(type)						\
-struct {								\
-	struct type *tqe_next;	/* next element */			\
-	struct type **tqe_prev;	/* address of previous next element */	\
-	TRACEBUF							\
-}
-
-#define	TAILQ_CLASS_ENTRY(type)						\
-struct {								\
-	class type *tqe_next;	/* next element */			\
-	class type **tqe_prev;	/* address of previous next element */	\
-	TRACEBUF							\
-}
-
-/*
- * Tail queue functions.
- */
-#if (defined(_KERNEL) && defined(INVARIANTS))
-/*
- * QMD_TAILQ_CHECK_HEAD(TAILQ_HEAD *head, TAILQ_ENTRY NAME)
- *
- * If the tailq is non-empty, validates that the first element of the tailq
- * points back at 'head.'
- */
-#define	QMD_TAILQ_CHECK_HEAD(head, field) do {				\
-	if (!TAILQ_EMPTY(head) &&					\
-	    TAILQ_FIRST((head))->field.tqe_prev !=			\
-	     &TAILQ_FIRST((head)))					\
-		panic("Bad tailq head %p first->prev != head", (head));	\
-} while (0)
-
-/*
- * QMD_TAILQ_CHECK_TAIL(TAILQ_HEAD *head, TAILQ_ENTRY NAME)
- *
- * Validates that the tail of the tailq is a pointer to pointer to NULL.
- */
-#define	QMD_TAILQ_CHECK_TAIL(head, field) do {				\
-	if (*(head)->tqh_last != NULL)					\
-	    	panic("Bad tailq NEXT(%p->tqh_last) != NULL", (head)); 	\
-} while (0)
-
-/*
- * QMD_TAILQ_CHECK_NEXT(TYPE *elm, TAILQ_ENTRY NAME)
- *
- * If an element follows 'elm' in the tailq, validates that the next element
- * points back at 'elm.'
- */
-#define	QMD_TAILQ_CHECK_NEXT(elm, field) do {				\
-	if (TAILQ_NEXT((elm), field) != NULL &&				\
-	    TAILQ_NEXT((elm), field)->field.tqe_prev !=			\
-	     &((elm)->field.tqe_next))					\
-		panic("Bad link elm %p next->prev != elm", (elm));	\
-} while (0)
-
-/*
- * QMD_TAILQ_CHECK_PREV(TYPE *elm, TAILQ_ENTRY NAME)
- *
- * Validates that the previous element (or head of the tailq) points to 'elm.'
- */
-#define	QMD_TAILQ_CHECK_PREV(elm, field) do {				\
-	if (*(elm)->field.tqe_prev != (elm))				\
-		panic("Bad link elm %p prev->next != elm", (elm));	\
-} while (0)
-#else
-#define	QMD_TAILQ_CHECK_HEAD(head, field)
-#define	QMD_TAILQ_CHECK_TAIL(head, headname)
-#define	QMD_TAILQ_CHECK_NEXT(elm, field)
-#define	QMD_TAILQ_CHECK_PREV(elm, field)
-#endif /* (_KERNEL && INVARIANTS) */
-
-#define	TAILQ_CONCAT(head1, head2, field) do {				\
-	if (!TAILQ_EMPTY(head2)) {					\
-		*(head1)->tqh_last = (head2)->tqh_first;		\
-		(head2)->tqh_first->field.tqe_prev = (head1)->tqh_last;	\
-		(head1)->tqh_last = (head2)->tqh_last;			\
-		TAILQ_INIT((head2));					\
-		QMD_TRACE_HEAD(head1);					\
-		QMD_TRACE_HEAD(head2);					\
-	}								\
-} while (0)
-
-#define	TAILQ_EMPTY(head)	((head)->tqh_first == NULL)
-
-#define	TAILQ_FIRST(head)	((head)->tqh_first)
-
-#define	TAILQ_FOREACH(var, head, field)					\
-	for ((var) = TAILQ_FIRST((head));				\
-	    (var);							\
-	    (var) = TAILQ_NEXT((var), field))
-
-#define	TAILQ_FOREACH_FROM(var, head, field)				\
-	for ((var) = ((var) ? (var) : TAILQ_FIRST((head)));		\
-	    (var);							\
-	    (var) = TAILQ_NEXT((var), field))
-
-#define	TAILQ_FOREACH_SAFE(var, head, field, tvar)			\
-	for ((var) = TAILQ_FIRST((head));				\
-	    (var) && ((tvar) = TAILQ_NEXT((var), field), 1);		\
-	    (var) = (tvar))
-
-#define	TAILQ_FOREACH_FROM_SAFE(var, head, field, tvar)			\
-	for ((var) = ((var) ? (var) : TAILQ_FIRST((head)));		\
-	    (var) && ((tvar) = TAILQ_NEXT((var), field), 1);		\
-	    (var) = (tvar))
-
-#define	TAILQ_FOREACH_REVERSE(var, head, headname, field)		\
-	for ((var) = TAILQ_LAST((head), headname);			\
-	    (var);							\
-	    (var) = TAILQ_PREV((var), headname, field))
-
-#define	TAILQ_FOREACH_REVERSE_FROM(var, head, headname, field)		\
-	for ((var) = ((var) ? (var) : TAILQ_LAST((head), headname));	\
-	    (var);							\
-	    (var) = TAILQ_PREV((var), headname, field))
-
-#define	TAILQ_FOREACH_REVERSE_SAFE(var, head, headname, field, tvar)	\
-	for ((var) = TAILQ_LAST((head), headname);			\
-	    (var) && ((tvar) = TAILQ_PREV((var), headname, field), 1);	\
-	    (var) = (tvar))
-
-#define	TAILQ_FOREACH_REVERSE_FROM_SAFE(var, head, headname, field, tvar) \
-	for ((var) = ((var) ? (var) : TAILQ_LAST((head), headname));	\
-	    (var) && ((tvar) = TAILQ_PREV((var), headname, field), 1);	\
-	    (var) = (tvar))
-
-#define	TAILQ_INIT(head) do {						\
-	TAILQ_FIRST((head)) = NULL;					\
-	(head)->tqh_last = &TAILQ_FIRST((head));			\
-	QMD_TRACE_HEAD(head);						\
-} while (0)
-
-#define	TAILQ_INSERT_AFTER(head, listelm, elm, field) do {		\
-	QMD_TAILQ_CHECK_NEXT(listelm, field);				\
-	if ((TAILQ_NEXT((elm), field) = TAILQ_NEXT((listelm), field)) != NULL)\
-		TAILQ_NEXT((elm), field)->field.tqe_prev = 		\
-		    &TAILQ_NEXT((elm), field);				\
-	else {								\
-		(head)->tqh_last = &TAILQ_NEXT((elm), field);		\
-		QMD_TRACE_HEAD(head);					\
-	}								\
-	TAILQ_NEXT((listelm), field) = (elm);				\
-	(elm)->field.tqe_prev = &TAILQ_NEXT((listelm), field);		\
-	QMD_TRACE_ELEM(&(elm)->field);					\
-	QMD_TRACE_ELEM(&(listelm)->field);				\
-} while (0)
-
-#define	TAILQ_INSERT_BEFORE(listelm, elm, field) do {			\
-	QMD_TAILQ_CHECK_PREV(listelm, field);				\
-	(elm)->field.tqe_prev = (listelm)->field.tqe_prev;		\
-	TAILQ_NEXT((elm), field) = (listelm);				\
-	*(listelm)->field.tqe_prev = (elm);				\
-	(listelm)->field.tqe_prev = &TAILQ_NEXT((elm), field);		\
-	QMD_TRACE_ELEM(&(elm)->field);					\
-	QMD_TRACE_ELEM(&(listelm)->field);				\
-} while (0)
-
-#define	TAILQ_INSERT_HEAD(head, elm, field) do {			\
-	QMD_TAILQ_CHECK_HEAD(head, field);				\
-	if ((TAILQ_NEXT((elm), field) = TAILQ_FIRST((head))) != NULL)	\
-		TAILQ_FIRST((head))->field.tqe_prev =			\
-		    &TAILQ_NEXT((elm), field);				\
-	else								\
-		(head)->tqh_last = &TAILQ_NEXT((elm), field);		\
-	TAILQ_FIRST((head)) = (elm);					\
-	(elm)->field.tqe_prev = &TAILQ_FIRST((head));			\
-	QMD_TRACE_HEAD(head);						\
-	QMD_TRACE_ELEM(&(elm)->field);					\
-} while (0)
-
-#define	TAILQ_INSERT_TAIL(head, elm, field) do {			\
-	QMD_TAILQ_CHECK_TAIL(head, field);				\
-	TAILQ_NEXT((elm), field) = NULL;				\
-	(elm)->field.tqe_prev = (head)->tqh_last;			\
-	*(head)->tqh_last = (elm);					\
-	(head)->tqh_last = &TAILQ_NEXT((elm), field);			\
-	QMD_TRACE_HEAD(head);						\
-	QMD_TRACE_ELEM(&(elm)->field);					\
-} while (0)
-
-#define	TAILQ_LAST(head, headname)					\
-	(*(((struct headname *)((head)->tqh_last))->tqh_last))
-
-/*
- * The FAST function is fast in that it causes no data access other
- * then the access to the head. The standard LAST function above
- * will cause a data access of both the element you want and 
- * the previous element. FAST is very useful for instances when
- * you may want to prefetch the last data element.
- */
-#define	TAILQ_LAST_FAST(head, type, field)			\
-    (TAILQ_EMPTY(head) ? NULL : __containerof((head)->tqh_last, QUEUE_TYPEOF(type), field.tqe_next))
-
-#define	TAILQ_NEXT(elm, field) ((elm)->field.tqe_next)
-
-#define	TAILQ_PREV(elm, headname, field)				\
-	(*(((struct headname *)((elm)->field.tqe_prev))->tqh_last))
-
-#define	TAILQ_REMOVE(head, elm, field) do {				\
-	QMD_SAVELINK(oldnext, (elm)->field.tqe_next);			\
-	QMD_SAVELINK(oldprev, (elm)->field.tqe_prev);			\
-	QMD_TAILQ_CHECK_NEXT(elm, field);				\
-	QMD_TAILQ_CHECK_PREV(elm, field);				\
-	if ((TAILQ_NEXT((elm), field)) != NULL)				\
-		TAILQ_NEXT((elm), field)->field.tqe_prev = 		\
-		    (elm)->field.tqe_prev;				\
-	else {								\
-		(head)->tqh_last = (elm)->field.tqe_prev;		\
-		QMD_TRACE_HEAD(head);					\
-	}								\
-	*(elm)->field.tqe_prev = TAILQ_NEXT((elm), field);		\
-	TRASHIT(*oldnext);						\
-	TRASHIT(*oldprev);						\
-	QMD_TRACE_ELEM(&(elm)->field);					\
-} while (0)
-
-#define TAILQ_SWAP(head1, head2, type, field) do {			\
-	QUEUE_TYPEOF(type) *swap_first = (head1)->tqh_first;		\
-	QUEUE_TYPEOF(type) **swap_last = (head1)->tqh_last;		\
-	(head1)->tqh_first = (head2)->tqh_first;			\
-	(head1)->tqh_last = (head2)->tqh_last;				\
-	(head2)->tqh_first = swap_first;				\
-	(head2)->tqh_last = swap_last;					\
-	if ((swap_first = (head1)->tqh_first) != NULL)			\
-		swap_first->field.tqe_prev = &(head1)->tqh_first;	\
-	else								\
-		(head1)->tqh_last = &(head1)->tqh_first;		\
-	if ((swap_first = (head2)->tqh_first) != NULL)			\
-		swap_first->field.tqe_prev = &(head2)->tqh_first;	\
-	else								\
-		(head2)->tqh_last = &(head2)->tqh_first;		\
-} while (0)
-
-#endif /* !_SYS_QUEUE_H_ */
diff --git a/src/common/tusb_compiler.h b/src/common/tusb_compiler.h
index 505542078..22c8366ab 100644
--- a/src/common/tusb_compiler.h
+++ b/src/common/tusb_compiler.h
@@ -102,6 +102,7 @@
   #define TU_BSWAP32(u32) (__builtin_bswap32(u32))
 
 #elif defined(__ICCARM__)
+  #include <intrinsics.h>
   #define TU_ATTR_ALIGNED(Bytes)        __attribute__ ((aligned(Bytes)))
   #define TU_ATTR_SECTION(sec_name)     __attribute__ ((section(#sec_name)))
   #define TU_ATTR_PACKED                __attribute__ ((packed))
diff --git a/src/common/tusb_error.h b/src/common/tusb_error.h
index f600c4ae3..d7ad8c318 100644
--- a/src/common/tusb_error.h
+++ b/src/common/tusb_error.h
@@ -54,6 +54,7 @@
     ENTRY(TUSB_ERROR_FAILED                          )\
 
 /// \brief Error Code returned
+/// TODO obsolete and to be remove
 typedef enum
 {
   ERROR_TABLE(ERROR_ENUM)
diff --git a/src/common/tusb_fifo.c b/src/common/tusb_fifo.c
index f41cf9f6c..00f2434a7 100644
--- a/src/common/tusb_fifo.c
+++ b/src/common/tusb_fifo.c
@@ -30,6 +30,12 @@
 #include "osal/osal.h"
 #include "tusb_fifo.h"
 
+// Supress IAR warning
+// Warning[Pa082]: undefined behavior: the order of volatile accesses is undefined in this statement
+#if defined(__ICCARM__)
+#pragma diag_suppress = Pa082
+#endif
+
 // implement mutex lock and unlock
 #if CFG_FIFO_MUTEX
 
@@ -106,7 +112,7 @@ static void _ff_push_n(tu_fifo_t* f, void const * data, uint16_t n, uint16_t wRe
     memcpy(f->buffer + (wRel * f->item_size), data, nLin*f->item_size);
 
     // Write data wrapped around
-    memcpy(f->buffer, data + nLin*f->item_size, (n - nLin) * f->item_size);
+    memcpy(f->buffer, ((uint8_t const*) data) + nLin*f->item_size, (n - nLin) * f->item_size);
   }
 }
 
@@ -131,7 +137,7 @@ static void _ff_pull_n(tu_fifo_t* f, void * p_buffer, uint16_t n, uint16_t rRel)
     memcpy(p_buffer, f->buffer + (rRel * f->item_size), nLin*f->item_size);
 
     // Read data wrapped part
-    memcpy(p_buffer + nLin*f->item_size, f->buffer, (n - nLin) * f->item_size);
+    memcpy((uint8_t*)p_buffer + nLin*f->item_size, f->buffer, (n - nLin) * f->item_size);
   }
 }
 
@@ -599,6 +605,27 @@ bool tu_fifo_clear(tu_fifo_t *f)
 
 /******************************************************************************/
 /*!
+    @brief Change the fifo mode to overwritable or not overwritable
+
+    @param[in]  f
+                Pointer to the FIFO buffer to manipulate
+    @param[in]  overwritable
+                Overwritable mode the fifo is set to
+*/
+/******************************************************************************/
+bool tu_fifo_set_overwritable(tu_fifo_t *f, bool overwritable)
+{
+  tu_fifo_lock(f);
+
+  f->overwritable = overwritable;
+
+  tu_fifo_unlock(f);
+
+  return true;
+}
+
+/******************************************************************************/
+/*!
     @brief Advance write pointer - intended to be used in combination with DMA.
     It is possible to fill the FIFO by use of a DMA in circular mode. Within
     DMA ISRs you may update the write pointer to be able to read from the FIFO.
diff --git a/src/common/tusb_fifo.h b/src/common/tusb_fifo.h
index b87695743..b965386ab 100644
--- a/src/common/tusb_fifo.h
+++ b/src/common/tusb_fifo.h
@@ -89,6 +89,7 @@ typedef struct
         .non_used_index_space   = 0xFFFF - 2*_depth-1,                  \
     }
 
+bool tu_fifo_set_overwritable(tu_fifo_t *f, bool overwritable);
 bool tu_fifo_clear(tu_fifo_t *f);
 bool tu_fifo_config(tu_fifo_t *f, void* buffer, uint16_t depth, uint16_t item_size, bool overwritable);
 
diff --git a/src/common/tusb_types.h b/src/common/tusb_types.h
index d6b6ea627..5ab15aa3c 100644
--- a/src/common/tusb_types.h
+++ b/src/common/tusb_types.h
@@ -250,6 +250,13 @@ typedef enum
   MS_OS_20_FEATURE_VENDOR_REVISION     = 0x08
 } microsoft_os_20_type_t;
 
+enum
+{
+  CONTROL_STAGE_SETUP,
+  CONTROL_STAGE_DATA,
+  CONTROL_STAGE_ACK
+};
+
 //--------------------------------------------------------------------+
 // USB Descriptors
 //--------------------------------------------------------------------+
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 90edc3dde..f6b00563d 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -93,131 +93,121 @@ static usbd_class_driver_t const _usbd_driver[] =
 {
   #if CFG_TUD_CDC
   {
-      DRIVER_NAME("CDC")
-      .init             = cdcd_init,
-      .reset            = cdcd_reset,
-      .open             = cdcd_open,
-      .control_request  = cdcd_control_request,
-      .control_complete = cdcd_control_complete,
-      .xfer_cb          = cdcd_xfer_cb,
-      .sof              = NULL
+    DRIVER_NAME("CDC")
+    .init             = cdcd_init,
+    .reset            = cdcd_reset,
+    .open             = cdcd_open,
+    .control_xfer_cb  = cdcd_control_xfer_cb,
+    .xfer_cb          = cdcd_xfer_cb,
+    .sof              = NULL
   },
   #endif
 
   #if CFG_TUD_MSC
   {
-      DRIVER_NAME("MSC")
-      .init             = mscd_init,
-      .reset            = mscd_reset,
-      .open             = mscd_open,
-      .control_request  = mscd_control_request,
-      .control_complete = mscd_control_complete,
-      .xfer_cb          = mscd_xfer_cb,
-      .sof              = NULL
+    DRIVER_NAME("MSC")
+    .init             = mscd_init,
+    .reset            = mscd_reset,
+    .open             = mscd_open,
+    .control_xfer_cb  = mscd_control_xfer_cb,
+    .xfer_cb          = mscd_xfer_cb,
+    .sof              = NULL
   },
   #endif
 
   #if CFG_TUD_HID
   {
-      DRIVER_NAME("HID")
-      .init             = hidd_init,
-      .reset            = hidd_reset,
-      .open             = hidd_open,
-      .control_request  = hidd_control_request,
-      .control_complete = hidd_control_complete,
-      .xfer_cb          = hidd_xfer_cb,
-      .sof              = NULL
+    DRIVER_NAME("HID")
+    .init             = hidd_init,
+    .reset            = hidd_reset,
+    .open             = hidd_open,
+    .control_xfer_cb  = hidd_control_xfer_cb,
+    .xfer_cb          = hidd_xfer_cb,
+    .sof              = NULL
   },
   #endif
 
-#if CFG_TUD_AUDIO
-{
-	DRIVER_NAME("AUDIO")
+  #if CFG_TUD_AUDIO
+  {
+    DRIVER_NAME("AUDIO")
     .init             = audiod_init,
-	.reset            = audiod_reset,
+    .reset            = audiod_reset,
     .open             = audiod_open,
-    .control_request  = audiod_control_request,
-    .control_complete = audiod_control_complete,
+    .control_xfer_cb  = audiod_control_xfer_cb,
     .xfer_cb          = audiod_xfer_cb,
     .sof              = NULL
-},
-#endif
+  },
+  #endif
 
   #if CFG_TUD_MIDI
   {
-      DRIVER_NAME("MIDI")
-      .init             = midid_init,
-      .open             = midid_open,
-      .reset            = midid_reset,
-      .control_request  = midid_control_request,
-      .control_complete = midid_control_complete,
-      .xfer_cb          = midid_xfer_cb,
-      .sof              = NULL
+    DRIVER_NAME("MIDI")
+    .init             = midid_init,
+    .open             = midid_open,
+    .reset            = midid_reset,
+    .control_xfer_cb  = midid_control_xfer_cb,
+    .xfer_cb          = midid_xfer_cb,
+    .sof              = NULL
   },
   #endif
 
   #if CFG_TUD_VENDOR
   {
-      DRIVER_NAME("VENDOR")
-      .init             = vendord_init,
-      .reset            = vendord_reset,
-      .open             = vendord_open,
-      .control_request  = tud_vendor_control_request_cb,
-      .control_complete = tud_vendor_control_complete_cb,
-      .xfer_cb          = vendord_xfer_cb,
-      .sof              = NULL
+    DRIVER_NAME("VENDOR")
+    .init             = vendord_init,
+    .reset            = vendord_reset,
+    .open             = vendord_open,
+    .control_xfer_cb  = tud_vendor_control_xfer_cb,
+    .xfer_cb          = vendord_xfer_cb,
+    .sof              = NULL
   },
   #endif
 
   #if CFG_TUD_USBTMC
   {
-      DRIVER_NAME("TMC")
-      .init             = usbtmcd_init_cb,
-      .reset            = usbtmcd_reset_cb,
-      .open             = usbtmcd_open_cb,
-      .control_request  = usbtmcd_control_request_cb,
-      .control_complete = usbtmcd_control_complete_cb,
-      .xfer_cb          = usbtmcd_xfer_cb,
-      .sof              = NULL
+    DRIVER_NAME("TMC")
+    .init             = usbtmcd_init_cb,
+    .reset            = usbtmcd_reset_cb,
+    .open             = usbtmcd_open_cb,
+    .control_xfer_cb  = usbtmcd_control_xfer_cb,
+    .xfer_cb          = usbtmcd_xfer_cb,
+    .sof              = NULL
   },
   #endif
 
   #if CFG_TUD_DFU_RT
   {
-      DRIVER_NAME("DFU-RT")
-      .init             = dfu_rtd_init,
-      .reset            = dfu_rtd_reset,
-      .open             = dfu_rtd_open,
-      .control_request  = dfu_rtd_control_request,
-      .control_complete = dfu_rtd_control_complete,
-      .xfer_cb          = dfu_rtd_xfer_cb,
-      .sof              = NULL
+    DRIVER_NAME("DFU-RT")
+    .init             = dfu_rtd_init,
+    .reset            = dfu_rtd_reset,
+    .open             = dfu_rtd_open,
+    .control_xfer_cb  = dfu_rtd_control_xfer_cb,
+    .xfer_cb          = dfu_rtd_xfer_cb,
+    .sof              = NULL
   },
   #endif
 
   #if CFG_TUD_NET
   {
-      DRIVER_NAME("NET")
-      .init             = netd_init,
-      .reset            = netd_reset,
-      .open             = netd_open,
-      .control_request  = netd_control_request,
-      .control_complete = netd_control_complete,
-      .xfer_cb          = netd_xfer_cb,
-      .sof              = NULL,
+    DRIVER_NAME("NET")
+    .init             = netd_init,
+    .reset            = netd_reset,
+    .open             = netd_open,
+    .control_xfer_cb  = netd_control_xfer_cb,
+    .xfer_cb          = netd_xfer_cb,
+    .sof              = NULL,
   },
   #endif
 
   #if CFG_TUD_BTH
   {
-      DRIVER_NAME("BTH")
-      .init             = btd_init,
-      .reset            = btd_reset,
-      .open             = btd_open,
-      .control_request  = btd_control_request,
-      .control_complete = btd_control_complete,
-      .xfer_cb          = btd_xfer_cb,
-      .sof              = NULL
+    DRIVER_NAME("BTH")
+    .init             = btd_init,
+    .reset            = btd_reset,
+    .open             = btd_open,
+    .control_xfer_cb  = btd_control_xfer_cb,
+    .xfer_cb          = btd_xfer_cb,
+    .sof              = NULL
   },
   #endif
 };
@@ -274,7 +264,7 @@ static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const
 // from usbd_control.c
 void usbd_control_reset(void);
 void usbd_control_set_request(tusb_control_request_t const *request);
-void usbd_control_set_complete_callback( bool (*fp) (uint8_t, tusb_control_request_t const * ) );
+void usbd_control_set_complete_callback( usbd_control_xfer_cb_t fp );
 bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
 
 
@@ -313,12 +303,12 @@ static char const* const _tusb_std_request_str[] =
 };
 
 // for usbd_control to print the name of control complete driver
-void usbd_driver_print_control_complete_name(bool (*control_complete) (uint8_t, tusb_control_request_t const * ))
+void usbd_driver_print_control_complete_name(usbd_control_xfer_cb_t callback)
 {
   for (uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++)
   {
     usbd_class_driver_t const * driver = get_driver(i);
-    if ( driver->control_complete == control_complete )
+    if ( driver->control_xfer_cb == callback )
     {
       TU_LOG2("  %s control complete\r\n", driver->name);
       return;
@@ -336,9 +326,14 @@ tusb_speed_t tud_speed_get(void)
   return (tusb_speed_t) _usbd_dev.speed;
 }
 
+bool tud_connected(void)
+{
+  return _usbd_dev.connected;
+}
+
 bool tud_mounted(void)
 {
-  return _usbd_dev.cfg_num ? 1 : 0;
+  return _usbd_dev.cfg_num ? true : false;
 }
 
 bool tud_suspended(void)
@@ -522,7 +517,7 @@ void tud_task (void)
           TU_ASSERT(driver, );
 
           TU_LOG2("  %s xfer callback\r\n", driver->name);
-          driver->xfer_cb(event.rhport, ep_addr, event.xfer_complete.result, event.xfer_complete.len);
+          driver->xfer_cb(event.rhport, ep_addr, (xfer_result_t)event.xfer_complete.result, event.xfer_complete.len);
         }
       }
       break;
@@ -565,9 +560,9 @@ void tud_task (void)
 // Helper to invoke class driver control request handler
 static bool invoke_class_control(uint8_t rhport, usbd_class_driver_t const * driver, tusb_control_request_t const * request)
 {
-  usbd_control_set_complete_callback(driver->control_complete);
+  usbd_control_set_complete_callback(driver->control_xfer_cb);
   TU_LOG2("  %s control request\r\n", driver->name);
-  return driver->control_request(rhport, request);
+  return driver->control_xfer_cb(rhport, CONTROL_STAGE_SETUP, request);
 }
 
 // This handles the actual request and its response.
@@ -581,10 +576,10 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
   // Vendor request
   if ( p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_VENDOR )
   {
-    TU_VERIFY(tud_vendor_control_request_cb);
+    TU_VERIFY(tud_vendor_control_xfer_cb);
 
-    if (tud_vendor_control_complete_cb) usbd_control_set_complete_callback(tud_vendor_control_complete_cb);
-    return tud_vendor_control_request_cb(rhport, p_request);
+    usbd_control_set_complete_callback(tud_vendor_control_xfer_cb);
+    return tud_vendor_control_xfer_cb(rhport, CONTROL_STAGE_SETUP, p_request);
   }
 
 #if CFG_TUSB_DEBUG >= 2
@@ -938,6 +933,7 @@ static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const
     break;
 
     case TUSB_DESC_STRING:
+    {
       TU_LOG2(" String[%u]\r\n", desc_index);
 
       // String Descriptor always uses the desc set from user
@@ -946,6 +942,7 @@ static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const
 
       // first byte of descriptor is its size
       return tud_control_xfer(rhport, p_request, (void*) desc_str, desc_str[0]);
+    }
     break;
 
     case TUSB_DESC_DEVICE_QUALIFIER:
@@ -1106,6 +1103,24 @@ bool usbd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * desc_ep)
 {
   TU_LOG2("  Open EP %02X with Size = %u\r\n", desc_ep->bEndpointAddress, desc_ep->wMaxPacketSize.size);
 
+  if (TUSB_XFER_ISOCHRONOUS == desc_ep->bmAttributes.xfer)
+  {
+    TU_ASSERT(desc_ep->wMaxPacketSize.size <= (_usbd_dev.speed == TUSB_SPEED_HIGH ? 1024 : 1023));
+  }
+  else
+  {
+    uint16_t const max_epsize = (_usbd_dev.speed == TUSB_SPEED_HIGH ? 512 : 64);
+
+    if (TUSB_XFER_BULK == desc_ep->bmAttributes.xfer)
+    {
+      // Bulk must be EXACTLY 512/64 bytes
+      TU_ASSERT(desc_ep->wMaxPacketSize.size == max_epsize);
+    }else
+    {
+      TU_ASSERT(desc_ep->wMaxPacketSize.size <= max_epsize);
+    }
+  }
+
   return dcd_edpt_open(rhport, desc_ep);
 }
 
diff --git a/src/device/usbd.h b/src/device/usbd.h
index e88f64ba8..360567743 100644
--- a/src/device/usbd.h
+++ b/src/device/usbd.h
@@ -56,6 +56,9 @@ extern void dcd_int_handler(uint8_t rhport);
 // Get current bus speed
 tusb_speed_t tud_speed_get(void);
 
+// Check if device is connected (may not mounted/configured yet)
+bool tud_connected(void);
+
 // Check if device is connected and configured
 bool tud_mounted(void);
 
@@ -125,11 +128,7 @@ TU_ATTR_WEAK void tud_suspend_cb(bool remote_wakeup_en);
 TU_ATTR_WEAK void tud_resume_cb(void);
 
 // Invoked when received control request with VENDOR TYPE
-TU_ATTR_WEAK bool tud_vendor_control_request_cb(uint8_t rhport, tusb_control_request_t const * request);
-
-// Invoked when vendor control request is complete
-TU_ATTR_WEAK bool tud_vendor_control_complete_cb(uint8_t rhport, tusb_control_request_t const * request);
-
+TU_ATTR_WEAK bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
 
 //--------------------------------------------------------------------+
 // Binary Device Object Store (BOS) Descriptor Templates
diff --git a/src/device/usbd_control.c b/src/device/usbd_control.c
index db0eb70a7..8ed0ad1c0 100644
--- a/src/device/usbd_control.c
+++ b/src/device/usbd_control.c
@@ -33,7 +33,7 @@
 #include "dcd.h"
 
 #if CFG_TUSB_DEBUG >= 2
-extern void usbd_driver_print_control_complete_name(bool (*control_complete) (uint8_t, tusb_control_request_t const *));
+extern void usbd_driver_print_control_complete_name(usbd_control_xfer_cb_t callback);
 #endif
 
 enum
@@ -50,7 +50,7 @@ typedef struct
   uint16_t data_len;
   uint16_t total_xferred;
 
-  bool (*complete_cb) (uint8_t, tusb_control_request_t const *);
+  usbd_control_xfer_cb_t complete_cb;
 } usbd_control_xfer_t;
 
 static usbd_control_xfer_t _ctrl_xfer;
@@ -140,13 +140,21 @@ bool tud_control_xfer(uint8_t rhport, tusb_control_request_t const * request, vo
 // USBD API
 //--------------------------------------------------------------------+
 
+//--------------------------------------------------------------------+
+// Prototypes
+//--------------------------------------------------------------------+
+void usbd_control_reset(void);
+void usbd_control_set_request(tusb_control_request_t const *request);
+void usbd_control_set_complete_callback( usbd_control_xfer_cb_t fp );
+bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
+
 void usbd_control_reset(void)
 {
   tu_varclr(&_ctrl_xfer);
 }
 
 // TODO may find a better way
-void usbd_control_set_complete_callback( bool (*fp) (uint8_t, tusb_control_request_t const * ) )
+void usbd_control_set_complete_callback( usbd_control_xfer_cb_t fp )
 {
   _ctrl_xfer.complete_cb = fp;
 }
@@ -171,7 +179,16 @@ bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t result
   if ( tu_edpt_dir(ep_addr) != _ctrl_xfer.request.bmRequestType_bit.direction )
   {
     TU_ASSERT(0 == xferred_bytes);
+
+    // invoke optional dcd hook if available
     if (dcd_edpt0_status_complete) dcd_edpt0_status_complete(rhport, &_ctrl_xfer.request);
+
+    if (_ctrl_xfer.complete_cb)
+    {
+      // TODO refactor with usbd_driver_print_control_complete_name
+      _ctrl_xfer.complete_cb(rhport, CONTROL_STAGE_ACK, &_ctrl_xfer.request);
+    }
+
     return true;
   }
 
@@ -199,7 +216,7 @@ bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t result
       usbd_driver_print_control_complete_name(_ctrl_xfer.complete_cb);
       #endif
 
-      is_ok = _ctrl_xfer.complete_cb(rhport, &_ctrl_xfer.request);
+      is_ok = _ctrl_xfer.complete_cb(rhport, CONTROL_STAGE_DATA, &_ctrl_xfer.request);
     }
 
     if ( is_ok )
diff --git a/src/device/usbd_pvt.h b/src/device/usbd_pvt.h
index 09b285581..212c3a202 100644
--- a/src/device/usbd_pvt.h
+++ b/src/device/usbd_pvt.h
@@ -46,8 +46,7 @@ typedef struct
   void     (* init             ) (void);
   void     (* reset            ) (uint8_t rhport);
   uint16_t (* open             ) (uint8_t rhport, tusb_desc_interface_t const * desc_intf, uint16_t max_len);
-  bool     (* control_request  ) (uint8_t rhport, tusb_control_request_t const * request);
-  bool     (* control_complete ) (uint8_t rhport, tusb_control_request_t const * request);
+  bool     (* control_xfer_cb  ) (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
   bool     (* xfer_cb          ) (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
   void     (* sof              ) (uint8_t rhport); /* optional */
 } usbd_class_driver_t;
@@ -57,6 +56,9 @@ typedef struct
 // Note: The drivers array must be accessible at all time when stack is active
 usbd_class_driver_t const* usbd_app_driver_get_cb(uint8_t* driver_count) TU_ATTR_WEAK;
 
+
+typedef bool (*usbd_control_xfer_cb_t)(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
+
 //--------------------------------------------------------------------+
 // USBD Endpoint API
 //--------------------------------------------------------------------+
diff --git a/src/portable/dialog/da146xx/dcd_da146xx.c b/src/portable/dialog/da146xx/dcd_da146xx.c
index 42ba867d7..59f728d08 100644
--- a/src/portable/dialog/da146xx/dcd_da146xx.c
+++ b/src/portable/dialog/da146xx/dcd_da146xx.c
@@ -664,7 +664,7 @@ static void handle_bus_reset(void)
   (void)USB->USB_ALTEV_REG;
   _dcd.in_reset = true;
 
-  dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+  dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
   USB->USB_DMA_CTRL_REG = 0;
 
   USB->USB_MAMSK_REG = USB_USB_MAMSK_REG_USB_M_INTR_Msk |
@@ -821,14 +821,13 @@ void dcd_disconnect(uint8_t rhport)
 
 bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
 {
+  (void)rhport;
+
   uint8_t const epnum = tu_edpt_number(desc_edpt->bEndpointAddress);
   uint8_t const dir   = tu_edpt_dir(desc_edpt->bEndpointAddress);
   xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir);
   uint8_t iso_mask = 0;
-  
-  (void)rhport;
 
-  TU_ASSERT(desc_edpt->wMaxPacketSize.size <= 1023);
   TU_ASSERT(epnum < EP_MAX);
 
   xfer->max_packet_size = desc_edpt->wMaxPacketSize.size;
diff --git a/src/portable/espressif/esp32s2/dcd_esp32s2.c b/src/portable/espressif/esp32s2/dcd_esp32s2.c
index 58d92e728..703841759 100644
--- a/src/portable/espressif/esp32s2/dcd_esp32s2.c
+++ b/src/portable/espressif/esp32s2/dcd_esp32s2.c
@@ -218,6 +218,9 @@ void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
 void dcd_remote_wakeup(uint8_t rhport)
 {
   (void)rhport;
+
+  // TODO must manually clear this bit after 1-15 ms
+  // USB0.DCTL |= USB_RMTWKUPSIG_M;
 }
 
 // connect by enabling internal pull-up resistor on D+/D-
@@ -249,7 +252,6 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *desc_edpt)
   uint8_t const epnum = tu_edpt_number(desc_edpt->bEndpointAddress);
   uint8_t const dir = tu_edpt_dir(desc_edpt->bEndpointAddress);
 
-  TU_ASSERT(desc_edpt->wMaxPacketSize.size <= 64);
   TU_ASSERT(epnum < EP_MAX);
 
   xfer_ctl_t *xfer = XFER_CTL_BASE(epnum, dir);
@@ -670,6 +672,7 @@ static void handle_epin_ints(void)
 static void _dcd_int_handler(void* arg)
 {
   (void) arg;
+  uint8_t const rhport = 0;
 
   const uint32_t int_status = USB0.gintsts;
   //const uint32_t int_msk = USB0.gintmsk;
@@ -695,7 +698,19 @@ static void _dcd_int_handler(void* arg)
     // the end of reset.
     USB0.gintsts = USB_ENUMDONE_M;
     enum_done_processing();
-    dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+    dcd_event_bus_reset(rhport, TUSB_SPEED_FULL, true);
+  }
+
+  if(int_status & USB_USBSUSP_M)
+  {
+    USB0.gintsts = USB_USBSUSP_M;
+    dcd_event_bus_signal(rhport, DCD_EVENT_SUSPEND, true);
+  }
+
+  if(int_status & USB_WKUPINT_M)
+  {
+    USB0.gintsts = USB_WKUPINT_M;
+    dcd_event_bus_signal(rhport, DCD_EVENT_RESUME, true);
   }
 
   if (int_status & USB_OTGINT_M)
@@ -707,7 +722,7 @@ static void _dcd_int_handler(void* arg)
 
     if (otg_int & USB_SESENDDET_M)
     {
-      dcd_event_bus_signal(0, DCD_EVENT_UNPLUGGED, true);
+      dcd_event_bus_signal(rhport, DCD_EVENT_UNPLUGGED, true);
     }
 
     USB0.gotgint = otg_int;
@@ -716,7 +731,7 @@ static void _dcd_int_handler(void* arg)
 #if USE_SOF
   if (int_status & USB_SOF_M) {
     USB0.gintsts = USB_SOF_M;
-    dcd_event_bus_signal(0, DCD_EVENT_SOF, true); // do nothing actually
+    dcd_event_bus_signal(rhport, DCD_EVENT_SOF, true); // do nothing actually
   }
 #endif
 
diff --git a/src/portable/microchip/samd/dcd_samd.c b/src/portable/microchip/samd/dcd_samd.c
index f1dca88a5..3cff92fcd 100644
--- a/src/portable/microchip/samd/dcd_samd.c
+++ b/src/portable/microchip/samd/dcd_samd.c
@@ -37,14 +37,23 @@
 /* MACRO TYPEDEF CONSTANT ENUM
  *------------------------------------------------------------------*/
 static TU_ATTR_ALIGNED(4) UsbDeviceDescBank sram_registers[8][2];
-static TU_ATTR_ALIGNED(4) uint8_t _setup_packet[8];
+
+// Setup packet is only 8 bytes in length. However under certain scenario,
+// USB DMA controller may decide to overwrite/overflow the buffer  with
+// 2 extra bytes of CRC. From datasheet's "Management of SETUP Transactions" section
+//    If the number of received data bytes is the maximum data payload specified by
+//    PCKSIZE.SIZE minus one, only the first CRC data is written to the data buffer.
+//    If the number of received data is equal or less than the data payload specified
+//    by PCKSIZE.SIZE minus two, both CRC data bytes are written to the data buffer.
+// Therefore we will need to increase it to 10 bytes here.
+static TU_ATTR_ALIGNED(4) uint8_t _setup_packet[8+2];
 
 // ready for receiving SETUP packet
 static inline void prepare_setup(void)
 {
   // Only make sure the EP0 OUT buffer is ready
   sram_registers[0][0].ADDR.reg = (uint32_t) _setup_packet;
-  sram_registers[0][0].PCKSIZE.bit.MULTI_PACKET_SIZE = sizeof(_setup_packet);
+  sram_registers[0][0].PCKSIZE.bit.MULTI_PACKET_SIZE = sizeof(tusb_control_request_t);
   sram_registers[0][0].PCKSIZE.bit.BYTE_COUNT = 0;
 }
 
@@ -378,7 +387,7 @@ void dcd_int_handler (uint8_t rhport)
     USB->DEVICE.INTENCLR.reg = USB_DEVICE_INTFLAG_WAKEUP | USB_DEVICE_INTFLAG_SUSPEND;
 
     bus_reset();
-    dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+    dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
   }
 
   // Handle SETUP packet
diff --git a/src/portable/microchip/samg/dcd_samg.c b/src/portable/microchip/samg/dcd_samg.c
index 2b64df50b..2f9f1097f 100644
--- a/src/portable/microchip/samg/dcd_samg.c
+++ b/src/portable/microchip/samg/dcd_samg.c
@@ -338,7 +338,7 @@ void dcd_int_handler(uint8_t rhport)
   if (intr_status & UDP_ISR_ENDBUSRES_Msk)
   {
     bus_reset();
-    dcd_event_bus_signal(rhport, DCD_EVENT_BUS_RESET, true);
+    dcd_event_bus_reset(rhport, TUSB_SPEED_FULL, true);
   }
 
   // SOF
diff --git a/src/portable/nordic/nrf5x/dcd_nrf5x.c b/src/portable/nordic/nrf5x/dcd_nrf5x.c
index b85d3776f..d9fe2b6ae 100644
--- a/src/portable/nordic/nrf5x/dcd_nrf5x.c
+++ b/src/portable/nordic/nrf5x/dcd_nrf5x.c
@@ -533,7 +533,7 @@ void dcd_int_handler(uint8_t rhport)
   if ( int_status & USBD_INTEN_USBRESET_Msk )
   {
     bus_reset();
-    dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+    dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
   }
 
   // ISOIN: Data was moved to endpoint buffer, client will be notified in SOF
diff --git a/src/portable/nuvoton/nuc120/dcd_nuc120.c b/src/portable/nuvoton/nuc120/dcd_nuc120.c
index 1a0007d8a..dc48e54cc 100644
--- a/src/portable/nuvoton/nuc120/dcd_nuc120.c
+++ b/src/portable/nuvoton/nuc120/dcd_nuc120.c
@@ -329,8 +329,7 @@ void dcd_int_handler(uint8_t rhport)
       USBD->ATTR |= USBD_ATTR_USB_EN_Msk | USBD_ATTR_PHY_EN_Msk;
 
       bus_reset();
-
-      dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+      dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
     }
 
     if(state & USBD_STATE_SUSPEND)
diff --git a/src/portable/nuvoton/nuc121/dcd_nuc121.c b/src/portable/nuvoton/nuc121/dcd_nuc121.c
index af7fee8b0..c9ead6de0 100644
--- a/src/portable/nuvoton/nuc121/dcd_nuc121.c
+++ b/src/portable/nuvoton/nuc121/dcd_nuc121.c
@@ -340,7 +340,7 @@ void dcd_int_handler(uint8_t rhport)
 
       bus_reset();
 
-      dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+      dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
     }
 
     if(state & USBD_ATTR_SUSPEND_Msk)
diff --git a/src/portable/nxp/khci/dcd_khci.c b/src/portable/nxp/khci/dcd_khci.c
new file mode 100644
index 000000000..519b8fb7b
--- /dev/null
+++ b/src/portable/nxp/khci/dcd_khci.c
@@ -0,0 +1,477 @@
+/* 
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2020 Koji Kitayama
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * This file is part of the TinyUSB stack.
+ */
+
+#include "tusb_option.h"
+
+#if TUSB_OPT_DEVICE_ENABLED && ( CFG_TUSB_MCU == OPT_MCU_MKL25ZXX )
+
+#include "fsl_device_registers.h"
+#define KHCI        USB0
+
+#include "device/dcd.h"
+
+//--------------------------------------------------------------------+
+// MACRO TYPEDEF CONSTANT ENUM DECLARATION
+//--------------------------------------------------------------------+
+
+enum {
+  TOK_PID_OUT   = 0x1u,
+  TOK_PID_IN    = 0x9u,
+  TOK_PID_SETUP = 0xDu,
+};
+
+typedef struct TU_ATTR_PACKED
+{
+  union {
+    uint32_t head;
+    struct {
+      union {
+        struct {
+          uint16_t          :  2;
+          uint16_t tok_pid  :  4;
+          uint16_t data     :  1;
+          uint16_t own      :  1;
+          uint16_t          :  8;
+        };
+        struct {
+          uint16_t          :  2;
+          uint16_t bdt_stall:  1;
+          uint16_t dts      :  1;
+          uint16_t ninc     :  1;
+          uint16_t keep     :  1;
+          uint16_t          : 10;
+        };
+      };
+      uint16_t bc          : 10;
+      uint16_t             :  6;
+    };
+  };
+  uint8_t *addr;
+}buffer_descriptor_t;
+
+TU_VERIFY_STATIC( sizeof(buffer_descriptor_t) == 8, "size is not correct" );
+
+typedef struct TU_ATTR_PACKED
+{
+  union {
+    uint32_t state;
+    struct {
+      uint32_t max_packet_size :11;
+      uint32_t                 : 5;
+      uint32_t odd             : 1;
+      uint32_t                 :15;
+    };
+  };
+  uint16_t length;
+  uint16_t remaining;
+}endpoint_state_t;
+
+TU_VERIFY_STATIC( sizeof(endpoint_state_t) == 8, "size is not correct" );
+
+typedef struct
+{
+  union {
+    /* [#EP][OUT,IN][EVEN,ODD] */
+    buffer_descriptor_t bdt[16][2][2];
+    uint16_t            bda[512];
+  };
+  TU_ATTR_ALIGNED(4) union {
+    endpoint_state_t endpoint[16][2];
+    endpoint_state_t endpoint_unified[16 * 2];
+  };
+  uint8_t setup_packet[8];
+  uint8_t addr;
+}dcd_data_t;
+
+//--------------------------------------------------------------------+
+// INTERNAL OBJECT & FUNCTION DECLARATION
+//--------------------------------------------------------------------+
+// BDT(Buffer Descriptor Table) must be 256-byte aligned
+CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(512) static dcd_data_t _dcd;
+
+TU_VERIFY_STATIC( sizeof(_dcd.bdt) == 512, "size is not correct" );
+
+static void prepare_next_setup_packet(uint8_t rhport)
+{
+  const unsigned out_odd = _dcd.endpoint[0][0].odd;
+  const unsigned in_odd  = _dcd.endpoint[0][1].odd;
+  if (_dcd.bdt[0][0][out_odd].own) {
+    TU_LOG1("DCD fail to prepare the next SETUP %d %d\r\n", out_odd, in_odd);
+    return;
+  }
+  _dcd.bdt[0][0][out_odd].data     = 0;
+  _dcd.bdt[0][0][out_odd ^ 1].data = 1;
+  _dcd.bdt[0][1][in_odd].data      = 1;
+  _dcd.bdt[0][1][in_odd ^ 1].data  = 0;
+  dcd_edpt_xfer(rhport, tu_edpt_addr(0, TUSB_DIR_OUT),
+                _dcd.setup_packet, sizeof(_dcd.setup_packet));
+}
+
+static void process_stall(uint8_t rhport)
+{
+  if (KHCI->ENDPOINT[0].ENDPT & USB_ENDPT_EPSTALL_MASK) {
+    /* clear stall condition of the control pipe */
+    prepare_next_setup_packet(rhport);
+    KHCI->ENDPOINT[0].ENDPT &= ~USB_ENDPT_EPSTALL_MASK;
+  }
+}
+
+static void process_tokdne(uint8_t rhport)
+{
+  const unsigned s = KHCI->STAT;
+  KHCI->ISTAT = USB_ISTAT_TOKDNE_MASK; /* fetch the next token if received */
+  buffer_descriptor_t *bd = (buffer_descriptor_t *)&_dcd.bda[s];
+  endpoint_state_t    *ep = &_dcd.endpoint_unified[s >> 3];
+  unsigned odd = (s & USB_STAT_ODD_MASK) ? 1 : 0;
+
+  /* fetch pid before discarded by the next steps */
+  const unsigned pid = bd->tok_pid;
+  /* reset values for a next transfer */
+  bd->bdt_stall = 0;
+  bd->dts       = 1;
+  bd->ninc      = 0;
+  bd->keep      = 0;
+  /* update the odd variable to prepare for the next transfer */
+  ep->odd       = odd ^ 1;
+  if (pid == TOK_PID_SETUP) {
+    dcd_event_setup_received(rhport, bd->addr, true);
+    KHCI->CTL &= ~USB_CTL_TXSUSPENDTOKENBUSY_MASK;
+    return;
+  }
+  if (s >> 4) {
+    TU_LOG1("TKDNE %x\r\n", s);
+  }
+
+  const unsigned bc = bd->bc;
+  const unsigned remaining = ep->remaining - bc;
+  if (remaining && bc == ep->max_packet_size) {
+    /* continue the transferring consecutive data */
+    ep->remaining = remaining;
+    const int next_remaining = remaining - ep->max_packet_size;
+    if (next_remaining > 0) {
+      /* prepare to the after next transfer */
+      bd->addr += ep->max_packet_size * 2;
+      bd->bc    = next_remaining > ep->max_packet_size ? ep->max_packet_size: next_remaining;
+      __DSB();
+      bd->own   = 1; /* the own bit must set after addr */
+    }
+    return;
+  }
+  const unsigned length = ep->length;
+  dcd_event_xfer_complete(rhport,
+                          ((s & USB_STAT_TX_MASK) << 4) | (s >> USB_STAT_ENDP_SHIFT),
+                          length - remaining, XFER_RESULT_SUCCESS, true);
+  if (0 == (s & USB_STAT_ENDP_MASK) && 0 == length) {
+    /* After completion a ZLP of control transfer,
+     * it prepares for the next steup transfer. */
+    if (_dcd.addr) {
+      /* When the transfer was the SetAddress,
+       * the device address should be updated here. */
+      KHCI->ADDR = _dcd.addr;
+      _dcd.addr  = 0;
+    }
+    prepare_next_setup_packet(rhport);
+  }
+}
+
+static void process_bus_reset(uint8_t rhport)
+{
+  KHCI->USBCTRL &= ~USB_USBCTRL_SUSP_MASK;
+  KHCI->CTL     |= USB_CTL_ODDRST_MASK;
+  KHCI->ADDR     = 0;
+  KHCI->INTEN    = (KHCI->INTEN & ~USB_INTEN_RESUMEEN_MASK) | USB_INTEN_SLEEPEN_MASK;
+
+  KHCI->ENDPOINT[0].ENDPT = USB_ENDPT_EPHSHK_MASK | USB_ENDPT_EPRXEN_MASK | USB_ENDPT_EPTXEN_MASK;
+  for (unsigned i = 1; i < 16; ++i) {
+    KHCI->ENDPOINT[i].ENDPT = 0;
+  }
+  buffer_descriptor_t *bd = _dcd.bdt[0][0];
+  for (unsigned i = 0; i < sizeof(_dcd.bdt)/sizeof(*bd); ++i, ++bd) {
+    bd->head = 0;
+  }
+  const endpoint_state_t ep0 = {
+    .max_packet_size = CFG_TUD_ENDPOINT0_SIZE,
+    .odd             = 0,
+    .length          = 0,
+    .remaining       = 0,
+  };
+  _dcd.endpoint[0][0] = ep0;
+  _dcd.endpoint[0][1] = ep0;
+  tu_memclr(_dcd.endpoint[1], sizeof(_dcd.endpoint) - sizeof(_dcd.endpoint[0]));
+  _dcd.addr = 0;
+  prepare_next_setup_packet(rhport);
+  KHCI->CTL &= ~USB_CTL_ODDRST_MASK;
+  dcd_event_bus_reset(rhport, TUSB_SPEED_FULL, true);
+}
+
+static void process_bus_inactive(uint8_t rhport)
+{
+  (void) rhport;
+  const unsigned inten = KHCI->INTEN;
+  KHCI->INTEN    = (inten & ~USB_INTEN_SLEEPEN_MASK) | USB_INTEN_RESUMEEN_MASK;
+  KHCI->USBCTRL |= USB_USBCTRL_SUSP_MASK;
+  dcd_event_bus_signal(rhport, DCD_EVENT_SUSPEND, true);
+}
+
+static void process_bus_active(uint8_t rhport)
+{
+  (void) rhport;
+  KHCI->USBCTRL &= ~USB_USBCTRL_SUSP_MASK;
+  const unsigned inten = KHCI->INTEN;
+  KHCI->INTEN    = (inten & ~USB_INTEN_RESUMEEN_MASK) | USB_INTEN_SLEEPEN_MASK;
+  dcd_event_bus_signal(rhport, DCD_EVENT_RESUME, true);
+}
+
+/*------------------------------------------------------------------*/
+/* Device API
+ *------------------------------------------------------------------*/
+void dcd_init(uint8_t rhport)
+{
+  (void) rhport;
+
+  KHCI->USBTRC0 |= USB_USBTRC0_USBRESET_MASK;
+  while (KHCI->USBTRC0 & USB_USBTRC0_USBRESET_MASK);
+  tu_memclr(&_dcd, sizeof(_dcd));
+  KHCI->USBTRC0 |= TU_BIT(6); /* software must set this bit to 1 */
+  KHCI->BDTPAGE1 = (uint8_t)((uintptr_t)_dcd.bdt >>  8);
+  KHCI->BDTPAGE2 = (uint8_t)((uintptr_t)_dcd.bdt >> 16);
+  KHCI->BDTPAGE3 = (uint8_t)((uintptr_t)_dcd.bdt >> 24);
+
+  dcd_connect(rhport);
+  NVIC_ClearPendingIRQ(USB0_IRQn);
+}
+
+void dcd_int_enable(uint8_t rhport)
+{
+  (void) rhport;
+  KHCI->INTEN = USB_INTEN_USBRSTEN_MASK | USB_INTEN_TOKDNEEN_MASK |
+    USB_INTEN_SLEEPEN_MASK | USB_INTEN_ERROREN_MASK | USB_INTEN_STALLEN_MASK;
+  NVIC_EnableIRQ(USB0_IRQn);
+}
+
+void dcd_int_disable(uint8_t rhport)
+{
+  (void) rhport;
+  NVIC_DisableIRQ(USB0_IRQn);
+  KHCI->INTEN = 0;
+}
+
+void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
+{
+  (void) rhport;
+  _dcd.addr = dev_addr & 0x7F;
+  /* Response with status first before changing device address */
+  dcd_edpt_xfer(rhport, tu_edpt_addr(0, TUSB_DIR_IN), NULL, 0);
+}
+
+void dcd_remote_wakeup(uint8_t rhport)
+{
+  (void) rhport;
+  unsigned cnt = SystemCoreClock / 100;
+  KHCI->CTL |= USB_CTL_RESUME_MASK;
+  while (cnt--) __NOP();
+  KHCI->CTL &= ~USB_CTL_RESUME_MASK;
+}
+
+void dcd_connect(uint8_t rhport)
+{
+  (void) rhport;
+  KHCI->USBCTRL  = 0;
+  KHCI->CONTROL |= USB_CONTROL_DPPULLUPNONOTG_MASK;
+  KHCI->CTL     |= USB_CTL_USBENSOFEN_MASK;
+}
+
+void dcd_disconnect(uint8_t rhport)
+{
+  (void) rhport;
+  KHCI->CTL      = 0;
+  KHCI->CONTROL &= ~USB_CONTROL_DPPULLUPNONOTG_MASK;
+}
+
+//--------------------------------------------------------------------+
+// Endpoint API
+//--------------------------------------------------------------------+
+bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * ep_desc)
+{
+  (void) rhport;
+
+  const unsigned ep_addr  = ep_desc->bEndpointAddress;
+  const unsigned epn      = ep_addr & 0xFu;
+  const unsigned dir      = (ep_addr & TUSB_DIR_IN_MASK) ? TUSB_DIR_IN : TUSB_DIR_OUT;
+  const unsigned xfer     = ep_desc->bmAttributes.xfer;
+  endpoint_state_t *ep    = &_dcd.endpoint[epn][dir];
+  const unsigned odd      = ep->odd;
+  buffer_descriptor_t *bd = &_dcd.bdt[epn][dir][0];
+
+  /* No support for control transfer */
+  TU_ASSERT(epn && (xfer != TUSB_XFER_CONTROL));
+
+  ep->max_packet_size = ep_desc->wMaxPacketSize.size;
+  unsigned val = USB_ENDPT_EPCTLDIS_MASK;
+  val |= (xfer != TUSB_XFER_ISOCHRONOUS) ? USB_ENDPT_EPHSHK_MASK: 0;
+  val |= dir ? USB_ENDPT_EPTXEN_MASK : USB_ENDPT_EPRXEN_MASK;
+  KHCI->ENDPOINT[epn].ENDPT |= val;
+
+  if (xfer != TUSB_XFER_ISOCHRONOUS) {
+    bd[odd].dts      = 1;
+    bd[odd].data     = 0;
+    bd[odd ^ 1].dts  = 1;
+    bd[odd ^ 1].data = 1;
+  }
+
+  return true;
+}
+
+void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+
+  const unsigned epn      = ep_addr & 0xFu;
+  const unsigned dir      = (ep_addr & TUSB_DIR_IN_MASK) ? TUSB_DIR_IN : TUSB_DIR_OUT;
+  endpoint_state_t *ep    = &_dcd.endpoint[epn][dir];
+  buffer_descriptor_t *bd = &_dcd.bdt[epn][dir][0];
+  const unsigned msk      = dir ? USB_ENDPT_EPTXEN_MASK : USB_ENDPT_EPRXEN_MASK;
+  KHCI->ENDPOINT[epn].ENDPT &= ~msk;
+  ep->max_packet_size = 0;
+  ep->length          = 0;
+  ep->remaining       = 0;
+  bd->head            = 0;
+}
+
+bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes)
+{
+  (void) rhport;
+  NVIC_DisableIRQ(USB0_IRQn);
+  const unsigned epn = ep_addr & 0xFu;
+  const unsigned dir = (ep_addr & TUSB_DIR_IN_MASK) ? TUSB_DIR_IN : TUSB_DIR_OUT;
+  endpoint_state_t    *ep = &_dcd.endpoint[epn][dir];
+  buffer_descriptor_t *bd = &_dcd.bdt[epn][dir][ep->odd];
+
+  if (bd->own) {
+    TU_LOG1("DCD XFER fail %x %d %lx %lx\r\n", ep_addr, total_bytes, ep->state, bd->head);
+    return false; /* The last transfer has not completed */
+  }
+  ep->length    = total_bytes;
+  ep->remaining = total_bytes;
+
+  const unsigned mps = ep->max_packet_size;
+  if (total_bytes > mps) {
+    buffer_descriptor_t *next = ep->odd ? bd - 1: bd + 1;
+    /* When total_bytes is greater than the max packet size,
+     * it prepares to the next transfer to avoid NAK in advance. */
+    next->bc   = total_bytes >= 2 * mps ? mps: total_bytes - mps;
+    next->addr = buffer + mps;
+    next->own  = 1;
+  }
+  bd->bc        = total_bytes >= mps ? mps: total_bytes;
+  bd->addr      = buffer;
+  __DSB();
+  bd->own  = 1; /* the own bit must set after addr */
+  NVIC_EnableIRQ(USB0_IRQn);
+  return true;
+}
+
+void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+  const unsigned epn = ep_addr & 0xFu;
+  if (0 == epn) {
+    KHCI->ENDPOINT[epn].ENDPT |=  USB_ENDPT_EPSTALL_MASK;
+  } else {
+    const unsigned dir      = (ep_addr & TUSB_DIR_IN_MASK) ? TUSB_DIR_IN : TUSB_DIR_OUT;
+    buffer_descriptor_t *bd = _dcd.bdt[epn][dir];
+    bd[0].bdt_stall = 1;
+    bd[1].bdt_stall = 1;
+  }
+}
+
+void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+  const unsigned epn      = ep_addr & 0xFu;
+  const unsigned dir      = (ep_addr & TUSB_DIR_IN_MASK) ? TUSB_DIR_IN : TUSB_DIR_OUT;
+  const unsigned odd      = _dcd.endpoint[epn][dir].odd;
+  buffer_descriptor_t *bd = _dcd.bdt[epn][dir];
+
+  bd[odd ^ 1].own       = 0;
+  bd[odd ^ 1].data      = 1;
+  bd[odd ^ 1].bdt_stall = 0;
+  bd[odd].own           = 0;
+  bd[odd].data          = 0;
+  bd[odd].bdt_stall     = 0;
+}
+
+//--------------------------------------------------------------------+
+// ISR
+//--------------------------------------------------------------------+
+void dcd_int_handler(uint8_t rhport)
+{
+  (void) rhport;
+
+  uint32_t is  = KHCI->ISTAT;
+  uint32_t msk = KHCI->INTEN;
+  KHCI->ISTAT = is & ~msk;
+  is &= msk;
+  if (is & USB_ISTAT_ERROR_MASK) {
+    /* TODO: */
+    uint32_t es = KHCI->ERRSTAT;
+    KHCI->ERRSTAT = es;
+    KHCI->ISTAT   = is; /* discard any pending events */
+    return;
+  }
+
+  if (is & USB_ISTAT_USBRST_MASK) {
+    KHCI->ISTAT = is; /* discard any pending events */
+    process_bus_reset(rhport);
+    return;
+  }
+  if (is & USB_ISTAT_SLEEP_MASK) {
+    KHCI->ISTAT = USB_ISTAT_SLEEP_MASK;
+    process_bus_inactive(rhport);
+    return;
+  }
+  if (is & USB_ISTAT_RESUME_MASK) {
+    KHCI->ISTAT = USB_ISTAT_RESUME_MASK;
+    process_bus_active(rhport);
+    return;
+  }
+  if (is & USB_ISTAT_SOFTOK_MASK) {
+    KHCI->ISTAT = USB_ISTAT_SOFTOK_MASK;
+    dcd_event_bus_signal(rhport, DCD_EVENT_SOF, true);
+    return;
+  }
+  if (is & USB_ISTAT_STALL_MASK) {
+    KHCI->ISTAT = USB_ISTAT_STALL_MASK;
+    process_stall(rhport);
+    return;
+  }
+  if (is & USB_ISTAT_TOKDNE_MASK) {
+    process_tokdne(rhport);
+    return;
+  }
+}
+
+#endif
diff --git a/src/portable/nxp/lpc17_40/dcd_lpc17_40.c b/src/portable/nxp/lpc17_40/dcd_lpc17_40.c
index 39c5e6638..7d867aaba 100644
--- a/src/portable/nxp/lpc17_40/dcd_lpc17_40.c
+++ b/src/portable/nxp/lpc17_40/dcd_lpc17_40.c
@@ -471,7 +471,7 @@ static void bus_event_isr(uint8_t rhport)
   if (dev_status & SIE_DEV_STATUS_RESET_MASK)
   {
     bus_reset();
-    dcd_event_bus_signal(rhport, DCD_EVENT_BUS_RESET, true);
+    dcd_event_bus_reset(rhport, TUSB_SPEED_FULL, true);
   }
 
   if (dev_status & SIE_DEV_STATUS_CONNECT_CHANGE_MASK)
diff --git a/src/portable/nxp/lpc_ip3511/dcd_lpc_ip3511.c b/src/portable/nxp/lpc_ip3511/dcd_lpc_ip3511.c
index 3d1420c65..e4d92458c 100644
--- a/src/portable/nxp/lpc_ip3511/dcd_lpc_ip3511.c
+++ b/src/portable/nxp/lpc_ip3511/dcd_lpc_ip3511.c
@@ -243,7 +243,7 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc)
   (void) rhport;
 
   // TODO not support ISO yet
-  if (p_endpoint_desc->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS) return false;
+  TU_VERIFY(p_endpoint_desc->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS);
 
   //------------- Prepare Queue Head -------------//
   uint8_t ep_id = ep_addr2id(p_endpoint_desc->bEndpointAddress);
@@ -357,7 +357,7 @@ void dcd_int_handler(uint8_t rhport)
     if ( cmd_stat & CMDSTAT_RESET_CHANGE_MASK) // bus reset
     {
       bus_reset();
-      dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+      dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
     }
 
     if (cmd_stat & CMDSTAT_CONNECT_CHANGE_MASK)
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index dc125a758..18f0bc8e1 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -547,7 +547,7 @@ void dcd_int_handler(uint8_t rhport) {
     // USBRST is start of reset.
     clear_istr_bits(USB_ISTR_RESET);
     dcd_handle_bus_reset();
-    dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+    dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
     return; // Don't do the rest of the things here; perhaps they've been cleared?
   }
 
diff --git a/src/portable/st/synopsys/dcd_synopsys.c b/src/portable/st/synopsys/dcd_synopsys.c
index 9aeec79e6..548438b68 100644
--- a/src/portable/st/synopsys/dcd_synopsys.c
+++ b/src/portable/st/synopsys/dcd_synopsys.c
@@ -45,13 +45,13 @@
 #define STM32L4_SYNOPSYS
 #endif
 
-#if TUSB_OPT_DEVICE_ENABLED && \
+#if TUSB_OPT_DEVICE_ENABLED &&                                          \
     ( (CFG_TUSB_MCU == OPT_MCU_STM32F1 && defined(STM32F1_SYNOPSYS)) || \
-        CFG_TUSB_MCU == OPT_MCU_STM32F2 || \
-        CFG_TUSB_MCU == OPT_MCU_STM32F4 || \
-        CFG_TUSB_MCU == OPT_MCU_STM32F7 || \
-        CFG_TUSB_MCU == OPT_MCU_STM32H7 || \
-        (CFG_TUSB_MCU == OPT_MCU_STM32L4 && defined(STM32L4_SYNOPSYS)) \
+       CFG_TUSB_MCU == OPT_MCU_STM32F2                               || \
+       CFG_TUSB_MCU == OPT_MCU_STM32F4                               || \
+       CFG_TUSB_MCU == OPT_MCU_STM32F7                               || \
+       CFG_TUSB_MCU == OPT_MCU_STM32H7                               || \
+      (CFG_TUSB_MCU == OPT_MCU_STM32L4 && defined(STM32L4_SYNOPSYS))    \
     )
 
 // EP_MAX       : Max number of bi-directional endpoints including EP0
@@ -115,6 +115,7 @@
 #define EP_FIFO_SIZE      EP_FIFO_SIZE_HS
 #define RHPORT_REGS_BASE  USB_OTG_HS_PERIPH_BASE
 #define RHPORT_IRQn       OTG_HS_IRQn
+
 #endif
 
 #define GLOBAL_BASE(_port)     ((USB_OTG_GlobalTypeDef*) RHPORT_REGS_BASE)
@@ -139,25 +140,40 @@ typedef struct {
   uint8_t interval;
 } xfer_ctl_t;
 
-// EP size and transfer type report
-typedef struct TU_ATTR_PACKED {
-  // The following format may look complicated but it is the most elegant way of addressing the required fields: EP number, EP direction, and EP transfer type.
-  // The codes assigned to those fields, according to the USB specification, can be neatly used as indices.
-  uint16_t ep_size[EP_MAX][2];          ///< dim 1: EP number, dim 2: EP direction denoted by TUSB_DIR_OUT (= 0) and TUSB_DIR_IN (= 1)
-  bool ep_transfer_type[EP_MAX][2][4];      ///< dim 1: EP number, dim 2: EP direction, dim 3: transfer type, where 0 = Control, 1 = Isochronous, 2 = Bulk, and 3 = Interrupt
-  ///< I know very well that EP0 can only be used as control EP and we waste space here but for the sake of simplicity we accept that. It is used in a non-persistent way anyway!
-} ep_sz_tt_report_t;
-
 typedef volatile uint32_t * usb_fifo_t;
 
 xfer_ctl_t xfer_status[EP_MAX][2];
 #define XFER_CTL_BASE(_ep, _dir) &xfer_status[_ep][_dir]
 
 // EP0 transfers are limited to 1 packet - larger sizes has to be split
-static uint16_t ep0_pending[2];     // Index determines direction as tusb_dir_t type
+static uint16_t ep0_pending[2];                   // Index determines direction as tusb_dir_t type
+
+// TX FIFO RAM allocation so far in words - RX FIFO size is readily available from usb_otg->GRXFSIZ
+static uint16_t _allocated_fifo_words_tx;         // TX FIFO size in words (IN EPs)
+static bool _out_ep_closed;                       // Flag to check if RX FIFO size needs an update (reduce its size)
+
+// Calculate the RX FIFO size according to recommendations from reference manual
+static inline uint16_t calc_rx_ff_size(uint16_t ep_size)
+{
+  return 15 + 2*(ep_size/4) + 2*EP_MAX;
+}
+
+static void update_grxfsiz(uint8_t rhport)
+{
+  (void) rhport;
 
-// FIFO RAM allocation so far in words
-static uint16_t _allocated_fifo_words;
+  USB_OTG_GlobalTypeDef * usb_otg = GLOBAL_BASE(rhport);
+
+  // Determine largest EP size for RX FIFO
+  uint16_t max_epsize = 0;
+  for (uint8_t epnum = 0; epnum < EP_MAX; epnum++)
+  {
+    max_epsize = tu_max16(max_epsize, xfer_status[epnum][TUSB_DIR_OUT].max_size);
+  }
+
+  // Update size of RX FIFO
+  usb_otg->GRXFSIZ = calc_rx_ff_size(max_epsize);
+}
 
 // Setup the control endpoint 0.
 static void bus_reset(uint8_t rhport)
@@ -170,6 +186,7 @@ static void bus_reset(uint8_t rhport)
   USB_OTG_INEndpointTypeDef * in_ep = IN_EP_BASE(rhport);
 
   tu_memclr(xfer_status, sizeof(xfer_status));
+  _out_ep_closed = false;
 
   for(uint8_t n = 0; n < EP_MAX; n++) {
     out_ep[n].DOEPCTL |= USB_OTG_DOEPCTL_SNAK;
@@ -182,16 +199,28 @@ static void bus_reset(uint8_t rhport)
   // "USB Data FIFOs" section in reference manual
   // Peripheral FIFO architecture
   //
+  // The FIFO is split up in a lower part where the RX FIFO is located and an upper part where the TX FIFOs start.
+  // We do this to allow the RX FIFO to grow dynamically which is possible since the free space is located
+  // between the RX and TX FIFOs. This is required by ISO OUT EPs which need a bigger FIFO than the standard
+  // configuration done below.
+  //
+  // Dynamically FIFO sizes are of interest only for ISO EPs since all others are usually not opened and closed.
+  // All EPs other than ISO are opened as soon as the driver starts up i.e. when the host sends a
+  // configure interface command. Hence, all IN EPs other the ISO will be located at the top. IN ISO EPs are usually
+  // opened when the host sends an additional command: setInterface. At this point in time
+  // the ISO EP will be located next to the free space and can change its size. In case more IN EPs change its size
+  // an additional memory
+  //
   // --------------- 320 or 1024 ( 1280 or 4096 bytes )
+  // | IN FIFO 0   |
+  // --------------- (320 or 1024) - 16
+  // | IN FIFO 1   |
+  // --------------- (320 or 1024) - 16 - x
+  // |   . . . .   |
+  // --------------- (320 or 1024) - 16 - x - y - ... - z
   // | IN FIFO MAX |
   // ---------------
-  // |    ...      |
-  // --------------- y + x + 16 + GRXFSIZ
-  // | IN FIFO 2   |
-  // --------------- x + 16 + GRXFSIZ
-  // | IN FIFO 1   |
-  // --------------- 16 + GRXFSIZ
-  // | IN FIFO 0   |
+  // |    FREE     |
   // --------------- GRXFSIZ
   // | OUT FIFO    |
   // | ( Shared )  |
@@ -213,24 +242,16 @@ static void bus_reset(uint8_t rhport)
   //   NOTE: Largest-EPsize & EPOUTnum is actual used endpoints in configuration. Since DCD has no knowledge
   //   of the overall picture yet. We will use the worst scenario: largest possible + EP_MAX
   //
-  //   FIXME: for Isochronous, largest EP size can be 1023/1024 for FS/HS respectively. In addition if multiple ISO
+  //   For Isochronous, largest EP size can be 1023/1024 for FS/HS respectively. In addition if multiple ISO
   //   are enabled at least "2 x (Largest-EPsize/4) + 1" are recommended.  Maybe provide a macro for application to
   //   overwrite this.
 
-#if TUD_OPT_HIGH_SPEED
-  _allocated_fifo_words = 271 + 2*EP_MAX;
-#else
-  _allocated_fifo_words =  47 + 2*EP_MAX;
-#endif
+  usb_otg->GRXFSIZ = calc_rx_ff_size(TUD_OPT_HIGH_SPEED ? 512 : 64);
 
-  usb_otg->GRXFSIZ = _allocated_fifo_words;
+  _allocated_fifo_words_tx = 16;
 
   // Control IN uses FIFO 0 with 64 bytes ( 16 32-bit word )
-  usb_otg->DIEPTXF0_HNPTXFSIZ = (16 << USB_OTG_TX0FD_Pos) | _allocated_fifo_words;
-
-  _allocated_fifo_words += 16;
-
-  // TU_LOG2_INT(_allocated_fifo_words);
+  usb_otg->DIEPTXF0_HNPTXFSIZ = (16 << USB_OTG_TX0FD_Pos) | (EP_FIFO_SIZE/4 - _allocated_fifo_words_tx);
 
   // Fixed control EP0 size to 64 bytes
   in_ep[0].DIEPCTL &= ~(0x03 << USB_OTG_DIEPCTL_MPSIZ_Pos);
@@ -507,13 +528,16 @@ void dcd_set_address (uint8_t rhport, uint8_t dev_addr)
 void dcd_remote_wakeup(uint8_t rhport)
 {
   (void) rhport;
+
+  // TODO must manually clear this bit after 1-15 ms
+  // USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
+  // dev->DCTL |= USB_OTG_DCTL_RWUSIG;
 }
 
 void dcd_connect(uint8_t rhport)
 {
   (void) rhport;
   USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
-
   dev->DCTL &= ~USB_OTG_DCTL_SDIS;
 }
 
@@ -521,7 +545,6 @@ void dcd_disconnect(uint8_t rhport)
 {
   (void) rhport;
   USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
-
   dev->DCTL |= USB_OTG_DCTL_SDIS;
 }
 
@@ -532,6 +555,8 @@ void dcd_disconnect(uint8_t rhport)
 
 bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
 {
+  (void) rhport;
+
   USB_OTG_GlobalTypeDef * usb_otg = GLOBAL_BASE(rhport);
   USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
   USB_OTG_OUTEndpointTypeDef * out_ep = OUT_EP_BASE(rhport);
@@ -542,21 +567,26 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
 
   TU_ASSERT(epnum < EP_MAX);
 
-  if (desc_edpt->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS)
-  {
-    TU_ASSERT(desc_edpt->wMaxPacketSize.size <= (get_speed(rhport) == TUSB_SPEED_HIGH ? 1024 : 1023));
-  }
-  else
-  {
-    TU_ASSERT(desc_edpt->wMaxPacketSize.size <= (get_speed(rhport) == TUSB_SPEED_HIGH ? 512 : 64));
-  }
-
   xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir);
   xfer->max_size = desc_edpt->wMaxPacketSize.size;
   xfer->interval = desc_edpt->bInterval;
 
+  uint16_t const fifo_size = (desc_edpt->wMaxPacketSize.size + 3) / 4; // Round up to next full word
+
   if(dir == TUSB_DIR_OUT)
   {
+    // Calculate required size of RX FIFO
+    uint16_t const sz = calc_rx_ff_size(4*fifo_size);
+
+    // If size_rx needs to be extended check if possible and if so enlarge it
+    if (usb_otg->GRXFSIZ < sz)
+    {
+      TU_ASSERT(sz + _allocated_fifo_words_tx <= EP_FIFO_SIZE/4);
+
+      // Enlarge RX FIFO
+      usb_otg->GRXFSIZ = sz;
+    }
+
     out_ep[epnum].DOEPCTL |= (1 << USB_OTG_DOEPCTL_USBAEP_Pos) |
         (desc_edpt->bmAttributes.xfer << USB_OTG_DOEPCTL_EPTYP_Pos) |
         (desc_edpt->wMaxPacketSize.size << USB_OTG_DOEPCTL_MPSIZ_Pos);
@@ -569,15 +599,15 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
     // Peripheral FIFO architecture
     //
     // --------------- 320 or 1024 ( 1280 or 4096 bytes )
+    // | IN FIFO 0   |
+    // --------------- (320 or 1024) - 16
+    // | IN FIFO 1   |
+    // --------------- (320 or 1024) - 16 - x
+    // |   . . . .   |
+    // --------------- (320 or 1024) - 16 - x - y - ... - z
     // | IN FIFO MAX |
     // ---------------
-    // |    ...      |
-    // --------------- y + x + 16 + GRXFSIZ
-    // | IN FIFO 2   |
-    // --------------- x + 16 + GRXFSIZ
-    // | IN FIFO 1   |
-    // --------------- 16 + GRXFSIZ
-    // | IN FIFO 0   |
+    // |    FREE     |
     // --------------- GRXFSIZ
     // | OUT FIFO    |
     // | ( Shared )  |
@@ -585,34 +615,15 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
     //
     // In FIFO is allocated by following rules:
     // - IN EP 1 gets FIFO 1, IN EP "n" gets FIFO "n".
-    // - Offset: allocated so far
-    // - Size
-    //    - Interrupt is EPSize
-    //    - Bulk/ISO is max(EPSize, remaining-fifo / non-opened-EPIN)
-
-    uint16_t const fifo_remaining = EP_FIFO_SIZE/4 - _allocated_fifo_words;
-    uint16_t fifo_size = (desc_edpt->wMaxPacketSize.size + 3) / 4;  // +3 for rounding up to next full word
-
-    if ( desc_edpt->bmAttributes.xfer != TUSB_XFER_INTERRUPT )
-    {
-      uint8_t opened = 0;
-      for(uint8_t i = 0; i < EP_MAX; i++)
-      {
-        if ( (i != epnum) && (xfer_status[i][TUSB_DIR_IN].max_size > 0) ) opened++;
-      }
 
-      // EP Size or equally divided of remaining whichever is larger
-      fifo_size = tu_max16(fifo_size, fifo_remaining / (EP_MAX - opened));
-    }
+    // Check if free space is available
+    TU_ASSERT(_allocated_fifo_words_tx + fifo_size + usb_otg->GRXFSIZ <= EP_FIFO_SIZE/4);
 
-    // FIFO overflows, we probably need a better allocating scheme
-    TU_ASSERT(fifo_size <= fifo_remaining);
+    _allocated_fifo_words_tx += fifo_size;
 
     // DIEPTXF starts at FIFO #1.
     // Both TXFD and TXSA are in unit of 32-bit words.
-    usb_otg->DIEPTXF[epnum - 1] = (fifo_size << USB_OTG_DIEPTXF_INEPTXFD_Pos) | _allocated_fifo_words;
-
-    _allocated_fifo_words += fifo_size;
+    usb_otg->DIEPTXF[epnum - 1] = (fifo_size << USB_OTG_DIEPTXF_INEPTXFD_Pos) | (EP_FIFO_SIZE/4 - _allocated_fifo_words_tx);
 
     in_ep[epnum].DIEPCTL |= (1 << USB_OTG_DIEPCTL_USBAEP_Pos) |
         (epnum << USB_OTG_DIEPCTL_TXFNUM_Pos) |
@@ -722,13 +733,21 @@ void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr)
   uint8_t const dir   = tu_edpt_dir(ep_addr);
 
   dcd_edpt_disable(rhport, ep_addr, false);
+
+  // Update max_size
+  xfer_status[epnum][dir].max_size = 0;  // max_size = 0 marks a disabled EP - required for changing FIFO allocation
+
   if (dir == TUSB_DIR_IN)
   {
     uint16_t const fifo_size = (usb_otg->DIEPTXF[epnum - 1] & USB_OTG_DIEPTXF_INEPTXFD_Msk) >> USB_OTG_DIEPTXF_INEPTXFD_Pos;
     uint16_t const fifo_start = (usb_otg->DIEPTXF[epnum - 1] & USB_OTG_DIEPTXF_INEPTXSA_Msk) >> USB_OTG_DIEPTXF_INEPTXSA_Pos;
-    // For now only endpoint that has FIFO at the end of FIFO memory can be closed without fuss.
-    TU_ASSERT(fifo_start + fifo_size == _allocated_fifo_words,);
-    _allocated_fifo_words -= fifo_size;
+    // For now only the last opened endpoint can be closed without fuss.
+    TU_ASSERT(fifo_start == EP_FIFO_SIZE/4 - _allocated_fifo_words_tx,);
+    _allocated_fifo_words_tx -= fifo_size;
+  }
+  else
+  {
+    _out_ep_closed = true;     // Set flag such that RX FIFO gets reduced in size once RX FIFO is empty
   }
 }
 
@@ -984,13 +1003,15 @@ void dcd_int_handler(uint8_t rhport)
 
   uint32_t int_status = usb_otg->GINTSTS;
 
-  if(int_status & USB_OTG_GINTSTS_USBRST) {
+  if(int_status & USB_OTG_GINTSTS_USBRST)
+  {
     // USBRST is start of reset.
     usb_otg->GINTSTS = USB_OTG_GINTSTS_USBRST;
     bus_reset(rhport);
   }
 
-  if(int_status & USB_OTG_GINTSTS_ENUMDNE) {
+  if(int_status & USB_OTG_GINTSTS_ENUMDNE)
+  {
     // ENUMDNE is the end of reset where speed of the link is detected
 
     usb_otg->GINTSTS = USB_OTG_GINTSTS_ENUMDNE;
@@ -1027,45 +1048,59 @@ void dcd_int_handler(uint8_t rhport)
   }
 
 #if USE_SOF
-  if(int_status & USB_OTG_GINTSTS_SOF) {
+  if(int_status & USB_OTG_GINTSTS_SOF)
+  {
     usb_otg->GINTSTS = USB_OTG_GINTSTS_SOF;
     dcd_event_bus_signal(rhport, DCD_EVENT_SOF, true);
   }
 #endif
 
   // RxFIFO non-empty interrupt handling.
-  if(int_status & USB_OTG_GINTSTS_RXFLVL) {
+  if(int_status & USB_OTG_GINTSTS_RXFLVL)
+  {
     // RXFLVL bit is read-only
 
     // Mask out RXFLVL while reading data from FIFO
     usb_otg->GINTMSK &= ~USB_OTG_GINTMSK_RXFLVLM;
 
     // Loop until all available packets were handled
-    do {
+    do
+    {
       handle_rxflvl_ints(rhport, out_ep);
       int_status = usb_otg->GINTSTS;
     } while(int_status & USB_OTG_GINTSTS_RXFLVL);
 
+    // Manage RX FIFO size
+    if (_out_ep_closed)
+    {
+      update_grxfsiz(rhport);
+
+      // Disable flag
+      _out_ep_closed = false;
+    }
+
     usb_otg->GINTMSK |= USB_OTG_GINTMSK_RXFLVLM;
   }
 
   // OUT endpoint interrupt handling.
-  if(int_status & USB_OTG_GINTSTS_OEPINT) {
+  if(int_status & USB_OTG_GINTSTS_OEPINT)
+  {
     // OEPINT is read-only
     handle_epout_ints(rhport, dev, out_ep);
   }
 
   // IN endpoint interrupt handling.
-  if(int_status & USB_OTG_GINTSTS_IEPINT) {
+  if(int_status & USB_OTG_GINTSTS_IEPINT)
+  {
     // IEPINT bit read-only
     handle_epin_ints(rhport, dev, in_ep);
   }
 
-//  // Check for Incomplete isochronous IN transfer
-//  if(int_status & USB_OTG_GINTSTS_IISOIXFR) {
-//    printf("      IISOIXFR!\r\n");
-////    TU_LOG2("      IISOIXFR!\r\n");
-//  }
+  //  // Check for Incomplete isochronous IN transfer
+  //  if(int_status & USB_OTG_GINTSTS_IISOIXFR) {
+  //    printf("      IISOIXFR!\r\n");
+  ////    TU_LOG2("      IISOIXFR!\r\n");
+  //  }
 }
 
 #endif
diff --git a/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c b/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
index ad71e1168..e08c3536a 100644
--- a/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
+++ b/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
@@ -232,9 +232,9 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
   uint8_t const epnum = tu_edpt_number(desc_edpt->bEndpointAddress);
   uint8_t const dir   = tu_edpt_dir(desc_edpt->bEndpointAddress);
 
-  // Unsupported endpoint numbers/size or type (Iso not supported. Control
+  // Unsupported endpoint numbers or type (Iso not supported. Control
   // not supported on nonzero endpoints).
-  if((desc_edpt->wMaxPacketSize.size > 64) || (epnum > 7) || \
+  if( (epnum > 7) || \
       (desc_edpt->bmAttributes.xfer == 0) || \
       (desc_edpt->bmAttributes.xfer == 1)) {
     return false;
@@ -572,7 +572,7 @@ void dcd_int_handler(uint8_t rhport)
   {
     case USBVECINT_RSTR:
       bus_reset();
-      dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+      dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
       break;
 
     // Clear the (hardware-enforced) NAK on EP 0 after a SETUP packet
diff --git a/src/portable/valentyusb/eptri/dcd_eptri.c b/src/portable/valentyusb/eptri/dcd_eptri.c
index d670dd38e..b68f04faa 100644
--- a/src/portable/valentyusb/eptri/dcd_eptri.c
+++ b/src/portable/valentyusb/eptri/dcd_eptri.c
@@ -332,7 +332,7 @@ static void dcd_reset(void)
   usb_out_ev_enable_write(1);
   usb_setup_ev_enable_write(3);
 
-  dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+  dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
 }
 
 // Initializes the USB peripheral for device mode and enables it.
diff --git a/src/tusb.c b/src/tusb.c
index b4731f844..31452e897 100644
--- a/src/tusb.c
+++ b/src/tusb.c
@@ -52,7 +52,7 @@ bool tusb_init(void)
 
   _initialized = true;
 
-  return TUSB_ERROR_NONE;
+  return true;
 }
 
 bool tusb_inited(void)
diff --git a/src/tusb_option.h b/src/tusb_option.h
index a4eeeeb7c..a89c6db9d 100644
--- a/src/tusb_option.h
+++ b/src/tusb_option.h
@@ -100,6 +100,9 @@
 // Raspberry Pi
 #define OPT_MCU_RP2040           1100 ///< Raspberry Pi RP2040
 
+// NXP Kinetis
+#define OPT_MCU_MKL25ZXX         1200 ///< NXP MKL25Zxx
+
 /** @} */
 
 /** \defgroup group_supported_os Supported RTOS
@@ -151,22 +154,22 @@
   #define CFG_TUSB_RHPORT1_MODE OPT_MODE_NONE
 #endif
 
-#if ((CFG_TUSB_RHPORT0_MODE & OPT_MODE_HOST  ) && (CFG_TUSB_RHPORT1_MODE & OPT_MODE_HOST  )) || \
-    ((CFG_TUSB_RHPORT0_MODE & OPT_MODE_DEVICE) && (CFG_TUSB_RHPORT1_MODE & OPT_MODE_DEVICE))
+#if (((CFG_TUSB_RHPORT0_MODE) & OPT_MODE_HOST  ) && ((CFG_TUSB_RHPORT1_MODE) & OPT_MODE_HOST  )) || \
+    (((CFG_TUSB_RHPORT0_MODE) & OPT_MODE_DEVICE) && ((CFG_TUSB_RHPORT1_MODE) & OPT_MODE_DEVICE))
   #error "TinyUSB currently does not support same modes on more than 1 roothub port"
 #endif
 
 // Which roothub port is configured as host
-#define TUH_OPT_RHPORT          ( (CFG_TUSB_RHPORT0_MODE & OPT_MODE_HOST) ? 0 : ((CFG_TUSB_RHPORT1_MODE & OPT_MODE_HOST) ? 1 : -1) )
+#define TUH_OPT_RHPORT          ( ((CFG_TUSB_RHPORT0_MODE) & OPT_MODE_HOST) ? 0 : (((CFG_TUSB_RHPORT1_MODE) & OPT_MODE_HOST) ? 1 : -1) )
 #define TUSB_OPT_HOST_ENABLED   ( TUH_OPT_RHPORT >= 0 )
 
 // Which roothub port is configured as device
-#define TUD_OPT_RHPORT          ( (CFG_TUSB_RHPORT0_MODE & OPT_MODE_DEVICE) ? 0 : ((CFG_TUSB_RHPORT1_MODE & OPT_MODE_DEVICE) ? 1 : -1) )
+#define TUD_OPT_RHPORT          ( ((CFG_TUSB_RHPORT0_MODE) & OPT_MODE_DEVICE) ? 0 : (((CFG_TUSB_RHPORT1_MODE) & OPT_MODE_DEVICE) ? 1 : -1) )
 
 #if TUD_OPT_RHPORT == 0
-#define TUD_OPT_HIGH_SPEED      ( CFG_TUSB_RHPORT0_MODE & OPT_MODE_HIGH_SPEED )
+#define TUD_OPT_HIGH_SPEED      ( (CFG_TUSB_RHPORT0_MODE) & OPT_MODE_HIGH_SPEED )
 #else
-#define TUD_OPT_HIGH_SPEED      ( CFG_TUSB_RHPORT1_MODE & OPT_MODE_HIGH_SPEED )
+#define TUD_OPT_HIGH_SPEED      ( (CFG_TUSB_RHPORT1_MODE) & OPT_MODE_HIGH_SPEED )
 #endif
 
 #define TUSB_OPT_DEVICE_ENABLED ( TUD_OPT_RHPORT >= 0 )