Merge branch 'master' into test-mode-support

91 files changed, 6538 insertions, 4796 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 076e1e1eb..272962332 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -41,7 +41,7 @@ function(add_tinyusb TARGET)
     ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/../lib/networking
     )
 
-  if (CMAKE_C_COMPILER_ID STREQUAL "GNU")
+  if (CMAKE_C_COMPILER_ID STREQUAL "GNU" OR CMAKE_C_COMPILER_ID STREQUAL "Clang")
     target_compile_options(${TARGET} PRIVATE
       -Wall
       -Wextra
diff --git a/src/class/audio/audio.h b/src/class/audio/audio.h
index 70d431282..d6f3e22e2 100644
--- a/src/class/audio/audio.h
+++ b/src/class/audio/audio.h
@@ -924,6 +924,31 @@ typedef struct TU_ATTR_PACKED {
     } subrange[numSubRanges];                   \
 }
 
+// 6.1 Interrupt Data Message Format
+typedef struct TU_ATTR_PACKED
+{
+  uint8_t bInfo;
+  uint8_t bAttribute;
+  union
+  {
+    uint16_t wValue;
+    struct
+    {
+      uint8_t wValue_cn_or_mcn;
+      uint8_t wValue_cs;
+    };
+  };
+  union
+  {
+    uint16_t wIndex;
+    struct
+    {
+      uint8_t wIndex_ep_or_int;
+      uint8_t wIndex_entity_id;
+    };
+  };
+} audio_interrupt_data_t;
+
 /** @} */
 
 #ifdef __cplusplus
diff --git a/src/class/audio/audio_device.c b/src/class/audio/audio_device.c
index d438d0715..46db96ea9 100644
--- a/src/class/audio/audio_device.c
+++ b/src/class/audio/audio_device.c
@@ -96,13 +96,6 @@
   #define  USE_LINEAR_BUFFER     1
 #endif
 
-// Temporarily put the check here for stm32_fsdev
-#ifdef TUP_USBIP_FSDEV
-  #define  USE_ISO_EP_ALLOCATION   1
-#else
-  #define  USE_ISO_EP_ALLOCATION   0
-#endif
-
 // Declaration of buffers
 
 // Check for maximum supported numbers
@@ -301,10 +294,12 @@ typedef struct
 
 #endif
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-  uint8_t ep_int_ctr;           // Audio control interrupt EP.
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+  uint8_t ep_int;               // Audio control interrupt EP.
 #endif
 
+  bool mounted;                 // Device opened
+
   /*------------- From this point, data is not cleared by bus reset -------------*/
 
   uint16_t desc_length;         // Length of audio function descriptor
@@ -358,8 +353,8 @@ typedef struct
 #endif
 
   // Audio control interrupt buffer - no FIFO - 6 Bytes according to UAC 2 specification (p. 74)
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-  CFG_TUSB_MEM_ALIGN uint8_t ep_int_ctr_buf[CFG_TUD_AUDIO_INT_CTR_EP_IN_SW_BUFFER_SIZE];
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+  CFG_TUSB_MEM_ALIGN uint8_t ep_int_buf[6];
 #endif
 
   // Decoding parameters - parameters are set when alternate AS interface is set by host
@@ -486,23 +481,7 @@ bool tud_audio_n_mounted(uint8_t func_id)
   TU_VERIFY(func_id < CFG_TUD_AUDIO);
   audiod_function_t* audio = &_audiod_fct[func_id];
 
-#if CFG_TUD_AUDIO_ENABLE_EP_OUT
-  if (audio->ep_out == 0) return false;
-#endif
-
-#if CFG_TUD_AUDIO_ENABLE_EP_IN
-  if (audio->ep_in == 0) return false;
-#endif
-
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-  if (audio->ep_int_ctr == 0) return false;
-#endif
-
-#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
-  if (audio->ep_fb == 0) return false;
-#endif
-
-  return true;
+  return audio->mounted;
 }
 
 //--------------------------------------------------------------------+
@@ -825,24 +804,30 @@ tu_fifo_t* tud_audio_n_get_tx_support_ff(uint8_t func_id, uint8_t ff_idx)
 #endif
 
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-
-// If no interrupt transmit is pending bytes get written into buffer and a transmit is scheduled - once transmit completed tud_audio_int_ctr_done_cb() is called in inform user
-uint16_t tud_audio_int_ctr_n_write(uint8_t func_id, uint8_t const* buffer, uint16_t len)
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+// If no interrupt transmit is pending bytes get written into buffer and a transmit is scheduled - once transmit completed tud_audio_int_done_cb() is called in inform user
+bool tud_audio_int_n_write(uint8_t func_id, const audio_interrupt_data_t * data)
 {
   TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL);
 
-  // We write directly into the EP's buffer - abort if previous transfer not complete
-  TU_VERIFY(!usbd_edpt_busy(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_int_ctr));
+  TU_VERIFY(_audiod_fct[func_id].ep_int != 0);
 
-  TU_VERIFY(tu_memcpy_s(_audiod_fct[func_id].ep_int_ctr_buf, CFG_TUD_AUDIO_INT_CTR_EP_IN_SW_BUFFER_SIZE,  buffer, len)==0);
+  // We write directly into the EP's buffer - abort if previous transfer not complete
+  TU_VERIFY(usbd_edpt_claim(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_int));
 
-  // Schedule transmit
-  TU_VERIFY(usbd_edpt_xfer(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_int_ctr, _audiod_fct[func_id].ep_int_ctr_buf, len));
+  // Check length
+  if (tu_memcpy_s(_audiod_fct[func_id].ep_int_buf, sizeof(_audiod_fct[func_id].ep_int_buf),  data, sizeof(audio_interrupt_data_t)) == 0)
+  {
+    // Schedule transmit
+    TU_ASSERT(usbd_edpt_xfer(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_int, _audiod_fct[func_id].ep_int_buf, sizeof(_audiod_fct[func_id].ep_int_buf)), 0);
+  } else
+  {
+    // Release endpoint since we don't make any transfer
+    usbd_edpt_release(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_int);
+  }
 
   return true;
 }
-
 #endif
 
 // This function is called once a transmit of an audio packet was successfully completed. Here, we encode samples and place it in IN EP's buffer for next transmission.
@@ -1404,6 +1389,10 @@ void audiod_init(void)
   }
 }
 
+bool audiod_deinit(void) {
+  return false; // TODO not implemented yet
+}
+
 void audiod_reset(uint8_t rhport)
 {
   (void) rhport;
@@ -1447,10 +1436,11 @@ uint16_t audiod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
   // Verify version is correct - this check can be omitted
   TU_VERIFY(itf_desc->bInterfaceProtocol == AUDIO_INT_PROTOCOL_CODE_V2);
 
-  // Verify interrupt control EP is enabled if demanded by descriptor - this should be best some static check however - this check can be omitted
-  if (itf_desc->bNumEndpoints == 1) // 0 or 1 EPs are allowed
+  // Verify interrupt control EP is enabled if demanded by descriptor
+  TU_ASSERT(itf_desc->bNumEndpoints <= 1); // 0 or 1 EPs are allowed
+  if (itf_desc->bNumEndpoints == 1)
   {
-    TU_VERIFY(CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN > 0);
+    TU_ASSERT(CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP);
   }
 
   // Alternate setting MUST be zero - this check can be omitted
@@ -1483,7 +1473,7 @@ uint16_t audiod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
 #endif
       }
 
-#if USE_ISO_EP_ALLOCATION
+#ifdef TUP_DCD_EDPT_ISO_ALLOC
       {
   #if CFG_TUD_AUDIO_ENABLE_EP_IN
         uint8_t  ep_in = 0;
@@ -1559,7 +1549,7 @@ uint16_t audiod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
         }
   #endif
       }
-#endif // USE_ISO_EP_ALLOCATION
+#endif // TUP_DCD_EDPT_ISO_ALLOC
 
 #if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
       {
@@ -1594,6 +1584,32 @@ uint16_t audiod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
       }
 #endif // CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
 
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+      {
+        uint8_t const *p_desc = _audiod_fct[i].p_desc;
+        uint8_t const *p_desc_end = p_desc + _audiod_fct[i].desc_length - TUD_AUDIO_DESC_IAD_LEN;
+        // Condition modified from p_desc < p_desc_end to prevent gcc>=12 strict-overflow warning
+        while (p_desc_end - p_desc > 0)
+        {
+          // For each endpoint
+          if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT)
+          {
+            tusb_desc_endpoint_t const* desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+            uint8_t const ep_addr = desc_ep->bEndpointAddress;
+            // If endpoint is input-direction and interrupt-type
+            if (tu_edpt_dir(ep_addr) == TUSB_DIR_IN && desc_ep->bmAttributes.xfer == TUSB_XFER_INTERRUPT)
+            {
+              // Store endpoint number and open endpoint
+              _audiod_fct[i].ep_int = ep_addr;
+              TU_ASSERT(usbd_edpt_open(_audiod_fct[i].rhport, desc_ep));
+            }
+          }
+          p_desc = tu_desc_next(p_desc);
+        }
+      }
+#endif
+
+      _audiod_fct[i].mounted = true;
       break;
     }
   }
@@ -1655,7 +1671,7 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
   if (audio->ep_in_as_intf_num == itf)
   {
     audio->ep_in_as_intf_num = 0;
-  #if !USE_ISO_EP_ALLOCATION
+  #ifndef TUP_DCD_EDPT_ISO_ALLOC
     usbd_edpt_close(rhport, audio->ep_in);
   #endif
 
@@ -1686,7 +1702,7 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
   if (audio->ep_out_as_intf_num == itf)
   {
     audio->ep_out_as_intf_num = 0;
-  #if !USE_ISO_EP_ALLOCATION
+  #ifndef TUP_DCD_EDPT_ISO_ALLOC
     usbd_edpt_close(rhport, audio->ep_out);
   #endif
 
@@ -1707,7 +1723,7 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
 
     // Close corresponding feedback EP
   #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
-    #if !USE_ISO_EP_ALLOCATION
+    #ifndef TUP_DCD_EDPT_ISO_ALLOC
     usbd_edpt_close(rhport, audio->ep_fb);
     #endif
     audio->ep_fb = 0;
@@ -1729,7 +1745,7 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
     // Find correct interface
     if (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE && ((tusb_desc_interface_t const * )p_desc)->bInterfaceNumber == itf && ((tusb_desc_interface_t const * )p_desc)->bAlternateSetting == alt)
     {
-#if CFG_TUD_AUDIO_ENABLE_ENCODING || CFG_TUD_AUDIO_ENABLE_DECODING || CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
+#if (CFG_TUD_AUDIO_ENABLE_EP_IN && (CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL || CFG_TUD_AUDIO_ENABLE_ENCODING)) || (CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING)
       uint8_t const * p_desc_parse_for_params = p_desc;
 #endif
       // From this point forward follow the EP descriptors associated to the current alternate setting interface - Open EPs if necessary
@@ -1739,7 +1755,7 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
         if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT)
         {
           tusb_desc_endpoint_t const* desc_ep = (tusb_desc_endpoint_t const *) p_desc;
-#if USE_ISO_EP_ALLOCATION
+#ifdef TUP_DCD_EDPT_ISO_ALLOC
           TU_ASSERT(usbd_edpt_iso_activate(rhport, desc_ep));
 #else
           TU_ASSERT(usbd_edpt_open(rhport, desc_ep));
@@ -1819,7 +1835,7 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
             audio->feedback.frame_shift = desc_ep->bInterval -1;
 
             // Enable SOF interrupt if callback is implemented
-            if (tud_audio_feedback_interval_isr) usbd_sof_enable(rhport, true);
+            if (tud_audio_feedback_interval_isr) usbd_sof_enable(rhport, SOF_CONSUMER_AUDIO, true);
           }
   #endif
 #endif // CFG_TUD_AUDIO_ENABLE_EP_OUT
@@ -1893,7 +1909,7 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
       break;
     }
   }
-  if (disable) usbd_sof_enable(rhport, false);
+  if (disable) usbd_sof_enable(rhport, SOF_CONSUMER_AUDIO, false);
 #endif
 
 #if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
@@ -1999,7 +2015,10 @@ static bool audiod_control_request(uint8_t rhport, tusb_control_request_t const
       case TUSB_REQ_SET_INTERFACE:
         return audiod_set_interface(rhport, p_request);
 
-        // Unknown/Unsupported request
+      case TUSB_REQ_CLEAR_FEATURE:
+        return true;
+
+      // Unknown/Unsupported request
       default: TU_BREAKPOINT(); return false;
     }
   }
@@ -2120,10 +2139,10 @@ bool audiod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint3
   {
     audiod_function_t* audio = &_audiod_fct[func_id];
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
 
     // Data transmission of control interrupt finished
-    if (audio->ep_int_ctr == ep_addr)
+    if (audio->ep_int == ep_addr)
     {
       // According to USB2 specification, maximum payload of interrupt EP is 8 bytes on low speed, 64 bytes on full speed, and 1024 bytes on high speed (but only if an alternate interface other than 0 is used - see specification p. 49)
       // In case there is nothing to send we have to return a NAK - this is taken care of by PHY ???
@@ -2132,7 +2151,8 @@ bool audiod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint3
       // I assume here, that things above are handled by PHY
       // All transmission is done - what remains to do is to inform job was completed
 
-      if (tud_audio_int_ctr_done_cb) TU_VERIFY(tud_audio_int_ctr_done_cb(rhport, (uint16_t) xferred_bytes));
+      if (tud_audio_int_done_cb) tud_audio_int_done_cb(rhport);
+      return true;
     }
 
 #endif
diff --git a/src/class/audio/audio_device.h b/src/class/audio/audio_device.h
index ef3e12a06..b16514fd4 100644
--- a/src/class/audio/audio_device.h
+++ b/src/class/audio/audio_device.h
@@ -196,13 +196,9 @@
 #define CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION     0                             // 0 or 1
 #endif
 
-// Audio interrupt control EP size - disabled if 0
-#ifndef CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-#define CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN                     0                             // Audio interrupt control - if required - 6 Bytes according to UAC 2 specification (p. 74)
-#endif
-
-#ifndef CFG_TUD_AUDIO_INT_CTR_EP_IN_SW_BUFFER_SIZE
-#define CFG_TUD_AUDIO_INT_CTR_EP_IN_SW_BUFFER_SIZE          6                             // Buffer size of audio control interrupt EP - 6 Bytes according to UAC 2 specification (p. 74)
+// Enable/disable interrupt EP (required for notifying host of control changes)
+#ifndef CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+#define CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP                   0                             // Feedback - 0 or 1
 #endif
 
 // Use software encoding/decoding
@@ -393,8 +389,8 @@ uint16_t tud_audio_n_write_support_ff             (uint8_t func_id, uint8_t ff_i
 tu_fifo_t* tud_audio_n_get_tx_support_ff          (uint8_t func_id, uint8_t ff_idx);
 #endif
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-uint16_t    tud_audio_int_ctr_n_write             (uint8_t func_id, uint8_t const* buffer, uint16_t len);
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+bool    tud_audio_int_n_write                     (uint8_t func_id, const audio_interrupt_data_t * data);
 #endif
 
 
@@ -437,8 +433,8 @@ static inline tu_fifo_t* tud_audio_get_tx_support_ff        (uint8_t ff_idx);
 
 // INT CTR API
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-static inline uint16_t tud_audio_int_ctr_write              (uint8_t const* buffer, uint16_t len);
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+static inline bool tud_audio_int_write                      (const audio_interrupt_data_t * data);
 #endif
 
 // Buffer control EP data and schedule a transmit
@@ -537,8 +533,8 @@ TU_ATTR_WEAK TU_ATTR_FAST_FUNC void tud_audio_feedback_interval_isr(uint8_t func
 
 #endif // CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-TU_ATTR_WEAK bool tud_audio_int_ctr_done_cb(uint8_t rhport, uint16_t n_bytes_copied);
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+TU_ATTR_WEAK void tud_audio_int_done_cb(uint8_t rhport);
 #endif
 
 // Invoked when audio set interface request received
@@ -669,10 +665,10 @@ static inline tu_fifo_t* tud_audio_get_tx_support_ff(uint8_t ff_idx)
 
 #endif
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-static inline uint16_t tud_audio_int_ctr_write(uint8_t const* buffer, uint16_t len)
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+static inline bool tud_audio_int_write(const audio_interrupt_data_t * data)
 {
-  return tud_audio_int_ctr_n_write(0, buffer, len);
+  return tud_audio_int_n_write(0, data);
 }
 #endif
 
@@ -689,6 +685,7 @@ static inline bool tud_audio_fb_set(uint32_t feedback)
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     audiod_init           (void);
+bool     audiod_deinit         (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);
diff --git a/src/class/bth/bth_device.c b/src/class/bth/bth_device.c
index f145e2ead..cbf6e1332 100755
--- a/src/class/bth/bth_device.c
+++ b/src/class/bth/bth_device.c
@@ -91,11 +91,14 @@ bool tud_bt_acl_data_send(void *event, uint16_t event_len)
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void btd_init(void)
-{
+void btd_init(void) {
   tu_memclr(&_btd_itf, sizeof(_btd_itf));
 }
 
+bool btd_deinit(void) {
+  return true;
+}
+
 void btd_reset(uint8_t rhport)
 {
   (void)rhport;
diff --git a/src/class/bth/bth_device.h b/src/class/bth/bth_device.h
index e782c532b..4f6350839 100755
--- a/src/class/bth/bth_device.h
+++ b/src/class/bth/bth_device.h
@@ -104,6 +104,7 @@ bool tud_bt_acl_data_send(void *acl_data, uint16_t data_len);
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     btd_init            (void);
+bool     btd_deinit          (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);
diff --git a/src/class/cdc/cdc.h b/src/class/cdc/cdc.h
index deec32ae4..5cbd658fe 100644
--- a/src/class/cdc/cdc.h
+++ b/src/class/cdc/cdc.h
@@ -136,8 +136,7 @@ typedef enum{
 //--------------------------------------------------------------------+
 
 /// Communication Interface Management Element Request Codes
-typedef enum
-{
+typedef enum {
   CDC_REQUEST_SEND_ENCAPSULATED_COMMAND                    = 0x00, ///< is used to issue a command in the format of the supported control protocol of the Communications Class interface
   CDC_REQUEST_GET_ENCAPSULATED_RESPONSE                    = 0x01, ///< is used to request a response in the format of the supported control protocol of the Communications Class interface.
   CDC_REQUEST_SET_COMM_FEATURE                             = 0x02,
@@ -180,39 +179,38 @@ typedef enum
   CDC_REQUEST_GET_ATM_VC_STATISTICS                        = 0x53,
 
   CDC_REQUEST_MDLM_SEMANTIC_MODEL                          = 0x60,
-}cdc_management_request_t;
+} cdc_management_request_t;
 
-enum {
+typedef enum {
   CDC_CONTROL_LINE_STATE_DTR = 0x01,
   CDC_CONTROL_LINE_STATE_RTS = 0x02,
-};
+} cdc_control_line_state_t;
 
-enum {
+typedef enum {
   CDC_LINE_CODING_STOP_BITS_1   = 0, // 1   bit
   CDC_LINE_CODING_STOP_BITS_1_5 = 1, // 1.5 bits
   CDC_LINE_CODING_STOP_BITS_2   = 2, // 2   bits
-};
+} cdc_line_coding_stopbits_t;
 
 // TODO Backward compatible for typos. Maybe removed in the future release
 #define CDC_LINE_CONDING_STOP_BITS_1   CDC_LINE_CODING_STOP_BITS_1
 #define CDC_LINE_CONDING_STOP_BITS_1_5 CDC_LINE_CODING_STOP_BITS_1_5
 #define CDC_LINE_CONDING_STOP_BITS_2   CDC_LINE_CODING_STOP_BITS_2
 
-enum {
+typedef enum {
   CDC_LINE_CODING_PARITY_NONE  = 0,
   CDC_LINE_CODING_PARITY_ODD   = 1,
   CDC_LINE_CODING_PARITY_EVEN  = 2,
   CDC_LINE_CODING_PARITY_MARK  = 3,
   CDC_LINE_CODING_PARITY_SPACE = 4,
-};
+} cdc_line_coding_parity_t;
 
 //--------------------------------------------------------------------+
 // Management Element Notification (Notification Endpoint)
 //--------------------------------------------------------------------+
 
 /// 6.3 Notification Codes
-typedef enum
-{
+typedef enum {
   CDC_NOTIF_NETWORK_CONNECTION               = 0x00, ///< This notification allows the device to notify the host about network connection status.
   CDC_NOTIF_RESPONSE_AVAILABLE               = 0x01, ///< This notification allows the device to notify the hostthat a response is available. This response can be retrieved with a subsequent \ref CDC_REQUEST_GET_ENCAPSULATED_RESPONSE request.
   CDC_NOTIF_AUX_JACK_HOOK_STATE              = 0x08,
diff --git a/src/class/cdc/cdc_device.c b/src/class/cdc/cdc_device.c
index c26264e60..f36725eba 100644
--- a/src/class/cdc/cdc_device.c
+++ b/src/class/cdc/cdc_device.c
@@ -43,10 +43,7 @@
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-enum
-{
-  BULK_PACKET_SIZE = (TUD_OPT_HIGH_SPEED ? 512 : 64)
-};
+#define BULK_PACKET_SIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
 
 typedef struct
 {
@@ -176,9 +173,11 @@ uint32_t tud_cdc_n_write(uint8_t itf, void const* buffer, uint32_t bufsize)
   uint16_t ret = tu_fifo_write_n(&p_cdc->tx_ff, buffer, (uint16_t) TU_MIN(bufsize, UINT16_MAX));
 
   // flush if queue more than packet size
-  // may need to suppress -Wunreachable-code since most of the time CFG_TUD_CDC_TX_BUFSIZE < BULK_PACKET_SIZE
-  if ( (tu_fifo_count(&p_cdc->tx_ff) >= BULK_PACKET_SIZE) || ((CFG_TUD_CDC_TX_BUFSIZE < BULK_PACKET_SIZE) && tu_fifo_full(&p_cdc->tx_ff)) )
-  {
+  if ( tu_fifo_count(&p_cdc->tx_ff) >= BULK_PACKET_SIZE
+       #if CFG_TUD_CDC_TX_BUFSIZE < BULK_PACKET_SIZE
+       || tu_fifo_full(&p_cdc->tx_ff) // check full if fifo size is less than packet size
+       #endif
+      ) {
     tud_cdc_n_write_flush(itf);
   }
 
@@ -253,11 +252,39 @@ void cdcd_init(void)
     // 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);
 
-    tu_fifo_config_mutex(&p_cdc->rx_ff, NULL, osal_mutex_create(&p_cdc->rx_ff_mutex));
-    tu_fifo_config_mutex(&p_cdc->tx_ff, osal_mutex_create(&p_cdc->tx_ff_mutex), NULL);
+    #if OSAL_MUTEX_REQUIRED
+    osal_mutex_t mutex_rd = osal_mutex_create(&p_cdc->rx_ff_mutex);
+    osal_mutex_t mutex_wr = osal_mutex_create(&p_cdc->tx_ff_mutex);
+    TU_ASSERT(mutex_rd != NULL && mutex_wr != NULL, );
+
+    tu_fifo_config_mutex(&p_cdc->rx_ff, NULL, mutex_rd);
+    tu_fifo_config_mutex(&p_cdc->tx_ff, mutex_wr, NULL);
+    #endif
   }
 }
 
+bool cdcd_deinit(void) {
+  #if OSAL_MUTEX_REQUIRED
+  for(uint8_t i=0; i<CFG_TUD_CDC; i++) {
+    cdcd_interface_t* p_cdc = &_cdcd_itf[i];
+    osal_mutex_t mutex_rd = p_cdc->rx_ff.mutex_rd;
+    osal_mutex_t mutex_wr = p_cdc->tx_ff.mutex_wr;
+
+    if (mutex_rd) {
+      osal_mutex_delete(mutex_rd);
+      tu_fifo_config_mutex(&p_cdc->rx_ff, NULL, NULL);
+    }
+
+    if (mutex_wr) {
+      osal_mutex_delete(mutex_wr);
+      tu_fifo_config_mutex(&p_cdc->tx_ff, NULL, NULL);
+    }
+  }
+  #endif
+
+  return true;
+}
+
 void cdcd_reset(uint8_t rhport)
 {
   (void) rhport;
@@ -268,7 +295,9 @@ void cdcd_reset(uint8_t rhport)
 
     tu_memclr(p_cdc, ITF_MEM_RESET_SIZE);
     tu_fifo_clear(&p_cdc->rx_ff);
+    #if !CFG_TUD_CDC_PERSISTENT_TX_BUFF
     tu_fifo_clear(&p_cdc->tx_ff);
+    #endif
     tu_fifo_set_overwritable(&p_cdc->tx_ff, true);
   }
 }
diff --git a/src/class/cdc/cdc_device.h b/src/class/cdc/cdc_device.h
index a6e07aa5c..db709b3bc 100644
--- a/src/class/cdc/cdc_device.h
+++ b/src/class/cdc/cdc_device.h
@@ -41,6 +41,12 @@
   #define CFG_TUD_CDC_EP_BUFSIZE    (TUD_OPT_HIGH_SPEED ? 512 : 64)
 #endif
 
+// By default the TX fifo buffer is cleared on connect / bus reset.
+// Enable this to persist any data in the fifo instead.
+#ifndef CFG_TUD_CDC_PERSISTENT_TX_BUFF
+  #define CFG_TUD_CDC_PERSISTENT_TX_BUFF    (0)
+#endif
+
 #ifdef __cplusplus
  extern "C" {
 #endif
@@ -247,6 +253,7 @@ static inline bool tud_cdc_write_clear(void)
 // INTERNAL USBD-CLASS DRIVER API
 //--------------------------------------------------------------------+
 void     cdcd_init            (void);
+bool     cdcd_deinit          (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);
diff --git a/src/class/cdc/cdc_host.c b/src/class/cdc/cdc_host.c
index 79477ef53..133a10f6e 100644
--- a/src/class/cdc/cdc_host.c
+++ b/src/class/cdc/cdc_host.c
@@ -22,6 +22,9 @@
  * THE SOFTWARE.
  *
  * This file is part of the TinyUSB stack.
+ *
+ * Contribution
+ * - Heiko Kuester: CH34x support
  */
 
 #include "tusb_option.h"
@@ -50,12 +53,18 @@ typedef struct {
   uint8_t bInterfaceSubClass;
   uint8_t bInterfaceProtocol;
 
+  uint8_t ep_notif;
   uint8_t serial_drid; // Serial Driver ID
+  bool mounted;        // Enumeration is complete
   cdc_acm_capability_t acm_capability;
-  uint8_t ep_notif;
 
-  uint8_t line_state;                               // DTR (bit0), RTS (bit1)
   TU_ATTR_ALIGNED(4) cdc_line_coding_t line_coding; // Baudrate, stop bits, parity, data width
+  uint8_t line_state;                               // DTR (bit0), RTS (bit1)
+
+  #if CFG_TUH_CDC_FTDI || CFG_TUH_CDC_CP210X || CFG_TUH_CDC_CH34X
+  cdc_line_coding_t requested_line_coding;
+  // 1 byte padding
+  #endif
 
   tuh_xfer_cb_t user_control_cb;
 
@@ -69,7 +78,6 @@ typedef struct {
     uint8_t rx_ff_buf[CFG_TUH_CDC_TX_BUFSIZE];
     CFG_TUH_MEM_ALIGN uint8_t rx_ep_buf[CFG_TUH_CDC_TX_EPSIZE];
   } stream;
-
 } cdch_interface_t;
 
 CFG_TUH_MEM_SECTION
@@ -83,45 +91,57 @@ static cdch_interface_t cdch_data[CFG_TUH_CDC];
 static bool acm_open(uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len);
 static void acm_process_config(tuh_xfer_t* xfer);
 
+static bool acm_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool acm_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
 static bool acm_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
 static bool acm_set_control_line_state(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
-static bool acm_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
 
 //------------- FTDI prototypes -------------//
 #if CFG_TUH_CDC_FTDI
 #include "serial/ftdi_sio.h"
 
-static uint16_t const ftdi_pids[] = { CFG_TUH_CDC_FTDI_PID_LIST };
-enum {
-  FTDI_PID_COUNT = sizeof(ftdi_pids) / sizeof(ftdi_pids[0])
-};
-
-// Store last request baudrate since divisor to baudrate is not easy
-static uint32_t _ftdi_requested_baud;
+static uint16_t const ftdi_vid_pid_list[][2] = {CFG_TUH_CDC_FTDI_VID_PID_LIST};
 
 static bool ftdi_open(uint8_t daddr, const tusb_desc_interface_t *itf_desc, uint16_t max_len);
 static void ftdi_process_config(tuh_xfer_t* xfer);
 
-static bool ftdi_sio_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
 static bool ftdi_sio_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool ftdi_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool ftdi_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool ftdi_sio_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
 #endif
 
 //------------- CP210X prototypes -------------//
 #if CFG_TUH_CDC_CP210X
 #include "serial/cp210x.h"
 
-static uint16_t const cp210x_pids[] = { CFG_TUH_CDC_CP210X_PID_LIST };
-enum {
-  CP210X_PID_COUNT = sizeof(cp210x_pids) / sizeof(cp210x_pids[0])
-};
+static uint16_t const cp210x_vid_pid_list[][2] = {CFG_TUH_CDC_CP210X_VID_PID_LIST};
 
 static bool cp210x_open(uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len);
 static void cp210x_process_config(tuh_xfer_t* xfer);
 
-static bool cp210x_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
 static bool cp210x_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool cp210x_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool cp210x_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool cp210x_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
 #endif
 
+//------------- CH34x prototypes -------------//
+#if CFG_TUH_CDC_CH34X
+#include "serial/ch34x.h"
+
+static uint16_t const ch34x_vid_pid_list[][2] = {CFG_TUH_CDC_CH34X_VID_PID_LIST};
+
+static bool ch34x_open(uint8_t daddr, tusb_desc_interface_t const* itf_desc, uint16_t max_len);
+static void ch34x_process_config(tuh_xfer_t* xfer);
+
+static bool ch34x_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool ch34x_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool ch34x_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool ch34x_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+#endif
+
+//------------- Common -------------//
 enum {
   SERIAL_DRIVER_ACM = 0,
 
@@ -132,60 +152,96 @@ enum {
 #if CFG_TUH_CDC_CP210X
   SERIAL_DRIVER_CP210X,
 #endif
+
+#if CFG_TUH_CDC_CH34X
+  SERIAL_DRIVER_CH34X,
+#endif
+
+  SERIAL_DRIVER_COUNT
 };
 
 typedef struct {
+  uint16_t const (*vid_pid_list)[2];
+  uint16_t const vid_pid_count;
+  bool (*const open)(uint8_t daddr, const tusb_desc_interface_t *itf_desc, uint16_t max_len);
   void (*const process_set_config)(tuh_xfer_t* xfer);
   bool (*const set_control_line_state)(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
   bool (*const set_baudrate)(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+  bool (*const set_data_format)(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+  bool (*const set_line_coding)(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
 } cdch_serial_driver_t;
 
 // Note driver list must be in the same order as SERIAL_DRIVER enum
 static const cdch_serial_driver_t serial_drivers[] = {
-  { .process_set_config     = acm_process_config,
-    .set_control_line_state = acm_set_control_line_state,
-    .set_baudrate           = acm_set_baudrate
+  {
+      .vid_pid_list           = NULL,
+      .vid_pid_count          = 0,
+      .open                   = acm_open,
+      .process_set_config     = acm_process_config,
+      .set_control_line_state = acm_set_control_line_state,
+      .set_baudrate           = acm_set_baudrate,
+      .set_data_format        = acm_set_data_format,
+      .set_line_coding        = acm_set_line_coding
   },
 
   #if CFG_TUH_CDC_FTDI
-  { .process_set_config     = ftdi_process_config,
-    .set_control_line_state = ftdi_sio_set_modem_ctrl,
-    .set_baudrate           = ftdi_sio_set_baudrate
+  {
+      .vid_pid_list           = ftdi_vid_pid_list,
+      .vid_pid_count          = TU_ARRAY_SIZE(ftdi_vid_pid_list),
+      .open                   = ftdi_open,
+      .process_set_config     = ftdi_process_config,
+      .set_control_line_state = ftdi_sio_set_modem_ctrl,
+      .set_baudrate           = ftdi_sio_set_baudrate,
+      .set_data_format        = ftdi_set_data_format,
+      .set_line_coding        = ftdi_set_line_coding
   },
   #endif
 
   #if CFG_TUH_CDC_CP210X
-  { .process_set_config     = cp210x_process_config,
-    .set_control_line_state = cp210x_set_modem_ctrl,
-    .set_baudrate           = cp210x_set_baudrate
+  {
+      .vid_pid_list           = cp210x_vid_pid_list,
+      .vid_pid_count          = TU_ARRAY_SIZE(cp210x_vid_pid_list),
+      .open                   = cp210x_open,
+      .process_set_config     = cp210x_process_config,
+      .set_control_line_state = cp210x_set_modem_ctrl,
+      .set_baudrate           = cp210x_set_baudrate,
+      .set_data_format        = cp210x_set_data_format,
+      .set_line_coding        = cp210x_set_line_coding
   },
   #endif
-};
 
-enum {
-  SERIAL_DRIVER_COUNT = sizeof(serial_drivers) / sizeof(serial_drivers[0])
+  #if CFG_TUH_CDC_CH34X
+  {
+      .vid_pid_list           = ch34x_vid_pid_list,
+      .vid_pid_count          = TU_ARRAY_SIZE(ch34x_vid_pid_list),
+      .open                   = ch34x_open,
+      .process_set_config     = ch34x_process_config,
+      .set_control_line_state = ch34x_set_modem_ctrl,
+      .set_baudrate           = ch34x_set_baudrate,
+      .set_data_format        = ch34x_set_data_format,
+      .set_line_coding        = ch34x_set_line_coding
+  },
+  #endif
 };
 
+TU_VERIFY_STATIC(TU_ARRAY_SIZE(serial_drivers) == SERIAL_DRIVER_COUNT, "Serial driver count mismatch");
+
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
 //--------------------------------------------------------------------+
 
-static inline cdch_interface_t* get_itf(uint8_t idx)
-{
+static inline cdch_interface_t* get_itf(uint8_t idx) {
   TU_ASSERT(idx < CFG_TUH_CDC, NULL);
   cdch_interface_t* p_cdc = &cdch_data[idx];
 
   return (p_cdc->daddr != 0) ? p_cdc : NULL;
 }
 
-static inline uint8_t get_idx_by_ep_addr(uint8_t daddr, uint8_t ep_addr)
-{
-  for(uint8_t i=0; i<CFG_TUH_CDC; i++)
-  {
+static inline uint8_t get_idx_by_ep_addr(uint8_t daddr, uint8_t ep_addr) {
+  for(uint8_t i=0; i<CFG_TUH_CDC; i++) {
     cdch_interface_t* p_cdc = &cdch_data[i];
     if ( (p_cdc->daddr == daddr) &&
-         (ep_addr == p_cdc->ep_notif || ep_addr == p_cdc->stream.rx.ep_addr || ep_addr == p_cdc->stream.tx.ep_addr))
-    {
+         (ep_addr == p_cdc->ep_notif || ep_addr == p_cdc->stream.rx.ep_addr || ep_addr == p_cdc->stream.tx.ep_addr)) {
       return i;
     }
   }
@@ -193,14 +249,10 @@ static inline uint8_t get_idx_by_ep_addr(uint8_t daddr, uint8_t ep_addr)
   return TUSB_INDEX_INVALID_8;
 }
 
-
-static cdch_interface_t* make_new_itf(uint8_t daddr, tusb_desc_interface_t const *itf_desc)
-{
-  for(uint8_t i=0; i<CFG_TUH_CDC; i++)
-  {
+static cdch_interface_t* make_new_itf(uint8_t daddr, tusb_desc_interface_t const *itf_desc) {
+  for(uint8_t i=0; i<CFG_TUH_CDC; i++) {
     if (cdch_data[i].daddr == 0) {
       cdch_interface_t* p_cdc = &cdch_data[i];
-
       p_cdc->daddr              = daddr;
       p_cdc->bInterfaceNumber   = itf_desc->bInterfaceNumber;
       p_cdc->bInterfaceSubClass = itf_desc->bInterfaceSubClass;
@@ -221,20 +273,16 @@ static void cdch_internal_control_complete(tuh_xfer_t* xfer);
 // APPLICATION API
 //--------------------------------------------------------------------+
 
-uint8_t tuh_cdc_itf_get_index(uint8_t daddr, uint8_t itf_num)
-{
-  for(uint8_t i=0; i<CFG_TUH_CDC; i++)
-  {
+uint8_t tuh_cdc_itf_get_index(uint8_t daddr, uint8_t itf_num) {
+  for (uint8_t i = 0; i < CFG_TUH_CDC; i++) {
     const cdch_interface_t* p_cdc = &cdch_data[i];
-
     if (p_cdc->daddr == daddr && p_cdc->bInterfaceNumber == itf_num) return i;
   }
 
   return TUSB_INDEX_INVALID_8;
 }
 
-bool tuh_cdc_itf_get_info(uint8_t idx, tuh_itf_info_t* info)
-{
+bool tuh_cdc_itf_get_info(uint8_t idx, tuh_itf_info_t* info) {
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_VERIFY(p_cdc && info);
 
@@ -256,30 +304,27 @@ bool tuh_cdc_itf_get_info(uint8_t idx, tuh_itf_info_t* info)
   return true;
 }
 
-bool tuh_cdc_mounted(uint8_t idx)
-{
+bool tuh_cdc_mounted(uint8_t idx) {
   cdch_interface_t* p_cdc = get_itf(idx);
-  return p_cdc != NULL;
+  TU_VERIFY(p_cdc);
+  return p_cdc->mounted;
 }
 
-bool tuh_cdc_get_dtr(uint8_t idx)
-{
+bool tuh_cdc_get_dtr(uint8_t idx) {
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_VERIFY(p_cdc);
 
   return (p_cdc->line_state & CDC_CONTROL_LINE_STATE_DTR) ? true : false;
 }
 
-bool tuh_cdc_get_rts(uint8_t idx)
-{
+bool tuh_cdc_get_rts(uint8_t idx) {
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_VERIFY(p_cdc);
 
   return (p_cdc->line_state & CDC_CONTROL_LINE_STATE_RTS) ? true : false;
 }
 
-bool tuh_cdc_get_local_line_coding(uint8_t idx, cdc_line_coding_t* line_coding)
-{
+bool tuh_cdc_get_local_line_coding(uint8_t idx, cdc_line_coding_t* line_coding) {
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_VERIFY(p_cdc);
 
@@ -292,32 +337,28 @@ bool tuh_cdc_get_local_line_coding(uint8_t idx, cdc_line_coding_t* line_coding)
 // Write
 //--------------------------------------------------------------------+
 
-uint32_t tuh_cdc_write(uint8_t idx, void const* buffer, uint32_t bufsize)
-{
+uint32_t tuh_cdc_write(uint8_t idx, void const* buffer, uint32_t bufsize) {
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_VERIFY(p_cdc);
 
   return tu_edpt_stream_write(&p_cdc->stream.tx, buffer, bufsize);
 }
 
-uint32_t tuh_cdc_write_flush(uint8_t idx)
-{
+uint32_t tuh_cdc_write_flush(uint8_t idx) {
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_VERIFY(p_cdc);
 
   return tu_edpt_stream_write_xfer(&p_cdc->stream.tx);
 }
 
-bool tuh_cdc_write_clear(uint8_t idx)
-{
+bool tuh_cdc_write_clear(uint8_t idx) {
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_VERIFY(p_cdc);
 
   return tu_edpt_stream_clear(&p_cdc->stream.tx);
 }
 
-uint32_t tuh_cdc_write_available(uint8_t idx)
-{
+uint32_t tuh_cdc_write_available(uint8_t idx) {
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_VERIFY(p_cdc);
 
@@ -328,32 +369,28 @@ uint32_t tuh_cdc_write_available(uint8_t idx)
 // Read
 //--------------------------------------------------------------------+
 
-uint32_t tuh_cdc_read (uint8_t idx, void* buffer, uint32_t bufsize)
-{
+uint32_t tuh_cdc_read (uint8_t idx, void* buffer, uint32_t bufsize) {
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_VERIFY(p_cdc);
 
   return tu_edpt_stream_read(&p_cdc->stream.rx, buffer, bufsize);
 }
 
-uint32_t tuh_cdc_read_available(uint8_t idx)
-{
+uint32_t tuh_cdc_read_available(uint8_t idx) {
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_VERIFY(p_cdc);
 
   return tu_edpt_stream_read_available(&p_cdc->stream.rx);
 }
 
-bool tuh_cdc_peek(uint8_t idx, uint8_t* ch)
-{
+bool tuh_cdc_peek(uint8_t idx, uint8_t* ch) {
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_VERIFY(p_cdc);
 
   return tu_edpt_stream_peek(&p_cdc->stream.rx, ch);
 }
 
-bool tuh_cdc_read_clear (uint8_t idx)
-{
+bool tuh_cdc_read_clear (uint8_t idx) {
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_VERIFY(p_cdc);
 
@@ -366,28 +403,25 @@ bool tuh_cdc_read_clear (uint8_t idx)
 // Control Endpoint API
 //--------------------------------------------------------------------+
 
-// internal control complete to update state such as line state, encoding
-static void cdch_internal_control_complete(tuh_xfer_t* xfer)
-{
-  uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
+static void process_internal_control_complete(tuh_xfer_t* xfer, uint8_t itf_num) {
   uint8_t idx = tuh_cdc_itf_get_index(xfer->daddr, itf_num);
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_ASSERT(p_cdc, );
+  uint16_t const value = tu_le16toh(xfer->setup->wValue);
 
-  if (xfer->result == XFER_RESULT_SUCCESS)
-  {
+  if (xfer->result == XFER_RESULT_SUCCESS) {
     switch (p_cdc->serial_drid) {
       case SERIAL_DRIVER_ACM:
         switch (xfer->setup->bRequest) {
           case CDC_REQUEST_SET_CONTROL_LINE_STATE:
-            p_cdc->line_state = (uint8_t) tu_le16toh(xfer->setup->wValue);
+            p_cdc->line_state = (uint8_t) value;
             break;
 
           case CDC_REQUEST_SET_LINE_CODING: {
             uint16_t const len = tu_min16(sizeof(cdc_line_coding_t), tu_le16toh(xfer->setup->wLength));
             memcpy(&p_cdc->line_coding, xfer->buffer, len);
-          }
             break;
+          }
 
           default: break;
         }
@@ -397,12 +431,11 @@ static void cdch_internal_control_complete(tuh_xfer_t* xfer)
       case SERIAL_DRIVER_FTDI:
         switch (xfer->setup->bRequest) {
           case FTDI_SIO_MODEM_CTRL:
-            p_cdc->line_state = (uint8_t) (tu_le16toh(xfer->setup->wValue) & 0x00ff);
+            p_cdc->line_state = (uint8_t) value;
             break;
 
           case FTDI_SIO_SET_BAUD_RATE:
-            // convert from divisor to baudrate is not supported
-            p_cdc->line_coding.bit_rate = _ftdi_requested_baud;
+            p_cdc->line_coding.bit_rate = p_cdc->requested_line_coding.bit_rate;
             break;
 
           default: break;
@@ -414,15 +447,61 @@ static void cdch_internal_control_complete(tuh_xfer_t* xfer)
       case SERIAL_DRIVER_CP210X:
         switch(xfer->setup->bRequest) {
           case CP210X_SET_MHS:
-            p_cdc->line_state = (uint8_t) (tu_le16toh(xfer->setup->wValue) & 0x00ff);
+            p_cdc->line_state = (uint8_t) value;
             break;
 
           case CP210X_SET_BAUDRATE: {
             uint32_t baudrate;
             memcpy(&baudrate, xfer->buffer, sizeof(uint32_t));
             p_cdc->line_coding.bit_rate = tu_le32toh(baudrate);
+            break;
           }
+
+          default: break;
+        }
+        break;
+      #endif
+
+      #if CFG_TUH_CDC_CH34X
+      case SERIAL_DRIVER_CH34X:
+        switch (xfer->setup->bRequest) {
+          case CH34X_REQ_WRITE_REG:
+            // register write request
+            switch (value) {
+              case CH34X_REG16_DIVISOR_PRESCALER:
+                // baudrate
+                p_cdc->line_coding.bit_rate = p_cdc->requested_line_coding.bit_rate;
+                break;
+
+              case CH32X_REG16_LCR2_LCR:
+                // data format
+                p_cdc->line_coding.stop_bits = p_cdc->requested_line_coding.stop_bits;
+                p_cdc->line_coding.parity = p_cdc->requested_line_coding.parity;
+                p_cdc->line_coding.data_bits = p_cdc->requested_line_coding.data_bits;
+                break;
+
+              default: break;
+            }
             break;
+
+          case CH34X_REQ_MODEM_CTRL: {
+            // set modem controls RTS/DTR request. Note: signals are inverted
+            uint16_t const modem_signal = ~value;
+            if (modem_signal & CH34X_BIT_RTS) {
+              p_cdc->line_state |= CDC_CONTROL_LINE_STATE_RTS;
+            } else {
+              p_cdc->line_state &= (uint8_t) ~CDC_CONTROL_LINE_STATE_RTS;
+            }
+
+            if (modem_signal & CH34X_BIT_DTR) {
+              p_cdc->line_state |= CDC_CONTROL_LINE_STATE_DTR;
+            } else {
+              p_cdc->line_state &= (uint8_t) ~CDC_CONTROL_LINE_STATE_DTR;
+            }
+            break;
+          }
+
+          default: break;
         }
         break;
       #endif
@@ -437,14 +516,20 @@ static void cdch_internal_control_complete(tuh_xfer_t* xfer)
   }
 }
 
+// internal control complete to update state such as line state, encoding
+static void cdch_internal_control_complete(tuh_xfer_t* xfer) {
+  uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
+  process_internal_control_complete(xfer, itf_num);
+}
+
 bool tuh_cdc_set_control_line_state(uint8_t idx, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   cdch_interface_t* p_cdc = get_itf(idx);
   TU_VERIFY(p_cdc && p_cdc->serial_drid < SERIAL_DRIVER_COUNT);
   cdch_serial_driver_t const* driver = &serial_drivers[p_cdc->serial_drid];
 
-  if ( complete_cb ) {
+  if (complete_cb) {
     return driver->set_control_line_state(p_cdc, line_state, complete_cb, user_data);
-  }else {
+  } else {
     // blocking
     xfer_result_t result = XFER_RESULT_INVALID;
     bool ret = driver->set_control_line_state(p_cdc, line_state, complete_cb, (uintptr_t) &result);
@@ -455,7 +540,6 @@ bool tuh_cdc_set_control_line_state(uint8_t idx, uint16_t line_state, tuh_xfer_c
     }
 
     TU_VERIFY(ret && result == XFER_RESULT_SUCCESS);
-
     p_cdc->line_state = (uint8_t) line_state;
     return true;
   }
@@ -466,9 +550,9 @@ bool tuh_cdc_set_baudrate(uint8_t idx, uint32_t baudrate, tuh_xfer_cb_t complete
   TU_VERIFY(p_cdc && p_cdc->serial_drid < SERIAL_DRIVER_COUNT);
   cdch_serial_driver_t const* driver = &serial_drivers[p_cdc->serial_drid];
 
-  if ( complete_cb ) {
+  if (complete_cb) {
     return driver->set_baudrate(p_cdc, baudrate, complete_cb, user_data);
-  }else {
+  } else {
     // blocking
     xfer_result_t result = XFER_RESULT_INVALID;
     bool ret = driver->set_baudrate(p_cdc, baudrate, complete_cb, (uintptr_t) &result);
@@ -479,25 +563,23 @@ bool tuh_cdc_set_baudrate(uint8_t idx, uint32_t baudrate, tuh_xfer_cb_t complete
     }
 
     TU_VERIFY(ret && result == XFER_RESULT_SUCCESS);
-
     p_cdc->line_coding.bit_rate = baudrate;
     return true;
   }
 }
 
-bool tuh_cdc_set_line_coding(uint8_t idx, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+bool tuh_cdc_set_data_format(uint8_t idx, uint8_t stop_bits, uint8_t parity, uint8_t data_bits,
+                             tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   cdch_interface_t* p_cdc = get_itf(idx);
-  // only ACM support this set line coding request
-  TU_VERIFY(p_cdc && p_cdc->serial_drid == SERIAL_DRIVER_ACM);
-  TU_VERIFY(p_cdc->acm_capability.support_line_request);
+  TU_VERIFY(p_cdc && p_cdc->serial_drid < SERIAL_DRIVER_COUNT);
+  cdch_serial_driver_t const* driver = &serial_drivers[p_cdc->serial_drid];
 
-  if ( complete_cb ) {
-    return acm_set_line_coding(p_cdc, line_coding, complete_cb, user_data);
-  }else {
+  if (complete_cb) {
+    return driver->set_data_format(p_cdc, stop_bits, parity, data_bits, complete_cb, user_data);
+  } else {
     // blocking
     xfer_result_t result = XFER_RESULT_INVALID;
-    bool ret = acm_set_line_coding(p_cdc, line_coding, complete_cb, (uintptr_t) &result);
+    bool ret = driver->set_data_format(p_cdc, stop_bits, parity, data_bits, complete_cb, (uintptr_t) &result);
 
     if (user_data) {
       // user_data is not NULL, return result via user_data
@@ -505,7 +587,31 @@ bool tuh_cdc_set_line_coding(uint8_t idx, cdc_line_coding_t const* line_coding,
     }
 
     TU_VERIFY(ret && result == XFER_RESULT_SUCCESS);
+    p_cdc->line_coding.stop_bits = stop_bits;
+    p_cdc->line_coding.parity = parity;
+    p_cdc->line_coding.data_bits = data_bits;
+    return true;
+  }
+}
+
+bool tuh_cdc_set_line_coding(uint8_t idx, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc && p_cdc->serial_drid < SERIAL_DRIVER_COUNT);
+  cdch_serial_driver_t const* driver = &serial_drivers[p_cdc->serial_drid];
 
+  if ( complete_cb ) {
+    return driver->set_line_coding(p_cdc, line_coding, complete_cb, user_data);
+  } else {
+    // blocking
+    xfer_result_t result = XFER_RESULT_INVALID;
+    bool ret = driver->set_line_coding(p_cdc, line_coding, complete_cb, (uintptr_t) &result);
+
+    if (user_data) {
+      // user_data is not NULL, return result via user_data
+      *((xfer_result_t*) user_data) = result;
+    }
+
+    TU_VERIFY(ret && result == XFER_RESULT_SUCCESS);
     p_cdc->line_coding = *line_coding;
     return true;
   }
@@ -515,39 +621,44 @@ bool tuh_cdc_set_line_coding(uint8_t idx, cdc_line_coding_t const* line_coding,
 // CLASS-USBH API
 //--------------------------------------------------------------------+
 
-void cdch_init(void)
-{
+bool cdch_init(void) {
+  TU_LOG_DRV("sizeof(cdch_interface_t) = %u\r\n", sizeof(cdch_interface_t));
   tu_memclr(cdch_data, sizeof(cdch_data));
-
-  for(size_t i=0; i<CFG_TUH_CDC; i++)
-  {
+  for (size_t i = 0; i < CFG_TUH_CDC; i++) {
     cdch_interface_t* p_cdc = &cdch_data[i];
-
     tu_edpt_stream_init(&p_cdc->stream.tx, true, true, false,
-                          p_cdc->stream.tx_ff_buf, CFG_TUH_CDC_TX_BUFSIZE,
-                          p_cdc->stream.tx_ep_buf, CFG_TUH_CDC_TX_EPSIZE);
+                        p_cdc->stream.tx_ff_buf, CFG_TUH_CDC_TX_BUFSIZE,
+                        p_cdc->stream.tx_ep_buf, CFG_TUH_CDC_TX_EPSIZE);
 
     tu_edpt_stream_init(&p_cdc->stream.rx, true, false, false,
-                          p_cdc->stream.rx_ff_buf, CFG_TUH_CDC_RX_BUFSIZE,
-                          p_cdc->stream.rx_ep_buf, CFG_TUH_CDC_RX_EPSIZE);
+                        p_cdc->stream.rx_ff_buf, CFG_TUH_CDC_RX_BUFSIZE,
+                        p_cdc->stream.rx_ep_buf, CFG_TUH_CDC_RX_EPSIZE);
   }
+
+  return true;
 }
 
-void cdch_close(uint8_t daddr)
-{
-  for(uint8_t idx=0; idx<CFG_TUH_CDC; idx++)
-  {
+bool cdch_deinit(void) {
+  for (size_t i = 0; i < CFG_TUH_CDC; i++) {
+    cdch_interface_t* p_cdc = &cdch_data[i];
+    tu_edpt_stream_deinit(&p_cdc->stream.tx);
+    tu_edpt_stream_deinit(&p_cdc->stream.rx);
+  }
+  return true;
+}
+
+void cdch_close(uint8_t daddr) {
+  for (uint8_t idx = 0; idx < CFG_TUH_CDC; idx++) {
     cdch_interface_t* p_cdc = &cdch_data[idx];
-    if (p_cdc->daddr == daddr)
-    {
+    if (p_cdc->daddr == daddr) {
       TU_LOG_DRV("  CDCh close addr = %u index = %u\r\n", daddr, idx);
 
       // Invoke application callback
       if (tuh_cdc_umount_cb) tuh_cdc_umount_cb(idx);
 
-      //tu_memclr(p_cdc, sizeof(cdch_interface_t));
       p_cdc->daddr = 0;
       p_cdc->bInterfaceNumber = 0;
+      p_cdc->mounted = false;
       tu_edpt_stream_close(&p_cdc->stream.tx);
       tu_edpt_stream_close(&p_cdc->stream.rx);
     }
@@ -571,16 +682,11 @@ bool cdch_xfer_cb(uint8_t daddr, uint8_t ep_addr, xfer_result_t event, uint32_t
       // - xferred_bytes is multiple of EP Packet size and not zero
       tu_edpt_stream_write_zlp_if_needed(&p_cdc->stream.tx, xferred_bytes);
     }
-  }
-  else if ( ep_addr == p_cdc->stream.rx.ep_addr ) {
+  } else if ( ep_addr == p_cdc->stream.rx.ep_addr ) {
     #if CFG_TUH_CDC_FTDI
     if (p_cdc->serial_drid == SERIAL_DRIVER_FTDI) {
       // FTDI reserve 2 bytes for status
-      // FTDI status
-//      uint8_t status[2] = {
-//        p_cdc->stream.rx.ep_buf[0],
-//        p_cdc->stream.rx.ep_buf[1]
-//      };
+      // uint8_t status[2] = {p_cdc->stream.rx.ep_buf[0], p_cdc->stream.rx.ep_buf[1]};
       tu_edpt_stream_read_xfer_complete_offset(&p_cdc->stream.rx, xferred_bytes, 2);
     }else
     #endif
@@ -606,20 +712,15 @@ bool cdch_xfer_cb(uint8_t daddr, uint8_t ep_addr, xfer_result_t event, uint32_t
 // Enumeration
 //--------------------------------------------------------------------+
 
-static bool open_ep_stream_pair(cdch_interface_t* p_cdc, tusb_desc_endpoint_t const *desc_ep)
-{
-  for(size_t i=0; i<2; i++)
-  {
+static bool open_ep_stream_pair(cdch_interface_t* p_cdc, tusb_desc_endpoint_t const* desc_ep) {
+  for (size_t i = 0; i < 2; i++) {
     TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType &&
-              TUSB_XFER_BULK     == desc_ep->bmAttributes.xfer);
-
+              TUSB_XFER_BULK == desc_ep->bmAttributes.xfer);
     TU_ASSERT(tuh_edpt_open(p_cdc->daddr, desc_ep));
 
-    if ( tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN )
-    {
+    if (tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN) {
       tu_edpt_stream_open(&p_cdc->stream.rx, p_cdc->daddr, desc_ep);
-    }else
-    {
+    } else {
       tu_edpt_stream_open(&p_cdc->stream.tx, p_cdc->daddr, desc_ep);
     }
 
@@ -629,49 +730,36 @@ static bool open_ep_stream_pair(cdch_interface_t* p_cdc, tusb_desc_endpoint_t co
   return true;
 }
 
-bool cdch_open(uint8_t rhport, uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
-{
+bool cdch_open(uint8_t rhport, uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len) {
   (void) rhport;
 
-  // Only support ACM subclass
+  // For CDC: only support ACM subclass
   // Note: Protocol 0xFF can be RNDIS device
-  if ( TUSB_CLASS_CDC                           == itf_desc->bInterfaceClass &&
-       CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL == itf_desc->bInterfaceSubClass)
-  {
+  if (TUSB_CLASS_CDC                           == itf_desc->bInterfaceClass &&
+      CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL == itf_desc->bInterfaceSubClass) {
     return acm_open(daddr, itf_desc, max_len);
   }
-  #if CFG_TUH_CDC_FTDI || CFG_TUH_CDC_CP210X
-  else if ( TUSB_CLASS_VENDOR_SPECIFIC == itf_desc->bInterfaceClass )
-  {
+  else if (SERIAL_DRIVER_COUNT > 1 &&
+           TUSB_CLASS_VENDOR_SPECIFIC == itf_desc->bInterfaceClass) {
     uint16_t vid, pid;
     TU_VERIFY(tuh_vid_pid_get(daddr, &vid, &pid));
 
-    #if CFG_TUH_CDC_FTDI
-    if (TU_FTDI_VID == vid) {
-      for (size_t i = 0; i < FTDI_PID_COUNT; i++) {
-        if (ftdi_pids[i] == pid) {
-          return ftdi_open(daddr, itf_desc, max_len);
-        }
-      }
-    }
-    #endif
-
-    #if CFG_TUH_CDC_CP210X
-    if (TU_CP210X_VID == vid) {
-      for (size_t i = 0; i < CP210X_PID_COUNT; i++) {
-        if (cp210x_pids[i] == pid) {
-          return cp210x_open(daddr, itf_desc, max_len);
+    for (size_t dr = 1; dr < SERIAL_DRIVER_COUNT; dr++) {
+      cdch_serial_driver_t const* driver = &serial_drivers[dr];
+      for (size_t i = 0; i < driver->vid_pid_count; i++) {
+        if (driver->vid_pid_list[i][0] == vid && driver->vid_pid_list[i][1] == pid) {
+          return driver->open(daddr, itf_desc, max_len);
         }
       }
     }
-    #endif
   }
-  #endif
 
   return false;
 }
 
 static void set_config_complete(cdch_interface_t * p_cdc, uint8_t idx, uint8_t itf_num) {
+  TU_LOG_DRV("CDCh Set Configure complete\r\n");
+  p_cdc->mounted = true;
   if (tuh_cdc_mount_cb) tuh_cdc_mount_cb(idx);
 
   // Prepare for incoming data
@@ -681,9 +769,7 @@ static void set_config_complete(cdch_interface_t * p_cdc, uint8_t idx, uint8_t i
   usbh_driver_set_config_complete(p_cdc->daddr, itf_num);
 }
 
-
-bool cdch_set_config(uint8_t daddr, uint8_t itf_num)
-{
+bool cdch_set_config(uint8_t daddr, uint8_t itf_num) {
   tusb_control_request_t request;
   request.wIndex = tu_htole16((uint16_t) itf_num);
 
@@ -712,94 +798,84 @@ enum {
   CONFIG_ACM_COMPLETE,
 };
 
-static bool acm_open(uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
-{
-  uint8_t const * p_desc_end = ((uint8_t const*) itf_desc) + max_len;
+static bool acm_open(uint8_t daddr, tusb_desc_interface_t const* itf_desc, uint16_t max_len) {
+  uint8_t const* p_desc_end = ((uint8_t const*) itf_desc) + max_len;
 
-  cdch_interface_t * p_cdc = make_new_itf(daddr, itf_desc);
+  cdch_interface_t* p_cdc = make_new_itf(daddr, itf_desc);
   TU_VERIFY(p_cdc);
-
   p_cdc->serial_drid = SERIAL_DRIVER_ACM;
 
   //------------- Control Interface -------------//
-  uint8_t const * p_desc = tu_desc_next(itf_desc);
+  uint8_t const* p_desc = tu_desc_next(itf_desc);
 
   // Communication Functional Descriptors
-  while( (p_desc < p_desc_end) && (TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc)) )
-  {
-    if ( CDC_FUNC_DESC_ABSTRACT_CONTROL_MANAGEMENT == cdc_functional_desc_typeof(p_desc) )
-    {
+  while ((p_desc < p_desc_end) && (TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc))) {
+    if (CDC_FUNC_DESC_ABSTRACT_CONTROL_MANAGEMENT == cdc_functional_desc_typeof(p_desc)) {
       // save ACM bmCapabilities
-      p_cdc->acm_capability = ((cdc_desc_func_acm_t const *) p_desc)->bmCapabilities;
+      p_cdc->acm_capability = ((cdc_desc_func_acm_t const*) p_desc)->bmCapabilities;
     }
 
     p_desc = tu_desc_next(p_desc);
   }
 
   // Open notification endpoint of control interface if any
-  if (itf_desc->bNumEndpoints == 1)
-  {
+  if (itf_desc->bNumEndpoints == 1) {
     TU_ASSERT(TUSB_DESC_ENDPOINT == tu_desc_type(p_desc));
-    tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+    tusb_desc_endpoint_t const* desc_ep = (tusb_desc_endpoint_t const*) p_desc;
 
-    TU_ASSERT( tuh_edpt_open(daddr, desc_ep) );
+    TU_ASSERT(tuh_edpt_open(daddr, desc_ep));
     p_cdc->ep_notif = desc_ep->bEndpointAddress;
 
     p_desc = tu_desc_next(p_desc);
   }
 
   //------------- Data Interface (if any) -------------//
-  if ( (TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) &&
-       (TUSB_CLASS_CDC_DATA == ((tusb_desc_interface_t const *) p_desc)->bInterfaceClass) )
-  {
+  if ((TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) &&
+      (TUSB_CLASS_CDC_DATA == ((tusb_desc_interface_t const*) p_desc)->bInterfaceClass)) {
     // next to endpoint descriptor
     p_desc = tu_desc_next(p_desc);
 
     // data endpoints expected to be in pairs
-    TU_ASSERT(open_ep_stream_pair(p_cdc, (tusb_desc_endpoint_t const *) p_desc));
+    TU_ASSERT(open_ep_stream_pair(p_cdc, (tusb_desc_endpoint_t const*) p_desc));
   }
 
   return true;
 }
 
-static void acm_process_config(tuh_xfer_t* xfer)
-{
+static void acm_process_config(tuh_xfer_t* xfer) {
   uintptr_t const state = xfer->user_data;
   uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
   uint8_t const idx = tuh_cdc_itf_get_index(xfer->daddr, itf_num);
-  cdch_interface_t * p_cdc = get_itf(idx);
-  TU_ASSERT(p_cdc, );
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_ASSERT(p_cdc,);
 
-  switch(state)
-  {
+  switch (state) {
     case CONFIG_ACM_SET_CONTROL_LINE_STATE:
       #if CFG_TUH_CDC_LINE_CONTROL_ON_ENUM
-      if (p_cdc->acm_capability.support_line_request)
-      {
-        TU_ASSERT(acm_set_control_line_state(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, acm_process_config,
-                                             CONFIG_ACM_SET_LINE_CODING), );
+      if (p_cdc->acm_capability.support_line_request) {
+        TU_ASSERT(acm_set_control_line_state(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, acm_process_config, CONFIG_ACM_SET_LINE_CODING),);
         break;
       }
-          #endif
+      #endif
       TU_ATTR_FALLTHROUGH;
 
     case CONFIG_ACM_SET_LINE_CODING:
-        #ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
-      if (p_cdc->acm_capability.support_line_request)
-      {
+      #ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
+      if (p_cdc->acm_capability.support_line_request) {
         cdc_line_coding_t line_coding = CFG_TUH_CDC_LINE_CODING_ON_ENUM;
-        TU_ASSERT(acm_set_line_coding(p_cdc, &line_coding, acm_process_config, CONFIG_ACM_COMPLETE), );
+        TU_ASSERT(acm_set_line_coding(p_cdc, &line_coding, acm_process_config, CONFIG_ACM_COMPLETE),);
         break;
       }
-        #endif
+      #endif
       TU_ATTR_FALLTHROUGH;
 
     case CONFIG_ACM_COMPLETE:
       // itf_num+1 to account for data interface as well
-      set_config_complete(p_cdc, idx, itf_num+1);
+      set_config_complete(p_cdc, idx, itf_num + 1);
       break;
 
-    default: break;
+    default:
+      break;
   }
 }
 
@@ -867,6 +943,19 @@ static bool acm_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const
   return true;
 }
 
+static bool acm_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits,
+                                tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  TU_LOG_DRV("CDC ACM Set Data Format\r\n");
+
+  cdc_line_coding_t line_coding;
+  line_coding.bit_rate = p_cdc->line_coding.bit_rate;
+  line_coding.stop_bits = stop_bits;
+  line_coding.parity = parity;
+  line_coding.data_bits = data_bits;
+
+  return acm_set_line_coding(p_cdc, &line_coding, complete_cb, user_data);
+}
+
 static bool acm_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   TU_VERIFY(p_cdc->acm_capability.support_line_request);
   cdc_line_coding_t line_coding = p_cdc->line_coding;
@@ -896,7 +985,6 @@ static bool ftdi_open(uint8_t daddr, const tusb_desc_interface_t *itf_desc, uint
   TU_VERIFY(p_cdc);
 
   TU_LOG_DRV("FTDI opened\r\n");
-
   p_cdc->serial_drid = SERIAL_DRIVER_FTDI;
 
   // endpoint pair
@@ -932,13 +1020,32 @@ static bool ftdi_sio_set_request(cdch_interface_t* p_cdc, uint8_t command, uint1
   return tuh_control_xfer(&xfer);
 }
 
-static bool ftdi_sio_reset(cdch_interface_t* p_cdc, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+static bool ftdi_sio_reset(cdch_interface_t* p_cdc, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   return ftdi_sio_set_request(p_cdc, FTDI_SIO_RESET, FTDI_SIO_RESET_SIO, complete_cb, user_data);
 }
 
-static bool ftdi_sio_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+static bool ftdi_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits,
+                                 tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  (void) p_cdc;
+  (void) stop_bits;
+  (void) parity;
+  (void) data_bits;
+  (void) complete_cb;
+  (void) user_data;
+  // TODO not implemented yet
+  return false;
+}
+
+static bool ftdi_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  (void) p_cdc;
+  (void) line_coding;
+  (void) complete_cb;
+  (void) user_data;
+  // TODO not implemented yet
+  return false;
+}
+
+static bool ftdi_sio_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   TU_LOG_DRV("CDC FTDI Set Control Line State\r\n");
   p_cdc->user_control_cb = complete_cb;
   TU_ASSERT(ftdi_sio_set_request(p_cdc, FTDI_SIO_MODEM_CTRL, 0x0300 | line_state,
@@ -946,8 +1053,7 @@ static bool ftdi_sio_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state
   return true;
 }
 
-static uint32_t ftdi_232bm_baud_base_to_divisor(uint32_t baud, uint32_t base)
-{
+static uint32_t ftdi_232bm_baud_base_to_divisor(uint32_t baud, uint32_t base) {
   const uint8_t divfrac[8] = { 0, 3, 2, 4, 1, 5, 6, 7 };
   uint32_t divisor;
 
@@ -967,18 +1073,16 @@ static uint32_t ftdi_232bm_baud_base_to_divisor(uint32_t baud, uint32_t base)
   return divisor;
 }
 
-static uint32_t ftdi_232bm_baud_to_divisor(uint32_t baud)
-{
+static uint32_t ftdi_232bm_baud_to_divisor(uint32_t baud) {
   return ftdi_232bm_baud_base_to_divisor(baud, 48000000u);
 }
 
-static bool ftdi_sio_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+static bool ftdi_sio_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   uint16_t const divisor = (uint16_t) ftdi_232bm_baud_to_divisor(baudrate);
-  TU_LOG_DRV("CDC FTDI Set BaudRate = %lu, divisor = 0x%04x\r\n", baudrate, divisor);
+  TU_LOG_DRV("CDC FTDI Set BaudRate = %" PRIu32 ", divisor = 0x%04x\r\n", baudrate, divisor);
 
   p_cdc->user_control_cb = complete_cb;
-  _ftdi_requested_baud = baudrate;
+  p_cdc->requested_line_coding.bit_rate = baudrate;
   TU_ASSERT(ftdi_sio_set_request(p_cdc, FTDI_SIO_SET_BAUD_RATE, divisor,
                                  complete_cb ? cdch_internal_control_complete : NULL, user_data));
 
@@ -1000,8 +1104,7 @@ static void ftdi_process_config(tuh_xfer_t* xfer) {
 
     case CONFIG_FTDI_MODEM_CTRL:
       #if CFG_TUH_CDC_LINE_CONTROL_ON_ENUM
-      TU_ASSERT(
-        ftdi_sio_set_modem_ctrl(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, ftdi_process_config, CONFIG_FTDI_SET_BAUDRATE),);
+      TU_ASSERT(ftdi_sio_set_modem_ctrl(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, ftdi_process_config, CONFIG_FTDI_SET_BAUDRATE),);
       break;
       #else
       TU_ATTR_FALLTHROUGH;
@@ -1109,16 +1212,36 @@ static bool cp210x_ifc_enable(cdch_interface_t* p_cdc, uint16_t enabled, tuh_xfe
   return cp210x_set_request(p_cdc, CP210X_IFC_ENABLE, enabled, NULL, 0, complete_cb, user_data);
 }
 
+static bool cp210x_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  // TODO implement later
+  (void) p_cdc;
+  (void) line_coding;
+  (void) complete_cb;
+  (void) user_data;
+  return false;
+}
+
 static bool cp210x_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
-  TU_LOG_DRV("CDC CP210x Set BaudRate = %lu\r\n", baudrate);
+  TU_LOG_DRV("CDC CP210x Set BaudRate = %" PRIu32 "\r\n", baudrate);
   uint32_t baud_le = tu_htole32(baudrate);
   p_cdc->user_control_cb = complete_cb;
   return cp210x_set_request(p_cdc, CP210X_SET_BAUDRATE, 0, (uint8_t *) &baud_le, 4,
                             complete_cb ? cdch_internal_control_complete : NULL, user_data);
 }
 
-static bool cp210x_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+static bool cp210x_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits,
+                                   tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  (void) p_cdc;
+  (void) stop_bits;
+  (void) parity;
+  (void) data_bits;
+  (void) complete_cb;
+  (void) user_data;
+  // TODO not implemented yet
+  return false;
+}
+
+static bool cp210x_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   TU_LOG_DRV("CDC CP210x Set Control Line State\r\n");
   p_cdc->user_control_cb = complete_cb;
   return cp210x_set_request(p_cdc, CP210X_SET_MHS, 0x0300 | line_state, NULL, 0,
@@ -1158,8 +1281,7 @@ static void cp210x_process_config(tuh_xfer_t* xfer) {
 
     case CONFIG_CP210X_SET_DTR_RTS:
       #if CFG_TUH_CDC_LINE_CONTROL_ON_ENUM
-      TU_ASSERT(
-        cp210x_set_modem_ctrl(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, cp210x_process_config, CONFIG_CP210X_COMPLETE),);
+      TU_ASSERT(cp210x_set_modem_ctrl(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, cp210x_process_config, CONFIG_CP210X_COMPLETE),);
       break;
       #else
       TU_ATTR_FALLTHROUGH;
@@ -1175,4 +1297,374 @@ static void cp210x_process_config(tuh_xfer_t* xfer) {
 
 #endif
 
+//--------------------------------------------------------------------+
+// CH34x (CH340 & CH341)
+//--------------------------------------------------------------------+
+
+#if CFG_TUH_CDC_CH34X
+
+static uint8_t ch34x_get_lcr(uint8_t stop_bits, uint8_t parity, uint8_t data_bits);
+static uint16_t ch34x_get_divisor_prescaler(uint32_t baval);
+
+//------------- control request -------------//
+
+static bool ch34x_set_request(cdch_interface_t* p_cdc, uint8_t direction, uint8_t request, uint16_t value,
+                              uint16_t index, uint8_t* buffer, uint16_t length, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  tusb_control_request_t const request_setup = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_DEVICE,
+          .type      = TUSB_REQ_TYPE_VENDOR,
+          .direction = direction & 0x01u
+      },
+      .bRequest = request,
+      .wValue   = tu_htole16 (value),
+      .wIndex   = tu_htole16 (index),
+      .wLength  = tu_htole16 (length)
+  };
+
+  // use usbh enum buf since application variable does not live long enough
+  uint8_t* enum_buf = NULL;
+
+  if (buffer && length > 0) {
+    enum_buf = usbh_get_enum_buf();
+    if (direction == TUSB_DIR_OUT) {
+      tu_memcpy_s(enum_buf, CFG_TUH_ENUMERATION_BUFSIZE, buffer, length);
+    }
+  }
+
+  tuh_xfer_t xfer = {
+      .daddr       = p_cdc->daddr,
+      .ep_addr     = 0,
+      .setup       = &request_setup,
+      .buffer      = enum_buf,
+      .complete_cb = complete_cb,
+      .user_data   = user_data
+  };
+
+  return tuh_control_xfer(&xfer);
+}
+
+static inline bool ch34x_control_out(cdch_interface_t* p_cdc, uint8_t request, uint16_t value, uint16_t index,
+                                     tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  return ch34x_set_request(p_cdc, TUSB_DIR_OUT, request, value, index, NULL, 0, complete_cb, user_data);
+}
+
+static inline bool ch34x_control_in(cdch_interface_t* p_cdc, uint8_t request, uint16_t value, uint16_t index,
+                                    uint8_t* buffer, uint16_t buffersize, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  return ch34x_set_request(p_cdc, TUSB_DIR_IN, request, value, index, buffer, buffersize,
+                           complete_cb, user_data);
+}
+
+static inline bool ch34x_write_reg(cdch_interface_t* p_cdc, uint16_t reg, uint16_t reg_value, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  return ch34x_control_out(p_cdc, CH34X_REQ_WRITE_REG, reg, reg_value, complete_cb, user_data);
+}
+
+//static bool ch34x_read_reg_request ( cdch_interface_t* p_cdc, uint16_t reg,
+//                                     uint8_t *buffer, uint16_t buffersize, tuh_xfer_cb_t complete_cb, uintptr_t user_data )
+//{
+//  return ch34x_control_in ( p_cdc, CH34X_REQ_READ_REG, reg, 0, buffer, buffersize, complete_cb, user_data );
+//}
+
+static bool ch34x_write_reg_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate,
+                                     tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  uint16_t const div_ps = ch34x_get_divisor_prescaler(baudrate);
+  TU_VERIFY(div_ps);
+  TU_ASSERT(ch34x_write_reg(p_cdc, CH34X_REG16_DIVISOR_PRESCALER, div_ps,
+                            complete_cb, user_data));
+  return true;
+}
+
+//------------- Driver API -------------//
+
+// internal control complete to update state such as line state, encoding
+static void ch34x_control_complete(tuh_xfer_t* xfer) {
+  // CH34x only has 1 interface and use wIndex as payload and not for bInterfaceNumber
+  process_internal_control_complete(xfer, 0);
+}
+
+static bool ch34x_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits,
+                                tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  p_cdc->requested_line_coding.stop_bits = stop_bits;
+  p_cdc->requested_line_coding.parity = parity;
+  p_cdc->requested_line_coding.data_bits = data_bits;
+
+  uint8_t const lcr = ch34x_get_lcr(stop_bits, parity, data_bits);
+  TU_VERIFY(lcr);
+  TU_ASSERT (ch34x_control_out(p_cdc, CH34X_REQ_WRITE_REG, CH32X_REG16_LCR2_LCR, lcr,
+                               complete_cb ? ch34x_control_complete : NULL, user_data));
+  return true;
+}
+
+static bool ch34x_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate,
+                               tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  p_cdc->requested_line_coding.bit_rate = baudrate;
+  p_cdc->user_control_cb = complete_cb;
+  TU_ASSERT(ch34x_write_reg_baudrate(p_cdc, baudrate,
+                                     complete_cb ? ch34x_control_complete : NULL, user_data));
+  return true;
+}
+
+static void ch34x_set_line_coding_stage1_complete(tuh_xfer_t* xfer) {
+  // CH34x only has 1 interface and use wIndex as payload and not for bInterfaceNumber
+  uint8_t const itf_num = 0;
+  uint8_t const idx = tuh_cdc_itf_get_index(xfer->daddr, itf_num);
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_ASSERT(p_cdc, );
+
+  if (xfer->result == XFER_RESULT_SUCCESS) {
+    // stage 1 success, continue to stage 2
+    p_cdc->line_coding.bit_rate = p_cdc->requested_line_coding.bit_rate;
+    TU_ASSERT(ch34x_set_data_format(p_cdc, p_cdc->requested_line_coding.stop_bits, p_cdc->requested_line_coding.parity,
+                                    p_cdc->requested_line_coding.data_bits, ch34x_control_complete, xfer->user_data), );
+  } else {
+    // stage 1 failed, notify user
+    xfer->complete_cb = p_cdc->user_control_cb;
+    if (xfer->complete_cb) {
+      xfer->complete_cb(xfer);
+    }
+  }
+}
+
+// 2 stages: set baudrate (stage1) + set data format (stage2)
+static bool ch34x_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding,
+                                  tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  p_cdc->requested_line_coding = *line_coding;
+  p_cdc->user_control_cb = complete_cb;
+
+  if (complete_cb) {
+    // stage 1 set baudrate
+    TU_ASSERT(ch34x_write_reg_baudrate(p_cdc, line_coding->bit_rate,
+                                       ch34x_set_line_coding_stage1_complete, user_data));
+  } else {
+    // sync call
+    xfer_result_t result;
+
+    // stage 1 set baudrate
+    TU_ASSERT(ch34x_write_reg_baudrate(p_cdc, line_coding->bit_rate, NULL, (uintptr_t) &result));
+    TU_VERIFY(result == XFER_RESULT_SUCCESS);
+    p_cdc->line_coding.bit_rate = line_coding->bit_rate;
+
+    // stage 2 set data format
+    TU_ASSERT(ch34x_set_data_format(p_cdc, line_coding->stop_bits, line_coding->parity, line_coding->data_bits,
+                                    NULL, (uintptr_t) &result));
+    TU_VERIFY(result == XFER_RESULT_SUCCESS);
+    p_cdc->line_coding.stop_bits = line_coding->stop_bits;
+    p_cdc->line_coding.parity = line_coding->parity;
+    p_cdc->line_coding.data_bits = line_coding->data_bits;
+
+    // update transfer result, user_data is expected to point to xfer_result_t
+    if (user_data) {
+      *((xfer_result_t*) user_data) = result;
+    }
+  }
+
+  return true;
+}
+
+static bool ch34x_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state,
+                                 tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  uint8_t control = 0;
+  if (line_state & CDC_CONTROL_LINE_STATE_RTS) {
+    control |= CH34X_BIT_RTS;
+  }
+  if (line_state & CDC_CONTROL_LINE_STATE_DTR) {
+    control |= CH34X_BIT_DTR;
+  }
+
+  // CH34x signals are inverted
+  control = ~control;
+
+  p_cdc->user_control_cb = complete_cb;
+  TU_ASSERT (ch34x_control_out(p_cdc, CH34X_REQ_MODEM_CTRL, control, 0,
+                               complete_cb ? ch34x_control_complete : NULL, user_data));
+  return true;
+}
+
+//------------- Enumeration -------------//
+enum {
+  CONFIG_CH34X_READ_VERSION = 0,
+  CONFIG_CH34X_SERIAL_INIT,
+  CONFIG_CH34X_SPECIAL_REG_WRITE,
+  CONFIG_CH34X_FLOW_CONTROL,
+  CONFIG_CH34X_MODEM_CONTROL,
+  CONFIG_CH34X_COMPLETE
+};
+
+static bool ch34x_open(uint8_t daddr, tusb_desc_interface_t const* itf_desc, uint16_t max_len) {
+  // CH34x Interface includes 1 vendor interface + 2 bulk + 1 interrupt endpoints
+  TU_VERIFY (itf_desc->bNumEndpoints == 3);
+  TU_VERIFY (sizeof(tusb_desc_interface_t) + 3 * sizeof(tusb_desc_endpoint_t) <= max_len);
+
+  cdch_interface_t* p_cdc = make_new_itf(daddr, itf_desc);
+  TU_VERIFY (p_cdc);
+
+  TU_LOG_DRV ("CH34x opened\r\n");
+  p_cdc->serial_drid = SERIAL_DRIVER_CH34X;
+
+  tusb_desc_endpoint_t const* desc_ep = (tusb_desc_endpoint_t const*) tu_desc_next(itf_desc);
+
+  // data endpoints expected to be in pairs
+  TU_ASSERT(open_ep_stream_pair(p_cdc, desc_ep));
+  desc_ep += 2;
+
+  // Interrupt endpoint: not used for now
+  TU_ASSERT(TUSB_DESC_ENDPOINT == tu_desc_type(desc_ep) &&
+            TUSB_XFER_INTERRUPT == desc_ep->bmAttributes.xfer);
+  TU_ASSERT(tuh_edpt_open(daddr, desc_ep));
+  p_cdc->ep_notif = desc_ep->bEndpointAddress;
+
+  return true;
+}
+
+static void ch34x_process_config(tuh_xfer_t* xfer) {
+  // CH34x only has 1 interface and use wIndex as payload and not for bInterfaceNumber
+  uint8_t const itf_num = 0;
+  uint8_t const idx = tuh_cdc_itf_get_index(xfer->daddr, itf_num);
+  cdch_interface_t* p_cdc = get_itf(idx);
+  uintptr_t const state = xfer->user_data;
+  uint8_t buffer[2]; // TODO remove
+  TU_ASSERT (p_cdc,);
+  TU_ASSERT (xfer->result == XFER_RESULT_SUCCESS,);
+
+  switch (state) {
+    case CONFIG_CH34X_READ_VERSION:
+      TU_LOG_DRV("[%u] CDCh CH34x attempt to read Chip Version\r\n", p_cdc->daddr);
+      TU_ASSERT (ch34x_control_in(p_cdc, CH34X_REQ_READ_VERSION, 0, 0, buffer, 2, ch34x_process_config, CONFIG_CH34X_SERIAL_INIT),);
+      break;
+
+    case CONFIG_CH34X_SERIAL_INIT: {
+      // handle version read data, set CH34x line coding (incl. baudrate)
+      uint8_t const version = xfer->buffer[0];
+      TU_LOG_DRV("[%u] CDCh CH34x Chip Version = %02x\r\n", p_cdc->daddr, version);
+      // only versions >= 0x30 are tested, below 0x30 seems having other programming, see drivers from WCH vendor, Linux kernel and FreeBSD
+      TU_ASSERT (version >= 0x30,);
+      // init CH34x with line coding
+      cdc_line_coding_t const line_coding = CFG_TUH_CDC_LINE_CODING_ON_ENUM_CH34X;
+      uint16_t const div_ps = ch34x_get_divisor_prescaler(line_coding.bit_rate);
+      TU_ASSERT(div_ps, );
+      uint8_t const lcr = ch34x_get_lcr(line_coding.stop_bits, line_coding.parity, line_coding.data_bits);
+      TU_ASSERT(lcr, );
+      TU_ASSERT (ch34x_control_out(p_cdc, CH34X_REQ_SERIAL_INIT, tu_u16(lcr, 0x9c), div_ps,
+                                   ch34x_process_config, CONFIG_CH34X_SPECIAL_REG_WRITE),);
+      break;
+    }
+
+    case CONFIG_CH34X_SPECIAL_REG_WRITE:
+      // overtake line coding and do special reg write, purpose unknown, overtaken from WCH driver
+      p_cdc->line_coding = ((cdc_line_coding_t) CFG_TUH_CDC_LINE_CODING_ON_ENUM_CH34X);
+      TU_ASSERT (ch34x_write_reg(p_cdc, TU_U16(CH341_REG_0x0F, CH341_REG_0x2C), 0x0007, ch34x_process_config, CONFIG_CH34X_FLOW_CONTROL),);
+      break;
+
+    case CONFIG_CH34X_FLOW_CONTROL:
+      // no hardware flow control
+      TU_ASSERT (ch34x_write_reg(p_cdc, TU_U16(CH341_REG_0x27, CH341_REG_0x27), 0x0000, ch34x_process_config, CONFIG_CH34X_MODEM_CONTROL),);
+      break;
+
+    case CONFIG_CH34X_MODEM_CONTROL:
+      // !always! set modem controls RTS/DTR (CH34x has no reset state after CH34X_REQ_SERIAL_INIT)
+      TU_ASSERT (ch34x_set_modem_ctrl(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, ch34x_process_config, CONFIG_CH34X_COMPLETE),);
+      break;
+
+    case CONFIG_CH34X_COMPLETE:
+      set_config_complete(p_cdc, idx, itf_num);
+      break;
+
+    default:
+      TU_ASSERT (false,);
+      break;
+  }
+}
+
+//------------- CH34x helper  -------------//
+
+// calculate divisor and prescaler for baudrate, return it as 16-bit combined value
+static uint16_t ch34x_get_divisor_prescaler(uint32_t baval) {
+  uint8_t a;
+  uint8_t b;
+  uint32_t c;
+
+  TU_VERIFY(baval != 0 && baval <= 2000000, 0);
+  switch (baval) {
+    case 921600:
+      a = 0xf3;
+      b = 7;
+      break;
+
+    case 307200:
+      a = 0xd9;
+      b = 7;
+      break;
+
+    default:
+      if (baval > 6000000 / 255) {
+        b = 3;
+        c = 6000000;
+      } else if (baval > 750000 / 255) {
+        b = 2;
+        c = 750000;
+      } else if (baval > 93750 / 255) {
+        b = 1;
+        c = 93750;
+      } else {
+        b = 0;
+        c = 11719;
+      }
+      a = (uint8_t) (c / baval);
+      if (a == 0 || a == 0xFF) {
+        return 0;
+      }
+      if ((c / a - baval) > (baval - c / (a + 1))) {
+        a++;
+      }
+      a = (uint8_t) (256 - a);
+      break;
+  }
+
+  // reg divisor = a, reg prescaler = b
+  // According to linux code we need to set bit 7 of UCHCOM_REG_BPS_PRE,
+  // otherwise the chip will buffer data.
+  return (uint16_t) ((uint16_t)a << 8 | 0x80 | b);
+}
+
+// calculate lcr value from data coding
+static uint8_t ch34x_get_lcr(uint8_t stop_bits, uint8_t parity, uint8_t data_bits) {
+  uint8_t lcr = CH34X_LCR_ENABLE_RX | CH34X_LCR_ENABLE_TX;
+  TU_VERIFY(data_bits >= 5 && data_bits <= 8, 0);
+  lcr |= (uint8_t) (data_bits - 5);
+
+  switch(parity) {
+    case CDC_LINE_CODING_PARITY_NONE:
+      break;
+
+    case CDC_LINE_CODING_PARITY_ODD:
+      lcr |= CH34X_LCR_ENABLE_PAR;
+      break;
+
+    case CDC_LINE_CODING_PARITY_EVEN:
+      lcr |= CH34X_LCR_ENABLE_PAR | CH34X_LCR_PAR_EVEN;
+      break;
+
+    case CDC_LINE_CODING_PARITY_MARK:
+      lcr |= CH34X_LCR_ENABLE_PAR | CH34X_LCR_MARK_SPACE;
+      break;
+
+    case CDC_LINE_CODING_PARITY_SPACE:
+      lcr |= CH34X_LCR_ENABLE_PAR | CH34X_LCR_MARK_SPACE | CH34X_LCR_PAR_EVEN;
+      break;
+
+    default: break;
+  }
+
+  // 1.5 stop bits not supported
+  TU_VERIFY(stop_bits != CDC_LINE_CODING_STOP_BITS_1_5, 0);
+  if (stop_bits == CDC_LINE_CODING_STOP_BITS_2) {
+    lcr |= CH34X_LCR_STOP_BITS_2;
+  }
+
+  return lcr;
+}
+
+
+#endif // CFG_TUH_CDC_CH34X
+
 #endif
diff --git a/src/class/cdc/cdc_host.h b/src/class/cdc/cdc_host.h
index 9e5edd94e..b63dd1530 100644
--- a/src/class/cdc/cdc_host.h
+++ b/src/class/cdc/cdc_host.h
@@ -148,8 +148,11 @@ bool tuh_cdc_set_control_line_state(uint8_t idx, uint16_t line_state, tuh_xfer_c
 // Request to set baudrate
 bool tuh_cdc_set_baudrate(uint8_t idx, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
 
-// Request to Set Line Coding (ACM only)
-// Should only use if you don't work with serial devices such as FTDI/CP210x
+// Request to set data format
+bool tuh_cdc_set_data_format(uint8_t idx, uint8_t stop_bits, uint8_t parity, uint8_t data_bits, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+
+// Request to Set Line Coding = baudrate + data format
+// Note: only implemented by ACM and CH34x, not supported by FTDI and CP210x yet
 bool tuh_cdc_set_line_coding(uint8_t idx, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
 
 // Request to Get Line Coding (ACM only)
@@ -159,15 +162,13 @@ bool tuh_cdc_set_line_coding(uint8_t idx, cdc_line_coding_t const* line_coding,
 
 // Connect by set both DTR, RTS
 TU_ATTR_ALWAYS_INLINE static inline
-bool tuh_cdc_connect(uint8_t idx, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+bool tuh_cdc_connect(uint8_t idx, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   return tuh_cdc_set_control_line_state(idx, CDC_CONTROL_LINE_STATE_DTR | CDC_CONTROL_LINE_STATE_RTS, complete_cb, user_data);
 }
 
 // Disconnect by clear both DTR, RTS
 TU_ATTR_ALWAYS_INLINE static inline
-bool tuh_cdc_disconnect(uint8_t idx, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+bool tuh_cdc_disconnect(uint8_t idx, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   return tuh_cdc_set_control_line_state(idx, 0x00, complete_cb, user_data);
 }
 
@@ -191,7 +192,8 @@ TU_ATTR_WEAK extern void tuh_cdc_tx_complete_cb(uint8_t idx);
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-void cdch_init       (void);
+bool cdch_init       (void);
+bool cdch_deinit     (void);
 bool cdch_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len);
 bool cdch_set_config (uint8_t dev_addr, uint8_t itf_num);
 bool cdch_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
diff --git a/src/class/cdc/serial/ch34x.h b/src/class/cdc/serial/ch34x.h
new file mode 100644
index 000000000..c18066f57
--- /dev/null
+++ b/src/class/cdc/serial/ch34x.h
@@ -0,0 +1,84 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2023 Heiko Kuester
+ *
+ * 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.
+ */
+
+#ifndef _CH34X_H_
+#define _CH34X_H_
+
+// There is no official documentation for the CH34x (CH340, CH341) chips. Reference can be found
+// - https://github.com/WCHSoftGroup/ch341ser_linux
+// - https://github.com/torvalds/linux/blob/master/drivers/usb/serial/ch341.c
+// - https://github.com/freebsd/freebsd-src/blob/main/sys/dev/usb/serial/uchcom.c
+
+// set line_coding @ enumeration
+#ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
+#define CFG_TUH_CDC_LINE_CODING_ON_ENUM_CH34X CFG_TUH_CDC_LINE_CODING_ON_ENUM
+#else // this default is necessary to work properly
+#define CFG_TUH_CDC_LINE_CODING_ON_ENUM_CH34X { 9600, CDC_LINE_CONDING_STOP_BITS_1, CDC_LINE_CODING_PARITY_NONE, 8 }
+#endif
+
+// USB requests
+#define CH34X_REQ_READ_VERSION 0x5F // dec  95
+#define CH34X_REQ_WRITE_REG    0x9A // dec 154
+#define CH34X_REQ_READ_REG     0x95 // dec 149
+#define CH34X_REQ_SERIAL_INIT  0xA1 // dec 161
+#define CH34X_REQ_MODEM_CTRL   0xA4 // dev 164
+
+// registers
+#define CH34X_REG_BREAK        0x05
+#define CH34X_REG_PRESCALER    0x12
+#define CH34X_REG_DIVISOR      0x13
+#define CH34X_REG_LCR          0x18
+#define CH34X_REG_LCR2         0x25
+#define CH34X_REG_MCR_MSR      0x06
+#define CH34X_REG_MCR_MSR2     0x07
+#define CH34X_NBREAK_BITS      0x01
+
+#define CH341_REG_0x0F         0x0F // undocumented register
+#define CH341_REG_0x2C         0x2C // undocumented register
+#define CH341_REG_0x27         0x27 // hardware flow control (cts/rts)
+
+#define CH34X_REG16_DIVISOR_PRESCALER  TU_U16(CH34X_REG_DIVISOR, CH34X_REG_PRESCALER)
+#define CH32X_REG16_LCR2_LCR           TU_U16(CH34X_REG_LCR2, CH34X_REG_LCR)
+
+// modem control bits
+#define CH34X_BIT_RTS ( 1 << 6 )
+#define CH34X_BIT_DTR ( 1 << 5 )
+
+// line control bits
+#define CH34X_LCR_ENABLE_RX    0x80
+#define CH34X_LCR_ENABLE_TX    0x40
+#define CH34X_LCR_MARK_SPACE   0x20
+#define CH34X_LCR_PAR_EVEN     0x10
+#define CH34X_LCR_ENABLE_PAR   0x08
+#define CH34X_LCR_PAR_MASK     0x38 // all parity bits
+#define CH34X_LCR_STOP_BITS_2  0x04
+#define CH34X_LCR_CS8          0x03
+#define CH34X_LCR_CS7          0x02
+#define CH34X_LCR_CS6          0x01
+#define CH34X_LCR_CS5          0x00
+#define CH34X_LCR_CS_MASK      0x03 // all CSx bits
+
+#endif /* _CH34X_H_ */
diff --git a/src/class/dfu/dfu_device.c b/src/class/dfu/dfu_device.c
index 3e7c13dea..71e7ac2b3 100644
--- a/src/class/dfu/dfu_device.c
+++ b/src/class/dfu/dfu_device.c
@@ -160,11 +160,14 @@ void dfu_moded_reset(uint8_t rhport)
   reset_state();
 }
 
-void dfu_moded_init(void)
-{
+void dfu_moded_init(void) {
   dfu_moded_reset(0);
 }
 
+bool dfu_moded_deinit(void) {
+  return true;
+}
+
 uint16_t dfu_moded_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len)
 {
   (void) rhport;
diff --git a/src/class/dfu/dfu_device.h b/src/class/dfu/dfu_device.h
index fecf8596f..00c22ea8b 100644
--- a/src/class/dfu/dfu_device.h
+++ b/src/class/dfu/dfu_device.h
@@ -86,6 +86,7 @@ TU_ATTR_WEAK void tud_dfu_abort_cb(uint8_t alt);
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     dfu_moded_init(void);
+bool     dfu_moded_deinit(void);
 void     dfu_moded_reset(uint8_t rhport);
 uint16_t dfu_moded_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     dfu_moded_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
diff --git a/src/class/dfu/dfu_rt_device.c b/src/class/dfu/dfu_rt_device.c
index a940d8d62..3b801b787 100644
--- a/src/class/dfu/dfu_rt_device.c
+++ b/src/class/dfu/dfu_rt_device.c
@@ -51,8 +51,11 @@
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void dfu_rtd_init(void)
-{
+void dfu_rtd_init(void) {
+}
+
+bool dfu_rtd_deinit(void) {
+  return true;
 }
 
 void dfu_rtd_reset(uint8_t rhport)
diff --git a/src/class/dfu/dfu_rt_device.h b/src/class/dfu/dfu_rt_device.h
index babaa8214..67eb26d95 100644
--- a/src/class/dfu/dfu_rt_device.h
+++ b/src/class/dfu/dfu_rt_device.h
@@ -43,6 +43,7 @@ void tud_dfu_runtime_reboot_to_dfu_cb(void);
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     dfu_rtd_init(void);
+bool     dfu_rtd_deinit(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_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
diff --git a/src/class/hid/hid.h b/src/class/hid/hid.h
index fbd3eef38..2fe922136 100644
--- a/src/class/hid/hid.h
+++ b/src/class/hid/hid.h
@@ -300,6 +300,19 @@ typedef struct TU_ATTR_PACKED
   int8_t  pan;     // using AC Pan
 } hid_mouse_report_t;
 
+
+// Absolute Mouse: same as the Standard (relative) Mouse Report but
+// with int16_t instead of int8_t for X and Y coordinates.
+typedef struct TU_ATTR_PACKED
+{
+    uint8_t buttons; /**< buttons mask for currently pressed buttons in the mouse. */
+    int16_t x;       /**< Current x position of the mouse. */
+    int16_t y;       /**< Current y position of the mouse. */
+    int8_t wheel;    /**< Current delta wheel movement on the mouse. */
+    int8_t pan;      // using AC Pan
+} hid_abs_mouse_report_t;
+
+
 /// Standard Mouse Buttons Bitmap
 typedef enum
 {
@@ -353,177 +366,224 @@ typedef enum
 //--------------------------------------------------------------------+
 // HID KEYCODE
 //--------------------------------------------------------------------+
-#define HID_KEY_NONE                      0x00
-#define HID_KEY_A                         0x04
-#define HID_KEY_B                         0x05
-#define HID_KEY_C                         0x06
-#define HID_KEY_D                         0x07
-#define HID_KEY_E                         0x08
-#define HID_KEY_F                         0x09
-#define HID_KEY_G                         0x0A
-#define HID_KEY_H                         0x0B
-#define HID_KEY_I                         0x0C
-#define HID_KEY_J                         0x0D
-#define HID_KEY_K                         0x0E
-#define HID_KEY_L                         0x0F
-#define HID_KEY_M                         0x10
-#define HID_KEY_N                         0x11
-#define HID_KEY_O                         0x12
-#define HID_KEY_P                         0x13
-#define HID_KEY_Q                         0x14
-#define HID_KEY_R                         0x15
-#define HID_KEY_S                         0x16
-#define HID_KEY_T                         0x17
-#define HID_KEY_U                         0x18
-#define HID_KEY_V                         0x19
-#define HID_KEY_W                         0x1A
-#define HID_KEY_X                         0x1B
-#define HID_KEY_Y                         0x1C
-#define HID_KEY_Z                         0x1D
-#define HID_KEY_1                         0x1E
-#define HID_KEY_2                         0x1F
-#define HID_KEY_3                         0x20
-#define HID_KEY_4                         0x21
-#define HID_KEY_5                         0x22
-#define HID_KEY_6                         0x23
-#define HID_KEY_7                         0x24
-#define HID_KEY_8                         0x25
-#define HID_KEY_9                         0x26
-#define HID_KEY_0                         0x27
-#define HID_KEY_ENTER                     0x28
-#define HID_KEY_ESCAPE                    0x29
-#define HID_KEY_BACKSPACE                 0x2A
-#define HID_KEY_TAB                       0x2B
-#define HID_KEY_SPACE                     0x2C
-#define HID_KEY_MINUS                     0x2D
-#define HID_KEY_EQUAL                     0x2E
-#define HID_KEY_BRACKET_LEFT              0x2F
-#define HID_KEY_BRACKET_RIGHT             0x30
-#define HID_KEY_BACKSLASH                 0x31
-#define HID_KEY_EUROPE_1                  0x32
-#define HID_KEY_SEMICOLON                 0x33
-#define HID_KEY_APOSTROPHE                0x34
-#define HID_KEY_GRAVE                     0x35
-#define HID_KEY_COMMA                     0x36
-#define HID_KEY_PERIOD                    0x37
-#define HID_KEY_SLASH                     0x38
-#define HID_KEY_CAPS_LOCK                 0x39
-#define HID_KEY_F1                        0x3A
-#define HID_KEY_F2                        0x3B
-#define HID_KEY_F3                        0x3C
-#define HID_KEY_F4                        0x3D
-#define HID_KEY_F5                        0x3E
-#define HID_KEY_F6                        0x3F
-#define HID_KEY_F7                        0x40
-#define HID_KEY_F8                        0x41
-#define HID_KEY_F9                        0x42
-#define HID_KEY_F10                       0x43
-#define HID_KEY_F11                       0x44
-#define HID_KEY_F12                       0x45
-#define HID_KEY_PRINT_SCREEN              0x46
-#define HID_KEY_SCROLL_LOCK               0x47
-#define HID_KEY_PAUSE                     0x48
-#define HID_KEY_INSERT                    0x49
-#define HID_KEY_HOME                      0x4A
-#define HID_KEY_PAGE_UP                   0x4B
-#define HID_KEY_DELETE                    0x4C
-#define HID_KEY_END                       0x4D
-#define HID_KEY_PAGE_DOWN                 0x4E
-#define HID_KEY_ARROW_RIGHT               0x4F
-#define HID_KEY_ARROW_LEFT                0x50
-#define HID_KEY_ARROW_DOWN                0x51
-#define HID_KEY_ARROW_UP                  0x52
-#define HID_KEY_NUM_LOCK                  0x53
-#define HID_KEY_KEYPAD_DIVIDE             0x54
-#define HID_KEY_KEYPAD_MULTIPLY           0x55
-#define HID_KEY_KEYPAD_SUBTRACT           0x56
-#define HID_KEY_KEYPAD_ADD                0x57
-#define HID_KEY_KEYPAD_ENTER              0x58
-#define HID_KEY_KEYPAD_1                  0x59
-#define HID_KEY_KEYPAD_2                  0x5A
-#define HID_KEY_KEYPAD_3                  0x5B
-#define HID_KEY_KEYPAD_4                  0x5C
-#define HID_KEY_KEYPAD_5                  0x5D
-#define HID_KEY_KEYPAD_6                  0x5E
-#define HID_KEY_KEYPAD_7                  0x5F
-#define HID_KEY_KEYPAD_8                  0x60
-#define HID_KEY_KEYPAD_9                  0x61
-#define HID_KEY_KEYPAD_0                  0x62
-#define HID_KEY_KEYPAD_DECIMAL            0x63
-#define HID_KEY_EUROPE_2                  0x64
-#define HID_KEY_APPLICATION               0x65
-#define HID_KEY_POWER                     0x66
-#define HID_KEY_KEYPAD_EQUAL              0x67
-#define HID_KEY_F13                       0x68
-#define HID_KEY_F14                       0x69
-#define HID_KEY_F15                       0x6A
-#define HID_KEY_F16                       0x6B
-#define HID_KEY_F17                       0x6C
-#define HID_KEY_F18                       0x6D
-#define HID_KEY_F19                       0x6E
-#define HID_KEY_F20                       0x6F
-#define HID_KEY_F21                       0x70
-#define HID_KEY_F22                       0x71
-#define HID_KEY_F23                       0x72
-#define HID_KEY_F24                       0x73
-#define HID_KEY_EXECUTE                   0x74
-#define HID_KEY_HELP                      0x75
-#define HID_KEY_MENU                      0x76
-#define HID_KEY_SELECT                    0x77
-#define HID_KEY_STOP                      0x78
-#define HID_KEY_AGAIN                     0x79
-#define HID_KEY_UNDO                      0x7A
-#define HID_KEY_CUT                       0x7B
-#define HID_KEY_COPY                      0x7C
-#define HID_KEY_PASTE                     0x7D
-#define HID_KEY_FIND                      0x7E
-#define HID_KEY_MUTE                      0x7F
-#define HID_KEY_VOLUME_UP                 0x80
-#define HID_KEY_VOLUME_DOWN               0x81
-#define HID_KEY_LOCKING_CAPS_LOCK         0x82
-#define HID_KEY_LOCKING_NUM_LOCK          0x83
-#define HID_KEY_LOCKING_SCROLL_LOCK       0x84
-#define HID_KEY_KEYPAD_COMMA              0x85
-#define HID_KEY_KEYPAD_EQUAL_SIGN         0x86
-#define HID_KEY_KANJI1                    0x87
-#define HID_KEY_KANJI2                    0x88
-#define HID_KEY_KANJI3                    0x89
-#define HID_KEY_KANJI4                    0x8A
-#define HID_KEY_KANJI5                    0x8B
-#define HID_KEY_KANJI6                    0x8C
-#define HID_KEY_KANJI7                    0x8D
-#define HID_KEY_KANJI8                    0x8E
-#define HID_KEY_KANJI9                    0x8F
-#define HID_KEY_LANG1                     0x90
-#define HID_KEY_LANG2                     0x91
-#define HID_KEY_LANG3                     0x92
-#define HID_KEY_LANG4                     0x93
-#define HID_KEY_LANG5                     0x94
-#define HID_KEY_LANG6                     0x95
-#define HID_KEY_LANG7                     0x96
-#define HID_KEY_LANG8                     0x97
-#define HID_KEY_LANG9                     0x98
-#define HID_KEY_ALTERNATE_ERASE           0x99
-#define HID_KEY_SYSREQ_ATTENTION          0x9A
-#define HID_KEY_CANCEL                    0x9B
-#define HID_KEY_CLEAR                     0x9C
-#define HID_KEY_PRIOR                     0x9D
-#define HID_KEY_RETURN                    0x9E
-#define HID_KEY_SEPARATOR                 0x9F
-#define HID_KEY_OUT                       0xA0
-#define HID_KEY_OPER                      0xA1
-#define HID_KEY_CLEAR_AGAIN               0xA2
-#define HID_KEY_CRSEL_PROPS               0xA3
-#define HID_KEY_EXSEL                     0xA4
-// RESERVED					                      0xA5-DF
-#define HID_KEY_CONTROL_LEFT              0xE0
-#define HID_KEY_SHIFT_LEFT                0xE1
-#define HID_KEY_ALT_LEFT                  0xE2
-#define HID_KEY_GUI_LEFT                  0xE3
-#define HID_KEY_CONTROL_RIGHT             0xE4
-#define HID_KEY_SHIFT_RIGHT               0xE5
-#define HID_KEY_ALT_RIGHT                 0xE6
-#define HID_KEY_GUI_RIGHT                 0xE7
+#define HID_KEY_NONE                        0x00
+#define HID_KEY_A                           0x04
+#define HID_KEY_B                           0x05
+#define HID_KEY_C                           0x06
+#define HID_KEY_D                           0x07
+#define HID_KEY_E                           0x08
+#define HID_KEY_F                           0x09
+#define HID_KEY_G                           0x0A
+#define HID_KEY_H                           0x0B
+#define HID_KEY_I                           0x0C
+#define HID_KEY_J                           0x0D
+#define HID_KEY_K                           0x0E
+#define HID_KEY_L                           0x0F
+#define HID_KEY_M                           0x10
+#define HID_KEY_N                           0x11
+#define HID_KEY_O                           0x12
+#define HID_KEY_P                           0x13
+#define HID_KEY_Q                           0x14
+#define HID_KEY_R                           0x15
+#define HID_KEY_S                           0x16
+#define HID_KEY_T                           0x17
+#define HID_KEY_U                           0x18
+#define HID_KEY_V                           0x19
+#define HID_KEY_W                           0x1A
+#define HID_KEY_X                           0x1B
+#define HID_KEY_Y                           0x1C
+#define HID_KEY_Z                           0x1D
+#define HID_KEY_1                           0x1E
+#define HID_KEY_2                           0x1F
+#define HID_KEY_3                           0x20
+#define HID_KEY_4                           0x21
+#define HID_KEY_5                           0x22
+#define HID_KEY_6                           0x23
+#define HID_KEY_7                           0x24
+#define HID_KEY_8                           0x25
+#define HID_KEY_9                           0x26
+#define HID_KEY_0                           0x27
+#define HID_KEY_ENTER                       0x28
+#define HID_KEY_ESCAPE                      0x29
+#define HID_KEY_BACKSPACE                   0x2A
+#define HID_KEY_TAB                         0x2B
+#define HID_KEY_SPACE                       0x2C
+#define HID_KEY_MINUS                       0x2D
+#define HID_KEY_EQUAL                       0x2E
+#define HID_KEY_BRACKET_LEFT                0x2F
+#define HID_KEY_BRACKET_RIGHT               0x30
+#define HID_KEY_BACKSLASH                   0x31
+#define HID_KEY_EUROPE_1                    0x32
+#define HID_KEY_SEMICOLON                   0x33
+#define HID_KEY_APOSTROPHE                  0x34
+#define HID_KEY_GRAVE                       0x35
+#define HID_KEY_COMMA                       0x36
+#define HID_KEY_PERIOD                      0x37
+#define HID_KEY_SLASH                       0x38
+#define HID_KEY_CAPS_LOCK                   0x39
+#define HID_KEY_F1                          0x3A
+#define HID_KEY_F2                          0x3B
+#define HID_KEY_F3                          0x3C
+#define HID_KEY_F4                          0x3D
+#define HID_KEY_F5                          0x3E
+#define HID_KEY_F6                          0x3F
+#define HID_KEY_F7                          0x40
+#define HID_KEY_F8                          0x41
+#define HID_KEY_F9                          0x42
+#define HID_KEY_F10                         0x43
+#define HID_KEY_F11                         0x44
+#define HID_KEY_F12                         0x45
+#define HID_KEY_PRINT_SCREEN                0x46
+#define HID_KEY_SCROLL_LOCK                 0x47
+#define HID_KEY_PAUSE                       0x48
+#define HID_KEY_INSERT                      0x49
+#define HID_KEY_HOME                        0x4A
+#define HID_KEY_PAGE_UP                     0x4B
+#define HID_KEY_DELETE                      0x4C
+#define HID_KEY_END                         0x4D
+#define HID_KEY_PAGE_DOWN                   0x4E
+#define HID_KEY_ARROW_RIGHT                 0x4F
+#define HID_KEY_ARROW_LEFT                  0x50
+#define HID_KEY_ARROW_DOWN                  0x51
+#define HID_KEY_ARROW_UP                    0x52
+#define HID_KEY_NUM_LOCK                    0x53
+#define HID_KEY_KEYPAD_DIVIDE               0x54
+#define HID_KEY_KEYPAD_MULTIPLY             0x55
+#define HID_KEY_KEYPAD_SUBTRACT             0x56
+#define HID_KEY_KEYPAD_ADD                  0x57
+#define HID_KEY_KEYPAD_ENTER                0x58
+#define HID_KEY_KEYPAD_1                    0x59
+#define HID_KEY_KEYPAD_2                    0x5A
+#define HID_KEY_KEYPAD_3                    0x5B
+#define HID_KEY_KEYPAD_4                    0x5C
+#define HID_KEY_KEYPAD_5                    0x5D
+#define HID_KEY_KEYPAD_6                    0x5E
+#define HID_KEY_KEYPAD_7                    0x5F
+#define HID_KEY_KEYPAD_8                    0x60
+#define HID_KEY_KEYPAD_9                    0x61
+#define HID_KEY_KEYPAD_0                    0x62
+#define HID_KEY_KEYPAD_DECIMAL              0x63
+#define HID_KEY_EUROPE_2                    0x64
+#define HID_KEY_APPLICATION                 0x65
+#define HID_KEY_POWER                       0x66
+#define HID_KEY_KEYPAD_EQUAL                0x67
+#define HID_KEY_F13                         0x68
+#define HID_KEY_F14                         0x69
+#define HID_KEY_F15                         0x6A
+#define HID_KEY_F16                         0x6B
+#define HID_KEY_F17                         0x6C
+#define HID_KEY_F18                         0x6D
+#define HID_KEY_F19                         0x6E
+#define HID_KEY_F20                         0x6F
+#define HID_KEY_F21                         0x70
+#define HID_KEY_F22                         0x71
+#define HID_KEY_F23                         0x72
+#define HID_KEY_F24                         0x73
+#define HID_KEY_EXECUTE                     0x74
+#define HID_KEY_HELP                        0x75
+#define HID_KEY_MENU                        0x76
+#define HID_KEY_SELECT                      0x77
+#define HID_KEY_STOP                        0x78
+#define HID_KEY_AGAIN                       0x79
+#define HID_KEY_UNDO                        0x7A
+#define HID_KEY_CUT                         0x7B
+#define HID_KEY_COPY                        0x7C
+#define HID_KEY_PASTE                       0x7D
+#define HID_KEY_FIND                        0x7E
+#define HID_KEY_MUTE                        0x7F
+#define HID_KEY_VOLUME_UP                   0x80
+#define HID_KEY_VOLUME_DOWN                 0x81
+#define HID_KEY_LOCKING_CAPS_LOCK           0x82
+#define HID_KEY_LOCKING_NUM_LOCK            0x83
+#define HID_KEY_LOCKING_SCROLL_LOCK         0x84
+#define HID_KEY_KEYPAD_COMMA                0x85
+#define HID_KEY_KEYPAD_EQUAL_SIGN           0x86
+#define HID_KEY_KANJI1                      0x87
+#define HID_KEY_KANJI2                      0x88
+#define HID_KEY_KANJI3                      0x89
+#define HID_KEY_KANJI4                      0x8A
+#define HID_KEY_KANJI5                      0x8B
+#define HID_KEY_KANJI6                      0x8C
+#define HID_KEY_KANJI7                      0x8D
+#define HID_KEY_KANJI8                      0x8E
+#define HID_KEY_KANJI9                      0x8F
+#define HID_KEY_LANG1                       0x90
+#define HID_KEY_LANG2                       0x91
+#define HID_KEY_LANG3                       0x92
+#define HID_KEY_LANG4                       0x93
+#define HID_KEY_LANG5                       0x94
+#define HID_KEY_LANG6                       0x95
+#define HID_KEY_LANG7                       0x96
+#define HID_KEY_LANG8                       0x97
+#define HID_KEY_LANG9                       0x98
+#define HID_KEY_ALTERNATE_ERASE             0x99
+#define HID_KEY_SYSREQ_ATTENTION            0x9A
+#define HID_KEY_CANCEL                      0x9B
+#define HID_KEY_CLEAR                       0x9C
+#define HID_KEY_PRIOR                       0x9D
+#define HID_KEY_RETURN                      0x9E
+#define HID_KEY_SEPARATOR                   0x9F
+#define HID_KEY_OUT                         0xA0
+#define HID_KEY_OPER                        0xA1
+#define HID_KEY_CLEAR_AGAIN                 0xA2
+#define HID_KEY_CRSEL_PROPS                 0xA3
+#define HID_KEY_EXSEL                       0xA4
+// RESERVED					                        0xA5-AF
+#define HID_KEY_KEYPAD_00                   0xB0
+#define HID_KEY_KEYPAD_000                  0xB1
+#define HID_KEY_THOUSANDS_SEPARATOR         0xB2
+#define HID_KEY_DECIMAL_SEPARATOR           0xB3
+#define HID_KEY_CURRENCY_UNIT               0xB4
+#define HID_KEY_CURRENCY_SUBUNIT            0xB5
+#define HID_KEY_KEYPAD_LEFT_PARENTHESIS     0xB6
+#define HID_KEY_KEYPAD_RIGHT_PARENTHESIS    0xB7
+#define HID_KEY_KEYPAD_LEFT_BRACE           0xB8
+#define HID_KEY_KEYPAD_RIGHT_BRACE          0xB9
+#define HID_KEY_KEYPAD_TAB                  0xBA
+#define HID_KEY_KEYPAD_BACKSPACE            0xBB
+#define HID_KEY_KEYPAD_A                    0xBC
+#define HID_KEY_KEYPAD_B                    0xBD
+#define HID_KEY_KEYPAD_C                    0xBE
+#define HID_KEY_KEYPAD_D                    0xBF
+#define HID_KEY_KEYPAD_E                    0xC0
+#define HID_KEY_KEYPAD_F                    0xC1
+#define HID_KEY_KEYPAD_XOR                  0xC2
+#define HID_KEY_KEYPAD_CARET                0xC3
+#define HID_KEY_KEYPAD_PERCENT              0xC4
+#define HID_KEY_KEYPAD_LESS_THAN            0xC5
+#define HID_KEY_KEYPAD_GREATER_THAN         0xC6
+#define HID_KEY_KEYPAD_AMPERSAND            0xC7
+#define HID_KEY_KEYPAD_DOUBLE_AMPERSAND     0xC8
+#define HID_KEY_KEYPAD_VERTICAL_BAR         0xC9
+#define HID_KEY_KEYPAD_DOUBLE_VERTICAL_BAR  0xCA
+#define HID_KEY_KEYPAD_COLON                0xCB
+#define HID_KEY_KEYPAD_HASH                 0xCC
+#define HID_KEY_KEYPAD_SPACE                0xCD
+#define HID_KEY_KEYPAD_AT                   0xCE
+#define HID_KEY_KEYPAD_EXCLAMATION          0xCF
+#define HID_KEY_KEYPAD_MEMORY_STORE         0xD0
+#define HID_KEY_KEYPAD_MEMORY_RECALL        0xD1
+#define HID_KEY_KEYPAD_MEMORY_CLEAR         0xD2
+#define HID_KEY_KEYPAD_MEMORY_ADD           0xD3
+#define HID_KEY_KEYPAD_MEMORY_SUBTRACT      0xD4
+#define HID_KEY_KEYPAD_MEMORY_MULTIPLY      0xD5
+#define HID_KEY_KEYPAD_MEMORY_DIVIDE        0xD6
+#define HID_KEY_KEYPAD_PLUS_MINUS           0xD7
+#define HID_KEY_KEYPAD_CLEAR                0xD8
+#define HID_KEY_KEYPAD_CLEAR_ENTRY          0xD9
+#define HID_KEY_KEYPAD_BINARY               0xDA
+#define HID_KEY_KEYPAD_OCTAL                0xDB
+#define HID_KEY_KEYPAD_DECIMAL_2            0xDC
+#define HID_KEY_KEYPAD_HEXADECIMAL          0xDD
+// RESERVED					                        0xDE-DF
+#define HID_KEY_CONTROL_LEFT                0xE0
+#define HID_KEY_SHIFT_LEFT                  0xE1
+#define HID_KEY_ALT_LEFT                    0xE2
+#define HID_KEY_GUI_LEFT                    0xE3
+#define HID_KEY_CONTROL_RIGHT               0xE4
+#define HID_KEY_SHIFT_RIGHT                 0xE5
+#define HID_KEY_ALT_RIGHT                   0xE6
+#define HID_KEY_GUI_RIGHT                   0xE7
 
 
 //--------------------------------------------------------------------+
@@ -684,32 +744,33 @@ enum {
 
 /// HID Usage Table - Table 1: Usage Page Summary
 enum {
-  HID_USAGE_PAGE_DESKTOP         = 0x01,
-  HID_USAGE_PAGE_SIMULATE        = 0x02,
-  HID_USAGE_PAGE_VIRTUAL_REALITY = 0x03,
-  HID_USAGE_PAGE_SPORT           = 0x04,
-  HID_USAGE_PAGE_GAME            = 0x05,
-  HID_USAGE_PAGE_GENERIC_DEVICE  = 0x06,
-  HID_USAGE_PAGE_KEYBOARD        = 0x07,
-  HID_USAGE_PAGE_LED             = 0x08,
-  HID_USAGE_PAGE_BUTTON          = 0x09,
-  HID_USAGE_PAGE_ORDINAL         = 0x0a,
-  HID_USAGE_PAGE_TELEPHONY       = 0x0b,
-  HID_USAGE_PAGE_CONSUMER        = 0x0c,
-  HID_USAGE_PAGE_DIGITIZER       = 0x0d,
-  HID_USAGE_PAGE_PID             = 0x0f,
-  HID_USAGE_PAGE_UNICODE         = 0x10,
-  HID_USAGE_PAGE_ALPHA_DISPLAY   = 0x14,
-  HID_USAGE_PAGE_MEDICAL         = 0x40,
-  HID_USAGE_PAGE_MONITOR         = 0x80, //0x80 - 0x83
-  HID_USAGE_PAGE_POWER           = 0x84, // 0x084 - 0x87
-  HID_USAGE_PAGE_BARCODE_SCANNER = 0x8c,
-  HID_USAGE_PAGE_SCALE           = 0x8d,
-  HID_USAGE_PAGE_MSR             = 0x8e,
-  HID_USAGE_PAGE_CAMERA          = 0x90,
-  HID_USAGE_PAGE_ARCADE          = 0x91,
-  HID_USAGE_PAGE_FIDO            = 0xF1D0, // FIDO alliance HID usage page
-  HID_USAGE_PAGE_VENDOR          = 0xFF00 // 0xFF00 - 0xFFFF
+  HID_USAGE_PAGE_DESKTOP                   = 0x01,
+  HID_USAGE_PAGE_SIMULATE                  = 0x02,
+  HID_USAGE_PAGE_VIRTUAL_REALITY           = 0x03,
+  HID_USAGE_PAGE_SPORT                     = 0x04,
+  HID_USAGE_PAGE_GAME                      = 0x05,
+  HID_USAGE_PAGE_GENERIC_DEVICE            = 0x06,
+  HID_USAGE_PAGE_KEYBOARD                  = 0x07,
+  HID_USAGE_PAGE_LED                       = 0x08,
+  HID_USAGE_PAGE_BUTTON                    = 0x09,
+  HID_USAGE_PAGE_ORDINAL                   = 0x0a,
+  HID_USAGE_PAGE_TELEPHONY                 = 0x0b,
+  HID_USAGE_PAGE_CONSUMER                  = 0x0c,
+  HID_USAGE_PAGE_DIGITIZER                 = 0x0d,
+  HID_USAGE_PAGE_PID                       = 0x0f,
+  HID_USAGE_PAGE_UNICODE                   = 0x10,
+  HID_USAGE_PAGE_ALPHA_DISPLAY             = 0x14,
+  HID_USAGE_PAGE_MEDICAL                   = 0x40,
+  HID_USAGE_PAGE_LIGHTING_AND_ILLUMINATION = 0x59,
+  HID_USAGE_PAGE_MONITOR                   = 0x80, //0x80 - 0x83
+  HID_USAGE_PAGE_POWER                     = 0x84, // 0x084 - 0x87
+  HID_USAGE_PAGE_BARCODE_SCANNER           = 0x8c,
+  HID_USAGE_PAGE_SCALE                     = 0x8d,
+  HID_USAGE_PAGE_MSR                       = 0x8e,
+  HID_USAGE_PAGE_CAMERA                    = 0x90,
+  HID_USAGE_PAGE_ARCADE                    = 0x91,
+  HID_USAGE_PAGE_FIDO                      = 0xF1D0, // FIDO alliance HID usage page
+  HID_USAGE_PAGE_VENDOR                    = 0xFF00 // 0xFF00 - 0xFFFF
 };
 
 /// HID Usage Table - Table 6: Generic Desktop Page
@@ -788,8 +849,7 @@ enum {
 
 /// HID Usage Table: Consumer Page (0x0C)
 /// Only contains controls that supported by Windows (whole list is too long)
-enum
-{
+enum {
   // Generic Control
   HID_USAGE_CONSUMER_CONTROL                           = 0x0001,
 
@@ -845,9 +905,45 @@ enum
   HID_USAGE_CONSUMER_AC_PAN                            = 0x0238,
 };
 
+/// HID Usage Table - Lighting And Illumination Page (0x59)
+enum {
+  HID_USAGE_LIGHTING_LAMP_ARRAY                          = 0x01,
+  HID_USAGE_LIGHTING_LAMP_ARRAY_ATTRIBUTES_REPORT        = 0x02,
+  HID_USAGE_LIGHTING_LAMP_COUNT                          = 0x03,
+  HID_USAGE_LIGHTING_BOUNDING_BOX_WIDTH_IN_MICROMETERS   = 0x04,
+  HID_USAGE_LIGHTING_BOUNDING_BOX_HEIGHT_IN_MICROMETERS  = 0x05,
+  HID_USAGE_LIGHTING_BOUNDING_BOX_DEPTH_IN_MICROMETERS   = 0x06,
+  HID_USAGE_LIGHTING_LAMP_ARRAY_KIND                     = 0x07,
+  HID_USAGE_LIGHTING_MIN_UPDATE_INTERVAL_IN_MICROSECONDS = 0x08,
+  HID_USAGE_LIGHTING_LAMP_ATTRIBUTES_REQUEST_REPORT      = 0x20,
+  HID_USAGE_LIGHTING_LAMP_ID                             = 0x21,
+  HID_USAGE_LIGHTING_LAMP_ATTRIBUTES_RESPONSE_REPORT     = 0x22,
+  HID_USAGE_LIGHTING_POSITION_X_IN_MICROMETERS           = 0x23,
+  HID_USAGE_LIGHTING_POSITION_Y_IN_MICROMETERS           = 0x24,
+  HID_USAGE_LIGHTING_POSITION_Z_IN_MICROMETERS           = 0x25,
+  HID_USAGE_LIGHTING_LAMP_PURPOSES                       = 0x26,
+  HID_USAGE_LIGHTING_UPDATE_LATENCY_IN_MICROSECONDS      = 0x27,
+  HID_USAGE_LIGHTING_RED_LEVEL_COUNT                     = 0x28,
+  HID_USAGE_LIGHTING_GREEN_LEVEL_COUNT                   = 0x29,
+  HID_USAGE_LIGHTING_BLUE_LEVEL_COUNT                    = 0x2A,
+  HID_USAGE_LIGHTING_INTENSITY_LEVEL_COUNT               = 0x2B,
+  HID_USAGE_LIGHTING_IS_PROGRAMMABLE                     = 0x2C,
+  HID_USAGE_LIGHTING_INPUT_BINDING                       = 0x2D,
+  HID_USAGE_LIGHTING_LAMP_MULTI_UPDATE_REPORT            = 0x50,
+  HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL                  = 0x51,
+  HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL                = 0x52,
+  HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL                 = 0x53,
+  HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL            = 0x54,
+  HID_USAGE_LIGHTING_LAMP_UPDATE_FLAGS                   = 0x55,
+  HID_USAGE_LIGHTING_LAMP_RANGE_UPDATE_REPORT            = 0x60,
+  HID_USAGE_LIGHTING_LAMP_ID_START                       = 0x61,
+  HID_USAGE_LIGHTING_LAMP_ID_END                         = 0x62,
+  HID_USAGE_LIGHTING_LAMP_ARRAY_CONTROL_REPORT           = 0x70,
+  HID_USAGE_LIGHTING_AUTONOMOUS_MODE                     = 0x71,
+};
+
 /// HID Usage Table: FIDO Alliance Page (0xF1D0)
-enum
-{
+enum {
   HID_USAGE_FIDO_U2FHID   = 0x01, // U2FHID usage for top-level collection
   HID_USAGE_FIDO_DATA_IN  = 0x20, // Raw IN data report
   HID_USAGE_FIDO_DATA_OUT = 0x21  // Raw OUT data report
diff --git a/src/class/hid/hid_device.c b/src/class/hid/hid_device.c
index 5637ea6b4..ada01582e 100644
--- a/src/class/hid/hid_device.c
+++ b/src/class/hid/hid_device.c
@@ -39,23 +39,23 @@
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-typedef struct
-{
+typedef struct {
   uint8_t itf_num;
   uint8_t ep_in;
-  uint8_t ep_out;        // optional Out endpoint
-  uint8_t itf_protocol;  // Boot mouse or keyboard
+  uint8_t ep_out;       // optional Out endpoint
+  uint8_t itf_protocol; // Boot mouse or keyboard
 
-  uint8_t protocol_mode; // Boot (0) or Report protocol (1)
-  uint8_t idle_rate;     // up to application to handle idle rate
   uint16_t report_desc_len;
+  CFG_TUSB_MEM_ALIGN uint8_t protocol_mode; // Boot (0) or Report protocol (1)
+  CFG_TUSB_MEM_ALIGN uint8_t idle_rate;     // up to application to handle idle rate
 
   CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_HID_EP_BUFSIZE];
   CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_HID_EP_BUFSIZE];
+  CFG_TUSB_MEM_ALIGN uint8_t ctrl_buf[CFG_TUD_HID_EP_BUFSIZE];
 
   // TODO save hid descriptor since host can specifically request this after enumeration
   // Note: HID descriptor may be not available from application after enumeration
-  tusb_hid_descriptor_hid_t const * hid_descriptor;
+  tusb_hid_descriptor_hid_t const *hid_descriptor;
 } hidd_interface_t;
 
 CFG_TUD_MEM_SECTION tu_static hidd_interface_t _hidd_itf[CFG_TUD_HID];
@@ -63,12 +63,12 @@ CFG_TUD_MEM_SECTION tu_static hidd_interface_t _hidd_itf[CFG_TUD_HID];
 /*------------- Helpers -------------*/
 static inline uint8_t get_index_by_itfnum(uint8_t itf_num)
 {
-	for (uint8_t i=0; i < CFG_TUD_HID; i++ )
-	{
-		if ( itf_num == _hidd_itf[i].itf_num ) return i;
-	}
+  for (uint8_t i = 0; i < CFG_TUD_HID; i++) {
+    if (itf_num == _hidd_itf[i].itf_num)
+      return i;
+  }
 
-	return 0xFF;
+  return 0xFF;
 }
 
 //--------------------------------------------------------------------+
@@ -81,37 +81,29 @@ bool tud_hid_n_ready(uint8_t instance)
   return tud_ready() && (ep_in != 0) && !usbd_edpt_busy(rhport, ep_in);
 }
 
-bool tud_hid_n_report(uint8_t instance, uint8_t report_id, void const* report, uint16_t len)
+bool tud_hid_n_report(uint8_t instance, uint8_t report_id, void const *report, uint16_t len)
 {
   uint8_t const rhport = 0;
-  hidd_interface_t * p_hid = &_hidd_itf[instance];
+  hidd_interface_t *p_hid = &_hidd_itf[instance];
 
   // claim endpoint
-  TU_VERIFY( usbd_edpt_claim(rhport, p_hid->ep_in) );
+  TU_VERIFY(usbd_edpt_claim(rhport, p_hid->ep_in));
 
   // prepare data
-  if (report_id)
-  {
+  if (report_id) {
     p_hid->epin_buf[0] = report_id;
-    TU_VERIFY(0 == tu_memcpy_s(p_hid->epin_buf+1, CFG_TUD_HID_EP_BUFSIZE-1, report, len));
+    TU_VERIFY(0 == tu_memcpy_s(p_hid->epin_buf + 1, CFG_TUD_HID_EP_BUFSIZE - 1, report, len));
     len++;
-  }else
-  {
+  } else {
     TU_VERIFY(0 == tu_memcpy_s(p_hid->epin_buf, CFG_TUD_HID_EP_BUFSIZE, report, len));
   }
 
   return usbd_edpt_xfer(rhport, p_hid->ep_in, p_hid->epin_buf, len);
 }
 
-uint8_t tud_hid_n_interface_protocol(uint8_t instance)
-{
-  return _hidd_itf[instance].itf_protocol;
-}
+uint8_t tud_hid_n_interface_protocol(uint8_t instance) { return _hidd_itf[instance].itf_protocol; }
 
-uint8_t tud_hid_n_get_protocol(uint8_t instance)
-{
-  return _hidd_itf[instance].protocol_mode;
-}
+uint8_t tud_hid_n_get_protocol(uint8_t instance) { return _hidd_itf[instance].protocol_mode; }
 
 bool tud_hid_n_keyboard_report(uint8_t instance, uint8_t report_id, uint8_t modifier, uint8_t keycode[6])
 {
@@ -120,44 +112,51 @@ bool tud_hid_n_keyboard_report(uint8_t instance, uint8_t report_id, uint8_t modi
   report.modifier = modifier;
   report.reserved = 0;
 
-  if ( keycode )
-  {
+  if (keycode) {
     memcpy(report.keycode, keycode, sizeof(report.keycode));
-  }else
-  {
+  } else {
     tu_memclr(report.keycode, 6);
   }
 
   return tud_hid_n_report(instance, report_id, &report, sizeof(report));
 }
 
-bool tud_hid_n_mouse_report(uint8_t instance, uint8_t report_id,
-                            uint8_t buttons, int8_t x, int8_t y, int8_t vertical, int8_t horizontal)
+bool tud_hid_n_mouse_report(uint8_t instance, uint8_t report_id, uint8_t buttons, int8_t x, int8_t y, int8_t vertical, int8_t horizontal)
 {
-  hid_mouse_report_t report =
-  {
+  hid_mouse_report_t report = {
     .buttons = buttons,
-    .x       = x,
-    .y       = y,
-    .wheel   = vertical,
-    .pan     = horizontal
+    .x = x,
+    .y = y,
+    .wheel = vertical,
+    .pan = horizontal
   };
 
   return tud_hid_n_report(instance, report_id, &report, sizeof(report));
 }
 
-bool tud_hid_n_gamepad_report(uint8_t instance, uint8_t report_id,
-                              int8_t x, int8_t y, int8_t z, int8_t rz, int8_t rx, int8_t ry, uint8_t hat, uint32_t buttons) {
-  hid_gamepad_report_t report =
-  {
-    .x       = x,
-    .y       = y,
-    .z       = z,
-    .rz      = rz,
-    .rx      = rx,
-    .ry      = ry,
-    .hat     = hat,
+bool tud_hid_n_abs_mouse_report(uint8_t instance, uint8_t report_id, uint8_t buttons, int16_t x, int16_t y, int8_t vertical, int8_t horizontal)
+{
+  hid_abs_mouse_report_t report = {
     .buttons = buttons,
+    .x = x,
+    .y = y,
+    .wheel = vertical,
+    .pan = horizontal
+  };
+  return tud_hid_n_report(instance, report_id, &report, sizeof(report));
+}
+
+bool tud_hid_n_gamepad_report(uint8_t instance, uint8_t report_id, int8_t x, int8_t y, int8_t z, int8_t rz, int8_t rx, int8_t ry, uint8_t hat, uint32_t buttons)
+{
+  hid_gamepad_report_t report = {
+      .x = x,
+      .y = y,
+      .z = z,
+      .rz = rz,
+      .rx = rx,
+      .ry = ry,
+      .hat = hat,
+      .buttons = buttons,
   };
 
   return tud_hid_n_report(instance, report_id, &report, sizeof(report));
@@ -171,59 +170,59 @@ void hidd_init(void)
   hidd_reset(0);
 }
 
+bool hidd_deinit(void)
+{
+  return true;
+}
+
 void hidd_reset(uint8_t rhport)
 {
-  (void) rhport;
+  (void)rhport;
   tu_memclr(_hidd_itf, sizeof(_hidd_itf));
 }
 
-uint16_t hidd_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint16_t max_len)
- {
+uint16_t hidd_open(uint8_t rhport, tusb_desc_interface_t const *desc_itf, uint16_t max_len)
+{
   TU_VERIFY(TUSB_CLASS_HID == desc_itf->bInterfaceClass, 0);
 
   // len = interface + hid + n*endpoints
-  uint16_t const drv_len =
-      (uint16_t) (sizeof(tusb_desc_interface_t) + sizeof(tusb_hid_descriptor_hid_t) +
-                                       desc_itf->bNumEndpoints * sizeof(tusb_desc_endpoint_t));
+  uint16_t const drv_len = (uint16_t)(sizeof(tusb_desc_interface_t) + sizeof(tusb_hid_descriptor_hid_t) + desc_itf->bNumEndpoints * sizeof(tusb_desc_endpoint_t));
   TU_ASSERT(max_len >= drv_len, 0);
 
   // Find available interface
-  hidd_interface_t * p_hid = NULL;
+  hidd_interface_t *p_hid = NULL;
   uint8_t hid_id;
-  for(hid_id=0; hid_id<CFG_TUD_HID; hid_id++)
-  {
-    if ( _hidd_itf[hid_id].ep_in == 0 )
-    {
+  for (hid_id = 0; hid_id < CFG_TUD_HID; hid_id++) {
+    if (_hidd_itf[hid_id].ep_in == 0) {
       p_hid = &_hidd_itf[hid_id];
       break;
     }
   }
   TU_ASSERT(p_hid, 0);
 
-  uint8_t const *p_desc = (uint8_t const *) desc_itf;
+  uint8_t const *p_desc = (uint8_t const *)desc_itf;
 
   //------------- HID descriptor -------------//
   p_desc = tu_desc_next(p_desc);
   TU_ASSERT(HID_DESC_TYPE_HID == tu_desc_type(p_desc), 0);
-  p_hid->hid_descriptor = (tusb_hid_descriptor_hid_t const *) p_desc;
+  p_hid->hid_descriptor = (tusb_hid_descriptor_hid_t const *)p_desc;
 
   //------------- Endpoint Descriptor -------------//
   p_desc = tu_desc_next(p_desc);
   TU_ASSERT(usbd_open_edpt_pair(rhport, p_desc, desc_itf->bNumEndpoints, TUSB_XFER_INTERRUPT, &p_hid->ep_out, &p_hid->ep_in), 0);
 
-  if ( desc_itf->bInterfaceSubClass == HID_SUBCLASS_BOOT ) p_hid->itf_protocol = desc_itf->bInterfaceProtocol;
+  if (desc_itf->bInterfaceSubClass == HID_SUBCLASS_BOOT)
+    p_hid->itf_protocol = desc_itf->bInterfaceProtocol;
 
   p_hid->protocol_mode = HID_PROTOCOL_REPORT; // Per Specs: default is report mode
-  p_hid->itf_num       = desc_itf->bInterfaceNumber;
+  p_hid->itf_num = desc_itf->bInterfaceNumber;
 
   // Use offsetof to avoid pointer to the odd/misaligned address
-  p_hid->report_desc_len = tu_unaligned_read16((uint8_t const*) p_hid->hid_descriptor + offsetof(tusb_hid_descriptor_hid_t, wReportLength));
+  p_hid->report_desc_len = tu_unaligned_read16((uint8_t const *)p_hid->hid_descriptor + offsetof(tusb_hid_descriptor_hid_t, wReportLength));
 
   // Prepare for output endpoint
-  if (p_hid->ep_out)
-  {
-    if ( !usbd_edpt_xfer(rhport, p_hid->ep_out, p_hid->epout_buf, sizeof(p_hid->epout_buf)) )
-    {
+  if (p_hid->ep_out) {
+    if (!usbd_edpt_xfer(rhport, p_hid->ep_out, p_hid->epout_buf, sizeof(p_hid->epout_buf))) {
       TU_LOG_FAILED();
       TU_BREAKPOINT();
     }
@@ -235,144 +234,120 @@ uint16_t hidd_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint1
 // 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_xfer_cb (uint8_t rhport, uint8_t stage, 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);
 
-  uint8_t const hid_itf = get_index_by_itfnum((uint8_t) request->wIndex);
+  uint8_t const hid_itf = get_index_by_itfnum((uint8_t)request->wIndex);
   TU_VERIFY(hid_itf < CFG_TUD_HID);
 
-  hidd_interface_t* p_hid = &_hidd_itf[hid_itf];
+  hidd_interface_t *p_hid = &_hidd_itf[hid_itf];
 
-  if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD)
-  {
+  if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD) {
     //------------- STD Request -------------//
-    if ( stage == CONTROL_STAGE_SETUP )
-    {
-      uint8_t const desc_type  = tu_u16_high(request->wValue);
-      //uint8_t const desc_index = tu_u16_low (request->wValue);
+    if (stage == CONTROL_STAGE_SETUP) {
+      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)
-      {
+      if (request->bRequest == TUSB_REQ_GET_DESCRIPTOR && desc_type == HID_DESC_TYPE_HID) {
         TU_VERIFY(p_hid->hid_descriptor);
-        TU_VERIFY(tud_control_xfer(rhport, request, (void*)(uintptr_t) 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(hid_itf);
-        tud_control_xfer(rhport, request, (void*)(uintptr_t) desc_report, p_hid->report_desc_len);
-      }
-      else
-      {
+        TU_VERIFY(tud_control_xfer(rhport, request, (void *)(uintptr_t)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(hid_itf);
+        tud_control_xfer(rhport, request, (void *)(uintptr_t)desc_report, p_hid->report_desc_len);
+      } else {
         return false; // stall unsupported request
       }
     }
-  }
-  else if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS)
-  {
+  } else if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS) {
     //------------- Class Specific Request -------------//
-    switch( request->bRequest )
-    {
-      case HID_REQ_CONTROL_GET_REPORT:
-        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);
+    switch (request->bRequest) {
+    case HID_REQ_CONTROL_GET_REPORT:
+      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);
 
-          uint8_t* report_buf = p_hid->epin_buf;
-          uint16_t req_len = tu_min16(request->wLength, CFG_TUD_HID_EP_BUFSIZE);
+        uint8_t *report_buf = p_hid->ctrl_buf;
+        uint16_t req_len = tu_min16(request->wLength, CFG_TUD_HID_EP_BUFSIZE);
 
-          uint16_t xferlen = 0;
+        uint16_t xferlen = 0;
 
-          // If host request a specific Report ID, add ID to as 1 byte of response
-          if ( (report_id != HID_REPORT_TYPE_INVALID) && (req_len > 1) )
-          {
-            *report_buf++ = report_id;
-            req_len--;
+        // If host request a specific Report ID, add ID to as 1 byte of response
+        if ((report_id != HID_REPORT_TYPE_INVALID) && (req_len > 1)) {
+          *report_buf++ = report_id;
+          req_len--;
 
-            xferlen++;
-          }
+          xferlen++;
+        }
 
-          xferlen += tud_hid_get_report_cb(hid_itf, report_id, (hid_report_type_t) report_type, report_buf, req_len);
-          TU_ASSERT( xferlen > 0 );
+        xferlen += tud_hid_get_report_cb(hid_itf, report_id, (hid_report_type_t)report_type, report_buf, req_len);
+        TU_ASSERT(xferlen > 0);
 
-          tud_control_xfer(rhport, request, p_hid->epin_buf, xferlen);
-        }
+        tud_control_xfer(rhport, request, p_hid->ctrl_buf, xferlen);
+      }
       break;
 
-      case  HID_REQ_CONTROL_SET_REPORT:
-        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);
-
-          uint8_t const* report_buf = p_hid->epout_buf;
-          uint16_t report_len = tu_min16(request->wLength, CFG_TUD_HID_EP_BUFSIZE);
+    case HID_REQ_CONTROL_SET_REPORT:
+      if (stage == CONTROL_STAGE_SETUP) {
+        TU_VERIFY(request->wLength <= sizeof(p_hid->ctrl_buf));
+        tud_control_xfer(rhport, request, p_hid->ctrl_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);
 
-          // If host request a specific Report ID, extract report ID in buffer before invoking callback
-          if ( (report_id != HID_REPORT_TYPE_INVALID) && (report_len > 1) && (report_id == report_buf[0]) )
-          {
-            report_buf++;
-            report_len--;
-          }
+        uint8_t const *report_buf = p_hid->ctrl_buf;
+        uint16_t report_len = tu_min16(request->wLength, CFG_TUD_HID_EP_BUFSIZE);
 
-          tud_hid_set_report_cb(hid_itf, report_id, (hid_report_type_t) report_type, report_buf, report_len);
+        // If host request a specific Report ID, extract report ID in buffer before invoking callback
+        if ((report_id != HID_REPORT_TYPE_INVALID) && (report_len > 1) && (report_id == report_buf[0])) {
+          report_buf++;
+          report_len--;
         }
-      break;
 
-      case HID_REQ_CONTROL_SET_IDLE:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
-          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( hid_itf, p_hid->idle_rate) );
-          }
+        tud_hid_set_report_cb(hid_itf, report_id, (hid_report_type_t)report_type, report_buf, report_len);
+      }
+      break;
 
-          tud_control_status(rhport, request);
+    case HID_REQ_CONTROL_SET_IDLE:
+      if (stage == CONTROL_STAGE_SETUP) {
+        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(hid_itf, p_hid->idle_rate));
         }
+
+        tud_control_status(rhport, request);
+      }
       break;
 
-      case HID_REQ_CONTROL_GET_IDLE:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
-          // TODO idle rate of report
-          tud_control_xfer(rhport, request, &p_hid->idle_rate, 1);
-        }
+    case HID_REQ_CONTROL_GET_IDLE:
+      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:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
-          tud_control_xfer(rhport, request, &p_hid->protocol_mode, 1);
-        }
+    case HID_REQ_CONTROL_GET_PROTOCOL:
+      if (stage == CONTROL_STAGE_SETUP) {
+        tud_control_xfer(rhport, request, &p_hid->protocol_mode, 1);
+      }
       break;
 
-      case HID_REQ_CONTROL_SET_PROTOCOL:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
-          tud_control_status(rhport, request);
-        }
-        else if ( stage == CONTROL_STAGE_ACK )
-        {
-          p_hid->protocol_mode = (uint8_t) request->wValue;
-          if (tud_hid_set_protocol_cb)
-          {
-            tud_hid_set_protocol_cb(hid_itf, p_hid->protocol_mode);
-          }
+    case HID_REQ_CONTROL_SET_PROTOCOL:
+      if (stage == CONTROL_STAGE_SETUP) {
+        tud_control_status(rhport, request);
+      } else if (stage == CONTROL_STAGE_ACK) {
+        p_hid->protocol_mode = (uint8_t)request->wValue;
+        if (tud_hid_set_protocol_cb) {
+          tud_hid_set_protocol_cb(hid_itf, p_hid->protocol_mode);
         }
+      }
       break;
 
-      default: return false; // stall unsupported request
+    default:
+      return false; // stall unsupported request
     }
-  }else
-  {
+  } else {
     return false; // stall unsupported request
   }
 
@@ -381,31 +356,43 @@ bool hidd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t
 
 bool hidd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
 {
-  (void) result;
+  (void)result;
 
   uint8_t instance = 0;
-  hidd_interface_t * p_hid = _hidd_itf;
+  hidd_interface_t *p_hid = _hidd_itf;
 
   // Identify which interface to use
-  for (instance = 0; instance < CFG_TUD_HID; instance++)
-  {
+  for (instance = 0; instance < CFG_TUD_HID; instance++) {
     p_hid = &_hidd_itf[instance];
-    if ( (ep_addr == p_hid->ep_out) || (ep_addr == p_hid->ep_in) ) break;
+    if ((ep_addr == p_hid->ep_out) || (ep_addr == p_hid->ep_in))
+      break;
   }
   TU_ASSERT(instance < CFG_TUD_HID);
 
+  // Check if there was a problem
+  if (XFER_RESULT_SUCCESS != result) { // Inform application about the issue
+    if (tud_hid_report_fail_cb) {
+      tud_hid_report_fail_cb(instance, ep_addr, (uint16_t)xferred_bytes);
+    }
+
+    // Allow a new transfer to be received if issue happened on an OUT endpoint
+    if (ep_addr == p_hid->ep_out) {
+      // Prepare the OUT endpoint to be able to receive a new transfer
+      TU_ASSERT(usbd_edpt_xfer(rhport, p_hid->ep_out, p_hid->epout_buf, sizeof(p_hid->epout_buf)));
+    }
+
+    return true;
+  }
+
   // Sent report successfully
-  if (ep_addr == p_hid->ep_in)
-  {
-    if (tud_hid_report_complete_cb)
-    {
-      tud_hid_report_complete_cb(instance, p_hid->epin_buf, (uint16_t) xferred_bytes);
+  if (ep_addr == p_hid->ep_in) {
+    if (tud_hid_report_complete_cb) {
+      tud_hid_report_complete_cb(instance, p_hid->epin_buf, (uint16_t)xferred_bytes);
     }
   }
-  // Received report
-  else if (ep_addr == p_hid->ep_out)
-  {
-    tud_hid_set_report_cb(instance, 0, HID_REPORT_TYPE_INVALID, p_hid->epout_buf, (uint16_t) xferred_bytes);
+  // Received report successfully
+  else if (ep_addr == p_hid->ep_out) {
+    tud_hid_set_report_cb(instance, 0, HID_REPORT_TYPE_OUTPUT, p_hid->epout_buf, (uint16_t)xferred_bytes);
     TU_ASSERT(usbd_edpt_xfer(rhport, p_hid->ep_out, p_hid->epout_buf, sizeof(p_hid->epout_buf)));
   }
 
diff --git a/src/class/hid/hid_device.h b/src/class/hid/hid_device.h
index 17b24def1..fcbf161c4 100644
--- a/src/class/hid/hid_device.h
+++ b/src/class/hid/hid_device.h
@@ -72,6 +72,16 @@ bool tud_hid_n_keyboard_report(uint8_t instance, uint8_t report_id, uint8_t modi
 // use template layout report as defined by hid_mouse_report_t
 bool tud_hid_n_mouse_report(uint8_t instance, uint8_t report_id, uint8_t buttons, int8_t x, int8_t y, int8_t vertical, int8_t horizontal);
 
+// ABSOLUTE MOUSE: convenient helper to send absolute mouse report if application
+// use template layout report as defined by hid_abs_mouse_report_t
+bool tud_hid_n_abs_mouse_report(uint8_t instance, uint8_t report_id, uint8_t buttons, int16_t x, int16_t y, int8_t vertical, int8_t horizontal);
+
+
+static inline bool tud_hid_abs_mouse_report(uint8_t report_id, uint8_t buttons, int16_t x, int16_t y, int8_t vertical, int8_t horizontal)
+{
+  return tud_hid_n_abs_mouse_report(0, report_id, buttons, x, y, vertical, horizontal);
+}
+
 // Gamepad: convenient helper to send gamepad report if application
 // use template layout report TUD_HID_REPORT_DESC_GAMEPAD
 bool tud_hid_n_gamepad_report(uint8_t instance, uint8_t report_id, int8_t x, int8_t y, int8_t z, int8_t rz, int8_t rx, int8_t ry, uint8_t hat, uint32_t buttons);
@@ -118,6 +128,8 @@ TU_ATTR_WEAK bool tud_hid_set_idle_cb(uint8_t instance, uint8_t idle_rate);
 // Note: For composite reports, report[0] is report ID
 TU_ATTR_WEAK void tud_hid_report_complete_cb(uint8_t instance, uint8_t const* report, uint16_t len);
 
+// Invoked when a transfer wasn't successful
+TU_ATTR_WEAK void tud_hid_report_fail_cb(uint8_t instance, uint8_t ep_addr, uint16_t len);
 
 //--------------------------------------------------------------------+
 // Inline Functions
@@ -266,6 +278,55 @@ static inline bool  tud_hid_gamepad_report(uint8_t report_id, int8_t x, int8_t y
     HID_COLLECTION_END                                            , \
   HID_COLLECTION_END \
 
+// Absolute Mouse Report Descriptor Template
+#define TUD_HID_REPORT_DESC_ABSMOUSE(...) \
+  HID_USAGE_PAGE ( HID_USAGE_PAGE_DESKTOP      )                   ,\
+  HID_USAGE      ( HID_USAGE_DESKTOP_MOUSE     )                   ,\
+  HID_COLLECTION ( HID_COLLECTION_APPLICATION  )                   ,\
+    /* Report ID if any */\
+    __VA_ARGS__ \
+    HID_USAGE      ( HID_USAGE_DESKTOP_POINTER )                   ,\
+    HID_COLLECTION ( HID_COLLECTION_PHYSICAL   )                   ,\
+      HID_USAGE_PAGE  ( HID_USAGE_PAGE_BUTTON  )                   ,\
+        HID_USAGE_MIN   ( 1                                      ) ,\
+        HID_USAGE_MAX   ( 5                                      ) ,\
+        HID_LOGICAL_MIN ( 0                                      ) ,\
+        HID_LOGICAL_MAX ( 1                                      ) ,\
+        /* Left, Right, Middle, Backward, Forward buttons */ \
+        HID_REPORT_COUNT( 5                                      ) ,\
+        HID_REPORT_SIZE ( 1                                      ) ,\
+        HID_INPUT       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\
+        /* 3 bit padding */ \
+        HID_REPORT_COUNT( 1                                      ) ,\
+        HID_REPORT_SIZE ( 3                                      ) ,\
+        HID_INPUT       ( HID_CONSTANT                           ) ,\
+      HID_USAGE_PAGE  ( HID_USAGE_PAGE_DESKTOP )                   ,\
+        /* X, Y absolute position [0, 32767] */ \
+        HID_USAGE       ( HID_USAGE_DESKTOP_X                    ) ,\
+        HID_USAGE       ( HID_USAGE_DESKTOP_Y                    ) ,\
+        HID_LOGICAL_MIN  ( 0x00                                ) ,\
+        HID_LOGICAL_MAX_N( 0x7FFF, 2                           ) ,\
+        HID_REPORT_SIZE  ( 16                                  ) ,\
+        HID_REPORT_COUNT ( 2                                   ) ,\
+        HID_INPUT       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\
+        /* Vertical wheel scroll [-127, 127] */ \
+        HID_USAGE       ( HID_USAGE_DESKTOP_WHEEL                )  ,\
+        HID_LOGICAL_MIN ( 0x81                                   )  ,\
+        HID_LOGICAL_MAX ( 0x7f                                   )  ,\
+        HID_REPORT_COUNT( 1                                      )  ,\
+        HID_REPORT_SIZE ( 8                                      )  ,\
+        HID_INPUT       ( HID_DATA | HID_VARIABLE | HID_RELATIVE )  ,\
+      HID_USAGE_PAGE  ( HID_USAGE_PAGE_CONSUMER ), \
+       /* Horizontal wheel scroll [-127, 127] */ \
+        HID_USAGE_N     ( HID_USAGE_CONSUMER_AC_PAN, 2           ), \
+        HID_LOGICAL_MIN ( 0x81                                   ), \
+        HID_LOGICAL_MAX ( 0x7f                                   ), \
+        HID_REPORT_COUNT( 1                                      ), \
+        HID_REPORT_SIZE ( 8                                      ), \
+        HID_INPUT       ( HID_DATA | HID_VARIABLE | HID_RELATIVE ), \
+    HID_COLLECTION_END                                            , \
+  HID_COLLECTION_END \
+
 // Consumer Control Report Descriptor Template
 #define TUD_HID_REPORT_DESC_CONSUMER(...) \
   HID_USAGE_PAGE ( HID_USAGE_PAGE_CONSUMER    )              ,\
@@ -402,15 +463,189 @@ static inline bool  tud_hid_gamepad_report(uint8_t report_id, int8_t x, int8_t y
       HID_OUTPUT      ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE  ),\
     HID_COLLECTION_END \
 
+// HID Lighting and Illumination Report Descriptor Template
+// - 1st parameter is report id (required)
+//   Creates 6 report ids for lighting HID usages in the following order:
+//     report_id+0: HID_USAGE_LIGHTING_LAMP_ARRAY_ATTRIBUTES_REPORT
+//     report_id+1: HID_USAGE_LIGHTING_LAMP_ATTRIBUTES_REQUEST_REPORT
+//     report_id+2: HID_USAGE_LIGHTING_LAMP_ATTRIBUTES_RESPONSE_REPORT
+//     report_id+3: HID_USAGE_LIGHTING_LAMP_MULTI_UPDATE_REPORT
+//     report_id+4: HID_USAGE_LIGHTING_LAMP_RANGE_UPDATE_REPORT
+//     report_id+5: HID_USAGE_LIGHTING_LAMP_ARRAY_CONTROL_REPORT
+#define TUD_HID_REPORT_DESC_LIGHTING(report_id) \
+  HID_USAGE_PAGE ( HID_USAGE_PAGE_LIGHTING_AND_ILLUMINATION ),\
+  HID_USAGE      ( HID_USAGE_LIGHTING_LAMP_ARRAY            ),\
+  HID_COLLECTION ( HID_COLLECTION_APPLICATION               ),\
+    /* Lamp Array Attributes Report */ \
+    HID_REPORT_ID (report_id                                    ) \
+    HID_USAGE ( HID_USAGE_LIGHTING_LAMP_ARRAY_ATTRIBUTES_REPORT ),\
+    HID_COLLECTION ( HID_COLLECTION_LOGICAL                     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_COUNT                          ),\
+      HID_LOGICAL_MIN   ( 0                                                      ),\
+      HID_LOGICAL_MAX_N ( 65535, 3                                               ),\
+      HID_REPORT_SIZE   ( 16                                                     ),\
+      HID_REPORT_COUNT  ( 1                                                      ),\
+      HID_FEATURE       ( HID_CONSTANT | HID_VARIABLE | HID_ABSOLUTE             ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BOUNDING_BOX_WIDTH_IN_MICROMETERS   ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BOUNDING_BOX_HEIGHT_IN_MICROMETERS  ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BOUNDING_BOX_DEPTH_IN_MICROMETERS   ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_ARRAY_KIND                     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_MIN_UPDATE_INTERVAL_IN_MICROSECONDS ),\
+      HID_LOGICAL_MIN   ( 0                                                      ),\
+      HID_LOGICAL_MAX_N ( 2147483647, 3                                          ),\
+      HID_REPORT_SIZE   ( 32                                                     ),\
+      HID_REPORT_COUNT  ( 5                                                      ),\
+      HID_FEATURE       ( HID_CONSTANT | HID_VARIABLE | HID_ABSOLUTE             ),\
+    HID_COLLECTION_END ,\
+    /* Lamp Attributes Request Report */ \
+    HID_REPORT_ID       ( report_id + 1                                     ) \
+    HID_USAGE           ( HID_USAGE_LIGHTING_LAMP_ATTRIBUTES_REQUEST_REPORT ),\
+    HID_COLLECTION      ( HID_COLLECTION_LOGICAL                            ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_ID             ),\
+      HID_LOGICAL_MIN   ( 0                                      ),\
+      HID_LOGICAL_MAX_N ( 65535, 3                               ),\
+      HID_REPORT_SIZE   ( 16                                     ),\
+      HID_REPORT_COUNT  ( 1                                      ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ),\
+    HID_COLLECTION_END ,\
+    /* Lamp Attributes Response Report */ \
+    HID_REPORT_ID       ( report_id + 2                                      ) \
+    HID_USAGE           ( HID_USAGE_LIGHTING_LAMP_ATTRIBUTES_RESPONSE_REPORT ),\
+    HID_COLLECTION      ( HID_COLLECTION_LOGICAL                             ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_ID                        ),\
+      HID_LOGICAL_MIN   ( 0                                                 ),\
+      HID_LOGICAL_MAX_N ( 65535, 3                                          ),\
+      HID_REPORT_SIZE   ( 16                                                ),\
+      HID_REPORT_COUNT  ( 1                                                 ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE            ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_POSITION_X_IN_MICROMETERS      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_POSITION_Y_IN_MICROMETERS      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_POSITION_Z_IN_MICROMETERS      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_UPDATE_LATENCY_IN_MICROSECONDS ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_PURPOSES                  ),\
+      HID_LOGICAL_MIN   ( 0                                                 ),\
+      HID_LOGICAL_MAX_N ( 2147483647, 3                                     ),\
+      HID_REPORT_SIZE   ( 32                                                ),\
+      HID_REPORT_COUNT  ( 5                                                 ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE            ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_LEVEL_COUNT                ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_LEVEL_COUNT              ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_LEVEL_COUNT               ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_LEVEL_COUNT          ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_IS_PROGRAMMABLE                ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INPUT_BINDING                  ),\
+      HID_LOGICAL_MIN   ( 0                                                 ),\
+      HID_LOGICAL_MAX_N ( 255, 2                                            ),\
+      HID_REPORT_SIZE   ( 8                                                 ),\
+      HID_REPORT_COUNT  ( 6                                                 ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE            ),\
+    HID_COLLECTION_END ,\
+    /* Lamp Multi-Update Report */ \
+    HID_REPORT_ID       ( report_id + 3                               ) \
+    HID_USAGE           ( HID_USAGE_LIGHTING_LAMP_MULTI_UPDATE_REPORT ),\
+    HID_COLLECTION      ( HID_COLLECTION_LOGICAL                      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_COUNT               ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_UPDATE_FLAGS        ),\
+      HID_LOGICAL_MIN   ( 0                                           ),\
+      HID_LOGICAL_MAX   ( 8                                           ),\
+      HID_REPORT_SIZE   ( 8                                           ),\
+      HID_REPORT_COUNT  ( 2                                           ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_ID                  ),\
+      HID_LOGICAL_MIN   ( 0                                           ),\
+      HID_LOGICAL_MAX_N ( 65535, 3                                    ),\
+      HID_REPORT_SIZE   ( 16                                          ),\
+      HID_REPORT_COUNT  ( 8                                           ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_LOGICAL_MIN   ( 0                                           ),\
+      HID_LOGICAL_MAX_N ( 255, 2                                      ),\
+      HID_REPORT_SIZE   ( 8                                           ),\
+      HID_REPORT_COUNT  ( 32                                          ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE      ),\
+    HID_COLLECTION_END ,\
+    /* Lamp Range Update Report */ \
+    HID_REPORT_ID       ( report_id + 4 ) \
+    HID_USAGE           ( HID_USAGE_LIGHTING_LAMP_RANGE_UPDATE_REPORT ),\
+    HID_COLLECTION      ( HID_COLLECTION_LOGICAL                      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_UPDATE_FLAGS        ),\
+      HID_LOGICAL_MIN   ( 0                                           ),\
+      HID_LOGICAL_MAX   ( 8                                           ),\
+      HID_REPORT_SIZE   ( 8                                           ),\
+      HID_REPORT_COUNT  ( 1                                           ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_ID_START            ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_ID_END              ),\
+      HID_LOGICAL_MIN   ( 0                                           ),\
+      HID_LOGICAL_MAX_N ( 65535, 3                                    ),\
+      HID_REPORT_SIZE   ( 16                                          ),\
+      HID_REPORT_COUNT  ( 2                                           ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_LOGICAL_MIN   ( 0                                           ),\
+      HID_LOGICAL_MAX_N ( 255, 2                                      ),\
+      HID_REPORT_SIZE   ( 8                                           ),\
+      HID_REPORT_COUNT  ( 4                                           ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE      ),\
+    HID_COLLECTION_END ,\
+    /* Lamp Array Control Report */ \
+    HID_REPORT_ID      ( report_id + 5                                ) \
+    HID_USAGE          ( HID_USAGE_LIGHTING_LAMP_ARRAY_CONTROL_REPORT ),\
+    HID_COLLECTION     ( HID_COLLECTION_LOGICAL                       ),\
+      HID_USAGE        ( HID_USAGE_LIGHTING_AUTONOMOUS_MODE     ),\
+      HID_LOGICAL_MIN  ( 0                                      ),\
+      HID_LOGICAL_MAX  ( 1                                      ),\
+      HID_REPORT_SIZE  ( 8                                      ),\
+      HID_REPORT_COUNT ( 1                                      ),\
+      HID_FEATURE      ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ),\
+    HID_COLLECTION_END ,\
+  HID_COLLECTION_END \
+
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     hidd_init            (void);
+bool     hidd_deinit          (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
  }
 #endif
diff --git a/src/class/hid/hid_host.c b/src/class/hid/hid_host.c
index db52b503f..115a8a4df 100644
--- a/src/class/hid/hid_host.c
+++ b/src/class/hid/hid_host.c
@@ -39,22 +39,22 @@
 #endif
 
 #define TU_LOG_DRV(...)   TU_LOG(CFG_TUH_HID_LOG_LEVEL, __VA_ARGS__)
+
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-
-typedef struct
-{
+typedef struct {
   uint8_t daddr;
 
   uint8_t itf_num;
   uint8_t ep_in;
   uint8_t ep_out;
+  bool mounted;           // Enumeration is complete
 
   uint8_t itf_protocol;   // None, Keyboard, Mouse
   uint8_t protocol_mode;  // Boot (0) or Report protocol (1)
 
-  uint8_t  report_desc_type;
+  uint8_t report_desc_type;
   uint16_t report_desc_len;
 
   uint16_t epin_size;
@@ -72,78 +72,57 @@ tu_static uint8_t _hidh_default_protocol = HID_PROTOCOL_BOOT;
 //--------------------------------------------------------------------+
 // Helper
 //--------------------------------------------------------------------+
-
-TU_ATTR_ALWAYS_INLINE static inline
-hidh_interface_t* get_hid_itf(uint8_t daddr, uint8_t idx)
-{
+TU_ATTR_ALWAYS_INLINE static inline hidh_interface_t* get_hid_itf(uint8_t daddr, uint8_t idx) {
   TU_ASSERT(daddr > 0 && idx < CFG_TUH_HID, NULL);
   hidh_interface_t* p_hid = &_hidh_itf[idx];
   return (p_hid->daddr == daddr) ? p_hid : NULL;
 }
 
 // Get instance ID by endpoint address
-static uint8_t get_idx_by_epaddr(uint8_t daddr, uint8_t ep_addr)
-{
-  for ( uint8_t idx = 0; idx < CFG_TUH_HID; idx++ )
-  {
-    hidh_interface_t const * p_hid = &_hidh_itf[idx];
-
-    if ( p_hid->daddr == daddr &&
-         (p_hid->ep_in == ep_addr || p_hid->ep_out == ep_addr) )
-    {
+static uint8_t get_idx_by_epaddr(uint8_t daddr, uint8_t ep_addr) {
+  for (uint8_t idx = 0; idx < CFG_TUH_HID; idx++) {
+    hidh_interface_t const* p_hid = &_hidh_itf[idx];
+    if (p_hid->daddr == daddr &&
+        (p_hid->ep_in == ep_addr || p_hid->ep_out == ep_addr)) {
       return idx;
     }
   }
-
   return TUSB_INDEX_INVALID_8;
 }
 
-static hidh_interface_t* find_new_itf(void)
-{
-  for(uint8_t i=0; i<CFG_TUH_HID; i++)
-  {
+static hidh_interface_t* find_new_itf(void) {
+  for (uint8_t i = 0; i < CFG_TUH_HID; i++) {
     if (_hidh_itf[i].daddr == 0) return &_hidh_itf[i];
   }
-
   return NULL;
 }
 
 //--------------------------------------------------------------------+
 // Interface API
 //--------------------------------------------------------------------+
-
-uint8_t tuh_hid_itf_get_count(uint8_t daddr)
-{
+uint8_t tuh_hid_itf_get_count(uint8_t daddr) {
   uint8_t count = 0;
-
-  for(uint8_t i=0; i<CFG_TUH_HID; i++)
-  {
+  for (uint8_t i = 0; i < CFG_TUH_HID; i++) {
     if (_hidh_itf[i].daddr == daddr) count++;
   }
-
   return count;
 }
 
-uint8_t tuh_hid_itf_get_total_count(void)
-{
+uint8_t tuh_hid_itf_get_total_count(void) {
   uint8_t count = 0;
-
-  for(uint8_t i=0; i<CFG_TUH_HID; i++)
-  {
+  for (uint8_t i = 0; i < CFG_TUH_HID; i++) {
     if (_hidh_itf[i].daddr != 0) count++;
   }
-
   return count;
 }
 
-bool tuh_hid_mounted(uint8_t daddr, uint8_t idx)
-{
+bool tuh_hid_mounted(uint8_t daddr, uint8_t idx) {
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
-  return p_hid != NULL;
+  TU_VERIFY(p_hid);
+  return p_hid->mounted;
 }
 
-bool tuh_hid_itf_get_info(uint8_t daddr, uint8_t idx, tuh_itf_info_t* info)
-{
+bool tuh_hid_itf_get_info(uint8_t daddr, uint8_t idx, tuh_itf_info_t* info) {
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
   TU_VERIFY(p_hid && info);
 
@@ -151,34 +130,30 @@ bool tuh_hid_itf_get_info(uint8_t daddr, uint8_t idx, tuh_itf_info_t* info)
 
   // re-construct descriptor
   tusb_desc_interface_t* desc = &info->desc;
-  desc->bLength            = sizeof(tusb_desc_interface_t);
-  desc->bDescriptorType    = TUSB_DESC_INTERFACE;
+  desc->bLength = sizeof(tusb_desc_interface_t);
+  desc->bDescriptorType = TUSB_DESC_INTERFACE;
 
-  desc->bInterfaceNumber   = p_hid->itf_num;
-  desc->bAlternateSetting  = 0;
-  desc->bNumEndpoints      = (uint8_t) ((p_hid->ep_in ? 1u : 0u) + (p_hid->ep_out ? 1u : 0u));
-  desc->bInterfaceClass    = TUSB_CLASS_HID;
+  desc->bInterfaceNumber = p_hid->itf_num;
+  desc->bAlternateSetting = 0;
+  desc->bNumEndpoints = (uint8_t) ((p_hid->ep_in ? 1u : 0u) + (p_hid->ep_out ? 1u : 0u));
+  desc->bInterfaceClass = TUSB_CLASS_HID;
   desc->bInterfaceSubClass = (p_hid->itf_protocol ? HID_SUBCLASS_BOOT : HID_SUBCLASS_NONE);
   desc->bInterfaceProtocol = p_hid->itf_protocol;
-  desc->iInterface         = 0; // not used yet
+  desc->iInterface = 0; // not used yet
 
   return true;
 }
 
-uint8_t tuh_hid_itf_get_index(uint8_t daddr, uint8_t itf_num)
-{
-  for ( uint8_t idx = 0; idx < CFG_TUH_HID; idx++ )
-  {
-    hidh_interface_t const * p_hid = &_hidh_itf[idx];
-
-    if ( p_hid->daddr == daddr && p_hid->itf_num == itf_num) return idx;
+uint8_t tuh_hid_itf_get_index(uint8_t daddr, uint8_t itf_num) {
+  for (uint8_t idx = 0; idx < CFG_TUH_HID; idx++) {
+    hidh_interface_t const* p_hid = &_hidh_itf[idx];
+    if (p_hid->daddr == daddr && p_hid->itf_num == itf_num) return idx;
   }
 
   return TUSB_INDEX_INVALID_8;
 }
 
-uint8_t tuh_hid_interface_protocol(uint8_t daddr, uint8_t idx)
-{
+uint8_t tuh_hid_interface_protocol(uint8_t daddr, uint8_t idx) {
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
   return p_hid ? p_hid->itf_protocol : 0;
 }
@@ -186,29 +161,24 @@ uint8_t tuh_hid_interface_protocol(uint8_t daddr, uint8_t idx)
 //--------------------------------------------------------------------+
 // Control Endpoint API
 //--------------------------------------------------------------------+
-
-uint8_t tuh_hid_get_protocol(uint8_t daddr, uint8_t idx)
-{
+uint8_t tuh_hid_get_protocol(uint8_t daddr, uint8_t idx) {
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
   return p_hid ? p_hid->protocol_mode : 0;
 }
 
-static void set_protocol_complete(tuh_xfer_t* xfer)
-{
+static void set_protocol_complete(tuh_xfer_t* xfer) {
   uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
-  uint8_t const daddr   = xfer->daddr;
-  uint8_t const idx     = tuh_hid_itf_get_index(daddr, itf_num);
+  uint8_t const daddr = xfer->daddr;
+  uint8_t const idx = tuh_hid_itf_get_index(daddr, itf_num);
 
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
-  TU_VERIFY(p_hid, );
+  TU_VERIFY(p_hid,);
 
-  if (XFER_RESULT_SUCCESS == xfer->result)
-  {
+  if (XFER_RESULT_SUCCESS == xfer->result) {
     p_hid->protocol_mode = (uint8_t) tu_le16toh(xfer->setup->wValue);
   }
 
-  if (tuh_hid_set_protocol_complete_cb)
-  {
+  if (tuh_hid_set_protocol_complete_cb) {
     tuh_hid_set_protocol_complete_cb(daddr, idx, p_hid->protocol_mode);
   }
 }
@@ -217,123 +187,153 @@ void tuh_hid_set_default_protocol(uint8_t protocol) {
   _hidh_default_protocol = protocol;
 }
 
-static bool _hidh_set_protocol(uint8_t daddr, uint8_t itf_num, uint8_t protocol, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+static bool _hidh_set_protocol(uint8_t daddr, uint8_t itf_num, uint8_t protocol,
+                               tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   TU_LOG_DRV("HID Set Protocol = %d\r\n", protocol);
 
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_INTERFACE,
-      .type      = TUSB_REQ_TYPE_CLASS,
-      .direction = TUSB_DIR_OUT
-    },
-    .bRequest = HID_REQ_CONTROL_SET_PROTOCOL,
-    .wValue   = protocol,
-    .wIndex   = itf_num,
-    .wLength  = 0
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_INTERFACE,
+          .type      = TUSB_REQ_TYPE_CLASS,
+          .direction = TUSB_DIR_OUT
+      },
+      .bRequest = HID_REQ_CONTROL_SET_PROTOCOL,
+      .wValue   = protocol,
+      .wIndex   = itf_num,
+      .wLength  = 0
   };
 
-  tuh_xfer_t xfer =
-  {
-    .daddr       = daddr,
-    .ep_addr     = 0,
-    .setup       = &request,
-    .buffer      = NULL,
-    .complete_cb = complete_cb,
-    .user_data   = user_data
+  tuh_xfer_t xfer = {
+      .daddr       = daddr,
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = NULL,
+      .complete_cb = complete_cb,
+      .user_data   = user_data
   };
 
   return tuh_control_xfer(&xfer);
 }
 
-bool tuh_hid_set_protocol(uint8_t daddr, uint8_t idx, uint8_t protocol)
-{
+bool tuh_hid_set_protocol(uint8_t daddr, uint8_t idx, uint8_t protocol) {
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
   TU_VERIFY(p_hid && p_hid->itf_protocol != HID_ITF_PROTOCOL_NONE);
 
   return _hidh_set_protocol(daddr, p_hid->itf_num, protocol, set_protocol_complete, 0);
 }
 
-static void set_report_complete(tuh_xfer_t* xfer)
-{
+static void get_report_complete(tuh_xfer_t* xfer) {
+  TU_LOG_DRV("HID Get Report complete\r\n");
+
+  if (tuh_hid_get_report_complete_cb) {
+    uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
+    uint8_t const idx = tuh_hid_itf_get_index(xfer->daddr, itf_num);
+
+    uint8_t const report_type = tu_u16_high(xfer->setup->wValue);
+    uint8_t const report_id = tu_u16_low(xfer->setup->wValue);
+
+    tuh_hid_get_report_complete_cb(xfer->daddr, idx, report_id, report_type,
+                                   (xfer->result == XFER_RESULT_SUCCESS) ? xfer->setup->wLength : 0);
+  }
+}
+
+bool tuh_hid_get_report(uint8_t daddr, uint8_t idx, uint8_t report_id, uint8_t report_type, void* report, uint16_t len) {
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  TU_VERIFY(p_hid);
+  TU_LOG_DRV("HID Get Report: id = %u, type = %u, len = %u\r\n", report_id, report_type, len);
+
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_INTERFACE,
+          .type      = TUSB_REQ_TYPE_CLASS,
+          .direction = TUSB_DIR_IN
+      },
+      .bRequest = HID_REQ_CONTROL_GET_REPORT,
+      .wValue   = tu_htole16(tu_u16(report_type, report_id)),
+      .wIndex   = tu_htole16((uint16_t) p_hid->itf_num),
+      .wLength  = len
+  };
+
+  tuh_xfer_t xfer = {
+      .daddr       = daddr,
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = report,
+      .complete_cb = get_report_complete,
+      .user_data   = 0
+  };
+
+  return tuh_control_xfer(&xfer);
+}
+
+static void set_report_complete(tuh_xfer_t* xfer) {
   TU_LOG_DRV("HID Set Report complete\r\n");
 
-  if (tuh_hid_set_report_complete_cb)
-  {
+  if (tuh_hid_set_report_complete_cb) {
     uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
-    uint8_t const idx     = tuh_hid_itf_get_index(xfer->daddr, itf_num);
+    uint8_t const idx = tuh_hid_itf_get_index(xfer->daddr, itf_num);
 
     uint8_t const report_type = tu_u16_high(xfer->setup->wValue);
-    uint8_t const report_id   = tu_u16_low(xfer->setup->wValue);
+    uint8_t const report_id = tu_u16_low(xfer->setup->wValue);
 
     tuh_hid_set_report_complete_cb(xfer->daddr, idx, report_id, report_type,
                                    (xfer->result == XFER_RESULT_SUCCESS) ? xfer->setup->wLength : 0);
   }
 }
 
-bool tuh_hid_set_report(uint8_t daddr, uint8_t idx, uint8_t report_id, uint8_t report_type, void* report, uint16_t len)
-{
+bool tuh_hid_set_report(uint8_t daddr, uint8_t idx, uint8_t report_id, uint8_t report_type, void* report, uint16_t len) {
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
   TU_VERIFY(p_hid);
-
   TU_LOG_DRV("HID Set Report: id = %u, type = %u, len = %u\r\n", report_id, report_type, len);
 
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_INTERFACE,
-      .type      = TUSB_REQ_TYPE_CLASS,
-      .direction = TUSB_DIR_OUT
-    },
-    .bRequest = HID_REQ_CONTROL_SET_REPORT,
-    .wValue   = tu_htole16(tu_u16(report_type, report_id)),
-    .wIndex   = tu_htole16((uint16_t)p_hid->itf_num),
-    .wLength  = len
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_INTERFACE,
+          .type      = TUSB_REQ_TYPE_CLASS,
+          .direction = TUSB_DIR_OUT
+      },
+      .bRequest = HID_REQ_CONTROL_SET_REPORT,
+      .wValue   = tu_htole16(tu_u16(report_type, report_id)),
+      .wIndex   = tu_htole16((uint16_t) p_hid->itf_num),
+      .wLength  = len
   };
 
-  tuh_xfer_t xfer =
-  {
-    .daddr       = daddr,
-    .ep_addr     = 0,
-    .setup       = &request,
-    .buffer      = report,
-    .complete_cb = set_report_complete,
-    .user_data   = 0
+  tuh_xfer_t xfer = {
+      .daddr       = daddr,
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = report,
+      .complete_cb = set_report_complete,
+      .user_data   = 0
   };
 
   return tuh_control_xfer(&xfer);
 }
 
-static bool _hidh_set_idle(uint8_t daddr, uint8_t itf_num, uint16_t idle_rate, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+static bool _hidh_set_idle(uint8_t daddr, uint8_t itf_num, uint16_t idle_rate,
+                           tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   // SET IDLE request, device can stall if not support this request
   TU_LOG_DRV("HID Set Idle \r\n");
 
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_INTERFACE,
-      .type      = TUSB_REQ_TYPE_CLASS,
-      .direction = TUSB_DIR_OUT
-    },
-    .bRequest = HID_REQ_CONTROL_SET_IDLE,
-    .wValue   = tu_htole16(idle_rate),
-    .wIndex   = tu_htole16((uint16_t)itf_num),
-    .wLength  = 0
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_INTERFACE,
+          .type      = TUSB_REQ_TYPE_CLASS,
+          .direction = TUSB_DIR_OUT
+      },
+      .bRequest = HID_REQ_CONTROL_SET_IDLE,
+      .wValue   = tu_htole16(idle_rate),
+      .wIndex   = tu_htole16((uint16_t) itf_num),
+      .wLength  = 0
   };
 
-  tuh_xfer_t xfer =
-  {
-    .daddr       = daddr,
-    .ep_addr     = 0,
-    .setup       = &request,
-    .buffer      = NULL,
-    .complete_cb = complete_cb,
-    .user_data   = user_data
+  tuh_xfer_t xfer = {
+      .daddr       = daddr,
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = NULL,
+      .complete_cb = complete_cb,
+      .user_data   = user_data
   };
 
   return tuh_control_xfer(&xfer);
@@ -344,68 +344,60 @@ static bool _hidh_set_idle(uint8_t daddr, uint8_t itf_num, uint16_t idle_rate, t
 //--------------------------------------------------------------------+
 
 // Check if HID interface is ready to receive report
-bool tuh_hid_receive_ready(uint8_t dev_addr, uint8_t idx)
-{
+bool tuh_hid_receive_ready(uint8_t dev_addr, uint8_t idx) {
   hidh_interface_t* p_hid = get_hid_itf(dev_addr, idx);
   TU_VERIFY(p_hid);
-
   return !usbh_edpt_busy(dev_addr, p_hid->ep_in);
 }
 
-bool tuh_hid_receive_report(uint8_t daddr, uint8_t idx)
-{
+bool tuh_hid_receive_report(uint8_t daddr, uint8_t idx) {
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
   TU_VERIFY(p_hid);
 
   // claim endpoint
-  TU_VERIFY( usbh_edpt_claim(daddr, p_hid->ep_in) );
+  TU_VERIFY(usbh_edpt_claim(daddr, p_hid->ep_in));
 
-  if ( !usbh_edpt_xfer(daddr, p_hid->ep_in, p_hid->epin_buf, p_hid->epin_size) )
-  {
+  if (!usbh_edpt_xfer(daddr, p_hid->ep_in, p_hid->epin_buf, p_hid->epin_size)) {
     usbh_edpt_release(daddr, p_hid->ep_in);
     return false;
   }
 
   return true;
 }
-
-bool tuh_hid_send_ready(uint8_t dev_addr, uint8_t idx)
-{
+bool tuh_hid_receive_abort(uint8_t dev_addr, uint8_t idx) {
   hidh_interface_t* p_hid = get_hid_itf(dev_addr, idx);
   TU_VERIFY(p_hid);
+  return tuh_edpt_abort_xfer(dev_addr, p_hid->ep_in);
+}
 
+bool tuh_hid_send_ready(uint8_t dev_addr, uint8_t idx) {
+  hidh_interface_t* p_hid = get_hid_itf(dev_addr, idx);
+  TU_VERIFY(p_hid);
   return !usbh_edpt_busy(dev_addr, p_hid->ep_out);
 }
 
-bool tuh_hid_send_report(uint8_t daddr, uint8_t idx, uint8_t report_id, const void* report, uint16_t len)
-{
+bool tuh_hid_send_report(uint8_t daddr, uint8_t idx, uint8_t report_id, const void* report, uint16_t len) {
   TU_LOG_DRV("HID Send Report %d\r\n", report_id);
 
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
   TU_VERIFY(p_hid);
 
-  if (p_hid->ep_out == 0)
-  {
+  if (p_hid->ep_out == 0) {
     // This HID does not have an out endpoint (other than control)
     return false;
-  }
-  else if (len > CFG_TUH_HID_EPOUT_BUFSIZE ||
-           (report_id != 0 && len > (CFG_TUH_HID_EPOUT_BUFSIZE - 1)))
-  {
+  } else if (len > CFG_TUH_HID_EPOUT_BUFSIZE ||
+             (report_id != 0 && len > (CFG_TUH_HID_EPOUT_BUFSIZE - 1))) {
     // ep_out buffer is not large enough to hold contents
     return false;
   }
 
   // claim endpoint
-  TU_VERIFY( usbh_edpt_claim(daddr, p_hid->ep_out) );
+  TU_VERIFY(usbh_edpt_claim(daddr, p_hid->ep_out));
 
-  if (report_id == 0)
-  {
+  if (report_id == 0) {
     // No report ID in transmission
     memcpy(&p_hid->epout_buf[0], report, len);
-  }
-  else
-  {
+  } else {
     p_hid->epout_buf[0] = report_id;
     memcpy(&p_hid->epout_buf[1], report, len);
     ++len; // 1 more byte for report_id
@@ -413,8 +405,7 @@ bool tuh_hid_send_report(uint8_t daddr, uint8_t idx, uint8_t report_id, const vo
 
   TU_LOG3_MEM(p_hid->epout_buf, len, 2);
 
-  if ( !usbh_edpt_xfer(daddr, p_hid->ep_out, p_hid->epout_buf, len) )
-  {
+  if (!usbh_edpt_xfer(daddr, p_hid->ep_out, p_hid->epout_buf, len)) {
     usbh_edpt_release(daddr, p_hid->ep_out);
     return false;
   }
@@ -425,13 +416,17 @@ bool tuh_hid_send_report(uint8_t daddr, uint8_t idx, uint8_t report_id, const vo
 //--------------------------------------------------------------------+
 // USBH API
 //--------------------------------------------------------------------+
-void hidh_init(void)
-{
+bool hidh_init(void) {
+  TU_LOG_DRV("sizeof(hidh_interface_t) = %u\r\n", sizeof(hidh_interface_t));
   tu_memclr(_hidh_itf, sizeof(_hidh_itf));
+  return true;
+}
+
+bool hidh_deinit(void) {
+  return true;
 }
 
-bool hidh_xfer_cb(uint8_t daddr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
+bool hidh_xfer_cb(uint8_t daddr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
   (void) result;
 
   uint8_t const dir = tu_edpt_dir(ep_addr);
@@ -440,29 +435,26 @@ bool hidh_xfer_cb(uint8_t daddr, uint8_t ep_addr, xfer_result_t result, uint32_t
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
   TU_VERIFY(p_hid);
 
-  if ( dir == TUSB_DIR_IN )
-  {
+  if (dir == TUSB_DIR_IN) {
     TU_LOG_DRV("  Get Report callback (%u, %u)\r\n", daddr, idx);
     TU_LOG3_MEM(p_hid->epin_buf, xferred_bytes, 2);
     tuh_hid_report_received_cb(daddr, idx, p_hid->epin_buf, (uint16_t) xferred_bytes);
-  }else
-  {
-    if (tuh_hid_report_sent_cb) tuh_hid_report_sent_cb(daddr, idx, p_hid->epout_buf, (uint16_t) xferred_bytes);
+  } else {
+    if (tuh_hid_report_sent_cb) {
+      tuh_hid_report_sent_cb(daddr, idx, p_hid->epout_buf, (uint16_t) xferred_bytes);
+    }
   }
 
   return true;
 }
 
-void hidh_close(uint8_t daddr)
-{
-  for(uint8_t i=0; i<CFG_TUH_HID; i++)
-  {
+void hidh_close(uint8_t daddr) {
+  for (uint8_t i = 0; i < CFG_TUH_HID; i++) {
     hidh_interface_t* p_hid = &_hidh_itf[i];
-    if (p_hid->daddr == daddr)
-    {
+    if (p_hid->daddr == daddr) {
       TU_LOG_DRV("  HIDh close addr = %u index = %u\r\n", daddr, i);
-      if(tuh_hid_umount_cb) tuh_hid_umount_cb(daddr, i);
-      p_hid->daddr = 0;
+      if (tuh_hid_umount_cb) tuh_hid_umount_cb(daddr, i);
+      tu_memclr(p_hid, sizeof(hidh_interface_t));
     }
   }
 }
@@ -471,25 +463,22 @@ void hidh_close(uint8_t daddr)
 // Enumeration
 //--------------------------------------------------------------------+
 
-bool hidh_open(uint8_t rhport, uint8_t daddr, tusb_desc_interface_t const *desc_itf, uint16_t max_len)
-{
+bool hidh_open(uint8_t rhport, uint8_t daddr, tusb_desc_interface_t const* desc_itf, uint16_t max_len) {
   (void) rhport;
   (void) max_len;
 
   TU_VERIFY(TUSB_CLASS_HID == desc_itf->bInterfaceClass);
-
   TU_LOG_DRV("[%u] HID opening Interface %u\r\n", daddr, desc_itf->bInterfaceNumber);
 
   // len = interface + hid + n*endpoints
   uint16_t const drv_len = (uint16_t) (sizeof(tusb_desc_interface_t) + sizeof(tusb_hid_descriptor_hid_t) +
                                        desc_itf->bNumEndpoints * sizeof(tusb_desc_endpoint_t));
   TU_ASSERT(max_len >= drv_len);
-
-  uint8_t const *p_desc = (uint8_t const *) desc_itf;
+  uint8_t const* p_desc = (uint8_t const*) desc_itf;
 
   //------------- HID descriptor -------------//
   p_desc = tu_desc_next(p_desc);
-  tusb_hid_descriptor_hid_t const *desc_hid = (tusb_hid_descriptor_hid_t const *) p_desc;
+  tusb_hid_descriptor_hid_t const* desc_hid = (tusb_hid_descriptor_hid_t const*) p_desc;
   TU_ASSERT(HID_DESC_TYPE_HID == desc_hid->bDescriptorType);
 
   hidh_interface_t* p_hid = find_new_itf();
@@ -498,38 +487,33 @@ bool hidh_open(uint8_t rhport, uint8_t daddr, tusb_desc_interface_t const *desc_
 
   //------------- Endpoint Descriptors -------------//
   p_desc = tu_desc_next(p_desc);
-  tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+  tusb_desc_endpoint_t const* desc_ep = (tusb_desc_endpoint_t const*) p_desc;
 
-  for(int i = 0; i < desc_itf->bNumEndpoints; i++)
-  {
+  for (int i = 0; i < desc_itf->bNumEndpoints; i++) {
     TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType);
-    TU_ASSERT( tuh_edpt_open(daddr, desc_ep) );
+    TU_ASSERT(tuh_edpt_open(daddr, desc_ep));
 
-    if(tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN)
-    {
-      p_hid->ep_in     = desc_ep->bEndpointAddress;
+    if (tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN) {
+      p_hid->ep_in = desc_ep->bEndpointAddress;
       p_hid->epin_size = tu_edpt_packet_size(desc_ep);
-    }
-    else
-    {
-      p_hid->ep_out     = desc_ep->bEndpointAddress;
+    } else {
+      p_hid->ep_out = desc_ep->bEndpointAddress;
       p_hid->epout_size = tu_edpt_packet_size(desc_ep);
     }
 
     p_desc = tu_desc_next(p_desc);
-    desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+    desc_ep = (tusb_desc_endpoint_t const*) p_desc;
   }
 
-  p_hid->itf_num   = desc_itf->bInterfaceNumber;
+  p_hid->itf_num = desc_itf->bInterfaceNumber;
 
   // Assume bNumDescriptors = 1
   p_hid->report_desc_type = desc_hid->bReportType;
-  p_hid->report_desc_len  = tu_unaligned_read16(&desc_hid->wReportLength);
+  p_hid->report_desc_len = tu_unaligned_read16(&desc_hid->wReportLength);
 
   // Per HID Specs: default is Report protocol, though we will force Boot protocol when set_config
   p_hid->protocol_mode = _hidh_default_protocol;
-  if ( HID_SUBCLASS_BOOT == desc_itf->bInterfaceSubClass )
-  {
+  if (HID_SUBCLASS_BOOT == desc_itf->bInterfaceSubClass) {
     p_hid->itf_protocol = desc_itf->bInterfaceProtocol;
   }
 
@@ -550,15 +534,14 @@ enum {
 static void config_driver_mount_complete(uint8_t daddr, uint8_t idx, uint8_t const* desc_report, uint16_t desc_len);
 static void process_set_config(tuh_xfer_t* xfer);
 
-bool hidh_set_config(uint8_t daddr, uint8_t itf_num)
-{
+bool hidh_set_config(uint8_t daddr, uint8_t itf_num) {
   tusb_control_request_t request;
   request.wIndex = tu_htole16((uint16_t) itf_num);
 
   tuh_xfer_t xfer;
-  xfer.daddr     = daddr;
-  xfer.result    = XFER_RESULT_SUCCESS;
-  xfer.setup     = &request;
+  xfer.daddr = daddr;
+  xfer.result = XFER_RESULT_SUCCESS;
+  xfer.setup = &request;
   xfer.user_data = CONFG_SET_IDLE;
 
   // fake request to kick-off the set config process
@@ -567,71 +550,68 @@ bool hidh_set_config(uint8_t daddr, uint8_t itf_num)
   return true;
 }
 
-static void process_set_config(tuh_xfer_t* xfer)
-{
+static void process_set_config(tuh_xfer_t* xfer) {
   // Stall is a valid response for SET_IDLE, sometime SET_PROTOCOL as well
   // therefore we could ignore its result
-  if ( !(xfer->setup->bRequest == HID_REQ_CONTROL_SET_IDLE ||
-         xfer->setup->bRequest == HID_REQ_CONTROL_SET_PROTOCOL) )
-  {
-    TU_ASSERT(xfer->result == XFER_RESULT_SUCCESS, );
+  if (!(xfer->setup->bRequest == HID_REQ_CONTROL_SET_IDLE ||
+        xfer->setup->bRequest == HID_REQ_CONTROL_SET_PROTOCOL)) {
+    TU_ASSERT(xfer->result == XFER_RESULT_SUCCESS,);
   }
 
   uintptr_t const state = xfer->user_data;
   uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
-  uint8_t const daddr   = xfer->daddr;
+  uint8_t const daddr = xfer->daddr;
 
-  uint8_t const idx       = tuh_hid_itf_get_index(daddr, itf_num);
+  uint8_t const idx = tuh_hid_itf_get_index(daddr, itf_num);
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
-  TU_VERIFY(p_hid, );
+  TU_VERIFY(p_hid,);
 
-  switch(state)
-  {
-    case CONFG_SET_IDLE:
-    {
+  switch (state) {
+    case CONFG_SET_IDLE: {
       // Idle rate = 0 mean only report when there is changes
       const uint16_t idle_rate = 0;
-      const uintptr_t next_state = (p_hid->itf_protocol != HID_ITF_PROTOCOL_NONE) ? CONFIG_SET_PROTOCOL : CONFIG_GET_REPORT_DESC;
+      const uintptr_t next_state = (p_hid->itf_protocol != HID_ITF_PROTOCOL_NONE)
+                                   ? CONFIG_SET_PROTOCOL : CONFIG_GET_REPORT_DESC;
       _hidh_set_idle(daddr, itf_num, idle_rate, process_set_config, next_state);
+      break;
     }
-    break;
 
     case CONFIG_SET_PROTOCOL:
       _hidh_set_protocol(daddr, p_hid->itf_num, _hidh_default_protocol, process_set_config, CONFIG_GET_REPORT_DESC);
-    break;
+      break;
 
     case CONFIG_GET_REPORT_DESC:
       // Get Report Descriptor if possible
       // using usbh enumeration buffer since report descriptor can be very long
-      if( p_hid->report_desc_len > CFG_TUH_ENUMERATION_BUFSIZE )
-      {
+      if (p_hid->report_desc_len > CFG_TUH_ENUMERATION_BUFSIZE) {
         TU_LOG_DRV("HID Skip Report Descriptor since it is too large %u bytes\r\n", p_hid->report_desc_len);
 
         // Driver is mounted without report descriptor
         config_driver_mount_complete(daddr, idx, NULL, 0);
-      }else
-      {
-        tuh_descriptor_get_hid_report(daddr, itf_num, p_hid->report_desc_type, 0, usbh_get_enum_buf(), p_hid->report_desc_len, process_set_config, CONFIG_COMPLETE);
+      } else {
+        tuh_descriptor_get_hid_report(daddr, itf_num, p_hid->report_desc_type, 0,
+                                      usbh_get_enum_buf(), p_hid->report_desc_len,
+                                      process_set_config, CONFIG_COMPLETE);
       }
       break;
 
-    case CONFIG_COMPLETE:
-    {
+    case CONFIG_COMPLETE: {
       uint8_t const* desc_report = usbh_get_enum_buf();
-      uint16_t const desc_len    = tu_le16toh(xfer->setup->wLength);
+      uint16_t const desc_len = tu_le16toh(xfer->setup->wLength);
 
       config_driver_mount_complete(daddr, idx, desc_report, desc_len);
+      break;
     }
-    break;
 
-    default: break;
+    default:
+      break;
   }
 }
 
-static void config_driver_mount_complete(uint8_t daddr, uint8_t idx, uint8_t const* desc_report, uint16_t desc_len)
-{
+static void config_driver_mount_complete(uint8_t daddr, uint8_t idx, uint8_t const* desc_report, uint16_t desc_len) {
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
-  TU_VERIFY(p_hid, );
+  TU_VERIFY(p_hid,);
+  p_hid->mounted = true;
 
   // enumeration is complete
   if (tuh_hid_mount_cb) tuh_hid_mount_cb(daddr, idx, desc_report, desc_len);
@@ -644,21 +624,19 @@ static void config_driver_mount_complete(uint8_t daddr, uint8_t idx, uint8_t con
 // Report Descriptor Parser
 //--------------------------------------------------------------------+
 
-uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* report_info_arr, uint8_t arr_count, uint8_t const* desc_report, uint16_t desc_len)
-{
+uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* report_info_arr, uint8_t arr_count,
+                                        uint8_t const* desc_report, uint16_t desc_len) {
   // Report Item 6.2.2.2 USB HID 1.11
-  union TU_ATTR_PACKED
-  {
+  union TU_ATTR_PACKED {
     uint8_t byte;
-    struct TU_ATTR_PACKED
-    {
-        uint8_t size : 2;
-        uint8_t type : 2;
-        uint8_t tag  : 4;
+    struct TU_ATTR_PACKED {
+      uint8_t size : 2;
+      uint8_t type : 2;
+      uint8_t tag : 4;
     };
   } header;
 
-  tu_memclr(report_info_arr, arr_count*sizeof(tuh_hid_report_info_t));
+  tu_memclr(report_info_arr, arr_count * sizeof(tuh_hid_report_info_t));
 
   uint8_t report_num = 0;
   tuh_hid_report_info_t* info = report_info_arr;
@@ -668,114 +646,105 @@ uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* report_info_arr,
 //  uint8_t ri_report_size = 0;
 
   uint8_t ri_collection_depth = 0;
-
-  while(desc_len && report_num < arr_count)
-  {
+  while (desc_len && report_num < arr_count) {
     header.byte = *desc_report++;
     desc_len--;
 
-    uint8_t const tag  = header.tag;
+    uint8_t const tag = header.tag;
     uint8_t const type = header.type;
     uint8_t const size = header.size;
 
     uint8_t const data8 = desc_report[0];
 
     TU_LOG(3, "tag = %d, type = %d, size = %d, data = ", tag, type, size);
-    for(uint32_t i=0; i<size; i++) TU_LOG(3, "%02X ", desc_report[i]);
+    for (uint32_t i = 0; i < size; i++) TU_LOG(3, "%02X ", desc_report[i]);
     TU_LOG(3, "\r\n");
 
-    switch(type)
-    {
+    switch (type) {
       case RI_TYPE_MAIN:
-        switch (tag)
-        {
+        switch (tag) {
           case RI_MAIN_INPUT: break;
           case RI_MAIN_OUTPUT: break;
           case RI_MAIN_FEATURE: break;
-
           case RI_MAIN_COLLECTION:
             ri_collection_depth++;
-          break;
+            break;
 
           case RI_MAIN_COLLECTION_END:
             ri_collection_depth--;
-            if (ri_collection_depth == 0)
-            {
+            if (ri_collection_depth == 0) {
               info++;
               report_num++;
             }
-          break;
+            break;
 
-          default: break;
+          default:break;
         }
-      break;
+        break;
 
       case RI_TYPE_GLOBAL:
-        switch(tag)
-        {
+        switch (tag) {
           case RI_GLOBAL_USAGE_PAGE:
             // only take in account the "usage page" before REPORT ID
-            if ( ri_collection_depth == 0 ) memcpy(&info->usage_page, desc_report, size);
-          break;
+            if (ri_collection_depth == 0) memcpy(&info->usage_page, desc_report, size);
+            break;
 
-          case RI_GLOBAL_LOGICAL_MIN   : break;
-          case RI_GLOBAL_LOGICAL_MAX   : break;
-          case RI_GLOBAL_PHYSICAL_MIN  : break;
-          case RI_GLOBAL_PHYSICAL_MAX  : break;
+          case RI_GLOBAL_LOGICAL_MIN: break;
+          case RI_GLOBAL_LOGICAL_MAX: break;
+          case RI_GLOBAL_PHYSICAL_MIN: break;
+          case RI_GLOBAL_PHYSICAL_MAX: break;
 
           case RI_GLOBAL_REPORT_ID:
             info->report_id = data8;
-          break;
+            break;
 
           case RI_GLOBAL_REPORT_SIZE:
 //            ri_report_size = data8;
-          break;
+            break;
 
           case RI_GLOBAL_REPORT_COUNT:
 //            ri_report_count = data8;
-          break;
+            break;
 
-          case RI_GLOBAL_UNIT_EXPONENT : break;
-          case RI_GLOBAL_UNIT          : break;
-          case RI_GLOBAL_PUSH          : break;
-          case RI_GLOBAL_POP           : break;
+          case RI_GLOBAL_UNIT_EXPONENT: break;
+          case RI_GLOBAL_UNIT: break;
+          case RI_GLOBAL_PUSH: break;
+          case RI_GLOBAL_POP: break;
 
           default: break;
         }
-      break;
+        break;
 
       case RI_TYPE_LOCAL:
-        switch(tag)
-        {
+        switch (tag) {
           case RI_LOCAL_USAGE:
             // only take in account the "usage" before starting REPORT ID
-            if ( ri_collection_depth == 0 ) info->usage = data8;
-          break;
+            if (ri_collection_depth == 0) info->usage = data8;
+            break;
 
-          case RI_LOCAL_USAGE_MIN        : break;
-          case RI_LOCAL_USAGE_MAX        : break;
-          case RI_LOCAL_DESIGNATOR_INDEX : break;
-          case RI_LOCAL_DESIGNATOR_MIN   : break;
-          case RI_LOCAL_DESIGNATOR_MAX   : break;
-          case RI_LOCAL_STRING_INDEX     : break;
-          case RI_LOCAL_STRING_MIN       : break;
-          case RI_LOCAL_STRING_MAX       : break;
-          case RI_LOCAL_DELIMITER        : break;
+          case RI_LOCAL_USAGE_MIN: break;
+          case RI_LOCAL_USAGE_MAX: break;
+          case RI_LOCAL_DESIGNATOR_INDEX: break;
+          case RI_LOCAL_DESIGNATOR_MIN: break;
+          case RI_LOCAL_DESIGNATOR_MAX: break;
+          case RI_LOCAL_STRING_INDEX: break;
+          case RI_LOCAL_STRING_MIN: break;
+          case RI_LOCAL_STRING_MAX: break;
+          case RI_LOCAL_DELIMITER: break;
           default: break;
         }
-      break;
+        break;
 
-      // error
+        // error
       default: break;
     }
 
     desc_report += size;
-    desc_len    -= size;
+    desc_len -= size;
   }
 
-  for ( uint8_t i = 0; i < report_num; i++ )
-  {
-    info = report_info_arr+i;
+  for (uint8_t i = 0; i < report_num; i++) {
+    info = report_info_arr + i;
     TU_LOG_DRV("%u: id = %u, usage_page = %u, usage = %u\r\n", i, info->report_id, info->usage_page, info->usage);
   }
 
diff --git a/src/class/hid/hid_host.h b/src/class/hid/hid_host.h
index 238b7c627..9681c704b 100644
--- a/src/class/hid/hid_host.h
+++ b/src/class/hid/hid_host.h
@@ -30,7 +30,7 @@
 #include "hid.h"
 
 #ifdef __cplusplus
- extern "C" {
+extern "C" {
 #endif
 
 //--------------------------------------------------------------------+
@@ -47,10 +47,9 @@
 #endif
 
 
-typedef struct
-{
-  uint8_t  report_id;
-  uint8_t  usage;
+typedef struct {
+  uint8_t report_id;
+  uint8_t usage;
   uint16_t usage_page;
 
   // TODO still use the endpoint size for now
@@ -86,7 +85,8 @@ bool tuh_hid_mounted(uint8_t dev_addr, uint8_t idx);
 
 // Parse report descriptor into array of report_info struct and return number of reports.
 // For complicated report, application should write its own parser.
-uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* reports_info_arr, uint8_t arr_count, uint8_t const* desc_report, uint16_t desc_len) TU_ATTR_UNUSED;
+TU_ATTR_UNUSED uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* reports_info_arr, uint8_t arr_count,
+                                                       uint8_t const* desc_report, uint16_t desc_len);
 
 //--------------------------------------------------------------------+
 // Control Endpoint API
@@ -105,9 +105,14 @@ void tuh_hid_set_default_protocol(uint8_t protocol);
 // This function is only supported by Boot interface (tuh_n_hid_interface_protocol() != NONE)
 bool tuh_hid_set_protocol(uint8_t dev_addr, uint8_t idx, uint8_t protocol);
 
+// Get Report using control endpoint
+// report_type is either Input, Output or Feature, (value from hid_report_type_t)
+bool tuh_hid_get_report(uint8_t dev_addr, uint8_t idx, uint8_t report_id, uint8_t report_type, void* report, uint16_t len);
+
 // Set Report using control endpoint
 // report_type is either Input, Output or Feature, (value from hid_report_type_t)
-bool tuh_hid_set_report(uint8_t dev_addr, uint8_t idx, uint8_t report_id, uint8_t report_type, void* report, uint16_t len);
+bool tuh_hid_set_report(uint8_t dev_addr, uint8_t idx, uint8_t report_id, uint8_t report_type,
+                        void* report, uint16_t len);
 
 //--------------------------------------------------------------------+
 // Interrupt Endpoint API
@@ -121,6 +126,9 @@ bool tuh_hid_receive_ready(uint8_t dev_addr, uint8_t idx);
 // - false if failed to queue the transfer e.g endpoint is busy
 bool tuh_hid_receive_report(uint8_t dev_addr, uint8_t idx);
 
+// Abort receiving report on Interrupt Endpoint
+bool tuh_hid_receive_abort(uint8_t dev_addr, uint8_t idx);
+
 // Check if HID interface is ready to send report
 bool tuh_hid_send_ready(uint8_t dev_addr, uint8_t idx);
 
@@ -149,6 +157,10 @@ void tuh_hid_report_received_cb(uint8_t dev_addr, uint8_t idx, uint8_t const* re
 // Invoked when sent report to device successfully via interrupt endpoint
 TU_ATTR_WEAK void tuh_hid_report_sent_cb(uint8_t dev_addr, uint8_t idx, uint8_t const* report, uint16_t len);
 
+// Invoked when Get Report to device via either control endpoint
+// len = 0 indicate there is error in the transfer e.g stalled response
+TU_ATTR_WEAK void tuh_hid_get_report_complete_cb(uint8_t dev_addr, uint8_t idx, uint8_t report_id, uint8_t report_type, uint16_t len);
+
 // Invoked when Sent Report to device via either control endpoint
 // len = 0 indicate there is error in the transfer e.g stalled response
 TU_ATTR_WEAK void tuh_hid_set_report_complete_cb(uint8_t dev_addr, uint8_t idx, uint8_t report_id, uint8_t report_type, uint16_t len);
@@ -159,11 +171,12 @@ TU_ATTR_WEAK void tuh_hid_set_protocol_complete_cb(uint8_t dev_addr, uint8_t idx
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-void hidh_init       (void);
-bool hidh_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len);
-bool hidh_set_config (uint8_t dev_addr, uint8_t itf_num);
-bool hidh_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
-void hidh_close      (uint8_t dev_addr);
+bool hidh_init(void);
+bool hidh_deinit(void);
+bool hidh_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const* desc_itf, uint16_t max_len);
+bool hidh_set_config(uint8_t dev_addr, uint8_t itf_num);
+bool hidh_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
+void hidh_close(uint8_t dev_addr);
 
 #ifdef __cplusplus
 }
diff --git a/src/class/midi/midi_device.c b/src/class/midi/midi_device.c
index 052372a93..42905ab0d 100644
--- a/src/class/midi/midi_device.c
+++ b/src/class/midi/midi_device.c
@@ -373,12 +373,10 @@ bool tud_midi_n_packet_write (uint8_t itf, uint8_t const packet[4])
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void midid_init(void)
-{
+void midid_init(void) {
   tu_memclr(_midid_itf, sizeof(_midid_itf));
 
-  for(uint8_t i=0; i<CFG_TUD_MIDI; i++)
-  {
+  for (uint8_t i = 0; i < CFG_TUD_MIDI; i++) {
     midid_interface_t* midi = &_midid_itf[i];
 
     // config fifo
@@ -386,12 +384,38 @@ void midid_init(void)
     tu_fifo_config(&midi->tx_ff, midi->tx_ff_buf, CFG_TUD_MIDI_TX_BUFSIZE, 1, false); // OBVS.
 
     #if CFG_FIFO_MUTEX
-    tu_fifo_config_mutex(&midi->rx_ff, NULL, osal_mutex_create(&midi->rx_ff_mutex));
-    tu_fifo_config_mutex(&midi->tx_ff, osal_mutex_create(&midi->tx_ff_mutex), NULL);
+    osal_mutex_t mutex_rd = osal_mutex_create(&midi->rx_ff_mutex);
+    osal_mutex_t mutex_wr = osal_mutex_create(&midi->tx_ff_mutex);
+    TU_ASSERT(mutex_wr != NULL && mutex_wr != NULL, );
+
+    tu_fifo_config_mutex(&midi->rx_ff, NULL, mutex_rd);
+    tu_fifo_config_mutex(&midi->tx_ff, mutex_wr, NULL);
     #endif
   }
 }
 
+bool midid_deinit(void) {
+  #if CFG_FIFO_MUTEX
+  for(uint8_t i=0; i<CFG_TUD_MIDI; i++) {
+    midid_interface_t* midi = &_midid_itf[i];
+    osal_mutex_t mutex_rd = midi->rx_ff.mutex_rd;
+    osal_mutex_t mutex_wr = midi->tx_ff.mutex_wr;
+
+    if (mutex_rd) {
+      osal_mutex_delete(mutex_rd);
+      tu_fifo_config_mutex(&midi->rx_ff, NULL, NULL);
+    }
+
+    if (mutex_wr) {
+      osal_mutex_delete(mutex_wr);
+      tu_fifo_config_mutex(&midi->tx_ff, NULL, NULL);
+    }
+  }
+  #endif
+
+  return true;
+}
+
 void midid_reset(uint8_t rhport)
 {
   (void) rhport;
diff --git a/src/class/midi/midi_device.h b/src/class/midi/midi_device.h
index 1c6f996be..3e89cc0a3 100644
--- a/src/class/midi/midi_device.h
+++ b/src/class/midi/midi_device.h
@@ -158,6 +158,7 @@ static inline bool tud_midi_packet_write (uint8_t const packet[4])
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     midid_init            (void);
+bool     midid_deinit          (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);
diff --git a/src/class/msc/msc_device.c b/src/class/msc/msc_device.c
index 2589dcd2c..447560b4d 100644
--- a/src/class/msc/msc_device.c
+++ b/src/class/msc/msc_device.c
@@ -203,7 +203,7 @@ uint8_t rdwr10_validate_cmd(msc_cbw_t const* cbw)
 //--------------------------------------------------------------------+
 // Debug
 //--------------------------------------------------------------------+
-#if CFG_TUSB_DEBUG >= 2
+#if CFG_TUSB_DEBUG >= CFG_TUD_MSC_LOG_LEVEL
 
 TU_ATTR_UNUSED tu_static tu_lookup_entry_t const _msc_scsi_cmd_lookup[] =
 {
@@ -251,11 +251,15 @@ static inline void set_sense_medium_not_present(uint8_t lun)
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void mscd_init(void)
-{
+void mscd_init(void) {
   tu_memclr(&_mscd_itf, sizeof(mscd_interface_t));
 }
 
+bool mscd_deinit(void) {
+  // nothing to do
+  return true;
+}
+
 void mscd_reset(uint8_t rhport)
 {
   (void) rhport;
@@ -685,6 +689,24 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
       }
     break;
 
+    case SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL:
+      resplen = 0;
+
+      if (tud_msc_prevent_allow_medium_removal_cb)
+      {
+         scsi_prevent_allow_medium_removal_t const * prevent_allow = (scsi_prevent_allow_medium_removal_t const *) scsi_cmd;
+        if ( !tud_msc_prevent_allow_medium_removal_cb(lun, prevent_allow->prohibit_removal, prevent_allow->control) )
+        {
+          // Failed status response
+          resplen = - 1;
+
+          // set default sense if not set by callback
+          if ( p_msc->sense_key == 0 ) set_sense_medium_not_present(lun);
+        }
+      }
+    break;
+
+
     case SCSI_CMD_READ_CAPACITY_10:
     {
       uint32_t block_count;
diff --git a/src/class/msc/msc_device.h b/src/class/msc/msc_device.h
index 72f95be06..29acd280a 100644
--- a/src/class/msc/msc_device.h
+++ b/src/class/msc/msc_device.h
@@ -131,6 +131,9 @@ TU_ATTR_WEAK uint8_t tud_msc_get_maxlun_cb(void);
 // - Start = 1 : active mode, if load_eject = 1 : load disk storage
 TU_ATTR_WEAK bool tud_msc_start_stop_cb(uint8_t lun, uint8_t power_condition, bool start, bool load_eject);
 
+//Invoked when we receive the Prevent / Allow Medium Removal command
+TU_ATTR_WEAK bool tud_msc_prevent_allow_medium_removal_cb(uint8_t lun, uint8_t prohibit_removal, uint8_t control);
+
 // Invoked when received REQUEST_SENSE
 TU_ATTR_WEAK int32_t tud_msc_request_sense_cb(uint8_t lun, void* buffer, uint16_t bufsize);
 
@@ -150,6 +153,7 @@ TU_ATTR_WEAK bool tud_msc_is_writable_cb(uint8_t lun);
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     mscd_init            (void);
+bool     mscd_deinit          (void);
 void     mscd_reset           (uint8_t rhport);
 uint16_t mscd_open            (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     mscd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * p_request);
diff --git a/src/class/msc/msc_host.c b/src/class/msc/msc_host.c
index 39f2d9f1c..ce6e7fb2d 100644
--- a/src/class/msc/msc_host.c
+++ b/src/class/msc/msc_host.c
@@ -284,8 +284,14 @@ bool tuh_msc_reset(uint8_t dev_addr) {
 //--------------------------------------------------------------------+
 // CLASS-USBH API
 //--------------------------------------------------------------------+
-void msch_init(void) {
+bool msch_init(void) {
+  TU_LOG_DRV("sizeof(msch_interface_t) = %u\r\n", sizeof(msch_interface_t));
   tu_memclr(_msch_itf, sizeof(_msch_itf));
+  return true;
+}
+
+bool msch_deinit(void) {
+  return true;
 }
 
 void msch_close(uint8_t dev_addr) {
diff --git a/src/class/msc/msc_host.h b/src/class/msc/msc_host.h
index 9ca1b4703..9fda566d8 100644
--- a/src/class/msc/msc_host.h
+++ b/src/class/msc/msc_host.h
@@ -113,7 +113,8 @@ TU_ATTR_WEAK void tuh_msc_umount_cb(uint8_t dev_addr);
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 
-void msch_init       (void);
+bool msch_init       (void);
+bool msch_deinit     (void);
 bool msch_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len);
 bool msch_set_config (uint8_t dev_addr, uint8_t itf_num);
 void msch_close      (uint8_t dev_addr);
diff --git a/src/class/net/ecm_rndis_device.c b/src/class/net/ecm_rndis_device.c
index 762425732..f7a5fd225 100644
--- a/src/class/net/ecm_rndis_device.c
+++ b/src/class/net/ecm_rndis_device.c
@@ -132,11 +132,14 @@ void netd_report(uint8_t *buf, uint16_t len)
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void netd_init(void)
-{
+void netd_init(void) {
   tu_memclr(&_netd_itf, sizeof(_netd_itf));
 }
 
+bool netd_deinit(void) {
+  return true;
+}
+
 void netd_reset(uint8_t rhport)
 {
   (void) rhport;
diff --git a/src/class/net/ncm.h b/src/class/net/ncm.h
index 96ba11fbc..1b987fca0 100644
--- a/src/class/net/ncm.h
+++ b/src/class/net/ncm.h
@@ -2,6 +2,7 @@
  * The MIT License (MIT)
  *
  * Copyright (c) 2021, Ha Thach (tinyusb.org)
+ * Copyright (c) 2024, Hardy Griech
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -24,22 +25,58 @@
  * This file is part of the TinyUSB stack.
  */
 
-
 #ifndef _TUSB_NCM_H_
 #define _TUSB_NCM_H_
 
 #include "common/tusb_common.h"
 
-#ifdef __cplusplus
- extern "C" {
+// NTB buffers size for reception side, must be >> MTU to avoid TCP retransmission (driver issue ?)
+// Linux use 2048 as minimal size
+#ifndef CFG_TUD_NCM_OUT_NTB_MAX_SIZE
+  #define CFG_TUD_NCM_OUT_NTB_MAX_SIZE 3200
 #endif
 
-// Table 4.3 Data Class Interface Protocol Codes
-typedef enum
-{
-  NCM_DATA_PROTOCOL_NETWORK_TRANSFER_BLOCK = 0x01
-} ncm_data_interface_protocol_code_t;
+// NTB buffers size for reception side, must be > MTU
+// Linux use 2048 as minimal size
+#ifndef CFG_TUD_NCM_IN_NTB_MAX_SIZE
+  #define CFG_TUD_NCM_IN_NTB_MAX_SIZE 3200
+#endif
+
+// Number of NTB buffers for reception side
+// Depending on the configuration, this parameter could be increased with the cost of additional RAM requirements
+// On Full-Speed (RP2040) :
+//    1  - good performance
+//    2  - up to 30% more performance with iperf with small packets
+//    >2 - no performance gain
+// On High-Speed (STM32F7) :
+//    No performance gain
+#ifndef CFG_TUD_NCM_OUT_NTB_N
+  #define CFG_TUD_NCM_OUT_NTB_N 1
+#endif
 
+// Number of NTB buffers for transmission side
+// Depending on the configuration, this parameter could be increased with the cost of additional RAM requirements
+// On Full-Speed (RP2040) :
+//    1 - good performance but SystemView shows lost events (on load test)
+//    2 - up to 50% more performance with iperf with small packets, "tud_network_can_xmit: request blocked"
+//        happens from time to time with SystemView
+//    3 - "tud_network_can_xmit: request blocked" never happens
+//    >3 - no performance gain
+// On High-Speed (STM32F7) :
+//    No performance gain
+#ifndef CFG_TUD_NCM_IN_NTB_N
+  #define CFG_TUD_NCM_IN_NTB_N 1
+#endif
+
+// How many datagrams it is allowed to put into an NTB for transmission side
+#ifndef CFG_TUD_NCM_IN_MAX_DATAGRAMS_PER_NTB
+  #define CFG_TUD_NCM_IN_MAX_DATAGRAMS_PER_NTB 8
+#endif
+
+// This tells the host how many datagrams it is allowed to put into an NTB
+#ifndef CFG_TUD_NCM_OUT_MAX_DATAGRAMS_PER_NTB
+  #define CFG_TUD_NCM_OUT_MAX_DATAGRAMS_PER_NTB 6
+#endif
 
 // Table 6.2 Class-Specific Request Codes for Network Control Model subclass
 typedef enum
@@ -62,8 +99,65 @@ typedef enum
   NCM_SET_CRC_MODE                                 = 0x8A,
 } ncm_request_code_t;
 
-#ifdef __cplusplus
- }
-#endif
+#define NTH16_SIGNATURE 0x484D434E
+#define NDP16_SIGNATURE_NCM0 0x304D434E
+#define NDP16_SIGNATURE_NCM1 0x314D434E
+
+typedef struct TU_ATTR_PACKED {
+  uint16_t wLength;
+  uint16_t bmNtbFormatsSupported;
+  uint32_t dwNtbInMaxSize;
+  uint16_t wNdbInDivisor;
+  uint16_t wNdbInPayloadRemainder;
+  uint16_t wNdbInAlignment;
+  uint16_t wReserved;
+  uint32_t dwNtbOutMaxSize;
+  uint16_t wNdbOutDivisor;
+  uint16_t wNdbOutPayloadRemainder;
+  uint16_t wNdbOutAlignment;
+  uint16_t wNtbOutMaxDatagrams;
+} ntb_parameters_t;
+
+typedef struct TU_ATTR_PACKED {
+  uint32_t dwSignature;
+  uint16_t wHeaderLength;
+  uint16_t wSequence;
+  uint16_t wBlockLength;
+  uint16_t wNdpIndex;
+} nth16_t;
+
+typedef struct TU_ATTR_PACKED {
+  uint16_t wDatagramIndex;
+  uint16_t wDatagramLength;
+} ndp16_datagram_t;
+
+typedef struct TU_ATTR_PACKED {
+  uint32_t dwSignature;
+  uint16_t wLength;
+  uint16_t wNextNdpIndex;
+  //ndp16_datagram_t datagram[];
+} ndp16_t;
+
+typedef union TU_ATTR_PACKED {
+  struct {
+    nth16_t nth;
+    ndp16_t ndp;
+    ndp16_datagram_t ndp_datagram[CFG_TUD_NCM_IN_MAX_DATAGRAMS_PER_NTB + 1];
+  };
+  uint8_t data[CFG_TUD_NCM_IN_NTB_MAX_SIZE];
+} xmit_ntb_t;
+
+typedef union TU_ATTR_PACKED {
+  struct {
+    nth16_t nth;
+    // only the header is at a guaranteed position
+  };
+  uint8_t data[CFG_TUD_NCM_OUT_NTB_MAX_SIZE];
+} recv_ntb_t;
+
+struct ncm_notify_t {
+  tusb_control_request_t header;
+  uint32_t downlink, uplink;
+};
 
 #endif
diff --git a/src/class/net/ncm_device.c b/src/class/net/ncm_device.c
index 226c42c4e..64aba011a 100644
--- a/src/class/net/ncm_device.c
+++ b/src/class/net/ncm_device.c
@@ -1,9 +1,8 @@
 /*
  * The MIT License (MIT)
  *
- * Copyright (c) 2020 Jacob Berg Potter
- * Copyright (c) 2020 Peter Lawrence
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
+ * Copyright (c) 2024 Hardy Griech
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -26,12 +25,37 @@
  * This file is part of the TinyUSB stack.
  */
 
+/**
+ * Small Glossary (from the spec)
+ * --------------
+ * Datagram - A collection of bytes forming a single item of information, passed as a unit from source to destination.
+ * NCM      - Network Control Model
+ * NDP      - NCM Datagram Pointer: NTB structure that delineates Datagrams (typically Ethernet frames) within an NTB
+ * NTB      - NCM Transfer Block: a data structure for efficient USB encapsulation of one or more datagrams
+ *            Each NTB is designed to be a single USB transfer
+ * NTH      - NTB Header: a data structure at the front of each NTB, which provides the information needed to validate
+ *            the NTB and begin decoding
+ *
+ * Some explanations
+ * -----------------
+ * - rhport        is the USB port of the device, in most cases "0"
+ * - itf_data_alt  if != 0 -> data xmit/recv are allowed (see spec)
+ * - ep_in         IN endpoints take data from the device intended to go in to the host (the device transmits)
+ * - ep_out        OUT endpoints send data out of the host to the device (the device receives)
+ */
+
 #include "tusb_option.h"
 
-#if ( CFG_TUD_ENABLED && CFG_TUD_NCM )
+#if (CFG_TUD_ENABLED && CFG_TUD_NCM)
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
 
 #include "device/usbd.h"
 #include "device/usbd_pvt.h"
+
+#include "ncm.h"
 #include "net_device.h"
 
 // Level where CFG_TUSB_DEBUG must be at least for this driver is logged
@@ -41,478 +65,829 @@
 
 #define TU_LOG_DRV(...)   TU_LOG(CFG_TUD_NCM_LOG_LEVEL, __VA_ARGS__)
 
-//--------------------------------------------------------------------+
-// MACRO CONSTANT TYPEDEF
-//--------------------------------------------------------------------+
+// Alignment must be 4
+#define TUD_NCM_ALIGNMENT   4
+// calculate alignment of xmit datagrams within an NTB
+#define XMIT_ALIGN_OFFSET(x) ((TUD_NCM_ALIGNMENT - ((x) & (TUD_NCM_ALIGNMENT - 1))) & (TUD_NCM_ALIGNMENT - 1))
 
-#define NTH16_SIGNATURE      0x484D434E
-#define NDP16_SIGNATURE_NCM0 0x304D434E
-#define NDP16_SIGNATURE_NCM1 0x314D434E
+//-----------------------------------------------------------------------------
+//
+// Module global things
+//
+#define XMIT_NTB_N CFG_TUD_NCM_IN_NTB_N
+#define RECV_NTB_N CFG_TUD_NCM_OUT_NTB_N
 
-typedef struct TU_ATTR_PACKED
-{
-  uint16_t wLength;
-  uint16_t bmNtbFormatsSupported;
-  uint32_t dwNtbInMaxSize;
-  uint16_t wNdbInDivisor;
-  uint16_t wNdbInPayloadRemainder;
-  uint16_t wNdbInAlignment;
-  uint16_t wReserved;
-  uint32_t dwNtbOutMaxSize;
-  uint16_t wNdbOutDivisor;
-  uint16_t wNdbOutPayloadRemainder;
-  uint16_t wNdbOutAlignment;
-  uint16_t wNtbOutMaxDatagrams;
-} ntb_parameters_t;
+typedef struct {
+  // general
+  uint8_t ep_in;        // endpoint for outgoing datagrams (naming is a little bit confusing)
+  uint8_t ep_out;       // endpoint for incoming datagrams (naming is a little bit confusing)
+  uint8_t ep_notif;     // endpoint for notifications
+  uint8_t itf_num;      // interface number
+  uint8_t itf_data_alt; // ==0 -> no endpoints, i.e. no network traffic, ==1 -> normal operation with two endpoints (spec, chapter 5.3)
+  uint8_t rhport;       // storage of \a rhport because some callbacks are done without it
 
-typedef struct TU_ATTR_PACKED
-{
-  uint32_t dwSignature;
-  uint16_t wHeaderLength;
-  uint16_t wSequence;
-  uint16_t wBlockLength;
-  uint16_t wNdpIndex;
-} nth16_t;
+  // recv handling
+  CFG_TUSB_MEM_ALIGN recv_ntb_t recv_ntb[RECV_NTB_N];   // actual recv NTBs
+  recv_ntb_t *recv_free_ntb[RECV_NTB_N];                // free list of recv NTBs
+  recv_ntb_t *recv_ready_ntb[RECV_NTB_N];               // NTBs waiting for transmission to glue logic
+  recv_ntb_t *recv_tinyusb_ntb;                         // buffer for the running transfer TinyUSB -> driver
+  recv_ntb_t *recv_glue_ntb;                            // buffer for the running transfer driver -> glue logic
+  uint16_t recv_glue_ntb_datagram_ndx;                  // index into \a recv_glue_ntb_datagram
 
-typedef struct TU_ATTR_PACKED
-{
-  uint16_t wDatagramIndex;
-  uint16_t wDatagramLength;
-} ndp16_datagram_t;
+  // xmit handling
+  CFG_TUSB_MEM_ALIGN xmit_ntb_t xmit_ntb[XMIT_NTB_N];   // actual xmit NTBs
+  xmit_ntb_t *xmit_free_ntb[XMIT_NTB_N];                // free list of xmit NTBs
+  xmit_ntb_t *xmit_ready_ntb[XMIT_NTB_N];               // NTBs waiting for transmission to TinyUSB
+  xmit_ntb_t *xmit_tinyusb_ntb;                         // buffer for the running transfer driver -> TinyUSB
+  xmit_ntb_t *xmit_glue_ntb;                            // buffer for the running transfer glue logic -> driver
+  uint16_t xmit_sequence;                               // NTB sequence counter
+  uint16_t xmit_glue_ntb_datagram_ndx;                  // index into \a xmit_glue_ntb_datagram
 
-typedef struct TU_ATTR_PACKED
-{
-  uint32_t dwSignature;
-  uint16_t wLength;
-  uint16_t wNextNdpIndex;
-  ndp16_datagram_t datagram[];
-} ndp16_t;
+  // notification handling
+  enum {
+    NOTIFICATION_SPEED,
+    NOTIFICATION_CONNECTED,
+    NOTIFICATION_DONE
+  } notification_xmit_state;            // state of notification transmission
+  bool notification_xmit_is_running;    // notification is currently transmitted
+} ncm_interface_t;
 
-typedef union TU_ATTR_PACKED {
-  struct {
-    nth16_t nth;
-    ndp16_t ndp;
-  };
-  uint8_t data[CFG_TUD_NCM_IN_NTB_MAX_SIZE];
-} transmit_ntb_t;
+CFG_TUD_MEM_SECTION CFG_TUD_MEM_ALIGN tu_static ncm_interface_t ncm_interface;
 
-struct ecm_notify_struct
-{
-  tusb_control_request_t header;
-  uint32_t downlink, uplink;
+/**
+ * This is the NTB parameter structure
+ *
+ * \attention
+ *     We are lucky, that byte order is correct
+ */
+CFG_TUD_MEM_SECTION CFG_TUD_MEM_ALIGN tu_static const ntb_parameters_t ntb_parameters = {
+  .wLength                  = sizeof(ntb_parameters_t),
+  .bmNtbFormatsSupported    = 0x01,// 16-bit NTB supported
+  .dwNtbInMaxSize           = CFG_TUD_NCM_IN_NTB_MAX_SIZE,
+  .wNdbInDivisor            = 1,
+  .wNdbInPayloadRemainder   = 0,
+  .wNdbInAlignment          = TUD_NCM_ALIGNMENT,
+  .wReserved                = 0,
+  .dwNtbOutMaxSize          = CFG_TUD_NCM_OUT_NTB_MAX_SIZE,
+  .wNdbOutDivisor           = 1,
+  .wNdbOutPayloadRemainder  = 0,
+  .wNdbOutAlignment         = TUD_NCM_ALIGNMENT,
+  .wNtbOutMaxDatagrams      = CFG_TUD_NCM_OUT_MAX_DATAGRAMS_PER_NTB,
 };
 
-typedef struct
-{
-  uint8_t itf_num;      // Index number of Management Interface, +1 for Data Interface
-  uint8_t itf_data_alt; // Alternate setting of Data Interface. 0 : inactive, 1 : active
+// Some confusing remarks about wNtbOutMaxDatagrams...
+//      ==1 -> SystemView packets/s goes up to 2000 and events are lost during startup
+//      ==0 -> SystemView runs fine, iperf shows in wireshark a lot of error
+//      ==6 -> SystemView runs fine, iperf also
+//      >6  -> iperf starts to show errors
+//      -> 6 seems to be the best value.  Why?  Don't know, perhaps only on my system?
+//
+//      iperf:    for MSS in 100 200 400 800 1200 1450 1500; do iperf -c 192.168.14.1 -e -i 1 -M $MSS -l 8192 -P 1; sleep 2; done
+//      sysview:  SYSTICKS_PER_SEC=35000, IDLE_US=1000, PRINT_MOD=1000
+//
 
-  uint8_t ep_notif;
-  uint8_t ep_in;
-  uint8_t ep_out;
+//-----------------------------------------------------------------------------
+//
+// everything about notifications
+//
+tu_static struct ncm_notify_t ncm_notify_connected = {
+    .header = {
+        .bmRequestType_bit = {
+            .recipient = TUSB_REQ_RCPT_INTERFACE,
+            .type = TUSB_REQ_TYPE_CLASS,
+            .direction = TUSB_DIR_IN},
+        .bRequest = CDC_NOTIF_NETWORK_CONNECTION,
+        .wValue = 1 /* Connected */,
+        .wLength = 0,
+    },
+};
 
-  const ndp16_t *ndp;
-  uint8_t num_datagrams, current_datagram_index;
+tu_static struct ncm_notify_t ncm_notify_speed_change = {
+    .header = {
+        .bmRequestType_bit = {
+            .recipient = TUSB_REQ_RCPT_INTERFACE,
+            .type = TUSB_REQ_TYPE_CLASS,
+            .direction = TUSB_DIR_IN},
+        .bRequest = CDC_NOTIF_CONNECTION_SPEED_CHANGE,
+        .wLength = 8,
+    },
+    .downlink = TUD_OPT_HIGH_SPEED ? 480000000 : 12000000,
+    .uplink = TUD_OPT_HIGH_SPEED ? 480000000 : 12000000,
+};
 
-  enum {
-    REPORT_SPEED,
-    REPORT_CONNECTED,
-    REPORT_DONE
-  } report_state;
-  bool report_pending;
+/**
+ * Transmit next notification to the host (if appropriate).
+ * Notifications are transferred to the host once during connection setup.
+ */
+static void notification_xmit(uint8_t rhport, bool force_next) {
+  TU_LOG_DRV("notification_xmit(%d, %d) - %d %d\n", force_next, rhport, ncm_interface.notification_xmit_state, ncm_interface.notification_xmit_is_running);
 
-  uint8_t  current_ntb;           // Index in transmit_ntb[] that is currently being filled with datagrams
-  uint8_t  datagram_count;        // Number of datagrams in transmit_ntb[current_ntb]
-  uint16_t next_datagram_offset;  // Offset in transmit_ntb[current_ntb].data to place the next datagram
-  uint16_t ntb_in_size;           // Maximum size of transmitted (IN to host) NTBs; initially CFG_TUD_NCM_IN_NTB_MAX_SIZE
-  uint8_t  max_datagrams_per_ntb; // Maximum number of datagrams per NTB; initially CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB
+  if (!force_next && ncm_interface.notification_xmit_is_running) {
+    return;
+  }
 
-  uint16_t nth_sequence;          // Sequence number counter for transmitted NTBs
+  if (ncm_interface.notification_xmit_state == NOTIFICATION_SPEED) {
+    TU_LOG_DRV("  NOTIFICATION_SPEED\n");
+    ncm_notify_speed_change.header.wIndex = ncm_interface.itf_num;
+    usbd_edpt_xfer(rhport, ncm_interface.ep_notif, (uint8_t *) &ncm_notify_speed_change, sizeof(ncm_notify_speed_change));
+    ncm_interface.notification_xmit_state = NOTIFICATION_CONNECTED;
+    ncm_interface.notification_xmit_is_running = true;
+  } else if (ncm_interface.notification_xmit_state == NOTIFICATION_CONNECTED) {
+    TU_LOG_DRV("  NOTIFICATION_CONNECTED\n");
+    ncm_notify_connected.header.wIndex = ncm_interface.itf_num;
+    usbd_edpt_xfer(rhport, ncm_interface.ep_notif, (uint8_t *) &ncm_notify_connected, sizeof(ncm_notify_connected));
+    ncm_interface.notification_xmit_state = NOTIFICATION_DONE;
+    ncm_interface.notification_xmit_is_running = true;
+  } else {
+    TU_LOG_DRV("  NOTIFICATION_FINISHED\n");
+  }
+} // notification_xmit
 
-  bool transferring;
+//-----------------------------------------------------------------------------
+//
+// everything about packet transmission (driver -> TinyUSB)
+//
 
-} ncm_interface_t;
+/**
+ * Put NTB into the transmitter free list.
+ */
+static void xmit_put_ntb_into_free_list(xmit_ntb_t *free_ntb) {
+  TU_LOG_DRV("xmit_put_ntb_into_free_list() - %p\n", ncm_interface.xmit_tinyusb_ntb);
 
-//--------------------------------------------------------------------+
-// INTERNAL OBJECT & FUNCTION DECLARATION
-//--------------------------------------------------------------------+
+  if (free_ntb == NULL) { // can happen due to ZLPs
+    return;
+  }
 
-CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN tu_static const ntb_parameters_t ntb_parameters = {
-    .wLength                 = sizeof(ntb_parameters_t),
-    .bmNtbFormatsSupported   = 0x01,
-    .dwNtbInMaxSize          = CFG_TUD_NCM_IN_NTB_MAX_SIZE,
-    .wNdbInDivisor           = 4,
-    .wNdbInPayloadRemainder  = 0,
-    .wNdbInAlignment         = CFG_TUD_NCM_ALIGNMENT,
-    .wReserved               = 0,
-    .dwNtbOutMaxSize         = CFG_TUD_NCM_OUT_NTB_MAX_SIZE,
-    .wNdbOutDivisor          = 4,
-    .wNdbOutPayloadRemainder = 0,
-    .wNdbOutAlignment        = CFG_TUD_NCM_ALIGNMENT,
-    .wNtbOutMaxDatagrams     = 0
-};
+  for (int i = 0; i < XMIT_NTB_N; ++i) {
+    if (ncm_interface.xmit_free_ntb[i] == NULL) {
+      ncm_interface.xmit_free_ntb[i] = free_ntb;
+      return;
+    }
+  }
+  TU_LOG_DRV("(EE) xmit_put_ntb_into_free_list - no entry in free list\n");// this should not happen
+} // xmit_put_ntb_into_free_list
 
-CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN tu_static transmit_ntb_t transmit_ntb[2];
+/**
+ * Get an NTB from the free list
+ */
+static xmit_ntb_t *xmit_get_free_ntb(void) {
+  TU_LOG_DRV("xmit_get_free_ntb()\n");
 
-CFG_TUD_MEM_SECTION CFG_TUSB_MEM_ALIGN tu_static uint8_t receive_ntb[CFG_TUD_NCM_OUT_NTB_MAX_SIZE];
+  for (int i = 0; i < XMIT_NTB_N; ++i) {
+    if (ncm_interface.xmit_free_ntb[i] != NULL) {
+      xmit_ntb_t *free = ncm_interface.xmit_free_ntb[i];
+      ncm_interface.xmit_free_ntb[i] = NULL;
+      return free;
+    }
+  }
+  return NULL;
+} // xmit_get_free_ntb
 
-tu_static ncm_interface_t ncm_interface;
+/**
+ * Put a filled NTB into the ready list
+ */
+static void xmit_put_ntb_into_ready_list(xmit_ntb_t *ready_ntb) {
+  TU_LOG_DRV("xmit_put_ntb_into_ready_list(%p) %d\n", ready_ntb, ready_ntb->nth.wBlockLength);
 
-/*
- * Set up the NTB state in ncm_interface to be ready to add datagrams.
+  for (int i = 0; i < XMIT_NTB_N; ++i) {
+    if (ncm_interface.xmit_ready_ntb[i] == NULL) {
+      ncm_interface.xmit_ready_ntb[i] = ready_ntb;
+      return;
+    }
+  }
+  TU_LOG_DRV("(EE) xmit_put_ntb_into_ready_list: ready list full\n");// this should not happen
+} // xmit_put_ntb_into_ready_list
+
+/**
+ * Get the next NTB from the ready list (and remove it from the list).
+ * If the ready list is empty, return NULL.
  */
-static void ncm_prepare_for_tx(void) {
-  ncm_interface.datagram_count = 0;
-  // datagrams start after all the headers
-  ncm_interface.next_datagram_offset = sizeof(nth16_t) + sizeof(ndp16_t)
-      + ((CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB + 1) * sizeof(ndp16_datagram_t));
-}
+static xmit_ntb_t *xmit_get_next_ready_ntb(void) {
+  xmit_ntb_t *r = NULL;
 
-/*
- * If not already transmitting, start sending the current NTB to the host and swap buffers
- * to start filling the other one with datagrams.
+  r = ncm_interface.xmit_ready_ntb[0];
+  memmove(ncm_interface.xmit_ready_ntb + 0, ncm_interface.xmit_ready_ntb + 1, sizeof(ncm_interface.xmit_ready_ntb) - sizeof(ncm_interface.xmit_ready_ntb[0]));
+  ncm_interface.xmit_ready_ntb[XMIT_NTB_N - 1] = NULL;
+
+  TU_LOG_DRV("recv_get_next_ready_ntb: %p\n", r);
+  return r;
+} // xmit_get_next_ready_ntb
+
+/**
+ * Transmit a ZLP if required
+ *
+ * \note
+ *    Insertion of the ZLPs is a little bit different then described in the spec.
+ *    But the below implementation actually works.  Don't know if this is a spec
+ *    or TinyUSB issue.
+ *
+ * \pre
+ *    This must be called from netd_xfer_cb() so that ep_in is ready
  */
-static void ncm_start_tx(void) {
-  if (ncm_interface.transferring) {
+static bool xmit_insert_required_zlp(uint8_t rhport, uint32_t xferred_bytes) {
+  TU_LOG_DRV("xmit_insert_required_zlp(%d,%ld)\n", rhport, xferred_bytes);
+
+  if (xferred_bytes == 0 || xferred_bytes % CFG_TUD_NET_ENDPOINT_SIZE != 0) {
+    return false;
+  }
+
+  TU_ASSERT(ncm_interface.itf_data_alt == 1, false);
+  TU_ASSERT(!usbd_edpt_busy(rhport, ncm_interface.ep_in), false);
+
+  TU_LOG_DRV("xmit_insert_required_zlp! (%u)\n", (unsigned) xferred_bytes);
+
+  // start transmission of the ZLP
+  usbd_edpt_xfer(rhport, ncm_interface.ep_in, NULL, 0);
+
+  return true;
+} // xmit_insert_required_zlp
+
+/**
+ * Start transmission if it there is a waiting packet and if can be done from interface side.
+ */
+static void xmit_start_if_possible(uint8_t rhport) {
+  TU_LOG_DRV("xmit_start_if_possible()\n");
+
+  if (ncm_interface.xmit_tinyusb_ntb != NULL) {
+    TU_LOG_DRV("  !xmit_start_if_possible 1\n");
+    return;
+  }
+  if (ncm_interface.itf_data_alt != 1) {
+    TU_LOG_DRV("(EE) !xmit_start_if_possible 2\n");
+    return;
+  }
+  if (usbd_edpt_busy(rhport, ncm_interface.ep_in)) {
+    TU_LOG_DRV("  !xmit_start_if_possible 3\n");
     return;
   }
 
-  transmit_ntb_t *ntb = &transmit_ntb[ncm_interface.current_ntb];
-  size_t ntb_length = ncm_interface.next_datagram_offset;
+  ncm_interface.xmit_tinyusb_ntb = xmit_get_next_ready_ntb();
+  if (ncm_interface.xmit_tinyusb_ntb == NULL) {
+    if (ncm_interface.xmit_glue_ntb == NULL || ncm_interface.xmit_glue_ntb_datagram_ndx == 0) {
+      // -> really nothing is waiting
+      return;
+    }
+    ncm_interface.xmit_tinyusb_ntb = ncm_interface.xmit_glue_ntb;
+    ncm_interface.xmit_glue_ntb = NULL;
+  }
+
+  #if CFG_TUD_NCM_LOG_LEVEL >= 3
+  {
+    uint16_t len = ncm_interface.xmit_tinyusb_ntb->nth.wBlockLength;
+    TU_LOG_BUF(3, ncm_interface.xmit_tinyusb_ntb->data[i], len);
+  }
+  #endif
+
+  if (ncm_interface.xmit_glue_ntb_datagram_ndx != 1) {
+    TU_LOG_DRV(">> %d %d\n", ncm_interface.xmit_tinyusb_ntb->nth.wBlockLength, ncm_interface.xmit_glue_ntb_datagram_ndx);
+  }
+
+  // Kick off an endpoint transfer
+  usbd_edpt_xfer(0, ncm_interface.ep_in, ncm_interface.xmit_tinyusb_ntb->data, ncm_interface.xmit_tinyusb_ntb->nth.wBlockLength);
+} // xmit_start_if_possible
+
+/**
+ * check if a new datagram fits into the current NTB
+ */
+static bool xmit_requested_datagram_fits_into_current_ntb(uint16_t datagram_size) {
+  TU_LOG_DRV("xmit_requested_datagram_fits_into_current_ntb(%d) - %p %p\n", datagram_size, ncm_interface.xmit_tinyusb_ntb, ncm_interface.xmit_glue_ntb);
+
+  if (ncm_interface.xmit_glue_ntb == NULL) {
+    return false;
+  }
+  if (ncm_interface.xmit_glue_ntb_datagram_ndx >= CFG_TUD_NCM_IN_MAX_DATAGRAMS_PER_NTB) {
+    return false;
+  }
+  if (ncm_interface.xmit_glue_ntb->nth.wBlockLength + datagram_size + XMIT_ALIGN_OFFSET(datagram_size) > CFG_TUD_NCM_OUT_NTB_MAX_SIZE) {
+    return false;
+  }
+  return true;
+} // xmit_requested_datagram_fits_into_current_ntb
+
+/**
+ * Setup an NTB for the glue logic
+ */
+static bool xmit_setup_next_glue_ntb(void) {
+  TU_LOG_DRV("xmit_setup_next_glue_ntb - %p\n", ncm_interface.xmit_glue_ntb);
+
+  if (ncm_interface.xmit_glue_ntb != NULL) {
+    // put NTB into waiting list (the new datagram did not fit in)
+    xmit_put_ntb_into_ready_list(ncm_interface.xmit_glue_ntb);
+  }
+
+  ncm_interface.xmit_glue_ntb = xmit_get_free_ntb();// get next buffer (if any)
+  if (ncm_interface.xmit_glue_ntb == NULL) {
+    TU_LOG_DRV("  xmit_setup_next_glue_ntb - nothing free\n");// should happen rarely
+    return false;
+  }
+
+  ncm_interface.xmit_glue_ntb_datagram_ndx = 0;
+
+  xmit_ntb_t *ntb = ncm_interface.xmit_glue_ntb;
 
   // Fill in NTB header
   ntb->nth.dwSignature = NTH16_SIGNATURE;
-  ntb->nth.wHeaderLength = sizeof(nth16_t);
-  ntb->nth.wSequence = ncm_interface.nth_sequence++;
-  ntb->nth.wBlockLength = ntb_length;
-  ntb->nth.wNdpIndex = sizeof(nth16_t);
+  ntb->nth.wHeaderLength = sizeof(ntb->nth);
+  ntb->nth.wSequence = ncm_interface.xmit_sequence++;
+  ntb->nth.wBlockLength = sizeof(ntb->nth) + sizeof(ntb->ndp) + sizeof(ntb->ndp_datagram);
+  ntb->nth.wNdpIndex = sizeof(ntb->nth);
 
   // Fill in NDP16 header and terminator
   ntb->ndp.dwSignature = NDP16_SIGNATURE_NCM0;
-  ntb->ndp.wLength = sizeof(ndp16_t) + (ncm_interface.datagram_count + 1) * sizeof(ndp16_datagram_t);
+  ntb->ndp.wLength = sizeof(ntb->ndp) + sizeof(ntb->ndp_datagram);
   ntb->ndp.wNextNdpIndex = 0;
-  ntb->ndp.datagram[ncm_interface.datagram_count].wDatagramIndex = 0;
-  ntb->ndp.datagram[ncm_interface.datagram_count].wDatagramLength = 0;
 
-  // Kick off an endpoint transfer
-  usbd_edpt_xfer(0, ncm_interface.ep_in, ntb->data, ntb_length);
-  ncm_interface.transferring = true;
+  memset(ntb->ndp_datagram, 0, sizeof(ntb->ndp_datagram));
+  return true;
+} // xmit_setup_next_glue_ntb
 
-  // Swap to the other NTB and clear it out
-  ncm_interface.current_ntb = 1 - ncm_interface.current_ntb;
-  ncm_prepare_for_tx();
-}
+//-----------------------------------------------------------------------------
+//
+// all the recv_*() stuff (TinyUSB -> driver -> glue logic)
+//
 
-tu_static struct ecm_notify_struct ncm_notify_connected =
-{
-    .header = {
-        .bmRequestType_bit = {
-          .recipient = TUSB_REQ_RCPT_INTERFACE,
-          .type      = TUSB_REQ_TYPE_CLASS,
-          .direction = TUSB_DIR_IN
-        },
-        .bRequest      = CDC_NOTIF_NETWORK_CONNECTION,
-        .wValue        = 1 /* Connected */,
-        .wLength       = 0,
-    },
-};
-
-tu_static struct ecm_notify_struct ncm_notify_speed_change =
-{
-    .header = {
-        .bmRequestType_bit = {
-          .recipient = TUSB_REQ_RCPT_INTERFACE,
-          .type      = TUSB_REQ_TYPE_CLASS,
-          .direction = TUSB_DIR_IN
-        },
-        .bRequest = CDC_NOTIF_CONNECTION_SPEED_CHANGE,
-        .wLength = 8,
-    },
-    .downlink = 10000000,
-    .uplink = 10000000,
-};
+/**
+ * Return pointer to an available receive buffer or NULL.
+ * Returned buffer (if any) has the size \a CFG_TUD_NCM_OUT_NTB_MAX_SIZE.
+ */
+static recv_ntb_t *recv_get_free_ntb(void) {
+  TU_LOG_DRV("recv_get_free_ntb()\n");
 
-void tud_network_recv_renew(void)
-{
-  if (!ncm_interface.num_datagrams)
-  {
-    usbd_edpt_xfer(0, ncm_interface.ep_out, receive_ntb, sizeof(receive_ntb));
-    return;
+  for (int i = 0; i < RECV_NTB_N; ++i) {
+    if (ncm_interface.recv_free_ntb[i] != NULL) {
+      recv_ntb_t *free = ncm_interface.recv_free_ntb[i];
+      ncm_interface.recv_free_ntb[i] = NULL;
+      return free;
+    }
   }
+  return NULL;
+} // recv_get_free_ntb
 
-  const ndp16_t *ndp = ncm_interface.ndp;
-  const int i = ncm_interface.current_datagram_index;
-  ncm_interface.current_datagram_index++;
-  ncm_interface.num_datagrams--;
+/**
+ * Get the next NTB from the ready list (and remove it from the list).
+ * If the ready list is empty, return NULL.
+ */
+static recv_ntb_t *recv_get_next_ready_ntb(void) {
+  recv_ntb_t *r = NULL;
 
-  tud_network_recv_cb(receive_ntb + ndp->datagram[i].wDatagramIndex, ndp->datagram[i].wDatagramLength);
-}
+  r = ncm_interface.recv_ready_ntb[0];
+  memmove(ncm_interface.recv_ready_ntb + 0, ncm_interface.recv_ready_ntb + 1, sizeof(ncm_interface.recv_ready_ntb) - sizeof(ncm_interface.recv_ready_ntb[0]));
+  ncm_interface.recv_ready_ntb[RECV_NTB_N - 1] = NULL;
 
-//--------------------------------------------------------------------+
-// USBD Driver API
-//--------------------------------------------------------------------+
+  TU_LOG_DRV("recv_get_next_ready_ntb: %p\n", r);
+  return r;
+} // recv_get_next_ready_ntb
 
-void netd_init(void)
-{
-  tu_memclr(&ncm_interface, sizeof(ncm_interface));
-  ncm_interface.ntb_in_size = CFG_TUD_NCM_IN_NTB_MAX_SIZE;
-  ncm_interface.max_datagrams_per_ntb = CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB;
-  ncm_prepare_for_tx();
-}
+/**
+ * Put NTB into the receiver free list.
+ */
+static void recv_put_ntb_into_free_list(recv_ntb_t *free_ntb) {
+  TU_LOG_DRV("recv_put_ntb_into_free_list(%p)\n", free_ntb);
 
-void netd_reset(uint8_t rhport)
-{
-  (void) rhport;
+  for (int i = 0; i < RECV_NTB_N; ++i) {
+    if (ncm_interface.recv_free_ntb[i] == NULL) {
+      ncm_interface.recv_free_ntb[i] = free_ntb;
+      return;
+    }
+  }
+  TU_LOG_DRV("(EE) recv_put_ntb_into_free_list - no entry in free list\n");// this should not happen
+} // recv_put_ntb_into_free_list
 
-  netd_init();
-}
+/**
+ * \a ready_ntb holds a validated NTB,
+ * put this buffer into the waiting list.
+ */
+static void recv_put_ntb_into_ready_list(recv_ntb_t *ready_ntb) {
+  TU_LOG_DRV("recv_put_ntb_into_ready_list(%p) %d\n", ready_ntb, ready_ntb->nth.wBlockLength);
 
-uint16_t netd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len)
-{
-  // confirm interface hasn't already been allocated
-  TU_ASSERT(0 == ncm_interface.ep_notif, 0);
+  for (int i = 0; i < RECV_NTB_N; ++i) {
+    if (ncm_interface.recv_ready_ntb[i] == NULL) {
+      ncm_interface.recv_ready_ntb[i] = ready_ntb;
+      return;
+    }
+  }
+  TU_LOG_DRV("(EE) recv_put_ntb_into_ready_list: ready list full\n");// this should not happen
+} // recv_put_ntb_into_ready_list
 
-  //------------- Management Interface -------------//
-  ncm_interface.itf_num = itf_desc->bInterfaceNumber;
+/**
+ * If possible, start a new reception TinyUSB -> driver.
+ */
+static void recv_try_to_start_new_reception(uint8_t rhport) {
+  TU_LOG_DRV("recv_try_to_start_new_reception(%d)\n", rhport);
 
-  uint16_t drv_len = sizeof(tusb_desc_interface_t);
-  uint8_t const * p_desc = tu_desc_next( itf_desc );
+  if (ncm_interface.itf_data_alt != 1) {
+    return;
+  }
+  if (ncm_interface.recv_tinyusb_ntb != NULL) {
+    return;
+  }
+  if (usbd_edpt_busy(rhport, ncm_interface.ep_out)) {
+    return;
+  }
 
-  // Communication Functional Descriptors
-  while ( TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len )
-  {
-    drv_len += tu_desc_len(p_desc);
-    p_desc   = tu_desc_next(p_desc);
+  ncm_interface.recv_tinyusb_ntb = recv_get_free_ntb();
+  if (ncm_interface.recv_tinyusb_ntb == NULL) {
+    return;
   }
 
-  // notification endpoint (if any)
-  if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) )
-  {
-    TU_ASSERT( usbd_edpt_open(rhport, (tusb_desc_endpoint_t const *) p_desc), 0 );
+  // initiate transfer
+  TU_LOG_DRV("  start reception\n");
+  bool r = usbd_edpt_xfer(rhport, ncm_interface.ep_out, ncm_interface.recv_tinyusb_ntb->data, CFG_TUD_NCM_OUT_NTB_MAX_SIZE);
+  if (!r) {
+    recv_put_ntb_into_free_list(ncm_interface.recv_tinyusb_ntb);
+    ncm_interface.recv_tinyusb_ntb = NULL;
+  }
+} // recv_try_to_start_new_reception
 
-    ncm_interface.ep_notif = ((tusb_desc_endpoint_t const *) p_desc)->bEndpointAddress;
+/**
+ * Validate incoming datagram.
+ * \return true if valid
+ *
+ * \note
+ *    \a ndp16->wNextNdpIndex != 0 is not supported
+ */
+static bool recv_validate_datagram(const recv_ntb_t *ntb, uint32_t len) {
+  const nth16_t *nth16 = &(ntb->nth);
 
-    drv_len += tu_desc_len(p_desc);
-    p_desc   = tu_desc_next(p_desc);
+  TU_LOG_DRV("recv_validate_datagram(%p, %d)\n", ntb, (int) len);
+
+  // check header
+  if (nth16->wHeaderLength != sizeof(nth16_t)) {
+    TU_LOG_DRV("(EE) ill nth16 length: %d\n", nth16->wHeaderLength);
+    return false;
+  }
+  if (nth16->dwSignature != NTH16_SIGNATURE) {
+    TU_LOG_DRV("(EE) ill signature: 0x%08x\n", (unsigned) nth16->dwSignature);
+    return false;
+  }
+  if (len < sizeof(nth16_t) + sizeof(ndp16_t) + 2 * sizeof(ndp16_datagram_t)) {
+    TU_LOG_DRV("(EE) ill min len: %lu\n", len);
+    return false;
+  }
+  if (nth16->wBlockLength > len) {
+    TU_LOG_DRV("(EE) ill block length: %d > %lu\n", nth16->wBlockLength, len);
+    return false;
+  }
+  if (nth16->wBlockLength > CFG_TUD_NCM_OUT_NTB_MAX_SIZE) {
+    TU_LOG_DRV("(EE) ill block length2: %d > %d\n", nth16->wBlockLength, CFG_TUD_NCM_OUT_NTB_MAX_SIZE);
+    return false;
+  }
+  if (nth16->wNdpIndex < sizeof(nth16) || nth16->wNdpIndex > len - (sizeof(ndp16_t) + 2 * sizeof(ndp16_datagram_t))) {
+    TU_LOG_DRV("(EE) ill position of first ndp: %d (%lu)\n", nth16->wNdpIndex, len);
+    return false;
   }
 
-  //------------- Data Interface -------------//
-  // - CDC-NCM data interface has 2 alternate settings
-  //   - 0 : zero endpoints for inactive (default)
-  //   - 1 : IN & OUT endpoints for transfer of NTBs
-  TU_ASSERT(TUSB_DESC_INTERFACE == tu_desc_type(p_desc), 0);
+  // check (first) NDP(16)
+  const ndp16_t *ndp16 = (const ndp16_t *) (ntb->data + nth16->wNdpIndex);
 
-  do
-  {
-    tusb_desc_interface_t const * data_itf_desc = (tusb_desc_interface_t const *) p_desc;
-    TU_ASSERT(TUSB_CLASS_CDC_DATA == data_itf_desc->bInterfaceClass, 0);
+  if (ndp16->wLength < sizeof(ndp16_t) + 2 * sizeof(ndp16_datagram_t)) {
+    TU_LOG_DRV("(EE) ill ndp16 length: %d\n", ndp16->wLength);
+    return false;
+  }
+  if (ndp16->dwSignature != NDP16_SIGNATURE_NCM0 && ndp16->dwSignature != NDP16_SIGNATURE_NCM1) {
+    TU_LOG_DRV("(EE) ill signature: 0x%08x\n", (unsigned) ndp16->dwSignature);
+    return false;
+  }
+  if (ndp16->wNextNdpIndex != 0) {
+    TU_LOG_DRV("(EE) cannot handle wNextNdpIndex!=0 (%d)\n", ndp16->wNextNdpIndex);
+    return false;
+  }
 
-    drv_len += tu_desc_len(p_desc);
-    p_desc   = tu_desc_next(p_desc);
-  } while((TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) && (drv_len <= max_len));
+  const ndp16_datagram_t *ndp16_datagram = (const ndp16_datagram_t *) (ntb->data + nth16->wNdpIndex + sizeof(ndp16_t));
+  int ndx = 0;
+  uint16_t max_ndx = (uint16_t) ((ndp16->wLength - sizeof(ndp16_t)) / sizeof(ndp16_datagram_t));
 
-  // Pair of endpoints
-  TU_ASSERT(TUSB_DESC_ENDPOINT == tu_desc_type(p_desc), 0);
+  if (max_ndx > 2) { // number of datagrams in NTB > 1
+    TU_LOG_DRV("<< %d (%d)\n", max_ndx - 1, ntb->nth.wBlockLength);
+  }
+  if (ndp16_datagram[max_ndx - 1].wDatagramIndex != 0 || ndp16_datagram[max_ndx - 1].wDatagramLength != 0) {
+    TU_LOG_DRV("  max_ndx != 0\n");
+    return false;
+  }
+  while (ndp16_datagram[ndx].wDatagramIndex != 0 && ndp16_datagram[ndx].wDatagramLength != 0) {
+    TU_LOG_DRV("  << %d %d\n", ndp16_datagram[ndx].wDatagramIndex, ndp16_datagram[ndx].wDatagramLength);
+    if (ndp16_datagram[ndx].wDatagramIndex > len) {
+      TU_LOG_DRV("(EE) ill start of datagram[%d]: %d (%lu)\n", ndx, ndp16_datagram[ndx].wDatagramIndex, len);
+      return false;
+    }
+    if (ndp16_datagram[ndx].wDatagramIndex + ndp16_datagram[ndx].wDatagramLength > len) {
+      TU_LOG_DRV("(EE) ill end of datagram[%d]: %d (%lu)\n", ndx, ndp16_datagram[ndx].wDatagramIndex + ndp16_datagram[ndx].wDatagramLength, len);
+      return false;
+    }
+    ++ndx;
+  }
 
-  TU_ASSERT(usbd_open_edpt_pair(rhport, p_desc, 2, TUSB_XFER_BULK, &ncm_interface.ep_out, &ncm_interface.ep_in) );
+  #if CFG_TUD_NCM_LOG_LEVEL >= 3
+  TU_LOG_BUF(3, ntb->data[i], len);
+  #endif
 
-  drv_len += 2*sizeof(tusb_desc_endpoint_t);
+  // -> ntb contains a valid packet structure
+  //    ok... I did not check for garbage within the datagram indices...
+  return true;
+} // recv_validate_datagram
 
-  return drv_len;
-}
+/**
+ * Transfer the next (pending) datagram to the glue logic and return receive buffer if empty.
+ */
+static void recv_transfer_datagram_to_glue_logic(void) {
+  TU_LOG_DRV("recv_transfer_datagram_to_glue_logic()\n");
 
-static void ncm_report(void)
-{
-  uint8_t const rhport = 0;
-  if (ncm_interface.report_state == REPORT_SPEED) {
-    ncm_notify_speed_change.header.wIndex = ncm_interface.itf_num;
-    usbd_edpt_xfer(rhport, ncm_interface.ep_notif, (uint8_t *) &ncm_notify_speed_change, sizeof(ncm_notify_speed_change));
-    ncm_interface.report_state = REPORT_CONNECTED;
-    ncm_interface.report_pending = true;
-  } else if (ncm_interface.report_state == REPORT_CONNECTED) {
-    ncm_notify_connected.header.wIndex = ncm_interface.itf_num;
-    usbd_edpt_xfer(rhport, ncm_interface.ep_notif, (uint8_t *) &ncm_notify_connected, sizeof(ncm_notify_connected));
-    ncm_interface.report_state = REPORT_DONE;
-    ncm_interface.report_pending = true;
+  if (ncm_interface.recv_glue_ntb == NULL) {
+    ncm_interface.recv_glue_ntb = recv_get_next_ready_ntb();
+    TU_LOG_DRV("  new buffer for glue logic: %p\n", ncm_interface.recv_glue_ntb);
+    ncm_interface.recv_glue_ntb_datagram_ndx = 0;
   }
-}
 
-TU_ATTR_WEAK void tud_network_link_state_cb(bool state)
-{
-  (void)state;
-}
+  if (ncm_interface.recv_glue_ntb != NULL) {
+    const ndp16_datagram_t *ndp16_datagram = (ndp16_datagram_t *) (ncm_interface.recv_glue_ntb->data + ncm_interface.recv_glue_ntb->nth.wNdpIndex + sizeof(ndp16_t));
 
-// Handle class control request
-// return false to stall control endpoint (e.g unsupported request)
-bool netd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
-{
-  if ( stage != CONTROL_STAGE_SETUP ) return true;
+    if (ndp16_datagram[ncm_interface.recv_glue_ntb_datagram_ndx].wDatagramIndex == 0) {
+      TU_LOG_DRV("(EE) SOMETHING WENT WRONG 1\n");
+    } else if (ndp16_datagram[ncm_interface.recv_glue_ntb_datagram_ndx].wDatagramLength == 0) {
+      TU_LOG_DRV("(EE) SOMETHING WENT WRONG 2\n");
+    } else {
+      uint16_t datagramIndex = ndp16_datagram[ncm_interface.recv_glue_ntb_datagram_ndx].wDatagramIndex;
+      uint16_t datagramLength = ndp16_datagram[ncm_interface.recv_glue_ntb_datagram_ndx].wDatagramLength;
 
-  switch ( request->bmRequestType_bit.type )
-  {
-    case TUSB_REQ_TYPE_STANDARD:
-      switch ( request->bRequest )
-      {
-        case TUSB_REQ_GET_INTERFACE:
-        {
-          uint8_t const req_itfnum = (uint8_t) request->wIndex;
-          TU_VERIFY(ncm_interface.itf_num + 1 == req_itfnum);
+      TU_LOG_DRV("  recv[%d] - %d %d\n", ncm_interface.recv_glue_ntb_datagram_ndx, datagramIndex, datagramLength);
+      if (tud_network_recv_cb(ncm_interface.recv_glue_ntb->data + datagramIndex, datagramLength)) {
+        // send datagram successfully to glue logic
+        TU_LOG_DRV("    OK\n");
+        datagramIndex = ndp16_datagram[ncm_interface.recv_glue_ntb_datagram_ndx + 1].wDatagramIndex;
+        datagramLength = ndp16_datagram[ncm_interface.recv_glue_ntb_datagram_ndx + 1].wDatagramLength;
 
-          tud_control_xfer(rhport, request, &ncm_interface.itf_data_alt, 1);
+        if (datagramIndex != 0 && datagramLength != 0) {
+          // -> next datagram
+          ++ncm_interface.recv_glue_ntb_datagram_ndx;
+        } else {
+          // end of datagrams reached
+          recv_put_ntb_into_free_list(ncm_interface.recv_glue_ntb);
+          ncm_interface.recv_glue_ntb = NULL;
         }
-          break;
+      }
+    }
+  }
+} // recv_transfer_datagram_to_glue_logic
 
-        case TUSB_REQ_SET_INTERFACE:
-        {
-          uint8_t const req_itfnum = (uint8_t) request->wIndex;
-          uint8_t const req_alt    = (uint8_t) request->wValue;
+//-----------------------------------------------------------------------------
+//
+// all the tud_network_*() stuff (glue logic -> driver)
+//
 
-          // Only valid for Data Interface with Alternate is either 0 or 1
-          TU_VERIFY(ncm_interface.itf_num + 1 == req_itfnum && req_alt < 2);
+/**
+ * Check if the glue logic is allowed to call tud_network_xmit().
+ * This function also fetches a next buffer if required, so that tud_network_xmit() is ready for copy
+ * and transmission operation.
+ */
+bool tud_network_can_xmit(uint16_t size) {
+  TU_LOG_DRV("tud_network_can_xmit(%d)\n", size);
 
-          if (req_alt != ncm_interface.itf_data_alt) {
-            ncm_interface.itf_data_alt = req_alt;
+  TU_ASSERT(size <= CFG_TUD_NCM_OUT_NTB_MAX_SIZE - (sizeof(nth16_t) + sizeof(ndp16_t) + 2 * sizeof(ndp16_datagram_t)), false);
 
-            if (ncm_interface.itf_data_alt) {
-              if (!usbd_edpt_busy(rhport, ncm_interface.ep_out)) {
-                tud_network_recv_renew(); // prepare for incoming datagrams
-              }
-              if (!ncm_interface.report_pending) {
-                ncm_report();
-              }
-            }
+  if (xmit_requested_datagram_fits_into_current_ntb(size) || xmit_setup_next_glue_ntb()) {
+    // -> everything is fine
+    return true;
+  }
+  xmit_start_if_possible(ncm_interface.rhport);
+  TU_LOG_DRV("(II) tud_network_can_xmit: request blocked\n");// could happen if all xmit buffers are full (but should happen rarely)
+  return false;
+} // tud_network_can_xmit
 
-            tud_network_link_state_cb(ncm_interface.itf_data_alt);
-          }
+/**
+ * Put a datagram into a waiting NTB.
+ * If currently no transmission is started, then initiate transmission.
+ */
+void tud_network_xmit(void *ref, uint16_t arg) {
+  TU_LOG_DRV("tud_network_xmit(%p, %d)\n", ref, arg);
 
-          tud_control_status(rhport, request);
-        }
-          break;
+  if (ncm_interface.xmit_glue_ntb == NULL) {
+    TU_LOG_DRV("(EE) tud_network_xmit: no buffer\n");// must not happen (really)
+    return;
+  }
 
-          // unsupported request
-        default: return false;
-      }
-      break;
+  xmit_ntb_t *ntb = ncm_interface.xmit_glue_ntb;
 
-    case TUSB_REQ_TYPE_CLASS:
-      TU_VERIFY (ncm_interface.itf_num == request->wIndex);
+  // copy new datagram to the end of the current NTB
+  uint16_t size = tud_network_xmit_cb(ntb->data + ntb->nth.wBlockLength, ref, arg);
 
-      if (NCM_GET_NTB_PARAMETERS == request->bRequest)
-      {
-        tud_control_xfer(rhport, request, (void*)(uintptr_t) &ntb_parameters, sizeof(ntb_parameters));
-      }
+  // correct NTB internals
+  ntb->ndp_datagram[ncm_interface.xmit_glue_ntb_datagram_ndx].wDatagramIndex = ntb->nth.wBlockLength;
+  ntb->ndp_datagram[ncm_interface.xmit_glue_ntb_datagram_ndx].wDatagramLength = size;
+  ncm_interface.xmit_glue_ntb_datagram_ndx += 1;
 
-      break;
+  ntb->nth.wBlockLength += (uint16_t) (size + XMIT_ALIGN_OFFSET(size));
 
-      // unsupported request
-    default: return false;
+  if (ntb->nth.wBlockLength > CFG_TUD_NCM_OUT_NTB_MAX_SIZE) {
+    TU_LOG_DRV("(EE) tud_network_xmit: buffer overflow\n"); // must not happen (really)
+    return;
   }
 
-  return true;
-}
+  xmit_start_if_possible(ncm_interface.rhport);
+} // tud_network_xmit
 
-static void handle_incoming_datagram(uint32_t len)
-{
-  uint32_t size = len;
+/**
+ * Keep the receive logic busy and transfer pending packets to the glue logic.
+ */
+void tud_network_recv_renew(void) {
+  TU_LOG_DRV("tud_network_recv_renew()\n");
 
-  if (len == 0) {
-    return;
-  }
+  recv_transfer_datagram_to_glue_logic();
+  recv_try_to_start_new_reception(ncm_interface.rhport);
+} // tud_network_recv_renew
 
-  TU_ASSERT(size >= sizeof(nth16_t), );
+/**
+ * Same as tud_network_recv_renew() but knows \a rhport
+ */
+void tud_network_recv_renew_r(uint8_t rhport) {
+  TU_LOG_DRV("tud_network_recv_renew_r(%d)\n", rhport);
 
-  const nth16_t *hdr = (const nth16_t *)receive_ntb;
-  TU_ASSERT(hdr->dwSignature == NTH16_SIGNATURE, );
-  TU_ASSERT(hdr->wNdpIndex >= sizeof(nth16_t) && (hdr->wNdpIndex + sizeof(ndp16_t)) <= len, );
+  ncm_interface.rhport = rhport;
+  tud_network_recv_renew();
+} // tud_network_recv_renew
 
-  const ndp16_t *ndp = (const ndp16_t *)(receive_ntb + hdr->wNdpIndex);
-  TU_ASSERT(ndp->dwSignature == NDP16_SIGNATURE_NCM0 || ndp->dwSignature == NDP16_SIGNATURE_NCM1, );
-  TU_ASSERT(hdr->wNdpIndex + ndp->wLength <= len, );
+//-----------------------------------------------------------------------------
+//
+// all the netd_*() stuff (interface TinyUSB -> driver)
+//
+/**
+ * Initialize the driver data structures.
+ * Might be called several times.
+ */
+void netd_init(void) {
+  TU_LOG_DRV("netd_init()\n");
 
-  int num_datagrams = (ndp->wLength - 12) / 4;
-  ncm_interface.current_datagram_index = 0;
-  ncm_interface.num_datagrams = 0;
-  ncm_interface.ndp = ndp;
-  for (int i = 0; i < num_datagrams && ndp->datagram[i].wDatagramIndex && ndp->datagram[i].wDatagramLength; i++)
-  {
-    ncm_interface.num_datagrams++;
+  memset(&ncm_interface, 0, sizeof(ncm_interface));
+
+  for (int i = 0; i < XMIT_NTB_N; ++i) {
+    ncm_interface.xmit_free_ntb[i] = ncm_interface.xmit_ntb + i;
   }
+  for (int i = 0; i < RECV_NTB_N; ++i) {
+    ncm_interface.recv_free_ntb[i] = ncm_interface.recv_ntb + i;
+  }
+} // netd_init
 
-  tud_network_recv_renew();
+/**
+ * Deinit driver
+ */
+bool netd_deinit(void) {
+  return true;
 }
 
-bool netd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
+/**
+ * Resets the port.
+ * In this driver this is the same as netd_init()
+ */
+void netd_reset(uint8_t rhport) {
   (void) rhport;
-  (void) result;
 
-  /* new datagram receive_ntb */
-  if (ep_addr == ncm_interface.ep_out )
-  {
-    handle_incoming_datagram(xferred_bytes);
-  }
+  netd_init();
+} // netd_reset
 
-  /* data transmission finished */
-  if (ep_addr == ncm_interface.ep_in )
-  {
-    if (ncm_interface.transferring) {
-      ncm_interface.transferring = false;
-    }
+/**
+ * Open the USB interface.
+ * - parse the USB descriptor \a TUD_CDC_NCM_DESCRIPTOR for itfnum and endpoints
+ * - a specific order of elements in the descriptor is tested.
+ *
+ * \note
+ *   Actually all of the information could be read directly from \a itf_desc, because the
+ *   structure and the values are well known.  But we do it this way.
+ *
+ * \post
+ * - \a itf_num set
+ * - \a ep_notif, \a ep_in and \a ep_out are set
+ * - USB interface is open
+ */
+uint16_t netd_open(uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_t max_len) {
+  TU_ASSERT(ncm_interface.ep_notif == 0, 0);// assure that the interface is only opened once
 
-    // If there are datagrams queued up that we tried to send while this NTB was being emitted, send them now
-    if (ncm_interface.datagram_count && ncm_interface.itf_data_alt == 1) {
-      ncm_start_tx();
-    }
-  }
+  ncm_interface.itf_num = itf_desc->bInterfaceNumber;// management interface
 
-  if (ep_addr == ncm_interface.ep_notif )
-  {
-    ncm_interface.report_pending = false;
-    ncm_report();
+  // skip the two first entries and the following TUSB_DESC_CS_INTERFACE entries
+  uint16_t drv_len = sizeof(tusb_desc_interface_t);
+  uint8_t const *p_desc = tu_desc_next(itf_desc);
+  while (tu_desc_type(p_desc) == TUSB_DESC_CS_INTERFACE && drv_len <= max_len) {
+    drv_len += tu_desc_len(p_desc);
+    p_desc = tu_desc_next(p_desc);
   }
 
-  return true;
-}
+  // get notification endpoint
+  TU_ASSERT(tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT, 0);
+  TU_ASSERT(usbd_edpt_open(rhport, (tusb_desc_endpoint_t const *) p_desc), 0);
+  ncm_interface.ep_notif = ((tusb_desc_endpoint_t const *) p_desc)->bEndpointAddress;
+  drv_len += tu_desc_len(p_desc);
+  p_desc = tu_desc_next(p_desc);
 
-// poll network driver for its ability to accept another packet to transmit
-bool tud_network_can_xmit(uint16_t size)
-{
-  TU_VERIFY(ncm_interface.itf_data_alt == 1);
+  // skip the following TUSB_DESC_INTERFACE entries (which must be TUSB_CLASS_CDC_DATA)
+  while (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE && drv_len <= max_len) {
+    tusb_desc_interface_t const *data_itf_desc = (tusb_desc_interface_t const *) p_desc;
+    TU_ASSERT(data_itf_desc->bInterfaceClass == TUSB_CLASS_CDC_DATA, 0);
 
-  if (ncm_interface.datagram_count >= ncm_interface.max_datagrams_per_ntb) {
-    TU_LOG_DRV("NTB full [by count]\r\n");
-    return false;
+    drv_len += tu_desc_len(p_desc);
+    p_desc = tu_desc_next(p_desc);
   }
 
-  size_t next_datagram_offset = ncm_interface.next_datagram_offset;
-  if (next_datagram_offset + size > ncm_interface.ntb_in_size) {
-    TU_LOG_DRV("ntb full [by size]\r\n");
-    return false;
+  // a TUSB_DESC_ENDPOINT (actually two) must follow, open these endpoints
+  TU_ASSERT(tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT, 0);
+  TU_ASSERT(usbd_open_edpt_pair(rhport, p_desc, 2, TUSB_XFER_BULK, &ncm_interface.ep_out, &ncm_interface.ep_in));
+  drv_len += 2 * sizeof(tusb_desc_endpoint_t);
+
+  return drv_len;
+} // netd_open
+
+/**
+ * Handle TinyUSB requests to process transfer events.
+ */
+bool netd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
+  (void) result;
+
+  if (ep_addr == ncm_interface.ep_out) {
+    // new NTB received
+    // - make the NTB valid
+    // - if ready transfer datagrams to the glue logic for further processing
+    // - if there is a free receive buffer, initiate reception
+    if (!recv_validate_datagram(ncm_interface.recv_tinyusb_ntb, xferred_bytes)) {
+      // verification failed: ignore NTB and return it to free
+      TU_LOG_DRV("(EE) VALIDATION FAILED. WHAT CAN WE DO IN THIS CASE?\n");
+    } else {
+      // packet ok -> put it into ready list
+      recv_put_ntb_into_ready_list(ncm_interface.recv_tinyusb_ntb);
+    }
+    ncm_interface.recv_tinyusb_ntb = NULL;
+    tud_network_recv_renew_r(rhport);
+  } else if (ep_addr == ncm_interface.ep_in) {
+    // transmission of an NTB finished
+    // - free the transmitted NTB buffer
+    // - insert ZLPs when necessary
+    // - if there is another transmit NTB waiting, try to start transmission
+    xmit_put_ntb_into_free_list(ncm_interface.xmit_tinyusb_ntb);
+    ncm_interface.xmit_tinyusb_ntb = NULL;
+    if (!xmit_insert_required_zlp(rhport, xferred_bytes)) {
+      xmit_start_if_possible(rhport);
+    }
+  } else if (ep_addr == ncm_interface.ep_notif) {
+    // next transfer on notification channel
+    notification_xmit(rhport, true);
   }
 
   return true;
-}
+} // netd_xfer_cb
 
-void tud_network_xmit(void *ref, uint16_t arg)
-{
-  transmit_ntb_t *ntb = &transmit_ntb[ncm_interface.current_ntb];
-  size_t next_datagram_offset = ncm_interface.next_datagram_offset;
+/**
+ * Respond to TinyUSB control requests.
+ * At startup transmission of notification packets are done here.
+ */
+bool netd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request) {
+  if (stage != CONTROL_STAGE_SETUP) {
+    return true;
+  }
 
-  uint16_t size = tud_network_xmit_cb(ntb->data + next_datagram_offset, ref, arg);
+  switch (request->bmRequestType_bit.type) {
+    case TUSB_REQ_TYPE_STANDARD:
 
-  ntb->ndp.datagram[ncm_interface.datagram_count].wDatagramIndex = ncm_interface.next_datagram_offset;
-  ntb->ndp.datagram[ncm_interface.datagram_count].wDatagramLength = size;
+      switch (request->bRequest) {
+        case TUSB_REQ_GET_INTERFACE: {
+          TU_VERIFY(ncm_interface.itf_num + 1 == request->wIndex, false);
 
-  ncm_interface.datagram_count++;
-  next_datagram_offset += size;
+          tud_control_xfer(rhport, request, &ncm_interface.itf_data_alt, 1);
+        } break;
 
-  // round up so the next datagram is aligned correctly
-  next_datagram_offset += (CFG_TUD_NCM_ALIGNMENT - 1);
-  next_datagram_offset -= (next_datagram_offset % CFG_TUD_NCM_ALIGNMENT);
+        case TUSB_REQ_SET_INTERFACE: {
+          TU_VERIFY(ncm_interface.itf_num + 1 == request->wIndex && request->wValue < 2, false);
 
-  ncm_interface.next_datagram_offset = next_datagram_offset;
+          ncm_interface.itf_data_alt = (uint8_t) request->wValue;
 
-  ncm_start_tx();
-}
+          if (ncm_interface.itf_data_alt == 1) {
+            tud_network_recv_renew_r(rhport);
+            notification_xmit(rhport, false);
+          }
+          tud_control_status(rhport, request);
+        } break;
 
-#endif
+        // unsupported request
+        default:
+          return false;
+      }
+      break;
+
+    case TUSB_REQ_TYPE_CLASS:
+      TU_VERIFY(ncm_interface.itf_num == request->wIndex, false);
+      switch (request->bRequest) {
+        case NCM_GET_NTB_PARAMETERS: {
+          // transfer NTB parameters to host.
+          tud_control_xfer(rhport, request, (void *) (uintptr_t) &ntb_parameters, sizeof(ntb_parameters));
+        } break;
+
+          // unsupported request
+        default:
+          return false;
+      }
+      break;
+      // unsupported request
+    default:
+      return false;
+  }
+
+  return true;
+} // netd_control_xfer_cb
+
+#endif // ( CFG_TUD_ENABLED && CFG_TUD_NCM )
diff --git a/src/class/net/net_device.h b/src/class/net/net_device.h
index 399916355..4c9a92f2d 100644
--- a/src/class/net/net_device.h
+++ b/src/class/net/net_device.h
@@ -28,14 +28,13 @@
 #ifndef _TUSB_NET_DEVICE_H_
 #define _TUSB_NET_DEVICE_H_
 
+#include <stdint.h>
 #include "class/cdc/cdc.h"
 
 #if CFG_TUD_ECM_RNDIS && CFG_TUD_NCM
 #error "Cannot enable both ECM_RNDIS and NCM network drivers"
 #endif
 
-#include "ncm.h"
-
 /* declared here, NOT in usb_descriptors.c, so that the driver can intelligently ZLP as needed */
 #define CFG_TUD_NET_ENDPOINT_SIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
 
@@ -44,21 +43,13 @@
 #define CFG_TUD_NET_MTU           1514
 #endif
 
-#ifndef CFG_TUD_NCM_IN_NTB_MAX_SIZE
-#define CFG_TUD_NCM_IN_NTB_MAX_SIZE 3200
-#endif
-
-#ifndef CFG_TUD_NCM_OUT_NTB_MAX_SIZE
-#define CFG_TUD_NCM_OUT_NTB_MAX_SIZE 3200
-#endif
 
-#ifndef CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB
-#define CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB 8
-#endif
+// Table 4.3 Data Class Interface Protocol Codes
+typedef enum
+{
+  NCM_DATA_PROTOCOL_NETWORK_TRANSFER_BLOCK = 0x01
+} ncm_data_interface_protocol_code_t;
 
-#ifndef CFG_TUD_NCM_ALIGNMENT
-#define CFG_TUD_NCM_ALIGNMENT 4
-#endif
 
 #ifdef __cplusplus
  extern "C" {
@@ -96,15 +87,11 @@ void tud_network_init_cb(void);
 // TODO removed later since it is not part of tinyusb stack
 extern uint8_t tud_network_mac_address[6];
 
-//------------- NCM -------------//
-
-// callback to client providing optional indication of internal state of network driver
-void tud_network_link_state_cb(bool state);
-
 //--------------------------------------------------------------------+
 // INTERNAL USBD-CLASS DRIVER API
 //--------------------------------------------------------------------+
 void     netd_init            (void);
+bool     netd_deinit          (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);
diff --git a/src/class/usbtmc/usbtmc.h b/src/class/usbtmc/usbtmc.h
index 090ab3c4a..327de087c 100644
--- a/src/class/usbtmc/usbtmc.h
+++ b/src/class/usbtmc/usbtmc.h
@@ -184,6 +184,23 @@ typedef enum {
 } usmtmc_request_type_enum;
 
 typedef enum {
+  // The last and first valid bNotify1 for use by the USBTMC class specification.
+  USBTMC_bNOTIFY1_USBTMC_FIRST          = 0x00,
+  USBTMC_bNOTIFY1_USBTMC_LAST           = 0x3F,
+
+  // The last and first valid bNotify1 for use by vendors.
+  USBTMC_bNOTIFY1_VENDOR_SPECIFIC_FIRST = 0x40,
+  USBTMC_bNOTIFY1_VENDOR_SPECIFIC_LAST  = 0x7F,
+
+  // The last and first valid bNotify1 for use by USBTMC subclass specifications.
+  USBTMC_bNOTIFY1_SUBCLASS_FIRST        = 0x80,
+  USBTMC_bNOTIFY1_SUBCLASS_LAST         = 0xFF,
+
+  // From the USB488 Subclass Specification, Section 3.4.
+  USB488_bNOTIFY1_SRQ                   = 0x81,
+} usbtmc_int_in_payload_format;
+
+typedef enum {
   USBTMC_STATUS_SUCCESS = 0x01,
   USBTMC_STATUS_PENDING = 0x02,
   USBTMC_STATUS_FAILED = 0x80,
@@ -305,6 +322,14 @@ TU_VERIFY_STATIC(sizeof(usbtmc_read_stb_rsp_488_t) == 3u, "struct wrong length")
 
 typedef struct TU_ATTR_PACKED
 {
+  uint8_t bNotify1; // Must be USB488_bNOTIFY1_SRQ
+  uint8_t StatusByte;
+} usbtmc_srq_interrupt_488_t;
+
+TU_VERIFY_STATIC(sizeof(usbtmc_srq_interrupt_488_t) == 2u, "struct wrong length");
+
+typedef struct TU_ATTR_PACKED
+{
   struct TU_ATTR_PACKED
   {
       unsigned int bTag : 7;
diff --git a/src/class/usbtmc/usbtmc_device.c b/src/class/usbtmc/usbtmc_device.c
index 573654d58..129ff465d 100644
--- a/src/class/usbtmc/usbtmc_device.c
+++ b/src/class/usbtmc/usbtmc_device.c
@@ -86,6 +86,11 @@ tu_static char logMsg[150];
 // imposes a minimum buffer size of 32 bytes.
 #define USBTMCD_BUFFER_SIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
 
+// Interrupt endpoint buffer size, default to 2 bytes as USB488 specification.
+#ifndef CFG_TUD_USBTMC_INT_EP_SIZE
+#define CFG_TUD_USBTMC_INT_EP_SIZE 2
+#endif
+
 /*
  * The state machine does not allow simultaneous reading and writing. This is
  * consistent with USBTMC.
@@ -124,13 +129,15 @@ typedef struct
   uint8_t ep_bulk_in;
   uint8_t ep_bulk_out;
   uint8_t ep_int_in;
+  uint32_t ep_bulk_in_wMaxPacketSize;
+  uint32_t ep_bulk_out_wMaxPacketSize;
   // IN buffer is only used for first packet, not the remainder
   // in order to deal with prepending header
   CFG_TUSB_MEM_ALIGN uint8_t ep_bulk_in_buf[USBTMCD_BUFFER_SIZE];
-  uint32_t ep_bulk_in_wMaxPacketSize;
   // OUT buffer receives one packet at a time
   CFG_TUSB_MEM_ALIGN uint8_t ep_bulk_out_buf[USBTMCD_BUFFER_SIZE];
-  uint32_t ep_bulk_out_wMaxPacketSize;
+  // Buffer int msg to ensure alignment and placement correctness
+  CFG_TUSB_MEM_ALIGN uint8_t ep_int_in_buf[CFG_TUD_USBTMC_INT_EP_SIZE];
 
   uint32_t transfer_size_remaining; // also used for requested length for bulk IN.
   uint32_t transfer_size_sent;      // To keep track of data bytes that have been queued in FIFO (not header bytes)
@@ -240,6 +247,19 @@ bool tud_usbtmc_transmit_dev_msg_data(
   return true;
 }
 
+bool tud_usbtmc_transmit_notification_data(const void * data, size_t len)
+{
+#ifndef NDEBUG
+  TU_ASSERT(len > 0);
+  TU_ASSERT(usbtmc_state.ep_int_in != 0);
+#endif
+  TU_VERIFY(usbd_edpt_busy(usbtmc_state.rhport, usbtmc_state.ep_int_in));
+
+  TU_VERIFY(tu_memcpy_s(usbtmc_state.ep_int_in_buf, sizeof(usbtmc_state.ep_int_in_buf), data, len) == 0);
+  TU_VERIFY(usbd_edpt_xfer(usbtmc_state.rhport, usbtmc_state.ep_int_in, usbtmc_state.ep_int_in_buf, (uint16_t)len));
+  return true;
+}
+
 void usbtmcd_init_cb(void)
 {
   usbtmc_state.capabilities = tud_usbtmc_get_capabilities_cb();
@@ -260,6 +280,13 @@ void usbtmcd_init_cb(void)
   usbtmcLock = osal_mutex_create(&usbtmcLockBuffer);
 }
 
+bool usbtmcd_deinit(void) {
+  #if OSAL_MUTEX_REQUIRED
+  osal_mutex_delete(usbtmcLock);
+  #endif
+  return true;
+}
+
 uint16_t usbtmcd_open_cb(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len)
 {
   (void)rhport;
@@ -353,7 +380,7 @@ uint16_t usbtmcd_open_cb(uint8_t rhport, tusb_desc_interface_t const * itf_desc,
 // processing a command (such as a clear). Returns true if it was
 // in the NAK state and successfully transitioned to the ACK wait
 // state.
-bool tud_usbtmc_start_bus_read()
+bool tud_usbtmc_start_bus_read(void)
 {
   usbtmcd_state_enum oldState = usbtmc_state.state;
   switch(oldState)
@@ -540,9 +567,10 @@ bool usbtmcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint
     case STATE_TX_INITIATED:
       if(usbtmc_state.transfer_size_remaining >= sizeof(usbtmc_state.ep_bulk_in_buf))
       {
-        // FIXME! This removes const below!
+        // Copy buffer to ensure alignment correctness
+        memcpy(usbtmc_state.ep_bulk_in_buf, usbtmc_state.devInBuffer, sizeof(usbtmc_state.ep_bulk_in_buf));
         TU_VERIFY( usbd_edpt_xfer(rhport, usbtmc_state.ep_bulk_in,
-            (void*)(uintptr_t) usbtmc_state.devInBuffer, sizeof(usbtmc_state.ep_bulk_in_buf)));
+            usbtmc_state.ep_bulk_in_buf, sizeof(usbtmc_state.ep_bulk_in_buf)));
         usbtmc_state.devInBuffer += sizeof(usbtmc_state.ep_bulk_in_buf);
         usbtmc_state.transfer_size_remaining -= sizeof(usbtmc_state.ep_bulk_in_buf);
         usbtmc_state.transfer_size_sent += sizeof(usbtmc_state.ep_bulk_in_buf);
@@ -578,7 +606,9 @@ bool usbtmcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint
     }
   }
   else if (ep_addr == usbtmc_state.ep_int_in) {
-    // Good?
+    if (tud_usbtmc_notification_complete_cb) {
+      TU_VERIFY(tud_usbtmc_notification_complete_cb());
+    }
     return true;
   }
   return false;
diff --git a/src/class/usbtmc/usbtmc_device.h b/src/class/usbtmc/usbtmc_device.h
index c1298ddb8..b85ef12b5 100644
--- a/src/class/usbtmc/usbtmc_device.h
+++ b/src/class/usbtmc/usbtmc_device.h
@@ -73,6 +73,10 @@ bool tud_usbtmc_check_abort_bulk_in_cb(usbtmc_check_abort_bulk_rsp_t *rsp);
 bool tud_usbtmc_check_abort_bulk_out_cb(usbtmc_check_abort_bulk_rsp_t *rsp);
 bool tud_usbtmc_check_clear_cb(usbtmc_get_clear_status_rsp_t *rsp);
 
+// The interrupt-IN endpoint buffer was transmitted to the host. Use
+// tud_usbtmc_transmit_notification_data to send another notification.
+TU_ATTR_WEAK bool tud_usbtmc_notification_complete_cb(void);
+
 // Indicator pulse should be 0.5 to 1.0 seconds long
 TU_ATTR_WEAK bool tud_usbtmc_indicator_pulse_cb(tusb_control_request_t const * msg, uint8_t *tmcResult);
 
@@ -82,31 +86,33 @@ TU_ATTR_WEAK bool tud_usbtmc_msg_trigger_cb(usbtmc_msg_generic_t* msg);
 //TU_ATTR_WEAK bool tud_usbtmc_app_go_to_local_cb();
 #endif
 
-/*******************************************
- * Called from app
- *
- * We keep a reference to the buffer, so it MUST not change until the app is
- * notified that the transfer is complete.
- ******************************************/
-
+// Called from app
+//
+// We keep a reference to the buffer, so it MUST not change until the app is
+// notified that the transfer is complete.
 bool tud_usbtmc_transmit_dev_msg_data(
     const void * data, size_t len,
     bool endOfMessage, bool usingTermChar);
 
+// Buffers a notification to be sent to the host. The data starts
+// with the bNotify1 field, see the USBTMC Specification, Table 13.
+//
+// If the previous notification data has not yet been sent, this
+// returns false.
+//
+// Requires an interrupt endpoint in the interface.
+bool tud_usbtmc_transmit_notification_data(const void * data, size_t len);
+
 bool tud_usbtmc_start_bus_read(void);
 
 
 /* "callbacks" from USB device core */
 
+void     usbtmcd_init_cb(void);
+bool     usbtmcd_deinit(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_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
-void     usbtmcd_init_cb(void);
-
-/************************************************************
- * USBTMC Descriptor Templates
- *************************************************************/
-
 
 #endif /* CLASS_USBTMC_USBTMC_DEVICE_H_ */
diff --git a/src/class/vendor/vendor_device.c b/src/class/vendor/vendor_device.c
index 389a29696..56ef098c7 100644
--- a/src/class/vendor/vendor_device.c
+++ b/src/class/vendor/vendor_device.c
@@ -36,6 +36,8 @@
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
+#define BULK_PACKET_SIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
+
 typedef struct
 {
   uint8_t itf_num;
@@ -49,10 +51,8 @@ typedef struct
   uint8_t rx_ff_buf[CFG_TUD_VENDOR_RX_BUFSIZE];
   uint8_t tx_ff_buf[CFG_TUD_VENDOR_TX_BUFSIZE];
 
-#if CFG_FIFO_MUTEX
-  osal_mutex_def_t rx_ff_mutex;
-  osal_mutex_def_t tx_ff_mutex;
-#endif
+  OSAL_MUTEX_DEF(rx_ff_mutex);
+  OSAL_MUTEX_DEF(tx_ff_mutex);
 
   // Endpoint Transfer buffer
   CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_VENDOR_EPSIZE];
@@ -171,25 +171,49 @@ uint32_t tud_vendor_n_write_available (uint8_t itf)
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void vendord_init(void)
-{
+void vendord_init(void) {
   tu_memclr(_vendord_itf, sizeof(_vendord_itf));
 
-  for(uint8_t i=0; i<CFG_TUD_VENDOR; i++)
-  {
+  for(uint8_t i=0; i<CFG_TUD_VENDOR; i++) {
     vendord_interface_t* p_itf = &_vendord_itf[i];
 
     // config fifo
     tu_fifo_config(&p_itf->rx_ff, p_itf->rx_ff_buf, CFG_TUD_VENDOR_RX_BUFSIZE, 1, false);
     tu_fifo_config(&p_itf->tx_ff, p_itf->tx_ff_buf, CFG_TUD_VENDOR_TX_BUFSIZE, 1, false);
 
-#if CFG_FIFO_MUTEX
-    tu_fifo_config_mutex(&p_itf->rx_ff, NULL, osal_mutex_create(&p_itf->rx_ff_mutex));
-    tu_fifo_config_mutex(&p_itf->tx_ff, osal_mutex_create(&p_itf->tx_ff_mutex), NULL);
-#endif
+    #if OSAL_MUTEX_REQUIRED
+    osal_mutex_t mutex_rd = osal_mutex_create(&p_itf->rx_ff_mutex);
+    osal_mutex_t mutex_wr = osal_mutex_create(&p_itf->tx_ff_mutex);
+    TU_ASSERT(mutex_rd && mutex_wr,);
+
+    tu_fifo_config_mutex(&p_itf->rx_ff, NULL, mutex_rd);
+    tu_fifo_config_mutex(&p_itf->tx_ff, mutex_wr, NULL);
+    #endif
   }
 }
 
+bool vendord_deinit(void) {
+  #if OSAL_MUTEX_REQUIRED
+  for(uint8_t i=0; i<CFG_TUD_VENDOR; i++) {
+    vendord_interface_t* p_itf = &_vendord_itf[i];
+    osal_mutex_t mutex_rd = p_itf->rx_ff.mutex_rd;
+    osal_mutex_t mutex_wr = p_itf->tx_ff.mutex_wr;
+
+    if (mutex_rd) {
+      osal_mutex_delete(mutex_rd);
+      tu_fifo_config_mutex(&p_itf->rx_ff, NULL, NULL);
+    }
+
+    if (mutex_wr) {
+      osal_mutex_delete(mutex_wr);
+      tu_fifo_config_mutex(&p_itf->tx_ff, NULL, NULL);
+    }
+  }
+  #endif
+
+  return true;
+}
+
 void vendord_reset(uint8_t rhport)
 {
   (void) rhport;
@@ -251,7 +275,6 @@ uint16_t vendord_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, ui
 
 bool vendord_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
 {
-  (void) rhport;
   (void) result;
 
   uint8_t itf = 0;
@@ -278,7 +301,18 @@ bool vendord_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint
   {
     if (tud_vendor_tx_cb) tud_vendor_tx_cb(itf, (uint16_t) xferred_bytes);
     // Send complete, try to send more if possible
-    tud_vendor_n_write_flush(itf);
+    if ( 0 == tud_vendor_n_write_flush(itf) )
+    {
+      // If there is no data left, a ZLP should be sent if
+      // xferred_bytes is multiple of EP Packet size and not zero
+      if ( !tu_fifo_count(&p_itf->tx_ff) && xferred_bytes && (0 == (xferred_bytes & (BULK_PACKET_SIZE-1))) )
+      {
+        if ( usbd_edpt_claim(rhport, p_itf->ep_in) )
+        {
+          usbd_edpt_xfer(rhport, p_itf->ep_in, NULL, 0);
+        }
+      }
+    }
   }
 
   return true;
diff --git a/src/class/vendor/vendor_device.h b/src/class/vendor/vendor_device.h
index d239406b4..cd69ec7c6 100644
--- a/src/class/vendor/vendor_device.h
+++ b/src/class/vendor/vendor_device.h
@@ -139,6 +139,7 @@ static inline uint32_t tud_vendor_write_available (void)
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     vendord_init(void);
+bool     vendord_deinit(void);
 void     vendord_reset(uint8_t rhport);
 uint16_t vendord_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     vendord_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
diff --git a/src/class/video/video.h b/src/class/video/video.h
index d9880c291..b8a9b6369 100644
--- a/src/class/video/video.h
+++ b/src/class/video/video.h
@@ -29,6 +29,10 @@
 
 #include "common/tusb_common.h"
 
+enum {
+  VIDEO_BCD_1_50 = 0x0150,
+};
+
 // Table 3-19 Color Matching Descriptor
 typedef enum {
   VIDEO_COLOR_PRIMARIES_UNDEFINED = 0x00,
@@ -156,22 +160,23 @@ typedef enum {
 /* A.9.1 VideoControl Interface Control Selectors */
 typedef enum {
   VIDEO_VC_CTL_UNDEFINED = 0x00,
-  VIDEO_VC_CTL_VIDEO_POWER_MODE,
-  VIDEO_VC_CTL_REQUEST_ERROR_CODE,
+  VIDEO_VC_CTL_VIDEO_POWER_MODE,   // 0x01
+  VIDEO_VC_CTL_REQUEST_ERROR_CODE, // 0x02
 } video_interface_control_selector_t;
 
 /* A.9.8 VideoStreaming Interface Control Selectors */
 typedef enum {
   VIDEO_VS_CTL_UNDEFINED = 0x00,
-  VIDEO_VS_CTL_PROBE,
-  VIDEO_VS_CTL_COMMIT,
-  VIDEO_VS_CTL_STILL_PROBE,
-  VIDEO_VS_CTL_STILL_COMMIT,
-  VIDEO_VS_CTL_STILL_IMAGE_TRIGGER,
-  VIDEO_VS_CTL_STREAM_ERROR_CODE,
-  VIDEO_VS_CTL_GENERATE_KEY_FRAME,
-  VIDEO_VS_CTL_UPDATE_FRAME_SEGMENT,
-  VIDEO_VS_CTL_SYNCH_DELAY_CONTROL,
+  VIDEO_VS_CTL_PROBE,                // 0x01
+  VIDEO_VS_CTL_COMMIT,               // 0x02
+  VIDEO_VS_CTL_STILL_PROBE,          // 0x03
+  VIDEO_VS_CTL_STILL_COMMIT,         // 0x04
+  VIDEO_VS_CTL_STILL_IMAGE_TRIGGER,  // 0x05
+  VIDEO_VS_CTL_STREAM_ERROR_CODE,    // 0x06
+  VIDEO_VS_CTL_GENERATE_KEY_FRAME,   // 0x07
+  VIDEO_VS_CTL_UPDATE_FRAME_SEGMENT, // 0x08
+  VIDEO_VS_CTL_SYNCH_DELAY_CONTROL,  // 0x09
+
 } video_interface_streaming_selector_t;
 
 /* B. Terminal Types */
@@ -198,55 +203,98 @@ typedef enum {
 } video_terminal_type_t;
 
 //--------------------------------------------------------------------+
-// Descriptors
+// Video Control (VC) Descriptors
 //--------------------------------------------------------------------+
 
 /* 2.3.4.2 */
+#define tusb_desc_video_control_header_nitf_t(_nitf) \
+  struct TU_ATTR_PACKED { \
+    uint8_t  bLength; \
+    uint8_t  bDescriptorType; \
+    uint8_t  bDescriptorSubType; \
+    uint16_t bcdUVC; \
+    uint16_t wTotalLength; \
+    uint32_t dwClockFrequency; /* deprecated */ \
+    uint8_t  bInCollection; \
+    uint8_t  baInterfaceNr[_nitf]; \
+  }
+
+typedef tusb_desc_video_control_header_nitf_t()  tusb_desc_video_control_header_t;
+typedef tusb_desc_video_control_header_nitf_t(1) tusb_desc_video_control_header_1itf_t;
+typedef tusb_desc_video_control_header_nitf_t(2) tusb_desc_video_control_header_2itf_t;
+typedef tusb_desc_video_control_header_nitf_t(3) tusb_desc_video_control_header_3itf_t;
+typedef tusb_desc_video_control_header_nitf_t(4) tusb_desc_video_control_header_4itf_t;
+
 typedef struct TU_ATTR_PACKED {
   uint8_t  bLength;
   uint8_t  bDescriptorType;
   uint8_t  bDescriptorSubType;
-  uint16_t bcdUVC;
-  uint16_t wTotalLength;
-  uint32_t dwClockFrequency;
-  uint8_t  bInCollection;
-  uint8_t  baInterfaceNr[];
-} tusb_desc_cs_video_ctl_itf_hdr_t;
+  uint8_t  bTerminalID;
+  uint16_t wTerminalType;
+  uint8_t  bAssocTerminal;
+  uint8_t  iTerminal;
+} tusb_desc_video_control_input_terminal_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_control_input_terminal_t) == 8, "size is not correct");
 
-/* 2.4.3.3 */
 typedef struct TU_ATTR_PACKED {
-  uint8_t bHeaderLength;
-  union {
-    uint8_t bmHeaderInfo;
-    struct {
-      uint8_t FrameID:              1;
-      uint8_t EndOfFrame:           1;
-      uint8_t PresentationTime:     1;
-      uint8_t SourceClockReference: 1;
-      uint8_t PayloadSpecific:      1;
-      uint8_t StillImage:           1;
-      uint8_t Error:                1;
-      uint8_t EndOfHeader:          1;
-    };
-  };
-} tusb_video_payload_header_t;
+  uint8_t  bLength;
+  uint8_t  bDescriptorType;
+  uint8_t  bDescriptorSubType;
+  uint8_t  bTerminalID;
+  uint16_t wTerminalType;
+  uint8_t  bAssocTerminal;
+  uint8_t  bSourceID;
+  uint8_t  iTerminal;
+} tusb_desc_video_control_output_terminal_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_control_output_terminal_t) == 9, "size is not correct");
 
-/* 3.9.2.1 */
 typedef struct TU_ATTR_PACKED {
   uint8_t  bLength;
   uint8_t  bDescriptorType;
   uint8_t  bDescriptorSubType;
-  uint8_t  bNumFormats;
-  uint16_t wTotalLength;
-  uint8_t  bEndpointAddress;
-  uint8_t  bmInfo;
-  uint8_t  bTerminalLink;
-  uint8_t  bStillCaptureMethod;
-  uint8_t  bTriggerSupport;
-  uint8_t  bTriggerUsage;
+  uint8_t  bTerminalID;
+  uint16_t wTerminalType;
+  uint8_t  bAssocTerminal;
+  uint8_t  iTerminal;
+
+  uint16_t wObjectiveFocalLengthMin;
+  uint16_t wObjectiveFocalLengthMax;
+  uint16_t wOcularFocalLength;
   uint8_t  bControlSize;
-  uint8_t  bmaControls[];
-} tusb_desc_cs_video_stm_itf_in_hdr_t;
+  uint8_t  bmControls[3];
+} tusb_desc_video_control_camera_terminal_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_control_camera_terminal_t) == 18, "size is not correct");
+
+//--------------------------------------------------------------------+
+// Video Streaming (VS) Descriptors
+//--------------------------------------------------------------------+
+
+/* 3.9.2.1 */
+#define tusb_desc_video_streaming_input_header_nbyte_t(_nb) \
+  struct TU_ATTR_PACKED { \
+    uint8_t  bLength; \
+    uint8_t  bDescriptorType; \
+    uint8_t  bDescriptorSubType; \
+    uint8_t  bNumFormats; /* Number of video payload Format descriptors for this interface */ \
+    uint16_t wTotalLength; \
+    uint8_t  bEndpointAddress; \
+    uint8_t  bmInfo; /* Bit 0: dynamic format change supported */ \
+    uint8_t  bTerminalLink; \
+    uint8_t  bStillCaptureMethod; \
+    uint8_t  bTriggerSupport; /* Hardware trigger supported */ \
+    uint8_t  bTriggerUsage; \
+    uint8_t  bControlSize; /* sizeof of each control item */ \
+    uint8_t  bmaControls[_nb]; \
+  }
+
+typedef tusb_desc_video_streaming_input_header_nbyte_t() tusb_desc_video_streaming_input_header_t;
+typedef tusb_desc_video_streaming_input_header_nbyte_t(1) tusb_desc_video_streaming_input_header_1byte_t;
+typedef tusb_desc_video_streaming_input_header_nbyte_t(2) tusb_desc_video_streaming_input_header_2byte_t;
+typedef tusb_desc_video_streaming_input_header_nbyte_t(3) tusb_desc_video_streaming_input_header_3byte_t;
+typedef tusb_desc_video_streaming_input_header_nbyte_t(4) tusb_desc_video_streaming_input_header_4byte_t;
 
 /* 3.9.2.2 */
 typedef struct TU_ATTR_PACKED {
@@ -259,7 +307,7 @@ typedef struct TU_ATTR_PACKED {
   uint8_t  bTerminalLink;
   uint8_t  bControlSize;
   uint8_t  bmaControls[];
-} tusb_desc_cs_video_stm_itf_out_hdr_t;
+} tusb_desc_video_streaming_output_header_t;
 
 typedef struct TU_ATTR_PACKED {
   uint8_t  bLength;
@@ -285,14 +333,33 @@ typedef struct TU_ATTR_PACKED {
       uint8_t  bmaControls[];
     } output;
   };
-} tusb_desc_cs_video_stm_itf_hdr_t;
+} tusb_desc_video_streaming_inout_header_t;
 
+// 3.9.2.6 Color Matching Descriptor
+typedef struct TU_ATTR_PACKED {
+  uint8_t  bLength;
+  uint8_t  bDescriptorType;
+  uint8_t  bDescriptorSubType;
+  uint8_t  bColorPrimaries;
+  uint8_t  bTransferCharacteristics;
+  uint8_t  bMatrixCoefficients;
+} tusb_desc_video_streaming_color_matching_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_streaming_color_matching_t) == 6, "size is not correct");
+
+//--------------------------------------------------------------------+
+// Format and Frame Descriptor
+// Note: bFormatIndex & bFrameIndex are 1-based index
+//--------------------------------------------------------------------+
+
+//------------- Uncompressed -------------//
+// Uncompressed payload specs: 3.1.1 format descriptor
 typedef struct TU_ATTR_PACKED {
   uint8_t bLength;
   uint8_t bDescriptorType;
   uint8_t bDescriptorSubType;
   uint8_t bFormatIndex;
-  uint8_t bNumFrameDescriptors;
+  uint8_t bNumFrameDescriptors; // Number of frame descriptors for this format
   uint8_t guidFormat[16];
   uint8_t bBitsPerPixel;
   uint8_t bDefaultFrameIndex;
@@ -300,22 +367,69 @@ typedef struct TU_ATTR_PACKED {
   uint8_t bAspectRatioY;
   uint8_t bmInterlaceFlags;
   uint8_t bCopyProtect;
-} tusb_desc_cs_video_fmt_uncompressed_t;
+} tusb_desc_video_format_uncompressed_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_format_uncompressed_t) == 27, "size is not correct");
+
+// Uncompressed payload specs: 3.1.2 frame descriptor
+#define tusb_desc_video_frame_uncompressed_nint_t(_nint) \
+  struct TU_ATTR_PACKED { \
+    uint8_t  bLength; \
+    uint8_t  bDescriptorType; \
+    uint8_t  bDescriptorSubType; \
+    uint8_t  bFrameIndex; \
+    uint8_t  bmCapabilities; \
+    uint16_t wWidth; \
+    uint16_t wHeight; \
+    uint32_t dwMinBitRate; \
+    uint32_t dwMaxBitRate; \
+    uint32_t dwMaxVideoFrameBufferSize; /* deprecated in 1.5 */ \
+    uint32_t dwDefaultFrameInterval; /* 100ns unit */\
+    uint8_t  bFrameIntervalType; \
+    uint32_t dwFrameInterval[_nint]; \
+  }
 
+typedef tusb_desc_video_frame_uncompressed_nint_t() tusb_desc_video_frame_uncompressed_t;
+typedef tusb_desc_video_frame_uncompressed_nint_t(1) tusb_desc_video_frame_uncompressed_1int_t;
+typedef tusb_desc_video_frame_uncompressed_nint_t(2) tusb_desc_video_frame_uncompressed_2int_t;
+typedef tusb_desc_video_frame_uncompressed_nint_t(3) tusb_desc_video_frame_uncompressed_3int_t;
+typedef tusb_desc_video_frame_uncompressed_nint_t(4) tusb_desc_video_frame_uncompressed_4int_t;
+
+// continuous = 3 intervals: min, max, step
+typedef tusb_desc_video_frame_uncompressed_3int_t tusb_desc_video_frame_uncompressed_continuous_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_frame_uncompressed_continuous_t) == 38, "size is not correct");
+
+//------------- MJPEG -------------//
+// MJPEG payload specs: 3.1.1 format descriptor
 typedef struct TU_ATTR_PACKED {
   uint8_t bLength;
   uint8_t bDescriptorType;
   uint8_t bDescriptorSubType;
   uint8_t bFormatIndex;
   uint8_t bNumFrameDescriptors;
-  uint8_t bmFlags;
+  uint8_t bmFlags; // Bit 0: fixed size samples (1 = yes)
   uint8_t bDefaultFrameIndex;
   uint8_t bAspectRatioX;
   uint8_t bAspectRatioY;
   uint8_t bmInterlaceFlags;
   uint8_t bCopyProtect;
-} tusb_desc_cs_video_fmt_mjpeg_t;
+} tusb_desc_video_format_mjpeg_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_format_mjpeg_t) == 11, "size is not correct");
+
+// MJPEG payload specs: 3.1.2 frame descriptor (same as uncompressed)
+typedef tusb_desc_video_frame_uncompressed_t tusb_desc_video_frame_mjpeg_t;
+typedef tusb_desc_video_frame_uncompressed_1int_t tusb_desc_video_frame_mjpeg_1int_t;
+typedef tusb_desc_video_frame_uncompressed_2int_t tusb_desc_video_frame_mjpeg_2int_t;
+typedef tusb_desc_video_frame_uncompressed_3int_t tusb_desc_video_frame_mjpeg_3int_t;
+typedef tusb_desc_video_frame_uncompressed_4int_t tusb_desc_video_frame_mjpeg_4int_t;
+
+// continuous = 3 intervals: min, max, step
+typedef tusb_desc_video_frame_mjpeg_3int_t tusb_desc_video_frame_mjpeg_continuous_t;
 
+//------------- DV -------------//
+// DV payload specs: 3.1.1
 typedef struct TU_ATTR_PACKED {
   uint8_t  bLength;
   uint8_t  bDescriptorType;
@@ -323,8 +437,9 @@ typedef struct TU_ATTR_PACKED {
   uint8_t  bFormatIndex;
   uint32_t dwMaxVideoFrameBufferSize; /* deprecated */
   uint8_t  bFormatType;
-} tusb_desc_cs_video_fmt_dv_t;
+} tusb_desc_video_format_dv_t;
 
+// Frame Based payload specs: 3.1.1
 typedef struct TU_ATTR_PACKED {
   uint8_t bLength;
   uint8_t bDescriptorType;
@@ -339,25 +454,7 @@ typedef struct TU_ATTR_PACKED {
   uint8_t bmInterlaceFlags;
   uint8_t bCopyProtect;
   uint8_t bVaribaleSize;
-} tusb_desc_cs_video_fmt_frame_based_t;
-
-typedef struct TU_ATTR_PACKED {
-  uint8_t  bLength;
-  uint8_t  bDescriptorType;
-  uint8_t  bDescriptorSubType;
-  uint8_t  bFrameIndex;
-  uint8_t  bmCapabilities;
-  uint16_t wWidth;
-  uint16_t wHeight;
-  uint32_t dwMinBitRate;
-  uint32_t dwMaxBitRate;
-  uint32_t dwMaxVideoFrameBufferSize; /* deprecated */
-  uint32_t dwDefaultFrameInterval;
-  uint8_t  bFrameIntervalType;
-  uint32_t dwFrameInterval[];
-} tusb_desc_cs_video_frm_uncompressed_t;
-
-typedef tusb_desc_cs_video_frm_uncompressed_t tusb_desc_cs_video_frm_mjpeg_t;
+} tusb_desc_video_format_framebased_t;
 
 typedef struct TU_ATTR_PACKED {
   uint8_t  bLength;
@@ -373,12 +470,30 @@ typedef struct TU_ATTR_PACKED {
   uint8_t  bFrameIntervalType;
   uint32_t dwBytesPerLine;
   uint32_t dwFrameInterval[];
-} tusb_desc_cs_video_frm_frame_based_t;
+} tusb_desc_video_frame_framebased_t;
 
 //--------------------------------------------------------------------+
 // Requests
 //--------------------------------------------------------------------+
 
+/* 2.4.3.3 */
+typedef struct TU_ATTR_PACKED {
+  uint8_t bHeaderLength;
+  union {
+    uint8_t bmHeaderInfo;
+    struct {
+      uint8_t FrameID:              1;
+      uint8_t EndOfFrame:           1;
+      uint8_t PresentationTime:     1;
+      uint8_t SourceClockReference: 1;
+      uint8_t PayloadSpecific:      1;
+      uint8_t StillImage:           1;
+      uint8_t Error:                1;
+      uint8_t EndOfHeader:          1;
+    };
+  };
+} tusb_video_payload_header_t;
+
 /* 4.3.1.1 */
 typedef struct TU_ATTR_PACKED {
   union {
@@ -537,7 +652,7 @@ TU_VERIFY_STATIC( sizeof(video_probe_and_commit_control_t) == 48, "size is not c
 /* Motion-JPEG 3.1.1 Table 3-2 and 3-4 */
 #define TUD_VIDEO_DESC_CS_VS_FRM_MJPEG_DISC(_frmidx, _cap, _width, _height, _minbr, _maxbr, _maxfrmbufsz, _frminterval, ...) \
   TUD_VIDEO_DESC_CS_VS_FRM_MJPEG_DISC_LEN + (TU_ARGS_NUM(__VA_ARGS__)) * 4, \
-  TUSB_DESC_CS_INTERFACE, VIDEO_CS_VS_INTERFACE_FRAME_MJPEG, \
+  TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VS_FRAME_MJPEG, \
   _frmidx, _cap, U16_TO_U8S_LE(_width), U16_TO_U8S_LE(_height), U32_TO_U8S_LE(_minbr), U32_TO_U8S_LE(_maxbr), \
   U32_TO_U8S_LE(_maxfrmbufsz), U32_TO_U8S_LE(_frminterval), (TU_ARGS_NUM(__VA_ARGS__)), __VA_ARGS__
 
diff --git a/src/class/video/video_device.c b/src/class/video/video_device.c
index 3b29454a3..4218835aa 100644
--- a/src/class/video/video_device.c
+++ b/src/class/video/video_device.c
@@ -50,17 +50,17 @@
 
 typedef struct {
   tusb_desc_interface_t            std;
-  tusb_desc_cs_video_ctl_itf_hdr_t ctl;
+  tusb_desc_video_control_header_t ctl;
 } tusb_desc_vc_itf_t;
 
 typedef struct {
   tusb_desc_interface_t            std;
-  tusb_desc_cs_video_stm_itf_hdr_t stm;
+  tusb_desc_video_streaming_inout_header_t stm;
 } tusb_desc_vs_itf_t;
 
 typedef union {
-  tusb_desc_cs_video_ctl_itf_hdr_t ctl;
-  tusb_desc_cs_video_stm_itf_hdr_t stm;
+  tusb_desc_video_control_header_t ctl;
+  tusb_desc_video_streaming_inout_header_t stm;
 } tusb_desc_video_itf_hdr_t;
 
 typedef struct TU_ATTR_PACKED {
@@ -78,9 +78,9 @@ typedef union {
     uint8_t bFormatIndex;
     uint8_t bNumFrameDescriptors;
   };
-  tusb_desc_cs_video_fmt_uncompressed_t uncompressed;
-  tusb_desc_cs_video_fmt_mjpeg_t        mjpeg;
-  tusb_desc_cs_video_fmt_frame_based_t  frame_based;
+  tusb_desc_video_format_uncompressed_t uncompressed;
+  tusb_desc_video_format_mjpeg_t        mjpeg;
+  tusb_desc_video_format_framebased_t  frame_based;
 } tusb_desc_cs_video_fmt_t;
 
 typedef union {
@@ -93,9 +93,9 @@ typedef union {
     uint16_t wWidth;
     uint16_t wHeight;
   };
-  tusb_desc_cs_video_frm_uncompressed_t uncompressed;
-  tusb_desc_cs_video_frm_mjpeg_t        mjpeg;
-  tusb_desc_cs_video_frm_frame_based_t  frame_based;
+  tusb_desc_video_frame_uncompressed_t uncompressed;
+  tusb_desc_video_frame_mjpeg_t        mjpeg;
+  tusb_desc_video_frame_framebased_t  frame_based;
 } tusb_desc_cs_video_frm_t;
 
 /* video streaming interface */
@@ -114,6 +114,8 @@ typedef struct TU_ATTR_PACKED {
   uint32_t max_payload_transfer_size;
   uint8_t  error_code;/* error code */
   uint8_t  state;    /* 0:probing 1:committed 2:streaming */
+
+  video_probe_and_commit_control_t probe_commit_payload; /* Probe and Commit control */
   /*------------- From this point, data is not cleared by bus reset -------------*/
   CFG_TUSB_MEM_ALIGN uint8_t ep_buf[CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE]; /* EP transfer buffer for streaming */
 } videod_streaming_interface_t;
@@ -143,13 +145,65 @@ CFG_TUD_MEM_SECTION tu_static videod_streaming_interface_t _videod_streaming_itf
 tu_static uint8_t const _cap_get     = 0x1u; /* support for GET */
 tu_static uint8_t const _cap_get_set = 0x3u; /* support for GET and SET */
 
+//--------------------------------------------------------------------+
+// Debug
+//--------------------------------------------------------------------+
+#if CFG_TUSB_DEBUG >= CFG_TUD_VIDEO_LOG_LEVEL
+
+static tu_lookup_entry_t const tu_lookup_video_request[] = {
+    {.key = VIDEO_REQUEST_UNDEFINED, .data = "Undefined"},
+    {.key = VIDEO_REQUEST_SET_CUR, .data = "SetCur"},
+    {.key = VIDEO_REQUEST_SET_CUR_ALL, .data = "SetCurAll"},
+    {.key = VIDEO_REQUEST_GET_CUR, .data = "GetCur"},
+    {.key = VIDEO_REQUEST_GET_MIN, .data = "GetMin"},
+    {.key = VIDEO_REQUEST_GET_MAX, .data = "GetMax"},
+    {.key = VIDEO_REQUEST_GET_RES, .data = "GetRes"},
+    {.key = VIDEO_REQUEST_GET_LEN, .data = "GetLen"},
+    {.key = VIDEO_REQUEST_GET_INFO, .data = "GetInfo"},
+    {.key = VIDEO_REQUEST_GET_DEF, .data = "GetDef"},
+    {.key = VIDEO_REQUEST_GET_CUR_ALL, .data = "GetCurAll"},
+    {.key = VIDEO_REQUEST_GET_MIN_ALL, .data = "GetMinAll"},
+    {.key = VIDEO_REQUEST_GET_MAX_ALL, .data = "GetMaxAll"},
+    {.key = VIDEO_REQUEST_GET_RES_ALL, .data = "GetResAll"},
+    {.key = VIDEO_REQUEST_GET_DEF_ALL, .data = "GetDefAll"},
+};
+
+static tu_lookup_table_t const tu_table_video_request = {
+    .count = TU_ARRAY_SIZE(tu_lookup_video_request),
+    .items = tu_lookup_video_request
+};
+
+static char const* const tu_str_video_vc_control_selector[] = {
+    "Undefined",
+    "Video Power Mode",
+    "Request Error Code",
+};
+
+static char const* const tu_str_video_vs_control_selector[] = {
+    "Undefined",
+    "Probe",
+    "Commit",
+    "Still Probe",
+    "Still Commit",
+    "Still Image Trigger",
+    "Stream Error Code",
+    "Generate Key Frame",
+    "Update Frame Segment",
+    "Sync Delay",
+};
+
+#endif
+
+//--------------------------------------------------------------------+
+//
+//--------------------------------------------------------------------+
+
 /** Get interface number from the interface descriptor
  *
  * @param[in] desc    interface descriptor
  *
  * @return bInterfaceNumber */
-static inline uint8_t _desc_itfnum(void const *desc)
-{
+static inline uint8_t _desc_itfnum(void const *desc) {
   return ((uint8_t const*)desc)[2];
 }
 
@@ -158,8 +212,7 @@ static inline uint8_t _desc_itfnum(void const *desc)
  * @param[in] desc    endpoint descriptor
  *
  * @return bEndpointAddress */
-static inline uint8_t _desc_ep_addr(void const *desc)
-{
+static inline uint8_t _desc_ep_addr(void const *desc) {
   return ((uint8_t const*)desc)[2];
 }
 
@@ -169,8 +222,7 @@ static inline uint8_t _desc_ep_addr(void const *desc)
  * @param[in] stm_idx    index number of streaming interface
  *
  * @return instance */
-static videod_streaming_interface_t* _get_instance_streaming(uint_fast8_t ctl_idx, uint_fast8_t stm_idx)
-{
+static videod_streaming_interface_t* _get_instance_streaming(uint_fast8_t ctl_idx, uint_fast8_t stm_idx) {
   videod_interface_t *ctl = &_videod_itf[ctl_idx];
   if (!ctl->beg) return NULL;
   videod_streaming_interface_t *stm = &_videod_streaming_itf[ctl->stm[stm_idx]];
@@ -178,13 +230,11 @@ static videod_streaming_interface_t* _get_instance_streaming(uint_fast8_t ctl_id
   return stm;
 }
 
-static tusb_desc_vc_itf_t const* _get_desc_vc(videod_interface_t const *self)
-{
+static tusb_desc_vc_itf_t const* _get_desc_vc(videod_interface_t const *self) {
   return (tusb_desc_vc_itf_t const *)(self->beg + self->cur);
 }
 
-static tusb_desc_vs_itf_t const* _get_desc_vs(videod_streaming_interface_t const *self)
-{
+static tusb_desc_vs_itf_t const* _get_desc_vs(videod_streaming_interface_t const *self) {
   if (!self->desc.cur) return NULL;
   uint8_t const *desc = _videod_itf[self->index_vc].beg;
   return (tusb_desc_vs_itf_t const*)(desc + self->desc.cur);
@@ -198,8 +248,7 @@ static tusb_desc_vs_itf_t const* _get_desc_vs(videod_streaming_interface_t const
  *
  * @return The pointer for interface descriptor.
  * @retval end   did not found interface descriptor */
-static void const* _find_desc(void const *beg, void const *end, uint_fast8_t desc_type)
-{
+static void const* _find_desc(void const *beg, void const *end, uint_fast8_t desc_type) {
   void const *cur = beg;
   while ((cur < end) && (desc_type != tu_desc_type(cur))) {
     cur = tu_desc_next(cur);
@@ -207,6 +256,24 @@ static void const* _find_desc(void const *beg, void const *end, uint_fast8_t des
   return cur;
 }
 
+/** Find the first descriptor of two given types
+ *
+ * @param[in] beg        The head of descriptor byte array.
+ * @param[in] end        The tail of descriptor byte array.
+ * @param[in] desc_type_0 The first target descriptor type.
+ * @param[in] desc_type_1 The second target descriptor type.
+ *
+ * @return The pointer for interface descriptor.
+ * @retval end   did not found interface descriptor */
+static void const* _find_desc_2_type(void const *beg, void const *end, uint_fast8_t desc_type_0, uint_fast8_t desc_type_1)
+{
+  void const *cur = beg;
+  while ((cur < end) && (desc_type_0 != tu_desc_type(cur)) && (desc_type_1 != tu_desc_type(cur))) {
+    cur = tu_desc_next(cur);
+  }
+  return cur;
+}
+
 /** Find the first descriptor specified by the arguments
  *
  * @param[in] beg        The head of descriptor byte array.
@@ -220,8 +287,7 @@ static void const* _find_desc(void const *beg, void const *end, uint_fast8_t des
 static void const* _find_desc_3(void const *beg, void const *end,
                                 uint_fast8_t desc_type,
                                 uint_fast8_t element_0,
-                                uint_fast8_t element_1)
-{
+                                uint_fast8_t element_1) {
   for (void const *cur = beg; cur < end; cur = _find_desc(cur, end, desc_type)) {
     uint8_t const *p = (uint8_t const *)cur;
     if ((p[2] == element_0) && (p[3] == element_1)) {
@@ -233,19 +299,22 @@ static void const* _find_desc_3(void const *beg, void const *end,
 }
 
 /** Return the next interface descriptor which has another interface number.
+ *  If there are multiple VC interfaces, there will be an IAD descriptor before
+ *  the next interface descriptor. Check both the IAD descriptor and the interface
+ *  descriptor.
+ *  3.1 Descriptor Layout Overview
  *
  * @param[in] beg     The head of descriptor byte array.
  * @param[in] end     The tail of descriptor byte array.
  *
  * @return The pointer for interface descriptor.
  * @retval end   did not found interface descriptor */
-static void const* _next_desc_itf(void const *beg, void const *end)
-{
+static void const* _next_desc_itf(void const *beg, void const *end) {
   void const *cur = beg;
   uint_fast8_t itfnum = ((tusb_desc_interface_t const*)cur)->bInterfaceNumber;
   while ((cur < end) &&
          (itfnum == ((tusb_desc_interface_t const*)cur)->bInterfaceNumber)) {
-    cur = _find_desc(tu_desc_next(cur), end, TUSB_DESC_INTERFACE);
+    cur = _find_desc_2_type(tu_desc_next(cur), end, TUSB_DESC_INTERFACE, TUSB_DESC_INTERFACE_ASSOCIATION);
   }
   return cur;
 }
@@ -391,8 +460,10 @@ static bool _update_streaming_parameters(videod_streaming_interface_t const *stm
     case VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED:
       param->wCompQuality = 1; /* 1 to 10000 */
       break;
-  case VIDEO_CS_ITF_VS_FORMAT_MJPEG:
+
+    case VIDEO_CS_ITF_VS_FORMAT_MJPEG:
       break;
+
     default: return false;
   }
 
@@ -413,9 +484,11 @@ static bool _update_streaming_parameters(videod_streaming_interface_t const *stm
       case VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED:
         frame_size = (uint_fast32_t)frm->wWidth * frm->wHeight * fmt->uncompressed.bBitsPerPixel / 8;
         break;
+
       case VIDEO_CS_ITF_VS_FORMAT_MJPEG:
         frame_size = (uint_fast32_t)frm->wWidth * frm->wHeight * 16 / 8; /* YUV422 */
         break;
+
       default: break;
     }
     param->dwMaxVideoFrameSize = frame_size;
@@ -434,8 +507,9 @@ static bool _update_streaming_parameters(videod_streaming_interface_t const *stm
   uint_fast32_t interval_ms = interval / 10000;
   TU_ASSERT(interval_ms);
   uint_fast32_t payload_size = (frame_size + interval_ms - 1) / interval_ms + 2;
-  if (CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE < payload_size)
+  if (CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE < payload_size) {
     payload_size = CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE;
+  }
   param->dwMaxPayloadTransferSize = payload_size;
   return true;
 }
@@ -455,10 +529,12 @@ static bool _negotiate_streaming_parameters(videod_streaming_interface_t const *
         if (_get_desc_vs(stm))
           param->bFormatIndex = _get_desc_vs(stm)->stm.bNumFormats;
         break;
+
       case VIDEO_REQUEST_GET_MIN:
       case VIDEO_REQUEST_GET_DEF:
         param->bFormatIndex = 1;
         break;
+
       default: return false;
     }
     /* Set the parameters determined by the format  */
@@ -487,18 +563,22 @@ static bool _negotiate_streaming_parameters(videod_streaming_interface_t const *
       case VIDEO_REQUEST_GET_MAX:
         frmnum = fmt->bNumFrameDescriptors;
         break;
+
       case VIDEO_REQUEST_GET_MIN:
         frmnum = 1;
         break;
+
       case VIDEO_REQUEST_GET_DEF:
         switch (fmt->bDescriptorSubType) {
-        case VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED:
-          frmnum = fmt->uncompressed.bDefaultFrameIndex;
-          break;
-        case VIDEO_CS_ITF_VS_FORMAT_MJPEG:
-          frmnum = fmt->mjpeg.bDefaultFrameIndex;
-          break;
-        default: return false;
+          case VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED:
+            frmnum = fmt->uncompressed.bDefaultFrameIndex;
+            break;
+
+          case VIDEO_CS_ITF_VS_FORMAT_MJPEG:
+            frmnum = fmt->mjpeg.bDefaultFrameIndex;
+            break;
+
+          default: return false;
         }
         break;
       default: return false;
@@ -511,9 +591,11 @@ static bool _negotiate_streaming_parameters(videod_streaming_interface_t const *
       case VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED:
         frame_size = (uint_fast32_t)frm->wWidth * frm->wHeight * fmt->uncompressed.bBitsPerPixel / 8;
         break;
+
       case VIDEO_CS_ITF_VS_FORMAT_MJPEG:
         frame_size = (uint_fast32_t)frm->wWidth * frm->wHeight * 16 / 8; /* YUV422 */
         break;
+
       default: return false;
     }
     param->dwMaxVideoFrameSize = frame_size;
@@ -529,41 +611,43 @@ static bool _negotiate_streaming_parameters(videod_streaming_interface_t const *
 
     uint_fast32_t interval, interval_ms;
     switch (request) {
-      case VIDEO_REQUEST_GET_MAX:
-        {
-          uint_fast32_t min_interval, max_interval;
-          uint_fast8_t num_intervals = frm->uncompressed.bFrameIntervalType;
-          max_interval = num_intervals ? frm->uncompressed.dwFrameInterval[num_intervals - 1]: frm->uncompressed.dwFrameInterval[1];
-          min_interval = frm->uncompressed.dwFrameInterval[0];
-          interval = max_interval;
-          interval_ms = min_interval / 10000;
-        }
+      case VIDEO_REQUEST_GET_MAX: {
+        uint_fast32_t min_interval, max_interval;
+        uint_fast8_t num_intervals = frm->uncompressed.bFrameIntervalType;
+        max_interval = num_intervals ? frm->uncompressed.dwFrameInterval[num_intervals - 1]: frm->uncompressed.dwFrameInterval[1];
+        min_interval = frm->uncompressed.dwFrameInterval[0];
+        interval = max_interval;
+        interval_ms = min_interval / 10000;
         break;
-      case VIDEO_REQUEST_GET_MIN:
-        {
-          uint_fast32_t min_interval, max_interval;
-          uint_fast8_t num_intervals = frm->uncompressed.bFrameIntervalType;
-          max_interval = num_intervals ? frm->uncompressed.dwFrameInterval[num_intervals - 1]: frm->uncompressed.dwFrameInterval[1];
-          min_interval = frm->uncompressed.dwFrameInterval[0];
-          interval = min_interval;
-          interval_ms = max_interval / 10000;
-        }
+      }
+
+      case VIDEO_REQUEST_GET_MIN: {
+        uint_fast32_t min_interval, max_interval;
+        uint_fast8_t num_intervals = frm->uncompressed.bFrameIntervalType;
+        max_interval = num_intervals ? frm->uncompressed.dwFrameInterval[num_intervals - 1]: frm->uncompressed.dwFrameInterval[1];
+        min_interval = frm->uncompressed.dwFrameInterval[0];
+        interval = min_interval;
+        interval_ms = max_interval / 10000;
         break;
+      }
+
       case VIDEO_REQUEST_GET_DEF:
         interval = frm->uncompressed.dwDefaultFrameInterval;
         interval_ms = interval / 10000;
         break;
-      case VIDEO_REQUEST_GET_RES:
-        {
-          uint_fast8_t num_intervals = frm->uncompressed.bFrameIntervalType;
-          if (num_intervals) {
-            interval = 0;
-          } else {
-            interval = frm->uncompressed.dwFrameInterval[2];
-            interval_ms = interval / 10000;
-          }
+
+      case VIDEO_REQUEST_GET_RES: {
+        uint_fast8_t num_intervals = frm->uncompressed.bFrameIntervalType;
+        if (num_intervals) {
+          interval = 0;
+          interval_ms = 0;
+        } else {
+          interval = frm->uncompressed.dwFrameInterval[2];
+          interval_ms = interval / 10000;
         }
         break;
+      }
+
       default: return false;
     }
     param->dwFrameInterval = interval;
@@ -577,8 +661,9 @@ static bool _negotiate_streaming_parameters(videod_streaming_interface_t const *
       } else {
         payload_size = (frame_size + interval_ms - 1) / interval_ms + 2;
       }
-      if (CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE < payload_size)
+      if (CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE < payload_size) {
         payload_size = CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE;
+      }
       param->dwMaxPayloadTransferSize = payload_size;
     }
     return true;
@@ -651,13 +736,11 @@ static bool _open_vc_itf(uint8_t rhport, videod_interface_t *self, uint_fast8_t
   return true;
 }
 
-static bool _init_vs_configuration(videod_streaming_interface_t *stm)
-{
+static bool _init_vs_configuration(videod_streaming_interface_t *stm) {
   /* initialize streaming settings */
   stm->state = VS_STATE_PROBING;
   stm->max_payload_transfer_size = 0;
-  video_probe_and_commit_control_t *param =
-    (video_probe_and_commit_control_t *)&stm->ep_buf;
+  video_probe_and_commit_control_t *param = &stm->probe_commit_payload;
   tu_memclr(param, sizeof(*param));
   return _update_streaming_parameters(stm, param);
 }
@@ -672,15 +755,20 @@ static bool _open_vs_itf(uint8_t rhport, videod_streaming_interface_t *stm, uint
   TU_LOG_DRV("    reopen VS %d\r\n", altnum);
   uint8_t const *desc = _videod_itf[stm->index_vc].beg;
 
+#ifndef TUP_DCD_EDPT_ISO_ALLOC
   /* Close endpoints of previous settings. */
   for (i = 0; i < TU_ARRAY_SIZE(stm->desc.ep); ++i) {
     uint_fast16_t ofs_ep = stm->desc.ep[i];
     if (!ofs_ep) break;
-    uint8_t  ep_adr = _desc_ep_addr(desc + ofs_ep);
-    usbd_edpt_close(rhport, ep_adr);
-    stm->desc.ep[i] = 0;
-    TU_LOG_DRV("    close EP%02x\r\n", ep_adr);
+    tusb_desc_endpoint_t const *ep = (tusb_desc_endpoint_t const*)(desc + ofs_ep);
+    /* Only ISO endpoints needs to be closed */
+    if(ep->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS) {
+      stm->desc.ep[i] = 0;
+      usbd_edpt_close(rhport, ep->bEndpointAddress);
+      TU_LOG_DRV("    close EP%02x\r\n", ep->bEndpointAddress);
+    }
   }
+#endif
 
   /* clear transfer management information */
   stm->buffer  = NULL;
@@ -705,16 +793,18 @@ static bool _open_vs_itf(uint8_t rhport, videod_streaming_interface_t *stm, uint
     TU_ASSERT(cur < end);
     tusb_desc_endpoint_t const *ep = (tusb_desc_endpoint_t const*)cur;
     uint_fast32_t max_size = stm->max_payload_transfer_size;
-    if (altnum) {
-      if ((TUSB_XFER_ISOCHRONOUS == ep->bmAttributes.xfer) &&
-          (tu_edpt_packet_size(ep) < max_size)) {
-        /* FS must be less than or equal to max packet size */
-        return false;
-      }
+    if (altnum && (TUSB_XFER_ISOCHRONOUS == ep->bmAttributes.xfer)) {
+      /* FS must be less than or equal to max packet size */
+      TU_VERIFY (tu_edpt_packet_size(ep) >= max_size);
+#ifdef TUP_DCD_EDPT_ISO_ALLOC
+      usbd_edpt_iso_activate(rhport, ep);
+#else
+      TU_ASSERT(usbd_edpt_open(rhport, ep));
+#endif
     } else {
       TU_VERIFY(TUSB_XFER_BULK == ep->bmAttributes.xfer);
+      TU_ASSERT(usbd_edpt_open(rhport, ep));
     }
-    TU_ASSERT(usbd_edpt_open(rhport, ep));
     stm->desc.ep[i] = (uint16_t) (cur - desc);
     TU_LOG_DRV("    open EP%02x\r\n", _desc_ep_addr(cur));
   }
@@ -734,6 +824,7 @@ static uint_fast16_t _prepare_in_payload(videod_streaming_interface_t *stm)
   if (hdr_len + remaining < pkt_len) {
     pkt_len = hdr_len + remaining;
   }
+  TU_ASSERT(pkt_len >= hdr_len);
   uint_fast16_t data_len = pkt_len - hdr_len;
   memcpy(&stm->ep_buf[hdr_len], stm->buffer + stm->offset, data_len);
   stm->offset += data_len;
@@ -750,6 +841,7 @@ static int handle_video_ctl_std_req(uint8_t rhport, uint8_t stage,
                                     tusb_control_request_t const *request,
                                     uint_fast8_t ctl_idx)
 {
+  TU_LOG_DRV("\r\n");
   switch (request->bRequest) {
     case TUSB_REQ_GET_INTERFACE:
       if (stage == CONTROL_STAGE_SETUP)
@@ -787,7 +879,10 @@ static int handle_video_ctl_cs_req(uint8_t rhport, uint8_t stage,
   videod_interface_t *self = &_videod_itf[ctl_idx];
 
   /* 4.2.1 Interface Control Request */
-  switch (TU_U16_HIGH(request->wValue)) {
+  uint8_t const ctrl_sel = TU_U16_HIGH(request->wValue);
+  TU_LOG_DRV("%s_Control(%s)\r\n",  tu_str_video_vc_control_selector[ctrl_sel], tu_lookup_find(&tu_table_video_request, request->bRequest));
+
+  switch (ctrl_sel) {
     case VIDEO_VC_CTL_VIDEO_POWER_MODE:
       switch (request->bRequest) {
         case VIDEO_REQUEST_SET_CUR:
@@ -851,19 +946,19 @@ static int handle_video_ctl_req(uint8_t rhport, uint8_t stage,
                                 tusb_control_request_t const *request,
                                 uint_fast8_t ctl_idx)
 {
-  uint_fast8_t entity_id;
   switch (request->bmRequestType_bit.type) {
     case TUSB_REQ_TYPE_STANDARD:
       return handle_video_ctl_std_req(rhport, stage, request, ctl_idx);
 
-    case TUSB_REQ_TYPE_CLASS:
-      entity_id = TU_U16_HIGH(request->wIndex);
+    case TUSB_REQ_TYPE_CLASS: {
+      uint_fast8_t entity_id = TU_U16_HIGH(request->wIndex);
       if (!entity_id) {
         return handle_video_ctl_cs_req(rhport, stage, request, ctl_idx);
       } else {
         TU_VERIFY(_find_desc_entity(_get_desc_vc(&_videod_itf[ctl_idx]), entity_id), VIDEO_ERROR_INVALID_REQUEST);
         return VIDEO_ERROR_NONE;
       }
+    }
 
     default:
       return VIDEO_ERROR_INVALID_REQUEST;
@@ -874,6 +969,7 @@ static int handle_video_stm_std_req(uint8_t rhport, uint8_t stage,
                                     tusb_control_request_t const *request,
                                     uint_fast8_t stm_idx)
 {
+  TU_LOG_DRV("\r\n");
   videod_streaming_interface_t *self = &_videod_streaming_itf[stm_idx];
   switch (request->bRequest) {
     case TUSB_REQ_GET_INTERFACE:
@@ -889,8 +985,7 @@ static int handle_video_stm_std_req(uint8_t rhport, uint8_t stage,
       return VIDEO_ERROR_NONE;
 
     case TUSB_REQ_SET_INTERFACE:
-      if (stage == CONTROL_STAGE_SETUP)
-      {
+      if (stage == CONTROL_STAGE_SETUP) {
         TU_VERIFY(_open_vs_itf(rhport, self, request->wValue), VIDEO_ERROR_UNKNOWN);
         tud_control_status(rhport, request);
       }
@@ -904,26 +999,26 @@ static int handle_video_stm_std_req(uint8_t rhport, uint8_t stage,
 
 static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage,
                                    tusb_control_request_t const *request,
-                                   uint_fast8_t stm_idx)
-{
+                                   uint_fast8_t stm_idx) {
   (void)rhport;
   videod_streaming_interface_t *self = &_videod_streaming_itf[stm_idx];
 
+  uint8_t const ctrl_sel = TU_U16_HIGH(request->wValue);
+  TU_LOG_DRV("%s_Control(%s)\r\n", tu_str_video_vs_control_selector[ctrl_sel], tu_lookup_find(&tu_table_video_request, request->bRequest));
+
   /* 4.2.1 Interface Control Request */
-  switch (TU_U16_HIGH(request->wValue)) {
+  switch (ctrl_sel) {
     case VIDEO_VS_CTL_STREAM_ERROR_CODE:
       switch (request->bRequest) {
         case VIDEO_REQUEST_GET_CUR:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             /* TODO */
             TU_VERIFY(tud_control_xfer(rhport, request, &self->error_code, sizeof(uint8_t)), VIDEO_ERROR_UNKNOWN);
           }
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_INFO:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)(uintptr_t) &_cap_get, sizeof(_cap_get)), VIDEO_ERROR_UNKNOWN);
           }
           return VIDEO_ERROR_NONE;
@@ -935,25 +1030,23 @@ static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage,
     case VIDEO_VS_CTL_PROBE:
       if (self->state != VS_STATE_PROBING) {
         self->state = VS_STATE_PROBING;
-        _init_vs_configuration(self);
       }
+
       switch (request->bRequest) {
         case VIDEO_REQUEST_SET_CUR:
           if (stage == CONTROL_STAGE_SETUP) {
-            TU_VERIFY(sizeof(video_probe_and_commit_control_t) >= request->wLength, VIDEO_ERROR_UNKNOWN);
-            TU_VERIFY(tud_control_xfer(rhport, request, self->ep_buf, sizeof(video_probe_and_commit_control_t)),
+            TU_VERIFY(tud_control_xfer(rhport, request, &self->probe_commit_payload, sizeof(video_probe_and_commit_control_t)),
                       VIDEO_ERROR_UNKNOWN);
           } else if (stage == CONTROL_STAGE_DATA) {
-            TU_VERIFY(_update_streaming_parameters(self, (video_probe_and_commit_control_t*)self->ep_buf),
+            TU_VERIFY(_update_streaming_parameters(self, &self->probe_commit_payload),
                       VIDEO_ERROR_INVALID_VALUE_WITHIN_RANGE);
           }
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_CUR:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(request->wLength, VIDEO_ERROR_UNKNOWN);
-            TU_VERIFY(tud_control_xfer(rhport, request, self->ep_buf, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN);
+            TU_VERIFY(tud_control_xfer(rhport, request, &self->probe_commit_payload, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN);
           }
           return VIDEO_ERROR_NONE;
 
@@ -961,19 +1054,16 @@ static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage,
         case VIDEO_REQUEST_GET_MAX:
         case VIDEO_REQUEST_GET_RES:
         case VIDEO_REQUEST_GET_DEF:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(request->wLength, VIDEO_ERROR_UNKNOWN);
-            video_probe_and_commit_control_t tmp;
-            tmp = *(video_probe_and_commit_control_t*)&self->ep_buf;
+            video_probe_and_commit_control_t tmp = self->probe_commit_payload;
             TU_VERIFY(_negotiate_streaming_parameters(self, request->bRequest, &tmp), VIDEO_ERROR_INVALID_VALUE_WITHIN_RANGE);
             TU_VERIFY(tud_control_xfer(rhport, request, &tmp, sizeof(tmp)), VIDEO_ERROR_UNKNOWN);
           }
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_LEN:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(2 == request->wLength, VIDEO_ERROR_UNKNOWN);
             uint16_t len = sizeof(video_probe_and_commit_control_t);
             TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)&len, sizeof(len)), VIDEO_ERROR_UNKNOWN);
@@ -981,8 +1071,7 @@ static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage,
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_INFO:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(1 == request->wLength, VIDEO_ERROR_UNKNOWN);
             TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)(uintptr_t)&_cap_get_set, sizeof(_cap_get_set)), VIDEO_ERROR_UNKNOWN);
           }
@@ -996,10 +1085,9 @@ static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage,
       switch (request->bRequest) {
         case VIDEO_REQUEST_SET_CUR:
           if (stage == CONTROL_STAGE_SETUP) {
-            TU_VERIFY(sizeof(video_probe_and_commit_control_t) >= request->wLength, VIDEO_ERROR_UNKNOWN);
-            TU_VERIFY(tud_control_xfer(rhport, request, self->ep_buf, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN);
+            TU_VERIFY(tud_control_xfer(rhport, request, &self->probe_commit_payload, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN);
           } else if (stage == CONTROL_STAGE_DATA) {
-            video_probe_and_commit_control_t *param = (video_probe_and_commit_control_t*)self->ep_buf;
+            video_probe_and_commit_control_t *param = &self->probe_commit_payload;
             TU_VERIFY(_update_streaming_parameters(self, param), VIDEO_ERROR_INVALID_VALUE_WITHIN_RANGE);
             /* Set the negotiated value */
             self->max_payload_transfer_size = param->dwMaxPayloadTransferSize;
@@ -1021,16 +1109,14 @@ static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage,
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_CUR:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(request->wLength, VIDEO_ERROR_UNKNOWN);
-            TU_VERIFY(tud_control_xfer(rhport, request, self->ep_buf, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN);
+            TU_VERIFY(tud_control_xfer(rhport, request, &self->probe_commit_payload, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN);
           }
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_LEN:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(2 == request->wLength, VIDEO_ERROR_UNKNOWN);
             uint16_t len = sizeof(video_probe_and_commit_control_t);
             TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)&len, sizeof(len)), VIDEO_ERROR_UNKNOWN);
@@ -1038,8 +1124,7 @@ static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage,
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_INFO:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(1 == request->wLength, VIDEO_ERROR_UNKNOWN);
             TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)(uintptr_t) &_cap_get_set, sizeof(_cap_get_set)), VIDEO_ERROR_UNKNOWN);
           }
@@ -1101,6 +1186,16 @@ bool tud_video_n_streaming(uint_fast8_t ctl_idx, uint_fast8_t stm_idx)
   videod_streaming_interface_t *stm = _get_instance_streaming(ctl_idx, stm_idx);
   if (!stm || !stm->desc.ep[0]) return false;
   if (stm->state == VS_STATE_PROBING) return false;
+
+#ifdef TUP_DCD_EDPT_ISO_ALLOC
+  uint8_t const *desc = _videod_itf[stm->index_vc].beg;
+  uint_fast16_t ofs_ep = stm->desc.ep[0];
+  tusb_desc_endpoint_t const *ep = (tusb_desc_endpoint_t const*)(desc + ofs_ep);
+  if (ep->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS) {
+    if (stm->state == VS_STATE_COMMITTED) return false;
+  }
+#endif
+
   return true;
 }
 
@@ -1140,8 +1235,7 @@ bool tud_video_n_frame_xfer(uint_fast8_t ctl_idx, uint_fast8_t stm_idx, void *bu
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void videod_init(void)
-{
+void videod_init(void) {
   for (uint_fast8_t i = 0; i < CFG_TUD_VIDEO; ++i) {
     videod_interface_t* ctl = &_videod_itf[i];
     tu_memclr(ctl, sizeof(*ctl));
@@ -1152,8 +1246,11 @@ void videod_init(void)
   }
 }
 
-void videod_reset(uint8_t rhport)
-{
+bool videod_deinit(void) {
+  return true;
+}
+
+void videod_reset(uint8_t rhport) {
   (void) rhport;
   for (uint_fast8_t i = 0; i < CFG_TUD_VIDEO; ++i) {
     videod_interface_t* ctl = &_videod_itf[i];
@@ -1165,8 +1262,7 @@ void videod_reset(uint8_t rhport)
   }
 }
 
-uint16_t videod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len)
-{
+uint16_t videod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len) {
   TU_VERIFY((TUSB_CLASS_VIDEO       == itf_desc->bInterfaceClass) &&
             (VIDEO_SUBCLASS_CONTROL == itf_desc->bInterfaceSubClass) &&
             (VIDEO_ITF_PROTOCOL_15  == itf_desc->bInterfaceProtocol), 0);
@@ -1208,12 +1304,30 @@ uint16_t videod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
     cur = _next_desc_itf(cur, end);
     stm->desc.end = (uint16_t) ((uintptr_t)cur - (uintptr_t)itf_desc);
     stm->state = VS_STATE_PROBING;
+#ifdef TUP_DCD_EDPT_ISO_ALLOC
+    /* Allocate ISO endpoints */
+    uint16_t ep_size = 0;
+    uint16_t ep_addr = 0;
+    uint8_t const *p_desc = (uint8_t const*)itf_desc + stm->desc.beg;
+    uint8_t const *p_desc_end = (uint8_t const*)itf_desc + stm->desc.end;
+    while (p_desc < p_desc_end) {
+      if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT) {
+        tusb_desc_endpoint_t const *desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+        if (desc_ep->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS) {
+              ep_addr = desc_ep->bEndpointAddress;
+              ep_size = TU_MAX(tu_edpt_packet_size(desc_ep), ep_size);
+        }
+      }
+      p_desc = tu_desc_next(p_desc);
+    }
+    if(ep_addr > 0 && ep_size > 0) usbd_edpt_iso_alloc(rhport, ep_addr, ep_size);
+#endif
     if (0 == stm_idx && 1 == bInCollection) {
       /* If there is only one streaming interface and no alternate settings,
        * host may not issue set_interface so open the streaming interface here. */
       uint8_t const *sbeg = (uint8_t const*)itf_desc + stm->desc.beg;
       uint8_t const *send = (uint8_t const*)itf_desc + stm->desc.end;
-      if (end == _find_desc_itf(sbeg, send, _desc_itfnum(sbeg), 1)) {
+      if (send == _find_desc_itf(sbeg, send, _desc_itfnum(sbeg), 1)) {
         TU_VERIFY(_open_vs_itf(rhport, stm, 0), 0);
       }
     }
@@ -1225,8 +1339,7 @@ uint16_t videod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
 // 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 videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
-{
+bool videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) {
   int err;
   TU_VERIFY(request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE);
   uint_fast8_t itfnum = tu_u16_low(request->wIndex);
@@ -1239,6 +1352,7 @@ bool videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_
   }
 
   if (itf < CFG_TUD_VIDEO) {
+    TU_LOG_DRV("  VC[%d]: ", itf);
     err = handle_video_ctl_req(rhport, stage, request, itf);
     _videod_itf[itf].error_code = (uint8_t)err;
     if (err) return false;
@@ -1254,6 +1368,7 @@ bool videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_
   }
 
   if (itf < CFG_TUD_VIDEO_STREAMING) {
+    TU_LOG_DRV("  VS[%d]: ", itf);
     err = handle_video_stm_req(rhport, stage, request, itf);
     _videod_streaming_itf[itf].error_code = (uint8_t)err;
     if (err) return false;
@@ -1262,8 +1377,7 @@ bool videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_
   return false;
 }
 
-bool videod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
+bool videod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
   (void)result; (void)xferred_bytes;
 
   /* find streaming handle */
diff --git a/src/class/video/video_device.h b/src/class/video/video_device.h
index ee2fcb9d5..92930c013 100644
--- a/src/class/video/video_device.h
+++ b/src/class/video/video_device.h
@@ -85,6 +85,7 @@ TU_ATTR_WEAK int tud_video_commit_cb(uint_fast8_t ctl_idx, uint_fast8_t stm_idx,
 // INTERNAL USBD-CLASS DRIVER API
 //--------------------------------------------------------------------+
 void     videod_init           (void);
+bool     videod_deinit         (void);
 void     videod_reset          (uint8_t rhport);
 uint16_t videod_open           (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
diff --git a/src/common/tusb_common.h b/src/common/tusb_common.h
index caeb5f2ef..0d4082c03 100644
--- a/src/common/tusb_common.h
+++ b/src/common/tusb_common.h
@@ -37,6 +37,7 @@
 #define TU_ARRAY_SIZE(_arr)   ( sizeof(_arr) / sizeof(_arr[0]) )
 #define TU_MIN(_x, _y)        ( ( (_x) < (_y) ) ? (_x) : (_y) )
 #define TU_MAX(_x, _y)        ( ( (_x) > (_y) ) ? (_x) : (_y) )
+#define TU_DIV_CEIL(n, d)     (((n) + (d) - 1) / (d))
 
 #define TU_U16(_high, _low)   ((uint16_t) (((_high) << 8) | (_low)))
 #define TU_U16_HIGH(_u16)     ((uint8_t) (((_u16) >> 8) & 0x00ff))
@@ -64,6 +65,7 @@
 // Standard Headers
 #include <stdbool.h>
 #include <stdint.h>
+#include <inttypes.h>
 #include <stddef.h>
 #include <string.h>
 #include <stdio.h>
diff --git a/src/common/tusb_compiler.h b/src/common/tusb_compiler.h
index 6f07bdd53..0d5570b1c 100644
--- a/src/common/tusb_compiler.h
+++ b/src/common/tusb_compiler.h
@@ -56,7 +56,7 @@
 #elif defined (__STDC_VERSION__) && __STDC_VERSION__ >= 201112L
   #define TU_VERIFY_STATIC   _Static_assert
 #elif defined(__CCRX__)
-  #define TU_VERIFY_STATIC(const_expr, _mess) typedef char TU_XSTRCAT(Line, __LINE__)[(const_expr) ? 1 : 0];
+  #define TU_VERIFY_STATIC(const_expr, _mess) typedef char TU_XSTRCAT(_verify_static_, _TU_COUNTER_)[(const_expr) ? 1 : 0];
 #else
   #define TU_VERIFY_STATIC(const_expr, _mess) enum { TU_XSTRCAT(_verify_static_, _TU_COUNTER_) = 1/(!!(const_expr)) }
 #endif
diff --git a/src/common/tusb_debug.h b/src/common/tusb_debug.h
index 0f4dc93f3..2e9f1d9cd 100644
--- a/src/common/tusb_debug.h
+++ b/src/common/tusb_debug.h
@@ -60,6 +60,7 @@ void tu_print_mem(void const *buf, uint32_t count, uint8_t indent);
 
 static inline void tu_print_buf(uint8_t const* buf, uint32_t bufsize) {
   for(uint32_t i=0; i<bufsize; i++) tu_printf("%02X ", buf[i]);
+  tu_printf("\r\n");
 }
 
 // Log with Level
@@ -76,7 +77,7 @@ static inline void tu_print_buf(uint8_t const* buf, uint32_t bufsize) {
 #define TU_LOG1_MEM           tu_print_mem
 #define TU_LOG1_BUF(_x, _n)   tu_print_buf((uint8_t const*)(_x), _n)
 #define TU_LOG1_INT(_x)       tu_printf(#_x " = %ld\r\n", (unsigned long) (_x) )
-#define TU_LOG1_HEX(_x)       tu_printf(#_x " = %lX\r\n", (unsigned long) (_x) )
+#define TU_LOG1_HEX(_x)       tu_printf(#_x " = 0x%lX\r\n", (unsigned long) (_x) )
 
 // Log Level 2: Warn
 #if CFG_TUSB_DEBUG >= 2
diff --git a/src/common/tusb_fifo.c b/src/common/tusb_fifo.c
index d6c3db4ce..8a0fd4417 100644
--- a/src/common/tusb_fifo.c
+++ b/src/common/tusb_fifo.c
@@ -62,7 +62,9 @@ TU_ATTR_ALWAYS_INLINE static inline void _ff_unlock(osal_mutex_t mutex)
 typedef enum
 {
   TU_FIFO_COPY_INC,            ///< Copy from/to an increasing source/destination address - default mode
+#ifdef TUP_MEM_CONST_ADDR
   TU_FIFO_COPY_CST_FULL_WORDS, ///< Copy from/to a constant source/destination address - required for e.g. STM32 to write into USB hardware FIFO
+#endif
 } tu_fifo_copy_mode_t;
 
 bool tu_fifo_config(tu_fifo_t *f, void* buffer, uint16_t depth, uint16_t item_size, bool overwritable)
@@ -92,6 +94,7 @@ bool tu_fifo_config(tu_fifo_t *f, void* buffer, uint16_t depth, uint16_t item_si
 // Pull & Push
 //--------------------------------------------------------------------+
 
+#ifdef TUP_MEM_CONST_ADDR
 // Intended to be used to read from hardware USB FIFO in e.g. STM32 where all data is read from a constant address
 // Code adapted from dcd_synopsys.c
 // TODO generalize with configurable 1 byte or 4 byte each read
@@ -140,6 +143,7 @@ static void _ff_pull_const_addr(void * app_buf, const uint8_t * ff_buf, uint16_t
     *reg_tx = tmp32;
   }
 }
+#endif
 
 // send one item to fifo WITHOUT updating write pointer
 static inline void _ff_push(tu_fifo_t* f, void const * app_buf, uint16_t rel)
@@ -179,7 +183,7 @@ static void _ff_push_n(tu_fifo_t* f, void const * app_buf, uint16_t n, uint16_t
         memcpy(f->buffer, ((uint8_t const*) app_buf) + lin_bytes, wrap_bytes);
       }
       break;
-
+#ifdef TUP_MEM_CONST_ADDR
     case TU_FIFO_COPY_CST_FULL_WORDS:
       // Intended for hardware buffers from which it can be read word by word only
       if(n <= lin_count)
@@ -224,6 +228,8 @@ static void _ff_push_n(tu_fifo_t* f, void const * app_buf, uint16_t n, uint16_t
         if (wrap_bytes > 0) _ff_push_const_addr(ff_buf, app_buf, wrap_bytes);
       }
       break;
+#endif
+    default: break;
   }
 }
 
@@ -264,7 +270,7 @@ static void _ff_pull_n(tu_fifo_t* f, void* app_buf, uint16_t n, uint16_t rd_ptr,
         memcpy((uint8_t*) app_buf + lin_bytes, f->buffer, wrap_bytes);
       }
     break;
-
+#ifdef TUP_MEM_CONST_ADDR
     case TU_FIFO_COPY_CST_FULL_WORDS:
       if ( n <= lin_count )
       {
@@ -309,6 +315,7 @@ static void _ff_pull_n(tu_fifo_t* f, void* app_buf, uint16_t n, uint16_t rd_ptr,
         // Read data wrapped part
         if (wrap_bytes > 0) _ff_pull_const_addr(app_buf, ff_buf, wrap_bytes);
       }
+#endif
     break;
 
     default: break;
@@ -726,10 +733,29 @@ uint16_t tu_fifo_read_n(tu_fifo_t* f, void * buffer, uint16_t n)
   return _tu_fifo_read_n(f, buffer, n, TU_FIFO_COPY_INC);
 }
 
+#ifdef TUP_MEM_CONST_ADDR
+/******************************************************************************/
+/*!
+    @brief This function will read n elements from the array index specified by
+    the read pointer and increment the read index.
+    This function checks for an overflow and corrects read pointer if required.
+    The dest address will not be incremented which is useful for writing to registers.
+
+    @param[in]  f
+                Pointer to the FIFO buffer to manipulate
+    @param[in]  buffer
+                The pointer to data location
+    @param[in]  n
+                Number of element that buffer can afford
+
+    @returns number of items read from the FIFO
+ */
+/******************************************************************************/
 uint16_t tu_fifo_read_n_const_addr_full_words(tu_fifo_t* f, void * buffer, uint16_t n)
 {
   return _tu_fifo_read_n(f, buffer, n, TU_FIFO_COPY_CST_FULL_WORDS);
 }
+#endif
 
 /******************************************************************************/
 /*!
@@ -838,6 +864,7 @@ uint16_t tu_fifo_write_n(tu_fifo_t* f, const void * data, uint16_t n)
   return _tu_fifo_write_n(f, data, n, TU_FIFO_COPY_INC);
 }
 
+#ifdef TUP_MEM_CONST_ADDR
 /******************************************************************************/
 /*!
     @brief This function will write n elements into the array index specified by
@@ -857,6 +884,7 @@ uint16_t tu_fifo_write_n_const_addr_full_words(tu_fifo_t* f, const void * data,
 {
   return _tu_fifo_write_n(f, data, n, TU_FIFO_COPY_CST_FULL_WORDS);
 }
+#endif
 
 /******************************************************************************/
 /*!
diff --git a/src/common/tusb_fifo.h b/src/common/tusb_fifo.h
index 2f60ec2f4..6c0efb509 100644
--- a/src/common/tusb_fifo.h
+++ b/src/common/tusb_fifo.h
@@ -102,10 +102,8 @@ extern "C" {
  *                  |
  *      -------------------------
  *      | R | 1 | 2 | W | 4 | 5 |
-
  */
-typedef struct
-{
+typedef struct {
   uint8_t* buffer          ; // buffer pointer
   uint16_t depth           ; // max items
 
@@ -124,16 +122,14 @@ typedef struct
 
 } tu_fifo_t;
 
-typedef struct
-{
+typedef struct {
   uint16_t len_lin  ; ///< linear length in item size
   uint16_t len_wrap ; ///< wrapped length in item size
   void * ptr_lin    ; ///< linear part start pointer
   void * ptr_wrap   ; ///< wrapped part start pointer
 } tu_fifo_buffer_info_t;
 
-#define TU_FIFO_INIT(_buffer, _depth, _type, _overwritable) \
-{                                                           \
+#define TU_FIFO_INIT(_buffer, _depth, _type, _overwritable){\
   .buffer               = _buffer,                          \
   .depth                = _depth,                           \
   .item_size            = sizeof(_type),                    \
@@ -144,32 +140,31 @@ typedef struct
     uint8_t _name##_buf[_depth*sizeof(_type)];                                \
     tu_fifo_t _name = TU_FIFO_INIT(_name##_buf, _depth, _type, _overwritable)
 
-
 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);
 
 #if OSAL_MUTEX_REQUIRED
 TU_ATTR_ALWAYS_INLINE static inline
-void tu_fifo_config_mutex(tu_fifo_t *f, osal_mutex_t wr_mutex, osal_mutex_t rd_mutex)
-{
+void tu_fifo_config_mutex(tu_fifo_t *f, osal_mutex_t wr_mutex, osal_mutex_t rd_mutex) {
   f->mutex_wr = wr_mutex;
   f->mutex_rd = rd_mutex;
 }
-
 #else
-
 #define tu_fifo_config_mutex(_f, _wr_mutex, _rd_mutex)
-
 #endif
 
 bool     tu_fifo_write                  (tu_fifo_t* f, void const * p_data);
 uint16_t tu_fifo_write_n                (tu_fifo_t* f, void const * p_data, uint16_t n);
+#ifdef TUP_MEM_CONST_ADDR
 uint16_t tu_fifo_write_n_const_addr_full_words    (tu_fifo_t* f, const void * data, uint16_t n);
+#endif
 
 bool     tu_fifo_read                   (tu_fifo_t* f, void * p_buffer);
 uint16_t tu_fifo_read_n                 (tu_fifo_t* f, void * p_buffer, uint16_t n);
+#ifdef TUP_MEM_CONST_ADDR
 uint16_t tu_fifo_read_n_const_addr_full_words     (tu_fifo_t* f, void * buffer, uint16_t n);
+#endif
 
 bool     tu_fifo_peek                   (tu_fifo_t* f, void * p_buffer);
 uint16_t tu_fifo_peek_n                 (tu_fifo_t* f, void * p_buffer, uint16_t n);
@@ -182,8 +177,7 @@ bool     tu_fifo_overflowed             (tu_fifo_t* f);
 void     tu_fifo_correct_read_pointer   (tu_fifo_t* f);
 
 TU_ATTR_ALWAYS_INLINE static inline
-uint16_t tu_fifo_depth(tu_fifo_t* f)
-{
+uint16_t tu_fifo_depth(tu_fifo_t* f) {
   return f->depth;
 }
 
@@ -198,7 +192,6 @@ void tu_fifo_advance_read_pointer (tu_fifo_t *f, uint16_t n);
 void tu_fifo_get_read_info (tu_fifo_t *f, tu_fifo_buffer_info_t *info);
 void tu_fifo_get_write_info(tu_fifo_t *f, tu_fifo_buffer_info_t *info);
 
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/common/tusb_mcu.h b/src/common/tusb_mcu.h
index ef28ac8fb..e489e93e7 100644
--- a/src/common/tusb_mcu.h
+++ b/src/common/tusb_mcu.h
@@ -100,6 +100,13 @@
   #define TUP_DCD_ENDPOINT_MAX    8
   #define TUP_RHPORT_HIGHSPEED    1
 
+#elif TU_CHECK_MCU(OPT_MCU_MCXA15)
+  // USB0 is chipidea FS
+  #define TUP_USBIP_CHIPIDEA_FS
+  #define TUP_USBIP_CHIPIDEA_FS_MCX
+
+  #define TUP_DCD_ENDPOINT_MAX    16
+
 #elif TU_CHECK_MCU(OPT_MCU_MIMXRT1XXX)
   #define TUP_USBIP_CHIPIDEA_HS
   #define TUP_USBIP_EHCI
@@ -107,7 +114,7 @@
   #define TUP_DCD_ENDPOINT_MAX    8
   #define TUP_RHPORT_HIGHSPEED    1
 
-#elif TU_CHECK_MCU(OPT_MCU_KINETIS_KL, OPT_MCU_KINETIS_K32L)
+#elif TU_CHECK_MCU(OPT_MCU_KINETIS_KL, OPT_MCU_KINETIS_K32L, OPT_MCU_KINETIS_K)
   #define TUP_USBIP_CHIPIDEA_FS
   #define TUP_USBIP_CHIPIDEA_FS_KINETIS
   #define TUP_DCD_ENDPOINT_MAX    16
@@ -324,6 +331,9 @@
   #define TUP_USBIP_DWC2
   #define TUP_DCD_ENDPOINT_MAX    6
 
+#elif TU_CHECK_MCU(OPT_MCU_ESP32) && (CFG_TUD_ENABLED || !(defined(CFG_TUH_MAX3421) && CFG_TUH_MAX3421))
+  #error "MCUs are only supported with CFG_TUH_MAX3421 enabled"
+
 //--------------------------------------------------------------------+
 // Dialog
 //--------------------------------------------------------------------+
@@ -421,7 +431,7 @@
 #define TUP_MCU_MULTIPLE_CORE 0
 #endif
 
-#ifndef TUP_DCD_ENDPOINT_MAX
+#if !defined(TUP_DCD_ENDPOINT_MAX) && defined(CFG_TUD_ENABLED) && CFG_TUD_ENABLED
   #warning "TUP_DCD_ENDPOINT_MAX is not defined for this MCU, default to 8"
   #define TUP_DCD_ENDPOINT_MAX    8
 #endif
@@ -436,4 +446,12 @@
   #define TU_ATTR_FAST_FUNC
 #endif
 
+#if defined(TUP_USBIP_DWC2) || defined(TUP_USBIP_FSDEV)
+  #define TUP_DCD_EDPT_ISO_ALLOC
+#endif
+
+#if defined(TUP_USBIP_DWC2)
+  #define TUP_MEM_CONST_ADDR
+#endif
+
 #endif
diff --git a/src/common/tusb_private.h b/src/common/tusb_private.h
index db1ba974d..373a50256 100644
--- a/src/common/tusb_private.h
+++ b/src/common/tusb_private.h
@@ -60,7 +60,7 @@ typedef struct {
   tu_fifo_t ff;
 
   // mutex: read if ep rx, write if e tx
-  OSAL_MUTEX_DEF(ff_mutex);
+  OSAL_MUTEX_DEF(ff_mutexdef);
 
 }tu_edpt_stream_t;
 
@@ -87,15 +87,17 @@ bool tu_edpt_release(tu_edpt_state_t* ep_state, osal_mutex_t mutex);
 // Endpoint Stream
 //--------------------------------------------------------------------+
 
-// Init an stream, should only be called once
+// Init an endpoint stream
 bool tu_edpt_stream_init(tu_edpt_stream_t* s, bool is_host, bool is_tx, bool overwritable,
                          void* ff_buf, uint16_t ff_bufsize, uint8_t* ep_buf, uint16_t ep_bufsize);
 
+// Deinit an endpoint stream
+bool tu_edpt_stream_deinit(tu_edpt_stream_t* s);
+
 // Open an stream for an endpoint
 // hwid is either device address (host mode) or rhport (device mode)
 TU_ATTR_ALWAYS_INLINE static inline
-void tu_edpt_stream_open(tu_edpt_stream_t* s, uint8_t hwid, tusb_desc_endpoint_t const *desc_ep)
-{
+void tu_edpt_stream_open(tu_edpt_stream_t* s, uint8_t hwid, tusb_desc_endpoint_t const *desc_ep) {
   tu_fifo_clear(&s->ff);
   s->hwid = hwid;
   s->ep_addr = desc_ep->bEndpointAddress;
@@ -103,16 +105,14 @@ void tu_edpt_stream_open(tu_edpt_stream_t* s, uint8_t hwid, tusb_desc_endpoint_t
 }
 
 TU_ATTR_ALWAYS_INLINE static inline
-void tu_edpt_stream_close(tu_edpt_stream_t* s)
-{
+void tu_edpt_stream_close(tu_edpt_stream_t* s) {
   s->hwid = 0;
   s->ep_addr = 0;
 }
 
 // Clear fifo
 TU_ATTR_ALWAYS_INLINE static inline
-bool tu_edpt_stream_clear(tu_edpt_stream_t* s)
-{
+bool tu_edpt_stream_clear(tu_edpt_stream_t* s) {
   return tu_fifo_clear(&s->ff);
 }
 
@@ -131,8 +131,7 @@ bool tu_edpt_stream_write_zlp_if_needed(tu_edpt_stream_t* s, uint32_t last_xferr
 
 // Get the number of bytes available for writing
 TU_ATTR_ALWAYS_INLINE static inline
-uint32_t tu_edpt_stream_write_available(tu_edpt_stream_t* s)
-{
+uint32_t tu_edpt_stream_write_available(tu_edpt_stream_t* s) {
   return (uint32_t) tu_fifo_remaining(&s->ff);
 }
 
diff --git a/src/common/tusb_types.h b/src/common/tusb_types.h
index fab680989..b571f9b72 100644
--- a/src/common/tusb_types.h
+++ b/src/common/tusb_types.h
@@ -24,12 +24,8 @@
  * This file is part of the TinyUSB stack.
  */
 
-/** \ingroup group_usb_definitions
- *  \defgroup USBDef_Type USB Types
- *  @{ */
-
-#ifndef _TUSB_TYPES_H_
-#define _TUSB_TYPES_H_
+#ifndef TUSB_TYPES_H_
+#define TUSB_TYPES_H_
 
 #include <stdbool.h>
 #include <stdint.h>
@@ -44,43 +40,38 @@
  *------------------------------------------------------------------*/
 
 /// defined base on EHCI specs value for Endpoint Speed
-typedef enum
-{
+typedef enum {
   TUSB_SPEED_FULL = 0,
   TUSB_SPEED_LOW  = 1,
   TUSB_SPEED_HIGH = 2,
   TUSB_SPEED_INVALID = 0xff,
-}tusb_speed_t;
+} tusb_speed_t;
 
 /// defined base on USB Specs Endpoint's bmAttributes
-typedef enum
-{
+typedef enum {
   TUSB_XFER_CONTROL = 0 ,
   TUSB_XFER_ISOCHRONOUS ,
   TUSB_XFER_BULK        ,
   TUSB_XFER_INTERRUPT
-}tusb_xfer_type_t;
+} tusb_xfer_type_t;
 
-typedef enum
-{
+typedef enum {
   TUSB_DIR_OUT = 0,
   TUSB_DIR_IN  = 1,
 
   TUSB_DIR_IN_MASK = 0x80
-}tusb_dir_t;
+} tusb_dir_t;
 
-enum
-{
+enum {
   TUSB_EPSIZE_BULK_FS = 64,
-  TUSB_EPSIZE_BULK_HS= 512,
+  TUSB_EPSIZE_BULK_HS = 512,
 
   TUSB_EPSIZE_ISO_FS_MAX = 1023,
   TUSB_EPSIZE_ISO_HS_MAX = 1024,
 };
 
-/// Isochronous End Point Attributes
-typedef enum
-{
+/// Isochronous Endpoint Attributes
+typedef enum {
   TUSB_ISO_EP_ATT_NO_SYNC         = 0x00,
   TUSB_ISO_EP_ATT_ASYNCHRONOUS    = 0x04,
   TUSB_ISO_EP_ATT_ADAPTIVE        = 0x08,
@@ -88,11 +79,10 @@ typedef enum
   TUSB_ISO_EP_ATT_DATA            = 0x00, ///< Data End Point
   TUSB_ISO_EP_ATT_EXPLICIT_FB     = 0x10, ///< Feedback End Point
   TUSB_ISO_EP_ATT_IMPLICIT_FB     = 0x20, ///< Data endpoint that also serves as an implicit feedback
-}tusb_iso_ep_attribute_t;
+} tusb_iso_ep_attribute_t;
 
 /// USB Descriptor Types
-typedef enum
-{
+typedef enum {
   TUSB_DESC_DEVICE                = 0x01,
   TUSB_DESC_CONFIGURATION         = 0x02,
   TUSB_DESC_STRING                = 0x03,
@@ -119,10 +109,9 @@ typedef enum
 
   TUSB_DESC_SUPERSPEED_ENDPOINT_COMPANION     = 0x30,
   TUSB_DESC_SUPERSPEED_ISO_ENDPOINT_COMPANION = 0x31
-}tusb_desc_type_t;
+} tusb_desc_type_t;
 
-typedef enum
-{
+typedef enum {
   TUSB_REQ_GET_STATUS        = 0  ,
   TUSB_REQ_CLEAR_FEATURE     = 1  ,
   TUSB_REQ_RESERVED          = 2  ,
@@ -136,25 +125,22 @@ typedef enum
   TUSB_REQ_GET_INTERFACE     = 10 ,
   TUSB_REQ_SET_INTERFACE     = 11 ,
   TUSB_REQ_SYNCH_FRAME       = 12
-}tusb_request_code_t;
+} tusb_request_code_t;
 
-typedef enum
-{
+typedef enum {
   TUSB_REQ_FEATURE_EDPT_HALT     = 0,
   TUSB_REQ_FEATURE_REMOTE_WAKEUP = 1,
   TUSB_REQ_FEATURE_TEST_MODE     = 2
-}tusb_request_feature_selector_t;
+} tusb_request_feature_selector_t;
 
-typedef enum
-{
+typedef enum {
   TUSB_REQ_TYPE_STANDARD = 0,
   TUSB_REQ_TYPE_CLASS,
   TUSB_REQ_TYPE_VENDOR,
   TUSB_REQ_TYPE_INVALID
 } tusb_request_type_t;
 
-typedef enum
-{
+typedef enum {
   TUSB_REQ_RCPT_DEVICE =0,
   TUSB_REQ_RCPT_INTERFACE,
   TUSB_REQ_RCPT_ENDPOINT,
@@ -162,8 +148,7 @@ typedef enum
 } tusb_request_recipient_t;
 
 // https://www.usb.org/defined-class-codes
-typedef enum
-{
+typedef enum {
   TUSB_CLASS_UNSPECIFIED          = 0    ,
   TUSB_CLASS_AUDIO                = 1    ,
   TUSB_CLASS_CDC                  = 2    ,
@@ -187,26 +172,23 @@ typedef enum
   TUSB_CLASS_MISC                 = 0xEF ,
   TUSB_CLASS_APPLICATION_SPECIFIC = 0xFE ,
   TUSB_CLASS_VENDOR_SPECIFIC      = 0xFF
-}tusb_class_code_t;
+} tusb_class_code_t;
 
 typedef enum
 {
   MISC_SUBCLASS_COMMON = 2
 }misc_subclass_type_t;
 
-typedef enum
-{
+typedef enum {
   MISC_PROTOCOL_IAD = 1
-}misc_protocol_type_t;
+} misc_protocol_type_t;
 
-typedef enum
-{
+typedef enum {
   APP_SUBCLASS_USBTMC = 0x03,
   APP_SUBCLASS_DFU_RUNTIME = 0x01
 } app_subclass_type_t;
 
-typedef enum
-{
+typedef enum {
   DEVICE_CAPABILITY_WIRELESS_USB               = 0x01,
   DEVICE_CAPABILITY_USB20_EXTENSION            = 0x02,
   DEVICE_CAPABILITY_SUPERSPEED_USB             = 0x03,
@@ -223,11 +205,11 @@ typedef enum
   DEVICE_CAPABILITY_AUTHENTICATION             = 0x0E,
   DEVICE_CAPABILITY_BILLBOARD_EX               = 0x0F,
   DEVICE_CAPABILITY_CONFIGURATION_SUMMARY      = 0x10
-}device_capability_type_t;
+} device_capability_type_t;
 
 enum {
-  TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP = TU_BIT(5),
-  TUSB_DESC_CONFIG_ATT_SELF_POWERED  = TU_BIT(6),
+  TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP = 1u << 5,
+  TUSB_DESC_CONFIG_ATT_SELF_POWERED  = 1u << 6,
 };
 
 #define TUSB_DESC_CONFIG_POWER_MA(x)  ((x)/2)
@@ -235,28 +217,25 @@ enum {
 //--------------------------------------------------------------------+
 //
 //--------------------------------------------------------------------+
-typedef enum
-{
+typedef enum {
   XFER_RESULT_SUCCESS = 0,
   XFER_RESULT_FAILED,
   XFER_RESULT_STALLED,
   XFER_RESULT_TIMEOUT,
   XFER_RESULT_INVALID
-}xfer_result_t;
+} xfer_result_t;
 
-enum // TODO remove
-{
+// TODO remove
+enum {
   DESC_OFFSET_LEN  = 0,
   DESC_OFFSET_TYPE = 1
 };
 
-enum
-{
+enum {
   INTERFACE_INVALID_NUMBER = 0xff
 };
 
-typedef enum
-{
+typedef enum {
   MS_OS_20_SET_HEADER_DESCRIPTOR       = 0x00,
   MS_OS_20_SUBSET_HEADER_CONFIGURATION = 0x01,
   MS_OS_20_SUBSET_HEADER_FUNCTION      = 0x02,
@@ -268,16 +247,14 @@ typedef enum
   MS_OS_20_FEATURE_VENDOR_REVISION     = 0x08
 } microsoft_os_20_type_t;
 
-enum
-{
+enum {
   CONTROL_STAGE_IDLE,
   CONTROL_STAGE_SETUP,
   CONTROL_STAGE_DATA,
   CONTROL_STAGE_ACK
 };
 
-enum
-{
+enum {
   TUSB_INDEX_INVALID_8 = 0xFFu
 };
 
@@ -290,15 +267,14 @@ TU_ATTR_PACKED_BEGIN
 TU_ATTR_BIT_FIELD_ORDER_BEGIN
 
 /// USB Device Descriptor
-typedef struct TU_ATTR_PACKED
-{
+typedef struct TU_ATTR_PACKED {
   uint8_t  bLength            ; ///< Size of this descriptor in bytes.
   uint8_t  bDescriptorType    ; ///< DEVICE Descriptor Type.
-  uint16_t bcdUSB             ; ///< BUSB Specification Release Number in Binary-Coded Decimal (i.e., 2.10 is 210H). This field identifies the release of the USB Specification with which the device and its descriptors are compliant.
+  uint16_t bcdUSB             ; ///< BUSB Specification Release Number in Binary-Coded Decimal (i.e., 2.10 is 210H).
 
-  uint8_t  bDeviceClass       ; ///< Class code (assigned by the USB-IF). \li If this field is reset to zero, each interface within a configuration specifies its own class information and the various interfaces operate independently. \li If this field is set to a value between 1 and FEH, the device supports different class specifications on different interfaces and the interfaces may not operate independently. This value identifies the class definition used for the aggregate interfaces. \li If this field is set to FFH, the device class is vendor-specific.
-  uint8_t  bDeviceSubClass    ; ///< Subclass code (assigned by the USB-IF). These codes are qualified by the value of the bDeviceClass field. \li If the bDeviceClass field is reset to zero, this field must also be reset to zero. \li If the bDeviceClass field is not set to FFH, all values are reserved for assignment by the USB-IF.
-  uint8_t  bDeviceProtocol    ; ///< Protocol code (assigned by the USB-IF). These codes are qualified by the value of the bDeviceClass and the bDeviceSubClass fields. If a device supports class-specific protocols on a device basis as opposed to an interface basis, this code identifies the protocols that the device uses as defined by the specification of the device class. \li If this field is reset to zero, the device does not use class-specific protocols on a device basis. However, it may use classspecific protocols on an interface basis. \li If this field is set to FFH, the device uses a vendor-specific protocol on a device basis.
+  uint8_t  bDeviceClass       ; ///< Class code (assigned by the USB-IF).
+  uint8_t  bDeviceSubClass    ; ///< Subclass code (assigned by the USB-IF).
+  uint8_t  bDeviceProtocol    ; ///< Protocol code (assigned by the USB-IF).
   uint8_t  bMaxPacketSize0    ; ///< Maximum packet size for endpoint zero (only 8, 16, 32, or 64 are valid). For HS devices is fixed to 64.
 
   uint16_t idVendor           ; ///< Vendor ID (assigned by the USB-IF).
@@ -314,8 +290,7 @@ typedef struct TU_ATTR_PACKED
 TU_VERIFY_STATIC( sizeof(tusb_desc_device_t) == 18, "size is not correct");
 
 // USB Binary Device Object Store (BOS) Descriptor
-typedef struct TU_ATTR_PACKED
-{
+typedef struct TU_ATTR_PACKED {
   uint8_t  bLength         ; ///< Size of this descriptor in bytes
   uint8_t  bDescriptorType ; ///< CONFIGURATION Descriptor Type
   uint16_t wTotalLength    ; ///< Total length of data returned for this descriptor
@@ -325,8 +300,7 @@ typedef struct TU_ATTR_PACKED
 TU_VERIFY_STATIC( sizeof(tusb_desc_bos_t) == 5, "size is not correct");
 
 /// USB Configuration Descriptor
-typedef struct TU_ATTR_PACKED
-{
+typedef struct TU_ATTR_PACKED {
   uint8_t  bLength             ; ///< Size of this descriptor in bytes
   uint8_t  bDescriptorType     ; ///< CONFIGURATION Descriptor Type
   uint16_t wTotalLength        ; ///< Total length of data returned for this configuration. Includes the combined length of all descriptors (configuration, interface, endpoint, and class- or vendor-specific) returned for this configuration.
@@ -341,8 +315,7 @@ typedef struct TU_ATTR_PACKED
 TU_VERIFY_STATIC( sizeof(tusb_desc_configuration_t) == 9, "size is not correct");
 
 /// USB Interface Descriptor
-typedef struct TU_ATTR_PACKED
-{
+typedef struct TU_ATTR_PACKED {
   uint8_t  bLength            ; ///< Size of this descriptor in bytes
   uint8_t  bDescriptorType    ; ///< INTERFACE Descriptor Type
 
@@ -358,8 +331,7 @@ typedef struct TU_ATTR_PACKED
 TU_VERIFY_STATIC( sizeof(tusb_desc_interface_t) == 9, "size is not correct");
 
 /// USB Endpoint Descriptor
-typedef struct TU_ATTR_PACKED
-{
+typedef struct TU_ATTR_PACKED {
   uint8_t  bLength          ; // Size of this descriptor in bytes
   uint8_t  bDescriptorType  ; // ENDPOINT Descriptor Type
 
@@ -379,8 +351,7 @@ typedef struct TU_ATTR_PACKED
 TU_VERIFY_STATIC( sizeof(tusb_desc_endpoint_t) == 7, "size is not correct");
 
 /// USB Other Speed Configuration Descriptor
-typedef struct TU_ATTR_PACKED
-{
+typedef struct TU_ATTR_PACKED {
   uint8_t  bLength             ; ///< Size of descriptor
   uint8_t  bDescriptorType     ; ///< Other_speed_Configuration Type
   uint16_t wTotalLength        ; ///< Total length of data returned
@@ -393,8 +364,7 @@ typedef struct TU_ATTR_PACKED
 } tusb_desc_other_speed_t;
 
 /// USB Device Qualifier Descriptor
-typedef struct TU_ATTR_PACKED
-{
+typedef struct TU_ATTR_PACKED {
   uint8_t  bLength            ; ///< Size of descriptor
   uint8_t  bDescriptorType    ; ///< Device Qualifier Type
   uint16_t bcdUSB             ; ///< USB specification version number (e.g., 0200H for V2.00)
@@ -411,8 +381,7 @@ typedef struct TU_ATTR_PACKED
 TU_VERIFY_STATIC( sizeof(tusb_desc_device_qualifier_t) == 10, "size is not correct");
 
 /// USB Interface Association Descriptor (IAD ECN)
-typedef struct TU_ATTR_PACKED
-{
+typedef struct TU_ATTR_PACKED {
   uint8_t bLength           ; ///< Size of descriptor
   uint8_t bDescriptorType   ; ///< Other_speed_Configuration Type
 
@@ -426,17 +395,17 @@ typedef struct TU_ATTR_PACKED
   uint8_t iFunction         ; ///< Index of the string descriptor describing the interface association.
 } tusb_desc_interface_assoc_t;
 
+TU_VERIFY_STATIC( sizeof(tusb_desc_interface_assoc_t) == 8, "size is not correct");
+
 // USB String Descriptor
-typedef struct TU_ATTR_PACKED
-{
+typedef struct TU_ATTR_PACKED {
   uint8_t  bLength         ; ///< Size of this descriptor in bytes
   uint8_t  bDescriptorType ; ///< Descriptor Type
   uint16_t unicode_string[];
 } tusb_desc_string_t;
 
 // USB Binary Device Object Store (BOS)
-typedef struct TU_ATTR_PACKED
-{
+typedef struct TU_ATTR_PACKED {
   uint8_t bLength;
   uint8_t bDescriptorType ;
   uint8_t bDevCapabilityType;
@@ -445,9 +414,8 @@ typedef struct TU_ATTR_PACKED
   uint8_t CapabilityData[];
 } tusb_desc_bos_platform_t;
 
-// USB WebuSB URL Descriptor
-typedef struct TU_ATTR_PACKED
-{
+// USB WebUSB URL Descriptor
+typedef struct TU_ATTR_PACKED {
   uint8_t bLength;
   uint8_t bDescriptorType;
   uint8_t bScheme;
@@ -455,8 +423,7 @@ typedef struct TU_ATTR_PACKED
 } tusb_desc_webusb_url_t;
 
 // DFU Functional Descriptor
-typedef struct TU_ATTR_PACKED
-{
+typedef struct TU_ATTR_PACKED {
   uint8_t  bLength;
   uint8_t  bDescriptorType;
 
@@ -481,7 +448,7 @@ typedef struct TU_ATTR_PACKED
 //
 //--------------------------------------------------------------------+
 
-typedef struct TU_ATTR_PACKED{
+typedef struct TU_ATTR_PACKED {
   union {
     struct TU_ATTR_PACKED {
       uint8_t recipient :  5; ///< Recipient type tusb_request_recipient_t.
@@ -500,7 +467,6 @@ typedef struct TU_ATTR_PACKED{
 
 TU_VERIFY_STATIC( sizeof(tusb_control_request_t) == 8, "size is not correct");
 
-
 TU_ATTR_PACKED_END  // End of all packed definitions
 TU_ATTR_BIT_FIELD_ORDER_END
 
@@ -509,36 +475,25 @@ TU_ATTR_BIT_FIELD_ORDER_END
 //--------------------------------------------------------------------+
 
 // Get direction from Endpoint address
-TU_ATTR_ALWAYS_INLINE static inline tusb_dir_t tu_edpt_dir(uint8_t addr)
-{
+TU_ATTR_ALWAYS_INLINE static inline tusb_dir_t tu_edpt_dir(uint8_t addr) {
   return (addr & TUSB_DIR_IN_MASK) ? TUSB_DIR_IN : TUSB_DIR_OUT;
 }
 
 // Get Endpoint number from address
-TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_edpt_number(uint8_t addr)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_edpt_number(uint8_t addr) {
   return (uint8_t)(addr & (~TUSB_DIR_IN_MASK));
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_edpt_addr(uint8_t num, uint8_t dir)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_edpt_addr(uint8_t num, uint8_t dir) {
   return (uint8_t)(num | (dir ? TUSB_DIR_IN_MASK : 0));
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_edpt_packet_size(tusb_desc_endpoint_t const* desc_ep)
-{
-  return tu_le16toh(desc_ep->wMaxPacketSize) & TU_GENMASK(10, 0);
+TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_edpt_packet_size(tusb_desc_endpoint_t const* desc_ep) {
+  return tu_le16toh(desc_ep->wMaxPacketSize) & 0x7FF;
 }
 
 #if CFG_TUSB_DEBUG
-TU_ATTR_ALWAYS_INLINE static inline const char *tu_edpt_dir_str(tusb_dir_t dir)
-{
-  tu_static const char *str[] = {"out", "in"};
-  return str[dir];
-}
-
-TU_ATTR_ALWAYS_INLINE static inline const char *tu_edpt_type_str(tusb_xfer_type_t t)
-{
+TU_ATTR_ALWAYS_INLINE static inline const char *tu_edpt_type_str(tusb_xfer_type_t t) {
   tu_static const char *str[] = {"control", "isochronous", "bulk", "interrupt"};
   return str[t];
 }
@@ -549,21 +504,18 @@ TU_ATTR_ALWAYS_INLINE static inline const char *tu_edpt_type_str(tusb_xfer_type_
 //--------------------------------------------------------------------+
 
 // return next descriptor
-TU_ATTR_ALWAYS_INLINE static inline uint8_t const * tu_desc_next(void const* desc)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint8_t const * tu_desc_next(void const* desc) {
   uint8_t const* desc8 = (uint8_t const*) desc;
   return desc8 + desc8[DESC_OFFSET_LEN];
 }
 
 // get descriptor type
-TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_desc_type(void const* desc)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_desc_type(void const* desc) {
   return ((uint8_t const*) desc)[DESC_OFFSET_TYPE];
 }
 
 // get descriptor length
-TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_desc_len(void const* desc)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_desc_len(void const* desc) {
   return ((uint8_t const*) desc)[DESC_OFFSET_LEN];
 }
 
@@ -580,6 +532,4 @@ uint8_t const * tu_desc_find3(uint8_t const* desc, uint8_t const* end, uint8_t b
  }
 #endif
 
-#endif /* _TUSB_TYPES_H_ */
-
-/** @} */
+#endif // TUSB_TYPES_H_
diff --git a/src/common/tusb_verify.h b/src/common/tusb_verify.h
index 8aa66b4df..dde0550d3 100644
--- a/src/common/tusb_verify.h
+++ b/src/common/tusb_verify.h
@@ -76,14 +76,14 @@
 #endif
 
 // Halt CPU (breakpoint) when hitting error, only apply for Cortex M3, M4, M7, M33. M55
-#if defined(__ARM_ARCH_7M__) || defined (__ARM_ARCH_7EM__) || defined(__ARM_ARCH_8M_MAIN__) || defined(__ARM_ARCH_8_1M_MAIN__)
-  #define TU_BREAKPOINT() do                                                                                \
-  {                                                                                                         \
+#if defined(__ARM_ARCH_7M__) || defined (__ARM_ARCH_7EM__) || defined(__ARM_ARCH_8M_MAIN__) || defined(__ARM_ARCH_8_1M_MAIN__) || \
+    defined(__ARM7M__) || defined (__ARM7EM__) || defined(__ARM8M_MAINLINE__) || defined(__ARM8EM_MAINLINE__)
+  #define TU_BREAKPOINT() do {                                                                              \
     volatile uint32_t* ARM_CM_DHCSR =  ((volatile uint32_t*) 0xE000EDF0UL); /* Cortex M CoreDebug->DHCSR */ \
     if ( (*ARM_CM_DHCSR) & 1UL ) __asm("BKPT #0\n"); /* Only halt mcu if debugger is attached */            \
   } while(0)
 
-#elif defined(__riscv)
+#elif defined(__riscv) && !TUP_MCU_ESPRESSIF
   #define TU_BREAKPOINT() do { __asm("ebreak\n"); } while(0)
 
 #elif defined(_mips)
diff --git a/src/device/dcd.h b/src/device/dcd.h
index 0dca4b9f6..b71b99869 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -131,6 +131,9 @@ void dcd_dcache_clean_invalidate(void const* addr, uint32_t data_size) TU_ATTR_W
 // Initialize controller to device mode
 void dcd_init(uint8_t rhport);
 
+// Deinitialize controller, unset device mode.
+bool dcd_deinit(uint8_t rhport);
+
 // Interrupt Handler
 void dcd_int_handler(uint8_t rhport);
 
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 1da6f0b53..7dfdc8476 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -43,14 +43,23 @@
 #endif
 
 //--------------------------------------------------------------------+
-// Callback weak stubs (called if application does not provide)
+// Weak stubs: invoked if no strong implementation is available
 //--------------------------------------------------------------------+
+TU_ATTR_WEAK bool dcd_deinit(uint8_t rhport) {
+  (void) rhport;
+  return false;
+}
+
 TU_ATTR_WEAK void tud_event_hook_cb(uint8_t rhport, uint32_t eventid, bool in_isr) {
   (void)rhport;
   (void)eventid;
   (void)in_isr;
 }
 
+TU_ATTR_WEAK void tud_sof_cb(uint32_t frame_count) {
+  (void)frame_count;
+}
+
 //--------------------------------------------------------------------+
 // Device Data
 //--------------------------------------------------------------------+
@@ -68,9 +77,10 @@ typedef struct {
     uint8_t remote_wakeup_support : 1; // configuration descriptor's attribute
     uint8_t self_powered          : 1; // configuration descriptor's attribute
   };
-
   volatile uint8_t cfg_num; // current active configuration (0x00 is not configured)
   uint8_t speed;
+  volatile uint8_t setup_count;
+  volatile uint8_t sof_consumer;
 
   uint8_t itf2drv[CFG_TUD_INTERFACE_MAX];   // map interface number to driver (0xff is invalid)
   uint8_t ep2drv[CFG_TUD_ENDPPOINT_MAX][2]; // map endpoint to driver ( 0xff is invalid ), can use only 4-bit each
@@ -85,17 +95,18 @@ tu_static usbd_device_t _usbd_dev;
 // Class Driver
 //--------------------------------------------------------------------+
 #if CFG_TUSB_DEBUG >= CFG_TUD_LOG_LEVEL
-  #define DRIVER_NAME(_name)    .name = _name,
+  #define DRIVER_NAME(_name)  _name
 #else
-  #define DRIVER_NAME(_name)
+  #define DRIVER_NAME(_name)  NULL
 #endif
 
 // Built-in class drivers
 tu_static usbd_class_driver_t const _usbd_driver[] = {
     #if CFG_TUD_CDC
     {
-        DRIVER_NAME("CDC")
+        .name             = DRIVER_NAME("CDC"),
         .init             = cdcd_init,
+        .deinit           = cdcd_deinit,
         .reset            = cdcd_reset,
         .open             = cdcd_open,
         .control_xfer_cb  = cdcd_control_xfer_cb,
@@ -106,8 +117,9 @@ tu_static usbd_class_driver_t const _usbd_driver[] = {
 
     #if CFG_TUD_MSC
     {
-        DRIVER_NAME("MSC")
+        .name             = DRIVER_NAME("MSC"),
         .init             = mscd_init,
+        .deinit           = NULL,
         .reset            = mscd_reset,
         .open             = mscd_open,
         .control_xfer_cb  = mscd_control_xfer_cb,
@@ -118,121 +130,131 @@ tu_static usbd_class_driver_t const _usbd_driver[] = {
 
     #if CFG_TUD_HID
     {
-      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
+        .name             = DRIVER_NAME("HID"),
+        .init             = hidd_init,
+        .deinit           = hidd_deinit,
+        .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")
-      .init             = audiod_init,
-      .reset            = audiod_reset,
-      .open             = audiod_open,
-      .control_xfer_cb  = audiod_control_xfer_cb,
-      .xfer_cb          = audiod_xfer_cb,
-      .sof              = audiod_sof_isr
+        .name             = DRIVER_NAME("AUDIO"),
+        .init             = audiod_init,
+        .deinit           = audiod_deinit,
+        .reset            = audiod_reset,
+        .open             = audiod_open,
+        .control_xfer_cb  = audiod_control_xfer_cb,
+        .xfer_cb          = audiod_xfer_cb,
+        .sof              = audiod_sof_isr
     },
     #endif
 
     #if CFG_TUD_VIDEO
     {
-      DRIVER_NAME("VIDEO")
-      .init             = videod_init,
-      .reset            = videod_reset,
-      .open             = videod_open,
-      .control_xfer_cb  = videod_control_xfer_cb,
-      .xfer_cb          = videod_xfer_cb,
-      .sof              = NULL
+        .name             = DRIVER_NAME("VIDEO"),
+        .init             = videod_init,
+        .deinit           = videod_deinit,
+        .reset            = videod_reset,
+        .open             = videod_open,
+        .control_xfer_cb  = videod_control_xfer_cb,
+        .xfer_cb          = videod_xfer_cb,
+        .sof              = NULL
     },
     #endif
 
     #if CFG_TUD_MIDI
     {
-      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
+        .name             = DRIVER_NAME("MIDI"),
+        .init             = midid_init,
+        .deinit           = midid_deinit,
+        .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_xfer_cb  = tud_vendor_control_xfer_cb,
-      .xfer_cb          = vendord_xfer_cb,
-      .sof              = NULL
+        .name             = DRIVER_NAME("VENDOR"),
+        .init             = vendord_init,
+        .deinit           = vendord_deinit,
+        .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_xfer_cb  = usbtmcd_control_xfer_cb,
-      .xfer_cb          = usbtmcd_xfer_cb,
-      .sof              = NULL
+        .name             = DRIVER_NAME("TMC"),
+        .init             = usbtmcd_init_cb,
+        .deinit           = usbtmcd_deinit,
+        .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_RUNTIME
     {
-      DRIVER_NAME("DFU-RUNTIME")
-      .init             = dfu_rtd_init,
-      .reset            = dfu_rtd_reset,
-      .open             = dfu_rtd_open,
-      .control_xfer_cb  = dfu_rtd_control_xfer_cb,
-      .xfer_cb          = NULL,
-      .sof              = NULL
+        .name             = DRIVER_NAME("DFU-RUNTIME"),
+        .init             = dfu_rtd_init,
+        .deinit           = dfu_rtd_deinit,
+        .reset            = dfu_rtd_reset,
+        .open             = dfu_rtd_open,
+        .control_xfer_cb  = dfu_rtd_control_xfer_cb,
+        .xfer_cb          = NULL,
+        .sof              = NULL
     },
     #endif
 
     #if CFG_TUD_DFU
     {
-      DRIVER_NAME("DFU")
-      .init             = dfu_moded_init,
-      .reset            = dfu_moded_reset,
-      .open             = dfu_moded_open,
-      .control_xfer_cb  = dfu_moded_control_xfer_cb,
-      .xfer_cb          = NULL,
-      .sof              = NULL
+        .name             = DRIVER_NAME("DFU"),
+        .init             = dfu_moded_init,
+        .deinit           = dfu_moded_deinit,
+        .reset            = dfu_moded_reset,
+        .open             = dfu_moded_open,
+        .control_xfer_cb  = dfu_moded_control_xfer_cb,
+        .xfer_cb          = NULL,
+        .sof              = NULL
     },
     #endif
 
     #if CFG_TUD_ECM_RNDIS || CFG_TUD_NCM
     {
-      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,
+        .name             = DRIVER_NAME("NET"),
+        .init             = netd_init,
+        .deinit           = netd_deinit,
+        .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_xfer_cb  = btd_control_xfer_cb,
-      .xfer_cb          = btd_xfer_cb,
-      .sof              = NULL
+        .name             = DRIVER_NAME("BTH"),
+        .init             = btd_init,
+        .deinit           = btd_deinit,
+        .reset            = btd_reset,
+        .open             = btd_open,
+        .control_xfer_cb  = btd_control_xfer_cb,
+        .xfer_cb          = btd_xfer_cb,
+        .sof              = NULL
     },
     #endif
 };
@@ -258,6 +280,7 @@ TU_ATTR_ALWAYS_INLINE static inline usbd_class_driver_t const * get_driver(uint8
   return driver;
 }
 
+
 //--------------------------------------------------------------------+
 // DCD Event
 //--------------------------------------------------------------------+
@@ -279,9 +302,9 @@ tu_static osal_queue_t _usbd_q;
 #endif
 
 TU_ATTR_ALWAYS_INLINE static inline bool queue_event(dcd_event_t const * event, bool in_isr) {
-  bool ret = osal_queue_send(_usbd_q, event, in_isr);
+  TU_ASSERT(osal_queue_send(_usbd_q, event, in_isr));
   tud_event_hook_cb(event->rhport, event->event_id, in_isr);
-  return ret;
+  return true;
 }
 
 //--------------------------------------------------------------------+
@@ -320,7 +343,7 @@ 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 && driver->control_xfer_cb == callback) {
-      TU_LOG_USBD("  %s control complete\r\n", driver->name);
+      TU_LOG_USBD("%s control complete\r\n", driver->name);
       return;
     }
   }
@@ -366,6 +389,12 @@ bool tud_connect(void) {
   return true;
 }
 
+bool tud_sof_cb_enable(bool en)
+{
+  usbd_sof_enable(_usbd_rhport, SOF_CONSUMER_USER, en);
+  return true;
+}
+
 //--------------------------------------------------------------------+
 // USBD Task
 //--------------------------------------------------------------------+
@@ -373,13 +402,13 @@ bool tud_inited(void) {
   return _usbd_rhport != RHPORT_INVALID;
 }
 
-bool tud_init (uint8_t rhport)
-{
+bool tud_init(uint8_t rhport) {
   // skip if already initialized
-  if ( tud_inited() ) return true;
+  if (tud_inited()) return true;
 
   TU_LOG_USBD("USBD init on controller %u\r\n", rhport);
   TU_LOG_INT(CFG_TUD_LOG_LEVEL, sizeof(usbd_device_t));
+  TU_LOG_INT(CFG_TUD_LOG_LEVEL, sizeof(dcd_event_t));
   TU_LOG_INT(CFG_TUD_LOG_LEVEL, sizeof(tu_fifo_t));
   TU_LOG_INT(CFG_TUD_LOG_LEVEL, sizeof(tu_edpt_stream_t));
 
@@ -396,16 +425,14 @@ bool tud_init (uint8_t rhport)
   TU_ASSERT(_usbd_q);
 
   // Get application driver if available
-  if ( usbd_app_driver_get_cb )
-  {
+  if (usbd_app_driver_get_cb) {
     _app_driver = usbd_app_driver_get_cb(&_app_driver_count);
   }
 
   // Init class drivers
-  for (uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++)
-  {
-    usbd_class_driver_t const * driver = get_driver(i);
-    TU_ASSERT(driver);
+  for (uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++) {
+    usbd_class_driver_t const* driver = get_driver(i);
+    TU_ASSERT(driver && driver->init);
     TU_LOG_USBD("%s init\r\n", driver->name);
     driver->init();
   }
@@ -419,31 +446,61 @@ bool tud_init (uint8_t rhport)
   return true;
 }
 
-static void configuration_reset(uint8_t rhport)
-{
-  for ( uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++ )
-  {
-    usbd_class_driver_t const * driver = get_driver(i);
-    TU_ASSERT(driver, );
+bool tud_deinit(uint8_t rhport) {
+  // skip if not initialized
+  if (!tud_inited()) return true;
+
+  TU_LOG_USBD("USBD deinit on controller %u\r\n", rhport);
+
+  // Deinit device controller driver
+  dcd_int_disable(rhport);
+  dcd_disconnect(rhport);
+  dcd_deinit(rhport);
+
+  // Deinit class drivers
+  for (uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++) {
+    usbd_class_driver_t const* driver = get_driver(i);
+    if(driver && driver->deinit) {
+      TU_LOG_USBD("%s deinit\r\n", driver->name);
+      driver->deinit();
+    }
+  }
+
+  // Deinit device queue & task
+  osal_queue_delete(_usbd_q);
+  _usbd_q = NULL;
+
+#if OSAL_MUTEX_REQUIRED
+  // TODO make sure there is no task waiting on this mutex
+  osal_mutex_delete(_usbd_mutex);
+  _usbd_mutex = NULL;
+#endif
+
+  _usbd_rhport = RHPORT_INVALID;
+
+  return true;
+}
+
+static void configuration_reset(uint8_t rhport) {
+  for (uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++) {
+    usbd_class_driver_t const* driver = get_driver(i);
+    TU_ASSERT(driver,);
     driver->reset(rhport);
   }
 
   tu_varclr(&_usbd_dev);
   memset(_usbd_dev.itf2drv, DRVID_INVALID, sizeof(_usbd_dev.itf2drv)); // invalid mapping
-  memset(_usbd_dev.ep2drv , DRVID_INVALID, sizeof(_usbd_dev.ep2drv )); // invalid mapping
+  memset(_usbd_dev.ep2drv, DRVID_INVALID, sizeof(_usbd_dev.ep2drv)); // invalid mapping
 }
 
-static void usbd_reset(uint8_t rhport)
-{
+static void usbd_reset(uint8_t rhport) {
   configuration_reset(rhport);
   usbd_control_reset();
 }
 
-bool tud_task_event_ready(void)
-{
+bool tud_task_event_ready(void) {
   // Skip if stack is not initialized
-  if ( !tud_inited() ) return false;
-
+  if (!tud_inited()) return false;
   return !osal_queue_empty(_usbd_q);
 }
 
@@ -451,57 +508,52 @@ bool tud_task_event_ready(void)
  * This top level thread manages all device controller event and delegates events to class-specific drivers.
  * This should be called periodically within the mainloop or rtos thread.
  *
-   @code
-    int main(void)
-    {
+    int main(void) {
       application_init();
       tusb_init();
 
-      while(1) // the mainloop
-      {
+      while(1) { // the mainloop
         application_code();
         tud_task(); // tinyusb device task
       }
     }
-    @endcode
  */
-void tud_task_ext(uint32_t timeout_ms, bool in_isr)
-{
+void tud_task_ext(uint32_t timeout_ms, bool in_isr) {
   (void) in_isr; // not implemented yet
 
   // Skip if stack is not initialized
-  if ( !tud_inited() ) return;
+  if (!tud_inited()) return;
 
   // Loop until there is no more events in the queue
-  while (1)
-  {
+  while (1) {
     dcd_event_t event;
-    if ( !osal_queue_receive(_usbd_q, &event, timeout_ms) ) return;
+    if (!osal_queue_receive(_usbd_q, &event, timeout_ms)) return;
 
 #if CFG_TUSB_DEBUG >= CFG_TUD_LOG_LEVEL
     if (event.event_id == DCD_EVENT_SETUP_RECEIVED) TU_LOG_USBD("\r\n"); // extra line for setup
     TU_LOG_USBD("USBD %s ", event.event_id < DCD_EVENT_COUNT ? _usbd_event_str[event.event_id] : "CORRUPTED");
 #endif
 
-    switch ( event.event_id )
-    {
+    switch (event.event_id) {
       case DCD_EVENT_BUS_RESET:
         TU_LOG_USBD(": %s Speed\r\n", tu_str_speed[event.bus_reset.speed]);
         usbd_reset(event.rhport);
         _usbd_dev.speed = event.bus_reset.speed;
-      break;
+        break;
 
       case DCD_EVENT_UNPLUGGED:
         TU_LOG_USBD("\r\n");
         usbd_reset(event.rhport);
-
-        // invoke callback
         if (tud_umount_cb) tud_umount_cb();
-      break;
+        break;
 
       case DCD_EVENT_SETUP_RECEIVED:
+        _usbd_dev.setup_count--;
         TU_LOG_BUF(CFG_TUD_LOG_LEVEL, &event.setup_received, 8);
-        TU_LOG_USBD("\r\n");
+        if (_usbd_dev.setup_count) {
+          TU_LOG_USBD("  Skipped since there is other SETUP in queue\r\n");
+          break;
+        }
 
         // Mark as connected after receiving 1st setup packet.
         // But it is easier to set it every time instead of wasting time to check then set
@@ -510,81 +562,78 @@ void tud_task_ext(uint32_t timeout_ms, bool in_isr)
         // mark both in & out control as free
         _usbd_dev.ep_status[0][TUSB_DIR_OUT].busy = 0;
         _usbd_dev.ep_status[0][TUSB_DIR_OUT].claimed = 0;
-        _usbd_dev.ep_status[0][TUSB_DIR_IN ].busy = 0;
-        _usbd_dev.ep_status[0][TUSB_DIR_IN ].claimed = 0;
+        _usbd_dev.ep_status[0][TUSB_DIR_IN].busy = 0;
+        _usbd_dev.ep_status[0][TUSB_DIR_IN].claimed = 0;
 
         // Process control request
-        if ( !process_control_request(event.rhport, &event.setup_received) )
-        {
+        if (!process_control_request(event.rhport, &event.setup_received)) {
           TU_LOG_USBD("  Stall EP0\r\n");
           // Failed -> stall both control endpoint IN and OUT
           dcd_edpt_stall(event.rhport, 0);
           dcd_edpt_stall(event.rhport, 0 | TUSB_DIR_IN_MASK);
         }
-      break;
+        break;
 
-      case DCD_EVENT_XFER_COMPLETE:
-      {
+      case DCD_EVENT_XFER_COMPLETE: {
         // Invoke the class callback associated with the endpoint address
         uint8_t const ep_addr = event.xfer_complete.ep_addr;
-        uint8_t const epnum   = tu_edpt_number(ep_addr);
-        uint8_t const ep_dir  = tu_edpt_dir(ep_addr);
+        uint8_t const epnum = tu_edpt_number(ep_addr);
+        uint8_t const ep_dir = tu_edpt_dir(ep_addr);
 
         TU_LOG_USBD("on EP %02X with %u bytes\r\n", ep_addr, (unsigned int) event.xfer_complete.len);
 
         _usbd_dev.ep_status[epnum][ep_dir].busy = 0;
         _usbd_dev.ep_status[epnum][ep_dir].claimed = 0;
 
-        if ( 0 == epnum )
-        {
-          usbd_control_xfer_cb(event.rhport, ep_addr, (xfer_result_t) event.xfer_complete.result, event.xfer_complete
-          .len);
-        }
-        else
-        {
-          usbd_class_driver_t const * driver = get_driver( _usbd_dev.ep2drv[epnum][ep_dir] );
-          TU_ASSERT(driver, );
+        if (0 == epnum) {
+          usbd_control_xfer_cb(event.rhport, ep_addr, (xfer_result_t) event.xfer_complete.result,
+                               event.xfer_complete.len);
+        } else {
+          usbd_class_driver_t const* driver = get_driver(_usbd_dev.ep2drv[epnum][ep_dir]);
+          TU_ASSERT(driver,);
 
           TU_LOG_USBD("  %s xfer callback\r\n", driver->name);
           driver->xfer_cb(event.rhport, ep_addr, (xfer_result_t) event.xfer_complete.result, event.xfer_complete.len);
         }
+        break;
       }
-      break;
 
       case DCD_EVENT_SUSPEND:
         // NOTE: When plugging/unplugging device, the D+/D- state are unstable and
         // can accidentally meet the SUSPEND condition ( Bus Idle for 3ms ), which result in a series of event
         // e.g suspend -> resume -> unplug/plug. Skip suspend/resume if not connected
-        if ( _usbd_dev.connected )
-        {
+        if (_usbd_dev.connected) {
           TU_LOG_USBD(": Remote Wakeup = %u\r\n", _usbd_dev.remote_wakeup_en);
           if (tud_suspend_cb) tud_suspend_cb(_usbd_dev.remote_wakeup_en);
-        }else
-        {
+        } else {
           TU_LOG_USBD(" Skipped\r\n");
         }
-      break;
+        break;
 
       case DCD_EVENT_RESUME:
-        if ( _usbd_dev.connected )
-        {
+        if (_usbd_dev.connected) {
           TU_LOG_USBD("\r\n");
           if (tud_resume_cb) tud_resume_cb();
-        }else
-        {
+        } else {
           TU_LOG_USBD(" Skipped\r\n");
         }
-      break;
+        break;
 
       case USBD_EVENT_FUNC_CALL:
         TU_LOG_USBD("\r\n");
-        if ( event.func_call.func ) event.func_call.func(event.func_call.param);
-      break;
+        if (event.func_call.func) event.func_call.func(event.func_call.param);
+        break;
 
       case DCD_EVENT_SOF:
+        if (tu_bit_test(_usbd_dev.sof_consumer, SOF_CONSUMER_USER)) {
+          TU_LOG_USBD("\r\n");
+          tud_sof_cb(event.sof.frame_count);
+        }
+      break;
+
       default:
         TU_BREAKPOINT();
-      break;
+        break;
     }
 
 #if CFG_TUSB_OS != OPT_OS_NONE && CFG_TUSB_OS != OPT_OS_PICO
@@ -599,24 +648,20 @@ void tud_task_ext(uint32_t timeout_ms, bool in_isr)
 //--------------------------------------------------------------------+
 
 // 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)
-{
+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_xfer_cb);
   TU_LOG_USBD("  %s control request\r\n", driver->name);
   return driver->control_xfer_cb(rhport, CONTROL_STAGE_SETUP, request);
 }
 
 // This handles the actual request and its response.
-// return false will cause its caller to stall control endpoint
-static bool process_control_request(uint8_t rhport, tusb_control_request_t const * p_request)
-{
+// Returns false if unable to complete the request, causing caller to stall control endpoints.
+static bool process_control_request(uint8_t rhport, tusb_control_request_t const * p_request) {
   usbd_control_set_complete_callback(NULL);
-
   TU_ASSERT(p_request->bmRequestType_bit.type < TUSB_REQ_TYPE_INVALID);
 
   // Vendor request
-  if ( p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_VENDOR )
-  {
+  if ( p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_VENDOR ) {
     TU_VERIFY(tud_vendor_control_xfer_cb);
 
     usbd_control_set_complete_callback(tud_vendor_control_xfer_cb);
@@ -624,19 +669,16 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
   }
 
 #if CFG_TUSB_DEBUG >= CFG_TUD_LOG_LEVEL
-  if (TUSB_REQ_TYPE_STANDARD == p_request->bmRequestType_bit.type && p_request->bRequest <= TUSB_REQ_SYNCH_FRAME)
-  {
+  if (TUSB_REQ_TYPE_STANDARD == p_request->bmRequestType_bit.type && p_request->bRequest <= TUSB_REQ_SYNCH_FRAME) {
     TU_LOG_USBD("  %s", tu_str_std_request[p_request->bRequest]);
     if (TUSB_REQ_GET_DESCRIPTOR != p_request->bRequest) TU_LOG_USBD("\r\n");
   }
 #endif
 
-  switch ( p_request->bmRequestType_bit.recipient )
-  {
+  switch ( p_request->bmRequestType_bit.recipient ) {
     //------------- Device Requests e.g in enumeration -------------//
     case TUSB_REQ_RCPT_DEVICE:
-      if ( TUSB_REQ_TYPE_CLASS == p_request->bmRequestType_bit.type )
-      {
+      if ( TUSB_REQ_TYPE_CLASS == p_request->bmRequestType_bit.type ) {
         uint8_t const itf = tu_u16_low(p_request->wIndex);
         TU_VERIFY(itf < TU_ARRAY_SIZE(_usbd_dev.itf2drv));
 
@@ -647,15 +689,13 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
         return invoke_class_control(rhport, driver, p_request);
       }
 
-      if ( TUSB_REQ_TYPE_STANDARD != p_request->bmRequestType_bit.type )
-      {
+      if ( TUSB_REQ_TYPE_STANDARD != p_request->bmRequestType_bit.type ) {
         // Non standard request is not supported
         TU_BREAKPOINT();
         return false;
       }
 
-      switch ( p_request->bRequest )
-      {
+      switch ( p_request->bRequest ) {
         case TUSB_REQ_SET_ADDRESS:
           // Depending on mcu, status phase could be sent either before or after changing device address,
           // or even require stack to not response with status at all
@@ -666,25 +706,24 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
           _usbd_dev.addressed = 1;
         break;
 
-        case TUSB_REQ_GET_CONFIGURATION:
-        {
+        case TUSB_REQ_GET_CONFIGURATION: {
           uint8_t cfg_num = _usbd_dev.cfg_num;
           tud_control_xfer(rhport, p_request, &cfg_num, 1);
         }
         break;
 
-        case TUSB_REQ_SET_CONFIGURATION:
-        {
+        case TUSB_REQ_SET_CONFIGURATION: {
           uint8_t const cfg_num = (uint8_t) p_request->wValue;
 
           // Only process if new configure is different
-          if (_usbd_dev.cfg_num != cfg_num)
-          {
-            if ( _usbd_dev.cfg_num )
-            {
+          if (_usbd_dev.cfg_num != cfg_num) {
+            if ( _usbd_dev.cfg_num ) {
               // already configured: need to clear all endpoints and driver first
               TU_LOG_USBD("  Clear current Configuration (%u) before switching\r\n", _usbd_dev.cfg_num);
 
+              // disable SOF
+              dcd_sof_enable(rhport, false);
+
               // close all non-control endpoints, cancel all pending transfers if any
               dcd_edpt_close_all(rhport);
 
@@ -696,15 +735,11 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
             }
 
             // Handle the new configuration and execute the corresponding callback
-            if ( cfg_num )
-            {
+            if ( cfg_num ) {
               // switch to new configuration if not zero
               TU_ASSERT( process_set_config(rhport, cfg_num) );
-
               if ( tud_mount_cb ) tud_mount_cb();
-            }
-            else
-            {
+            } else {
               if ( tud_umount_cb ) tud_umount_cb();
             }
           }
@@ -772,15 +807,14 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
           tud_control_status(rhport, p_request);
         break;
 
-        case TUSB_REQ_GET_STATUS:
-        {
+        case TUSB_REQ_GET_STATUS: {
           // Device status bit mask
           // - Bit 0: Self Powered
           // - Bit 1: Remote Wakeup enabled
           uint16_t status = (uint16_t) ((_usbd_dev.self_powered ? 1u : 0u) | (_usbd_dev.remote_wakeup_en ? 2u : 0u));
           tud_control_xfer(rhport, p_request, &status, 2);
+          break;
         }
-        break;
 
         // Unknown/Unsupported request
         default: TU_BREAKPOINT(); return false;
@@ -788,8 +822,7 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
     break;
 
     //------------- Class/Interface Specific Request -------------//
-    case TUSB_REQ_RCPT_INTERFACE:
-    {
+    case TUSB_REQ_RCPT_INTERFACE: {
       uint8_t const itf = tu_u16_low(p_request->wIndex);
       TU_VERIFY(itf < TU_ARRAY_SIZE(_usbd_dev.itf2drv));
 
@@ -798,25 +831,21 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
 
       // all requests to Interface (STD or Class) is forwarded to class driver.
       // notable requests are: GET HID REPORT DESCRIPTOR, SET_INTERFACE, GET_INTERFACE
-      if ( !invoke_class_control(rhport, driver, p_request) )
-      {
+      if ( !invoke_class_control(rhport, driver, p_request) ) {
         // For GET_INTERFACE and SET_INTERFACE, it is mandatory to respond even if the class
         // driver doesn't use alternate settings or implement this
         TU_VERIFY(TUSB_REQ_TYPE_STANDARD == p_request->bmRequestType_bit.type);
 
-        switch(p_request->bRequest)
-        {
+        switch(p_request->bRequest) {
           case TUSB_REQ_GET_INTERFACE:
           case TUSB_REQ_SET_INTERFACE:
             // Clear complete callback if driver set since it can also stall the request.
             usbd_control_set_complete_callback(NULL);
 
-            if (TUSB_REQ_GET_INTERFACE == p_request->bRequest)
-            {
+            if (TUSB_REQ_GET_INTERFACE == p_request->bRequest) {
               uint8_t alternate = 0;
               tud_control_xfer(rhport, p_request, &alternate, 1);
-            }else
-            {
+            }else {
               tud_control_status(rhport, p_request);
             }
           break;
@@ -824,54 +853,42 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
           default: return false;
         }
       }
+      break;
     }
-    break;
 
     //------------- Endpoint Request -------------//
-    case TUSB_REQ_RCPT_ENDPOINT:
-    {
+    case TUSB_REQ_RCPT_ENDPOINT: {
       uint8_t const ep_addr = tu_u16_low(p_request->wIndex);
       uint8_t const ep_num  = tu_edpt_number(ep_addr);
       uint8_t const ep_dir  = tu_edpt_dir(ep_addr);
 
       TU_ASSERT(ep_num < TU_ARRAY_SIZE(_usbd_dev.ep2drv) );
-
       usbd_class_driver_t const * driver = get_driver(_usbd_dev.ep2drv[ep_num][ep_dir]);
 
-      if ( TUSB_REQ_TYPE_STANDARD != p_request->bmRequestType_bit.type )
-      {
+      if ( TUSB_REQ_TYPE_STANDARD != p_request->bmRequestType_bit.type ) {
         // Forward class request to its driver
         TU_VERIFY(driver);
         return invoke_class_control(rhport, driver, p_request);
-      }
-      else
-      {
+      } else {
         // Handle STD request to endpoint
-        switch ( p_request->bRequest )
-        {
-          case TUSB_REQ_GET_STATUS:
-          {
+        switch ( p_request->bRequest ) {
+          case TUSB_REQ_GET_STATUS: {
             uint16_t status = usbd_edpt_stalled(rhport, ep_addr) ? 0x0001 : 0x0000;
             tud_control_xfer(rhport, p_request, &status, 2);
           }
           break;
 
           case TUSB_REQ_CLEAR_FEATURE:
-          case TUSB_REQ_SET_FEATURE:
-          {
-            if ( TUSB_REQ_FEATURE_EDPT_HALT == p_request->wValue )
-            {
-              if ( TUSB_REQ_CLEAR_FEATURE ==  p_request->bRequest )
-              {
+          case TUSB_REQ_SET_FEATURE: {
+            if ( TUSB_REQ_FEATURE_EDPT_HALT == p_request->wValue ) {
+              if ( TUSB_REQ_CLEAR_FEATURE ==  p_request->bRequest ) {
                 usbd_edpt_clear_stall(rhport, ep_addr);
-              }else
-              {
+              }else {
                 usbd_edpt_stall(rhport, ep_addr);
               }
             }
 
-            if (driver)
-            {
+            if (driver) {
               // Some classes such as USBTMC needs to clear/re-init its buffer when receiving CLEAR_FEATURE request
               // We will also forward std request targeted endpoint to class drivers as well
 
@@ -887,14 +904,18 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
           break;
 
           // Unknown/Unsupported request
-          default: TU_BREAKPOINT(); return false;
+          default:
+            TU_BREAKPOINT();
+            return false;
         }
       }
     }
     break;
 
     // Unknown recipient
-    default: TU_BREAKPOINT(); return false;
+    default:
+      TU_BREAKPOINT();
+      return false;
   }
 
   return true;
@@ -1148,6 +1169,14 @@ TU_ATTR_FAST_FUNC void dcd_event_handler(dcd_event_t const* event, bool in_isr)
       break;
 
     case DCD_EVENT_SOF:
+      // SOF driver handler in ISR context
+      for (uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++) {
+        usbd_class_driver_t const* driver = get_driver(i);
+        if (driver && driver->sof) {
+          driver->sof(event->rhport, event->sof.frame_count);
+        }
+      }
+
       // Some MCUs after running dcd_remote_wakeup() does not have way to detect the end of remote wakeup
       // which last 1-15 ms. DCD can use SOF as a clear indicator that bus is back to operational
       if (_usbd_dev.suspended) {
@@ -1157,15 +1186,15 @@ TU_ATTR_FAST_FUNC void dcd_event_handler(dcd_event_t const* event, bool in_isr)
         queue_event(&event_resume, in_isr);
       }
 
-      // SOF driver handler in ISR context
-      for (uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++) {
-        usbd_class_driver_t const* driver = get_driver(i);
-        if (driver && driver->sof) {
-          driver->sof(event->rhport, event->sof.frame_count);
-        }
+      if (tu_bit_test(_usbd_dev.sof_consumer, SOF_CONSUMER_USER)) {
+        dcd_event_t const event_sof = {.rhport = event->rhport, .event_id = DCD_EVENT_SOF};
+        queue_event(&event_sof, in_isr);
       }
+      break;
 
-      // skip osal queue for SOF in usbd task
+    case DCD_EVENT_SETUP_RECEIVED:
+      _usbd_dev.setup_count++;
+      send = true;
       break;
 
     default:
@@ -1233,8 +1262,7 @@ void usbd_defer_func(osal_task_func_t func, void* param, bool in_isr) {
 // USBD Endpoint API
 //--------------------------------------------------------------------+
 
-bool usbd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * desc_ep)
-{
+bool usbd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const* desc_ep) {
   rhport = _usbd_rhport;
 
   TU_ASSERT(tu_edpt_number(desc_ep->bEndpointAddress) < CFG_TUD_ENDPPOINT_MAX);
@@ -1243,42 +1271,44 @@ bool usbd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * desc_ep)
   return dcd_edpt_open(rhport, desc_ep);
 }
 
-bool usbd_edpt_claim(uint8_t rhport, uint8_t ep_addr)
-{
+bool usbd_edpt_claim(uint8_t rhport, uint8_t ep_addr) {
   (void) rhport;
 
   // TODO add this check later, also make sure we don't starve an out endpoint while suspending
   // TU_VERIFY(tud_ready());
 
-  uint8_t const epnum       = tu_edpt_number(ep_addr);
-  uint8_t const dir         = tu_edpt_dir(ep_addr);
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
   tu_edpt_state_t* ep_state = &_usbd_dev.ep_status[epnum][dir];
 
   return tu_edpt_claim(ep_state, _usbd_mutex);
 }
 
-bool usbd_edpt_release(uint8_t rhport, uint8_t ep_addr)
-{
+bool usbd_edpt_release(uint8_t rhport, uint8_t ep_addr) {
   (void) rhport;
 
-  uint8_t const epnum       = tu_edpt_number(ep_addr);
-  uint8_t const dir         = tu_edpt_dir(ep_addr);
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
   tu_edpt_state_t* ep_state = &_usbd_dev.ep_status[epnum][dir];
 
   return tu_edpt_release(ep_state, _usbd_mutex);
 }
 
-bool usbd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
-{
+bool usbd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes) {
   rhport = _usbd_rhport;
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   // TODO skip ready() check for now since enumeration also use this API
   // TU_VERIFY(tud_ready());
 
   TU_LOG_USBD("  Queue EP %02X with %u bytes ...\r\n", ep_addr, total_bytes);
+#if CFG_TUD_LOG_LEVEL >= 3
+  if(dir == TUSB_DIR_IN) {
+    TU_LOG_MEM(CFG_TUD_LOG_LEVEL, buffer, total_bytes, 2);
+  }
+#endif
 
   // Attempt to transfer on a busy endpoint, sound like an race condition !
   TU_ASSERT(_usbd_dev.ep_status[epnum][dir].busy == 0);
@@ -1287,11 +1317,9 @@ bool usbd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
   // could return and USBD task can preempt and clear the busy
   _usbd_dev.ep_status[epnum][dir].busy = 1;
 
-  if ( dcd_edpt_xfer(rhport, ep_addr, buffer, total_bytes) )
-  {
+  if (dcd_edpt_xfer(rhport, ep_addr, buffer, total_bytes)) {
     return true;
-  }else
-  {
+  } else {
     // DCD error, mark endpoint as ready to allow next transfer
     _usbd_dev.ep_status[epnum][dir].busy = 0;
     _usbd_dev.ep_status[epnum][dir].claimed = 0;
@@ -1305,12 +1333,11 @@ bool usbd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
 // bytes should be written and second to keep the return value free to give back a boolean
 // success message. If total_bytes is too big, the FIFO will copy only what is available
 // into the USB buffer!
-bool usbd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16_t total_bytes)
-{
+bool usbd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t* ff, uint16_t total_bytes) {
   rhport = _usbd_rhport;
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   TU_LOG_USBD("  Queue ISO EP %02X with %u bytes ... ", ep_addr, total_bytes);
 
@@ -1321,12 +1348,10 @@ bool usbd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16
   // and usbd task can preempt and clear the busy
   _usbd_dev.ep_status[epnum][dir].busy = 1;
 
-  if (dcd_edpt_xfer_fifo(rhport, ep_addr, ff, total_bytes))
-  {
+  if (dcd_edpt_xfer_fifo(rhport, ep_addr, ff, total_bytes)) {
     TU_LOG_USBD("OK\r\n");
     return true;
-  }else
-  {
+  } else {
     // DCD error, mark endpoint as ready to allow next transfer
     _usbd_dev.ep_status[epnum][dir].busy = 0;
     _usbd_dev.ep_status[epnum][dir].claimed = 0;
@@ -1336,75 +1361,62 @@ bool usbd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16
   }
 }
 
-bool usbd_edpt_busy(uint8_t rhport, uint8_t ep_addr)
-{
+bool usbd_edpt_busy(uint8_t rhport, uint8_t ep_addr) {
   (void) rhport;
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   return _usbd_dev.ep_status[epnum][dir].busy;
 }
 
-void usbd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
-{
+void usbd_edpt_stall(uint8_t rhport, uint8_t ep_addr) {
   rhport = _usbd_rhport;
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   // only stalled if currently cleared
-  if ( !_usbd_dev.ep_status[epnum][dir].stalled )
-  {
-    TU_LOG_USBD("    Stall EP %02X\r\n", ep_addr);
-    dcd_edpt_stall(rhport, ep_addr);
-    _usbd_dev.ep_status[epnum][dir].stalled = 1;
-    _usbd_dev.ep_status[epnum][dir].busy = 1;
-  }
+  TU_LOG_USBD("    Stall EP %02X\r\n", ep_addr);
+  dcd_edpt_stall(rhport, ep_addr);
+  _usbd_dev.ep_status[epnum][dir].stalled = 1;
+  _usbd_dev.ep_status[epnum][dir].busy = 1;
 }
 
-void usbd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
-{
+void usbd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr) {
   rhport = _usbd_rhport;
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   // only clear if currently stalled
-  if ( _usbd_dev.ep_status[epnum][dir].stalled )
-  {
-    TU_LOG_USBD("    Clear Stall EP %02X\r\n", ep_addr);
-    dcd_edpt_clear_stall(rhport, ep_addr);
-    _usbd_dev.ep_status[epnum][dir].stalled = 0;
-    _usbd_dev.ep_status[epnum][dir].busy = 0;
-  }
+  TU_LOG_USBD("    Clear Stall EP %02X\r\n", ep_addr);
+  dcd_edpt_clear_stall(rhport, ep_addr);
+  _usbd_dev.ep_status[epnum][dir].stalled = 0;
+  _usbd_dev.ep_status[epnum][dir].busy = 0;
 }
 
-bool usbd_edpt_stalled(uint8_t rhport, uint8_t ep_addr)
-{
+bool usbd_edpt_stalled(uint8_t rhport, uint8_t ep_addr) {
   (void) rhport;
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   return _usbd_dev.ep_status[epnum][dir].stalled;
 }
 
 /**
  * usbd_edpt_close will disable an endpoint.
- *
  * In progress transfers on this EP may be delivered after this call.
- *
  */
-void usbd_edpt_close(uint8_t rhport, uint8_t ep_addr)
-{
+void usbd_edpt_close(uint8_t rhport, uint8_t ep_addr) {
   rhport = _usbd_rhport;
 
   TU_ASSERT(dcd_edpt_close, /**/);
   TU_LOG_USBD("  CLOSING Endpoint: 0x%02X\r\n", ep_addr);
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   dcd_edpt_close(rhport, ep_addr);
   _usbd_dev.ep_status[epnum][dir].stalled = 0;
@@ -1414,17 +1426,24 @@ void usbd_edpt_close(uint8_t rhport, uint8_t ep_addr)
   return;
 }
 
-void usbd_sof_enable(uint8_t rhport, bool en)
-{
+void usbd_sof_enable(uint8_t rhport, sof_consumer_t consumer, bool en) {
   rhport = _usbd_rhport;
 
-  // TODO: Check needed if all drivers including the user sof_cb does not need an active SOF ISR any more.
-  // Only if all drivers switched off SOF calls the SOF interrupt may be disabled
-  dcd_sof_enable(rhport, en);
+  uint8_t consumer_old = _usbd_dev.sof_consumer;
+  // Keep track how many class instances need the SOF interrupt
+  if (en) {
+    _usbd_dev.sof_consumer |= (uint8_t)(1 << consumer);
+  } else {
+    _usbd_dev.sof_consumer &= (uint8_t)(~(1 << consumer));
+  }
+
+  // Test logically unequal
+  if(!_usbd_dev.sof_consumer != !consumer_old) {
+    dcd_sof_enable(rhport, _usbd_dev.sof_consumer);
+  }
 }
 
-bool usbd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size)
-{
+bool usbd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size) {
   rhport = _usbd_rhport;
 
   TU_ASSERT(dcd_edpt_iso_alloc);
@@ -1433,12 +1452,11 @@ bool usbd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packe
   return dcd_edpt_iso_alloc(rhport, ep_addr, largest_packet_size);
 }
 
-bool usbd_edpt_iso_activate(uint8_t rhport, tusb_desc_endpoint_t const * desc_ep)
-{
+bool usbd_edpt_iso_activate(uint8_t rhport, tusb_desc_endpoint_t const* desc_ep) {
   rhport = _usbd_rhport;
 
   uint8_t const epnum = tu_edpt_number(desc_ep->bEndpointAddress);
-  uint8_t const dir   = tu_edpt_dir(desc_ep->bEndpointAddress);
+  uint8_t const dir = tu_edpt_dir(desc_ep->bEndpointAddress);
 
   TU_ASSERT(dcd_edpt_iso_activate);
   TU_ASSERT(epnum < CFG_TUD_ENDPPOINT_MAX);
diff --git a/src/device/usbd.h b/src/device/usbd.h
index 3ab6c813f..cba94fdae 100644
--- a/src/device/usbd.h
+++ b/src/device/usbd.h
@@ -37,9 +37,12 @@ extern "C" {
 // Application API
 //--------------------------------------------------------------------+
 
-// Init device stack
+// Init device stack on roothub port
 bool tud_init (uint8_t rhport);
 
+// Deinit device stack on roothub port
+bool tud_deinit(uint8_t rhport);
+
 // Check if device stack is already initialized
 bool tud_inited(void);
 
@@ -94,6 +97,9 @@ bool tud_disconnect(void);
 // Return false on unsupported MCUs
 bool tud_connect(void);
 
+// Enable or disable the Start Of Frame callback support
+bool tud_sof_cb_enable(bool en);
+
 // Carry out Data and Status stage of control transfer
 // - If len = 0, it is equivalent to sending status only
 // - If len > wLength : it will be truncated
@@ -149,6 +155,9 @@ TU_ATTR_WEAK void tud_resume_cb(void);
 // Invoked when there is a new usb event, which need to be processed by tud_task()/tud_task_ext()
 void tud_event_hook_cb(uint8_t rhport, uint32_t eventid, bool in_isr);
 
+// Invoked when a new (micro) frame started
+void tud_sof_cb(uint32_t frame_count);
+
 // Invoked when received control request with VENDOR TYPE
 TU_ATTR_WEAK bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
 
@@ -218,8 +227,8 @@ TU_ATTR_WEAK bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb
   5, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_HEADER, U16_TO_U8S_LE(0x0120),\
   /* CDC Call */\
   5, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_CALL_MANAGEMENT, 0, (uint8_t)((_itfnum) + 1),\
-  /* CDC ACM: support line request */\
-  4, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_ABSTRACT_CONTROL_MANAGEMENT, 2,\
+  /* CDC ACM: support line request + send break */\
+  4, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_ABSTRACT_CONTROL_MANAGEMENT, 6,\
   /* CDC Union */\
   5, TUSB_DESC_CS_INTERFACE, CDC_FUNC_DESC_UNION, _itfnum, (uint8_t)((_itfnum) + 1),\
   /* Endpoint Notification */\
@@ -393,6 +402,11 @@ TU_ATTR_WEAK bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb
 
 // For more channels, add definitions here
 
+/* Standard AC Interrupt Endpoint Descriptor(4.8.2.1) */
+#define TUD_AUDIO_DESC_STD_AC_INT_EP_LEN 7
+#define TUD_AUDIO_DESC_STD_AC_INT_EP(_ep, _interval) \
+  TUD_AUDIO_DESC_STD_AC_INT_EP_LEN, TUSB_DESC_ENDPOINT, _ep, TUSB_XFER_INTERRUPT, U16_TO_U8S_LE(6), _interval
+
 /* Standard AS Interface Descriptor(4.9.1) */
 #define TUD_AUDIO_DESC_STD_AS_INT_LEN 9
 #define TUD_AUDIO_DESC_STD_AS_INT(_itfnum, _altset, _nEPs, _stridx) \
@@ -421,7 +435,7 @@ TU_ATTR_WEAK bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb
 /* Standard AS Isochronous Feedback Endpoint Descriptor(4.10.2.1) */
 #define TUD_AUDIO_DESC_STD_AS_ISO_FB_EP_LEN 7
 #define TUD_AUDIO_DESC_STD_AS_ISO_FB_EP(_ep, _interval) \
-  TUD_AUDIO_DESC_STD_AS_ISO_FB_EP_LEN, TUSB_DESC_ENDPOINT, _ep, (uint8_t) (TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_NO_SYNC | TUSB_ISO_EP_ATT_EXPLICIT_FB), U16_TO_U8S_LE(4), _interval
+  TUD_AUDIO_DESC_STD_AS_ISO_FB_EP_LEN, TUSB_DESC_ENDPOINT, _ep, (uint8_t) ((uint8_t)TUSB_XFER_ISOCHRONOUS | (uint8_t)TUSB_ISO_EP_ATT_NO_SYNC | (uint8_t)TUSB_ISO_EP_ATT_EXPLICIT_FB), U16_TO_U8S_LE(4), _interval
 
 // AUDIO simple descriptor (UAC2) for 1 microphone input
 // - 1 Input Terminal, 1 Feature Unit (Mute and Volume Control), 1 Output Terminal, 1 Clock Source
@@ -468,7 +482,7 @@ TU_ATTR_WEAK bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb
   /* Type I Format Type Descriptor(2.3.1.6 - Audio Formats) */\
   TUD_AUDIO_DESC_TYPE_I_FORMAT(_nBytesPerSample, _nBitsUsedPerSample),\
   /* Standard AS Isochronous Audio Data Endpoint Descriptor(4.10.1.1) */\
-  TUD_AUDIO_DESC_STD_AS_ISO_EP(/*_ep*/ _epin, /*_attr*/ (uint8_t) (TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS | TUSB_ISO_EP_ATT_DATA), /*_maxEPsize*/ _epsize, /*_interval*/ 0x01),\
+  TUD_AUDIO_DESC_STD_AS_ISO_EP(/*_ep*/ _epin, /*_attr*/ (uint8_t) ((uint8_t)TUSB_XFER_ISOCHRONOUS | (uint8_t)TUSB_ISO_EP_ATT_ASYNCHRONOUS | (uint8_t)TUSB_ISO_EP_ATT_DATA), /*_maxEPsize*/ _epsize, /*_interval*/ 0x01),\
   /* Class-Specific AS Isochronous Audio Data Endpoint Descriptor(4.10.1.2) */\
   TUD_AUDIO_DESC_CS_AS_ISO_EP(/*_attr*/ AUDIO_CS_AS_ISO_DATA_EP_ATT_NON_MAX_PACKETS_OK, /*_ctrl*/ AUDIO_CTRL_NONE, /*_lockdelayunit*/ AUDIO_CS_AS_ISO_DATA_EP_LOCK_DELAY_UNIT_UNDEFINED, /*_lockdelay*/ 0x0000)
 
@@ -517,7 +531,7 @@ TU_ATTR_WEAK bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb
   /* Type I Format Type Descriptor(2.3.1.6 - Audio Formats) */\
   TUD_AUDIO_DESC_TYPE_I_FORMAT(_nBytesPerSample, _nBitsUsedPerSample),\
   /* Standard AS Isochronous Audio Data Endpoint Descriptor(4.10.1.1) */\
-  TUD_AUDIO_DESC_STD_AS_ISO_EP(/*_ep*/ _epin, /*_attr*/ (uint8_t) (TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS | TUSB_ISO_EP_ATT_DATA), /*_maxEPsize*/ _epsize, /*_interval*/ 0x01),\
+  TUD_AUDIO_DESC_STD_AS_ISO_EP(/*_ep*/ _epin, /*_attr*/ (uint8_t) ((uint8_t)TUSB_XFER_ISOCHRONOUS | (uint8_t)TUSB_ISO_EP_ATT_ASYNCHRONOUS | (uint8_t)TUSB_ISO_EP_ATT_DATA), /*_maxEPsize*/ _epsize, /*_interval*/ 0x01),\
   /* Class-Specific AS Isochronous Audio Data Endpoint Descriptor(4.10.1.2) */\
   TUD_AUDIO_DESC_CS_AS_ISO_EP(/*_attr*/ AUDIO_CS_AS_ISO_DATA_EP_ATT_NON_MAX_PACKETS_OK, /*_ctrl*/ AUDIO_CTRL_NONE, /*_lockdelayunit*/ AUDIO_CS_AS_ISO_DATA_EP_LOCK_DELAY_UNIT_UNDEFINED, /*_lockdelay*/ 0x0000)
 
@@ -565,7 +579,7 @@ TU_ATTR_WEAK bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb
   /* Type I Format Type Descriptor(2.3.1.6 - Audio Formats) */\
   TUD_AUDIO_DESC_TYPE_I_FORMAT(_nBytesPerSample, _nBitsUsedPerSample),\
   /* Standard AS Isochronous Audio Data Endpoint Descriptor(4.10.1.1) */\
-  TUD_AUDIO_DESC_STD_AS_ISO_EP(/*_ep*/ _epout, /*_attr*/ (uint8_t) (TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS | TUSB_ISO_EP_ATT_DATA), /*_maxEPsize*/ _epsize, /*_interval*/ 0x01),\
+  TUD_AUDIO_DESC_STD_AS_ISO_EP(/*_ep*/ _epout, /*_attr*/ (uint8_t) ((uint8_t)TUSB_XFER_ISOCHRONOUS | (uint8_t)TUSB_ISO_EP_ATT_ASYNCHRONOUS | (uint8_t)TUSB_ISO_EP_ATT_DATA), /*_maxEPsize*/ _epsize, /*_interval*/ 0x01),\
   /* Class-Specific AS Isochronous Audio Data Endpoint Descriptor(4.10.1.2) */\
   TUD_AUDIO_DESC_CS_AS_ISO_EP(/*_attr*/ AUDIO_CS_AS_ISO_DATA_EP_ATT_NON_MAX_PACKETS_OK, /*_ctrl*/ AUDIO_CTRL_NONE, /*_lockdelayunit*/ AUDIO_CS_AS_ISO_DATA_EP_LOCK_DELAY_UNIT_UNDEFINED, /*_lockdelay*/ 0x0000),\
   /* Standard AS Isochronous Feedback Endpoint Descriptor(4.10.2.1) */\
diff --git a/src/device/usbd_pvt.h b/src/device/usbd_pvt.h
index 9039bc9d6..335d46cd8 100644
--- a/src/device/usbd_pvt.h
+++ b/src/device/usbd_pvt.h
@@ -23,8 +23,8 @@
  *
  * This file is part of the TinyUSB stack.
  */
-#ifndef _TUSB_USBD_PVT_H_
-#define _TUSB_USBD_PVT_H_
+#ifndef TUSB_USBD_PVT_H_
+#define TUSB_USBD_PVT_H_
 
 #include "osal/osal.h"
 #include "common/tusb_fifo.h"
@@ -36,15 +36,22 @@
 #define TU_LOG_USBD(...)   TU_LOG(CFG_TUD_LOG_LEVEL, __VA_ARGS__)
 
 //--------------------------------------------------------------------+
+// MACRO CONSTANT TYPEDEF PROTYPES
+//--------------------------------------------------------------------+
+
+typedef enum {
+  SOF_CONSUMER_USER = 0,
+  SOF_CONSUMER_AUDIO,
+} sof_consumer_t;
+
+//--------------------------------------------------------------------+
 // Class Driver API
 //--------------------------------------------------------------------+
 
 typedef struct {
-  #if CFG_TUSB_DEBUG >= CFG_TUD_LOG_LEVEL
   char const* name;
-  #endif
-
   void     (* init             ) (void);
+  bool     (* deinit           ) (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_xfer_cb  ) (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
@@ -110,7 +117,7 @@ bool usbd_edpt_ready(uint8_t rhport, uint8_t ep_addr) {
 }
 
 // Enable SOF interrupt
-void usbd_sof_enable(uint8_t rhport, bool en);
+void usbd_sof_enable(uint8_t rhport, sof_consumer_t consumer, bool en);
 
 /*------------------------------------------------------------------*/
 /* Helper
diff --git a/src/host/hcd.h b/src/host/hcd.h
index 2bde289df..5547c7cc5 100644
--- a/src/host/hcd.h
+++ b/src/host/hcd.h
@@ -125,11 +125,14 @@ bool hcd_dcache_clean_invalidate(void const* addr, uint32_t data_size) TU_ATTR_W
 //--------------------------------------------------------------------+
 
 // optional hcd configuration, called by tuh_configure()
-bool hcd_configure(uint8_t rhport, uint32_t cfg_id, const void* cfg_param) TU_ATTR_WEAK;
+bool hcd_configure(uint8_t rhport, uint32_t cfg_id, const void* cfg_param);
 
 // Initialize controller to host mode
 bool hcd_init(uint8_t rhport);
 
+// De-initialize controller
+bool hcd_deinit(uint8_t rhport);
+
 // Interrupt Handler
 void hcd_int_handler(uint8_t rhport, bool in_isr);
 
diff --git a/src/host/hub.c b/src/host/hub.c
index 3bac18698..e97014443 100644
--- a/src/host/hub.c
+++ b/src/host/hub.c
@@ -182,9 +182,13 @@ bool hub_port_get_status(uint8_t hub_addr, uint8_t hub_port, void* resp,
 //--------------------------------------------------------------------+
 // CLASS-USBH API (don't require to verify parameters)
 //--------------------------------------------------------------------+
-void hub_init(void)
-{
+bool hub_init(void) {
   tu_memclr(hub_data, sizeof(hub_data));
+  return true;
+}
+
+bool hub_deinit(void) {
+  return true;
 }
 
 bool hub_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
diff --git a/src/host/hub.h b/src/host/hub.h
index 390740e1f..385efe6b2 100644
--- a/src/host/hub.h
+++ b/src/host/hub.h
@@ -187,16 +187,14 @@ bool hub_port_get_status    (uint8_t hub_addr, uint8_t hub_port, void* resp,
 bool hub_edpt_status_xfer(uint8_t dev_addr);
 
 // Reset a port
-static inline bool hub_port_reset(uint8_t hub_addr, uint8_t hub_port,
-                                  tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+TU_ATTR_ALWAYS_INLINE static inline
+bool hub_port_reset(uint8_t hub_addr, uint8_t hub_port, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   return hub_port_set_feature(hub_addr, hub_port, HUB_FEATURE_PORT_RESET, complete_cb, user_data);
 }
 
 // Clear Reset Change
-static inline bool hub_port_clear_reset_change(uint8_t hub_addr, uint8_t hub_port,
-                                               tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+TU_ATTR_ALWAYS_INLINE static inline
+bool hub_port_clear_reset_change(uint8_t hub_addr, uint8_t hub_port, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   return hub_port_clear_feature(hub_addr, hub_port, HUB_FEATURE_PORT_RESET_CHANGE, complete_cb, user_data);
 }
 
@@ -204,7 +202,8 @@ static inline bool hub_port_clear_reset_change(uint8_t hub_addr, uint8_t hub_por
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-void hub_init       (void);
+bool hub_init       (void);
+bool hub_deinit     (void);
 bool hub_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len);
 bool hub_set_config (uint8_t dev_addr, uint8_t itf_num);
 bool hub_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
diff --git a/src/host/usbh.c b/src/host/usbh.c
index dfe6ddb42..7a47f5056 100644
--- a/src/host/usbh.c
+++ b/src/host/usbh.c
@@ -45,8 +45,20 @@
 #endif
 
 //--------------------------------------------------------------------+
-// Callback weak stubs (called if application does not provide)
+// Weak stubs: invoked if no strong implementation is available
 //--------------------------------------------------------------------+
+TU_ATTR_WEAK bool hcd_deinit(uint8_t rhport) {
+  (void) rhport;
+  return false;
+}
+
+TU_ATTR_WEAK bool hcd_configure(uint8_t rhport, uint32_t cfg_id, const void* cfg_param) {
+  (void) rhport;
+  (void) cfg_id;
+  (void) cfg_param;
+  return false;
+}
+
 TU_ATTR_WEAK void tuh_event_hook_cb(uint8_t rhport, uint32_t eventid, bool in_isr) {
   (void) rhport;
   (void) eventid;
@@ -119,16 +131,17 @@ typedef struct {
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
 #if CFG_TUSB_DEBUG >= CFG_TUH_LOG_LEVEL
-  #define DRIVER_NAME(_name)    .name = _name,
+  #define DRIVER_NAME(_name)  _name
 #else
-  #define DRIVER_NAME(_name)
+  #define DRIVER_NAME(_name)  NULL
 #endif
 
 static usbh_class_driver_t const usbh_class_drivers[] = {
     #if CFG_TUH_CDC
     {
-        DRIVER_NAME("CDC")
+        .name       = DRIVER_NAME("CDC"),
         .init       = cdch_init,
+        .deinit     = cdch_deinit,
         .open       = cdch_open,
         .set_config = cdch_set_config,
         .xfer_cb    = cdch_xfer_cb,
@@ -138,8 +151,9 @@ static usbh_class_driver_t const usbh_class_drivers[] = {
 
     #if CFG_TUH_MSC
     {
-        DRIVER_NAME("MSC")
+        .name       = DRIVER_NAME("MSC"),
         .init       = msch_init,
+        .deinit     = msch_deinit,
         .open       = msch_open,
         .set_config = msch_set_config,
         .xfer_cb    = msch_xfer_cb,
@@ -149,8 +163,9 @@ static usbh_class_driver_t const usbh_class_drivers[] = {
 
     #if CFG_TUH_HID
     {
-        DRIVER_NAME("HID")
+        .name       = DRIVER_NAME("HID"),
         .init       = hidh_init,
+        .deinit     = hidh_deinit,
         .open       = hidh_open,
         .set_config = hidh_set_config,
         .xfer_cb    = hidh_xfer_cb,
@@ -160,8 +175,9 @@ static usbh_class_driver_t const usbh_class_drivers[] = {
 
     #if CFG_TUH_HUB
     {
-        DRIVER_NAME("HUB")
+        .name       = DRIVER_NAME("HUB"),
         .init       = hub_init,
+        .deinit     = hub_deinit,
         .open       = hub_open,
         .set_config = hub_set_config,
         .xfer_cb    = hub_xfer_cb,
@@ -171,9 +187,11 @@ static usbh_class_driver_t const usbh_class_drivers[] = {
 
     #if CFG_TUH_VENDOR
     {
-      DRIVER_NAME("VENDOR")
+      .name       = DRIVER_NAME("VENDOR"),
       .init       = cush_init,
-      .open       = cush_open_subtask,
+      .deinit     = cush_deinit,
+      .open       = cush_open,
+      .set_config = cush_set_config,
       .xfer_cb    = cush_isr,
       .close      = cush_close
     }
@@ -270,9 +288,9 @@ TU_ATTR_WEAK void osal_task_delay(uint32_t msec) {
 #endif
 
 TU_ATTR_ALWAYS_INLINE static inline bool queue_event(hcd_event_t const * event, bool in_isr) {
-  bool ret = osal_queue_send(_usbh_q, event, in_isr);
+  TU_ASSERT(osal_queue_send(_usbh_q, event, in_isr));
   tuh_event_hook_cb(event->rhport, event->event_id, in_isr);
-  return ret;
+  return true;
 }
 
 //--------------------------------------------------------------------+
@@ -321,11 +339,7 @@ bool tuh_rhport_reset_bus(uint8_t rhport, bool active) {
 //--------------------------------------------------------------------+
 
 bool tuh_configure(uint8_t rhport, uint32_t cfg_id, const void *cfg_param) {
-  if ( hcd_configure ) {
-    return hcd_configure(rhport, cfg_id, cfg_param);
-  } else {
-    return false;
-  }
+  return hcd_configure(rhport, cfg_id, cfg_param);
 }
 
 static void clear_device(usbh_device_t* dev) {
@@ -338,55 +352,94 @@ bool tuh_inited(void) {
   return _usbh_controller != TUSB_INDEX_INVALID_8;
 }
 
-bool tuh_init(uint8_t controller_id) {
+bool tuh_init(uint8_t rhport) {
   // skip if already initialized
-  if ( tuh_inited() ) return true;
+  if (tuh_rhport_is_active(rhport)) return true;
+
+  TU_LOG_USBH("USBH init on controller %u\r\n", rhport);
 
-  TU_LOG_USBH("USBH init on controller %u\r\n", controller_id);
-  TU_LOG_INT(CFG_TUH_LOG_LEVEL, sizeof(usbh_device_t));
-  TU_LOG_INT(CFG_TUH_LOG_LEVEL, sizeof(hcd_event_t));
-  TU_LOG_INT(CFG_TUH_LOG_LEVEL, sizeof(_ctrl_xfer));
-  TU_LOG_INT(CFG_TUH_LOG_LEVEL, sizeof(tuh_xfer_t));
-  TU_LOG_INT(CFG_TUH_LOG_LEVEL, sizeof(tu_fifo_t));
-  TU_LOG_INT(CFG_TUH_LOG_LEVEL, sizeof(tu_edpt_stream_t));
+  // Init host stack if not already
+  if (!tuh_inited()) {
+    TU_LOG_INT_USBH(sizeof(usbh_device_t));
+    TU_LOG_INT_USBH(sizeof(hcd_event_t));
+    TU_LOG_INT_USBH(sizeof(_ctrl_xfer));
+    TU_LOG_INT_USBH(sizeof(tuh_xfer_t));
+    TU_LOG_INT_USBH(sizeof(tu_fifo_t));
+    TU_LOG_INT_USBH(sizeof(tu_edpt_stream_t));
 
-  // Event queue
-  _usbh_q = osal_queue_create( &_usbh_qdef );
-  TU_ASSERT(_usbh_q != NULL);
+    // Event queue
+    _usbh_q = osal_queue_create(&_usbh_qdef);
+    TU_ASSERT(_usbh_q != NULL);
 
 #if OSAL_MUTEX_REQUIRED
-  // Init mutex
-  _usbh_mutex = osal_mutex_create(&_usbh_mutexdef);
-  TU_ASSERT(_usbh_mutex);
+    // Init mutex
+    _usbh_mutex = osal_mutex_create(&_usbh_mutexdef);
+    TU_ASSERT(_usbh_mutex);
 #endif
 
-  // Get application driver if available
-  if ( usbh_app_driver_get_cb ) {
-    _app_driver = usbh_app_driver_get_cb(&_app_driver_count);
-  }
+    // Get application driver if available
+    if (usbh_app_driver_get_cb) {
+      _app_driver = usbh_app_driver_get_cb(&_app_driver_count);
+    }
 
-  // Device
-  tu_memclr(&_dev0, sizeof(_dev0));
-  tu_memclr(_usbh_devices, sizeof(_usbh_devices));
-  tu_memclr(&_ctrl_xfer, sizeof(_ctrl_xfer));
+    // Device
+    tu_memclr(&_dev0, sizeof(_dev0));
+    tu_memclr(_usbh_devices, sizeof(_usbh_devices));
+    tu_memclr(&_ctrl_xfer, sizeof(_ctrl_xfer));
 
-  for(uint8_t i=0; i<TOTAL_DEVICES; i++) {
-    clear_device(&_usbh_devices[i]);
-  }
+    for (uint8_t i = 0; i < TOTAL_DEVICES; i++) {
+      clear_device(&_usbh_devices[i]);
+    }
 
-  // Class drivers
-  for (uint8_t drv_id = 0; drv_id < TOTAL_DRIVER_COUNT; drv_id++) {
-    usbh_class_driver_t const* driver = get_driver(drv_id);
-    if (driver) {
-      TU_LOG_USBH("%s init\r\n", driver->name);
-      driver->init();
+    // Class drivers
+    for (uint8_t drv_id = 0; drv_id < TOTAL_DRIVER_COUNT; drv_id++) {
+      usbh_class_driver_t const* driver = get_driver(drv_id);
+      if (driver) {
+        TU_LOG_USBH("%s init\r\n", driver->name);
+        driver->init();
+      }
     }
   }
 
-  _usbh_controller = controller_id;;
+  // Init host controller
+  _usbh_controller = rhport;;
+  TU_ASSERT(hcd_init(rhport));
+  hcd_int_enable(rhport);
+
+  return true;
+}
+
+bool tuh_deinit(uint8_t rhport) {
+  if (!tuh_rhport_is_active(rhport)) return true;
+
+  // deinit host controller
+  hcd_int_disable(rhport);
+  hcd_deinit(rhport);
+  _usbh_controller = TUSB_INDEX_INVALID_8;
+
+  // "unplug" all devices on this rhport (hub_addr = 0, hub_port = 0)
+  process_removing_device(rhport, 0, 0);
+
+  // deinit host stack if no controller is active
+  if (!tuh_inited()) {
+    // Class drivers
+    for (uint8_t drv_id = 0; drv_id < TOTAL_DRIVER_COUNT; drv_id++) {
+      usbh_class_driver_t const* driver = get_driver(drv_id);
+      if (driver && driver->deinit) {
+        TU_LOG_USBH("%s deinit\r\n", driver->name);
+        driver->deinit();
+      }
+    }
+
+    osal_queue_delete(_usbh_q);
+    _usbh_q = NULL;
 
-  TU_ASSERT(hcd_init(controller_id));
-  hcd_int_enable(controller_id);
+    #if OSAL_MUTEX_REQUIRED
+    // TODO make sure there is no task waiting on this mutex
+    osal_mutex_delete(_usbh_mutex);
+    _usbh_mutex = NULL;
+    #endif
+  }
 
   return true;
 }
@@ -420,20 +473,18 @@ void tuh_task_ext(uint32_t timeout_ms, bool in_isr) {
   (void) in_isr; // not implemented yet
 
   // Skip if stack is not initialized
-  if ( !tuh_inited() ) return;
+  if (!tuh_inited()) return;
 
   // Loop until there is no more events in the queue
-  while (1)
-  {
+  while (1) {
     hcd_event_t event;
-    if ( !osal_queue_receive(_usbh_q, &event, timeout_ms) ) return;
+    if (!osal_queue_receive(_usbh_q, &event, timeout_ms)) return;
 
-    switch (event.event_id)
-    {
+    switch (event.event_id) {
       case HCD_EVENT_DEVICE_ATTACH:
         // due to the shared _usbh_ctrl_buf, we must complete enumerating one device before enumerating another one.
         // TODO better to have an separated queue for newly attached devices
-        if ( _dev0.enumerating ) {
+        if (_dev0.enumerating) {
           TU_LOG_USBH("[%u:] USBH Defer Attach until current enumeration complete\r\n", event.rhport);
 
           bool is_empty = osal_queue_empty(_usbh_q);
@@ -443,12 +494,12 @@ void tuh_task_ext(uint32_t timeout_ms, bool in_isr) {
             // Exit if this is the only event in the queue, otherwise we may loop forever
             return;
           }
-        }else {
+        } else {
           TU_LOG_USBH("[%u:] USBH DEVICE ATTACH\r\n", event.rhport);
           _dev0.enumerating = 1;
           enum_new_device(&event);
         }
-      break;
+        break;
 
       case HCD_EVENT_DEVICE_REMOVE:
         TU_LOG_USBH("[%u:%u:%u] USBH DEVICE REMOVED\r\n", event.rhport, event.connection.hub_addr, event.connection.hub_port);
@@ -456,37 +507,34 @@ void tuh_task_ext(uint32_t timeout_ms, bool in_isr) {
 
         #if CFG_TUH_HUB
         // TODO remove
-        if ( event.connection.hub_addr != 0 && event.connection.hub_port != 0) {
+        if (event.connection.hub_addr != 0 && event.connection.hub_port != 0) {
           // done with hub, waiting for next data on status pipe
-          (void) hub_edpt_status_xfer( event.connection.hub_addr );
+          (void) hub_edpt_status_xfer(event.connection.hub_addr);
         }
         #endif
-      break;
+        break;
 
-      case HCD_EVENT_XFER_COMPLETE:
-      {
+      case HCD_EVENT_XFER_COMPLETE: {
         uint8_t const ep_addr = event.xfer_complete.ep_addr;
-        uint8_t const epnum   = tu_edpt_number(ep_addr);
-        uint8_t const ep_dir  = tu_edpt_dir(ep_addr);
+        uint8_t const epnum = tu_edpt_number(ep_addr);
+        uint8_t const ep_dir = (uint8_t) tu_edpt_dir(ep_addr);
 
-        TU_LOG_USBH("on EP %02X with %u bytes: %s\r\n", ep_addr, (unsigned int) event.xfer_complete.len,
-                    tu_str_xfer_result[event.xfer_complete.result]);
+        TU_LOG_USBH("on EP %02X with %u bytes: %s\r\n", ep_addr, (unsigned int) event.xfer_complete.len, tu_str_xfer_result[event.xfer_complete.result]);
 
         if (event.dev_addr == 0) {
           // device 0 only has control endpoint
-          TU_ASSERT(epnum == 0, );
+          TU_ASSERT(epnum == 0,);
           usbh_control_xfer_cb(event.dev_addr, ep_addr, (xfer_result_t) event.xfer_complete.result, event.xfer_complete.len);
         } else {
           usbh_device_t* dev = get_device(event.dev_addr);
-          TU_VERIFY(dev && dev->connected, );
+          TU_VERIFY(dev && dev->connected,);
 
-          dev->ep_status[epnum][ep_dir].busy    = 0;
+          dev->ep_status[epnum][ep_dir].busy = 0;
           dev->ep_status[epnum][ep_dir].claimed = 0;
 
-          if ( 0 == epnum ) {
-            usbh_control_xfer_cb(event.dev_addr, ep_addr, (xfer_result_t) event.xfer_complete.result,
-                                 event.xfer_complete.len);
-          }else {
+          if (0 == epnum) {
+            usbh_control_xfer_cb(event.dev_addr, ep_addr, (xfer_result_t) event.xfer_complete.result, event.xfer_complete.len);
+          } else {
             // Prefer application callback over built-in one if available. This occurs when tuh_edpt_xfer() is used
             // with enabled driver e.g HID endpoint
             #if CFG_TUH_API_EDPT_XFER
@@ -503,35 +551,32 @@ void tuh_task_ext(uint32_t timeout_ms, bool in_isr) {
                   .complete_cb = complete_cb,
                   .user_data   = dev->ep_callback[epnum][ep_dir].user_data
               };
-
               complete_cb(&xfer);
             }else
             #endif
             {
               uint8_t drv_id = dev->ep2drv[epnum][ep_dir];
-              usbh_class_driver_t const * driver = get_driver(drv_id);
-              if ( driver )
-              {
+              usbh_class_driver_t const* driver = get_driver(drv_id);
+              if (driver) {
                 TU_LOG_USBH("%s xfer callback\r\n", driver->name);
                 driver->xfer_cb(event.dev_addr, ep_addr, (xfer_result_t) event.xfer_complete.result,
                                 event.xfer_complete.len);
-              }
-              else
-              {
+              } else {
                 // no driver/callback responsible for this transfer
                 TU_ASSERT(false,);
               }
             }
           }
         }
+        break;
       }
-      break;
 
       case USBH_EVENT_FUNC_CALL:
-        if ( event.func_call.func ) event.func_call.func(event.func_call.param);
-      break;
+        if (event.func_call.func) event.func_call.func(event.func_call.param);
+        break;
 
-      default: break;
+      default:
+        break;
     }
 
 #if CFG_TUSB_OS != OPT_OS_NONE && CFG_TUSB_OS != OPT_OS_PICO
@@ -588,8 +633,7 @@ bool tuh_control_xfer (tuh_xfer_t* xfer) {
   TU_LOG_USBH("[%u:%u] %s: ", rhport, daddr,
               (xfer->setup->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD && xfer->setup->bRequest <= TUSB_REQ_SYNCH_FRAME) ?
                   tu_str_std_request[xfer->setup->bRequest] : "Class Request");
-  TU_LOG_BUF(CFG_TUH_LOG_LEVEL, xfer->setup, 8);
-  TU_LOG_USBH("\r\n");
+  TU_LOG_BUF_USBH(xfer->setup, 8);
 
   if (xfer->complete_cb) {
     TU_ASSERT( hcd_setup_send(rhport, daddr, (uint8_t const*) &_ctrl_xfer.request) );
@@ -630,7 +674,7 @@ TU_ATTR_ALWAYS_INLINE static inline void _set_control_xfer_stage(uint8_t stage)
   (void) osal_mutex_unlock(_usbh_mutex);
 }
 
-static void _xfer_complete(uint8_t daddr, xfer_result_t result) {
+static void _control_xfer_complete(uint8_t daddr, xfer_result_t result) {
   TU_LOG_USBH("\r\n");
 
   // duplicate xfer since user can execute control transfer within callback
@@ -653,46 +697,56 @@ static void _xfer_complete(uint8_t daddr, xfer_result_t result) {
   }
 }
 
-static bool usbh_control_xfer_cb (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
+static bool usbh_control_xfer_cb (uint8_t daddr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
   (void) ep_addr;
 
-  const uint8_t rhport = usbh_get_rhport(dev_addr);
+  const uint8_t rhport = usbh_get_rhport(daddr);
   tusb_control_request_t const * request = &_ctrl_xfer.request;
 
   if (XFER_RESULT_SUCCESS != result) {
-    TU_LOG1("[%u:%u] Control %s, xferred_bytes = %lu\r\n", rhport, dev_addr, result == XFER_RESULT_STALLED ? "STALLED" : "FAILED", xferred_bytes);
-    TU_LOG1_BUF(request, 8);
-    TU_LOG1("\r\n");
+    TU_LOG_USBH("[%u:%u] Control %s, xferred_bytes = %" PRIu32 "\r\n", rhport, daddr, result == XFER_RESULT_STALLED ? "STALLED" : "FAILED", xferred_bytes);
+    TU_LOG_BUF_USBH(request, 8);
 
     // terminate transfer if any stage failed
-    _xfer_complete(dev_addr, result);
+    _control_xfer_complete(daddr, result);
   }else {
     switch(_ctrl_xfer.stage) {
       case CONTROL_STAGE_SETUP:
         if (request->wLength) {
           // DATA stage: initial data toggle is always 1
           _set_control_xfer_stage(CONTROL_STAGE_DATA);
-          TU_ASSERT( hcd_edpt_xfer(rhport, dev_addr, tu_edpt_addr(0, request->bmRequestType_bit.direction), _ctrl_xfer.buffer, request->wLength) );
+          TU_ASSERT( hcd_edpt_xfer(rhport, daddr, tu_edpt_addr(0, request->bmRequestType_bit.direction), _ctrl_xfer.buffer, request->wLength) );
           return true;
         }
         TU_ATTR_FALLTHROUGH;
 
       case CONTROL_STAGE_DATA:
         if (request->wLength) {
-          TU_LOG_USBH("[%u:%u] Control data:\r\n", rhport, dev_addr);
-          TU_LOG_MEM(CFG_TUH_LOG_LEVEL, _ctrl_xfer.buffer, xferred_bytes, 2);
+          TU_LOG_USBH("[%u:%u] Control data:\r\n", rhport, daddr);
+          TU_LOG_MEM_USBH(_ctrl_xfer.buffer, xferred_bytes, 2);
         }
 
         _ctrl_xfer.actual_len = (uint16_t) xferred_bytes;
 
         // ACK stage: toggle is always 1
         _set_control_xfer_stage(CONTROL_STAGE_ACK);
-        TU_ASSERT( hcd_edpt_xfer(rhport, dev_addr, tu_edpt_addr(0, 1-request->bmRequestType_bit.direction), NULL, 0) );
-      break;
+        TU_ASSERT( hcd_edpt_xfer(rhport, daddr, tu_edpt_addr(0, 1 - request->bmRequestType_bit.direction), NULL, 0) );
+        break;
 
-      case CONTROL_STAGE_ACK:
-        _xfer_complete(dev_addr, result);
-      break;
+      case CONTROL_STAGE_ACK: {
+        // Abort all pending transfers if SET_CONFIGURATION request
+        // NOTE: should we force closing all non-control endpoints in the future?
+        if (request->bRequest == TUSB_REQ_SET_CONFIGURATION && request->bmRequestType == 0x00) {
+          for(uint8_t epnum=1; epnum<CFG_TUH_ENDPOINT_MAX; epnum++) {
+            for(uint8_t dir=0; dir<2; dir++) {
+              tuh_edpt_abort_xfer(daddr, tu_edpt_addr(epnum, dir));
+            }
+          }
+        }
+
+        _control_xfer_complete(daddr, result);
+        break;
+      }
 
       default: return false;
     }
@@ -710,7 +764,6 @@ bool tuh_edpt_xfer(tuh_xfer_t* xfer) {
   uint8_t const ep_addr = xfer->ep_addr;
 
   TU_VERIFY(daddr && ep_addr);
-
   TU_VERIFY(usbh_edpt_claim(daddr, ep_addr));
 
   if (!usbh_edpt_xfer_with_callback(daddr, ep_addr, xfer->buffer, (uint16_t) xfer->buflen,
@@ -743,7 +796,7 @@ bool tuh_edpt_abort_xfer(uint8_t daddr, uint8_t ep_addr) {
     TU_VERIFY(hcd_edpt_abort_xfer(dev->rhport, daddr, ep_addr));
     // mark as ready and release endpoint if transfer is aborted
     dev->ep_status[epnum][dir].busy = false;
-    usbh_edpt_release(daddr, ep_addr);
+    tu_edpt_release(&dev->ep_status[epnum][dir], _usbh_mutex);
   }
 
   return true;
@@ -785,14 +838,13 @@ void usbh_defer_func(osal_task_func_t func, void *param, bool in_isr) {
 //--------------------------------------------------------------------+
 
 // Claim an endpoint for transfer
-bool usbh_edpt_claim(uint8_t dev_addr, uint8_t ep_addr)
-{
+bool usbh_edpt_claim(uint8_t dev_addr, uint8_t ep_addr) {
   // Note: addr0 only use tuh_control_xfer
   usbh_device_t* dev = get_device(dev_addr);
   TU_ASSERT(dev && dev->connected);
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   TU_VERIFY(tu_edpt_claim(&dev->ep_status[epnum][dir], _usbh_mutex));
   TU_LOG_USBH("[%u] Claimed EP 0x%02x\r\n", dev_addr, ep_addr);
@@ -801,14 +853,13 @@ bool usbh_edpt_claim(uint8_t dev_addr, uint8_t ep_addr)
 }
 
 // Release an claimed endpoint due to failed transfer attempt
-bool usbh_edpt_release(uint8_t dev_addr, uint8_t ep_addr)
-{
+bool usbh_edpt_release(uint8_t dev_addr, uint8_t ep_addr) {
   // Note: addr0 only use tuh_control_xfer
   usbh_device_t* dev = get_device(dev_addr);
   TU_VERIFY(dev && dev->connected);
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   TU_VERIFY(tu_edpt_release(&dev->ep_status[epnum][dir], _usbh_mutex));
   TU_LOG_USBH("[%u] Released EP 0x%02x\r\n", dev_addr, ep_addr);
@@ -818,9 +869,8 @@ bool usbh_edpt_release(uint8_t dev_addr, uint8_t ep_addr)
 
 // Submit an transfer
 // TODO call usbh_edpt_release if failed
-bool usbh_edpt_xfer_with_callback(uint8_t dev_addr, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes,
-                                  tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+bool usbh_edpt_xfer_with_callback(uint8_t dev_addr, uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes,
+                                  tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   (void) complete_cb;
   (void) user_data;
 
@@ -828,7 +878,7 @@ bool usbh_edpt_xfer_with_callback(uint8_t dev_addr, uint8_t ep_addr, uint8_t * b
   TU_VERIFY(dev);
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
   tu_edpt_state_t* ep_state = &dev->ep_status[epnum][dir];
 
   TU_LOG_USBH("  Queue EP %02X with %u bytes ... \r\n", ep_addr, total_bytes);
@@ -845,14 +895,12 @@ bool usbh_edpt_xfer_with_callback(uint8_t dev_addr, uint8_t ep_addr, uint8_t * b
   dev->ep_callback[epnum][dir].user_data   = user_data;
 #endif
 
-  if ( hcd_edpt_xfer(dev->rhport, dev_addr, ep_addr, buffer, total_bytes) )
-  {
+  if (hcd_edpt_xfer(dev->rhport, dev_addr, ep_addr, buffer, total_bytes)) {
     TU_LOG_USBH("OK\r\n");
     return true;
-  }else
-  {
+  } else {
     // HCD error, mark endpoint as ready to allow next transfer
-    ep_state->busy    = 0;
+    ep_state->busy = 0;
     ep_state->claimed = 0;
     TU_LOG1("Failed\r\n");
 //    TU_BREAKPOINT();
@@ -860,12 +908,9 @@ bool usbh_edpt_xfer_with_callback(uint8_t dev_addr, uint8_t ep_addr, uint8_t * b
   }
 }
 
-static bool usbh_edpt_control_open(uint8_t dev_addr, uint8_t max_packet_size)
-{
+static bool usbh_edpt_control_open(uint8_t dev_addr, uint8_t max_packet_size) {
   TU_LOG_USBH("[%u:%u] Open EP0 with Size = %u\r\n", usbh_get_rhport(dev_addr), dev_addr, max_packet_size);
-
-  tusb_desc_endpoint_t ep0_desc =
-  {
+  tusb_desc_endpoint_t ep0_desc = {
     .bLength          = sizeof(tusb_desc_endpoint_t),
     .bDescriptorType  = TUSB_DESC_ENDPOINT,
     .bEndpointAddress = 0,
@@ -877,10 +922,8 @@ static bool usbh_edpt_control_open(uint8_t dev_addr, uint8_t max_packet_size)
   return hcd_edpt_open(usbh_get_rhport(dev_addr), dev_addr, &ep0_desc);
 }
 
-bool tuh_edpt_open(uint8_t dev_addr, tusb_desc_endpoint_t const * desc_ep)
-{
-  TU_ASSERT( tu_edpt_validate(desc_ep, tuh_speed_get(dev_addr)) );
-
+bool tuh_edpt_open(uint8_t dev_addr, tusb_desc_endpoint_t const* desc_ep) {
+  TU_ASSERT(tu_edpt_validate(desc_ep, tuh_speed_get(dev_addr)));
   return hcd_edpt_open(usbh_get_rhport(dev_addr), dev_addr, desc_ep);
 }
 
@@ -889,7 +932,7 @@ bool usbh_edpt_busy(uint8_t dev_addr, uint8_t ep_addr) {
   TU_VERIFY(dev);
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   return dev->ep_status[epnum][dir].busy;
 }
@@ -898,22 +941,18 @@ bool usbh_edpt_busy(uint8_t dev_addr, uint8_t ep_addr) {
 // HCD Event Handler
 //--------------------------------------------------------------------+
 
-void hcd_devtree_get_info(uint8_t dev_addr, hcd_devtree_info_t* devtree_info)
-{
+void hcd_devtree_get_info(uint8_t dev_addr, hcd_devtree_info_t* devtree_info) {
   usbh_device_t const* dev = get_device(dev_addr);
-
-  if (dev)
-  {
-    devtree_info->rhport   = dev->rhport;
+  if (dev) {
+    devtree_info->rhport = dev->rhport;
     devtree_info->hub_addr = dev->hub_addr;
     devtree_info->hub_port = dev->hub_port;
-    devtree_info->speed    = dev->speed;
-  }else
-  {
-    devtree_info->rhport   = _dev0.rhport;
+    devtree_info->speed = dev->speed;
+  } else {
+    devtree_info->rhport = _dev0.rhport;
     devtree_info->hub_addr = _dev0.hub_addr;
     devtree_info->hub_port = _dev0.hub_port;
-    devtree_info->speed    = _dev0.speed;
+    devtree_info->speed = _dev0.speed;
   }
 }
 
@@ -943,12 +982,9 @@ TU_ATTR_FAST_FUNC void hcd_event_handler(hcd_event_t const* event, bool in_isr)
 // generic helper to get a descriptor
 // if blocking, user_data is pointed to xfer_result
 static bool _get_descriptor(uint8_t daddr, uint8_t type, uint8_t index, uint16_t language_id, void* buffer, uint16_t len,
-                            tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
+                            tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  tusb_control_request_t const request = {
+    .bmRequestType_bit = {
       .recipient = TUSB_REQ_RCPT_DEVICE,
       .type      = TUSB_REQ_TYPE_STANDARD,
       .direction = TUSB_DIR_IN
@@ -958,9 +994,7 @@ static bool _get_descriptor(uint8_t daddr, uint8_t type, uint8_t index, uint16_t
     .wIndex   = tu_htole16(language_id),
     .wLength  = tu_htole16(len)
   };
-
-  tuh_xfer_t xfer =
-  {
+  tuh_xfer_t xfer = {
     .daddr       = daddr,
     .ep_addr     = 0,
     .setup       = &request,
@@ -973,29 +1007,25 @@ static bool _get_descriptor(uint8_t daddr, uint8_t type, uint8_t index, uint16_t
 }
 
 bool tuh_descriptor_get(uint8_t daddr, uint8_t type, uint8_t index, void* buffer, uint16_t len,
-                        tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+                        tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   return _get_descriptor(daddr, type, index, 0x0000, buffer, len, complete_cb, user_data);
 }
 
 bool tuh_descriptor_get_device(uint8_t daddr, void* buffer, uint16_t len,
-                               tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+                               tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   len = tu_min16(len, sizeof(tusb_desc_device_t));
   return tuh_descriptor_get(daddr, TUSB_DESC_DEVICE, 0, buffer, len, complete_cb, user_data);
 }
 
 bool tuh_descriptor_get_configuration(uint8_t daddr, uint8_t index, void* buffer, uint16_t len,
-                                      tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+                                      tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   return tuh_descriptor_get(daddr, TUSB_DESC_CONFIGURATION, index, buffer, len, complete_cb, user_data);
 }
 
 //------------- String Descriptor -------------//
 
 bool tuh_descriptor_get_string(uint8_t daddr, uint8_t index, uint16_t language_id, void* buffer, uint16_t len,
-                               tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+                               tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   return _get_descriptor(daddr, TUSB_DESC_STRING, index, language_id, buffer, len, complete_cb, user_data);
 }
 
@@ -1010,8 +1040,7 @@ bool tuh_descriptor_get_manufacturer_string(uint8_t daddr, uint16_t language_id,
 
 // Get product string descriptor
 bool tuh_descriptor_get_product_string(uint8_t daddr, uint16_t language_id, void* buffer, uint16_t len,
-                                       tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+                                       tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   usbh_device_t const* dev = get_device(daddr);
   TU_VERIFY(dev && dev->i_product);
   return tuh_descriptor_get_string(daddr, dev->i_product, language_id, buffer, len, complete_cb, user_data);
@@ -1019,8 +1048,7 @@ bool tuh_descriptor_get_product_string(uint8_t daddr, uint16_t language_id, void
 
 // Get serial string descriptor
 bool tuh_descriptor_get_serial_string(uint8_t daddr, uint16_t language_id, void* buffer, uint16_t len,
-                                      tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+                                      tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   usbh_device_t const* dev = get_device(daddr);
   TU_VERIFY(dev && dev->i_serial);
   return tuh_descriptor_get_string(daddr, dev->i_serial, language_id, buffer, len, complete_cb, user_data);
@@ -1029,95 +1057,78 @@ bool tuh_descriptor_get_serial_string(uint8_t daddr, uint16_t language_id, void*
 // Get HID report descriptor
 // if blocking, user_data is pointed to xfer_result
 bool tuh_descriptor_get_hid_report(uint8_t daddr, uint8_t itf_num, uint8_t desc_type, uint8_t index, void* buffer, uint16_t len,
-                                   tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+                                   tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   TU_LOG_USBH("HID Get Report Descriptor\r\n");
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_INTERFACE,
-      .type      = TUSB_REQ_TYPE_STANDARD,
-      .direction = TUSB_DIR_IN
-    },
-    .bRequest = TUSB_REQ_GET_DESCRIPTOR,
-    .wValue   = tu_htole16(TU_U16(desc_type, index)),
-    .wIndex   = tu_htole16((uint16_t) itf_num),
-    .wLength  = len
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_INTERFACE,
+          .type      = TUSB_REQ_TYPE_STANDARD,
+          .direction = TUSB_DIR_IN
+      },
+      .bRequest = TUSB_REQ_GET_DESCRIPTOR,
+      .wValue   = tu_htole16(TU_U16(desc_type, index)),
+      .wIndex   = tu_htole16((uint16_t) itf_num),
+      .wLength  = len
   };
-
-  tuh_xfer_t xfer =
-  {
-    .daddr       = daddr,
-    .ep_addr     = 0,
-    .setup       = &request,
-    .buffer      = buffer,
-    .complete_cb = complete_cb,
-    .user_data   = user_data
+  tuh_xfer_t xfer = {
+      .daddr       = daddr,
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = buffer,
+      .complete_cb = complete_cb,
+      .user_data   = user_data
   };
 
   return tuh_control_xfer(&xfer);
 }
 
 bool tuh_configuration_set(uint8_t daddr, uint8_t config_num,
-                           tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+                           tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   TU_LOG_USBH("Set Configuration = %d\r\n", config_num);
-
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_DEVICE,
-      .type      = TUSB_REQ_TYPE_STANDARD,
-      .direction = TUSB_DIR_OUT
-    },
-    .bRequest = TUSB_REQ_SET_CONFIGURATION,
-    .wValue   = tu_htole16(config_num),
-    .wIndex   = 0,
-    .wLength  = 0
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_DEVICE,
+          .type      = TUSB_REQ_TYPE_STANDARD,
+          .direction = TUSB_DIR_OUT
+      },
+      .bRequest = TUSB_REQ_SET_CONFIGURATION,
+      .wValue   = tu_htole16(config_num),
+      .wIndex   = 0,
+      .wLength  = 0
   };
-
-  tuh_xfer_t xfer =
-  {
-    .daddr       = daddr,
-    .ep_addr     = 0,
-    .setup       = &request,
-    .buffer      = NULL,
-    .complete_cb = complete_cb,
-    .user_data   = user_data
+  tuh_xfer_t xfer = {
+      .daddr       = daddr,
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = NULL,
+      .complete_cb = complete_cb,
+      .user_data   = user_data
   };
 
   return tuh_control_xfer(&xfer);
 }
 
 bool tuh_interface_set(uint8_t daddr, uint8_t itf_num, uint8_t itf_alt,
-                       tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+                       tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   TU_LOG_USBH("Set Interface %u Alternate %u\r\n", itf_num, itf_alt);
-
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_DEVICE,
-      .type      = TUSB_REQ_TYPE_STANDARD,
-      .direction = TUSB_DIR_OUT
-    },
-    .bRequest = TUSB_REQ_SET_INTERFACE,
-    .wValue   = tu_htole16(itf_alt),
-    .wIndex   = tu_htole16(itf_num),
-    .wLength  = 0
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_DEVICE,
+          .type      = TUSB_REQ_TYPE_STANDARD,
+          .direction = TUSB_DIR_OUT
+      },
+      .bRequest = TUSB_REQ_SET_INTERFACE,
+      .wValue   = tu_htole16(itf_alt),
+      .wIndex   = tu_htole16(itf_num),
+      .wLength  = 0
   };
-
-  tuh_xfer_t xfer =
-  {
-    .daddr       = daddr,
-    .ep_addr     = 0,
-    .setup       = &request,
-    .buffer      = NULL,
-    .complete_cb = complete_cb,
-    .user_data   = user_data
+  tuh_xfer_t xfer = {
+      .daddr       = daddr,
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = NULL,
+      .complete_cb = complete_cb,
+      .user_data   = user_data
   };
 
   return tuh_control_xfer(&xfer);
@@ -1132,43 +1143,42 @@ bool tuh_interface_set(uint8_t daddr, uint8_t itf_num, uint8_t itf_alt,
   TU_VERIFY(_async_func(__VA_ARGS__, NULL, (uintptr_t) &result), XFER_RESULT_TIMEOUT); \
   return (uint8_t) result
 
-uint8_t tuh_descriptor_get_sync(uint8_t daddr, uint8_t type, uint8_t index, void* buffer, uint16_t len)
-{
+uint8_t tuh_descriptor_get_sync(uint8_t daddr, uint8_t type, uint8_t index,
+                                void* buffer, uint16_t len) {
   _CONTROL_SYNC_API(tuh_descriptor_get, daddr, type, index, buffer, len);
 }
 
-uint8_t tuh_descriptor_get_device_sync(uint8_t daddr, void* buffer, uint16_t len)
-{
+uint8_t tuh_descriptor_get_device_sync(uint8_t daddr, void* buffer, uint16_t len) {
   _CONTROL_SYNC_API(tuh_descriptor_get_device, daddr, buffer, len);
 }
 
-uint8_t tuh_descriptor_get_configuration_sync(uint8_t daddr, uint8_t index, void* buffer, uint16_t len)
-{
+uint8_t tuh_descriptor_get_configuration_sync(uint8_t daddr, uint8_t index,
+                                              void* buffer, uint16_t len) {
   _CONTROL_SYNC_API(tuh_descriptor_get_configuration, daddr, index, buffer, len);
 }
 
-uint8_t tuh_descriptor_get_hid_report_sync(uint8_t daddr, uint8_t itf_num, uint8_t desc_type, uint8_t index, void* buffer, uint16_t len)
-{
+uint8_t tuh_descriptor_get_hid_report_sync(uint8_t daddr, uint8_t itf_num, uint8_t desc_type, uint8_t index,
+                                           void* buffer, uint16_t len) {
   _CONTROL_SYNC_API(tuh_descriptor_get_hid_report, daddr, itf_num, desc_type, index, buffer, len);
 }
 
-uint8_t tuh_descriptor_get_string_sync(uint8_t daddr, uint8_t index, uint16_t language_id, void* buffer, uint16_t len)
-{
+uint8_t tuh_descriptor_get_string_sync(uint8_t daddr, uint8_t index, uint16_t language_id,
+                                       void* buffer, uint16_t len) {
   _CONTROL_SYNC_API(tuh_descriptor_get_string, daddr, index, language_id, buffer, len);
 }
 
-uint8_t tuh_descriptor_get_manufacturer_string_sync(uint8_t daddr, uint16_t language_id, void* buffer, uint16_t len)
-{
+uint8_t tuh_descriptor_get_manufacturer_string_sync(uint8_t daddr, uint16_t language_id,
+                                                    void* buffer, uint16_t len) {
   _CONTROL_SYNC_API(tuh_descriptor_get_manufacturer_string, daddr, language_id, buffer, len);
 }
 
-uint8_t tuh_descriptor_get_product_string_sync(uint8_t daddr, uint16_t language_id, void* buffer, uint16_t len)
-{
+uint8_t tuh_descriptor_get_product_string_sync(uint8_t daddr, uint16_t language_id,
+                                               void* buffer, uint16_t len) {
   _CONTROL_SYNC_API(tuh_descriptor_get_product_string, daddr, language_id, buffer, len);
 }
 
-uint8_t tuh_descriptor_get_serial_string_sync(uint8_t daddr, uint16_t language_id, void* buffer, uint16_t len)
-{
+uint8_t tuh_descriptor_get_serial_string_sync(uint8_t daddr, uint16_t language_id,
+                                              void* buffer, uint16_t len) {
   _CONTROL_SYNC_API(tuh_descriptor_get_serial_string, daddr, language_id, buffer, len);
 }
 
@@ -1176,9 +1186,7 @@ uint8_t tuh_descriptor_get_serial_string_sync(uint8_t daddr, uint16_t language_i
 // Detaching
 //--------------------------------------------------------------------+
 
-TU_ATTR_ALWAYS_INLINE
-static inline bool is_hub_addr(uint8_t daddr)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool is_hub_addr(uint8_t daddr) {
   return (CFG_TUH_HUB > 0) && (daddr > CFG_TUH_DEVICE_MAX);
 }
 
@@ -1203,62 +1211,63 @@ static inline bool is_hub_addr(uint8_t daddr)
 //}
 
 // a device unplugged from rhport:hub_addr:hub_port
-static void process_removing_device(uint8_t rhport, uint8_t hub_addr, uint8_t hub_port)
-{
+static void process_removing_device(uint8_t rhport, uint8_t hub_addr, uint8_t hub_port) {
   //------------- find the all devices (star-network) under port that is unplugged -------------//
   // TODO mark as disconnected in ISR, also handle dev0
+  uint32_t removing_hubs = 0;
+  do {
+    for (uint8_t dev_id = 0; dev_id < TOTAL_DEVICES; dev_id++) {
+      usbh_device_t* dev = &_usbh_devices[dev_id];
+      uint8_t const daddr = dev_id + 1;
 
-#if 0
-  // index as hub addr, value is hub port (0xFF for invalid)
-  uint8_t removing_hubs[CFG_TUH_HUB];
-  memset(removing_hubs, TUSB_INDEX_INVALID_8, sizeof(removing_hubs));
-
-  removing_hubs[hub_addr-CFG_TUH_DEVICE_MAX] = hub_port;
+      // hub_addr = 0 means roothub, hub_port = 0 means all devices of downstream hub
+      if (dev->rhport == rhport && dev->connected &&
+          (hub_addr == 0 || dev->hub_addr == hub_addr) &&
+          (hub_port == 0 || dev->hub_port == hub_port)) {
+        TU_LOG_USBH("[%u:%u:%u] unplugged address = %u\r\n", rhport, hub_addr, hub_port, daddr);
 
-  // consecutive non-removing hub
-  uint8_t nop_count = 0;
-#endif
+        if (is_hub_addr(daddr)) {
+          TU_LOG_USBH("  is a HUB device %u\r\n", daddr);
+          removing_hubs |= TU_BIT(dev_id - CFG_TUH_DEVICE_MAX);
+        } else {
+          // Invoke callback before closing driver (maybe call it later ?)
+          if (tuh_umount_cb) tuh_umount_cb(daddr);
+        }
 
-  for (uint8_t dev_id = 0; dev_id < TOTAL_DEVICES; dev_id++)
-  {
-    usbh_device_t *dev = &_usbh_devices[dev_id];
-    uint8_t const daddr = dev_id + 1;
+        // Close class driver
+        for (uint8_t drv_id = 0; drv_id < TOTAL_DRIVER_COUNT; drv_id++) {
+          usbh_class_driver_t const* driver = get_driver(drv_id);
+          if (driver) driver->close(daddr);
+        }
 
-    // hub_addr = 0 means roothub, hub_port = 0 means all devices of downstream hub
-    if (dev->rhport == rhport && dev->connected &&
-        (hub_addr == 0 || dev->hub_addr == hub_addr) &&
-        (hub_port == 0 || dev->hub_port == hub_port)) {
-      TU_LOG_USBH("Device unplugged address = %u\r\n", daddr);
+        hcd_device_close(rhport, daddr);
+        clear_device(dev);
 
-      if (is_hub_addr(daddr)) {
-        TU_LOG_USBH("  is a HUB device %u\r\n", daddr);
+        // abort on-going control xfer on this device if any
+        if (_ctrl_xfer.daddr == daddr) _set_control_xfer_stage(CONTROL_STAGE_IDLE);
+      }
+    }
 
-        // Submit removed event If the device itself is a hub (un-rolled recursive)
-        // TODO a better to unroll recursrive is using array of removing_hubs and mark it here
-        hcd_event_t event;
-        event.rhport = rhport;
-        event.event_id = HCD_EVENT_DEVICE_REMOVE;
-        event.connection.hub_addr = daddr;
-        event.connection.hub_port = 0;
+    // if removing a hub, we need to remove its downstream devices
+    #if CFG_TUH_HUB
+    if (removing_hubs == 0) break;
 
-        hcd_event_handler(&event, false);
-      } else {
-        // Invoke callback before closing driver (maybe call it later ?)
-        if (tuh_umount_cb) tuh_umount_cb(daddr);
-      }
+    // find a marked hub to process
+    for (uint8_t h_id = 0; h_id < CFG_TUH_HUB; h_id++) {
+      if (tu_bit_test(removing_hubs, h_id)) {
+        removing_hubs &= ~TU_BIT(h_id);
 
-      // Close class driver
-      for (uint8_t drv_id = 0; drv_id < TOTAL_DRIVER_COUNT; drv_id++) {
-        usbh_class_driver_t const * driver = get_driver(drv_id);
-        if ( driver ) driver->close(daddr);
+        // update hub_addr and hub_port for next loop
+        hub_addr = h_id + 1 + CFG_TUH_DEVICE_MAX;
+        hub_port = 0;
+        break;
       }
-
-      hcd_device_close(rhport, daddr);
-      clear_device(dev);
-      // abort on-going control xfer if any
-      if (_ctrl_xfer.daddr == daddr) _set_control_xfer_stage(CONTROL_STAGE_IDLE);
     }
-  }
+    #else
+    (void) removing_hubs;
+    break;
+    #endif
+  } while(1);
 }
 
 //--------------------------------------------------------------------+
@@ -1327,227 +1336,221 @@ static void process_enumeration(tuh_xfer_t* xfer) {
   uint8_t const daddr = xfer->daddr;
   uintptr_t const state = xfer->user_data;
 
-  switch(state)
-  {
-#if CFG_TUH_HUB
+  switch (state) {
+    #if CFG_TUH_HUB
     //case ENUM_HUB_GET_STATUS_1: break;
 
-    case ENUM_HUB_CLEAR_RESET_1:
-    {
+    case ENUM_HUB_CLEAR_RESET_1: {
       hub_port_status_response_t port_status;
       memcpy(&port_status, _usbh_ctrl_buf, sizeof(hub_port_status_response_t));
 
-      if ( !port_status.status.connection )
-      {
+      if (!port_status.status.connection) {
         // device unplugged while delaying, nothing else to do
         enum_full_complete();
         return;
       }
 
       _dev0.speed = (port_status.status.high_speed) ? TUSB_SPEED_HIGH :
-                    (port_status.status.low_speed ) ? TUSB_SPEED_LOW  : TUSB_SPEED_FULL;
+                    (port_status.status.low_speed) ? TUSB_SPEED_LOW : TUSB_SPEED_FULL;
 
       // Acknowledge Port Reset Change
-      if (port_status.change.reset)
-      {
-        hub_port_clear_reset_change(_dev0.hub_addr, _dev0.hub_port, process_enumeration, ENUM_ADDR0_DEVICE_DESC);
+      if (port_status.change.reset) {
+        hub_port_clear_reset_change(_dev0.hub_addr, _dev0.hub_port,
+                                    process_enumeration, ENUM_ADDR0_DEVICE_DESC);
       }
+      break;
     }
-    break;
 
     case ENUM_HUB_GET_STATUS_2:
       osal_task_delay(ENUM_RESET_DELAY);
-      TU_ASSERT( hub_port_get_status(_dev0.hub_addr, _dev0.hub_port, _usbh_ctrl_buf, process_enumeration, ENUM_HUB_CLEAR_RESET_2), );
-    break;
+      TU_ASSERT(hub_port_get_status(_dev0.hub_addr, _dev0.hub_port, _usbh_ctrl_buf,
+                                    process_enumeration, ENUM_HUB_CLEAR_RESET_2),);
+      break;
 
-    case ENUM_HUB_CLEAR_RESET_2:
-    {
+    case ENUM_HUB_CLEAR_RESET_2: {
       hub_port_status_response_t port_status;
       memcpy(&port_status, _usbh_ctrl_buf, sizeof(hub_port_status_response_t));
 
       // Acknowledge Port Reset Change if Reset Successful
-      if (port_status.change.reset)
-      {
-        TU_ASSERT( hub_port_clear_reset_change(_dev0.hub_addr, _dev0.hub_port, process_enumeration, ENUM_SET_ADDR), );
+      if (port_status.change.reset) {
+        TU_ASSERT(hub_port_clear_reset_change(_dev0.hub_addr, _dev0.hub_port,
+                                              process_enumeration, ENUM_SET_ADDR),);
       }
+      break;
     }
-    break;
-#endif
+    #endif
 
-    case ENUM_ADDR0_DEVICE_DESC:
-    {
+    case ENUM_ADDR0_DEVICE_DESC: {
       // TODO probably doesn't need to open/close each enumeration
       uint8_t const addr0 = 0;
-      TU_ASSERT( usbh_edpt_control_open(addr0, 8), );
+      TU_ASSERT(usbh_edpt_control_open(addr0, 8),);
 
       // Get first 8 bytes of device descriptor for Control Endpoint size
       TU_LOG_USBH("Get 8 byte of Device Descriptor\r\n");
-      TU_ASSERT(tuh_descriptor_get_device(addr0, _usbh_ctrl_buf, 8, process_enumeration, ENUM_SET_ADDR), );
+      TU_ASSERT(tuh_descriptor_get_device(addr0, _usbh_ctrl_buf, 8,
+                                          process_enumeration, ENUM_SET_ADDR),);
+      break;
     }
-    break;
 
 #if 0
-    case ENUM_RESET_2:
-      // TODO not used by now, but may be needed for some devices !?
-      // Reset device again before Set Address
-      TU_LOG_USBH("Port reset2 \r\n");
-      if (_dev0.hub_addr == 0)
-      {
-        // connected directly to roothub
-        hcd_port_reset( _dev0.rhport );
-        osal_task_delay(RESET_DELAY); // TODO may not work for no-OS on MCU that require reset_end() since
-                                      // sof of controller may not running while resetting
-        hcd_port_reset_end(_dev0.rhport);
-        // TODO: fall through to SET ADDRESS, refactor later
-      }
-      #if CFG_TUH_HUB
-      else
-      {
-        // after RESET_DELAY the hub_port_reset() already complete
-        TU_ASSERT( hub_port_reset(_dev0.hub_addr, _dev0.hub_port, process_enumeration, ENUM_HUB_GET_STATUS_2), );
-        break;
-      }
-      #endif
-      TU_ATTR_FALLTHROUGH;
+      case ENUM_RESET_2:
+        // TODO not used by now, but may be needed for some devices !?
+        // Reset device again before Set Address
+        TU_LOG_USBH("Port reset2 \r\n");
+        if (_dev0.hub_addr == 0) {
+          // connected directly to roothub
+          hcd_port_reset( _dev0.rhport );
+          osal_task_delay(RESET_DELAY); // TODO may not work for no-OS on MCU that require reset_end() since
+                                        // sof of controller may not running while resetting
+          hcd_port_reset_end(_dev0.rhport);
+          // TODO: fall through to SET ADDRESS, refactor later
+        }
+#if CFG_TUH_HUB
+        else {
+          // after RESET_DELAY the hub_port_reset() already complete
+          TU_ASSERT( hub_port_reset(_dev0.hub_addr, _dev0.hub_port,
+                                    process_enumeration, ENUM_HUB_GET_STATUS_2), );
+          break;
+        }
+#endif
+        TU_ATTR_FALLTHROUGH;
 #endif
 
     case ENUM_SET_ADDR:
       enum_request_set_addr();
-    break;
+      break;
 
-    case ENUM_GET_DEVICE_DESC:
-    {
+    case ENUM_GET_DEVICE_DESC: {
       uint8_t const new_addr = (uint8_t) tu_le16toh(xfer->setup->wValue);
 
       usbh_device_t* new_dev = get_device(new_addr);
-      TU_ASSERT(new_dev, );
+      TU_ASSERT(new_dev,);
       new_dev->addressed = 1;
 
       // Close device 0
       hcd_device_close(_dev0.rhport, 0);
 
       // open control pipe for new address
-      TU_ASSERT( usbh_edpt_control_open(new_addr, new_dev->ep0_size), );
+      TU_ASSERT(usbh_edpt_control_open(new_addr, new_dev->ep0_size),);
 
       // Get full device descriptor
       TU_LOG_USBH("Get Device Descriptor\r\n");
-      TU_ASSERT(tuh_descriptor_get_device(new_addr, _usbh_ctrl_buf, sizeof(tusb_desc_device_t), process_enumeration, ENUM_GET_9BYTE_CONFIG_DESC), );
+      TU_ASSERT(tuh_descriptor_get_device(new_addr, _usbh_ctrl_buf, sizeof(tusb_desc_device_t),
+                                          process_enumeration, ENUM_GET_9BYTE_CONFIG_DESC),);
+      break;
     }
-    break;
 
-    case ENUM_GET_9BYTE_CONFIG_DESC:
-    {
-      tusb_desc_device_t const * desc_device = (tusb_desc_device_t const*) _usbh_ctrl_buf;
+    case ENUM_GET_9BYTE_CONFIG_DESC: {
+      tusb_desc_device_t const* desc_device = (tusb_desc_device_t const*) _usbh_ctrl_buf;
       usbh_device_t* dev = get_device(daddr);
-      TU_ASSERT(dev, );
+      TU_ASSERT(dev,);
 
-      dev->vid            = desc_device->idVendor;
-      dev->pid            = desc_device->idProduct;
+      dev->vid = desc_device->idVendor;
+      dev->pid = desc_device->idProduct;
       dev->i_manufacturer = desc_device->iManufacturer;
-      dev->i_product      = desc_device->iProduct;
-      dev->i_serial       = desc_device->iSerialNumber;
+      dev->i_product = desc_device->iProduct;
+      dev->i_serial = desc_device->iSerialNumber;
 
-    //  if (tuh_attach_cb) tuh_attach_cb((tusb_desc_device_t*) _usbh_ctrl_buf);
+      //  if (tuh_attach_cb) tuh_attach_cb((tusb_desc_device_t*) _usbh_ctrl_buf);
 
       // Get 9-byte for total length
       uint8_t const config_idx = CONFIG_NUM - 1;
       TU_LOG_USBH("Get Configuration[0] Descriptor (9 bytes)\r\n");
-      TU_ASSERT( tuh_descriptor_get_configuration(daddr, config_idx, _usbh_ctrl_buf, 9, process_enumeration, ENUM_GET_FULL_CONFIG_DESC), );
+      TU_ASSERT(tuh_descriptor_get_configuration(daddr, config_idx, _usbh_ctrl_buf, 9,
+                                                 process_enumeration, ENUM_GET_FULL_CONFIG_DESC),);
+      break;
     }
-    break;
 
-    case ENUM_GET_FULL_CONFIG_DESC:
-    {
-      uint8_t const * desc_config = _usbh_ctrl_buf;
+    case ENUM_GET_FULL_CONFIG_DESC: {
+      uint8_t const* desc_config = _usbh_ctrl_buf;
 
       // Use offsetof to avoid pointer to the odd/misaligned address
-      uint16_t const total_len = tu_le16toh( tu_unaligned_read16(desc_config + offsetof(tusb_desc_configuration_t, wTotalLength)) );
+      uint16_t const total_len = tu_le16toh(
+          tu_unaligned_read16(desc_config + offsetof(tusb_desc_configuration_t, wTotalLength)));
 
       // TODO not enough buffer to hold configuration descriptor
-      TU_ASSERT(total_len <= CFG_TUH_ENUMERATION_BUFSIZE, );
+      TU_ASSERT(total_len <= CFG_TUH_ENUMERATION_BUFSIZE,);
 
       // Get full configuration descriptor
       uint8_t const config_idx = CONFIG_NUM - 1;
       TU_LOG_USBH("Get Configuration[0] Descriptor\r\n");
-      TU_ASSERT( tuh_descriptor_get_configuration(daddr, config_idx, _usbh_ctrl_buf, total_len, process_enumeration, ENUM_SET_CONFIG), );
+      TU_ASSERT(tuh_descriptor_get_configuration(daddr, config_idx, _usbh_ctrl_buf, total_len,
+                                                 process_enumeration, ENUM_SET_CONFIG),);
+      break;
     }
-    break;
 
     case ENUM_SET_CONFIG:
-      // Parse configuration & set up drivers
-      // Driver open aren't allowed to make any usb transfer yet
-      TU_ASSERT( _parse_configuration_descriptor(daddr, (tusb_desc_configuration_t*) _usbh_ctrl_buf), );
-
-      TU_ASSERT( tuh_configuration_set(daddr, CONFIG_NUM, process_enumeration, ENUM_CONFIG_DRIVER), );
-    break;
+      TU_ASSERT(tuh_configuration_set(daddr, CONFIG_NUM, process_enumeration, ENUM_CONFIG_DRIVER),);
+      break;
 
-    case ENUM_CONFIG_DRIVER:
-    {
+    case ENUM_CONFIG_DRIVER: {
       TU_LOG_USBH("Device configured\r\n");
       usbh_device_t* dev = get_device(daddr);
-      TU_ASSERT(dev, );
+      TU_ASSERT(dev,);
 
       dev->configured = 1;
 
+      // Parse configuration & set up drivers
+      // driver_open() must not make any usb transfer
+      TU_ASSERT(_parse_configuration_descriptor(daddr, (tusb_desc_configuration_t*) _usbh_ctrl_buf),);
+
       // Start the Set Configuration process for interfaces (itf = TUSB_INDEX_INVALID_8)
       // Since driver can perform control transfer within its set_config, this is done asynchronously.
       // The process continue with next interface when class driver complete its sequence with usbh_driver_set_config_complete()
       // TODO use separated API instead of using TUSB_INDEX_INVALID_8
       usbh_driver_set_config_complete(daddr, TUSB_INDEX_INVALID_8);
+      break;
     }
-    break;
 
     default:
       // stop enumeration if unknown state
       enum_full_complete();
-    break;
+      break;
   }
 }
 
-static bool enum_new_device(hcd_event_t* event)
-{
-  _dev0.rhport   = event->rhport;
+static bool enum_new_device(hcd_event_t* event) {
+  _dev0.rhport = event->rhport;
   _dev0.hub_addr = event->connection.hub_addr;
   _dev0.hub_port = event->connection.hub_port;
 
-  if (_dev0.hub_addr == 0)
-  {
+  if (_dev0.hub_addr == 0) {
     // connected/disconnected directly with roothub
     hcd_port_reset(_dev0.rhport);
     osal_task_delay(ENUM_RESET_DELAY); // TODO may not work for no-OS on MCU that require reset_end() since
-                                        // sof of controller may not running while resetting
-    hcd_port_reset_end( _dev0.rhport);
+    // sof of controller may not running while resetting
+    hcd_port_reset_end(_dev0.rhport);
 
     // wait until device connection is stable TODO non blocking
     osal_task_delay(ENUM_CONTACT_DEBOUNCING_DELAY);
 
     // device unplugged while delaying
-    if ( !hcd_port_connect_status(_dev0.rhport) ) {
+    if (!hcd_port_connect_status(_dev0.rhport)) {
       enum_full_complete();
       return true;
     }
 
-    _dev0.speed = hcd_port_speed_get(_dev0.rhport );
+    _dev0.speed = hcd_port_speed_get(_dev0.rhport);
     TU_LOG_USBH("%s Speed\r\n", tu_str_speed[_dev0.speed]);
 
     // fake transfer to kick-off the enumeration process
     tuh_xfer_t xfer;
-    xfer.daddr     = 0;
-    xfer.result    = XFER_RESULT_SUCCESS;
+    xfer.daddr = 0;
+    xfer.result = XFER_RESULT_SUCCESS;
     xfer.user_data = ENUM_ADDR0_DEVICE_DESC;
 
     process_enumeration(&xfer);
   }
 #if CFG_TUH_HUB
-  else
-  {
+  else {
     // connected/disconnected via external hub
     // wait until device connection is stable TODO non blocking
     osal_task_delay(ENUM_CONTACT_DEBOUNCING_DELAY);
 
     // ENUM_HUB_GET_STATUS
     //TU_ASSERT( hub_port_get_status(_dev0.hub_addr, _dev0.hub_port, _usbh_ctrl_buf, enum_hub_get_status0_complete, 0) );
-    TU_ASSERT( hub_port_get_status(_dev0.hub_addr, _dev0.hub_port, _usbh_ctrl_buf, process_enumeration, ENUM_HUB_CLEAR_RESET_1) );
+    TU_ASSERT(hub_port_get_status(_dev0.hub_addr, _dev0.hub_port, _usbh_ctrl_buf,
+                                  process_enumeration, ENUM_HUB_CLEAR_RESET_1));
   }
 #endif // hub
 
@@ -1573,58 +1576,48 @@ static uint8_t get_new_address(bool is_hub) {
   return 0; // invalid address
 }
 
-static bool enum_request_set_addr(void)
-{
-  tusb_desc_device_t const * desc_device = (tusb_desc_device_t const*) _usbh_ctrl_buf;
+static bool enum_request_set_addr(void) {
+  tusb_desc_device_t const* desc_device = (tusb_desc_device_t const*) _usbh_ctrl_buf;
 
   // Get new address
   uint8_t const new_addr = get_new_address(desc_device->bDeviceClass == TUSB_CLASS_HUB);
   TU_ASSERT(new_addr != 0);
-
   TU_LOG_USBH("Set Address = %d\r\n", new_addr);
 
   usbh_device_t* new_dev = get_device(new_addr);
-
-  new_dev->rhport    = _dev0.rhport;
-  new_dev->hub_addr  = _dev0.hub_addr;
-  new_dev->hub_port  = _dev0.hub_port;
-  new_dev->speed     = _dev0.speed;
+  new_dev->rhport = _dev0.rhport;
+  new_dev->hub_addr = _dev0.hub_addr;
+  new_dev->hub_port = _dev0.hub_port;
+  new_dev->speed = _dev0.speed;
   new_dev->connected = 1;
-  new_dev->ep0_size  = desc_device->bMaxPacketSize0;
+  new_dev->ep0_size = desc_device->bMaxPacketSize0;
 
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_DEVICE,
-      .type      = TUSB_REQ_TYPE_STANDARD,
-      .direction = TUSB_DIR_OUT
-    },
-    .bRequest = TUSB_REQ_SET_ADDRESS,
-    .wValue   = tu_htole16(new_addr),
-    .wIndex   = 0,
-    .wLength  = 0
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_DEVICE,
+          .type      = TUSB_REQ_TYPE_STANDARD,
+          .direction = TUSB_DIR_OUT
+      },
+      .bRequest = TUSB_REQ_SET_ADDRESS,
+      .wValue   = tu_htole16(new_addr),
+      .wIndex   = 0,
+      .wLength  = 0
   };
-
-  tuh_xfer_t xfer =
-  {
-    .daddr       = 0, // dev0
-    .ep_addr     = 0,
-    .setup       = &request,
-    .buffer      = NULL,
-    .complete_cb = process_enumeration,
-    .user_data   = ENUM_GET_DEVICE_DESC
+  tuh_xfer_t xfer = {
+      .daddr       = 0, // dev0
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = NULL,
+      .complete_cb = process_enumeration,
+      .user_data   = ENUM_GET_DEVICE_DESC
   };
 
-  TU_ASSERT( tuh_control_xfer(&xfer) );
-
+  TU_ASSERT(tuh_control_xfer(&xfer));
   return true;
 }
 
-static bool _parse_configuration_descriptor(uint8_t dev_addr, tusb_desc_configuration_t const* desc_cfg)
-{
+static bool _parse_configuration_descriptor(uint8_t dev_addr, tusb_desc_configuration_t const* desc_cfg) {
   usbh_device_t* dev = get_device(dev_addr);
-
   uint16_t const total_len = tu_le16toh(desc_cfg->wTotalLength);
   uint8_t const* desc_end = ((uint8_t const*) desc_cfg) + total_len;
   uint8_t const* p_desc   = tu_desc_next(desc_cfg);
@@ -1632,13 +1625,11 @@ static bool _parse_configuration_descriptor(uint8_t dev_addr, tusb_desc_configur
   TU_LOG_USBH("Parsing Configuration descriptor (wTotalLength = %u)\r\n", total_len);
 
   // parse each interfaces
-  while( p_desc < desc_end )
-  {
+  while( p_desc < desc_end ) {
     uint8_t assoc_itf_count = 1;
 
     // Class will always starts with Interface Association (if any) and then Interface descriptor
-    if ( TUSB_DESC_INTERFACE_ASSOCIATION == tu_desc_type(p_desc) )
-    {
+    if ( TUSB_DESC_INTERFACE_ASSOCIATION == tu_desc_type(p_desc) ) {
       tusb_desc_interface_assoc_t const * desc_iad = (tusb_desc_interface_assoc_t const *) p_desc;
       assoc_itf_count = desc_iad->bInterfaceCount;
 
@@ -1658,8 +1649,7 @@ static bool _parse_configuration_descriptor(uint8_t dev_addr, tusb_desc_configur
     if (1                              == assoc_itf_count              &&
         TUSB_CLASS_AUDIO               == desc_itf->bInterfaceClass    &&
         AUDIO_SUBCLASS_CONTROL         == desc_itf->bInterfaceSubClass &&
-        AUDIO_FUNC_PROTOCOL_CODE_UNDEF == desc_itf->bInterfaceProtocol)
-    {
+        AUDIO_FUNC_PROTOCOL_CODE_UNDEF == desc_itf->bInterfaceProtocol) {
       assoc_itf_count = 2;
     }
 #endif
@@ -1669,8 +1659,7 @@ static bool _parse_configuration_descriptor(uint8_t dev_addr, tusb_desc_configur
     // manually force associated count = 2
     if (1                                        == assoc_itf_count              &&
         TUSB_CLASS_CDC                           == desc_itf->bInterfaceClass    &&
-        CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL == desc_itf->bInterfaceSubClass)
-    {
+        CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL == desc_itf->bInterfaceSubClass) {
       assoc_itf_count = 2;
     }
 #endif
@@ -1679,18 +1668,14 @@ static bool _parse_configuration_descriptor(uint8_t dev_addr, tusb_desc_configur
     TU_ASSERT(drv_len >= sizeof(tusb_desc_interface_t));
 
     // Find driver for this interface
-    for (uint8_t drv_id = 0; drv_id < TOTAL_DRIVER_COUNT; drv_id++)
-    {
+    for (uint8_t drv_id = 0; drv_id < TOTAL_DRIVER_COUNT; drv_id++) {
       usbh_class_driver_t const * driver = get_driver(drv_id);
-
-      if (driver && driver->open(dev->rhport, dev_addr, desc_itf, drv_len) )
-      {
+      if (driver && driver->open(dev->rhport, dev_addr, desc_itf, drv_len) ) {
         // open successfully
         TU_LOG_USBH("  %s opened\r\n", driver->name);
 
         // bind (associated) interfaces to found driver
-        for(uint8_t i=0; i<assoc_itf_count; i++)
-        {
+        for(uint8_t i=0; i<assoc_itf_count; i++) {
           uint8_t const itf_num = desc_itf->bInterfaceNumber+i;
 
           // Interface number must not be used already
@@ -1704,8 +1689,7 @@ static bool _parse_configuration_descriptor(uint8_t dev_addr, tusb_desc_configur
         break; // exit driver find loop
       }
 
-      if ( drv_id == TOTAL_DRIVER_COUNT - 1 )
-      {
+      if ( drv_id == TOTAL_DRIVER_COUNT - 1 ) {
         TU_LOG_USBH("[%u:%u] Interface %u: class = %u subclass = %u protocol = %u is not supported\r\n",
                dev->rhport, dev_addr, desc_itf->bInterfaceNumber, desc_itf->bInterfaceClass, desc_itf->bInterfaceSubClass, desc_itf->bInterfaceProtocol);
       }
diff --git a/src/host/usbh.h b/src/host/usbh.h
index 9ff118543..359684169 100644
--- a/src/host/usbh.h
+++ b/src/host/usbh.h
@@ -73,11 +73,25 @@ typedef struct {
   tusb_desc_interface_t desc;
 } tuh_itf_info_t;
 
-// ConfigID for tuh_config()
+// ConfigID for tuh_configure()
 enum {
-  TUH_CFGID_RPI_PIO_USB_CONFIGURATION = OPT_MCU_RP2040 << 8 // cfg_param: pio_usb_configuration_t
+  TUH_CFGID_INVALID = 0,
+  TUH_CFGID_RPI_PIO_USB_CONFIGURATION = 100, // cfg_param: pio_usb_configuration_t
+  TUH_CFGID_MAX3421 = 200,
 };
 
+typedef struct {
+  uint8_t max_nak; // max NAK per endpoint per frame
+  uint8_t cpuctl; // R16: CPU Control Register
+  uint8_t pinctl; // R17: Pin Control Register. FDUPSPI bit is ignored
+} tuh_configure_max3421_t;
+
+typedef union {
+  // For TUH_CFGID_RPI_PIO_USB_CONFIGURATION use pio_usb_configuration_t
+
+  tuh_configure_max3421_t max3421;
+} tuh_configure_param_t;
+
 //--------------------------------------------------------------------+
 // APPLICATION CALLBACK
 //--------------------------------------------------------------------+
@@ -109,7 +123,11 @@ bool tuh_configure(uint8_t rhport, uint32_t cfg_id, const void* cfg_param);
 // Init host stack
 bool tuh_init(uint8_t rhport);
 
+// Deinit host stack on rhport
+bool tuh_deinit(uint8_t rhport);
+
 // Check if host stack is already initialized with any roothub ports
+// To check if an rhport is initialized, use tuh_rhport_is_active()
 bool tuh_inited(void);
 
 // Task function should be called in main/rtos loop, extended version of tuh_task()
diff --git a/src/host/usbh_pvt.h b/src/host/usbh_pvt.h
index 4a97a1c18..95de915e9 100644
--- a/src/host/usbh_pvt.h
+++ b/src/host/usbh_pvt.h
@@ -35,7 +35,11 @@
  extern "C" {
 #endif
 
-#define TU_LOG_USBH(...)   TU_LOG(CFG_TUH_LOG_LEVEL, __VA_ARGS__)
+#define TU_LOG_USBH(...)      TU_LOG(CFG_TUH_LOG_LEVEL, __VA_ARGS__)
+#define TU_LOG_MEM_USBH(...)  TU_LOG_MEM(CFG_TUH_LOG_LEVEL, __VA_ARGS__)
+#define TU_LOG_BUF_USBH(...)  TU_LOG_BUF(CFG_TUH_LOG_LEVEL, __VA_ARGS__)
+#define TU_LOG_INT_USBH(...)  TU_LOG_INT(CFG_TUH_LOG_LEVEL, __VA_ARGS__)
+#define TU_LOG_HEX_USBH(...)  TU_LOG_HEX(CFG_TUH_LOG_LEVEL, __VA_ARGS__)
 
 enum {
   USBH_EPSIZE_BULK_MAX = (TUH_OPT_HIGH_SPEED ? TUSB_EPSIZE_BULK_HS : TUSB_EPSIZE_BULK_FS)
@@ -46,11 +50,9 @@ enum {
 //--------------------------------------------------------------------+
 
 typedef struct {
-  #if CFG_TUSB_DEBUG >= CFG_TUH_LOG_LEVEL
   char const* name;
-  #endif
-
-  void (* const init       )(void);
+  bool (* const init       )(void);
+  bool (* const deinit     )(void);
   bool (* const open       )(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
   bool (* const set_config )(uint8_t dev_addr, uint8_t itf_num);
   bool (* const xfer_cb    )(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
diff --git a/src/osal/osal.h b/src/osal/osal.h
index f092e8ffb..8f45ea5c1 100644
--- a/src/osal/osal.h
+++ b/src/osal/osal.h
@@ -74,15 +74,18 @@ typedef void (*osal_task_func_t)( void * );
 // Should be implemented as static inline function in osal_port.h header
 /*
    osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semdef);
+   bool osal_semaphore_delete(osal_semaphore_t semd_hdl);
    bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr);
    bool osal_semaphore_wait(osal_semaphore_t sem_hdl, uint32_t msec);
    void osal_semaphore_reset(osal_semaphore_t sem_hdl); // TODO removed
 
    osal_mutex_t osal_mutex_create(osal_mutex_def_t* mdef);
+   bool osal_mutex_delete(osal_mutex_t mutex_hdl)
    bool osal_mutex_lock (osal_mutex_t sem_hdl, uint32_t msec);
    bool osal_mutex_unlock(osal_mutex_t mutex_hdl);
 
    osal_queue_t osal_queue_create(osal_queue_def_t* qdef);
+   bool osal_queue_delete(osal_queue_t qhdl);
    bool osal_queue_receive(osal_queue_t qhdl, void* data, uint32_t msec);
    bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr);
    bool osal_queue_empty(osal_queue_t qhdl);
diff --git a/src/osal/osal_freertos.h b/src/osal/osal_freertos.h
index 501e0bddd..f1f05f353 100644
--- a/src/osal/osal_freertos.h
+++ b/src/osal/osal_freertos.h
@@ -24,8 +24,8 @@
  * This file is part of the TinyUSB stack.
  */
 
-#ifndef _TUSB_OSAL_FREERTOS_H_
-#define _TUSB_OSAL_FREERTOS_H_
+#ifndef TUSB_OSAL_FREERTOS_H_
+#define TUSB_OSAL_FREERTOS_H_
 
 // FreeRTOS Headers
 #include TU_INCLUDE_PATH(CFG_TUSB_OS_INC_PATH,FreeRTOS.h)
@@ -114,6 +114,11 @@ TU_ATTR_ALWAYS_INLINE static inline osal_semaphore_t osal_semaphore_create(osal_
 #endif
 }
 
+TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_delete(osal_semaphore_t semd_hdl) {
+  vSemaphoreDelete(semd_hdl);
+  return true;
+}
+
 TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr) {
   if ( !in_isr ) {
     return xSemaphoreGive(sem_hdl) != 0;
@@ -153,6 +158,11 @@ TU_ATTR_ALWAYS_INLINE static inline osal_mutex_t osal_mutex_create(osal_mutex_de
 #endif
 }
 
+TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_delete(osal_mutex_t mutex_hdl) {
+  vSemaphoreDelete(mutex_hdl);
+  return true;
+}
+
 TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_lock(osal_mutex_t mutex_hdl, uint32_t msec) {
   return osal_semaphore_wait(mutex_hdl, msec);
 }
@@ -181,6 +191,11 @@ TU_ATTR_ALWAYS_INLINE static inline osal_queue_t osal_queue_create(osal_queue_de
   return q;
 }
 
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_delete(osal_queue_t qhdl) {
+  vQueueDelete(qhdl);
+  return true;
+}
+
 TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_receive(osal_queue_t qhdl, void* data, uint32_t msec) {
   return xQueueReceive(qhdl, data, _osal_ms2tick(msec));
 }
diff --git a/src/osal/osal_mynewt.h b/src/osal/osal_mynewt.h
index b8ea2087c..16def0d2a 100644
--- a/src/osal/osal_mynewt.h
+++ b/src/osal/osal_mynewt.h
@@ -36,8 +36,7 @@
 //--------------------------------------------------------------------+
 // TASK API
 //--------------------------------------------------------------------+
-TU_ATTR_ALWAYS_INLINE static inline void osal_task_delay(uint32_t msec)
-{
+TU_ATTR_ALWAYS_INLINE static inline void osal_task_delay(uint32_t msec) {
   os_time_delay( os_time_ms_to_ticks32(msec) );
 }
 
@@ -47,25 +46,26 @@ TU_ATTR_ALWAYS_INLINE static inline void osal_task_delay(uint32_t msec)
 typedef struct os_sem  osal_semaphore_def_t;
 typedef struct os_sem* osal_semaphore_t;
 
-TU_ATTR_ALWAYS_INLINE static inline osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semdef)
-{
+TU_ATTR_ALWAYS_INLINE static inline osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semdef) {
   return (os_sem_init(semdef, 0) == OS_OK) ? (osal_semaphore_t) semdef : NULL;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_delete(osal_semaphore_t semd_hdl) {
+  (void) semd_hdl;
+  return true; // nothing to do
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr) {
   (void) in_isr;
   return os_sem_release(sem_hdl) == OS_OK;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_wait(osal_semaphore_t sem_hdl, uint32_t msec)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_wait(osal_semaphore_t sem_hdl, uint32_t msec) {
   uint32_t const ticks = (msec == OSAL_TIMEOUT_WAIT_FOREVER) ? OS_TIMEOUT_NEVER : os_time_ms_to_ticks32(msec);
   return os_sem_pend(sem_hdl, ticks) == OS_OK;
 }
 
-static inline void osal_semaphore_reset(osal_semaphore_t sem_hdl)
-{
+static inline void osal_semaphore_reset(osal_semaphore_t sem_hdl) {
   // TODO implement later
 }
 
@@ -75,19 +75,21 @@ static inline void osal_semaphore_reset(osal_semaphore_t sem_hdl)
 typedef struct os_mutex osal_mutex_def_t;
 typedef struct os_mutex* osal_mutex_t;
 
-TU_ATTR_ALWAYS_INLINE static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t* mdef)
-{
+TU_ATTR_ALWAYS_INLINE static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t* mdef) {
   return (os_mutex_init(mdef) == OS_OK) ? (osal_mutex_t) mdef : NULL;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_lock(osal_mutex_t mutex_hdl, uint32_t msec)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_delete(osal_mutex_t mutex_hdl) {
+  (void) mutex_hdl;
+  return true; // nothing to do
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_lock(osal_mutex_t mutex_hdl, uint32_t msec) {
   uint32_t const ticks = (msec == OSAL_TIMEOUT_WAIT_FOREVER) ? OS_TIMEOUT_NEVER : os_time_ms_to_ticks32(msec);
   return os_mutex_pend(mutex_hdl, ticks) == OS_OK;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl) {
   return os_mutex_release(mutex_hdl) == OS_OK;
 }
 
@@ -101,8 +103,7 @@ TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_unlock(osal_mutex_t mutex_hd
   static struct os_event _name##_##evbuf[_depth];\
   osal_queue_def_t _name = { .depth = _depth, .item_sz = sizeof(_type), .buf = _name##_##buf, .evbuf =  _name##_##evbuf};\
 
-typedef struct
-{
+typedef struct {
   uint16_t depth;
   uint16_t item_sz;
   void*    buf;
@@ -116,17 +117,20 @@ typedef struct
 
 typedef osal_queue_def_t* osal_queue_t;
 
-TU_ATTR_ALWAYS_INLINE static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef)
-{
-  if ( OS_OK != os_mempool_init(&qdef->mpool, qdef->depth, qdef->item_sz, qdef->buf, "usbd queue") ) return NULL;
-  if ( OS_OK != os_mempool_init(&qdef->epool, qdef->depth, sizeof(struct os_event), qdef->evbuf, "usbd evqueue") ) return NULL;
+TU_ATTR_ALWAYS_INLINE static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef) {
+  if ( OS_OK != os_mempool_init(&qdef->mpool, qdef->depth, qdef->item_sz, qdef->buf, "usb queue") ) return NULL;
+  if ( OS_OK != os_mempool_init(&qdef->epool, qdef->depth, sizeof(struct os_event), qdef->evbuf, "usb evqueue") ) return NULL;
 
   os_eventq_init(&qdef->evq);
   return (osal_queue_t) qdef;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_receive(osal_queue_t qhdl, void* data, uint32_t msec)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_delete(osal_queue_t qhdl) {
+  (void) qhdl;
+  return true; // nothing to do
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_receive(osal_queue_t qhdl, void* data, uint32_t msec) {
   (void) msec; // os_eventq_get() does not take timeout, always behave as msec = WAIT_FOREVER
 
   struct os_event* ev;
@@ -139,8 +143,7 @@ TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_receive(osal_queue_t qhdl, v
   return true;
 }
 
-static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr)
-{
+static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr) {
   (void) in_isr;
 
   // get a block from mem pool for data
@@ -150,8 +153,7 @@ static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in
 
   // get a block from event pool to put into queue
   struct os_event* ev = (struct os_event*) os_memblock_get(&qhdl->epool);
-  if (!ev)
-  {
+  if (!ev) {
     os_memblock_put(&qhdl->mpool, ptr);
     return false;
   }
@@ -163,8 +165,7 @@ static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in
   return true;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_empty(osal_queue_t qhdl)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_empty(osal_queue_t qhdl) {
   return STAILQ_EMPTY(&qhdl->evq.evq_list);
 }
 
diff --git a/src/osal/osal_none.h b/src/osal/osal_none.h
index a07d39828..c93f7a86c 100644
--- a/src/osal/osal_none.h
+++ b/src/osal/osal_none.h
@@ -54,6 +54,12 @@ TU_ATTR_ALWAYS_INLINE static inline osal_semaphore_t osal_semaphore_create(osal_
   return semdef;
 }
 
+TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_delete(osal_semaphore_t semd_hdl) {
+  (void) semd_hdl;
+  return true; // nothing to do
+}
+
+
 TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr) {
   (void) in_isr;
   sem_hdl->count++;
@@ -90,6 +96,11 @@ TU_ATTR_ALWAYS_INLINE static inline osal_mutex_t osal_mutex_create(osal_mutex_de
   return mdef;
 }
 
+TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_delete(osal_mutex_t mutex_hdl) {
+  (void) mutex_hdl;
+  return true; // nothing to do
+}
+
 TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_lock (osal_mutex_t mutex_hdl, uint32_t msec) {
   return osal_semaphore_wait(mutex_hdl, msec);
 }
@@ -143,6 +154,11 @@ TU_ATTR_ALWAYS_INLINE static inline osal_queue_t osal_queue_create(osal_queue_de
   return (osal_queue_t) qdef;
 }
 
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_delete(osal_queue_t qhdl) {
+  (void) qhdl;
+  return true; // nothing to do
+}
+
 TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_receive(osal_queue_t qhdl, void* data, uint32_t msec) {
   (void) msec; // not used, always behave as msec = 0
 
@@ -164,7 +180,6 @@ TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_send(osal_queue_t qhdl, void
     _osal_q_unlock(qhdl);
   }
 
-  TU_ASSERT(success);
   return success;
 }
 
diff --git a/src/osal/osal_pico.h b/src/osal/osal_pico.h
index e6efa0968..315de0950 100644
--- a/src/osal/osal_pico.h
+++ b/src/osal/osal_pico.h
@@ -24,8 +24,8 @@
  * This file is part of the TinyUSB stack.
  */
 
-#ifndef _TUSB_OSAL_PICO_H_
-#define _TUSB_OSAL_PICO_H_
+#ifndef TUSB_OSAL_PICO_H_
+#define TUSB_OSAL_PICO_H_
 
 #include "pico/time.h"
 #include "pico/sem.h"
@@ -33,42 +33,42 @@
 #include "pico/critical_section.h"
 
 #ifdef __cplusplus
- extern "C" {
+extern "C" {
 #endif
 
 //--------------------------------------------------------------------+
 // TASK API
 //--------------------------------------------------------------------+
-TU_ATTR_ALWAYS_INLINE static inline void osal_task_delay(uint32_t msec)
-{
+TU_ATTR_ALWAYS_INLINE static inline void osal_task_delay(uint32_t msec) {
   sleep_ms(msec);
 }
 
 //--------------------------------------------------------------------+
 // Binary Semaphore API
 //--------------------------------------------------------------------+
-typedef struct semaphore osal_semaphore_def_t, *osal_semaphore_t;
+typedef struct semaphore osal_semaphore_def_t, * osal_semaphore_t;
 
-TU_ATTR_ALWAYS_INLINE static inline osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semdef)
-{
+TU_ATTR_ALWAYS_INLINE static inline osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semdef) {
   sem_init(semdef, 0, 255);
   return semdef;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_delete(osal_semaphore_t semd_hdl) {
+  (void) semd_hdl;
+  return true; // nothing to do
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr) {
   (void) in_isr;
   sem_release(sem_hdl);
   return true;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_wait (osal_semaphore_t sem_hdl, uint32_t msec)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_wait(osal_semaphore_t sem_hdl, uint32_t msec) {
   return sem_acquire_timeout_ms(sem_hdl, msec);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void osal_semaphore_reset(osal_semaphore_t sem_hdl)
-{
+TU_ATTR_ALWAYS_INLINE static inline void osal_semaphore_reset(osal_semaphore_t sem_hdl) {
   sem_reset(sem_hdl, 0);
 }
 
@@ -76,21 +76,23 @@ TU_ATTR_ALWAYS_INLINE static inline void osal_semaphore_reset(osal_semaphore_t s
 // MUTEX API
 // Within tinyusb, mutex is never used in ISR context
 //--------------------------------------------------------------------+
-typedef struct mutex osal_mutex_def_t, *osal_mutex_t;
+typedef struct mutex osal_mutex_def_t, * osal_mutex_t;
 
-TU_ATTR_ALWAYS_INLINE static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t* mdef)
-{
+TU_ATTR_ALWAYS_INLINE static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t* mdef) {
   mutex_init(mdef);
   return mdef;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_lock (osal_mutex_t mutex_hdl, uint32_t msec)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_delete(osal_mutex_t mutex_hdl) {
+  (void) mutex_hdl;
+  return true; // nothing to do
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_lock(osal_mutex_t mutex_hdl, uint32_t msec) {
   return mutex_enter_timeout_ms(mutex_hdl, msec);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl) {
   mutex_exit(mutex_hdl);
   return true;
 }
@@ -100,75 +102,53 @@ TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_unlock(osal_mutex_t mutex_hd
 //--------------------------------------------------------------------+
 #include "common/tusb_fifo.h"
 
-typedef struct
-{
-    tu_fifo_t ff;
-    struct critical_section critsec; // osal_queue may be used in IRQs, so need critical section
+typedef struct {
+  tu_fifo_t ff;
+  struct critical_section critsec; // osal_queue may be used in IRQs, so need critical section
 } osal_queue_def_t;
 
 typedef osal_queue_def_t* osal_queue_t;
 
 // role device/host is used by OS NONE for mutex (disable usb isr) only
-#define OSAL_QUEUE_DEF(_int_set, _name, _depth, _type)       \
+#define OSAL_QUEUE_DEF(_int_set, _name, _depth, _type)    \
   uint8_t _name##_buf[_depth*sizeof(_type)];              \
   osal_queue_def_t _name = {                              \
     .ff = TU_FIFO_INIT(_name##_buf, _depth, _type, false) \
   }
 
-// lock queue by disable USB interrupt
-TU_ATTR_ALWAYS_INLINE static inline void _osal_q_lock(osal_queue_t qhdl)
-{
-  critical_section_enter_blocking(&qhdl->critsec);
-}
-
-// unlock queue
-TU_ATTR_ALWAYS_INLINE static inline void _osal_q_unlock(osal_queue_t qhdl)
-{
-  critical_section_exit(&qhdl->critsec);
-}
-
-TU_ATTR_ALWAYS_INLINE static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef)
-{
+TU_ATTR_ALWAYS_INLINE static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef) {
   critical_section_init(&qdef->critsec);
   tu_fifo_clear(&qdef->ff);
   return (osal_queue_t) qdef;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_receive(osal_queue_t qhdl, void* data, uint32_t msec)
-{
-  (void) msec; // not used, always behave as msec = 0
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_delete(osal_queue_t qhdl) {
+  osal_queue_def_t* qdef = (osal_queue_def_t*) qhdl;
+  critical_section_deinit(&qdef->critsec);
+  return true;
+}
 
-  // TODO: revisit... docs say that mutexes are never used from IRQ context,
-  //  however osal_queue_recieve may be. therefore my assumption is that
-  //  the fifo mutex is not populated for queues used from an IRQ context
-  //assert(!qhdl->ff.mutex);
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_receive(osal_queue_t qhdl, void* data, uint32_t msec) {
+  (void) msec; // not used, always behave as msec = 0
 
-  _osal_q_lock(qhdl);
+  critical_section_enter_blocking(&qhdl->critsec);
   bool success = tu_fifo_read(&qhdl->ff, data);
-  _osal_q_unlock(qhdl);
+  critical_section_exit(&qhdl->critsec);
 
   return success;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr)
-{
-  // TODO: revisit... docs say that mutexes are never used from IRQ context,
-  //  however osal_queue_recieve may be. therefore my assumption is that
-  //  the fifo mutex is not populated for queues used from an IRQ context
-  //assert(!qhdl->ff.mutex);
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_send(osal_queue_t qhdl, void const* data, bool in_isr) {
   (void) in_isr;
 
-  _osal_q_lock(qhdl);
+  critical_section_enter_blocking(&qhdl->critsec);
   bool success = tu_fifo_write(&qhdl->ff, data);
-  _osal_q_unlock(qhdl);
-
-  TU_ASSERT(success);
+  critical_section_exit(&qhdl->critsec);
 
   return success;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_empty(osal_queue_t qhdl)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_empty(osal_queue_t qhdl) {
   // TODO: revisit; whether this is true or not currently, tu_fifo_empty is a single
   //  volatile read.
 
@@ -178,7 +158,7 @@ TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_empty(osal_queue_t qhdl)
 }
 
 #ifdef __cplusplus
- }
+}
 #endif
 
-#endif /* _TUSB_OSAL_PICO_H_ */
+#endif
diff --git a/src/osal/osal_rtthread.h b/src/osal/osal_rtthread.h
index 18eb9c693..c27814835 100644
--- a/src/osal/osal_rtthread.h
+++ b/src/osal/osal_rtthread.h
@@ -2,6 +2,7 @@
  * The MIT License (MIT)
  *
  * Copyright (c) 2020 tfx2001 ([email protected])
+ * Copyright (c) 2020 yekai ([email protected])
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -24,8 +25,8 @@
  * This file is part of the TinyUSB stack.
  */
 
-#ifndef _TUSB_OSAL_RTTHREAD_H_
-#define _TUSB_OSAL_RTTHREAD_H_
+#ifndef TUSB_OSAL_RTTHREAD_H_
+#define TUSB_OSAL_RTTHREAD_H_
 
 // RT-Thread Headers
 #include "rtthread.h"
@@ -47,23 +48,27 @@ TU_ATTR_ALWAYS_INLINE static inline void osal_task_delay(uint32_t msec) {
 typedef struct rt_semaphore osal_semaphore_def_t;
 typedef rt_sem_t osal_semaphore_t;
 
-TU_ATTR_ALWAYS_INLINE static inline osal_semaphore_t
-osal_semaphore_create(osal_semaphore_def_t *semdef) {
-    rt_sem_init(semdef, "tusb", 0, RT_IPC_FLAG_PRIO);
-    return semdef;
+TU_ATTR_ALWAYS_INLINE static inline
+osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t *semdef) {
+  rt_sem_init(semdef, "tusb", 0, RT_IPC_FLAG_PRIO);
+  return semdef;
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_delete(osal_semaphore_t semd_hdl) {
+  return RT_EOK == rt_sem_detach(semd_hdl);
 }
 
 TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr) {
-    (void) in_isr;
-    return rt_sem_release(sem_hdl) == RT_EOK;
+  (void) in_isr;
+  return rt_sem_release(sem_hdl) == RT_EOK;
 }
 
 TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_wait(osal_semaphore_t sem_hdl, uint32_t msec) {
-    return rt_sem_take(sem_hdl, rt_tick_from_millisecond((rt_int32_t) msec)) == RT_EOK;
+  return rt_sem_take(sem_hdl, rt_tick_from_millisecond((rt_int32_t) msec)) == RT_EOK;
 }
 
 TU_ATTR_ALWAYS_INLINE static inline void osal_semaphore_reset(osal_semaphore_t const sem_hdl) {
-    rt_sem_control(sem_hdl, RT_IPC_CMD_RESET, 0);
+  rt_sem_control(sem_hdl, RT_IPC_CMD_RESET, 0);
 }
 
 //--------------------------------------------------------------------+
@@ -73,16 +78,20 @@ typedef struct rt_mutex osal_mutex_def_t;
 typedef rt_mutex_t osal_mutex_t;
 
 TU_ATTR_ALWAYS_INLINE static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t *mdef) {
-    rt_mutex_init(mdef, "tusb", RT_IPC_FLAG_PRIO);
-    return mdef;
+  rt_mutex_init(mdef, "tusb", RT_IPC_FLAG_PRIO);
+  return mdef;
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_delete(osal_mutex_t mutex_hdl) {
+  return RT_EOK == rt_mutex_detach(mutex_hdl);
 }
 
 TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_lock(osal_mutex_t mutex_hdl, uint32_t msec) {
-    return rt_mutex_take(mutex_hdl, rt_tick_from_millisecond((rt_int32_t) msec)) == RT_EOK;
+  return rt_mutex_take(mutex_hdl, rt_tick_from_millisecond((rt_int32_t) msec)) == RT_EOK;
 }
 
 TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl) {
-    return rt_mutex_release(mutex_hdl) == RT_EOK;
+  return rt_mutex_release(mutex_hdl) == RT_EOK;
 }
 
 //--------------------------------------------------------------------+
@@ -105,28 +114,35 @@ typedef struct {
 typedef rt_mq_t osal_queue_t;
 
 TU_ATTR_ALWAYS_INLINE static inline osal_queue_t osal_queue_create(osal_queue_def_t *qdef) {
-    rt_mq_init(&(qdef->sq), "tusb", qdef->buf, qdef->item_sz,
-               qdef->item_sz * qdef->depth, RT_IPC_FLAG_PRIO);
-    return &(qdef->sq);
+  rt_mq_init(&(qdef->sq), "tusb", qdef->buf, qdef->item_sz,
+             qdef->item_sz * qdef->depth, RT_IPC_FLAG_PRIO);
+  return &(qdef->sq);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_receive(osal_queue_t qhdl, void *data, uint32_t msec) {
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_delete(osal_queue_t qhdl) {
+  return RT_EOK == rt_mq_detach(qhdl);
+}
 
-    rt_tick_t tick = rt_tick_from_millisecond((rt_int32_t) msec);
-    return rt_mq_recv(qhdl, data, qhdl->msg_size, tick) == RT_EOK;
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_receive(osal_queue_t qhdl, void *data, uint32_t msec) {
+  rt_tick_t tick = rt_tick_from_millisecond((rt_int32_t) msec);
+#if RT_VERSION_MAJOR >= 5
+  return rt_mq_recv(qhdl, data, qhdl->msg_size, tick) > 0;
+#else
+  return rt_mq_recv(qhdl, data, qhdl->msg_size, tick) == RT_EOK;
+#endif  /* RT_VERSION_MAJOR >= 5 */
 }
 
 TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_send(osal_queue_t qhdl, void const *data, bool in_isr) {
-    (void) in_isr;
-    return rt_mq_send(qhdl, (void *)data, qhdl->msg_size) == RT_EOK;
+  (void) in_isr;
+  return rt_mq_send(qhdl, (void *)data, qhdl->msg_size) == RT_EOK;
 }
 
 TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_empty(osal_queue_t qhdl) {
-    return (qhdl->entry) == 0;
+  return (qhdl->entry) == 0;
 }
 
 #ifdef __cplusplus
 }
 #endif
 
-#endif /* _TUSB_OSAL_RTTHREAD_H_ */
+#endif
diff --git a/src/osal/osal_rtx4.h b/src/osal/osal_rtx4.h
index e443135e0..35909e4d6 100644
--- a/src/osal/osal_rtx4.h
+++ b/src/osal/osal_rtx4.h
@@ -25,8 +25,8 @@
  * This file is part of the TinyUSB stack.
  */
 
-#ifndef _TUSB_OSAL_RTX4_H_
-#define _TUSB_OSAL_RTX4_H_
+#ifndef TUSB_OSAL_RTX4_H_
+#define TUSB_OSAL_RTX4_H_
 
 #include <rtl.h>
 
@@ -37,8 +37,7 @@ extern "C" {
 //--------------------------------------------------------------------+
 // TASK API
 //--------------------------------------------------------------------+
-TU_ATTR_ALWAYS_INLINE static inline void osal_task_delay(uint32_t msec)
-{
+TU_ATTR_ALWAYS_INLINE static inline void osal_task_delay(uint32_t msec) {
   uint16_t hi = msec >> 16;
   uint16_t lo = msec;
   while (hi--) {
@@ -48,12 +47,13 @@ TU_ATTR_ALWAYS_INLINE static inline void osal_task_delay(uint32_t msec)
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint16_t msec2wait(uint32_t msec) {
-  if (msec == OSAL_TIMEOUT_WAIT_FOREVER)
+  if (msec == OSAL_TIMEOUT_WAIT_FOREVER) {
     return 0xFFFF;
-  else if (msec >= 0xFFFE)
+  } else if (msec >= 0xFFFE) {
     return 0xFFFE;
-  else
+  } else {
     return msec;
+  }
 }
 
 //--------------------------------------------------------------------+
@@ -67,6 +67,11 @@ TU_ATTR_ALWAYS_INLINE static inline OS_ID osal_semaphore_create(osal_semaphore_d
   return semdef;
 }
 
+TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_delete(osal_semaphore_t semd_hdl) {
+  (void) semd_hdl;
+  return true; // nothing to do
+}
+
 TU_ATTR_ALWAYS_INLINE static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr) {
   if ( !in_isr ) {
     os_sem_send(sem_hdl);
@@ -90,19 +95,21 @@ TU_ATTR_ALWAYS_INLINE static inline void osal_semaphore_reset(osal_semaphore_t c
 typedef OS_MUT osal_mutex_def_t;
 typedef OS_ID osal_mutex_t;
 
-TU_ATTR_ALWAYS_INLINE static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t* mdef)
-{
+TU_ATTR_ALWAYS_INLINE static inline osal_mutex_t osal_mutex_create(osal_mutex_def_t* mdef) {
   os_mut_init(mdef);
   return mdef;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_lock (osal_mutex_t mutex_hdl, uint32_t msec)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_delete(osal_mutex_t mutex_hdl) {
+  (void) mutex_hdl;
+  return true; // nothing to do
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_lock (osal_mutex_t mutex_hdl, uint32_t msec) {
   return os_mut_wait(mutex_hdl, msec2wait(msec)) != OS_R_TMO;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_unlock(osal_mutex_t mutex_hdl) {
   return os_mut_release(mutex_hdl) == OS_R_OK;
 }
 
@@ -116,9 +123,7 @@ TU_ATTR_ALWAYS_INLINE static inline bool osal_mutex_unlock(osal_mutex_t mutex_hd
   _declare_box(_name##__pool, sizeof(_type), _depth); \
   osal_queue_def_t _name = { .depth = _depth, .item_sz = sizeof(_type), .pool = _name##__pool, .mbox = _name##__mbox };
 
-
-typedef struct
-{
+typedef struct {
   uint16_t depth;
   uint16_t item_sz;
   U32* pool;
@@ -127,15 +132,13 @@ typedef struct
 
 typedef osal_queue_def_t* osal_queue_t;
 
-TU_ATTR_ALWAYS_INLINE static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef)
-{
+TU_ATTR_ALWAYS_INLINE static inline osal_queue_t osal_queue_create(osal_queue_def_t* qdef) {
   os_mbx_init(qdef->mbox, (qdef->depth + 4) * 4);
   _init_box(qdef->pool, ((qdef->item_sz+3)/4)*(qdef->depth) + 3, qdef->item_sz);
   return qdef;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_receive(osal_queue_t qhdl, void* data, uint32_t msec)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_receive(osal_queue_t qhdl, void* data, uint32_t msec) {
   void* buf;
   os_mbx_wait(qhdl->mbox, &buf, msec2wait(msec));
   memcpy(data, buf, qhdl->item_sz);
@@ -143,23 +146,23 @@ TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_receive(osal_queue_t qhdl, v
   return true;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_delete(osal_queue_t qhdl) {
+  (void) qhdl;
+  return true; // nothing to do ?
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr) {
   void* buf = _alloc_box(qhdl->pool);
   memcpy(buf, data, qhdl->item_sz);
-  if ( !in_isr )
-  {
+  if ( !in_isr ) {
     os_mbx_send(qhdl->mbox, buf, 0xFFFF);
-  }
-  else
-  {
+  } else {
     isr_mbx_send(qhdl->mbox, buf);
   }
   return true;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_empty(osal_queue_t qhdl)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool osal_queue_empty(osal_queue_t qhdl) {
   return os_mbx_check(qhdl->mbox) == qhdl->depth;
 }
 
diff --git a/src/portable/analog/max3421/hcd_max3421.c b/src/portable/analog/max3421/hcd_max3421.c
index cc4799dd6..4dc93d2d8 100644
--- a/src/portable/analog/max3421/hcd_max3421.c
+++ b/src/portable/analog/max3421/hcd_max3421.c
@@ -30,6 +30,7 @@
 
 #include <stdatomic.h>
 #include "host/hcd.h"
+#include "host/usbh.h"
 
 //--------------------------------------------------------------------+
 //
@@ -166,6 +167,17 @@ enum {
   DEFAULT_HIEN = HIRQ_CONDET_IRQ | HIRQ_FRAME_IRQ | HIRQ_HXFRDN_IRQ | HIRQ_RCVDAV_IRQ
 };
 
+enum {
+  MAX_NAK_DEFAULT = 1 // Number of NAK per endpoint per usb frame
+};
+
+enum {
+  EP_STATE_IDLE        = 0,
+  EP_STATE_COMPLETE    = 1,
+  EP_STATE_ATTEMPT_1   = 2, // pending 1st attempt
+  EP_STATE_ATTEMPT_MAX = 15
+};
+
 //--------------------------------------------------------------------+
 //
 //--------------------------------------------------------------------+
@@ -173,18 +185,21 @@ enum {
 typedef struct {
   uint8_t daddr;
 
-  struct TU_ATTR_PACKED {
-    uint8_t ep_dir        : 1;
-    uint8_t is_iso        : 1;
-    uint8_t is_setup      : 1;
-    uint8_t data_toggle   : 1;
-    uint8_t xfer_pending  : 1;
-    uint8_t xfer_complete : 1;
+  union { ;
+    struct TU_ATTR_PACKED {
+      uint8_t ep_num   : 4;
+      uint8_t is_setup : 1;
+      uint8_t is_out   : 1;
+      uint8_t is_iso   : 1;
+    }hxfr_bm;
+
+    uint8_t hxfr;
   };
 
   struct TU_ATTR_PACKED {
-    uint8_t ep_num : 4;
-    uint16_t packet_size : 12;
+    uint8_t state        : 4;
+    uint8_t data_toggle  : 1;
+    uint16_t packet_size : 11;
   };
 
   uint16_t total_len;
@@ -195,6 +210,8 @@ typedef struct {
 TU_VERIFY_STATIC(sizeof(max3421_ep_t) == 12, "size is not correct");
 
 typedef struct {
+  volatile uint16_t frame_count;
+
   // cached register
   uint8_t sndbc;
   uint8_t hirq;
@@ -204,18 +221,24 @@ typedef struct {
   uint8_t hxfr;
 
   atomic_flag busy; // busy transferring
-  volatile uint16_t frame_count;
-
-  max3421_ep_t ep[CFG_TUH_MAX3421_ENDPOINT_TOTAL]; // [0] is reserved for addr0
 
-  OSAL_MUTEX_DEF(spi_mutexdef);
 #if OSAL_MUTEX_REQUIRED
+  OSAL_MUTEX_DEF(spi_mutexdef);
   osal_mutex_t spi_mutex;
 #endif
+
+  max3421_ep_t ep[CFG_TUH_MAX3421_ENDPOINT_TOTAL]; // [0] is reserved for addr0
 } max3421_data_t;
 
 static max3421_data_t _hcd_data;
 
+// max NAK before giving up in a frame. 0 means infinite NAKs
+static tuh_configure_max3421_t _tuh_cfg = {
+    .max_nak = MAX_NAK_DEFAULT,
+    .cpuctl = 0, // default: INT pulse width = 10.6 us
+    .pinctl = 0, // default: negative edge interrupt
+};
+
 //--------------------------------------------------------------------+
 // API: SPI transfer with MAX3421E
 // - spi_cs_api(), spi_xfer_api(), int_api(): must be implemented by application
@@ -304,7 +327,6 @@ static void fifo_write(uint8_t rhport, uint8_t reg, uint8_t const * buffer, uint
   tuh_max3421_spi_xfer_api(rhport, buffer, NULL, len);
 
   max3421_spi_unlock(rhport, in_isr);
-
 }
 
 static void fifo_read(uint8_t rhport, uint8_t * buffer, uint16_t len, bool in_isr) {
@@ -321,35 +343,35 @@ static void fifo_read(uint8_t rhport, uint8_t * buffer, uint16_t len, bool in_is
 }
 
 //------------- register write helper -------------//
-static inline void hirq_write(uint8_t rhport, uint8_t data, bool in_isr) {
+TU_ATTR_ALWAYS_INLINE static inline void hirq_write(uint8_t rhport, uint8_t data, bool in_isr) {
   reg_write(rhport, HIRQ_ADDR, data, in_isr);
   // HIRQ write 1 is clear
   _hcd_data.hirq &= (uint8_t) ~data;
 }
 
-static inline void hien_write(uint8_t rhport, uint8_t data, bool in_isr) {
+TU_ATTR_ALWAYS_INLINE static inline void hien_write(uint8_t rhport, uint8_t data, bool in_isr) {
   _hcd_data.hien = data;
   reg_write(rhport, HIEN_ADDR, data, in_isr);
 }
 
-static inline void mode_write(uint8_t rhport, uint8_t data, bool in_isr) {
+TU_ATTR_ALWAYS_INLINE static inline void mode_write(uint8_t rhport, uint8_t data, bool in_isr) {
   _hcd_data.mode = data;
   reg_write(rhport, MODE_ADDR, data, in_isr);
 }
 
-static inline void peraddr_write(uint8_t rhport, uint8_t data, bool in_isr) {
+TU_ATTR_ALWAYS_INLINE static inline void peraddr_write(uint8_t rhport, uint8_t data, bool in_isr) {
   if ( _hcd_data.peraddr == data ) return; // no need to change address
 
   _hcd_data.peraddr = data;
   reg_write(rhport, PERADDR_ADDR, data, in_isr);
 }
 
-static inline void hxfr_write(uint8_t rhport, uint8_t data, bool in_isr) {
+TU_ATTR_ALWAYS_INLINE static inline void hxfr_write(uint8_t rhport, uint8_t data, bool in_isr) {
   _hcd_data.hxfr = data;
   reg_write(rhport, HXFR_ADDR, data, in_isr);
 }
 
-static inline void sndbc_write(uint8_t rhport, uint8_t data, bool in_isr) {
+TU_ATTR_ALWAYS_INLINE static inline void sndbc_write(uint8_t rhport, uint8_t data, bool in_isr) {
   _hcd_data.sndbc = data;
   reg_write(rhport, SNDBC_ADDR, data, in_isr);
 }
@@ -359,10 +381,11 @@ static inline void sndbc_write(uint8_t rhport, uint8_t data, bool in_isr) {
 //--------------------------------------------------------------------+
 
 static max3421_ep_t* find_ep_not_addr0(uint8_t daddr, uint8_t ep_num, uint8_t ep_dir) {
+  uint8_t const is_out = 1-ep_dir;
   for(size_t i=1; i<CFG_TUH_MAX3421_ENDPOINT_TOTAL; i++) {
     max3421_ep_t* ep = &_hcd_data.ep[i];
-    // for control endpoint, skip direction check
-    if (daddr == ep->daddr && ep_num == ep->ep_num && (ep_dir == ep->ep_dir || ep_num == 0)) {
+    // control endpoint is bi-direction (skip check)
+    if (daddr == ep->daddr && ep_num == ep->hxfr_bm.ep_num && (ep_num == 0 || is_out == ep->hxfr_bm.is_out)) {
       return ep;
     }
   }
@@ -393,14 +416,23 @@ static void free_ep(uint8_t daddr) {
   }
 }
 
+// Check if endpoint has an queued transfer and not reach max NAK
+TU_ATTR_ALWAYS_INLINE static inline bool is_ep_pending(max3421_ep_t const * ep) {
+  uint8_t const state = ep->state;
+  return ep->packet_size && (state >= EP_STATE_ATTEMPT_1) &&
+         (_tuh_cfg.max_nak == 0 || state < EP_STATE_ATTEMPT_1 + _tuh_cfg.max_nak);
+}
+
+// Find the next pending endpoint using round-robin scheduling, starting from next endpoint.
+// return NULL if not found
+// TODO respect interrupt endpoint's interval
 static max3421_ep_t * find_next_pending_ep(max3421_ep_t * cur_ep) {
   size_t const idx = (size_t) (cur_ep - _hcd_data.ep);
 
   // starting from next endpoint
   for (size_t i = idx + 1; i < CFG_TUH_MAX3421_ENDPOINT_TOTAL; i++) {
     max3421_ep_t* ep = &_hcd_data.ep[i];
-    if (ep->xfer_pending && ep->packet_size) {
-//      TU_LOG3("next pending i = %u\r\n", i);
+    if (is_ep_pending(ep)) {
       return ep;
     }
   }
@@ -408,8 +440,7 @@ static max3421_ep_t * find_next_pending_ep(max3421_ep_t * cur_ep) {
   // wrap around including current endpoint
   for (size_t i = 0; i <= idx; i++) {
     max3421_ep_t* ep = &_hcd_data.ep[i];
-    if (ep->xfer_pending && ep->packet_size) {
-//      TU_LOG3("next pending i = %u\r\n", i);
+    if (is_ep_pending(ep)) {
       return ep;
     }
   }
@@ -424,10 +455,11 @@ static max3421_ep_t * find_next_pending_ep(max3421_ep_t * cur_ep) {
 // optional hcd configuration, called by tuh_configure()
 bool hcd_configure(uint8_t rhport, uint32_t cfg_id, const void* cfg_param) {
   (void) rhport;
-  (void) cfg_id;
-  (void) cfg_param;
+  TU_VERIFY(cfg_id == TUH_CFGID_MAX3421 && cfg_param != NULL);
 
-  return false;
+  tuh_configure_param_t const* cfg = (tuh_configure_param_t const*) cfg_param;
+  _tuh_cfg = cfg->max3421;
+  return true;
 }
 
 // Initialize controller to host mode
@@ -438,6 +470,7 @@ bool hcd_init(uint8_t rhport) {
 
   TU_LOG2_INT(sizeof(max3421_ep_t));
   TU_LOG2_INT(sizeof(max3421_data_t));
+  TU_LOG2_INT(offsetof(max3421_data_t, ep));
 
   tu_memclr(&_hcd_data, sizeof(_hcd_data));
   _hcd_data.peraddr = 0xff; // invalid
@@ -446,13 +479,15 @@ bool hcd_init(uint8_t rhport) {
   _hcd_data.spi_mutex = osal_mutex_create(&_hcd_data.spi_mutexdef);
 #endif
 
+  // NOTE: driver does not seem to work without nRST pin signal
+
   // full duplex, interrupt negative edge
-  reg_write(rhport, PINCTL_ADDR, PINCTL_FDUPSPI, false);
+  reg_write(rhport, PINCTL_ADDR, _tuh_cfg.pinctl | PINCTL_FDUPSPI, false);
 
-  // V1 is 0x01, V2 is 0x12, V3 is 0x13
+  // v1 is 0x01, v2 is 0x12, v3 is 0x13
   uint8_t const revision = reg_read(rhport, REVISION_ADDR, false);
-  TU_ASSERT(revision == 0x01 || revision == 0x12 || revision == 0x13, false);
   TU_LOG2_HEX(revision);
+  TU_ASSERT(revision == 0x01 || revision == 0x12 || revision == 0x13, false);
 
   // reset
   reg_write(rhport, USBCTL_ADDR, USBCTL_CHIPRES, false);
@@ -476,7 +511,25 @@ bool hcd_init(uint8_t rhport) {
   tuh_max3421_int_api(rhport, true);
 
   // Enable Interrupt pin
-  reg_write(rhport, CPUCTL_ADDR, CPUCTL_IE, false);
+  reg_write(rhport, CPUCTL_ADDR, _tuh_cfg.cpuctl | CPUCTL_IE, false);
+
+  return true;
+}
+
+bool hcd_deinit(uint8_t rhport) {
+  (void) rhport;
+
+  // disable interrupt
+  tuh_max3421_int_api(rhport, false);
+
+  // reset max3421 and power down
+  reg_write(rhport, USBCTL_ADDR, USBCTL_CHIPRES, false);
+  reg_write(rhport, USBCTL_ADDR, USBCTL_PWRDOWN, false);
+
+  #if OSAL_MUTEX_REQUIRED
+  osal_mutex_delete(_hcd_data.spi_mutex);
+  _hcd_data.spi_mutex = NULL;
+  #endif
 
   return true;
 }
@@ -539,7 +592,6 @@ void hcd_device_close(uint8_t rhport, uint8_t dev_addr) {
 // Open an endpoint
 bool hcd_edpt_open(uint8_t rhport, uint8_t daddr, tusb_desc_endpoint_t const * ep_desc) {
   (void) rhport;
-  (void) daddr;
 
   uint8_t const ep_num = tu_edpt_number(ep_desc->bEndpointAddress);
   tusb_dir_t const ep_dir = tu_edpt_dir(ep_desc->bEndpointAddress);
@@ -551,12 +603,9 @@ bool hcd_edpt_open(uint8_t rhport, uint8_t daddr, tusb_desc_endpoint_t const * e
     ep = allocate_ep();
     TU_ASSERT(ep);
     ep->daddr = daddr;
-    ep->ep_num = (uint8_t) (ep_num & 0x0f);
-    ep->ep_dir = (ep_dir == TUSB_DIR_IN) ? 1 : 0;
-  }
-
-  if ( TUSB_XFER_ISOCHRONOUS == ep_desc->bmAttributes.xfer ) {
-    ep->is_iso = 1;
+    ep->hxfr_bm.ep_num = (uint8_t) (ep_num & 0x0f);
+    ep->hxfr_bm.is_out = (ep_dir == TUSB_DIR_OUT) ? 1 : 0;
+    ep->hxfr_bm.is_iso = (TUSB_XFER_ISOCHRONOUS == ep_desc->bmAttributes.xfer) ? 1 : 0;
   }
 
   ep->packet_size = (uint16_t) (tu_edpt_packet_size(ep_desc) & 0x7ff);
@@ -564,7 +613,7 @@ bool hcd_edpt_open(uint8_t rhport, uint8_t daddr, tusb_desc_endpoint_t const * e
   return true;
 }
 
-void xact_out(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
+static void xact_out(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
   // Page 12: Programming BULK-OUT Transfers
   // TODO double buffered
   if (switch_ep) {
@@ -580,12 +629,10 @@ void xact_out(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
     fifo_write(rhport, SNDFIFO_ADDR, ep->buf, xact_len, in_isr);
   }
   sndbc_write(rhport, xact_len, in_isr);
-
-  uint8_t const hxfr = (uint8_t ) (ep->ep_num | HXFR_OUT_NIN | (ep->is_iso ? HXFR_ISO : 0));
-  hxfr_write(rhport, hxfr, in_isr);
+  hxfr_write(rhport, ep->hxfr, in_isr);
 }
 
-void xact_in(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
+static void xact_in(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
   // Page 13: Programming BULK-IN Transfers
   if (switch_ep) {
     peraddr_write(rhport, ep->daddr, in_isr);
@@ -594,33 +641,36 @@ void xact_in(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
     reg_write(rhport, HCTL_ADDR, hctl, in_isr);
   }
 
-  uint8_t const hxfr = (uint8_t) (ep->ep_num | (ep->is_iso ? HXFR_ISO : 0));
-  hxfr_write(rhport, hxfr, in_isr);
+  hxfr_write(rhport, ep->hxfr, in_isr);
+}
+
+static void xact_setup(uint8_t rhport, max3421_ep_t *ep, bool in_isr) {
+  peraddr_write(rhport, ep->daddr, in_isr);
+  fifo_write(rhport, SUDFIFO_ADDR, ep->buf, 8, in_isr);
+  hxfr_write(rhport, HXFR_SETUP, in_isr);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void xact_inout(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
-  if (ep->ep_num == 0 ) {
+static void xact_generic(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
+  if (ep->hxfr_bm.ep_num == 0 ) {
     // setup
-    if (ep->is_setup) {
-      peraddr_write(rhport, ep->daddr, in_isr);
-      fifo_write(rhport, SUDFIFO_ADDR, ep->buf, 8, in_isr);
-      hxfr_write(rhport, HXFR_SETUP, in_isr);
+    if (ep->hxfr_bm.is_setup) {
+      xact_setup(rhport, ep, in_isr);
       return;
     }
 
     // status
     if (ep->buf == NULL || ep->total_len == 0) {
-      uint8_t const hxfr = HXFR_HS | (ep->ep_dir ? 0 : HXFR_OUT_NIN);
+      uint8_t const hxfr = (uint8_t) (HXFR_HS | (ep->hxfr & HXFR_OUT_NIN));
       peraddr_write(rhport, ep->daddr, in_isr);
       hxfr_write(rhport, hxfr, in_isr);
       return;
     }
   }
 
-  if (ep->ep_dir) {
-    xact_in(rhport, ep, switch_ep, in_isr);
-  }else {
+  if (ep->hxfr_bm.is_out) {
     xact_out(rhport, ep, switch_ep, in_isr);
+  }else {
+    xact_in(rhport, ep, switch_ep, in_isr);
   }
 }
 
@@ -632,25 +682,21 @@ bool hcd_edpt_xfer(uint8_t rhport, uint8_t daddr, uint8_t ep_addr, uint8_t * buf
   max3421_ep_t* ep = find_opened_ep(daddr, ep_num, ep_dir);
   TU_VERIFY(ep);
 
-  // control transfer can switch direction
-  ep->ep_dir = ep_dir ? 1u : 0u;
+  if (ep_num == 0) {
+    // control transfer can switch direction
+    ep->hxfr_bm.is_out = ep_dir ? 0 : 1;
+    ep->hxfr_bm.is_setup = 0;
+    ep->data_toggle = 1;
+  }
 
   ep->buf = buffer;
   ep->total_len = buflen;
   ep->xferred_len = 0;
-  ep->xfer_complete = 0;
-  ep->xfer_pending = 1;
-
-  if ( ep_num == 0 ) {
-    ep->is_setup = 0;
-    ep->data_toggle = 1;
-  }
+  ep->state = EP_STATE_ATTEMPT_1;
 
   // carry out transfer if not busy
-  if ( !atomic_flag_test_and_set(&_hcd_data.busy) ) {
-    xact_inout(rhport, ep, true, false);
-  } else {
-    return true;
+  if (!atomic_flag_test_and_set(&_hcd_data.busy)) {
+    xact_generic(rhport, ep, true, false);
   }
 
   return true;
@@ -673,17 +719,16 @@ bool hcd_setup_send(uint8_t rhport, uint8_t daddr, uint8_t const setup_packet[8]
   max3421_ep_t* ep = find_opened_ep(daddr, 0, 0);
   TU_ASSERT(ep);
 
-  ep->ep_dir = 0;
-  ep->is_setup = 1;
+  ep->hxfr_bm.is_out = 1;
+  ep->hxfr_bm.is_setup = 1;
   ep->buf = (uint8_t*)(uintptr_t) setup_packet;
   ep->total_len = 8;
   ep->xferred_len = 0;
-  ep->xfer_complete = 0;
-  ep->xfer_pending = 1;
+  ep->state = EP_STATE_ATTEMPT_1;
 
   // carry out transfer if not busy
-  if ( !atomic_flag_test_and_set(&_hcd_data.busy) ) {
-    xact_inout(rhport, ep, true, false);
+  if (!atomic_flag_test_and_set(&_hcd_data.busy)) {
+    xact_setup(rhport, ep, false);
   }
 
   return true;
@@ -748,22 +793,23 @@ static void handle_connect_irq(uint8_t rhport, bool in_isr) {
 }
 
 static void xfer_complete_isr(uint8_t rhport, max3421_ep_t *ep, xfer_result_t result, uint8_t hrsl, bool in_isr) {
-  uint8_t const ep_addr = tu_edpt_addr(ep->ep_num, ep->ep_dir);
+  uint8_t const ep_dir = 1-ep->hxfr_bm.is_out;
+  uint8_t const ep_addr = tu_edpt_addr(ep->hxfr_bm.ep_num, ep_dir);
 
   // save data toggle
-  if (ep->ep_dir) {
+  if (ep_dir) {
     ep->data_toggle = (hrsl & HRSL_RCVTOGRD) ? 1u : 0u;
   }else {
     ep->data_toggle = (hrsl & HRSL_SNDTOGRD) ? 1u : 0u;
   }
 
-  ep->xfer_pending = 0;
+  ep->state = EP_STATE_IDLE;
   hcd_event_xfer_complete(ep->daddr, ep_addr, ep->xferred_len, result, in_isr);
 
   // Find next pending endpoint
-  max3421_ep_t *next_ep = find_next_pending_ep(ep);
+  max3421_ep_t * next_ep = find_next_pending_ep(ep);
   if (next_ep) {
-    xact_inout(rhport, next_ep, true, in_isr);
+    xact_generic(rhport, next_ep, true, in_isr);
   }else {
     // no more pending
     atomic_flag_clear(&_hcd_data.busy);
@@ -793,20 +839,23 @@ static void handle_xfer_done(uint8_t rhport, bool in_isr) {
 
     case HRSL_NAK:
       if (ep_num == 0) {
-        // NAK on control, retry immediately
+        // control endpoint -> retry immediately
         hxfr_write(rhport, _hcd_data.hxfr, in_isr);
-      }else {
-        // NAK on non-control, find next pending to switch
-        max3421_ep_t *next_ep = find_next_pending_ep(ep);
+      } else {
+        if (ep->state < EP_STATE_ATTEMPT_MAX) {
+          ep->state++;
+        }
 
+        max3421_ep_t * next_ep = find_next_pending_ep(ep);
         if (ep == next_ep) {
-          // this endpoint is only one pending, retry immediately
+          // this endpoint is only one pending -> retry immediately
           hxfr_write(rhport, _hcd_data.hxfr, in_isr);
-        }else if (next_ep) {
-          // switch to next pending TODO could have issue with double buffered if not clear previously out data
-          xact_inout(rhport, next_ep, true, in_isr);
-        }else {
-          TU_ASSERT(false,);
+        } else if (next_ep) {
+          // switch to next pending endpoint TODO could have issue with double buffered if not clear previously out data
+          xact_generic(rhport, next_ep, true, in_isr);
+        } else {
+          // no more pending in this frame -> clear busy
+          atomic_flag_clear(&_hcd_data.busy);
         }
       }
       return;
@@ -828,12 +877,14 @@ static void handle_xfer_done(uint8_t rhport, bool in_isr) {
 
   if (ep_dir) {
     // IN transfer: fifo data is already received in RCVDAV IRQ
-    if ( hxfr_type & HXFR_HS ) {
-      ep->xfer_complete = 1;
+
+    // mark control handshake as complete
+    if (hxfr_type & HXFR_HS) {
+      ep->state = EP_STATE_COMPLETE;
     }
 
     // short packet or all bytes transferred
-    if ( ep->xfer_complete ) {
+    if (ep->state == EP_STATE_COMPLETE) {
       xfer_complete_isr(rhport, ep, xfer_result, hrsl, in_isr);
     }else {
       // more to transfer
@@ -867,13 +918,13 @@ static void handle_xfer_done(uint8_t rhport, bool in_isr) {
 void print_hirq(uint8_t hirq) {
   TU_LOG3_HEX(hirq);
 
-  if (hirq & HIRQ_HXFRDN_IRQ) TU_LOG3(" HXFRDN");
-  if (hirq & HIRQ_FRAME_IRQ)  TU_LOG3(" FRAME");
-  if (hirq & HIRQ_CONDET_IRQ) TU_LOG3(" CONDET");
-  if (hirq & HIRQ_SUSDN_IRQ)  TU_LOG3(" SUSDN");
-  if (hirq & HIRQ_SNDBAV_IRQ) TU_LOG3(" SNDBAV");
-  if (hirq & HIRQ_RCVDAV_IRQ) TU_LOG3(" RCVDAV");
-  if (hirq & HIRQ_RWU_IRQ)    TU_LOG3(" RWU");
+  if (hirq & HIRQ_HXFRDN_IRQ)   TU_LOG3(" HXFRDN");
+  if (hirq & HIRQ_FRAME_IRQ)    TU_LOG3(" FRAME");
+  if (hirq & HIRQ_CONDET_IRQ)   TU_LOG3(" CONDET");
+  if (hirq & HIRQ_SUSDN_IRQ)    TU_LOG3(" SUSDN");
+  if (hirq & HIRQ_SNDBAV_IRQ)   TU_LOG3(" SNDBAV");
+  if (hirq & HIRQ_RCVDAV_IRQ)   TU_LOG3(" RCVDAV");
+  if (hirq & HIRQ_RWU_IRQ)      TU_LOG3(" RWU");
   if (hirq & HIRQ_BUSEVENT_IRQ) TU_LOG3(" BUSEVENT");
 
   TU_LOG3("\r\n");
@@ -890,6 +941,25 @@ void hcd_int_handler(uint8_t rhport, bool in_isr) {
 
   if (hirq & HIRQ_FRAME_IRQ) {
     _hcd_data.frame_count++;
+
+    max3421_ep_t* ep_retry = NULL;
+
+    // reset all endpoints attempt counter
+    for (size_t i = 0; i < CFG_TUH_MAX3421_ENDPOINT_TOTAL; i++) {
+      max3421_ep_t* ep = &_hcd_data.ep[i];
+      if (ep->packet_size && ep->state > EP_STATE_ATTEMPT_1) {
+        ep->state = EP_STATE_ATTEMPT_1;
+
+        if (ep_retry == NULL) {
+          ep_retry = ep;
+        }
+      }
+    }
+
+    // start usb transfer if not busy
+    if (ep_retry != NULL && !atomic_flag_test_and_set(&_hcd_data.busy)) {
+      xact_generic(rhport, ep_retry, true, in_isr);
+    }
   }
 
   if (hirq & HIRQ_CONDET_IRQ) {
@@ -898,17 +968,17 @@ void hcd_int_handler(uint8_t rhport, bool in_isr) {
 
   // queue more transfer in handle_xfer_done() can cause hirq to be set again while external IRQ may not catch and/or
   // not call this handler again. So we need to loop until all IRQ are cleared
-  while ( hirq & (HIRQ_RCVDAV_IRQ | HIRQ_HXFRDN_IRQ) ) {
-    if ( hirq & HIRQ_RCVDAV_IRQ ) {
+  while (hirq & (HIRQ_RCVDAV_IRQ | HIRQ_HXFRDN_IRQ)) {
+    if (hirq & HIRQ_RCVDAV_IRQ) {
       uint8_t const ep_num = _hcd_data.hxfr & HXFR_EPNUM_MASK;
-      max3421_ep_t *ep = find_opened_ep(_hcd_data.peraddr, ep_num, 1);
+      max3421_ep_t* ep = find_opened_ep(_hcd_data.peraddr, ep_num, 1);
       uint8_t xact_len = 0;
 
       // RCVDAV_IRQ can trigger 2 times (dual buffered)
-      while ( hirq & HIRQ_RCVDAV_IRQ ) {
+      while (hirq & HIRQ_RCVDAV_IRQ) {
         uint8_t rcvbc = reg_read(rhport, RCVBC_ADDR, in_isr);
         xact_len = (uint8_t) tu_min16(rcvbc, ep->total_len - ep->xferred_len);
-        if ( xact_len ) {
+        if (xact_len) {
           fifo_read(rhport, ep->buf, xact_len, in_isr);
           ep->buf += xact_len;
           ep->xferred_len += xact_len;
@@ -919,12 +989,12 @@ void hcd_int_handler(uint8_t rhport, bool in_isr) {
         hirq = reg_read(rhport, HIRQ_ADDR, in_isr);
       }
 
-      if ( xact_len < ep->packet_size || ep->xferred_len >= ep->total_len ) {
-        ep->xfer_complete = 1;
+      if (xact_len < ep->packet_size || ep->xferred_len >= ep->total_len) {
+        ep->state = EP_STATE_COMPLETE;
       }
     }
 
-    if ( hirq & HIRQ_HXFRDN_IRQ ) {
+    if (hirq & HIRQ_HXFRDN_IRQ) {
       hirq_write(rhport, HIRQ_HXFRDN_IRQ, in_isr);
       handle_xfer_done(rhport, in_isr);
     }
diff --git a/src/portable/chipidea/ci_fs/ci_fs_kinetis.h b/src/portable/chipidea/ci_fs/ci_fs_kinetis.h
index cd21af1c7..31e14a546 100644
--- a/src/portable/chipidea/ci_fs/ci_fs_kinetis.h
+++ b/src/portable/chipidea/ci_fs/ci_fs_kinetis.h
@@ -36,14 +36,12 @@
 #define CI_FS_REG(_port)        ((ci_fs_regs_t*) USB0_BASE)
 #define CI_REG                  CI_FS_REG(0)
 
-void dcd_int_enable(uint8_t rhport)
-{
+void dcd_int_enable(uint8_t rhport) {
   (void) rhport;
   NVIC_EnableIRQ(USB0_IRQn);
 }
 
-void dcd_int_disable(uint8_t rhport)
-{
+void dcd_int_disable(uint8_t rhport) {
   (void) rhport;
   NVIC_DisableIRQ(USB0_IRQn);
 }
diff --git a/src/portable/chipidea/ci_fs/ci_fs_mcx.h b/src/portable/chipidea/ci_fs/ci_fs_mcx.h
index 3bfcd398e..4b93a03a7 100644
--- a/src/portable/chipidea/ci_fs/ci_fs_mcx.h
+++ b/src/portable/chipidea/ci_fs/ci_fs_mcx.h
@@ -29,19 +29,28 @@
 
 #include "fsl_device_registers.h"
 
-#define CI_FS_REG(_port)        ((ci_fs_regs_t*) USBFS0_BASE)
-#define CI_REG                  CI_FS_REG(0)
+#if CFG_TUSB_MCU == OPT_MCU_MCXN9
+  #define CI_FS_REG(_port)  ((ci_fs_regs_t*) USBFS0_BASE)
+  #define CIFS_IRQN 				USB0_FS_IRQn
 
-void dcd_int_enable(uint8_t rhport)
-{
+#elif CFG_TUSB_MCU == OPT_MCU_MCXA15
+  #define CI_FS_REG(_port)  ((ci_fs_regs_t*) USB0_BASE)
+  #define CIFS_IRQN         USB0_IRQn
+
+#else
+  #error "MCU is not supported"
+#endif
+
+#define CI_REG              CI_FS_REG(0)
+
+void dcd_int_enable(uint8_t rhport) {
   (void) rhport;
-  NVIC_EnableIRQ(USB0_FS_IRQn);
+  NVIC_EnableIRQ(CIFS_IRQN);
 }
 
-void dcd_int_disable(uint8_t rhport)
-{
+void dcd_int_disable(uint8_t rhport) {
   (void) rhport;
-  NVIC_DisableIRQ(USB0_FS_IRQn);
+  NVIC_DisableIRQ(CIFS_IRQN);
 }
 
 #endif
diff --git a/src/portable/chipidea/ci_fs/dcd_ci_fs.c b/src/portable/chipidea/ci_fs/dcd_ci_fs.c
index 9327e09d8..02f813ab5 100644
--- a/src/portable/chipidea/ci_fs/dcd_ci_fs.c
+++ b/src/portable/chipidea/ci_fs/dcd_ci_fs.c
@@ -271,9 +271,21 @@ void dcd_init(uint8_t rhport)
 {
   (void) rhport;
 
+  // save crystal-less setting (if available)
+  #if defined(FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED) && FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED == 1
+  uint32_t clk_recover_irc_en = CI_REG->CLK_RECOVER_IRC_EN;
+  uint32_t clk_recover_ctrl = CI_REG->CLK_RECOVER_CTRL;
+  #endif
+
   CI_REG->USBTRC0 |= USB_USBTRC0_USBRESET_MASK;
   while (CI_REG->USBTRC0 & USB_USBTRC0_USBRESET_MASK);
 
+  // restore crystal-less setting (if available)
+  #if defined(FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED) && FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED == 1
+  CI_REG->CLK_RECOVER_IRC_EN = clk_recover_irc_en;
+  CI_REG->CLK_RECOVER_CTRL  |= clk_recover_ctrl;
+  #endif
+
   tu_memclr(&_dcd, sizeof(_dcd));
   CI_REG->USBTRC0 |= TU_BIT(6); /* software must set this bit to 1 */
   CI_REG->BDT_PAGE1 = (uint8_t)((uintptr_t)_dcd.bdt >> 8);
@@ -283,7 +295,7 @@ void dcd_init(uint8_t rhport)
   CI_REG->INT_EN = USB_INTEN_USBRSTEN_MASK;
 
   dcd_connect(rhport);
-  // NVIC_ClearPendingIRQ(USB0_IRQn);
+  // NVIC_ClearPendingIRQ(CIFS_IRQN);
 }
 
 void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
diff --git a/src/portable/ehci/ehci.c b/src/portable/ehci/ehci.c
index e145cbb1b..01bbf62bf 100644
--- a/src/portable/ehci/ehci.c
+++ b/src/portable/ehci/ehci.c
@@ -92,7 +92,7 @@ CFG_TUH_MEM_SECTION TU_ATTR_ALIGNED(4096) static ehci_data_t ehci_data;
 //--------------------------------------------------------------------+
 // Debug
 //--------------------------------------------------------------------+
-#if CFG_TUSB_DEBUG >= (EHCI_DBG + 1)
+#if 0 && CFG_TUSB_DEBUG >= (EHCI_DBG + 1)
 static inline void print_portsc(ehci_registers_t* regs) {
   TU_LOG_HEX(EHCI_DBG, regs->portsc);
   TU_LOG(EHCI_DBG, "  Connect Status : %u\r\n", regs->portsc_bm.current_connect_status);
diff --git a/src/portable/espressif/esp32sx/dcd_esp32sx.c b/src/portable/espressif/esp32sx/dcd_esp32sx.c
index 1fcfb6241..bfc0baa56 100644
--- a/src/portable/espressif/esp32sx/dcd_esp32sx.c
+++ b/src/portable/espressif/esp32sx/dcd_esp32sx.c
@@ -31,16 +31,26 @@
 #if (((CFG_TUSB_MCU == OPT_MCU_ESP32S2) ||  (CFG_TUSB_MCU == OPT_MCU_ESP32S3)) && CFG_TUD_ENABLED)
 
 // Espressif
-#include "freertos/xtensa_api.h"
+#include "xtensa_api.h"
 #include "esp_intr_alloc.h"
 #include "esp_log.h"
 #include "soc/dport_reg.h"
 #include "soc/gpio_sig_map.h"
 #include "soc/usb_periph.h"
+#include "soc/usb_reg.h"
+#include "soc/usb_struct.h"
 #include "soc/periph_defs.h" // for interrupt source
 
 #include "device/dcd.h"
 
+#ifndef USB_OUT_EP_NUM
+#define USB_OUT_EP_NUM ((int) (sizeof(USB0.out_ep_reg) / sizeof(USB0.out_ep_reg[0])))
+#endif
+
+#ifndef USB_IN_EP_NUM
+#define USB_IN_EP_NUM ((int) (sizeof(USB0.in_ep_reg) / sizeof(USB0.in_ep_reg[0])))
+#endif
+
 // Max number of bi-directional endpoints including EP0
 // Note: ESP32S2 specs say there are only up to 5 IN active endpoints include EP0
 // We should probably prohibit enabling Endpoint IN > 4 (not done yet)
diff --git a/src/portable/mindmotion/mm32/dcd_mm32f327x_otg.c b/src/portable/mindmotion/mm32/dcd_mm32f327x_otg.c
index c3d0c7297..d5c0daaeb 100644
--- a/src/portable/mindmotion/mm32/dcd_mm32f327x_otg.c
+++ b/src/portable/mindmotion/mm32/dcd_mm32f327x_otg.c
@@ -283,7 +283,18 @@ void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
   /* Response with status first before changing device address */
   dcd_edpt_xfer(rhport, tu_edpt_addr(0, TUSB_DIR_IN), NULL, 0);
 }
+
+#ifdef __GNUC__ // caused by extra declaration of SystemCoreClock in freeRTOSConfig.h
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wredundant-decls"
+#endif
+
 extern u32 SystemCoreClock;
+
+#ifdef __GNUC__
+#pragma GCC diagnostic pop
+#endif
+
 void dcd_remote_wakeup(uint8_t rhport)
 {
   (void) rhport;
diff --git a/src/portable/nordic/nrf5x/dcd_nrf5x.c b/src/portable/nordic/nrf5x/dcd_nrf5x.c
index 4e702aed4..2fe721d6b 100644
--- a/src/portable/nordic/nrf5x/dcd_nrf5x.c
+++ b/src/portable/nordic/nrf5x/dcd_nrf5x.c
@@ -40,7 +40,6 @@
 
 #include "nrf.h"
 #include "nrf_clock.h"
-#include "nrf_power.h"
 #include "nrfx_usbd_errata.h"
 
 #ifdef __GNUC__
@@ -57,28 +56,46 @@
 #include "mcu/mcu.h"
 #endif
 
+/* Try to detect nrfx version if not configured with CFG_TUD_NRF_NRFX_VERSION
+ * nrfx v1 and v2 are concurrently developed. There is no NRFX_VERSION only MDK VERSION which is as follows:
+ * - v3.0.0: 8.53.1 (conflict with v2.11.0), v3.1.0: 8.55.0 ...
+ * - v2.11.0: 8.53.1, v2.6.0: 8.44.1, v2.5.0: 8.40.2, v2.4.0: 8.37.0, v2.3.0: 8.35.0, v2.2.0: 8.32.1, v2.1.0: 8.30.2, v2.0.0: 8.29.0
+ * - v1.9.0: 8.40.3, v1.8.6: 8.35.0 (conflict with v2.3.0), v1.8.5: 8.32.3, v1.8.4: 8.32.1 (conflict with v2.2.0),
+ *   v1.8.2: 8.32.1 (conflict with v2.2.0), v1.8.1: 8.27.1
+ * Therefore the check for v1 would be:
+ * - MDK < 8.29.0 (v2.0), MDK == 8.32.3, 8.40.3
+ * - in case of conflict User of those version must upgrade to other 1.x version or set CFG_TUD_NRF_NRFX_VERSION
+*/
+#ifndef CFG_TUD_NRF_NRFX_VERSION
+  #define _MDK_VERSION  (10000*MDK_MAJOR_VERSION + 100*MDK_MINOR_VERSION + MDK_MICRO_VERSION)
+
+  #if _MDK_VERSION < 82900 || _MDK_VERSION == 83203 || _MDK_VERSION == 84003
+    // nrfx <= 1.8.1, or 1.8.5 or 1.9.0
+    #define CFG_TUD_NRF_NRFX_VERSION 1
+  #else
+    #define CFG_TUD_NRF_NRFX_VERSION 2
+  #endif
+#endif
+
 /*------------------------------------------------------------------*/
 /* MACRO TYPEDEF CONSTANT ENUM
  *------------------------------------------------------------------*/
-enum
-{
+enum {
   // Max allowed by USB specs
-  MAX_PACKET_SIZE   = 64,
+  MAX_PACKET_SIZE = 64,
 
   // Mask of all END event (IN & OUT) for all endpoints. ENDEPIN0-7, ENDEPOUT0-7, ENDISOIN, ENDISOOUT
   EDPT_END_ALL_MASK = (0xff << USBD_INTEN_ENDEPIN0_Pos) | (0xff << USBD_INTEN_ENDEPOUT0_Pos) |
                       USBD_INTENCLR_ENDISOIN_Msk | USBD_INTEN_ENDISOOUT_Msk
 };
 
-enum
-{
-  EP_ISO_NUM   = 8, // Endpoint number is fixed (8) for ISOOUT and ISOIN
+enum {
+  EP_ISO_NUM = 8, // Endpoint number is fixed (8) for ISOOUT and ISOIN
   EP_CBI_COUNT = 8  // Control Bulk Interrupt endpoints count
 };
 
 // Transfer Descriptor
-typedef struct
-{
+typedef struct {
   uint8_t* buffer;
   uint16_t total_len;
   volatile uint16_t actual_len;
@@ -96,113 +113,82 @@ typedef struct
 } xfer_td_t;
 
 // Data for managing dcd
-static struct
-{
+static struct {
   // All 8 endpoints including control IN & OUT (offset 1)
   // +1 for ISO endpoints
   xfer_td_t xfer[EP_CBI_COUNT + 1][2];
 
   // nRF can only carry one DMA at a time, this is used to guard the access to EasyDMA
-  atomic_bool dma_running;
-}_dcd;
+  atomic_flag dma_running;
+} _dcd;
 
 /*------------------------------------------------------------------*/
 /* Control / Bulk / Interrupt (CBI) Transfer
  *------------------------------------------------------------------*/
 
-// NVIC_GetEnableIRQ is only available in CMSIS v5
-#ifndef NVIC_GetEnableIRQ
-static inline uint32_t NVIC_GetEnableIRQ(IRQn_Type IRQn)
-{
-  if ((int32_t)(IRQn) >= 0)
-  {
-    return((uint32_t)(((NVIC->ISER[(((uint32_t)(int32_t)IRQn) >> 5UL)] & (1UL << (((uint32_t)(int32_t)IRQn) & 0x1FUL))) != 0UL) ? 1UL : 0UL));
-  }
-  else
-  {
-    return(0U);
-  }
-}
-#endif
-
 // check if we are in ISR
-TU_ATTR_ALWAYS_INLINE static inline bool is_in_isr(void)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool is_in_isr(void) {
   return (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) ? true : false;
 }
 
 // helper to start DMA
-static void start_dma(volatile uint32_t* reg_startep)
-{
+static void start_dma(volatile uint32_t* reg_startep) {
   (*reg_startep) = 1;
-  __ISB(); __DSB();
+  __ISB();
+  __DSB();
 
   // TASKS_EP0STATUS, TASKS_EP0RCVOUT seem to need EasyDMA to be available
   // However these don't trigger any DMA transfer and got ENDED event subsequently
   // Therefore dma_pending is corrected right away
-  if ( (reg_startep == &NRF_USBD->TASKS_EP0STATUS) || (reg_startep == &NRF_USBD->TASKS_EP0RCVOUT) )
-  {
+  if ((reg_startep == &NRF_USBD->TASKS_EP0STATUS) || (reg_startep == &NRF_USBD->TASKS_EP0RCVOUT)) {
     atomic_flag_clear(&_dcd.dma_running);
   }
 }
 
-static void edpt_dma_start(volatile uint32_t* reg_startep)
-{
-  if ( atomic_flag_test_and_set(&_dcd.dma_running) )
-  {
-    usbd_defer_func((osal_task_func_t) edpt_dma_start, (void*) (uintptr_t) reg_startep, true);
-  }else
-  {
+static void edpt_dma_start(volatile uint32_t* reg_startep) {
+  if (atomic_flag_test_and_set(&_dcd.dma_running)) {
+    usbd_defer_func((osal_task_func_t)(uintptr_t ) edpt_dma_start, (void*) (uintptr_t) reg_startep, true);
+  } else {
     start_dma(reg_startep);
   }
 }
 
 // DMA is complete
-static void edpt_dma_end(void)
-{
-  TU_ASSERT(_dcd.dma_running, );
+static void edpt_dma_end(void) {
   atomic_flag_clear(&_dcd.dma_running);
 }
 
 // helper getting td
-static inline xfer_td_t* get_td(uint8_t epnum, uint8_t dir)
-{
+static inline xfer_td_t* get_td(uint8_t epnum, uint8_t dir) {
   return &_dcd.xfer[epnum][dir];
 }
 
 static void xact_out_dma(uint8_t epnum);
+
 // Function wraps xact_out_dma which wants uint8_t while usbd_defer_func wants void (*)(void *)
-static void xact_out_dma_wrapper(void *epnum)
-{
-  xact_out_dma((uint8_t)((uintptr_t)epnum));
+static void xact_out_dma_wrapper(void* epnum) {
+  xact_out_dma((uint8_t) ((uintptr_t) epnum));
 }
 
 // Start DMA to move data from Endpoint -> RAM
-static void xact_out_dma(uint8_t epnum)
-{
+static void xact_out_dma(uint8_t epnum) {
   xfer_td_t* xfer = get_td(epnum, TUSB_DIR_OUT);
   uint32_t xact_len;
 
   // DMA can't be active during read of SIZE.EPOUT or SIZE.ISOOUT, so try to lock,
   // If already running defer call regardless if it was called from ISR or task,
-  if ( atomic_flag_test_and_set(&_dcd.dma_running) )
-  {
-    usbd_defer_func((osal_task_func_t)xact_out_dma_wrapper, (void *)(uint32_t)epnum, is_in_isr());
+  if (atomic_flag_test_and_set(&_dcd.dma_running)) {
+    usbd_defer_func((osal_task_func_t) xact_out_dma_wrapper, (void*) (uint32_t) epnum, is_in_isr());
     return;
   }
-  if (epnum == EP_ISO_NUM)
-  {
+  if (epnum == EP_ISO_NUM) {
     xact_len = NRF_USBD->SIZE.ISOOUT;
     // If ZERO bit is set, ignore ISOOUT length
-    if (xact_len & USBD_SIZE_ISOOUT_ZERO_Msk)
-    {
+    if (xact_len & USBD_SIZE_ISOOUT_ZERO_Msk) {
       xact_len = 0;
       atomic_flag_clear(&_dcd.dma_running);
-    }
-    else
-    {
-      if (xfer->started)
-      {
+    } else {
+      if (xfer->started) {
         // Trigger DMA move data from Endpoint -> SRAM
         NRF_USBD->ISOOUT.PTR = (uint32_t) xfer->buffer;
         NRF_USBD->ISOOUT.MAXCNT = xact_len;
@@ -212,9 +198,7 @@ static void xact_out_dma(uint8_t epnum)
         atomic_flag_clear(&_dcd.dma_running);
       }
     }
-  }
-  else
-  {
+  } else {
     // limit xact len to remaining length
     xact_len = tu_min16((uint16_t) NRF_USBD->SIZE.EPOUT[epnum], xfer->total_len - xfer->actual_len);
 
@@ -228,14 +212,13 @@ static void xact_out_dma(uint8_t epnum)
 
 // Prepare for a CBI transaction IN, call at the start
 // it start DMA to transfer data from RAM -> Endpoint
-static void xact_in_dma(uint8_t epnum)
-{
+static void xact_in_dma(uint8_t epnum) {
   xfer_td_t* xfer = get_td(epnum, TUSB_DIR_IN);
 
   // Each transaction is up to Max Packet Size
   uint16_t const xact_len = tu_min16(xfer->total_len - xfer->actual_len, xfer->mps);
 
-  NRF_USBD->EPIN[epnum].PTR    = (uint32_t) xfer->buffer;
+  NRF_USBD->EPIN[epnum].PTR = (uint32_t) xfer->buffer;
   NRF_USBD->EPIN[epnum].MAXCNT = xact_len;
 
   edpt_dma_start(&NRF_USBD->TASKS_STARTEPIN[epnum]);
@@ -244,26 +227,22 @@ static void xact_in_dma(uint8_t epnum)
 //--------------------------------------------------------------------+
 // Controller API
 //--------------------------------------------------------------------+
-void dcd_init (uint8_t rhport)
-{
-  TU_LOG1("dcd init\r\n");
+void dcd_init(uint8_t rhport) {
+  TU_LOG2("dcd init\r\n");
   (void) rhport;
 }
 
-void dcd_int_enable(uint8_t rhport)
-{
+void dcd_int_enable(uint8_t rhport) {
   (void) rhport;
   NVIC_EnableIRQ(USBD_IRQn);
 }
 
-void dcd_int_disable(uint8_t rhport)
-{
+void dcd_int_disable(uint8_t rhport) {
   (void) rhport;
   NVIC_DisableIRQ(USBD_IRQn);
 }
 
-void dcd_set_address (uint8_t rhport, uint8_t dev_addr)
-{
+void dcd_set_address(uint8_t rhport, uint8_t dev_addr) {
   (void) rhport;
   (void) dev_addr;
   // Set Address is automatically update by hw controller, nothing to do
@@ -278,8 +257,7 @@ void dcd_set_address (uint8_t rhport, uint8_t dev_addr)
   NRF_USBD->INTENSET = USBD_INTEN_USBEVENT_Msk;
 }
 
-void dcd_remote_wakeup(uint8_t rhport)
-{
+void dcd_remote_wakeup(uint8_t rhport) {
   (void) rhport;
 
   // Bring controller out of low power mode
@@ -288,8 +266,7 @@ void dcd_remote_wakeup(uint8_t rhport)
 }
 
 // disconnect by disabling internal pull-up resistor on D+/D-
-void dcd_disconnect(uint8_t rhport)
-{
+void dcd_disconnect(uint8_t rhport) {
   (void) rhport;
   NRF_USBD->USBPULLUP = 0;
 
@@ -299,14 +276,12 @@ void dcd_disconnect(uint8_t rhport)
 }
 
 // connect by enabling internal pull-up resistor on D+/D-
-void dcd_connect(uint8_t rhport)
-{
+void dcd_connect(uint8_t rhport) {
   (void) rhport;
   NRF_USBD->USBPULLUP = 1;
 }
 
-void dcd_sof_enable(uint8_t rhport, bool en)
-{
+void dcd_sof_enable(uint8_t rhport, bool en) {
   (void) rhport;
   (void) en;
 
@@ -316,39 +291,32 @@ void dcd_sof_enable(uint8_t rhport, bool en)
 //--------------------------------------------------------------------+
 // Endpoint API
 //--------------------------------------------------------------------+
-bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
-{
+bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const* desc_edpt) {
   (void) rhport;
 
   uint8_t const ep_addr = desc_edpt->bEndpointAddress;
-  uint8_t const epnum   = tu_edpt_number(ep_addr);
-  uint8_t const dir     = tu_edpt_dir(ep_addr);
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   _dcd.xfer[epnum][dir].mps = tu_edpt_packet_size(desc_edpt);
 
-  if (desc_edpt->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS)
-  {
-    if (dir == TUSB_DIR_OUT)
-    {
+  if (desc_edpt->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS) {
+    if (dir == TUSB_DIR_OUT) {
       NRF_USBD->INTENSET = TU_BIT(USBD_INTEN_ENDEPOUT0_Pos + epnum);
       NRF_USBD->EPOUTEN |= TU_BIT(epnum);
 
       // Write any value to SIZE register will allow nRF to ACK/accept data
       NRF_USBD->SIZE.EPOUT[epnum] = 0;
-    }else
-    {
+    } else {
       NRF_USBD->INTENSET = TU_BIT(USBD_INTEN_ENDEPIN0_Pos + epnum);
-      NRF_USBD->EPINEN  |= TU_BIT(epnum);
+      NRF_USBD->EPINEN |= TU_BIT(epnum);
     }
     // clear stall and reset DataToggle
     NRF_USBD->EPSTALL = (USBD_EPSTALL_STALL_UnStall << USBD_EPSTALL_STALL_Pos) | ep_addr;
     NRF_USBD->DTOGGLE = (USBD_DTOGGLE_VALUE_Data0 << USBD_DTOGGLE_VALUE_Pos) | ep_addr;
-  }
-  else
-  {
+  } else {
     TU_ASSERT(epnum == EP_ISO_NUM);
-    if (dir == TUSB_DIR_OUT)
-    {
+    if (dir == TUSB_DIR_OUT) {
       // SPLIT ISO buffer when ISO IN endpoint is already opened.
       if (_dcd.xfer[EP_ISO_NUM][TUSB_DIR_IN].mps) NRF_USBD->ISOSPLIT = USBD_ISOSPLIT_SPLIT_HalfIN;
 
@@ -361,9 +329,7 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
       // Enable SOF and ISOOUT interrupts, and ISOOUT endpoint.
       NRF_USBD->INTENSET = USBD_INTENSET_ENDISOOUT_Msk | USBD_INTENSET_SOF_Msk;
       NRF_USBD->EPOUTEN |= USBD_EPOUTEN_ISOOUT_Msk;
-    }
-    else
-    {
+    } else {
       NRF_USBD->EVENTS_ENDISOIN = 0;
 
       // SPLIT ISO buffer when ISO OUT endpoint is already opened.
@@ -374,39 +340,38 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
 
       // Enable SOF and ISOIN interrupts, and ISOIN endpoint.
       NRF_USBD->INTENSET = USBD_INTENSET_ENDISOIN_Msk | USBD_INTENSET_SOF_Msk;
-      NRF_USBD->EPINEN  |= USBD_EPINEN_ISOIN_Msk;
+      NRF_USBD->EPINEN |= USBD_EPINEN_ISOIN_Msk;
     }
   }
 
-  __ISB(); __DSB();
+  __ISB();
+  __DSB();
 
   return true;
 }
 
-void dcd_edpt_close_all (uint8_t rhport)
-{
+void dcd_edpt_close_all(uint8_t rhport) {
   // disable interrupt to prevent race condition
   dcd_int_disable(rhport);
 
   // disable all non-control (bulk + interrupt) endpoints
-  for ( uint8_t ep = 1; ep < EP_CBI_COUNT; ep++ )
-  {
+  for (uint8_t ep = 1; ep < EP_CBI_COUNT; ep++) {
     NRF_USBD->INTENCLR = TU_BIT(USBD_INTEN_ENDEPOUT0_Pos + ep) | TU_BIT(USBD_INTEN_ENDEPIN0_Pos + ep);
 
     NRF_USBD->TASKS_STARTEPIN[ep] = 0;
     NRF_USBD->TASKS_STARTEPOUT[ep] = 0;
 
-    tu_memclr(_dcd.xfer[ep], 2*sizeof(xfer_td_t));
+    tu_memclr(_dcd.xfer[ep], 2 * sizeof(xfer_td_t));
   }
 
   // disable both ISO
   NRF_USBD->INTENCLR = USBD_INTENCLR_SOF_Msk | USBD_INTENCLR_ENDISOOUT_Msk | USBD_INTENCLR_ENDISOIN_Msk;
   NRF_USBD->ISOSPLIT = USBD_ISOSPLIT_SPLIT_OneDir;
 
-  NRF_USBD->TASKS_STARTISOIN  = 0;
+  NRF_USBD->TASKS_STARTISOIN = 0;
   NRF_USBD->TASKS_STARTISOOUT = 0;
 
-  tu_memclr(_dcd.xfer[EP_ISO_NUM], 2*sizeof(xfer_td_t));
+  tu_memclr(_dcd.xfer[EP_ISO_NUM], 2 * sizeof(xfer_td_t));
 
   // de-activate all non-control
   NRF_USBD->EPOUTEN = 1UL;
@@ -415,107 +380,89 @@ void dcd_edpt_close_all (uint8_t rhport)
   dcd_int_enable(rhport);
 }
 
-void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr)
-{
+void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr) {
   (void) rhport;
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
-  if (epnum != EP_ISO_NUM)
-  {
+  if (epnum != EP_ISO_NUM) {
     // CBI
-    if (dir == TUSB_DIR_OUT)
-    {
+    if (dir == TUSB_DIR_OUT) {
       NRF_USBD->INTENCLR = TU_BIT(USBD_INTEN_ENDEPOUT0_Pos + epnum);
       NRF_USBD->EPOUTEN &= ~TU_BIT(epnum);
-    }
-    else
-    {
+    } else {
       NRF_USBD->INTENCLR = TU_BIT(USBD_INTEN_ENDEPIN0_Pos + epnum);
       NRF_USBD->EPINEN &= ~TU_BIT(epnum);
     }
-  }
-  else
-  {
+  } else {
     _dcd.xfer[EP_ISO_NUM][dir].mps = 0;
     // ISO
-    if (dir == TUSB_DIR_OUT)
-    {
+    if (dir == TUSB_DIR_OUT) {
       NRF_USBD->INTENCLR = USBD_INTENCLR_ENDISOOUT_Msk;
       NRF_USBD->EPOUTEN &= ~USBD_EPOUTEN_ISOOUT_Msk;
       NRF_USBD->EVENTS_ENDISOOUT = 0;
-    }
-    else
-    {
+    } else {
       NRF_USBD->INTENCLR = USBD_INTENCLR_ENDISOIN_Msk;
       NRF_USBD->EPINEN &= ~USBD_EPINEN_ISOIN_Msk;
     }
     // One of the ISO endpoints closed, no need to split buffers any more.
     NRF_USBD->ISOSPLIT = USBD_ISOSPLIT_SPLIT_OneDir;
     // When both ISO endpoint are close there is no need for SOF any more.
-    if (_dcd.xfer[EP_ISO_NUM][TUSB_DIR_IN].mps + _dcd.xfer[EP_ISO_NUM][TUSB_DIR_OUT].mps == 0) NRF_USBD->INTENCLR = USBD_INTENCLR_SOF_Msk;
+    if (_dcd.xfer[EP_ISO_NUM][TUSB_DIR_IN].mps + _dcd.xfer[EP_ISO_NUM][TUSB_DIR_OUT].mps == 0)
+      NRF_USBD->INTENCLR = USBD_INTENCLR_SOF_Msk;
   }
   _dcd.xfer[epnum][dir].started = false;
-  __ISB(); __DSB();
+  __ISB();
+  __DSB();
 }
 
-bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
-{
+bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes) {
   (void) rhport;
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   xfer_td_t* xfer = get_td(epnum, dir);
 
   TU_ASSERT(!xfer->started);
-  xfer->buffer     = buffer;
-  xfer->total_len  = total_bytes;
+  xfer->buffer = buffer;
+  xfer->total_len = total_bytes;
   xfer->actual_len = 0;
 
   // Control endpoint with zero-length packet and opposite direction to 1st request byte --> status stage
   bool const control_status = (epnum == 0 && total_bytes == 0 && dir != tu_edpt_dir(NRF_USBD->BMREQUESTTYPE));
 
-  if ( control_status )
-  {
+  if (control_status) {
     // Status Phase also requires EasyDMA has to be available as well !!!!
     edpt_dma_start(&NRF_USBD->TASKS_EP0STATUS);
 
     // The nRF doesn't interrupt on status transmit so we queue up a success response.
     dcd_event_xfer_complete(0, ep_addr, 0, XFER_RESULT_SUCCESS, is_in_isr());
-  }
-  else if ( dir == TUSB_DIR_OUT )
-  {
+  } else if (dir == TUSB_DIR_OUT) {
     xfer->started = true;
-    if ( epnum == 0 )
-    {
+    if (epnum == 0) {
       // Accept next Control Out packet. TASKS_EP0RCVOUT also require EasyDMA
       edpt_dma_start(&NRF_USBD->TASKS_EP0RCVOUT);
-    }else
-    {
+    } else {
       // started just set, it could start DMA transfer if interrupt was trigger after this line
       // code only needs to start transfer (from Endpoint to RAM) when data_received was set
       // before started was set. If started is NOT set but data_received is, it means that
       // current transfer was already finished and next data is already present in endpoint and
       // can be consumed by future transfer
-      __ISB(); __DSB();
-      if ( xfer->data_received && xfer->started )
-      {
+      __ISB();
+      __DSB();
+      if (xfer->data_received && xfer->started) {
         // Data is already received previously
         // start DMA to copy to SRAM
         xfer->data_received = false;
         xact_out_dma(epnum);
-      }
-      else
-      {
+      } else {
         // nRF auto accept next Bulk/Interrupt OUT packet
         // nothing to do
       }
     }
-  }
-  else
-  {
+  } else {
     // Start DMA to copy data from RAM -> Endpoint
     xact_in_dma(epnum);
   }
@@ -523,42 +470,37 @@ bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
   return true;
 }
 
-void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
-{
+void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr) {
   (void) rhport;
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   xfer_td_t* xfer = get_td(epnum, dir);
 
-  if ( epnum == 0 )
-  {
+  if (epnum == 0) {
     NRF_USBD->TASKS_EP0STALL = 1;
-  }else if (epnum != EP_ISO_NUM)
-  {
+  } else if (epnum != EP_ISO_NUM) {
     NRF_USBD->EPSTALL = (USBD_EPSTALL_STALL_Stall << USBD_EPSTALL_STALL_Pos) | ep_addr;
 
     // Note: nRF can auto ACK packet OUT before get stalled.
     // There maybe data in endpoint fifo already, we need to pull it out
-    if ( (dir == TUSB_DIR_OUT) && xfer->data_received )
-    {
+    if ((dir == TUSB_DIR_OUT) && xfer->data_received) {
       xfer->data_received = false;
       xact_out_dma(epnum);
     }
   }
 
-  __ISB(); __DSB();
+  __ISB();
+  __DSB();
 }
 
-void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
-{
+void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr) {
   (void) rhport;
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
-  if ( epnum != 0 && epnum != EP_ISO_NUM )
-  {
+  if (epnum != 0 && epnum != EP_ISO_NUM) {
     // reset data toggle to DATA0
     // First write this register with VALUE=Nop to select the endpoint, then either read it to get the status from
     // VALUE, or write it again with VALUE=Data0 or Data1
@@ -571,26 +513,25 @@ void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
     // Write any value to SIZE register will allow nRF to ACK/accept data
     if (dir == TUSB_DIR_OUT) NRF_USBD->SIZE.EPOUT[epnum] = 0;
 
-    __ISB(); __DSB();
+    __ISB();
+    __DSB();
   }
 }
 
 /*------------------------------------------------------------------*/
 /* Interrupt Handler
  *------------------------------------------------------------------*/
-void bus_reset(void)
-{
+void bus_reset(void) {
   // 6.35.6 USB controller automatically disabled all endpoints (except control)
   NRF_USBD->EPOUTEN = 1UL;
   NRF_USBD->EPINEN = 1UL;
 
-  for(int i=0; i<8; i++)
-  {
+  for (int i = 0; i < 8; i++) {
     NRF_USBD->TASKS_STARTEPIN[i] = 0;
     NRF_USBD->TASKS_STARTEPOUT[i] = 0;
   }
 
-  NRF_USBD->TASKS_STARTISOIN  = 0;
+  NRF_USBD->TASKS_STARTISOIN = 0;
   NRF_USBD->TASKS_STARTISOOUT = 0;
 
   // Clear USB Event Interrupt
@@ -600,43 +541,40 @@ void bus_reset(void)
   // Reset interrupt
   NRF_USBD->INTENCLR = NRF_USBD->INTEN;
   NRF_USBD->INTENSET = USBD_INTEN_USBRESET_Msk | USBD_INTEN_USBEVENT_Msk | USBD_INTEN_EPDATA_Msk |
-          USBD_INTEN_EP0SETUP_Msk | USBD_INTEN_EP0DATADONE_Msk | USBD_INTEN_ENDEPIN0_Msk | USBD_INTEN_ENDEPOUT0_Msk;
+                       USBD_INTEN_EP0SETUP_Msk | USBD_INTEN_EP0DATADONE_Msk | USBD_INTEN_ENDEPIN0_Msk |
+                       USBD_INTEN_ENDEPOUT0_Msk;
 
   tu_varclr(&_dcd);
   _dcd.xfer[0][TUSB_DIR_IN].mps = MAX_PACKET_SIZE;
   _dcd.xfer[0][TUSB_DIR_OUT].mps = MAX_PACKET_SIZE;
 }
 
-void dcd_int_handler(uint8_t rhport)
-{
+void dcd_int_handler(uint8_t rhport) {
   (void) rhport;
 
-  uint32_t const inten  = NRF_USBD->INTEN;
+  uint32_t const inten = NRF_USBD->INTEN;
   uint32_t int_status = 0;
 
   volatile uint32_t* regevt = &NRF_USBD->EVENTS_USBRESET;
 
-  for(uint8_t i=0; i<USBD_INTEN_EPDATA_Pos+1; i++)
-  {
-    if ( tu_bit_test(inten, i) && regevt[i]  )
-    {
+  for (uint8_t i = 0; i < USBD_INTEN_EPDATA_Pos + 1; i++) {
+    if (tu_bit_test(inten, i) && regevt[i]) {
       int_status |= TU_BIT(i);
 
       // event clear
       regevt[i] = 0;
-      __ISB(); __DSB();
+      __ISB();
+      __DSB();
     }
   }
 
-  if ( int_status & USBD_INTEN_USBRESET_Msk )
-  {
+  if (int_status & USBD_INTEN_USBRESET_Msk) {
     bus_reset();
     dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
   }
 
   // ISOIN: Data was moved to endpoint buffer, client will be notified in SOF
-  if ( int_status & USBD_INTEN_ENDISOIN_Msk )
-  {
+  if (int_status & USBD_INTEN_ENDISOIN_Msk) {
     xfer_td_t* xfer = get_td(EP_ISO_NUM, TUSB_DIR_IN);
 
     xfer->actual_len = NRF_USBD->ISOIN.AMOUNT;
@@ -645,32 +583,31 @@ void dcd_int_handler(uint8_t rhport)
     xfer->iso_in_transfer_ready = true;
   }
 
-  if ( int_status & USBD_INTEN_SOF_Msk )
-  {
+  if (int_status & USBD_INTEN_SOF_Msk) {
     bool iso_enabled = false;
 
     // ISOOUT: Transfer data gathered in previous frame from buffer to RAM
-    if (NRF_USBD->EPOUTEN & USBD_EPOUTEN_ISOOUT_Msk)
-    {
+    if (NRF_USBD->EPOUTEN & USBD_EPOUTEN_ISOOUT_Msk) {
       iso_enabled = true;
-      xact_out_dma(EP_ISO_NUM);
+      // Transfer from endpoint to RAM only if data is not corrupted
+      if ((int_status & USBD_INTEN_USBEVENT_Msk) == 0 ||
+          (NRF_USBD->EVENTCAUSE & USBD_EVENTCAUSE_ISOOUTCRC_Msk) == 0) {
+        xact_out_dma(EP_ISO_NUM);
+      }
     }
 
     // ISOIN: Notify client that data was transferred
-    if (NRF_USBD->EPINEN & USBD_EPINEN_ISOIN_Msk)
-    {
+    if (NRF_USBD->EPINEN & USBD_EPINEN_ISOIN_Msk) {
       iso_enabled = true;
 
       xfer_td_t* xfer = get_td(EP_ISO_NUM, TUSB_DIR_IN);
-      if ( xfer->iso_in_transfer_ready )
-      {
+      if (xfer->iso_in_transfer_ready) {
         xfer->iso_in_transfer_ready = false;
         dcd_event_xfer_complete(0, EP_ISO_NUM | TUSB_DIR_IN_MASK, xfer->actual_len, XFER_RESULT_SUCCESS, true);
       }
     }
 
-    if ( !iso_enabled )
-    {
+    if (!iso_enabled) {
       // ISO endpoint is not used, SOF is only enabled one-time for remote wakeup
       // so we disable it now
       NRF_USBD->INTENCLR = USBD_INTENSET_SOF_Msk;
@@ -679,16 +616,17 @@ void dcd_int_handler(uint8_t rhport)
     dcd_event_bus_signal(0, DCD_EVENT_SOF, true);
   }
 
-  if ( int_status & USBD_INTEN_USBEVENT_Msk )
-  {
-    TU_LOG(2, "EVENTCAUSE = 0x%04lX\r\n", NRF_USBD->EVENTCAUSE);
+  if (int_status & USBD_INTEN_USBEVENT_Msk) {
+    TU_LOG(3, "EVENTCAUSE = 0x%04" PRIX32 "\r\n", NRF_USBD->EVENTCAUSE);
 
-    enum { EVT_CAUSE_MASK = USBD_EVENTCAUSE_SUSPEND_Msk | USBD_EVENTCAUSE_RESUME_Msk | USBD_EVENTCAUSE_USBWUALLOWED_Msk };
+    enum {
+      EVT_CAUSE_MASK = USBD_EVENTCAUSE_SUSPEND_Msk | USBD_EVENTCAUSE_RESUME_Msk | USBD_EVENTCAUSE_USBWUALLOWED_Msk |
+                       USBD_EVENTCAUSE_ISOOUTCRC_Msk
+    };
     uint32_t const evt_cause = NRF_USBD->EVENTCAUSE & EVT_CAUSE_MASK;
     NRF_USBD->EVENTCAUSE = evt_cause; // clear interrupt
 
-    if ( evt_cause & USBD_EVENTCAUSE_SUSPEND_Msk )
-    {
+    if (evt_cause & USBD_EVENTCAUSE_SUSPEND_Msk) {
       // Put controller into low power mode
       // Leave HFXO disable to application, since it may be used by other peripherals
       NRF_USBD->LOWPOWER = 1;
@@ -696,8 +634,7 @@ void dcd_int_handler(uint8_t rhport)
       dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
     }
 
-    if ( evt_cause & USBD_EVENTCAUSE_USBWUALLOWED_Msk )
-    {
+    if (evt_cause & USBD_EVENTCAUSE_USBWUALLOWED_Msk) {
       // USB is out of low power mode, and wakeup is allowed
       // Initiate RESUME signal
       NRF_USBD->DPDMVALUE = USBD_DPDMVALUE_STATE_Resume;
@@ -709,34 +646,29 @@ void dcd_int_handler(uint8_t rhport)
       NRF_USBD->INTENSET = USBD_INTENSET_SOF_Msk;
     }
 
-    if ( evt_cause & USBD_EVENTCAUSE_RESUME_Msk )
-    {
+    if (evt_cause & USBD_EVENTCAUSE_RESUME_Msk) {
       dcd_event_bus_signal(0, DCD_EVENT_RESUME, true);
     }
   }
 
   // Setup tokens are specific to the Control endpoint.
-  if ( int_status & USBD_INTEN_EP0SETUP_Msk )
-  {
-    uint8_t const setup[8] =
-    {
-      NRF_USBD->BMREQUESTTYPE , NRF_USBD->BREQUEST, NRF_USBD->WVALUEL , NRF_USBD->WVALUEH,
-      NRF_USBD->WINDEXL       , NRF_USBD->WINDEXH , NRF_USBD->WLENGTHL, NRF_USBD->WLENGTHH
+  if (int_status & USBD_INTEN_EP0SETUP_Msk) {
+    uint8_t const setup[8] = {
+        NRF_USBD->BMREQUESTTYPE, NRF_USBD->BREQUEST, NRF_USBD->WVALUEL, NRF_USBD->WVALUEH,
+        NRF_USBD->WINDEXL, NRF_USBD->WINDEXH, NRF_USBD->WLENGTHL, NRF_USBD->WLENGTHH
     };
 
     // nrf5x hw auto handle set address, there is no need to inform usb stack
-    tusb_control_request_t const * request = (tusb_control_request_t const *) setup;
+    tusb_control_request_t const* request = (tusb_control_request_t const*) setup;
 
-    if ( !(TUSB_REQ_RCPT_DEVICE   == request->bmRequestType_bit.recipient &&
-           TUSB_REQ_TYPE_STANDARD == request->bmRequestType_bit.type &&
-           TUSB_REQ_SET_ADDRESS   == request->bRequest) )
-    {
+    if (!(TUSB_REQ_RCPT_DEVICE == request->bmRequestType_bit.recipient &&
+          TUSB_REQ_TYPE_STANDARD == request->bmRequestType_bit.type &&
+          TUSB_REQ_SET_ADDRESS == request->bRequest)) {
       dcd_event_setup_received(0, setup, true);
     }
   }
 
-  if ( int_status & EDPT_END_ALL_MASK )
-  {
+  if (int_status & EDPT_END_ALL_MASK) {
     // DMA complete move data from SRAM <-> Endpoint
     // Must before endpoint transfer handling
     edpt_dma_end();
@@ -778,30 +710,24 @@ void dcd_int_handler(uint8_t rhport)
    * len if Host decides to sent fewer bytes, it this case transaction is also
    * complete and next transfer is not initiated here like for CBI.
    */
-  for(uint8_t epnum=0; epnum<EP_CBI_COUNT+1; epnum++)
-  {
-    if ( tu_bit_test(int_status, USBD_INTEN_ENDEPOUT0_Pos+epnum))
-    {
+  for (uint8_t epnum = 0; epnum < EP_CBI_COUNT + 1; epnum++) {
+    if (tu_bit_test(int_status, USBD_INTEN_ENDEPOUT0_Pos + epnum)) {
       xfer_td_t* xfer = get_td(epnum, TUSB_DIR_OUT);
       uint16_t const xact_len = NRF_USBD->EPOUT[epnum].AMOUNT;
 
-      xfer->buffer     += xact_len;
+      xfer->buffer += xact_len;
       xfer->actual_len += xact_len;
 
       // Transfer complete if transaction len < Max Packet Size or total len is transferred
-      if ( (epnum != EP_ISO_NUM) && (xact_len == xfer->mps) && (xfer->actual_len < xfer->total_len) )
-      {
-        if ( epnum == 0 )
-        {
+      if ((epnum != EP_ISO_NUM) && (xact_len == xfer->mps) && (xfer->actual_len < xfer->total_len)) {
+        if (epnum == 0) {
           // Accept next Control Out packet. TASKS_EP0RCVOUT also require EasyDMA
           edpt_dma_start(&NRF_USBD->TASKS_EP0RCVOUT);
-        }else
-        {
+        } else {
           // nRF auto accept next Bulk/Interrupt OUT packet
           // nothing to do
         }
-      }else
-      {
+      } else {
         TU_ASSERT(xfer->started,);
         xfer->total_len = xfer->actual_len;
         xfer->started = false;
@@ -815,11 +741,11 @@ void dcd_int_handler(uint8_t rhport)
   }
 
   // Endpoint <-> Host ( In & OUT )
-  if ( int_status & (USBD_INTEN_EPDATA_Msk | USBD_INTEN_EP0DATADONE_Msk) )
-  {
+  if (int_status & (USBD_INTEN_EPDATA_Msk | USBD_INTEN_EP0DATADONE_Msk)) {
     uint32_t data_status = NRF_USBD->EPDATASTATUS;
     NRF_USBD->EPDATASTATUS = data_status;
-    __ISB(); __DSB();
+    __ISB();
+    __DSB();
 
     // EP0DATADONE is set with either Control Out on IN Data
     // Since EPDATASTATUS cannot be used to determine whether it is control OUT or IN.
@@ -828,22 +754,18 @@ void dcd_int_handler(uint8_t rhport)
     bool const is_control_out = (int_status & USBD_INTEN_EP0DATADONE_Msk) && !(NRF_USBD->BMREQUESTTYPE & TUSB_DIR_IN_MASK);
 
     // CBI In: Endpoint -> Host (transaction complete)
-    for(uint8_t epnum=0; epnum<EP_CBI_COUNT; epnum++)
-    {
-      if ( tu_bit_test(data_status, epnum) || (epnum == 0 && is_control_in) )
-      {
+    for (uint8_t epnum = 0; epnum < EP_CBI_COUNT; epnum++) {
+      if (tu_bit_test(data_status, epnum) || (epnum == 0 && is_control_in)) {
         xfer_td_t* xfer = get_td(epnum, TUSB_DIR_IN);
         uint8_t const xact_len = NRF_USBD->EPIN[epnum].AMOUNT;
 
-        xfer->buffer     += xact_len;
+        xfer->buffer += xact_len;
         xfer->actual_len += xact_len;
 
-        if ( xfer->actual_len < xfer->total_len )
-        {
+        if (xfer->actual_len < xfer->total_len) {
           // Start DMA to copy next data packet
           xact_in_dma(epnum);
-        } else
-        {
+        } else {
           // CBI IN complete
           dcd_event_xfer_complete(0, epnum | TUSB_DIR_IN_MASK, xfer->actual_len, XFER_RESULT_SUCCESS, true);
         }
@@ -851,17 +773,13 @@ void dcd_int_handler(uint8_t rhport)
     }
 
     // CBI OUT: Host -> Endpoint
-    for(uint8_t epnum=0; epnum<EP_CBI_COUNT; epnum++)
-    {
-      if ( tu_bit_test(data_status, 16+epnum) || (epnum == 0 && is_control_out) )
-      {
+    for (uint8_t epnum = 0; epnum < EP_CBI_COUNT; epnum++) {
+      if (tu_bit_test(data_status, 16 + epnum) || (epnum == 0 && is_control_out)) {
         xfer_td_t* xfer = get_td(epnum, TUSB_DIR_OUT);
 
-        if ( xfer->started && xfer->actual_len < xfer->total_len )
-        {
+        if (xfer->started && xfer->actual_len < xfer->total_len) {
           xact_out_dma(epnum);
-        }else
-        {
+        } else {
           // Data overflow !!! Nah, nRF will auto accept next Bulk/Interrupt OUT packet
           // Mark this endpoint with data received
           xfer->data_received = true;
@@ -885,76 +803,80 @@ void dcd_int_handler(uint8_t rhport)
   #define SD_MAGIC_NUMBER   0x51B1E5DB
 #endif
 
-static inline bool is_sd_existed(void)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool is_sd_existed(void) {
   return *((uint32_t*)(SOFTDEVICE_INFO_STRUCT_ADDRESS+4)) == SD_MAGIC_NUMBER;
 }
 
 // check if SD is existed and enabled
-static inline bool is_sd_enabled(void)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool is_sd_enabled(void) {
   if ( !is_sd_existed() ) return false;
-
   uint8_t sd_en = false;
   (void) sd_softdevice_is_enabled(&sd_en);
   return sd_en;
 }
 #endif
 
-static bool hfclk_running(void)
-{
+static bool hfclk_running(void) {
 #ifdef SOFTDEVICE_PRESENT
-  if ( is_sd_enabled() )
-  {
+  if ( is_sd_enabled() ) {
     uint32_t is_running = 0;
     (void) sd_clock_hfclk_is_running(&is_running);
     return (is_running ? true : false);
   }
 #endif
 
+#if CFG_TUD_NRF_NRFX_VERSION == 1
+  return nrf_clock_hf_is_running(NRF_CLOCK_HFCLK_HIGH_ACCURACY);
+#else
   return nrf_clock_hf_is_running(NRF_CLOCK, NRF_CLOCK_HFCLK_HIGH_ACCURACY);
+#endif
 }
 
-static void hfclk_enable(void)
-{
+static void hfclk_enable(void) {
 #if CFG_TUSB_OS == OPT_OS_MYNEWT
   usb_clock_request();
   return;
 #else
 
   // already running, nothing to do
-  if ( hfclk_running() ) return;
+  if (hfclk_running()) return;
 
 #ifdef SOFTDEVICE_PRESENT
-  if ( is_sd_enabled() )
-  {
+  if ( is_sd_enabled() ) {
     (void)sd_clock_hfclk_request();
     return;
   }
 #endif
 
+#if CFG_TUD_NRF_NRFX_VERSION == 1
+  nrf_clock_event_clear(NRF_CLOCK_EVENT_HFCLKSTARTED);
+  nrf_clock_task_trigger(NRF_CLOCK_TASK_HFCLKSTART);
+#else
   nrf_clock_event_clear(NRF_CLOCK, NRF_CLOCK_EVENT_HFCLKSTARTED);
   nrf_clock_task_trigger(NRF_CLOCK, NRF_CLOCK_TASK_HFCLKSTART);
 #endif
+#endif
 }
 
-static void hfclk_disable(void)
-{
+static void hfclk_disable(void) {
 #if CFG_TUSB_OS == OPT_OS_MYNEWT
   usb_clock_release();
   return;
 #else
 
 #ifdef SOFTDEVICE_PRESENT
-  if ( is_sd_enabled() )
-  {
+  if ( is_sd_enabled() ) {
     (void)sd_clock_hfclk_release();
     return;
   }
 #endif
 
+#if CFG_TUD_NRF_NRFX_VERSION == 1
+  nrf_clock_task_trigger(NRF_CLOCK_TASK_HFCLKSTOP);
+#else
   nrf_clock_task_trigger(NRF_CLOCK, NRF_CLOCK_TASK_HFCLKSTOP);
 #endif
+#endif
 }
 
 // Power & Clock Peripheral on nRF5x to manage USB
@@ -967,8 +889,7 @@ static void hfclk_disable(void)
 // Therefore this function must be called to handle USB power event by
 // - nrfx_power_usbevt_init() : if Softdevice is not used or enabled
 // - SoftDevice SOC event : if SD is used and enabled
-void tusb_hal_nrf_power_event (uint32_t event)
-{
+void tusb_hal_nrf_power_event(uint32_t event) {
   // Value is chosen to be as same as NRFX_POWER_USB_EVT_* in nrfx_power.h
   enum {
     USB_EVT_DETECTED = 0,
@@ -977,50 +898,41 @@ void tusb_hal_nrf_power_event (uint32_t event)
   };
 
 #if CFG_TUSB_DEBUG >= 2
-  const char* const power_evt_str[] = { "Detected", "Removed", "Ready" };
+  const char* const power_evt_str[] = {"Detected", "Removed", "Ready"};
   TU_LOG(2, "Power USB event: %s\r\n", power_evt_str[event]);
 #endif
 
-  switch ( event )
-  {
+  switch (event) {
     case USB_EVT_DETECTED:
-      if ( !NRF_USBD->ENABLE )
-      {
+      if (!NRF_USBD->ENABLE) {
         // Prepare for receiving READY event: disable interrupt since we will blocking wait
         NRF_USBD->INTENCLR = USBD_INTEN_USBEVENT_Msk;
         NRF_USBD->EVENTCAUSE = USBD_EVENTCAUSE_READY_Msk;
-        __ISB(); __DSB(); // for sync
+        __ISB();
+        __DSB(); // for sync
 
 #ifdef NRF52_SERIES // NRF53 does not need this errata
         // ERRATA 171, 187, 166
-        if ( nrfx_usbd_errata_187() )
-        {
+        if (nrfx_usbd_errata_187()) {
           // CRITICAL_REGION_ENTER();
-          if ( *((volatile uint32_t *) (0x4006EC00)) == 0x00000000 )
-          {
-            *((volatile uint32_t *) (0x4006EC00)) = 0x00009375;
-            *((volatile uint32_t *) (0x4006ED14)) = 0x00000003;
-            *((volatile uint32_t *) (0x4006EC00)) = 0x00009375;
-          }
-          else
-          {
-            *((volatile uint32_t *) (0x4006ED14)) = 0x00000003;
+          if (*((volatile uint32_t*) (0x4006EC00)) == 0x00000000) {
+            *((volatile uint32_t*) (0x4006EC00)) = 0x00009375;
+            *((volatile uint32_t*) (0x4006ED14)) = 0x00000003;
+            *((volatile uint32_t*) (0x4006EC00)) = 0x00009375;
+          } else {
+            *((volatile uint32_t*) (0x4006ED14)) = 0x00000003;
           }
           // CRITICAL_REGION_EXIT();
         }
 
-        if ( nrfx_usbd_errata_171() )
-        {
+        if (nrfx_usbd_errata_171()) {
           // CRITICAL_REGION_ENTER();
-          if ( *((volatile uint32_t *) (0x4006EC00)) == 0x00000000 )
-          {
-            *((volatile uint32_t *) (0x4006EC00)) = 0x00009375;
-            *((volatile uint32_t *) (0x4006EC14)) = 0x000000C0;
-            *((volatile uint32_t *) (0x4006EC00)) = 0x00009375;
-          }
-          else
-          {
-            *((volatile uint32_t *) (0x4006EC14)) = 0x000000C0;
+          if (*((volatile uint32_t*) (0x4006EC00)) == 0x00000000) {
+            *((volatile uint32_t*) (0x4006EC00)) = 0x00009375;
+            *((volatile uint32_t*) (0x4006EC14)) = 0x000000C0;
+            *((volatile uint32_t*) (0x4006EC00)) = 0x00009375;
+          } else {
+            *((volatile uint32_t*) (0x4006EC14)) = 0x000000C0;
           }
           // CRITICAL_REGION_EXIT();
         }
@@ -1028,64 +940,58 @@ void tusb_hal_nrf_power_event (uint32_t event)
 
         // Enable the peripheral (will cause Ready event)
         NRF_USBD->ENABLE = 1;
-        __ISB(); __DSB(); // for sync
+        __ISB();
+        __DSB(); // for sync
 
         // Enable HFCLK
         hfclk_enable();
       }
-    break;
+      break;
 
     case USB_EVT_READY:
       // Skip if pull-up is enabled and HCLK is already running.
       // Application probably call this more than necessary.
-      if ( NRF_USBD->USBPULLUP && hfclk_running() ) break;
+      if (NRF_USBD->USBPULLUP && hfclk_running()) break;
 
       // Waiting for USBD peripheral enabled
-      while ( !(USBD_EVENTCAUSE_READY_Msk & NRF_USBD->EVENTCAUSE) ) { }
+      while (!(USBD_EVENTCAUSE_READY_Msk & NRF_USBD->EVENTCAUSE)) {}
 
       NRF_USBD->EVENTCAUSE = USBD_EVENTCAUSE_READY_Msk;
-      __ISB(); __DSB(); // for sync
+      __ISB();
+      __DSB(); // for sync
 
 #ifdef NRF52_SERIES
-      if ( nrfx_usbd_errata_171() )
-      {
+      if (nrfx_usbd_errata_171()) {
         // CRITICAL_REGION_ENTER();
-        if ( *((volatile uint32_t *) (0x4006EC00)) == 0x00000000 )
-        {
-          *((volatile uint32_t *) (0x4006EC00)) = 0x00009375;
-          *((volatile uint32_t *) (0x4006EC14)) = 0x00000000;
-          *((volatile uint32_t *) (0x4006EC00)) = 0x00009375;
-        }
-        else
-        {
-          *((volatile uint32_t *) (0x4006EC14)) = 0x00000000;
+        if (*((volatile uint32_t*) (0x4006EC00)) == 0x00000000) {
+          *((volatile uint32_t*) (0x4006EC00)) = 0x00009375;
+          *((volatile uint32_t*) (0x4006EC14)) = 0x00000000;
+          *((volatile uint32_t*) (0x4006EC00)) = 0x00009375;
+        } else {
+          *((volatile uint32_t*) (0x4006EC14)) = 0x00000000;
         }
 
         // CRITICAL_REGION_EXIT();
       }
 
-      if ( nrfx_usbd_errata_187() )
-      {
+      if (nrfx_usbd_errata_187()) {
         // CRITICAL_REGION_ENTER();
-        if ( *((volatile uint32_t *) (0x4006EC00)) == 0x00000000 )
-        {
-          *((volatile uint32_t *) (0x4006EC00)) = 0x00009375;
-          *((volatile uint32_t *) (0x4006ED14)) = 0x00000000;
-          *((volatile uint32_t *) (0x4006EC00)) = 0x00009375;
-        }
-        else
-        {
-          *((volatile uint32_t *) (0x4006ED14)) = 0x00000000;
+        if (*((volatile uint32_t*) (0x4006EC00)) == 0x00000000) {
+          *((volatile uint32_t*) (0x4006EC00)) = 0x00009375;
+          *((volatile uint32_t*) (0x4006ED14)) = 0x00000000;
+          *((volatile uint32_t*) (0x4006EC00)) = 0x00009375;
+        } else {
+          *((volatile uint32_t*) (0x4006ED14)) = 0x00000000;
         }
         // CRITICAL_REGION_EXIT();
       }
 
-      if ( nrfx_usbd_errata_166() )
-      {
-        *((volatile uint32_t *) (NRF_USBD_BASE + 0x800)) = 0x7E3;
-        *((volatile uint32_t *) (NRF_USBD_BASE + 0x804)) = 0x40;
+      if (nrfx_usbd_errata_166()) {
+        *((volatile uint32_t*) (NRF_USBD_BASE + 0x800)) = 0x7E3;
+        *((volatile uint32_t*) (NRF_USBD_BASE + 0x804)) = 0x40;
 
-        __ISB(); __DSB();
+        __ISB();
+        __DSB();
       }
 #endif
 
@@ -1097,30 +1003,31 @@ void tusb_hal_nrf_power_event (uint32_t event)
 
       // Enable interrupt, priorities should be set by application
       NVIC_ClearPendingIRQ(USBD_IRQn);
+
       // Don't enable USBD interrupt yet, if dcd_init() did not finish yet
       // Interrupt will be enabled by tud_init(), when USB stack is ready
       // to handle interrupts.
-      if (tud_inited())
-      {
+      if (tud_inited()) {
         NVIC_EnableIRQ(USBD_IRQn);
       }
 
       // Wait for HFCLK
-      while ( !hfclk_running() ) { }
+      while (!hfclk_running()) {}
 
       // Enable pull up
       NRF_USBD->USBPULLUP = 1;
-      __ISB(); __DSB(); // for sync
-    break;
+      __ISB();
+      __DSB(); // for sync
+      break;
 
     case USB_EVT_REMOVED:
-      if ( NRF_USBD->ENABLE )
-      {
+      if (NRF_USBD->ENABLE) {
         // Abort all transfers
 
         // Disable pull up
         NRF_USBD->USBPULLUP = 0;
-        __ISB(); __DSB(); // for sync
+        __ISB();
+        __DSB(); // for sync
 
         // Disable Interrupt
         NVIC_DisableIRQ(USBD_IRQn);
@@ -1129,15 +1036,17 @@ void tusb_hal_nrf_power_event (uint32_t event)
         NRF_USBD->INTENCLR = NRF_USBD->INTEN;
 
         NRF_USBD->ENABLE = 0;
-        __ISB(); __DSB(); // for sync
+        __ISB();
+        __DSB(); // for sync
 
         hfclk_disable();
 
         dcd_event_bus_signal(0, DCD_EVENT_UNPLUGGED, is_in_isr());
       }
-    break;
+      break;
 
-    default: break;
+    default:
+      break;
   }
 }
 
diff --git a/src/portable/nxp/khci/dcd_khci.c b/src/portable/nxp/khci/dcd_khci.c
index 5c65ea33d..dc71117b3 100644
--- a/src/portable/nxp/khci/dcd_khci.c
+++ b/src/portable/nxp/khci/dcd_khci.c
@@ -269,9 +269,21 @@ void dcd_init(uint8_t rhport)
 {
   (void) rhport;
 
+  // save crystal-less setting (if available)
+  #if defined(FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED) && FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED == 1
+  uint32_t clk_recover_irc_en = KHCI->CLK_RECOVER_IRC_EN;
+  uint32_t clk_recover_ctrl = KHCI->CLK_RECOVER_CTRL;
+  #endif
+
   KHCI->USBTRC0 |= USB_USBTRC0_USBRESET_MASK;
   while (KHCI->USBTRC0 & USB_USBTRC0_USBRESET_MASK);
 
+  // restore crystal-less setting (if available)
+  #if defined(FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED) && FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED == 1
+  KHCI->CLK_RECOVER_IRC_EN = clk_recover_irc_en;
+  KHCI->CLK_RECOVER_CTRL  |= clk_recover_ctrl;
+  #endif
+
   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);
diff --git a/src/portable/nxp/lpc_ip3511/dcd_lpc_ip3511.c b/src/portable/nxp/lpc_ip3511/dcd_lpc_ip3511.c
index f4ed09d83..cc18cf59b 100644
--- a/src/portable/nxp/lpc_ip3511/dcd_lpc_ip3511.c
+++ b/src/portable/nxp/lpc_ip3511/dcd_lpc_ip3511.c
@@ -42,7 +42,18 @@
 
 #if TU_CHECK_MCU(OPT_MCU_LPC11UXX, OPT_MCU_LPC13XX, OPT_MCU_LPC15XX)
   // LPCOpen
+  #ifdef __GNUC__
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wunused-parameter"
+  #pragma GCC diagnostic ignored "-Wstrict-prototypes"
+  #endif
+
   #include "chip.h"
+
+  #ifdef __GNUC__
+  #pragma GCC diagnostic pop
+  #endif
+
 #else
   // SDK
   #include "fsl_device_registers.h"
@@ -239,7 +250,7 @@ TU_ATTR_ALWAYS_INLINE static inline bool ep_is_iso(ep_cmd_sts_t* ep_cs, bool is_
   return is_highspeed ? (ep_cs[0].cmd_sts.type && !ep_cs[0].cmd_sts.rf_tv) : ep_cs->cmd_sts.type;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline bool ep_is_bulk(ep_cmd_sts_t* ep_cs) {
+TU_ATTR_ALWAYS_INLINE TU_ATTR_UNUSED static inline bool ep_is_bulk(ep_cmd_sts_t* ep_cs) {
   return (ep_cs[0].cmd_sts.type == 0) && (ep_cs[0].cmd_sts.rf_tv == 0);
 }
 
diff --git a/src/portable/ohci/ohci.h b/src/portable/ohci/ohci.h
index 4feefd771..94bad5df7 100644
--- a/src/portable/ohci/ohci.h
+++ b/src/portable/ohci/ohci.h
@@ -83,7 +83,7 @@ typedef struct TU_ATTR_ALIGNED(16)
   volatile uint32_t condition_code : 4;
 
 	// Word 1
-	volatile uint8_t* current_buffer_pointer;
+	uint8_t* volatile current_buffer_pointer;
 
 	// Word 2 : next TD
 	volatile uint32_t next;
diff --git a/src/portable/raspberrypi/rp2040/dcd_rp2040.c b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
index e8cee73fd..bc0deee32 100644
--- a/src/portable/raspberrypi/rp2040/dcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
@@ -48,7 +48,7 @@
  *------------------------------------------------------------------*/
 
 // Init these in dcd_init
-static uint8_t *next_buffer_ptr;
+static uint8_t* next_buffer_ptr;
 
 // USB_MAX_ENDPOINTS Endpoints, direction TUSB_DIR_OUT for out and TUSB_DIR_IN for in.
 static struct hw_endpoint hw_endpoints[USB_MAX_ENDPOINTS][2];
@@ -56,79 +56,70 @@ static struct hw_endpoint hw_endpoints[USB_MAX_ENDPOINTS][2];
 // SOF may be used by remote wakeup as RESUME, this indicate whether SOF is actually used by usbd
 static bool _sof_enable = false;
 
-TU_ATTR_ALWAYS_INLINE static inline struct hw_endpoint *hw_endpoint_get_by_num(uint8_t num, tusb_dir_t dir)
-{
+TU_ATTR_ALWAYS_INLINE static inline struct hw_endpoint* hw_endpoint_get_by_num(uint8_t num, tusb_dir_t dir) {
   return &hw_endpoints[num][dir];
 }
 
-static struct hw_endpoint *hw_endpoint_get_by_addr(uint8_t ep_addr)
-{
+TU_ATTR_ALWAYS_INLINE static inline struct hw_endpoint* hw_endpoint_get_by_addr(uint8_t ep_addr) {
   uint8_t num = tu_edpt_number(ep_addr);
   tusb_dir_t dir = tu_edpt_dir(ep_addr);
   return hw_endpoint_get_by_num(num, dir);
 }
 
-static void _hw_endpoint_alloc(struct hw_endpoint *ep, uint8_t transfer_type)
-{
+static void _hw_endpoint_alloc(struct hw_endpoint* ep, uint8_t transfer_type) {
   // size must be multiple of 64
   uint size = tu_div_ceil(ep->wMaxPacketSize, 64) * 64u;
 
   // double buffered Bulk endpoint
-  if ( transfer_type == TUSB_XFER_BULK )
-  {
+  if (transfer_type == TUSB_XFER_BULK) {
     size *= 2u;
   }
 
   ep->hw_data_buf = next_buffer_ptr;
   next_buffer_ptr += size;
 
-  assert(((uintptr_t )next_buffer_ptr & 0b111111u) == 0);
+  assert(((uintptr_t) next_buffer_ptr & 0b111111u) == 0);
   uint dpram_offset = hw_data_offset(ep->hw_data_buf);
   hard_assert(hw_data_offset(next_buffer_ptr) <= USB_DPRAM_MAX);
 
   pico_info("  Allocated %d bytes at offset 0x%x (0x%p)\r\n", size, dpram_offset, ep->hw_data_buf);
 
   // Fill in endpoint control register with buffer offset
-  uint32_t const reg = EP_CTRL_ENABLE_BITS | ((uint)transfer_type << EP_CTRL_BUFFER_TYPE_LSB) | dpram_offset;
+  uint32_t const reg = EP_CTRL_ENABLE_BITS | ((uint) transfer_type << EP_CTRL_BUFFER_TYPE_LSB) | dpram_offset;
 
   *ep->endpoint_control = reg;
 }
 
-static void _hw_endpoint_close(struct hw_endpoint *ep)
-{
-    // Clear hardware registers and then zero the struct
-    // Clears endpoint enable
-    *ep->endpoint_control = 0;
-    // Clears buffer available, etc
-    *ep->buffer_control = 0;
-    // Clear any endpoint state
-    memset(ep, 0, sizeof(struct hw_endpoint));
+static void _hw_endpoint_close(struct hw_endpoint* ep) {
+  // Clear hardware registers and then zero the struct
+  // Clears endpoint enable
+  *ep->endpoint_control = 0;
+  // Clears buffer available, etc
+  *ep->buffer_control = 0;
+  // Clear any endpoint state
+  memset(ep, 0, sizeof(struct hw_endpoint));
 
-    // Reclaim buffer space if all endpoints are closed
-    bool reclaim_buffers = true;
-    for ( uint8_t i = 1; i < USB_MAX_ENDPOINTS; i++ )
-    {
-        if (hw_endpoint_get_by_num(i, TUSB_DIR_OUT)->hw_data_buf != NULL || hw_endpoint_get_by_num(i, TUSB_DIR_IN)->hw_data_buf != NULL)
-        {
-            reclaim_buffers = false;
-            break;
-        }
-    }
-    if (reclaim_buffers)
-    {
-        next_buffer_ptr = &usb_dpram->epx_data[0];
+  // Reclaim buffer space if all endpoints are closed
+  bool reclaim_buffers = true;
+  for (uint8_t i = 1; i < USB_MAX_ENDPOINTS; i++) {
+    if (hw_endpoint_get_by_num(i, TUSB_DIR_OUT)->hw_data_buf != NULL ||
+        hw_endpoint_get_by_num(i, TUSB_DIR_IN)->hw_data_buf != NULL) {
+      reclaim_buffers = false;
+      break;
     }
+  }
+  if (reclaim_buffers) {
+    next_buffer_ptr = &usb_dpram->epx_data[0];
+  }
 }
 
-static void hw_endpoint_close(uint8_t ep_addr)
-{
-    struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
-    _hw_endpoint_close(ep);
+static void hw_endpoint_close(uint8_t ep_addr) {
+  struct hw_endpoint* ep = hw_endpoint_get_by_addr(ep_addr);
+  _hw_endpoint_close(ep);
 }
 
-static void hw_endpoint_init(uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t transfer_type)
-{
-  struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
+static void hw_endpoint_init(uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t transfer_type) {
+  struct hw_endpoint* ep = hw_endpoint_get_by_addr(ep_addr);
 
   const uint8_t num = tu_edpt_number(ep_addr);
   const tusb_dir_t dir = tu_edpt_dir(ep_addr);
@@ -143,35 +134,26 @@ static void hw_endpoint_init(uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t t
   ep->transfer_type = transfer_type;
 
   // Every endpoint has a buffer control register in dpram
-  if ( dir == TUSB_DIR_IN )
-  {
+  if (dir == TUSB_DIR_IN) {
     ep->buffer_control = &usb_dpram->ep_buf_ctrl[num].in;
-  }
-  else
-  {
+  } else {
     ep->buffer_control = &usb_dpram->ep_buf_ctrl[num].out;
   }
 
   // Clear existing buffer control state
   *ep->buffer_control = 0;
 
-  if ( num == 0 )
-  {
+  if (num == 0) {
     // EP0 has no endpoint control register because the buffer offsets are fixed
     ep->endpoint_control = NULL;
 
     // Buffer offset is fixed (also double buffered)
     ep->hw_data_buf = (uint8_t*) &usb_dpram->ep0_buf_a[0];
-  }
-  else
-  {
+  } else {
     // Set the endpoint control register (starts at EP1, hence num-1)
-    if ( dir == TUSB_DIR_IN )
-    {
+    if (dir == TUSB_DIR_IN) {
       ep->endpoint_control = &usb_dpram->ep_ctrl[num - 1].in;
-    }
-    else
-    {
+    } else {
       ep->endpoint_control = &usb_dpram->ep_ctrl[num - 1].out;
     }
 
@@ -180,76 +162,82 @@ static void hw_endpoint_init(uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t t
   }
 }
 
-static void hw_endpoint_xfer(uint8_t ep_addr, uint8_t *buffer, uint16_t total_bytes)
-{
-    struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
-    hw_endpoint_xfer_start(ep, buffer, total_bytes);
+static void hw_endpoint_xfer(uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes) {
+  struct hw_endpoint* ep = hw_endpoint_get_by_addr(ep_addr);
+  hw_endpoint_xfer_start(ep, buffer, total_bytes);
 }
 
-static void __tusb_irq_path_func(hw_handle_buff_status)(void)
-{
-    uint32_t remaining_buffers = usb_hw->buf_status;
-    pico_trace("buf_status = 0x%08lx\r\n", remaining_buffers);
-    uint bit = 1u;
-    for (uint8_t i = 0; remaining_buffers && i < USB_MAX_ENDPOINTS * 2; i++)
-    {
-        if (remaining_buffers & bit)
-        {
-            // clear this in advance
-            usb_hw_clear->buf_status = bit;
+static void __tusb_irq_path_func(hw_handle_buff_status)(void) {
+  uint32_t remaining_buffers = usb_hw->buf_status;
+  pico_trace("buf_status = 0x%08lx\r\n", remaining_buffers);
+  uint bit = 1u;
+  for (uint8_t i = 0; remaining_buffers && i < USB_MAX_ENDPOINTS * 2; i++) {
+    if (remaining_buffers & bit) {
+      // clear this in advance
+      usb_hw_clear->buf_status = bit;
 
-            // IN transfer for even i, OUT transfer for odd i
-            struct hw_endpoint *ep = hw_endpoint_get_by_num(i >> 1u, (i & 1u) ? TUSB_DIR_OUT : TUSB_DIR_IN);
+      // IN transfer for even i, OUT transfer for odd i
+      struct hw_endpoint* ep = hw_endpoint_get_by_num(i >> 1u, (i & 1u) ? TUSB_DIR_OUT : TUSB_DIR_IN);
 
-            // Continue xfer
-            bool done = hw_endpoint_xfer_continue(ep);
-            if (done)
-            {
-                // Notify
-                dcd_event_xfer_complete(0, ep->ep_addr, ep->xferred_len, XFER_RESULT_SUCCESS, true);
-                hw_endpoint_reset_transfer(ep);
-            }
-            remaining_buffers &= ~bit;
-        }
-        bit <<= 1u;
+      // Continue xfer
+      bool done = hw_endpoint_xfer_continue(ep);
+      if (done) {
+        // Notify
+        dcd_event_xfer_complete(0, ep->ep_addr, ep->xferred_len, XFER_RESULT_SUCCESS, true);
+        hw_endpoint_reset_transfer(ep);
+      }
+      remaining_buffers &= ~bit;
     }
+    bit <<= 1u;
+  }
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void reset_ep0_pid(void)
-{
-    // If we have finished this transfer on EP0 set pid back to 1 for next
-    // setup transfer. Also clear a stall in case
-    uint8_t addrs[] = {0x0, 0x80};
-    for (uint i = 0 ; i < TU_ARRAY_SIZE(addrs); i++)
-    {
-        struct hw_endpoint *ep = hw_endpoint_get_by_addr(addrs[i]);
-        ep->next_pid = 1u;
+TU_ATTR_ALWAYS_INLINE static inline void reset_ep0(void) {
+  // If we have finished this transfer on EP0 set pid back to 1 for next
+  // setup transfer. Also clear a stall in case
+  for (uint8_t dir = 0; dir < 2; dir++) {
+    struct hw_endpoint* ep = hw_endpoint_get_by_num(0, dir);
+    if (ep->active) {
+      // Abort any pending transfer from a prior control transfer per USB specs
+      // Due to Errata RP2040-E2: ABORT flag is only applicable for B2 and later (unusable for B0, B1).
+      // Which means we are not guaranteed to safely abort pending transfer on B0 and B1.
+      uint32_t const abort_mask = (dir ? USB_EP_ABORT_EP0_IN_BITS : USB_EP_ABORT_EP0_OUT_BITS);
+      if (rp2040_chip_version() >= 2) {
+        usb_hw_set->abort = abort_mask;
+        while ((usb_hw->abort_done & abort_mask) != abort_mask) {}
+      }
+
+      _hw_endpoint_buffer_control_set_value32(ep, USB_BUF_CTRL_DATA1_PID | USB_BUF_CTRL_SEL);
+      hw_endpoint_reset_transfer(ep);
+
+      if (rp2040_chip_version() >= 2) {
+        usb_hw_clear->abort_done = abort_mask;
+        usb_hw_clear->abort = abort_mask;
+      }
     }
+    ep->next_pid = 1u;
+  }
 }
 
-static void __tusb_irq_path_func(reset_non_control_endpoints)(void)
-{
+static void __tusb_irq_path_func(reset_non_control_endpoints)(void) {
   // Disable all non-control
-  for ( uint8_t i = 0; i < USB_MAX_ENDPOINTS-1; i++ )
-  {
+  for (uint8_t i = 0; i < USB_MAX_ENDPOINTS - 1; i++) {
     usb_dpram->ep_ctrl[i].in = 0;
     usb_dpram->ep_ctrl[i].out = 0;
   }
 
   // clear non-control hw endpoints
-  tu_memclr(hw_endpoints[1], sizeof(hw_endpoints) - 2*sizeof(hw_endpoint_t));
+  tu_memclr(hw_endpoints[1], sizeof(hw_endpoints) - 2 * sizeof(hw_endpoint_t));
 
   // reclaim buffer space
   next_buffer_ptr = &usb_dpram->epx_data[0];
 }
 
-static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
-{
+static void __tusb_irq_path_func(dcd_rp2040_irq)(void) {
   uint32_t const status = usb_hw->ints;
   uint32_t handled = 0;
 
-  if ( status & USB_INTF_DEV_SOF_BITS )
-  {
+  if (status & USB_INTF_DEV_SOF_BITS) {
     bool keep_sof_alive = false;
 
     handled |= USB_INTF_DEV_SOF_BITS;
@@ -258,20 +246,17 @@ static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
     // Errata 15 workaround for Device Bulk-In endpoint
     e15_last_sof = time_us_32();
 
-    for ( uint8_t i = 0; i < USB_MAX_ENDPOINTS; i++ )
-    {
-      struct hw_endpoint * ep = hw_endpoint_get_by_num(i, TUSB_DIR_IN);
+    for (uint8_t i = 0; i < USB_MAX_ENDPOINTS; i++) {
+      struct hw_endpoint* ep = hw_endpoint_get_by_num(i, TUSB_DIR_IN);
 
       // Active Bulk IN endpoint requires SOF
-      if ( (ep->transfer_type == TUSB_XFER_BULK) && ep->active )
-      {
+      if ((ep->transfer_type == TUSB_XFER_BULK) && ep->active) {
         keep_sof_alive = true;
 
         hw_endpoint_lock_update(ep, 1);
 
         // Deferred enable?
-        if ( ep->pending )
-        {
+        if (ep->pending) {
           ep->pending = 0;
           hw_endpoint_start_next_buffer(ep);
         }
@@ -282,26 +267,24 @@ static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
 #endif
 
     // disable SOF interrupt if it is used for RESUME in remote wakeup
-    if ( !keep_sof_alive && !_sof_enable ) usb_hw_clear->inte = USB_INTS_DEV_SOF_BITS;
+    if (!keep_sof_alive && !_sof_enable) usb_hw_clear->inte = USB_INTS_DEV_SOF_BITS;
 
     dcd_event_sof(0, usb_hw->sof_rd & USB_SOF_RD_BITS, true);
   }
 
   // xfer events are handled before setup req. So if a transfer completes immediately
   // before closing the EP, the events will be delivered in same order.
-  if ( status & USB_INTS_BUFF_STATUS_BITS )
-  {
+  if (status & USB_INTS_BUFF_STATUS_BITS) {
     handled |= USB_INTS_BUFF_STATUS_BITS;
     hw_handle_buff_status();
   }
 
-  if ( status & USB_INTS_SETUP_REQ_BITS )
-  {
+  if (status & USB_INTS_SETUP_REQ_BITS) {
     handled |= USB_INTS_SETUP_REQ_BITS;
-    uint8_t const * setup = remove_volatile_cast(uint8_t const*, &usb_dpram->setup_packet);
+    uint8_t const* setup = remove_volatile_cast(uint8_t const*, &usb_dpram->setup_packet);
 
     // reset pid to both 1 (data and ack)
-    reset_ep0_pid();
+    reset_ep0();
 
     // Pass setup packet to tiny usb
     dcd_event_setup_received(0, setup, true);
@@ -329,8 +312,7 @@ static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
 #endif
 
   // SE0 for 2.5 us or more (will last at least 10ms)
-  if ( status & USB_INTS_BUS_RESET_BITS )
-  {
+  if (status & USB_INTS_BUS_RESET_BITS) {
     pico_trace("BUS RESET\r\n");
 
     handled |= USB_INTS_BUS_RESET_BITS;
@@ -342,7 +324,7 @@ static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
 
 #if TUD_OPT_RP2040_USB_DEVICE_ENUMERATION_FIX
     // Only run enumeration workaround if pull up is enabled
-    if ( usb_hw->sie_ctrl & USB_SIE_CTRL_PULLUP_EN_BITS ) rp2040_usb_device_enumeration_fix();
+    if (usb_hw->sie_ctrl & USB_SIE_CTRL_PULLUP_EN_BITS) rp2040_usb_device_enumeration_fix();
 #endif
   }
 
@@ -354,22 +336,19 @@ static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
    * because without VBUS detection, it is impossible to tell the difference between
    * being disconnected and suspended.
    */
-  if ( status & USB_INTS_DEV_SUSPEND_BITS )
-  {
+  if (status & USB_INTS_DEV_SUSPEND_BITS) {
     handled |= USB_INTS_DEV_SUSPEND_BITS;
     dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
     usb_hw_clear->sie_status = USB_SIE_STATUS_SUSPENDED_BITS;
   }
 
-  if ( status & USB_INTS_DEV_RESUME_FROM_HOST_BITS )
-  {
+  if (status & USB_INTS_DEV_RESUME_FROM_HOST_BITS) {
     handled |= USB_INTS_DEV_RESUME_FROM_HOST_BITS;
     dcd_event_bus_signal(0, DCD_EVENT_RESUME, true);
     usb_hw_clear->sie_status = USB_SIE_STATUS_RESUME_BITS;
   }
 
-  if ( status ^ handled )
-  {
+  if (status ^ handled) {
     panic("Unhandled IRQ 0x%x\n", (uint) (status ^ handled));
   }
 }
@@ -390,10 +369,11 @@ static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
 #define PICO_SHARED_IRQ_HANDLER_HIGHEST_ORDER_PRIORITY 0xff
 #endif
 
-void dcd_init (uint8_t rhport)
-{
+void dcd_init(uint8_t rhport) {
   assert(rhport == 0);
 
+  TU_LOG(2, "Chip Version B%u\r\n", rp2040_chip_version());
+
   // Reset hardware to default state
   rp2040_usb_init();
 
@@ -405,7 +385,7 @@ void dcd_init (uint8_t rhport)
   irq_add_shared_handler(USBCTRL_IRQ, dcd_rp2040_irq, PICO_SHARED_IRQ_HANDLER_HIGHEST_ORDER_PRIORITY);
 
   // Init control endpoints
-  tu_memclr(hw_endpoints[0], 2*sizeof(hw_endpoint_t));
+  tu_memclr(hw_endpoints[0], 2 * sizeof(hw_endpoint_t));
   hw_endpoint_init(0x0, 64, TUSB_XFER_CONTROL);
   hw_endpoint_init(0x80, 64, TUSB_XFER_CONTROL);
 
@@ -420,27 +400,37 @@ void dcd_init (uint8_t rhport)
   // for the global interrupt enable...
   // Note: Force VBUS detect cause disconnection not detectable
   usb_hw->sie_ctrl = USB_SIE_CTRL_EP0_INT_1BUF_BITS;
-  usb_hw->inte     = USB_INTS_BUFF_STATUS_BITS | USB_INTS_BUS_RESET_BITS | USB_INTS_SETUP_REQ_BITS |
-                     USB_INTS_DEV_SUSPEND_BITS | USB_INTS_DEV_RESUME_FROM_HOST_BITS |
-                     (FORCE_VBUS_DETECT ? 0 : USB_INTS_DEV_CONN_DIS_BITS);
+  usb_hw->inte = USB_INTS_BUFF_STATUS_BITS | USB_INTS_BUS_RESET_BITS | USB_INTS_SETUP_REQ_BITS |
+                 USB_INTS_DEV_SUSPEND_BITS | USB_INTS_DEV_RESUME_FROM_HOST_BITS |
+                 (FORCE_VBUS_DETECT ? 0 : USB_INTS_DEV_CONN_DIS_BITS);
 
   dcd_connect(rhport);
 }
 
-void dcd_int_enable(__unused uint8_t rhport)
-{
-    assert(rhport == 0);
-    irq_set_enabled(USBCTRL_IRQ, true);
+bool dcd_deinit(uint8_t rhport) {
+  (void) rhport;
+
+  reset_non_control_endpoints();
+  irq_remove_handler(USBCTRL_IRQ, dcd_rp2040_irq);
+
+  // reset usb hardware into initial state
+  reset_block(RESETS_RESET_USBCTRL_BITS);
+  unreset_block_wait(RESETS_RESET_USBCTRL_BITS);
+
+  return true;
+}
+
+void dcd_int_enable(__unused uint8_t rhport) {
+  assert(rhport == 0);
+  irq_set_enabled(USBCTRL_IRQ, true);
 }
 
-void dcd_int_disable(__unused uint8_t rhport)
-{
-    assert(rhport == 0);
-    irq_set_enabled(USBCTRL_IRQ, false);
+void dcd_int_disable(__unused uint8_t rhport) {
+  assert(rhport == 0);
+  irq_set_enabled(USBCTRL_IRQ, false);
 }
 
-void dcd_set_address (__unused uint8_t rhport, __unused uint8_t dev_addr)
-{
+void dcd_set_address(__unused uint8_t rhport, __unused uint8_t dev_addr) {
   assert(rhport == 0);
 
   // Can't set device address in hardware until status xfer has complete
@@ -448,8 +438,7 @@ void dcd_set_address (__unused uint8_t rhport, __unused uint8_t dev_addr)
   hw_endpoint_xfer(0x80, NULL, 0);
 }
 
-void dcd_remote_wakeup(__unused uint8_t rhport)
-{
+void dcd_remote_wakeup(__unused uint8_t rhport) {
   pico_info("dcd_remote_wakeup %d\n", rhport);
   assert(rhport == 0);
 
@@ -460,100 +449,88 @@ void dcd_remote_wakeup(__unused uint8_t rhport)
 }
 
 // disconnect by disabling internal pull-up resistor on D+/D-
-void dcd_disconnect(__unused uint8_t rhport)
-{
+void dcd_disconnect(__unused uint8_t rhport) {
   (void) rhport;
   usb_hw_clear->sie_ctrl = USB_SIE_CTRL_PULLUP_EN_BITS;
 }
 
 // connect by enabling internal pull-up resistor on D+/D-
-void dcd_connect(__unused uint8_t rhport)
-{
+void dcd_connect(__unused uint8_t rhport) {
   (void) rhport;
   usb_hw_set->sie_ctrl = USB_SIE_CTRL_PULLUP_EN_BITS;
 }
 
-void dcd_sof_enable(uint8_t rhport, bool en)
-{
+void dcd_sof_enable(uint8_t rhport, bool en) {
   (void) rhport;
 
   _sof_enable = en;
 
-  if (en)
-  {
+  if (en) {
     usb_hw_set->inte = USB_INTS_DEV_SOF_BITS;
-  }else
-  {
+  }
+#if !TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
+  else {
     // Don't clear immediately if the SOF workaround is in use.
     // The SOF handler will conditionally disable the interrupt.
-#if !TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
     usb_hw_clear->inte = USB_INTS_DEV_SOF_BITS;
-#endif
   }
+#endif
 }
 
 /*------------------------------------------------------------------*/
 /* DCD Endpoint port
  *------------------------------------------------------------------*/
 
-void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const * request)
-{
+void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const* request) {
   (void) rhport;
 
-  if ( request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_DEVICE &&
-       request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD &&
-       request->bRequest == TUSB_REQ_SET_ADDRESS )
-  {
+  if (request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_DEVICE &&
+      request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD &&
+      request->bRequest == TUSB_REQ_SET_ADDRESS) {
     usb_hw->dev_addr_ctrl = (uint8_t) request->wValue;
   }
 }
 
-bool dcd_edpt_open (__unused uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
-{
-    assert(rhport == 0);
-    hw_endpoint_init(desc_edpt->bEndpointAddress, tu_edpt_packet_size(desc_edpt), desc_edpt->bmAttributes.xfer);
-    return true;
+bool dcd_edpt_open(__unused uint8_t rhport, tusb_desc_endpoint_t const* desc_edpt) {
+  assert(rhport == 0);
+  hw_endpoint_init(desc_edpt->bEndpointAddress, tu_edpt_packet_size(desc_edpt), desc_edpt->bmAttributes.xfer);
+  return true;
 }
 
-void dcd_edpt_close_all (uint8_t rhport)
-{
+void dcd_edpt_close_all(uint8_t rhport) {
   (void) rhport;
 
   // may need to use EP Abort
   reset_non_control_endpoints();
 }
 
-bool dcd_edpt_xfer(__unused uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
-{
-    assert(rhport == 0);
-    hw_endpoint_xfer(ep_addr, buffer, total_bytes);
-    return true;
+bool dcd_edpt_xfer(__unused uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes) {
+  assert(rhport == 0);
+  hw_endpoint_xfer(ep_addr, buffer, total_bytes);
+  return true;
 }
 
-void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
-{
+void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr) {
   (void) rhport;
 
-  if ( tu_edpt_number(ep_addr) == 0 )
-  {
+  if (tu_edpt_number(ep_addr) == 0) {
     // A stall on EP0 has to be armed so it can be cleared on the next setup packet
-    usb_hw_set->ep_stall_arm = (tu_edpt_dir(ep_addr) == TUSB_DIR_IN) ? USB_EP_STALL_ARM_EP0_IN_BITS : USB_EP_STALL_ARM_EP0_OUT_BITS;
+    usb_hw_set->ep_stall_arm = (tu_edpt_dir(ep_addr) == TUSB_DIR_IN) ? USB_EP_STALL_ARM_EP0_IN_BITS
+                                                                     : USB_EP_STALL_ARM_EP0_OUT_BITS;
   }
 
-  struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
+  struct hw_endpoint* ep = hw_endpoint_get_by_addr(ep_addr);
 
   // stall and clear current pending buffer
   // may need to use EP_ABORT
   _hw_endpoint_buffer_control_set_value32(ep, USB_BUF_CTRL_STALL);
 }
 
-void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
-{
+void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr) {
   (void) rhport;
 
-  if (tu_edpt_number(ep_addr))
-  {
-    struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
+  if (tu_edpt_number(ep_addr)) {
+    struct hw_endpoint* ep = hw_endpoint_get_by_addr(ep_addr);
 
     // clear stall also reset toggle to DATA0, ready for next transfer
     ep->next_pid = 0;
@@ -561,16 +538,13 @@ void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
   }
 }
 
-void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr)
-{
-    (void) rhport;
-
-    pico_trace("dcd_edpt_close %02x\r\n", ep_addr);
-    hw_endpoint_close(ep_addr);
+void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr) {
+  (void) rhport;
+  pico_trace("dcd_edpt_close %02x\r\n", ep_addr);
+  hw_endpoint_close(ep_addr);
 }
 
-void __tusb_irq_path_func(dcd_int_handler)(uint8_t rhport)
-{
+void __tusb_irq_path_func(dcd_int_handler)(uint8_t rhport) {
   (void) rhport;
   dcd_rp2040_irq();
 }
diff --git a/src/portable/raspberrypi/rp2040/hcd_rp2040.c b/src/portable/raspberrypi/rp2040/hcd_rp2040.c
index 08aef9314..222dbbbf0 100644
--- a/src/portable/raspberrypi/rp2040/hcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/hcd_rp2040.c
@@ -113,7 +113,7 @@ static void __tusb_irq_path_func(_handle_buff_status_bit)(uint bit, struct hw_en
 static void __tusb_irq_path_func(hw_handle_buff_status)(void)
 {
   uint32_t remaining_buffers = usb_hw->buf_status;
-  pico_trace("buf_status 0x%08x\n", remaining_buffers);
+  pico_trace("buf_status 0x%08lx\n", remaining_buffers);
 
   // Check EPX first
   uint bit = 0b1;
@@ -325,10 +325,8 @@ static void _hw_endpoint_init(struct hw_endpoint *ep, uint8_t dev_addr, uint8_t
   ep->wMaxPacketSize = wMaxPacketSize;
   ep->transfer_type = transfer_type;
 
-  pico_trace("hw_endpoint_init dev %d ep %d %s xfer %d\n", ep->dev_addr, tu_edpt_number(ep->ep_addr),
-             ep_dir_string[tu_edpt_dir(ep->ep_addr)], ep->transfer_type);
-  pico_trace("dev %d ep %d %s setup buffer @ 0x%p\n", ep->dev_addr, tu_edpt_number(ep->ep_addr),
-             ep_dir_string[tu_edpt_dir(ep->ep_addr)], ep->hw_data_buf);
+  pico_trace("hw_endpoint_init dev %d ep %02X xfer %d\n", ep->dev_addr, ep->ep_addr, ep->transfer_type);
+  pico_trace("dev %d ep %02X setup buffer @ 0x%p\n", ep->dev_addr, ep->ep_addr, ep->hw_data_buf);
   uint dpram_offset = hw_data_offset(ep->hw_data_buf);
   // Bits 0-5 should be 0
   assert(!(dpram_offset & 0b111111));
@@ -343,7 +341,7 @@ static void _hw_endpoint_init(struct hw_endpoint *ep, uint8_t dev_addr, uint8_t
     ep_reg |= (uint32_t) ((bmInterval - 1) << EP_CTRL_HOST_INTERRUPT_INTERVAL_LSB);
   }
   *ep->endpoint_control = ep_reg;
-  pico_trace("endpoint control (0x%p) <- 0x%x\n", ep->endpoint_control, ep_reg);
+  pico_trace("endpoint control (0x%p) <- 0x%lx\n", ep->endpoint_control, ep_reg);
   ep->configured = true;
 
   if ( ep != &epx )
@@ -411,6 +409,16 @@ bool hcd_init(uint8_t rhport)
   return true;
 }
 
+bool hcd_deinit(uint8_t rhport) {
+  (void) rhport;
+
+  irq_remove_handler(USBCTRL_IRQ, hcd_rp2040_irq);
+  reset_block(RESETS_RESET_USBCTRL_BITS);
+  unreset_block_wait(RESETS_RESET_USBCTRL_BITS);
+
+  return true;
+}
+
 void hcd_port_reset(uint8_t rhport)
 {
   (void) rhport;
diff --git a/src/portable/raspberrypi/rp2040/rp2040_usb.c b/src/portable/raspberrypi/rp2040/rp2040_usb.c
index a512dc34f..1ca711c77 100644
--- a/src/portable/raspberrypi/rp2040/rp2040_usb.c
+++ b/src/portable/raspberrypi/rp2040/rp2040_usb.c
@@ -35,26 +35,18 @@
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF PROTOTYPE
 //--------------------------------------------------------------------+
-
-// Direction strings for debug
-const char *ep_dir_string[] = {
-        "out",
-        "in",
-};
-
-static void _hw_endpoint_xfer_sync(struct hw_endpoint *ep);
+static void _hw_endpoint_xfer_sync(struct hw_endpoint* ep);
 
 #if TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
-  static bool e15_is_bulkin_ep(struct hw_endpoint *ep);
-  static bool e15_is_critical_frame_period(struct hw_endpoint *ep);
+  static bool e15_is_bulkin_ep(struct hw_endpoint* ep);
+  static bool e15_is_critical_frame_period(struct hw_endpoint* ep);
 #else
   #define e15_is_bulkin_ep(x)             (false)
   #define e15_is_critical_frame_period(x) (false)
 #endif
 
 // if usb hardware is in host mode
-TU_ATTR_ALWAYS_INLINE static inline bool is_host_mode(void)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool is_host_mode(void) {
   return (usb_hw->main_ctrl & USB_MAIN_CTRL_HOST_NDEVICE_BITS) ? true : false;
 }
 
@@ -62,8 +54,7 @@ TU_ATTR_ALWAYS_INLINE static inline bool is_host_mode(void)
 // Implementation
 //--------------------------------------------------------------------+
 
-void rp2040_usb_init(void)
-{
+void rp2040_usb_init(void) {
   // Reset usb controller
   reset_block(RESETS_RESET_USBCTRL_BITS);
   unreset_block_wait(RESETS_RESET_USBCTRL_BITS);
@@ -88,46 +79,33 @@ void rp2040_usb_init(void)
   TU_LOG2_INT(sizeof(hw_endpoint_t));
 }
 
-void __tusb_irq_path_func(hw_endpoint_reset_transfer)(struct hw_endpoint *ep)
-{
+void __tusb_irq_path_func(hw_endpoint_reset_transfer)(struct hw_endpoint* ep) {
   ep->active = false;
   ep->remaining_len = 0;
   ep->xferred_len = 0;
   ep->user_buf = 0;
 }
 
-void __tusb_irq_path_func(_hw_endpoint_buffer_control_update32)(struct hw_endpoint *ep, uint32_t and_mask, uint32_t or_mask)
-{
+void __tusb_irq_path_func(_hw_endpoint_buffer_control_update32)(struct hw_endpoint* ep, uint32_t and_mask,
+                                                               uint32_t or_mask) {
   uint32_t value = 0;
 
-  if ( and_mask )
-  {
+  if (and_mask) {
     value = *ep->buffer_control & and_mask;
   }
 
-  if ( or_mask )
-  {
+  if (or_mask) {
     value |= or_mask;
-    if ( or_mask & USB_BUF_CTRL_AVAIL )
-    {
-      if ( *ep->buffer_control & USB_BUF_CTRL_AVAIL )
-      {
-        panic("ep %d %s was already available", tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
+    if (or_mask & USB_BUF_CTRL_AVAIL) {
+      if (*ep->buffer_control & USB_BUF_CTRL_AVAIL) {
+        panic("ep %02X was already available", ep->ep_addr);
       }
       *ep->buffer_control = value & ~USB_BUF_CTRL_AVAIL;
-      // 12 cycle delay.. (should be good for 48*12Mhz = 576Mhz)
+      // 4.1.2.5.1 Con-current access: 12 cycles (should be good for 48*12Mhz = 576Mhz) after write to buffer control
       // Don't need delay in host mode as host is in charge
-#if !CFG_TUH_ENABLED
-            __asm volatile (
-                    "b 1f\n"
-                    "1: b 1f\n"
-                    "1: b 1f\n"
-                    "1: b 1f\n"
-                    "1: b 1f\n"
-                    "1: b 1f\n"
-                    "1:\n"
-                    : : : "memory");
-#endif
+      if ( !is_host_mode()) {
+        busy_wait_at_least_cycles(12);
+      }
     }
   }
 
@@ -135,10 +113,9 @@ void __tusb_irq_path_func(_hw_endpoint_buffer_control_update32)(struct hw_endpoi
 }
 
 // prepare buffer, return buffer control
-static uint32_t __tusb_irq_path_func(prepare_ep_buffer)(struct hw_endpoint *ep, uint8_t buf_id)
-{
+static uint32_t __tusb_irq_path_func(prepare_ep_buffer)(struct hw_endpoint* ep, uint8_t buf_id) {
   uint16_t const buflen = tu_min16(ep->remaining_len, ep->wMaxPacketSize);
-  ep->remaining_len = (uint16_t)(ep->remaining_len - buflen);
+  ep->remaining_len = (uint16_t) (ep->remaining_len - buflen);
 
   uint32_t buf_ctrl = buflen | USB_BUF_CTRL_AVAIL;
 
@@ -146,10 +123,9 @@ static uint32_t __tusb_irq_path_func(prepare_ep_buffer)(struct hw_endpoint *ep,
   buf_ctrl |= ep->next_pid ? USB_BUF_CTRL_DATA1_PID : USB_BUF_CTRL_DATA0_PID;
   ep->next_pid ^= 1u;
 
-  if ( !ep->rx )
-  {
+  if (!ep->rx) {
     // Copy data from user buffer to hw buffer
-    memcpy(ep->hw_data_buf + buf_id*64, ep->user_buf, buflen);
+    memcpy(ep->hw_data_buf + buf_id * 64, ep->user_buf, buflen);
     ep->user_buf += buflen;
 
     // Mark as full
@@ -159,8 +135,7 @@ static uint32_t __tusb_irq_path_func(prepare_ep_buffer)(struct hw_endpoint *ep,
   // Is this the last buffer? Only really matters for host mode. Will trigger
   // the trans complete irq but also stop it polling. We only really care about
   // trans complete for setup packets being sent
-  if (ep->remaining_len == 0)
-  {
+  if (ep->remaining_len == 0) {
     buf_ctrl |= USB_BUF_CTRL_LAST;
   }
 
@@ -170,8 +145,7 @@ static uint32_t __tusb_irq_path_func(prepare_ep_buffer)(struct hw_endpoint *ep,
 }
 
 // Prepare buffer control register value
-void __tusb_irq_path_func(hw_endpoint_start_next_buffer)(struct hw_endpoint *ep)
-{
+void __tusb_irq_path_func(hw_endpoint_start_next_buffer)(struct hw_endpoint* ep) {
   uint32_t ep_ctrl = *ep->endpoint_control;
 
   // always compute and start with buffer 0
@@ -186,8 +160,7 @@ void __tusb_irq_path_func(hw_endpoint_start_next_buffer)(struct hw_endpoint *ep)
   bool const force_single = (!is_host && !tu_edpt_dir(ep->ep_addr)) ||
                             (is_host && tu_edpt_number(ep->ep_addr) != 0);
 
-  if(ep->remaining_len && !force_single)
-  {
+  if (ep->remaining_len && !force_single) {
     // Use buffer 1 (double buffered) if there is still data
     // TODO: Isochronous for buffer1 bit-field is different than CBI (control bulk, interrupt)
 
@@ -196,8 +169,7 @@ void __tusb_irq_path_func(hw_endpoint_start_next_buffer)(struct hw_endpoint *ep)
     // Set endpoint control double buffered bit if needed
     ep_ctrl &= ~EP_CTRL_INTERRUPT_PER_BUFFER;
     ep_ctrl |= EP_CTRL_DOUBLE_BUFFERED_BITS | EP_CTRL_INTERRUPT_PER_DOUBLE_BUFFER;
-  }else
-  {
+  } else {
     // Single buffered since 1 is enough
     ep_ctrl &= ~(EP_CTRL_DOUBLE_BUFFERED_BITS | EP_CTRL_INTERRUPT_PER_DOUBLE_BUFFER);
     ep_ctrl |= EP_CTRL_INTERRUPT_PER_BUFFER;
@@ -212,35 +184,28 @@ void __tusb_irq_path_func(hw_endpoint_start_next_buffer)(struct hw_endpoint *ep)
   _hw_endpoint_buffer_control_set_value32(ep, buf_ctrl);
 }
 
-void hw_endpoint_xfer_start(struct hw_endpoint *ep, uint8_t *buffer, uint16_t total_len)
-{
+void hw_endpoint_xfer_start(struct hw_endpoint* ep, uint8_t* buffer, uint16_t total_len) {
   hw_endpoint_lock_update(ep, 1);
 
-  if ( ep->active )
-  {
+  if (ep->active) {
     // TODO: Is this acceptable for interrupt packets?
-    TU_LOG(1, "WARN: starting new transfer on already active ep %d %s\r\n", tu_edpt_number(ep->ep_addr),
-              ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
-
+    TU_LOG(1, "WARN: starting new transfer on already active ep %02X\r\n", ep->ep_addr);
     hw_endpoint_reset_transfer(ep);
   }
 
   // Fill in info now that we're kicking off the hw
   ep->remaining_len = total_len;
-  ep->xferred_len   = 0;
-  ep->active        = true;
-  ep->user_buf      = buffer;
+  ep->xferred_len = 0;
+  ep->active = true;
+  ep->user_buf = buffer;
 
-  if ( e15_is_bulkin_ep(ep) )
-  {
+  if (e15_is_bulkin_ep(ep)) {
     usb_hw_set->inte = USB_INTS_DEV_SOF_BITS;
   }
 
-  if ( e15_is_critical_frame_period(ep) )
-  {
+  if (e15_is_critical_frame_period(ep)) {
     ep->pending = 1;
-  } else
-  {
+  } else {
     hw_endpoint_start_next_buffer(ep);
   }
 
@@ -248,34 +213,30 @@ void hw_endpoint_xfer_start(struct hw_endpoint *ep, uint8_t *buffer, uint16_t to
 }
 
 // sync endpoint buffer and return transferred bytes
-static uint16_t __tusb_irq_path_func(sync_ep_buffer)(struct hw_endpoint *ep, uint8_t buf_id)
-{
+static uint16_t __tusb_irq_path_func(sync_ep_buffer)(struct hw_endpoint* ep, uint8_t buf_id) {
   uint32_t buf_ctrl = _hw_endpoint_buffer_control_get_value32(ep);
-  if (buf_id)  buf_ctrl = buf_ctrl >> 16;
+  if (buf_id) buf_ctrl = buf_ctrl >> 16;
 
   uint16_t xferred_bytes = buf_ctrl & USB_BUF_CTRL_LEN_MASK;
 
-  if ( !ep->rx )
-  {
+  if (!ep->rx) {
     // We are continuing a transfer here. If we are TX, we have successfully
     // sent some data can increase the length we have sent
     assert(!(buf_ctrl & USB_BUF_CTRL_FULL));
 
-    ep->xferred_len = (uint16_t)(ep->xferred_len + xferred_bytes);
-  }else
-  {
+    ep->xferred_len = (uint16_t) (ep->xferred_len + xferred_bytes);
+  } else {
     // If we have received some data, so can increase the length
     // we have received AFTER we have copied it to the user buffer at the appropriate offset
     assert(buf_ctrl & USB_BUF_CTRL_FULL);
 
-    memcpy(ep->user_buf, ep->hw_data_buf + buf_id*64, xferred_bytes);
-    ep->xferred_len = (uint16_t)(ep->xferred_len + xferred_bytes);
+    memcpy(ep->user_buf, ep->hw_data_buf + buf_id * 64, xferred_bytes);
+    ep->xferred_len = (uint16_t) (ep->xferred_len + xferred_bytes);
     ep->user_buf += xferred_bytes;
   }
 
   // Short packet
-  if (xferred_bytes < ep->wMaxPacketSize)
-  {
+  if (xferred_bytes < ep->wMaxPacketSize) {
     pico_trace("  Short packet on buffer %d with %u bytes\r\n", buf_id, xferred_bytes);
     // Reduce total length as this is last packet
     ep->remaining_len = 0;
@@ -284,8 +245,7 @@ static uint16_t __tusb_irq_path_func(sync_ep_buffer)(struct hw_endpoint *ep, uin
   return xferred_bytes;
 }
 
-static void __tusb_irq_path_func(_hw_endpoint_xfer_sync) (struct hw_endpoint *ep)
-{
+static void __tusb_irq_path_func(_hw_endpoint_xfer_sync)(struct hw_endpoint* ep) {
   // Update hw endpoint struct with info from hardware
   // after a buff status interrupt
 
@@ -296,14 +256,11 @@ static void __tusb_irq_path_func(_hw_endpoint_xfer_sync) (struct hw_endpoint *ep
   uint16_t buf0_bytes = sync_ep_buffer(ep, 0);
 
   // sync buffer 1 if double buffered
-  if ( (*ep->endpoint_control) & EP_CTRL_DOUBLE_BUFFERED_BITS )
-  {
-    if (buf0_bytes == ep->wMaxPacketSize)
-    {
+  if ((*ep->endpoint_control) & EP_CTRL_DOUBLE_BUFFERED_BITS) {
+    if (buf0_bytes == ep->wMaxPacketSize) {
       // sync buffer 1 if not short packet
       sync_ep_buffer(ep, 1);
-    }else
-    {
+    } else {
       // short packet on buffer 0
       // TODO couldn't figure out how to handle this case which happen with net_lwip_webserver example
       // At this time (currently trigger per 2 buffer), the buffer1 is probably filled with data from
@@ -335,14 +292,12 @@ static void __tusb_irq_path_func(_hw_endpoint_xfer_sync) (struct hw_endpoint *ep
 }
 
 // Returns true if transfer is complete
-bool __tusb_irq_path_func(hw_endpoint_xfer_continue)(struct hw_endpoint *ep)
-{
+bool __tusb_irq_path_func(hw_endpoint_xfer_continue)(struct hw_endpoint* ep) {
   hw_endpoint_lock_update(ep, 1);
 
   // Part way through a transfer
-  if (!ep->active)
-  {
-    panic("Can't continue xfer on inactive ep %d %s", tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
+  if (!ep->active) {
+    panic("Can't continue xfer on inactive ep %02X", ep->ep_addr);
   }
 
   // Update EP struct from hardware state
@@ -350,21 +305,15 @@ bool __tusb_irq_path_func(hw_endpoint_xfer_continue)(struct hw_endpoint *ep)
 
   // Now we have synced our state with the hardware. Is there more data to transfer?
   // If we are done then notify tinyusb
-  if (ep->remaining_len == 0)
-  {
-    pico_trace("Completed transfer of %d bytes on ep %d %s\r\n",
-               ep->xferred_len, tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
+  if (ep->remaining_len == 0) {
+    pico_trace("Completed transfer of %d bytes on ep %02X\r\n", ep->xferred_len, ep->ep_addr);
     // Notify caller we are done so it can notify the tinyusb stack
     hw_endpoint_lock_update(ep, -1);
     return true;
-  }
-  else
-  {
-    if ( e15_is_critical_frame_period(ep) )
-    {
+  } else {
+    if (e15_is_critical_frame_period(ep)) {
       ep->pending = 1;
-    } else
-    {
+    } else {
       hw_endpoint_start_next_buffer(ep);
     }
   }
@@ -399,16 +348,14 @@ bool __tusb_irq_path_func(hw_endpoint_xfer_continue)(struct hw_endpoint *ep)
 volatile uint32_t e15_last_sof = 0;
 
 // check if Errata 15 is needed for this endpoint i.e device bulk-in
-static bool __tusb_irq_path_func(e15_is_bulkin_ep) (struct hw_endpoint *ep)
-{
+static bool __tusb_irq_path_func(e15_is_bulkin_ep)(struct hw_endpoint* ep) {
   return (!is_host_mode() && tu_edpt_dir(ep->ep_addr) == TUSB_DIR_IN &&
           ep->transfer_type == TUSB_XFER_BULK);
 }
 
 // check if we need to apply Errata 15 workaround : i.e
 // Endpoint is BULK IN and is currently in critical frame period i.e 20% of last usb frame
-static bool __tusb_irq_path_func(e15_is_critical_frame_period) (struct hw_endpoint *ep)
-{
+static bool __tusb_irq_path_func(e15_is_critical_frame_period)(struct hw_endpoint* ep) {
   TU_VERIFY(e15_is_bulkin_ep(ep));
 
   /* Avoid the last 200us (uframe 6.5-7) of a frame, up to the EOF2 point.
@@ -419,11 +366,10 @@ static bool __tusb_irq_path_func(e15_is_critical_frame_period) (struct hw_endpoi
   if (delta < 800 || delta > 998) {
     return false;
   }
-  TU_LOG(3, "Avoiding sof %lu now %lu last %lu\r\n", (usb_hw->sof_rd + 1) & USB_SOF_RD_BITS, time_us_32(), e15_last_sof);
+  TU_LOG(3, "Avoiding sof %lu now %lu last %lu\r\n", (usb_hw->sof_rd + 1) & USB_SOF_RD_BITS, time_us_32(),
+         e15_last_sof);
   return true;
 }
 
-#endif
-
-
+#endif // TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
 #endif
diff --git a/src/portable/renesas/rusb2/dcd_rusb2.c b/src/portable/renesas/rusb2/dcd_rusb2.c
index 24edc30e7..50400d1f5 100644
--- a/src/portable/renesas/rusb2/dcd_rusb2.c
+++ b/src/portable/renesas/rusb2/dcd_rusb2.c
@@ -57,29 +57,22 @@
 //--------------------------------------------------------------------+
 // MACRO TYPEDEF CONSTANT ENUM
 //--------------------------------------------------------------------+
+enum {
+  PIPE_COUNT = 10,
+};
 
-/* Start of definition of packed structs (used by the CCRX toolchain) */
-TU_ATTR_PACKED_BEGIN
-TU_ATTR_BIT_FIELD_ORDER_BEGIN
-
-typedef struct TU_ATTR_PACKED
-{
+typedef struct {
   void      *buf;      /* the start address of a transfer data buffer */
   uint16_t  length;    /* the number of bytes in the buffer */
   uint16_t  remaining; /* the number of bytes remaining in the buffer */
-  struct {
-    uint32_t ep  : 8;  /* an assigned endpoint address */
-    uint32_t ff  : 1;  /* `buf` is TU_FUFO or POD */
-    uint32_t     : 0;
-  };
-} pipe_state_t;
 
-TU_ATTR_PACKED_END  // End of definition of packed structs (used by the CCRX toolchain)
-TU_ATTR_BIT_FIELD_ORDER_END
+  uint8_t ep; /* an assigned endpoint address */
+  uint8_t ff; /* `buf` is TU_FUFO or POD */
+} pipe_state_t;
 
 typedef struct
 {
-  pipe_state_t pipe[10];
+  pipe_state_t pipe[PIPE_COUNT];
   uint8_t ep[2][16];   /* a lookup table for a pipe index from an endpoint address */
 } dcd_data_t;
 
@@ -89,52 +82,44 @@ static dcd_data_t _dcd;
 // INTERNAL OBJECT & FUNCTION DECLARATION
 //--------------------------------------------------------------------+
 
-// Transfer conditions specifiable for each pipe:
-// - Pipe 0: Control transfer with 64-byte single buffer
-// - Pipes 1 and 2: Bulk isochronous transfer continuous transfer mode with programmable buffer size up
-//   to 2 KB and optional double buffer
-// - Pipes 3 to 5: Bulk transfer continuous transfer mode with programmable buffer size up to 2 KB and
-//   optional double buffer
-// - Pipes 6 to 9: Interrupt transfer with 64-byte single buffer
-enum {
-  PIPE_1ST_BULK = 3,
-  PIPE_1ST_INTERRUPT = 6,
-  PIPE_COUNT = 10,
-};
 
-static unsigned find_pipe(unsigned xfer)
-{
-  switch (xfer) {
-    case TUSB_XFER_ISOCHRONOUS:
-      for (int i = 1; i < PIPE_1ST_BULK; ++i) {
-        if (0 == _dcd.pipe[i].ep) return i;
-      }
-      break;
-
-    case TUSB_XFER_BULK:
-      for (int i = PIPE_1ST_BULK; i < PIPE_1ST_INTERRUPT; ++i) {
-        if (0 == _dcd.pipe[i].ep) return i;
-      }
-      for (int i = 1; i < PIPE_1ST_BULK; ++i) {
-        if (0 == _dcd.pipe[i].ep) return i;
-      }
-      break;
+// Transfer conditions specifiable for each pipe for most MCUs
+// - Pipe 0: Control transfer with 64-byte single buffer
+// - Pipes 1 and 2: Bulk or ISO
+// - Pipes 3 to 5: Bulk
+// - Pipes 6 to 9: Interrupt
+//
+// Note: for small mcu such as
+// - RA2A1: only pipe 4-7 are available, and no support for ISO
+static unsigned find_pipe(unsigned xfer_type) {
+  #if defined(BSP_MCU_GROUP_RA2A1)
+  const uint8_t pipe_idx_arr[4][2] = {
+      { 0, 0 }, // Control
+      { 0, 0 }, // Isochronous not supported
+      { 4, 5 }, // Bulk
+      { 6, 7 }, // Interrupt
+  };
+  #else
+  const uint8_t pipe_idx_arr[4][2] = {
+      { 0, 0 }, // Control
+      { 1, 2 }, // Isochronous
+      { 1, 5 }, // Bulk
+      { 6, 9 }, // Interrupt
+  };
+  #endif
 
-    case TUSB_XFER_INTERRUPT:
-      for (int i = PIPE_1ST_INTERRUPT; i < PIPE_COUNT; ++i) {
-        if (0 == _dcd.pipe[i].ep) return i;
-      }
-      break;
+  // find backward since only pipe 1, 2 support ISO
+  const uint8_t idx_first = pipe_idx_arr[xfer_type][0];
+  const uint8_t idx_last  = pipe_idx_arr[xfer_type][1];
 
-    default:
-      /* No support for control transfer */
-      break;
+  for (int i = idx_last; i >= idx_first; i--) {
+    if (0 == _dcd.pipe[i].ep) return i;
   }
+
   return 0;
 }
 
-static volatile uint16_t* get_pipectr(rusb2_reg_t *rusb, unsigned num)
-{
+static volatile uint16_t* get_pipectr(rusb2_reg_t *rusb, unsigned num) {
   if (num) {
     return (volatile uint16_t*)&(rusb->PIPE_CTR[num - 1]);
   } else {
@@ -142,8 +127,7 @@ static volatile uint16_t* get_pipectr(rusb2_reg_t *rusb, unsigned num)
   }
 }
 
-static volatile reg_pipetre_t* get_pipetre(rusb2_reg_t *rusb, unsigned num)
-{
+static volatile reg_pipetre_t* get_pipetre(rusb2_reg_t *rusb, unsigned num) {
   volatile reg_pipetre_t* tre = NULL;
   if ((1 <= num) && (num <= 5)) {
     tre = (volatile reg_pipetre_t*)&(rusb->PIPE_TR[num - 1].E);
@@ -151,8 +135,7 @@ static volatile reg_pipetre_t* get_pipetre(rusb2_reg_t *rusb, unsigned num)
   return tre;
 }
 
-static volatile uint16_t* ep_addr_to_pipectr(uint8_t rhport, unsigned ep_addr)
-{
+static volatile uint16_t* ep_addr_to_pipectr(uint8_t rhport, unsigned ep_addr) {
   rusb2_reg_t *rusb = RUSB2_REG(rhport);
   const unsigned epn = tu_edpt_number(ep_addr);
 
@@ -165,19 +148,16 @@ static volatile uint16_t* ep_addr_to_pipectr(uint8_t rhport, unsigned ep_addr)
   }
 }
 
-static uint16_t edpt0_max_packet_size(rusb2_reg_t* rusb)
-{
+static uint16_t edpt0_max_packet_size(rusb2_reg_t* rusb) {
   return rusb->DCPMAXP_b.MXPS;
 }
 
-static uint16_t edpt_max_packet_size(rusb2_reg_t *rusb, unsigned num)
-{
+static uint16_t edpt_max_packet_size(rusb2_reg_t *rusb, unsigned num) {
   rusb->PIPESEL = num;
   return rusb->PIPEMAXP;
 }
 
-static inline void pipe_wait_for_ready(rusb2_reg_t * rusb, unsigned num)
-{
+static inline void pipe_wait_for_ready(rusb2_reg_t * rusb, unsigned num) {
   while ( rusb->D0FIFOSEL_b.CURPIPE != num ) {}
   while ( !rusb->D0FIFOCTR_b.FRDY ) {}
 }
@@ -266,14 +246,6 @@ static void pipe_read_packet_ff(rusb2_reg_t * rusb, tu_fifo_t *f, volatile void
   tu_fifo_advance_write_pointer(f, count);
 }
 
-
-static bool wait_pipe_fifo_empty(rusb2_reg_t* rusb, uint8_t num) {
-  TU_ASSERT(num);
-  while( (rusb->PIPE_CTR[num-1] & RUSB2_PIPE_CTR_INBUFM_Msk) > 0 ) {}
-  return true;
-}
-
-
 //--------------------------------------------------------------------+
 // Pipe Transfer
 //--------------------------------------------------------------------+
@@ -347,7 +319,6 @@ static bool pipe_xfer_in(rusb2_reg_t* rusb, unsigned num)
   const unsigned rem  = pipe->remaining;
 
   if (!rem) {
-    wait_pipe_fifo_empty(rusb, num);
     pipe->buf = NULL;
     return true;
   }
@@ -844,7 +815,7 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * ep_desc)
   }
 
   rusb->PIPECFG = cfg;
-  rusb->BRDYSTS = 0x1FFu ^ TU_BIT(num);
+  rusb->BRDYSTS = 0x3FFu ^ TU_BIT(num);
   rusb->BRDYENB |= TU_BIT(num);
 
   if (dir || (xfer != TUSB_XFER_BULK)) {
@@ -944,6 +915,18 @@ void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
 //--------------------------------------------------------------------+
 // ISR
 //--------------------------------------------------------------------+
+
+#if defined(__CCRX__)
+TU_ATTR_ALWAYS_INLINE static inline unsigned __builtin_ctz(unsigned int value) {
+  unsigned int count = 0;
+  while ((value & 1) == 0) {
+    value >>= 1;
+    count++;
+  }
+  return count;
+}
+#endif
+
 void dcd_int_handler(uint8_t rhport)
 {
   rusb2_reg_t* rusb = RUSB2_REG(rhport);
@@ -1035,17 +1018,7 @@ void dcd_int_handler(uint8_t rhport)
     /* clear active bits (don't write 0 to already cleared bits according to the HW manual) */
     rusb->BRDYSTS = ~s;
     while (s) {
-#if defined(__CCRX__)
-      static const int Mod37BitPosition[] = {
-        -1, 0, 1, 26, 2, 23, 27, 0, 3, 16, 24, 30, 28, 11, 0, 13, 4,
-        7, 17, 0, 25, 22, 31, 15, 29, 10, 12, 6, 0, 21, 14, 9, 5,
-        20, 8, 19, 18
-      };
-
-      const unsigned num = Mod37BitPosition[(-s & s) % 37];
-#else
       const unsigned num = __builtin_ctz(s);
-#endif
       process_pipe_brdy(rhport, num);
       s &= ~TU_BIT(num);
     }
diff --git a/src/portable/renesas/rusb2/hcd_rusb2.c b/src/portable/renesas/rusb2/hcd_rusb2.c
index bf95be707..f140da690 100644
--- a/src/portable/renesas/rusb2/hcd_rusb2.c
+++ b/src/portable/renesas/rusb2/hcd_rusb2.c
@@ -45,6 +45,9 @@
 //--------------------------------------------------------------------+
 // MACRO TYPEDEF CONSTANT ENUM DECLARATION
 //--------------------------------------------------------------------+
+enum {
+  PIPE_COUNT = 10,
+};
 
 TU_ATTR_PACKED_BEGIN
 TU_ATTR_BIT_FIELD_ORDER_BEGIN
@@ -75,7 +78,7 @@ TU_ATTR_BIT_FIELD_ORDER_END
 typedef struct
 {
   bool         need_reset; /* The device has not been reset after connection. */
-  pipe_state_t pipe[10];
+  pipe_state_t pipe[PIPE_COUNT];
   uint8_t ep[4][2][15];   /* a lookup table for a pipe index from an endpoint address */
   uint8_t      ctl_mps[5]; /* EP0 max packet size for each device */
 } hcd_data_t;
@@ -86,46 +89,30 @@ typedef struct
 static hcd_data_t _hcd;
 
 // TODO merged with DCD
-// Transfer conditions specifiable for each pipe:
+// Transfer conditions specifiable for each pipe for most MCUs
 // - Pipe 0: Control transfer with 64-byte single buffer
-// - Pipes 1 and 2: Bulk isochronous transfer continuous transfer mode with programmable buffer size up
-//   to 2 KB and optional double buffer
-// - Pipes 3 to 5: Bulk transfer continuous transfer mode with programmable buffer size up to 2 KB and
-//   optional double buffer
-// - Pipes 6 to 9: Interrupt transfer with 64-byte single buffer
-enum {
-  PIPE_1ST_BULK = 3,
-  PIPE_1ST_INTERRUPT = 6,
-  PIPE_COUNT = 10,
-};
-
-static unsigned find_pipe(unsigned xfer) {
-  switch ( xfer ) {
-    case TUSB_XFER_ISOCHRONOUS:
-      for (int i = 1; i < PIPE_1ST_BULK; ++i) {
-        if ( 0 == _hcd.pipe[i].ep ) return i;
-      }
-      break;
-
-    case TUSB_XFER_BULK:
-      for (int i = PIPE_1ST_BULK; i < PIPE_1ST_INTERRUPT; ++i) {
-        if ( 0 == _hcd.pipe[i].ep ) return i;
-      }
-      for (int i = 1; i < PIPE_1ST_BULK; ++i) {
-        if ( 0 == _hcd.pipe[i].ep ) return i;
-      }
-      break;
+// - Pipes 1 and 2: Bulk or ISO
+// - Pipes 3 to 5: Bulk
+// - Pipes 6 to 9: Interrupt
+//
+// Note: for small mcu such as
+// - RA2A1: only pipe 4-7 are available, and no support for ISO
+static unsigned find_pipe(unsigned xfer_type) {
+  const uint8_t pipe_idx_arr[4][2] = {
+      { 0, 0 }, // Control
+      { 1, 2 }, // Isochronous
+      { 1, 5 }, // Bulk
+      { 6, 9 }, // Interrupt
+  };
 
-    case TUSB_XFER_INTERRUPT:
-      for (int i = PIPE_1ST_INTERRUPT; i < PIPE_COUNT; ++i) {
-        if ( 0 == _hcd.pipe[i].ep ) return i;
-      }
-      break;
+  // find backward since only pipe 1, 2 support ISO
+  const uint8_t idx_first = pipe_idx_arr[xfer_type][0];
+  const uint8_t idx_last  = pipe_idx_arr[xfer_type][1];
 
-    default:
-      /* No support for control transfer */
-      break;
+  for (int i = idx_last; i >= idx_first; i--) {
+    if (0 == _hcd.pipe[i].ep) return i;
   }
+
   return 0;
 }
 
@@ -718,7 +705,7 @@ bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const
   }
 
   rusb->PIPECFG = cfg;
-  rusb->BRDYSTS = 0x1FFu ^ TU_BIT(num);
+  rusb->BRDYSTS = 0x3FFu ^ TU_BIT(num);
   rusb->NRDYENB |= TU_BIT(num);
   rusb->BRDYENB |= TU_BIT(num);
 
@@ -771,6 +758,17 @@ bool hcd_edpt_clear_stall(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr) {
 //--------------------------------------------------------------------+
 // ISR
 //--------------------------------------------------------------------+
+#if defined(__CCRX__)
+TU_ATTR_ALWAYS_INLINE static inline unsigned __builtin_ctz(unsigned int value) {
+  unsigned int count = 0;
+  while ((value & 1) == 0) {
+    value >>= 1;
+    count++;
+  }
+  return count;
+}
+#endif
+
 void hcd_int_handler(uint8_t rhport, bool in_isr) {
   (void) in_isr;
 
@@ -820,23 +818,12 @@ void hcd_int_handler(uint8_t rhport, bool in_isr) {
     }
   }
 
-#if defined(__CCRX__)
-  static const int Mod37BitPosition[] = {
-    -1, 0, 1, 26, 2, 23, 27, 0, 3, 16, 24, 30, 28, 11, 0, 13, 4,
-    7, 17, 0, 25, 22, 31, 15, 29, 10, 12, 6, 0, 21, 14, 9, 5,
-    20, 8, 19, 18};
-#endif
-
   if (is0 & RUSB2_INTSTS0_NRDY_Msk) {
     const unsigned m = rusb->NRDYENB;
     unsigned s = rusb->NRDYSTS & m;
     rusb->NRDYSTS = ~s;
     while (s) {
-#if defined(__CCRX__)
-      const unsigned num = Mod37BitPosition[(-s & s) % 37];
-#else
       const unsigned num = __builtin_ctz(s);
-#endif
       process_pipe_nrdy(rhport, num);
       s &= ~TU_BIT(num);
     }
@@ -847,11 +834,7 @@ void hcd_int_handler(uint8_t rhport, bool in_isr) {
     /* clear active bits (don't write 0 to already cleared bits according to the HW manual) */
     rusb->BRDYSTS = ~s;
     while (s) {
-#if defined(__CCRX__)
-      const unsigned num = Mod37BitPosition[(-s & s) % 37];
-#else
       const unsigned num = __builtin_ctz(s);
-#endif
       process_pipe_brdy(rhport, num);
       s &= ~TU_BIT(num);
     }
diff --git a/src/portable/sony/cxd56/dcd_cxd56.c b/src/portable/sony/cxd56/dcd_cxd56.c
index 6677891a5..41814370e 100644
--- a/src/portable/sony/cxd56/dcd_cxd56.c
+++ b/src/portable/sony/cxd56/dcd_cxd56.c
@@ -102,17 +102,25 @@ static int _dcd_bind(FAR struct usbdevclass_driver_s *driver, FAR struct usbdev_
   usbdev = dev;
   usbdcd_driver.ep[0] = dev->ep0;
 
+  #ifdef EP_ALLOCREQ
+  // SDK v2
   usbdcd_driver.req[0] = EP_ALLOCREQ(usbdcd_driver.ep[0]);
-  if (usbdcd_driver.req[0] != NULL)
-  {
+  if (usbdcd_driver.req[0] != NULL) {
     usbdcd_driver.req[0]->len = 64;
     usbdcd_driver.req[0]->buf = EP_ALLOCBUFFER(usbdcd_driver.ep[0], 64);
-    if (!usbdcd_driver.req[0]->buf)
-    {
+    if (!usbdcd_driver.req[0]->buf) {
       EP_FREEREQ(usbdcd_driver.ep[0], usbdcd_driver.req[0]);
       usbdcd_driver.req[0] = NULL;
+      return ENOMEM;
     }
   }
+  #else
+  // SDK v3
+  usbdcd_driver.req[0] = usbdev_allocreq(usbdcd_driver.ep[0], 64);
+  if (usbdcd_driver.req[0] == NULL) {
+    return ENOMEM;
+  }
+  #endif
 
   usbdcd_driver.req[0]->callback = usbdcd_ep0incomplete;
 
@@ -295,13 +303,19 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *p_endpoint_desc)
   }
 
   usbdcd_driver.req[epnum] = NULL;
+
+  #ifdef EP_ALLOCREQ
+  // sdk v2
   usbdcd_driver.req[epnum] = EP_ALLOCREQ(usbdcd_driver.ep[epnum]);
-  if (usbdcd_driver.req[epnum] != NULL)
-  {
+  if (usbdcd_driver.req[epnum] != NULL) {
     usbdcd_driver.req[epnum]->len = ep_mps;
   }
-  else
-  {
+  #else
+  // sdk v3
+  usbdcd_driver.req[epnum] = usbdev_allocreq(usbdcd_driver.ep[epnum], ep_mps);
+  #endif
+
+  if(usbdcd_driver.req[epnum] == NULL) {
     return false;
   }
 
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index 9c37f1f98..a26c66892 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -107,9 +107,9 @@
 #include "device/dcd.h"
 
 #ifdef TUP_USBIP_FSDEV_STM32
-  // Undefine to reduce the dependence on HAL
-  #undef USE_HAL_DRIVER
-  #include "portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h"
+// Undefine to reduce the dependence on HAL
+#undef USE_HAL_DRIVER
+#include "portable/st/stm32_fsdev/dcd_stm32_fsdev.h"
 #endif
 
 /*****************************************************
@@ -119,17 +119,17 @@
 // HW supports max of 8 bidirectional endpoints, but this can be reduced to save RAM
 // (8u here would mean 8 IN and 8 OUT)
 #ifndef MAX_EP_COUNT
-#  define MAX_EP_COUNT 8U
+#define MAX_EP_COUNT 8U
 #endif
 
 // If sharing with CAN, one can set this to be non-zero to give CAN space where it wants it
 // Both of these MUST be a multiple of 2, and are in byte units.
 #ifndef DCD_STM32_BTABLE_BASE
-#  define DCD_STM32_BTABLE_BASE 0U
+#define DCD_STM32_BTABLE_BASE 0U
 #endif
 
 #ifndef DCD_STM32_BTABLE_SIZE
-#  define DCD_STM32_BTABLE_SIZE (FSDEV_PMA_SIZE - DCD_STM32_BTABLE_BASE)
+#define DCD_STM32_BTABLE_SIZE (FSDEV_PMA_SIZE - DCD_STM32_BTABLE_BASE)
 #endif
 
 /***************************************************
@@ -145,21 +145,18 @@ TU_VERIFY_STATIC(((DCD_STM32_BTABLE_BASE) % 8) == 0, "BTABLE base must be aligne
 //--------------------------------------------------------------------+
 
 // One of these for every EP IN & OUT, uses a bit of RAM....
-typedef struct
-{
-  uint8_t * buffer;
-  tu_fifo_t * ff;
+typedef struct {
+  uint8_t *buffer;
+  tu_fifo_t *ff;
   uint16_t total_len;
   uint16_t queued_len;
-  uint16_t pma_ptr;
   uint16_t max_packet_size;
-  uint16_t pma_alloc_size;
-  uint8_t ep_idx; // index for USB_EPnR register
+  uint8_t ep_idx;   // index for USB_EPnR register
+  bool iso_in_sending; // Workaround for ISO IN EP doesn't have interrupt mask
 } xfer_ctl_t;
 
 // EP allocator
-typedef struct
-{
+typedef struct {
   uint8_t ep_num;
   uint8_t ep_type;
   bool allocated[2];
@@ -179,28 +176,25 @@ static uint8_t remoteWakeCountdown; // When wake is requested
 
 // into the stack.
 static void dcd_handle_bus_reset(void);
-static void dcd_transmit_packet(xfer_ctl_t * xfer, uint16_t ep_ix);
+static void dcd_transmit_packet(xfer_ctl_t *xfer, uint16_t ep_ix);
+static bool edpt_xfer(uint8_t rhport, uint8_t ep_addr);
 static void dcd_ep_ctr_handler(void);
 
 // PMA allocation/access
-static uint8_t open_ep_count;
 static uint16_t ep_buf_ptr; ///< Points to first free memory location
-static void dcd_pma_alloc_reset(void);
-static uint16_t dcd_pma_alloc(uint8_t ep_addr, uint16_t length);
-static void dcd_pma_free(uint8_t ep_addr);
-static void dcd_ep_free(uint8_t ep_addr);
+static uint32_t dcd_pma_alloc(uint16_t length, bool dbuf);
 static uint8_t dcd_ep_alloc(uint8_t ep_addr, uint8_t ep_type);
 static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t wNBytes);
 static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t wNBytes);
 
-static bool dcd_write_packet_memory_ff(tu_fifo_t * ff, uint16_t dst, uint16_t wNBytes);
-static bool dcd_read_packet_memory_ff(tu_fifo_t * ff, uint16_t src, uint16_t wNBytes);
+static bool dcd_write_packet_memory_ff(tu_fifo_t *ff, uint16_t dst, uint16_t wNBytes);
+static bool dcd_read_packet_memory_ff(tu_fifo_t *ff, uint16_t src, uint16_t wNBytes);
 
 //--------------------------------------------------------------------+
 // Inline helper
 //--------------------------------------------------------------------+
 
-TU_ATTR_ALWAYS_INLINE static inline xfer_ctl_t* xfer_ctl_ptr(uint32_t ep_addr)
+TU_ATTR_ALWAYS_INLINE static inline xfer_ctl_t *xfer_ctl_ptr(uint32_t ep_addr)
 {
   uint8_t epnum = tu_edpt_number(ep_addr);
   uint8_t dir = tu_edpt_dir(ep_addr);
@@ -214,7 +208,7 @@ TU_ATTR_ALWAYS_INLINE static inline xfer_ctl_t* xfer_ctl_ptr(uint32_t ep_addr)
 // Controller API
 //--------------------------------------------------------------------+
 
-void dcd_init (uint8_t rhport)
+void dcd_init(uint8_t rhport)
 {
   /* Clocks should already be enabled */
   /* Use __HAL_RCC_USB_CLK_ENABLE(); to enable the clocks before calling this function */
@@ -222,43 +216,41 @@ void dcd_init (uint8_t rhport)
   /* The RM mentions to use a special ordering of PDWN and FRES, but this isn't done in HAL.
    * Here, the RM is followed. */
 
-  for(uint32_t i = 0; i<200; i++) // should be a few us
-  {
+  for (uint32_t i = 0; i < 200; i++) { // should be a few us
     asm("NOP");
   }
   // Perform USB peripheral reset
   USB->CNTR = USB_CNTR_FRES | USB_CNTR_PDWN;
-  for(uint32_t i = 0; i<200; i++) // should be a few us
-  {
+  for (uint32_t i = 0; i < 200; i++) { // should be a few us
     asm("NOP");
   }
 
   USB->CNTR &= ~USB_CNTR_PDWN;
 
   // Wait startup time, for F042 and F070, this is <= 1 us.
-  for(uint32_t i = 0; i<200; i++) // should be a few us
-  {
+  for (uint32_t i = 0; i < 200; i++) { // should be a few us
     asm("NOP");
   }
   USB->CNTR = 0; // Enable USB
 
-#if !defined(STM32G0) && !defined(STM32H5)  // BTABLE register does not exist any more on STM32G0, it is fixed to USB SRAM base address
+#if !defined(STM32G0) && !defined(STM32H5) // BTABLE register does not exist any more on STM32G0, it is fixed to USB SRAM base address
   USB->BTABLE = DCD_STM32_BTABLE_BASE;
 #endif
   USB->ISTR = 0; // Clear pending interrupts
 
   // Reset endpoints to disabled
-  for(uint32_t i=0; i<STFSDEV_EP_COUNT; i++)
-  {
+  for (uint32_t i = 0; i < STFSDEV_EP_COUNT; i++) {
     // This doesn't clear all bits since some bits are "toggle", but does set the type to DISABLED.
-    pcd_set_endpoint(USB,i,0u);
+    pcd_set_endpoint(USB, i, 0u);
   }
 
   USB->CNTR |= USB_CNTR_RESETM | USB_CNTR_ESOFM | USB_CNTR_CTRM | USB_CNTR_SUSPM | USB_CNTR_WKUPM;
   dcd_handle_bus_reset();
 
   // Enable pull-up if supported
-  if ( dcd_connect ) dcd_connect(rhport);
+  if (dcd_connect) {
+    dcd_connect(rhport);
+  }
 }
 
 // Define only on MCU with internal pull-up. BSP can define on MCU without internal PU.
@@ -267,14 +259,14 @@ void dcd_init (uint8_t rhport)
 // Disable internal D+ PU
 void dcd_disconnect(uint8_t rhport)
 {
-  (void) rhport;
+  (void)rhport;
   USB->BCDR &= ~(USB_BCDR_DPPU);
 }
 
 // Enable internal D+ PU
 void dcd_connect(uint8_t rhport)
 {
-  (void) rhport;
+  (void)rhport;
   USB->BCDR |= USB_BCDR_DPPU;
 }
 
@@ -282,60 +274,54 @@ void dcd_connect(uint8_t rhport)
 // Disable internal D+ PU
 void dcd_disconnect(uint8_t rhport)
 {
-  (void) rhport;
+  (void)rhport;
   SYSCFG->PMC &= ~(SYSCFG_PMC_USB_PU);
 }
 
 // Enable internal D+ PU
 void dcd_connect(uint8_t rhport)
 {
-  (void) rhport;
+  (void)rhport;
   SYSCFG->PMC |= SYSCFG_PMC_USB_PU;
 }
 #endif
 
 void dcd_sof_enable(uint8_t rhport, bool en)
 {
-  (void) rhport;
-  (void) en;
+  (void)rhport;
+  (void)en;
 
-  if (en)
-  {
+  if (en) {
     USB->CNTR |= USB_CNTR_SOFM;
-  }
-  else
-  {
+  } else {
     USB->CNTR &= ~USB_CNTR_SOFM;
   }
 }
 
 // Enable device interrupt
-void dcd_int_enable (uint8_t rhport)
+void dcd_int_enable(uint8_t rhport)
 {
   (void)rhport;
   // Member here forces write to RAM before allowing ISR to execute
   __DSB();
   __ISB();
-#if CFG_TUSB_MCU == OPT_MCU_STM32F0 || CFG_TUSB_MCU == OPT_MCU_STM32L0 || \
-    CFG_TUSB_MCU == OPT_MCU_STM32L4
+#if CFG_TUSB_MCU == OPT_MCU_STM32F0 || CFG_TUSB_MCU == OPT_MCU_STM32L0 || CFG_TUSB_MCU == OPT_MCU_STM32L4
   NVIC_EnableIRQ(USB_IRQn);
 
 #elif CFG_TUSB_MCU == OPT_MCU_STM32L1
   NVIC_EnableIRQ(USB_LP_IRQn);
 
 #elif CFG_TUSB_MCU == OPT_MCU_STM32F3
-  // Some STM32F302/F303 devices allow to remap the USB interrupt vectors from
-  // shared USB/CAN IRQs to separate CAN and USB IRQs.
-  // This dynamically checks if this remap is active to enable the right IRQs.
-  #ifdef SYSCFG_CFGR1_USB_IT_RMP
-  if (SYSCFG->CFGR1 & SYSCFG_CFGR1_USB_IT_RMP)
-  {
+// Some STM32F302/F303 devices allow to remap the USB interrupt vectors from
+// shared USB/CAN IRQs to separate CAN and USB IRQs.
+// This dynamically checks if this remap is active to enable the right IRQs.
+#ifdef SYSCFG_CFGR1_USB_IT_RMP
+  if (SYSCFG->CFGR1 & SYSCFG_CFGR1_USB_IT_RMP) {
     NVIC_EnableIRQ(USB_HP_IRQn);
     NVIC_EnableIRQ(USB_LP_IRQn);
     NVIC_EnableIRQ(USBWakeUp_RMP_IRQn);
-  }
-  else
-  #endif
+  } else
+#endif
   {
     NVIC_EnableIRQ(USB_HP_CAN_TX_IRQn);
     NVIC_EnableIRQ(USB_LP_CAN_RX0_IRQn);
@@ -352,14 +338,14 @@ void dcd_int_enable (uint8_t rhport)
   NVIC_EnableIRQ(USBWakeUp_IRQn);
 
 #elif CFG_TUSB_MCU == OPT_MCU_STM32G0
-  #ifdef STM32G0B0xx
-    NVIC_EnableIRQ(USB_IRQn);
-  #else
-    NVIC_EnableIRQ(USB_UCPD1_2_IRQn);
-  #endif
+#ifdef STM32G0B0xx
+  NVIC_EnableIRQ(USB_IRQn);
+#else
+  NVIC_EnableIRQ(USB_UCPD1_2_IRQn);
+#endif
 
 #elif CFG_TUSB_MCU == OPT_MCU_STM32H5
-    NVIC_EnableIRQ(USB_DRD_FS_IRQn);
+  NVIC_EnableIRQ(USB_DRD_FS_IRQn);
 
 #elif CFG_TUSB_MCU == OPT_MCU_STM32WB
   NVIC_EnableIRQ(USB_HP_IRQn);
@@ -369,7 +355,7 @@ void dcd_int_enable (uint8_t rhport)
   NVIC_EnableIRQ(USB_FS_IRQn);
 
 #else
-  #error Unknown arch in USB driver
+#error Unknown arch in USB driver
 #endif
 }
 
@@ -378,24 +364,21 @@ void dcd_int_disable(uint8_t rhport)
 {
   (void)rhport;
 
-#if CFG_TUSB_MCU == OPT_MCU_STM32F0 || CFG_TUSB_MCU == OPT_MCU_STM32L0 || \
-    CFG_TUSB_MCU == OPT_MCU_STM32L4
+#if CFG_TUSB_MCU == OPT_MCU_STM32F0 || CFG_TUSB_MCU == OPT_MCU_STM32L0 || CFG_TUSB_MCU == OPT_MCU_STM32L4
   NVIC_DisableIRQ(USB_IRQn);
 #elif CFG_TUSB_MCU == OPT_MCU_STM32L1
   NVIC_DisableIRQ(USB_LP_IRQn);
 #elif CFG_TUSB_MCU == OPT_MCU_STM32F3
-  // Some STM32F302/F303 devices allow to remap the USB interrupt vectors from
-  // shared USB/CAN IRQs to separate CAN and USB IRQs.
-  // This dynamically checks if this remap is active to disable the right IRQs.
-  #ifdef SYSCFG_CFGR1_USB_IT_RMP
-  if (SYSCFG->CFGR1 & SYSCFG_CFGR1_USB_IT_RMP)
-  {
+// Some STM32F302/F303 devices allow to remap the USB interrupt vectors from
+// shared USB/CAN IRQs to separate CAN and USB IRQs.
+// This dynamically checks if this remap is active to disable the right IRQs.
+#ifdef SYSCFG_CFGR1_USB_IT_RMP
+  if (SYSCFG->CFGR1 & SYSCFG_CFGR1_USB_IT_RMP) {
     NVIC_DisableIRQ(USB_HP_IRQn);
     NVIC_DisableIRQ(USB_LP_IRQn);
     NVIC_DisableIRQ(USBWakeUp_RMP_IRQn);
-  }
-  else
-  #endif
+  } else
+#endif
   {
     NVIC_DisableIRQ(USB_HP_CAN_TX_IRQn);
     NVIC_DisableIRQ(USB_LP_CAN_RX0_IRQn);
@@ -412,14 +395,14 @@ void dcd_int_disable(uint8_t rhport)
   NVIC_DisableIRQ(USBWakeUp_IRQn);
 
 #elif CFG_TUSB_MCU == OPT_MCU_STM32G0
-  #ifdef STM32G0B0xx
-    NVIC_DisableIRQ(USB_IRQn);
-  #else
-    NVIC_DisableIRQ(USB_UCPD1_2_IRQn);
-  #endif
+#ifdef STM32G0B0xx
+  NVIC_DisableIRQ(USB_IRQn);
+#else
+  NVIC_DisableIRQ(USB_UCPD1_2_IRQn);
+#endif
 
 #elif CFG_TUSB_MCU == OPT_MCU_STM32H5
-    NVIC_DisableIRQ(USB_DRD_FS_IRQn);
+  NVIC_DisableIRQ(USB_DRD_FS_IRQn);
 
 #elif CFG_TUSB_MCU == OPT_MCU_STM32WB
   NVIC_DisableIRQ(USB_HP_IRQn);
@@ -429,7 +412,7 @@ void dcd_int_disable(uint8_t rhport)
   NVIC_DisableIRQ(USB_FS_IRQn);
 
 #else
-  #error Unknown arch in USB driver
+#error Unknown arch in USB driver
 #endif
 
   // CMSIS has a membar after disabling interrupts
@@ -438,8 +421,8 @@ void dcd_int_disable(uint8_t rhport)
 // Receive Set Address request, mcu port must also include status IN response
 void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
 {
-  (void) rhport;
-  (void) dev_addr;
+  (void)rhport;
+  (void)dev_addr;
 
   // Respond with status
   dcd_edpt_xfer(rhport, TUSB_DIR_IN_MASK | 0x00, NULL, 0);
@@ -450,44 +433,35 @@ void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
 
 void dcd_remote_wakeup(uint8_t rhport)
 {
-  (void) rhport;
+  (void)rhport;
 
   USB->CNTR |= USB_CNTR_RESUME;
   remoteWakeCountdown = 4u; // required to be 1 to 15 ms, ESOF should trigger every 1ms.
 }
 
-static const tusb_desc_endpoint_t ep0OUT_desc =
-{
-  .bLength          = sizeof(tusb_desc_endpoint_t),
-  .bDescriptorType  = TUSB_DESC_ENDPOINT,
-
-  .bEndpointAddress = 0x00,
-  .bmAttributes     = { .xfer = TUSB_XFER_CONTROL },
-  .wMaxPacketSize   = CFG_TUD_ENDPOINT0_SIZE,
-  .bInterval        = 0
+static const tusb_desc_endpoint_t ep0OUT_desc = {
+    .bLength = sizeof(tusb_desc_endpoint_t),
+    .bDescriptorType = TUSB_DESC_ENDPOINT,
+    .bEndpointAddress = 0x00,
+    .bmAttributes = {.xfer = TUSB_XFER_CONTROL},
+    .wMaxPacketSize = CFG_TUD_ENDPOINT0_SIZE,
+    .bInterval = 0
 };
 
-static const tusb_desc_endpoint_t ep0IN_desc =
-{
-  .bLength          = sizeof(tusb_desc_endpoint_t),
-  .bDescriptorType  = TUSB_DESC_ENDPOINT,
-
-  .bEndpointAddress = 0x80,
-  .bmAttributes     = { .xfer = TUSB_XFER_CONTROL },
-  .wMaxPacketSize   = CFG_TUD_ENDPOINT0_SIZE,
-  .bInterval        = 0
+static const tusb_desc_endpoint_t ep0IN_desc = {
+    .bLength = sizeof(tusb_desc_endpoint_t),
+    .bDescriptorType = TUSB_DESC_ENDPOINT,
+    .bEndpointAddress = 0x80,
+    .bmAttributes = {.xfer = TUSB_XFER_CONTROL},
+    .wMaxPacketSize = CFG_TUD_ENDPOINT0_SIZE,
+    .bInterval = 0
 };
 
 static void dcd_handle_bus_reset(void)
 {
-  //__IO uint16_t * const epreg = &(EPREG(0));
   USB->DADDR = 0u; // disable USB peripheral by clearing the EF flag
 
-  for(uint32_t i=0; i<STFSDEV_EP_COUNT; i++)
-  {
-    // Clear all EPREG (or maybe this is automatic? I'm not sure)
-    pcd_set_endpoint(USB,i,0u);
-
+  for (uint32_t i = 0; i < STFSDEV_EP_COUNT; i++) {
     // Clear EP allocation status
     ep_alloc_status[i].ep_num = 0xFF;
     ep_alloc_status[i].ep_type = 0xFF;
@@ -495,9 +469,11 @@ static void dcd_handle_bus_reset(void)
     ep_alloc_status[i].allocated[1] = false;
   }
 
-  dcd_pma_alloc_reset();
-  dcd_edpt_open (0, &ep0OUT_desc);
-  dcd_edpt_open (0, &ep0IN_desc);
+  // Reset PMA allocation
+  ep_buf_ptr = DCD_STM32_BTABLE_BASE + 8 * MAX_EP_COUNT;
+
+  dcd_edpt_open(0, &ep0OUT_desc);
+  dcd_edpt_open(0, &ep0IN_desc);
 
   USB->DADDR = USB_DADDR_EF; // Set enable flag, and leaving the device address as zero.
 }
@@ -513,30 +489,63 @@ static void dcd_ep_ctr_tx_handler(uint32_t wIstr)
 
   // Verify the CTR_TX bit is set. This was in the ST Micro code,
   // but I'm not sure it's actually necessary?
-  if((wEPRegVal & USB_EP_CTR_TX) == 0U)
-  {
+  if ((wEPRegVal & USB_EP_CTR_TX) == 0U) {
     return;
   }
 
   /* clear int flag */
   pcd_clear_tx_ep_ctr(USB, EPindex);
 
-  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
-  if((xfer->total_len != xfer->queued_len)) /* TX not complete */
-  {
-      dcd_transmit_packet(xfer, EPindex);
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
+
+  if ((wEPRegVal & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS) {
+    // Ignore spurious interrupts that we don't schedule
+    // host can send IN token while there is no data to send, since ISO does not have NAK
+    // this will result to zero length packet --> trigger interrupt (which cannot be masked)
+    if (!xfer->iso_in_sending) {
+      return;
+    }
+    xfer->iso_in_sending = false;
+
+    if (wEPRegVal & USB_EP_DTOG_TX) {
+      pcd_set_ep_tx_dbuf0_cnt(USB, EPindex, 0);
+    } else {
+      pcd_set_ep_tx_dbuf1_cnt(USB, EPindex, 0);
+    }
   }
-  else /* TX Complete */
-  {
+
+  if ((xfer->total_len != xfer->queued_len)) {
+    dcd_transmit_packet(xfer, EPindex);
+  } else {
     dcd_event_xfer_complete(0, ep_addr, xfer->total_len, XFER_RESULT_SUCCESS, true);
   }
 }
 
 // Handle CTR interrupt for the RX/OUT direction
-//
 // Upon call, (wIstr & USB_ISTR_DIR) == 0U
 static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
 {
+#ifdef FSDEV_BUS_32BIT
+  /* https://www.st.com/resource/en/errata_sheet/es0561-stm32h503cbebkbrb-device-errata-stmicroelectronics.pdf
+   * From STM32H503 errata 2.15.1: Buffer description table update completes after CTR interrupt triggers
+   * Description:
+   * - During OUT transfers, the correct transfer interrupt (CTR) is triggered a little before the last USB SRAM accesses
+   * have completed. If the software responds quickly to the interrupt, the full buffer contents may not be correct.
+   * Workaround:
+   * - Software should ensure that a small delay is included before accessing the SRAM contents. This delay
+   * should be 800 ns in Full Speed mode and 6.4 μs in Low Speed mode
+   * - Since H5 can run up to 250Mhz -> 1 cycle = 4ns. Per errata, we need to wait 200 cycles. Though executing code
+   * also takes time, so we'll wait 60 cycles (count = 20).
+   * - Since Low Speed mode is not supported/popular, we will ignore it for now.
+   *
+   * Note: this errata also seems to apply to G0, U5, H5 etc.
+   */
+  volatile uint32_t cycle_count = 20; // defined as PCD_RX_PMA_CNT in stm32 hal_driver
+  while (cycle_count > 0U) {
+    cycle_count--; // each count take 3 cycles (1 for sub, jump, and compare)
+  }
+#endif
+
   uint32_t EPindex = wIstr & USB_ISTR_EP_ID;
   uint32_t wEPRegVal = pcd_get_endpoint(USB, EPindex);
   uint8_t ep_addr = wEPRegVal & USB_EPADDR_FIELD;
@@ -545,96 +554,83 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
 
   // Verify the CTR_RX bit is set. This was in the ST Micro code,
   // but I'm not sure it's actually necessary?
-  if((wEPRegVal & USB_EP_CTR_RX) == 0U)
-  {
+  if ((wEPRegVal & USB_EP_CTR_RX) == 0U) {
     return;
   }
 
-  if((ep_addr == 0U) && ((wEPRegVal & USB_EP_SETUP) != 0U)) /* Setup packet */
-  {
+  if ((ep_addr == 0U) && ((wEPRegVal & USB_EP_SETUP) != 0U)) {
+    /* Setup packet */
     uint32_t count = pcd_get_ep_rx_cnt(USB, EPindex);
-    /* Get SETUP Packet*/
-    if(count == 8) // Setup packet should always be 8 bytes. If not, ignore it, and try again.
-    {
+    // Setup packet should always be 8 bytes. If not, ignore it, and try again.
+    if (count == 8) {
       // Must reset EP to NAK (in case it had been stalling) (though, maybe too late here)
-      pcd_set_ep_rx_status(USB,0u,USB_EP_RX_NAK);
-      pcd_set_ep_tx_status(USB,0u,USB_EP_TX_NAK);
+      pcd_set_ep_rx_status(USB, 0u, USB_EP_RX_NAK);
+      pcd_set_ep_tx_status(USB, 0u, USB_EP_TX_NAK);
 #ifdef FSDEV_BUS_32BIT
-      dcd_event_setup_received(0, (uint8_t*)(USB_PMAADDR + pcd_get_ep_rx_address(USB, EPindex)), true);
+      dcd_event_setup_received(0, (uint8_t *)(USB_PMAADDR + pcd_get_ep_rx_address(USB, EPindex)), true);
 #else
       // The setup_received function uses memcpy, so this must first copy the setup data into
       // user memory, to allow for the 32-bit access that memcpy performs.
       uint8_t userMemBuf[8];
-      dcd_read_packet_memory(userMemBuf, pcd_get_ep_rx_address(USB,EPindex), 8);
-      dcd_event_setup_received(0, (uint8_t*)userMemBuf, true);
+      dcd_read_packet_memory(userMemBuf, pcd_get_ep_rx_address(USB, EPindex), 8);
+      dcd_event_setup_received(0, (uint8_t *)userMemBuf, true);
 #endif
     }
-  }
-  else
-  {
+  } else {
+    // Clear RX CTR interrupt flag
+    if (ep_addr != 0u) {
+      pcd_clear_rx_ep_ctr(USB, EPindex);
+    }
+
     uint32_t count;
+    uint16_t addr;
     /* Read from correct register when ISOCHRONOUS (double buffered) */
-    if ( (wEPRegVal & USB_EP_DTOG_RX) && ( (wEPRegVal & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS) ) {
-      count = pcd_get_ep_tx_cnt(USB, EPindex);
+    if ((wEPRegVal & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS) {
+      if (wEPRegVal & USB_EP_DTOG_RX) {
+        count = pcd_get_ep_dbuf0_cnt(USB, EPindex);
+        addr = pcd_get_ep_dbuf0_address(USB, EPindex);
+      } else {
+        count = pcd_get_ep_dbuf1_cnt(USB, EPindex);
+        addr = pcd_get_ep_dbuf1_address(USB, EPindex);
+      }
     } else {
       count = pcd_get_ep_rx_cnt(USB, EPindex);
+      addr = pcd_get_ep_rx_address(USB, EPindex);
     }
 
     TU_ASSERT(count <= xfer->max_packet_size, /**/);
 
-    // Clear RX CTR interrupt flag
-    if(ep_addr != 0u)
-    {
-      pcd_clear_rx_ep_ctr(USB, EPindex);
-    }
-
-    if (count != 0U)
-    {
-      uint16_t addr = pcd_get_ep_rx_address(USB, EPindex);
-
-      if (xfer->ff)
-      {
+    if (count != 0U) {
+      if (xfer->ff) {
         dcd_read_packet_memory_ff(xfer->ff, addr, count);
-      }
-      else
-      {
+      } else {
         dcd_read_packet_memory(&(xfer->buffer[xfer->queued_len]), addr, count);
       }
 
       xfer->queued_len = (uint16_t)(xfer->queued_len + count);
     }
 
-    if ((count < xfer->max_packet_size) || (xfer->queued_len == xfer->total_len))
-    {
-      /* RX COMPLETE */
+    if ((count < xfer->max_packet_size) || (xfer->queued_len == xfer->total_len)) {
+      // all bytes received or short packet
       dcd_event_xfer_complete(0, ep_addr, xfer->queued_len, XFER_RESULT_SUCCESS, true);
-      // Though the host could still send, we don't know.
-      // Does the bulk pipe need to be reset to valid to allow for a ZLP?
-    }
-    else
-    {
-      uint32_t remaining = (uint32_t)xfer->total_len - (uint32_t)xfer->queued_len;
-      if(remaining >= xfer->max_packet_size) {
-        pcd_set_ep_rx_bufsize(USB, EPindex,xfer->max_packet_size);
-      } else {
-        pcd_set_ep_rx_bufsize(USB, EPindex,remaining);
-      }
-
-      if (!((wEPRegVal & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS)) {
-        /* Set endpoint active again for receiving more data.
-         * Note that isochronous endpoints stay active always */
-        pcd_set_ep_rx_status(USB, EPindex, USB_EP_RX_VALID);
+    } else {
+      /* Set endpoint active again for receiving more data.
+       * Note that isochronous endpoints stay active always */
+      if ((wEPRegVal & USB_EP_TYPE_MASK) != USB_EP_ISOCHRONOUS) {
+        uint16_t remaining = xfer->total_len - xfer->queued_len;
+        uint16_t cnt = tu_min16(remaining, xfer->max_packet_size);
+        pcd_set_ep_rx_cnt(USB, EPindex, cnt);
       }
+      pcd_set_ep_rx_status(USB, EPindex, USB_EP_RX_VALID);
     }
   }
 
   // For EP0, prepare to receive another SETUP packet.
   // Clear CTR last so that a new packet does not overwrite the packing being read.
   // (Based on the docs, it seems SETUP will always be accepted after CTR is cleared)
-  if(ep_addr == 0u)
-  {
-      // Always be prepared for a status packet...
-    pcd_set_ep_rx_bufsize(USB, EPindex, CFG_TUD_ENDPOINT0_SIZE);
+  if (ep_addr == 0u) {
+    // Always be prepared for a status packet...
+    pcd_set_ep_rx_cnt(USB, EPindex, CFG_TUD_ENDPOINT0_SIZE);
     pcd_clear_rx_ep_ctr(USB, EPindex);
   }
 }
@@ -644,40 +640,38 @@ static void dcd_ep_ctr_handler(void)
   uint32_t wIstr;
 
   /* stay in loop while pending interrupts */
-  while (((wIstr = USB->ISTR) & USB_ISTR_CTR) != 0U)
-  {
-
-    if ((wIstr & USB_ISTR_DIR) == 0U) /* TX/IN */
-    {
+  while (((wIstr = USB->ISTR) & USB_ISTR_CTR) != 0U) {
+    if ((wIstr & USB_ISTR_DIR) == 0U) {
+      /* TX/IN */
       dcd_ep_ctr_tx_handler(wIstr);
-    }
-    else /* RX/OUT*/
-    {
+    } else {
+      /* RX/OUT*/
       dcd_ep_ctr_rx_handler(wIstr);
     }
   }
 }
 
-void dcd_int_handler(uint8_t rhport) {
+void dcd_int_handler(uint8_t rhport)
+{
 
-  (void) rhport;
+  (void)rhport;
 
   uint32_t int_status = USB->ISTR;
-  //const uint32_t handled_ints = USB_ISTR_CTR | USB_ISTR_RESET | USB_ISTR_WKUP
-  //    | USB_ISTR_SUSP | USB_ISTR_SOF | USB_ISTR_ESOF;
-  // unused IRQs: (USB_ISTR_PMAOVR | USB_ISTR_ERR | USB_ISTR_L1REQ )
+  // const uint32_t handled_ints = USB_ISTR_CTR | USB_ISTR_RESET | USB_ISTR_WKUP
+  //     | USB_ISTR_SUSP | USB_ISTR_SOF | USB_ISTR_ESOF;
+  //  unused IRQs: (USB_ISTR_PMAOVR | USB_ISTR_ERR | USB_ISTR_L1REQ )
 
   // The ST driver loops here on the CTR bit, but that loop has been moved into the
   // dcd_ep_ctr_handler(), so less need to loop here. The other interrupts shouldn't
   // be triggered repeatedly.
 
   /* Put SOF flag at the beginning of ISR in case to get least amount of jitter if it is used for timing purposes */
-  if(int_status & USB_ISTR_SOF) {
+  if (int_status & USB_ISTR_SOF) {
     USB->ISTR = (fsdev_bus_t)~USB_ISTR_SOF;
     dcd_event_sof(0, USB->FNR & USB_FNR_FN, true);
   }
 
-  if(int_status & USB_ISTR_RESET) {
+  if (int_status & USB_ISTR_RESET) {
     // USBRST is start of reset.
     USB->ISTR = (fsdev_bus_t)~USB_ISTR_RESET;
     dcd_handle_bus_reset();
@@ -685,15 +679,13 @@ void dcd_int_handler(uint8_t rhport) {
     return; // Don't do the rest of the things here; perhaps they've been cleared?
   }
 
-  if (int_status & USB_ISTR_CTR)
-  {
+  if (int_status & USB_ISTR_CTR) {
     /* servicing of the endpoint correct transfer interrupt */
     /* clear of the CTR flag into the sub */
     dcd_ep_ctr_handler();
   }
 
-  if (int_status & USB_ISTR_WKUP)
-  {
+  if (int_status & USB_ISTR_WKUP) {
     USB->CNTR &= ~USB_CNTR_LPMODE;
     USB->CNTR &= ~USB_CNTR_FSUSP;
 
@@ -701,8 +693,7 @@ void dcd_int_handler(uint8_t rhport) {
     dcd_event_bus_signal(0, DCD_EVENT_RESUME, true);
   }
 
-  if (int_status & USB_ISTR_SUSP)
-  {
+  if (int_status & USB_ISTR_SUSP) {
     /* Suspend is asserted for both suspend and unplug events. without Vbus monitoring,
      * these events cannot be differentiated, so we only trigger suspend. */
 
@@ -715,13 +706,11 @@ void dcd_int_handler(uint8_t rhport) {
     dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
   }
 
-  if(int_status & USB_ISTR_ESOF) {
-    if(remoteWakeCountdown == 1u)
-    {
+  if (int_status & USB_ISTR_ESOF) {
+    if (remoteWakeCountdown == 1u) {
       USB->CNTR &= ~USB_CNTR_RESUME;
     }
-    if(remoteWakeCountdown > 0u)
-    {
+    if (remoteWakeCountdown > 0u) {
       remoteWakeCountdown--;
     }
     USB->ISTR = (fsdev_bus_t)~USB_ISTR_ESOF;
@@ -734,15 +723,14 @@ void dcd_int_handler(uint8_t rhport) {
 
 // Invoked when a control transfer's status stage is complete.
 // May help DCD to prepare for next control transfer, this API is optional.
-void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const * request)
+void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const *request)
 {
-  (void) rhport;
+  (void)rhport;
 
   if (request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_DEVICE &&
       request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD &&
-      request->bRequest == TUSB_REQ_SET_ADDRESS )
-  {
-    uint8_t const dev_addr = (uint8_t) request->wValue;
+      request->bRequest == TUSB_REQ_SET_ADDRESS) {
+    uint8_t const dev_addr = (uint8_t)request->wValue;
 
     // Setting new address after the whole request is complete
     USB->DADDR &= ~USB_DADDR_ADD;
@@ -750,41 +738,13 @@ void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const * re
   }
 }
 
-static void dcd_pma_alloc_reset(void)
-{
-  open_ep_count = 0;
-  ep_buf_ptr = DCD_STM32_BTABLE_BASE + 8*MAX_EP_COUNT; // 8 bytes per endpoint (two TX and two RX words, each)
-  //TU_LOG2("dcd_pma_alloc_reset()\r\n");
-  for(uint32_t i=0; i<MAX_EP_COUNT; i++)
-  {
-    xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_OUT))->pma_alloc_size = 0U;
-    xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_IN))->pma_alloc_size = 0U;
-    xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_OUT))->pma_ptr = 0U;
-    xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_IN))->pma_ptr = 0U;
-  }
-}
-
 /***
  * Allocate a section of PMA
- *
- * If the EP number has already been allocated, and the new allocation
- * is larger than the old allocation, then this will fail with a TU_ASSERT.
- * (This is done to simplify the code. More complicated algorithms could be used)
- *
+ * In case of double buffering, high 16bit is the address of 2nd buffer
  * During failure, TU_ASSERT is used. If this happens, rework/reallocate memory manually.
  */
-static uint16_t dcd_pma_alloc(uint8_t ep_addr, uint16_t length)
+static uint32_t dcd_pma_alloc(uint16_t length, bool dbuf)
 {
-  xfer_ctl_t* epXferCtl = xfer_ctl_ptr(ep_addr);
-
-  if(epXferCtl->pma_alloc_size != 0U)
-  {
-    //TU_LOG2("dcd_pma_alloc(%x,%x)=%x (cached)\r\n",ep_addr,length,epXferCtl->pma_ptr);
-    // Previously allocated
-    TU_ASSERT(length <= epXferCtl->pma_alloc_size, 0xFFFF);  // Verify no larger than previous alloc
-    return epXferCtl->pma_ptr;
-  }
-
   // Ensure allocated buffer is aligned
 #ifdef FSDEV_BUS_32BIT
   length = (length + 3) & ~0x03;
@@ -792,79 +752,44 @@ static uint16_t dcd_pma_alloc(uint8_t ep_addr, uint16_t length)
   length = (length + 1) & ~0x01;
 #endif
 
-  open_ep_count++;
-
-  uint16_t addr = ep_buf_ptr;
+  uint32_t addr = ep_buf_ptr;
   ep_buf_ptr = (uint16_t)(ep_buf_ptr + length); // increment buffer pointer
 
-  // Verify no overflow
-  TU_ASSERT(ep_buf_ptr <= FSDEV_PMA_SIZE, 0xFFFF);
+  if (dbuf) {
+    addr |= ((uint32_t)ep_buf_ptr) << 16;
+    ep_buf_ptr = (uint16_t)(ep_buf_ptr + length); // increment buffer pointer
+  }
 
-  epXferCtl->pma_ptr = addr;
-  epXferCtl->pma_alloc_size = length;
-  //TU_LOG1("dcd_pma_alloc(%x,%x)=%x\r\n",ep_addr,length,addr);
+  // Verify packet buffer is not overflowed
+  TU_ASSERT(ep_buf_ptr <= FSDEV_PMA_SIZE, 0xFFFF);
 
   return addr;
 }
 
 /***
- * Free a block of PMA space
- */
-static void dcd_pma_free(uint8_t ep_addr)
-{
-  // Presently, this should never be called for EP0 IN/OUT
-  TU_ASSERT(open_ep_count > 2, /**/);
-  TU_ASSERT(xfer_ctl_ptr(ep_addr)->max_packet_size != 0, /**/);
-  open_ep_count--;
-
-  // If count is 2, only EP0 should be open, so allocations can be mostly reset.
-
-  if(open_ep_count == 2)
-  {
-    ep_buf_ptr = DCD_STM32_BTABLE_BASE + 8*MAX_EP_COUNT + 2*CFG_TUD_ENDPOINT0_SIZE; // 8 bytes per endpoint (two TX and two RX words, each), and EP0
-
-    // Skip EP0
-    for(uint32_t i=1; i<MAX_EP_COUNT; i++)
-    {
-      xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_OUT))->pma_alloc_size = 0U;
-      xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_IN))->pma_alloc_size = 0U;
-      xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_OUT))->pma_ptr = 0U;
-      xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_IN))->pma_ptr = 0U;
-    }
-  }
-}
-
-/***
  * Allocate hardware endpoint
  */
 static uint8_t dcd_ep_alloc(uint8_t ep_addr, uint8_t ep_type)
 {
   uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
-  for(uint8_t i = 0; i < STFSDEV_EP_COUNT; i++)
-  {
+  for (uint8_t i = 0; i < STFSDEV_EP_COUNT; i++) {
     // Check if already allocated
-    if(ep_alloc_status[i].allocated[dir] &&
-       ep_alloc_status[i].ep_type == ep_type &&
-       ep_alloc_status[i].ep_num == epnum)
-    {
+    if (ep_alloc_status[i].allocated[dir] &&
+        ep_alloc_status[i].ep_type == ep_type &&
+        ep_alloc_status[i].ep_num == epnum) {
       return i;
     }
 
     // If EP of current direction is not allocated
     // Except for ISO endpoint, both direction should be free
-    if(!ep_alloc_status[i].allocated[dir] &&
-       (ep_type != TUSB_XFER_ISOCHRONOUS || !ep_alloc_status[i].allocated[dir ^ 1]))
-    {
+    if (!ep_alloc_status[i].allocated[dir] &&
+        (ep_type != TUSB_XFER_ISOCHRONOUS || !ep_alloc_status[i].allocated[dir ^ 1])) {
       // Check if EP number is the same
-      if(ep_alloc_status[i].ep_num == 0xFF ||
-         ep_alloc_status[i].ep_num == epnum)
-      {
+      if (ep_alloc_status[i].ep_num == 0xFF || ep_alloc_status[i].ep_num == epnum) {
         // One EP pair has to be the same type
-        if(ep_alloc_status[i].ep_type == 0xFF ||
-           ep_alloc_status[i].ep_type == ep_type)
-        {
+        if (ep_alloc_status[i].ep_type == 0xFF || ep_alloc_status[i].ep_type == ep_type) {
           ep_alloc_status[i].ep_num = epnum;
           ep_alloc_status[i].ep_type = ep_type;
           ep_alloc_status[i].allocated[dir] = true;
@@ -879,121 +804,79 @@ static uint8_t dcd_ep_alloc(uint8_t ep_addr, uint8_t ep_type)
   TU_ASSERT(0);
 }
 
-/***
- * Free hardware endpoint
- */
-static void dcd_ep_free(uint8_t ep_addr)
-{
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
-
-  for(uint8_t i = 0; i < STFSDEV_EP_COUNT; i++)
-  {
-    // Check if EP number & dir are the same
-    if(ep_alloc_status[i].ep_num == epnum &&
-       ep_alloc_status[i].allocated[dir] == dir)
-    {
-      ep_alloc_status[i].allocated[dir] = false;
-      // Reset entry if ISO endpoint or both direction are free
-      if(ep_alloc_status[i].ep_type == TUSB_XFER_ISOCHRONOUS ||
-         !ep_alloc_status[i].allocated[dir ^ 1])
-      {
-        ep_alloc_status[i].ep_num = 0xFF;
-        ep_alloc_status[i].ep_type = 0xFF;
-
-        return;
-      }
-    }
-  }
-}
-
 // The STM32F0 doesn't seem to like |= or &= to manipulate the EP#R registers,
 // so I'm using the #define from HAL here, instead.
 
-bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc)
+bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *p_endpoint_desc)
 {
   (void)rhport;
-  uint8_t const ep_idx = dcd_ep_alloc(p_endpoint_desc->bEndpointAddress, p_endpoint_desc->bmAttributes.xfer);
-  uint8_t const dir   = tu_edpt_dir(p_endpoint_desc->bEndpointAddress);
+  uint8_t const ep_addr = p_endpoint_desc->bEndpointAddress;
+  uint8_t const ep_idx = dcd_ep_alloc(ep_addr, p_endpoint_desc->bmAttributes.xfer);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
   const uint16_t packet_size = tu_edpt_packet_size(p_endpoint_desc);
   const uint16_t buffer_size = pcd_aligned_buffer_size(packet_size);
   uint16_t pma_addr;
   uint32_t wType;
 
   TU_ASSERT(ep_idx < STFSDEV_EP_COUNT);
-  TU_ASSERT(buffer_size <= 1024);
+  TU_ASSERT(buffer_size <= 64);
 
   // Set type
-  switch(p_endpoint_desc->bmAttributes.xfer) {
-  case TUSB_XFER_CONTROL:
-    wType = USB_EP_CONTROL;
-    break;
-  case TUSB_XFER_ISOCHRONOUS:
-    wType = USB_EP_ISOCHRONOUS;
-    break;
-  case TUSB_XFER_BULK:
-    wType = USB_EP_CONTROL;
-    break;
+  switch (p_endpoint_desc->bmAttributes.xfer) {
+    case TUSB_XFER_CONTROL:
+      wType = USB_EP_CONTROL;
+      break;
+    case TUSB_XFER_BULK:
+      wType = USB_EP_CONTROL;
+      break;
 
-  case TUSB_XFER_INTERRUPT:
-    wType = USB_EP_INTERRUPT;
-    break;
+    case TUSB_XFER_INTERRUPT:
+      wType = USB_EP_INTERRUPT;
+      break;
 
-  default:
-    TU_ASSERT(false);
+    default:
+      // Note: ISO endpoint should use alloc / active functions
+      TU_ASSERT(false);
   }
 
   pcd_set_eptype(USB, ep_idx, wType);
-  pcd_set_ep_address(USB, ep_idx, tu_edpt_number(p_endpoint_desc->bEndpointAddress));
-  // Be normal, for now, instead of only accepting zero-byte packets (on control endpoint)
-  // or being double-buffered (bulk endpoints)
-  pcd_clear_ep_kind(USB,0);
+  pcd_set_ep_address(USB, ep_idx, tu_edpt_number(ep_addr));
 
-  /* Create a packet memory buffer area. For isochronous endpoints,
-   * use the same buffer as the double buffer, essentially disabling double buffering */
-  pma_addr = dcd_pma_alloc(p_endpoint_desc->bEndpointAddress, buffer_size);
+  /* Create a packet memory buffer area. */
+  pma_addr = dcd_pma_alloc(buffer_size, false);
 
-  if( (dir == TUSB_DIR_IN) || (wType == USB_EP_ISOCHRONOUS) )
-  {
+  if (dir == TUSB_DIR_IN) {
     pcd_set_ep_tx_address(USB, ep_idx, pma_addr);
-    pcd_set_ep_tx_bufsize(USB, ep_idx, buffer_size);
+    pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_NAK);
     pcd_clear_tx_dtog(USB, ep_idx);
-  }
-
-  if( (dir == TUSB_DIR_OUT) || (wType == USB_EP_ISOCHRONOUS) )
-  {
+  } else {
     pcd_set_ep_rx_address(USB, ep_idx, pma_addr);
-    pcd_set_ep_rx_bufsize(USB, ep_idx, buffer_size);
+    pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_NAK);
     pcd_clear_rx_dtog(USB, ep_idx);
   }
 
-  /* Enable endpoint */
-  if (dir == TUSB_DIR_IN)
-  {
-    if(wType == USB_EP_ISOCHRONOUS) {
-      pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_DIS);
-    } else {
-      pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_NAK);
-    }
-  } else
-  {
-    if(wType == USB_EP_ISOCHRONOUS) {
-      pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_DIS);
-    } else {
-      pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_NAK);
-    }
-  }
-
-  xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->max_packet_size = packet_size;
-  xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->ep_idx = ep_idx;
+  xfer_ctl_ptr(ep_addr)->max_packet_size = packet_size;
+  xfer_ctl_ptr(ep_addr)->ep_idx = ep_idx;
 
   return true;
 }
 
-void dcd_edpt_close_all (uint8_t rhport)
+void dcd_edpt_close_all(uint8_t rhport)
 {
-  (void) rhport;
-  // TODO implement dcd_edpt_close_all()
+  (void)rhport;
+
+  for (uint32_t i = 1; i < STFSDEV_EP_COUNT; i++) {
+    // Reset endpoint
+    pcd_set_endpoint(USB, i, 0);
+    // Clear EP allocation status
+    ep_alloc_status[i].ep_num = 0xFF;
+    ep_alloc_status[i].ep_type = 0xFF;
+    ep_alloc_status[i].allocated[0] = false;
+    ep_alloc_status[i].allocated[1] = false;
+  }
+
+  // Reset PMA allocation
+  ep_buf_ptr = DCD_STM32_BTABLE_BASE + 8 * MAX_EP_COUNT + 2 * CFG_TUD_ENDPOINT0_SIZE;
 }
 
 /**
@@ -1003,223 +886,203 @@ void dcd_edpt_close_all (uint8_t rhport)
  *
  * This also clears transfers in progress, should there be any.
  */
-void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr)
+void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr)
 {
   (void)rhport;
 
-  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
   uint8_t const ep_idx = xfer->ep_idx;
-  uint8_t const dir    = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
-  if(dir == TUSB_DIR_IN)
-  {
+  if (dir == TUSB_DIR_IN) {
     pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_DIS);
-  }
-  else
-  {
+  } else {
     pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_DIS);
   }
-
-  dcd_ep_free(ep_addr);
-
-  dcd_pma_free(ep_addr);
 }
 
 bool dcd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size)
 {
   (void)rhport;
 
-  TU_ASSERT(largest_packet_size <= 1024);
-
   uint8_t const ep_idx = dcd_ep_alloc(ep_addr, TUSB_XFER_ISOCHRONOUS);
   const uint16_t buffer_size = pcd_aligned_buffer_size(largest_packet_size);
 
-  /* Create a packet memory buffer area. For isochronous endpoints,
-   * use the same buffer as the double buffer, essentially disabling double buffering */
-  uint16_t pma_addr = dcd_pma_alloc(ep_addr, buffer_size);
-
-  xfer_ctl_ptr(ep_addr)->ep_idx = ep_idx;
+  /* Create a packet memory buffer area. Enable double buffering for devices with 2048 bytes PMA,
+     for smaller devices double buffering occupy too much space. */
+#if FSDEV_PMA_SIZE > 1024u
+  uint32_t pma_addr = dcd_pma_alloc(buffer_size, true);
+  uint16_t pma_addr2 = pma_addr >> 16;
+#else
+  uint32_t pma_addr = dcd_pma_alloc(buffer_size, true);
+  uint16_t pma_addr2 = pma_addr;
+#endif
+  pcd_set_ep_tx_address(USB, ep_idx, pma_addr);
+  pcd_set_ep_rx_address(USB, ep_idx, pma_addr2);
 
   pcd_set_eptype(USB, ep_idx, USB_EP_ISOCHRONOUS);
 
-  pcd_set_ep_tx_address(USB, ep_idx, pma_addr);
-  pcd_set_ep_rx_address(USB, ep_idx, pma_addr);
+  xfer_ctl_ptr(ep_addr)->ep_idx = ep_idx;
 
   return true;
 }
 
-bool dcd_edpt_iso_activate(uint8_t rhport,  tusb_desc_endpoint_t const * p_endpoint_desc)
+bool dcd_edpt_iso_activate(uint8_t rhport, tusb_desc_endpoint_t const *p_endpoint_desc)
 {
   (void)rhport;
-  uint8_t const ep_idx = xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->ep_idx;
-  uint8_t const dir    = tu_edpt_dir(p_endpoint_desc->bEndpointAddress);
+  uint8_t const ep_addr = p_endpoint_desc->bEndpointAddress;
+  uint8_t const ep_idx = xfer_ctl_ptr(ep_addr)->ep_idx;
+  uint8_t const dir = tu_edpt_dir(ep_addr);
   const uint16_t packet_size = tu_edpt_packet_size(p_endpoint_desc);
-  const uint16_t buffer_size = pcd_aligned_buffer_size(packet_size);
 
-  /* Disable endpoint */
-  if(dir == TUSB_DIR_IN)
-  {
-    pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_DIS);
-  }
-  else
-  {
-    pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_DIS);
-  }
+  pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_DIS);
+  pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_DIS);
 
-  pcd_set_ep_address(USB, ep_idx, tu_edpt_number(p_endpoint_desc->bEndpointAddress));
-  // Be normal, for now, instead of only accepting zero-byte packets (on control endpoint)
-  // or being double-buffered (bulk endpoints)
-  pcd_clear_ep_kind(USB,0);
+  pcd_set_ep_address(USB, ep_idx, tu_edpt_number(ep_addr));
 
-  pcd_set_ep_tx_bufsize(USB, ep_idx, buffer_size);
-  pcd_set_ep_rx_bufsize(USB, ep_idx, buffer_size);
   pcd_clear_tx_dtog(USB, ep_idx);
   pcd_clear_rx_dtog(USB, ep_idx);
 
-  xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->max_packet_size = packet_size;
+  if (dir == TUSB_DIR_IN) {
+    pcd_rx_dtog(USB, ep_idx);
+  } else {
+    pcd_tx_dtog(USB, ep_idx);
+  }
+
+  xfer_ctl_ptr(ep_addr)->max_packet_size = packet_size;
 
   return true;
 }
 
 // Currently, single-buffered, and only 64 bytes at a time (max)
 
-static void dcd_transmit_packet(xfer_ctl_t * xfer, uint16_t ep_ix)
+static void dcd_transmit_packet(xfer_ctl_t *xfer, uint16_t ep_ix)
 {
   uint16_t len = (uint16_t)(xfer->total_len - xfer->queued_len);
-
-  if(len > xfer->max_packet_size) // max packet size for FS transfer
-  {
+  if (len > xfer->max_packet_size) {
     len = xfer->max_packet_size;
   }
 
   uint16_t ep_reg = pcd_get_endpoint(USB, ep_ix);
-  uint16_t addr_ptr = pcd_get_ep_tx_address(USB, ep_ix);
+  bool const is_iso = (ep_reg & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS;
+  uint16_t addr_ptr;
 
-  if (xfer->ff)
-  {
-    dcd_write_packet_memory_ff(xfer->ff, addr_ptr, len);
-  }
-  else
-  {
-    dcd_write_packet_memory(addr_ptr, &(xfer->buffer[xfer->queued_len]), len);
+  if (is_iso) {
+    if (ep_reg & USB_EP_DTOG_TX) {
+      addr_ptr = pcd_get_ep_dbuf1_address(USB, ep_ix);
+      pcd_set_ep_tx_dbuf1_cnt(USB, ep_ix, len);
+    } else {
+      addr_ptr = pcd_get_ep_dbuf0_address(USB, ep_ix);
+      pcd_set_ep_tx_dbuf0_cnt(USB, ep_ix, len);
+    }
+  } else {
+    addr_ptr = pcd_get_ep_tx_address(USB, ep_ix);
+    pcd_set_ep_tx_cnt(USB, ep_ix, len);
   }
-  xfer->queued_len = (uint16_t)(xfer->queued_len + len);
 
-  /* Write into correct register when ISOCHRONOUS (double buffered) */
-  if ( (ep_reg & USB_EP_DTOG_TX) && ( (ep_reg & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS) ) {
-    pcd_set_ep_rx_cnt(USB, ep_ix, len);
+  if (xfer->ff) {
+    dcd_write_packet_memory_ff(xfer->ff, addr_ptr, len);
   } else {
-    pcd_set_ep_tx_cnt(USB, ep_ix, len);
+    dcd_write_packet_memory(addr_ptr, &(xfer->buffer[xfer->queued_len]), len);
   }
+  xfer->queued_len = (uint16_t)(xfer->queued_len + len);
 
+  dcd_int_disable(0);
   pcd_set_ep_tx_status(USB, ep_ix, USB_EP_TX_VALID);
+  if (is_iso) {
+    xfer->iso_in_sending = true;
+  }
+  dcd_int_enable(0);
 }
 
-bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
+static bool edpt_xfer(uint8_t rhport, uint8_t ep_addr)
 {
-  (void) rhport;
+  (void)rhport;
 
-  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
   uint8_t const ep_idx = xfer->ep_idx;
-  uint8_t const dir    = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
-  xfer->buffer = buffer;
-  xfer->ff     = NULL;
-  xfer->total_len = total_bytes;
-  xfer->queued_len = 0;
-
-  if ( dir == TUSB_DIR_OUT )
-  {
+  if (dir == TUSB_DIR_IN) {
+    dcd_transmit_packet(xfer, ep_idx);
+  } else {
     // A setup token can occur immediately after an OUT STATUS packet so make sure we have a valid
     // buffer for the control endpoint.
-    if (ep_idx == 0 && buffer == NULL)
-    {
-        xfer->buffer = (uint8_t*)_setup_packet;
+    if (ep_idx == 0 && xfer->buffer == NULL) {
+      xfer->buffer = (uint8_t *)_setup_packet;
     }
 
-    if(total_bytes > xfer->max_packet_size)
-    {
-      pcd_set_ep_rx_bufsize(USB,ep_idx,xfer->max_packet_size);
+    uint32_t cnt = (uint32_t ) tu_min16(xfer->total_len, xfer->max_packet_size);
+    uint16_t ep_reg = pcd_get_endpoint(USB, ep_idx);
+
+    if ((ep_reg & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS) {
+      pcd_set_ep_rx_dbuf0_cnt(USB, ep_idx, cnt);
+      pcd_set_ep_rx_dbuf1_cnt(USB, ep_idx, cnt);
     } else {
-      pcd_set_ep_rx_bufsize(USB,ep_idx,total_bytes);
+      pcd_set_ep_rx_cnt(USB, ep_idx, cnt);
     }
+
     pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_VALID);
   }
-  else // IN
-  {
-    dcd_transmit_packet(xfer,ep_idx);
-  }
+
   return true;
 }
 
-bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16_t total_bytes)
+bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t *buffer, uint16_t total_bytes)
 {
-  (void) rhport;
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
 
-  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
-  uint8_t const epnum = xfer->ep_idx;
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+  xfer->buffer = buffer;
+  xfer->ff = NULL;
+  xfer->total_len = total_bytes;
+  xfer->queued_len = 0;
 
+  return edpt_xfer(rhport, ep_addr);
+}
+
+bool dcd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t *ff, uint16_t total_bytes)
+{
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
   xfer->buffer = NULL;
-  xfer->ff     = ff;
+  xfer->ff = ff;
   xfer->total_len = total_bytes;
   xfer->queued_len = 0;
 
-  if ( dir == TUSB_DIR_OUT )
-  {
-    if(total_bytes > xfer->max_packet_size)
-    {
-      pcd_set_ep_rx_bufsize(USB,epnum,xfer->max_packet_size);
-    } else {
-      pcd_set_ep_rx_bufsize(USB,epnum,total_bytes);
-    }
-    pcd_set_ep_rx_status(USB, epnum, USB_EP_RX_VALID);
-  }
-  else // IN
-  {
-    dcd_transmit_packet(xfer,epnum);
-  }
-  return true;
+  return edpt_xfer(rhport, ep_addr);
 }
 
-void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
+void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
 {
   (void)rhport;
 
-  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
   uint8_t const ep_idx = xfer->ep_idx;
-  uint8_t const dir    = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
-  if (dir == TUSB_DIR_IN)
-  { // IN
+  if (dir == TUSB_DIR_IN) {
     pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_STALL);
-  }
-  else
-  { // OUT
+  } else {
     pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_STALL);
   }
 }
 
-void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
+void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
 {
   (void)rhport;
 
-  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
   uint8_t const ep_idx = xfer->ep_idx;
-  uint8_t const dir    = tu_edpt_dir(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
-  if (dir == TUSB_DIR_IN)
-  { // IN
-    if (pcd_get_eptype(USB, ep_idx) !=  USB_EP_ISOCHRONOUS) {
+  if (dir == TUSB_DIR_IN) { // IN
+    if (pcd_get_eptype(USB, ep_idx) != USB_EP_ISOCHRONOUS) {
       pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_NAK);
     }
 
     /* Reset to DATA0 if clearing stall condition. */
     pcd_clear_tx_dtog(USB, ep_idx);
-  }
-  else
-  { // OUT
-    if (pcd_get_eptype(USB, ep_idx) !=  USB_EP_ISOCHRONOUS) {
+  } else { // OUT
+    if (pcd_get_eptype(USB, ep_idx) != USB_EP_ISOCHRONOUS) {
       pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_NAK);
     }
     /* Reset to DATA0 if clearing stall condition. */
@@ -1230,8 +1093,8 @@ void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
 #ifdef FSDEV_BUS_32BIT
 static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t wNBytes)
 {
-  const uint8_t* srcVal = src;
-  volatile uint32_t* dst32 = (volatile uint32_t*)(USB_PMAADDR + dst);
+  const uint8_t *srcVal = src;
+  volatile uint32_t *dst32 = (volatile uint32_t *)(USB_PMAADDR + dst);
 
   for (uint32_t n = wNBytes / 4; n > 0; --n) {
     *dst32++ = tu_unaligned_read32(srcVal);
@@ -1239,18 +1102,15 @@ static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, ui
   }
 
   wNBytes = wNBytes & 0x03;
-  if (wNBytes)
-  {
+  if (wNBytes) {
     uint32_t wrVal = *srcVal;
     wNBytes--;
 
-    if (wNBytes)
-    {
+    if (wNBytes) {
       wrVal |= *++srcVal << 8;
       wNBytes--;
 
-      if (wNBytes)
-      {
+      if (wNBytes) {
         wrVal |= *++srcVal << 16;
       }
     }
@@ -1263,20 +1123,20 @@ static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, ui
 #else
 // Packet buffer access can only be 8- or 16-bit.
 /**
-  * @brief Copy a buffer from user memory area to packet memory area (PMA).
-  *        This uses byte-access for user memory (so support non-aligned buffers)
-  *        and 16-bit access for packet memory.
-  * @param   dst, byte address in PMA; must be 16-bit aligned
-  * @param   src pointer to user memory area.
-  * @param   wPMABufAddr address into PMA.
-  * @param   wNBytes no. of bytes to be copied.
-  * @retval None
-  */
+ * @brief Copy a buffer from user memory area to packet memory area (PMA).
+ *        This uses byte-access for user memory (so support non-aligned buffers)
+ *        and 16-bit access for packet memory.
+ * @param   dst, byte address in PMA; must be 16-bit aligned
+ * @param   src pointer to user memory area.
+ * @param   wPMABufAddr address into PMA.
+ * @param   wNBytes no. of bytes to be copied.
+ * @retval None
+ */
 static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t wNBytes)
 {
   uint32_t n = (uint32_t)wNBytes >> 1U;
   uint16_t temp1, temp2;
-  const uint8_t * srcVal;
+  const uint8_t *srcVal;
 
   // The GCC optimizer will combine access to 32-bit sizes if we let it. Force
   // it volatile so that it won't do that.
@@ -1285,18 +1145,16 @@ static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, ui
   srcVal = src;
   pdwVal = &pma[FSDEV_PMA_STRIDE * (dst >> 1)];
 
-  while (n--)
-  {
+  while (n--) {
     temp1 = (uint16_t)*srcVal;
     srcVal++;
-    temp2 = temp1 | ((uint16_t)(((uint16_t)(*srcVal)) << 8U)) ;
+    temp2 = temp1 | ((uint16_t)(((uint16_t)(*srcVal)) << 8U));
     *pdwVal = temp2;
     pdwVal += FSDEV_PMA_STRIDE;
     srcVal++;
   }
 
-  if (wNBytes)
-  {
+  if (wNBytes) {
     temp1 = *srcVal;
     *pdwVal = temp1;
   }
@@ -1306,42 +1164,39 @@ static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, ui
 #endif
 
 /**
-  * @brief Copy from FIFO to packet memory area (PMA).
-  *        Uses byte-access of system memory and 16-bit access of packet memory
-  * @param   wNBytes no. of bytes to be copied.
-  * @retval None
-  */
-static bool dcd_write_packet_memory_ff(tu_fifo_t * ff, uint16_t dst, uint16_t wNBytes)
+ * @brief Copy from FIFO to packet memory area (PMA).
+ *        Uses byte-access of system memory and 16-bit access of packet memory
+ * @param   wNBytes no. of bytes to be copied.
+ * @retval None
+ */
+static bool dcd_write_packet_memory_ff(tu_fifo_t *ff, uint16_t dst, uint16_t wNBytes)
 {
   // Since we copy from a ring buffer FIFO, a wrap might occur making it necessary to conduct two copies
   tu_fifo_buffer_info_t info;
   tu_fifo_get_read_info(ff, &info);
 
-  uint16_t cnt_lin =  TU_MIN(wNBytes, info.len_lin);
+  uint16_t cnt_lin = TU_MIN(wNBytes, info.len_lin);
   uint16_t cnt_wrap = TU_MIN(wNBytes - cnt_lin, info.len_wrap);
 
   // We want to read from the FIFO and write it into the PMA, if LIN part is ODD and has WRAPPED part,
   // last lin byte will be combined with wrapped part
   // To ensure PMA is always access aligned (dst aligned to 16 or 32 bit)
 #ifdef FSDEV_BUS_32BIT
-  if((cnt_lin & 0x03) && cnt_wrap)
-  {
+  if ((cnt_lin & 0x03) && cnt_wrap) {
     // Copy first linear part
-    dcd_write_packet_memory(dst, info.ptr_lin, cnt_lin &~0x03);
-    dst += cnt_lin &~0x03;
+    dcd_write_packet_memory(dst, info.ptr_lin, cnt_lin & ~0x03);
+    dst += cnt_lin & ~0x03;
 
     // Copy last linear bytes & first wrapped bytes to buffer
     uint32_t i;
     uint8_t tmp[4];
-    for (i = 0; i < (cnt_lin & 0x03); i++)
-    {
-      tmp[i] = ((uint8_t*)info.ptr_lin)[(cnt_lin &~0x03) + i];
+    for (i = 0; i < (cnt_lin & 0x03); i++) {
+      tmp[i] = ((uint8_t *)info.ptr_lin)[(cnt_lin & ~0x03) + i];
     }
     uint32_t wCnt = cnt_wrap;
-    for (; i < 4 && wCnt > 0; i++, wCnt--)
-    {
-      tmp[i] = *(uint8_t*)info.ptr_wrap;
-      info.ptr_wrap = (uint8_t*)info.ptr_wrap + 1;
+    for (; i < 4 && wCnt > 0; i++, wCnt--) {
+      tmp[i] = *(uint8_t *)info.ptr_wrap;
+      info.ptr_wrap = (uint8_t *)info.ptr_wrap + 1;
     }
 
     // Write unaligned buffer
@@ -1350,32 +1205,29 @@ static bool dcd_write_packet_memory_ff(tu_fifo_t * ff, uint16_t dst, uint16_t wN
 
     // Copy rest of wrapped byte
     if (wCnt)
-        dcd_write_packet_memory(dst, info.ptr_wrap, wCnt);
+      dcd_write_packet_memory(dst, info.ptr_wrap, wCnt);
   }
 #else
-  if((cnt_lin & 0x01) && cnt_wrap)
-  {
+  if ((cnt_lin & 0x01) && cnt_wrap) {
     // Copy first linear part
-    dcd_write_packet_memory(dst, info.ptr_lin, cnt_lin &~0x01);
-    dst += cnt_lin &~0x01;
+    dcd_write_packet_memory(dst, info.ptr_lin, cnt_lin & ~0x01);
+    dst += cnt_lin & ~0x01;
 
     // Copy last linear byte & first wrapped byte
-    uint16_t tmp = ((uint8_t*)info.ptr_lin)[cnt_lin - 1] | ((uint16_t)(((uint8_t*)info.ptr_wrap)[0]) << 8U);
+    uint16_t tmp = ((uint8_t *)info.ptr_lin)[cnt_lin - 1] | ((uint16_t)(((uint8_t *)info.ptr_wrap)[0]) << 8U);
     dcd_write_packet_memory(dst, &tmp, 2);
     dst += 2;
 
     // Copy rest of wrapped byte
-    dcd_write_packet_memory(dst, ((uint8_t*)info.ptr_wrap) + 1, cnt_wrap - 1);
+    dcd_write_packet_memory(dst, ((uint8_t *)info.ptr_wrap) + 1, cnt_wrap - 1);
   }
 #endif
-  else
-  {
+  else {
     // Copy linear part
     dcd_write_packet_memory(dst, info.ptr_lin, cnt_lin);
     dst += info.len_lin;
 
-    if(info.len_wrap)
-    {
+    if (info.len_wrap) {
       // Copy wrapped byte
       dcd_write_packet_memory(dst, info.ptr_wrap, cnt_wrap);
     }
@@ -1389,8 +1241,8 @@ static bool dcd_write_packet_memory_ff(tu_fifo_t * ff, uint16_t dst, uint16_t wN
 #ifdef FSDEV_BUS_32BIT
 static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t wNBytes)
 {
-  uint8_t* dstVal = dst;
-  volatile uint32_t* src32 = (volatile uint32_t*)(USB_PMAADDR + src);
+  uint8_t *dstVal = dst;
+  volatile uint32_t *src32 = (volatile uint32_t *)(USB_PMAADDR + src);
 
   for (uint32_t n = wNBytes / 4; n > 0; --n) {
     tu_unaligned_write32(dstVal, *src32++);
@@ -1398,20 +1250,17 @@ static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t
   }
 
   wNBytes = wNBytes & 0x03;
-  if (wNBytes)
-  {
+  if (wNBytes) {
     uint32_t rdVal = *src32;
 
     *dstVal = tu_u32_byte0(rdVal);
     wNBytes--;
 
-    if (wNBytes)
-    {
+    if (wNBytes) {
       *++dstVal = tu_u32_byte1(rdVal);
       wNBytes--;
 
-      if (wNBytes)
-      {
+      if (wNBytes) {
         *++dstVal = tu_u32_byte2(rdVal);
       }
     }
@@ -1421,11 +1270,11 @@ static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t
 }
 #else
 /**
-  * @brief Copy a buffer from packet memory area (PMA) to user memory area.
-  *        Uses byte-access of system memory and 16-bit access of packet memory
-  * @param   wNBytes no. of bytes to be copied.
-  * @retval None
-  */
+ * @brief Copy a buffer from packet memory area (PMA) to user memory area.
+ *        Uses byte-access of system memory and 16-bit access of packet memory
+ * @param   wNBytes no. of bytes to be copied.
+ * @retval None
+ */
 static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t wNBytes)
 {
   uint32_t n = (uint32_t)wNBytes >> 1U;
@@ -1435,18 +1284,16 @@ static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t
   uint32_t temp;
 
   pdwVal = &pma[FSDEV_PMA_STRIDE * (src >> 1)];
-  uint8_t *dstVal = (uint8_t*)dst;
+  uint8_t *dstVal = (uint8_t *)dst;
 
-  while (n--)
-  {
+  while (n--) {
     temp = *pdwVal;
     pdwVal += FSDEV_PMA_STRIDE;
     *dstVal++ = ((temp >> 0) & 0xFF);
     *dstVal++ = ((temp >> 8) & 0xFF);
   }
 
-  if (wNBytes & 0x01)
-  {
+  if (wNBytes & 0x01) {
     temp = *pdwVal;
     pdwVal += FSDEV_PMA_STRIDE;
     *dstVal++ = ((temp >> 0) & 0xFF);
@@ -1456,31 +1303,29 @@ static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t
 #endif
 
 /**
-  * @brief Copy a buffer from user packet memory area (PMA) to FIFO.
-  *        Uses byte-access of system memory and 16-bit access of packet memory
-  * @param   wNBytes no. of bytes to be copied.
-  * @retval None
-  */
-static bool dcd_read_packet_memory_ff(tu_fifo_t * ff, uint16_t src, uint16_t wNBytes)
+ * @brief Copy a buffer from user packet memory area (PMA) to FIFO.
+ *        Uses byte-access of system memory and 16-bit access of packet memory
+ * @param   wNBytes no. of bytes to be copied.
+ * @retval None
+ */
+static bool dcd_read_packet_memory_ff(tu_fifo_t *ff, uint16_t src, uint16_t wNBytes)
 {
   // Since we copy into a ring buffer FIFO, a wrap might occur making it necessary to conduct two copies
   // Check for first linear part
   tu_fifo_buffer_info_t info;
-  tu_fifo_get_write_info(ff, &info);  // We want to read from the FIFO
+  tu_fifo_get_write_info(ff, &info); // We want to read from the FIFO
 
-  uint16_t cnt_lin =  TU_MIN(wNBytes, info.len_lin);
+  uint16_t cnt_lin = TU_MIN(wNBytes, info.len_lin);
   uint16_t cnt_wrap = TU_MIN(wNBytes - cnt_lin, info.len_wrap);
 
-
   // We want to read from PMA and write it into the FIFO, if LIN part is ODD and has WRAPPED part,
   // last lin byte will be combined with wrapped part
   // To ensure PMA is always access aligned (src aligned to 16 or 32 bit)
 #ifdef FSDEV_BUS_32BIT
-  if((cnt_lin & 0x03) && cnt_wrap)
-  {
+  if ((cnt_lin & 0x03) && cnt_wrap) {
     // Copy first linear part
-    dcd_read_packet_memory(info.ptr_lin, src, cnt_lin &~0x03);
-    src += cnt_lin &~0x03;
+    dcd_read_packet_memory(info.ptr_lin, src, cnt_lin & ~0x03);
+    src += cnt_lin & ~0x03;
 
     // Copy last linear bytes & first wrapped bytes
     uint8_t tmp[4];
@@ -1488,15 +1333,13 @@ static bool dcd_read_packet_memory_ff(tu_fifo_t * ff, uint16_t src, uint16_t wNB
     src += 4;
 
     uint32_t i;
-    for (i = 0; i < (cnt_lin & 0x03); i++)
-    {
-      ((uint8_t*)info.ptr_lin)[(cnt_lin &~0x03) + i] = tmp[i];
+    for (i = 0; i < (cnt_lin & 0x03); i++) {
+      ((uint8_t *)info.ptr_lin)[(cnt_lin & ~0x03) + i] = tmp[i];
     }
     uint32_t wCnt = cnt_wrap;
-    for (; i < 4 && wCnt > 0; i++, wCnt--)
-    {
-      *(uint8_t*)info.ptr_wrap = tmp[i];
-      info.ptr_wrap = (uint8_t*)info.ptr_wrap + 1;
+    for (; i < 4 && wCnt > 0; i++, wCnt--) {
+      *(uint8_t *)info.ptr_wrap = tmp[i];
+      info.ptr_wrap = (uint8_t *)info.ptr_wrap + 1;
     }
 
     // Copy rest of wrapped byte
@@ -1504,32 +1347,29 @@ static bool dcd_read_packet_memory_ff(tu_fifo_t * ff, uint16_t src, uint16_t wNB
       dcd_read_packet_memory(info.ptr_wrap, src, wCnt);
   }
 #else
-  if((cnt_lin & 0x01) && cnt_wrap)
-  {
+  if ((cnt_lin & 0x01) && cnt_wrap) {
     // Copy first linear part
-    dcd_read_packet_memory(info.ptr_lin, src, cnt_lin &~0x01);
-    src += cnt_lin &~0x01;
+    dcd_read_packet_memory(info.ptr_lin, src, cnt_lin & ~0x01);
+    src += cnt_lin & ~0x01;
 
     // Copy last linear byte & first wrapped byte
     uint8_t tmp[2];
     dcd_read_packet_memory(tmp, src, 2);
     src += 2;
 
-    ((uint8_t*)info.ptr_lin)[cnt_lin - 1] = tmp[0];
-    ((uint8_t*)info.ptr_wrap)[0] = tmp[1];
+    ((uint8_t *)info.ptr_lin)[cnt_lin - 1] = tmp[0];
+    ((uint8_t *)info.ptr_wrap)[0] = tmp[1];
 
     // Copy rest of wrapped byte
-    dcd_read_packet_memory(((uint8_t*)info.ptr_wrap) + 1, src, cnt_wrap - 1);
+    dcd_read_packet_memory(((uint8_t *)info.ptr_wrap) + 1, src, cnt_wrap - 1);
   }
 #endif
-  else
-  {
+  else {
     // Copy linear part
     dcd_read_packet_memory(info.ptr_lin, src, cnt_lin);
     src += cnt_lin;
 
-    if(info.len_wrap)
-    {
+    if (info.len_wrap) {
       // Copy wrapped byte
       dcd_read_packet_memory(info.ptr_wrap, src, cnt_wrap);
     }
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.h
index 3f4db985d..7992f34a1 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.h
@@ -1,30 +1,32 @@
-/**
-  * Copyright(c) 2016 STMicroelectronics
-  * Copyright(c) N Conrad
-  *
-  * 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 STMicroelectronics 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 COPYRIGHT HOLDERS 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 COPYRIGHT HOLDER 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.
-  *
-  */
+/*
+ * Copyright(c) 2016 STMicroelectronics
+ * Copyright(c) N Conrad
+ * Copyright (c) 2019 Ha Thach (tinyusb.org)
+ *
+ * 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 STMicroelectronics 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 COPYRIGHT HOLDERS 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 COPYRIGHT HOLDER 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.
+ *
+ * This file is part of the TinyUSB stack.
+ */
 
 // This file contains source copied from ST's HAL, and thus should have their copyright statement.
 
@@ -113,6 +115,11 @@
 #elif CFG_TUSB_MCU == OPT_MCU_STM32H5
   #include "stm32h5xx.h"
   #define FSDEV_BUS_32BIT
+
+  #if !defined(USB_DRD_BASE) && defined(USB_DRD_FS_BASE)
+  #define USB_DRD_BASE USB_DRD_FS_BASE
+  #endif
+
   #define FSDEV_PMA_SIZE (2048u)
   #undef USB_PMAADDR
   #define USB_PMAADDR USB_DRD_PMAADDR
@@ -138,7 +145,6 @@
   #define USB_CNTR_LPMODE USB_CNTR_SUSPRDY
   #define USB_CNTR_FSUSP USB_CNTR_SUSPEN
 
-
 #elif CFG_TUSB_MCU == OPT_MCU_STM32WB
   #include "stm32wbxx.h"
   #define FSDEV_PMA_SIZE (1024u)
@@ -182,27 +188,23 @@ typedef uint16_t fsdev_bus_t;
 // Volatile is also needed to prevent the optimizer from changing access to 32-bit (as 32-bit access is forbidden)
 static __IO uint16_t * const pma = (__IO uint16_t*)USB_PMAADDR;
 
-TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t * pcd_btable_word_ptr(USB_TypeDef * USBx, size_t x)
-{
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t * pcd_btable_word_ptr(USB_TypeDef * USBx, size_t x) {
   size_t total_word_offset = (((USBx)->BTABLE)>>1) + x;
   total_word_offset *= FSDEV_PMA_STRIDE;
   return &(pma[total_word_offset]);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx) {
   return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 1u);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx) {
   return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 3u);
 }
 #endif
 
 /* Aligned buffer size according to hardware */
-TU_ATTR_ALWAYS_INLINE static inline uint16_t pcd_aligned_buffer_size(uint16_t size)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint16_t pcd_aligned_buffer_size(uint16_t size) {
   /* The STM32 full speed USB peripheral supports only a limited set of
    * buffer sizes given by the RX buffer entry format in the USB_BTABLE. */
   uint16_t blocksize = (size > 62) ? 32 : 2;
@@ -213,9 +215,7 @@ TU_ATTR_ALWAYS_INLINE static inline uint16_t pcd_aligned_buffer_size(uint16_t si
   return numblocks * blocksize;
 }
 
-/* SetENDPOINT */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wRegValue)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wRegValue) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   __O uint32_t *reg = (__O uint32_t *)(USB_DRD_BASE + bEpIdx*4);
@@ -226,7 +226,6 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, ui
 #endif
 }
 
-/* GetENDPOINT */
 TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
@@ -237,8 +236,7 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_endpoint(USB_TypeDef * USBx
   return *reg;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_eptype(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wType)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_eptype(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wType) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= (uint32_t)USB_EP_T_MASK;
   regVal |= wType;
@@ -246,20 +244,19 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_eptype(USB_TypeDef * USBx, uint
   pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_eptype(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_eptype(USB_TypeDef * USBx, uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EP_T_FIELD;
   return regVal;
 }
+
 /**
   * @brief  Clears bit CTR_RX / CTR_TX in the endpoint register.
   * @param  USBx USB peripheral instance register address.
   * @param  bEpIdx Endpoint Number.
   * @retval None
   */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal &= ~USB_EP_CTR_RX;
@@ -267,22 +264,21 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_ep_ctr(USB_TypeDef * USBx,
   pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal &= ~USB_EP_CTR_TX;
   regVal |= USB_EP_CTR_RX; // preserve CTR_RX (clears on writing 0)
   pcd_set_endpoint(USBx, bEpIdx,regVal);
 }
+
 /**
   * @brief  gets counter of the tx buffer.
   * @param  USBx USB peripheral instance register address.
   * @param  bEpIdx Endpoint Number.
   * @retval Counter value
   */
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   return (pma32[2*bEpIdx] & 0x03FF0000) >> 16;
@@ -292,8 +288,7 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_cnt(USB_TypeDef * USB
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   return (pma32[2*bEpIdx + 1] & 0x03FF0000) >> 16;
@@ -303,6 +298,9 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USB
 #endif
 }
 
+#define pcd_get_ep_dbuf0_cnt pcd_get_ep_tx_cnt
+#define pcd_get_ep_dbuf1_cnt pcd_get_ep_rx_cnt
+
 /**
   * @brief  Sets address in an endpoint register.
   * @param  USBx USB peripheral instance register address.
@@ -310,8 +308,7 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USB
   * @param  bAddr Address.
   * @retval None
   */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_address(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t bAddr)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_address(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t bAddr) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal |= bAddr;
@@ -319,8 +316,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_address(USB_TypeDef * USBx,
   pcd_set_endpoint(USBx, bEpIdx,regVal);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_address(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_address(USB_TypeDef * USBx, uint32_t bEpIdx) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   return pma32[2*bEpIdx] & 0x0000FFFFu ;
@@ -329,8 +325,7 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_address(USB_TypeDef *
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_address(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_address(USB_TypeDef * USBx, uint32_t bEpIdx) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   return pma32[2*bEpIdx + 1] & 0x0000FFFFu;
@@ -339,8 +334,10 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_address(USB_TypeDef *
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_address(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t addr)
-{
+#define pcd_get_ep_dbuf0_address pcd_get_ep_tx_address
+#define pcd_get_ep_dbuf1_address pcd_get_ep_rx_address
+
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_address(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t addr) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   pma32[2*bEpIdx] = (pma32[2*bEpIdx] & 0xFFFF0000u) | (addr & 0x0000FFFCu);
@@ -349,8 +346,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_address(USB_TypeDef * USB
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_address(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t addr)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_address(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t addr) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   pma32[2*bEpIdx + 1] = (pma32[2*bEpIdx + 1] & 0xFFFF0000u) | (addr & 0x0000FFFCu);
@@ -359,8 +355,10 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_address(USB_TypeDef * USB
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount)
-{
+#define pcd_set_ep_dbuf0_address pcd_set_ep_tx_address
+#define pcd_set_ep_dbuf1_address pcd_set_ep_rx_address
+
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   pma32[2*bEpIdx] = (pma32[2*bEpIdx] & ~0x03FF0000u) | ((wCount & 0x3FFu) << 16);
@@ -370,8 +368,9 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_cnt(USB_TypeDef * USBx, u
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount)
-{
+#define pcd_set_ep_tx_dbuf0_cnt pcd_set_ep_tx_cnt
+
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_dbuf1_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   pma32[2*bEpIdx + 1] = (pma32[2*bEpIdx + 1] & ~0x03FF0000u) | ((wCount & 0x3FFu) << 16);
@@ -381,8 +380,8 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_cnt(USB_TypeDef * USBx, u
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_blsize_num_blocks(USB_TypeDef * USBx, uint32_t rxtx_idx, uint32_t blocksize, uint32_t numblocks)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_blsize_num_blocks(USB_TypeDef * USBx, uint32_t rxtx_idx,
+                                                                      uint32_t blocksize, uint32_t numblocks) {
   /* Encode into register. When BLSIZE==1, we need to subtract 1 block count */
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
@@ -393,8 +392,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_blsize_num_blocks(USB_TypeDe
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_bufsize(USB_TypeDef * USBx, uint32_t rxtx_idx, uint32_t wCount)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_bufsize(USB_TypeDef * USBx, uint32_t rxtx_idx, uint32_t wCount) {
   wCount = pcd_aligned_buffer_size(wCount);
 
   /* We assume that the buffer size is already aligned to hardware requirements. */
@@ -408,16 +406,16 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_bufsize(USB_TypeDef * USBx,
   pcd_set_ep_blsize_num_blocks(USBx, rxtx_idx, blocksize, numblocks);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_bufsize(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_dbuf0_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount) {
   pcd_set_ep_bufsize(USBx, 2*bEpIdx, wCount);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_bufsize(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount) {
   pcd_set_ep_bufsize(USBx, 2*bEpIdx + 1, wCount);
 }
 
+#define pcd_set_ep_rx_dbuf1_cnt pcd_set_ep_rx_cnt
+
 /**
   * @brief  sets the status for tx transfer (bits STAT_TX[1:0]).
   * @param  USBx USB peripheral instance register address.
@@ -425,8 +423,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_bufsize(USB_TypeDef * USB
   * @param  wState new state
   * @retval None
   */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wState)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wState) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPTX_DTOGMASK;
 
@@ -443,7 +440,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx
 
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
   pcd_set_endpoint(USBx, bEpIdx, regVal);
-} /* pcd_set_ep_tx_status */
+}
 
 /**
   * @brief  sets the status for rx transfer (bits STAT_TX[1:0])
@@ -453,31 +450,27 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx
   * @retval None
   */
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wState)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wState) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPRX_DTOGMASK;
 
   /* toggle first bit ? */
-  if((USB_EPRX_DTOG1 & wState)!= 0U)
-  {
+  if((USB_EPRX_DTOG1 & wState)!= 0U) {
     regVal ^= USB_EPRX_DTOG1;
   }
   /* toggle second bit ? */
-  if((USB_EPRX_DTOG2 & wState)!= 0U)
-  {
+  if((USB_EPRX_DTOG2 & wState)!= 0U) {
     regVal ^= USB_EPRX_DTOG2;
   }
 
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
   pcd_set_endpoint(USBx, bEpIdx, regVal);
-} /* pcd_set_ep_rx_status */
+}
 
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   return (regVal & USB_EPRX_STAT) >> (12u);
-} /* pcd_get_ep_rx_status */
+}
 
 
 /**
@@ -486,16 +479,14 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_status(USB_TypeDef *
   * @param  bEpIdx Endpoint Number.
   * @retval None
   */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX|USB_EP_DTOG_RX;
   pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX|USB_EP_DTOG_TX;
@@ -508,21 +499,16 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_tx_dtog(USB_TypeDef * USBx,  uint32
   * @param  bEpIdx Endpoint Number.
   * @retval None
   */
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  if((regVal & USB_EP_DTOG_RX) != 0)
-  {
+  if((regVal & USB_EP_DTOG_RX) != 0) {
     pcd_rx_dtog(USBx,bEpIdx);
   }
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  if((regVal & USB_EP_DTOG_TX) != 0)
-  {
+  if((regVal & USB_EP_DTOG_TX) != 0) {
     pcd_tx_dtog(USBx,bEpIdx);
   }
 }
@@ -533,17 +519,15 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_dtog(USB_TypeDef * USBx,
   * @param  bEpIdx Endpoint Number.
   * @retval None
   */
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_kind(USB_TypeDef * USBx,  uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_kind(USB_TypeDef * USBx,  uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal |= USB_EP_KIND;
   regVal &= USB_EPREG_MASK;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
   pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_ep_kind(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_ep_kind(USB_TypeDef * USBx, uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPKIND_MASK;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
diff --git a/src/portable/sunxi/dcd_sunxi_musb.c b/src/portable/sunxi/dcd_sunxi_musb.c
index 6cc1975a8..6f36ad441 100644
--- a/src/portable/sunxi/dcd_sunxi_musb.c
+++ b/src/portable/sunxi/dcd_sunxi_musb.c
@@ -35,7 +35,9 @@
 #include <f1c100s-irq.h>
 #include <device/dcd.h>
 #include "musb_def.h"
-#include "bsp/board.h"
+
+//#include "bsp/board_api.h"
+extern uint32_t board_millis(void); // TODO remove
 
 typedef uint32_t u32;
 typedef uint16_t u16;
@@ -58,7 +60,7 @@ typedef struct TU_ATTR_PACKED
 
 typedef struct
 {
-  tusb_control_request_t setup_packet;
+  CFG_TUD_MEM_ALIGN tusb_control_request_t setup_packet;
   uint16_t     remaining_ctrl; /* The number of bytes remaining in data stage of control transfer. */
   int8_t       status_out;
   pipe_state_t pipe0;
@@ -350,7 +352,7 @@ static void USBC_INT_DisableRxEp(u8 ep_index)
  * INTERNAL FUNCTION DECLARATION
  *------------------------------------------------------------------*/
 
-static dcd_data_t _dcd;
+CFG_TUD_MEM_ALIGN static dcd_data_t _dcd;
 
 static inline free_block_t *find_containing_block(free_block_t *beg, free_block_t *end, uint_fast16_t addr)
 {
@@ -560,7 +562,7 @@ static void pipe_read_write_packet_ff(tu_fifo_t *f, volatile void *fifo, unsigne
 
 static void process_setup_packet(uint8_t rhport)
 {
-  uint32_t *p = (uint32_t*)&_dcd.setup_packet;
+  uint32_t *p = (uint32_t*)(uintptr_t) &_dcd.setup_packet;
   p[0]        = USBC_Readl(USBC_REG_EPFIFO0(USBC0_BASE));
   p[1]        = USBC_Readl(USBC_REG_EPFIFO0(USBC0_BASE));
 
@@ -594,7 +596,7 @@ static bool handle_xfer_in(uint_fast8_t ep_addr)
   if (len) {
     volatile void* addr = (volatile void*)(USBC_REG_EPFIFO1(USBC0_BASE) + (epnum_minus1 << 2));
     if (_dcd.pipe_buf_is_fifo[TUSB_DIR_IN] & TU_BIT(epnum_minus1)) {
-      pipe_read_write_packet_ff((tu_fifo_t *)buf, addr, len, TUSB_DIR_IN);
+      pipe_read_write_packet_ff((tu_fifo_t *)(uintptr_t) buf, addr, len, TUSB_DIR_IN);
     } else {
       pipe_write_packet(buf, addr, len);
       pipe->buf       = buf + len;
@@ -622,7 +624,7 @@ static bool handle_xfer_out(uint_fast8_t ep_addr)
   if (len) {
     volatile void* addr = (volatile void*)(USBC_REG_EPFIFO1(USBC0_BASE) + (epnum_minus1 << 2));
     if (_dcd.pipe_buf_is_fifo[TUSB_DIR_OUT] & TU_BIT(epnum_minus1)) {
-      pipe_read_write_packet_ff((tu_fifo_t *)buf, addr, len, TUSB_DIR_OUT);
+      pipe_read_write_packet_ff((tu_fifo_t *)(uintptr_t )buf, addr, len, TUSB_DIR_OUT);
     } else {
       pipe_read_packet(buf, addr, len);
       pipe->buf       = buf + len;
diff --git a/src/portable/synopsys/dwc2/dcd_dwc2.c b/src/portable/synopsys/dwc2/dcd_dwc2.c
index 8163ba8e1..c9580655f 100644
--- a/src/portable/synopsys/dwc2/dcd_dwc2.c
+++ b/src/portable/synopsys/dwc2/dcd_dwc2.c
@@ -97,28 +97,170 @@ static uint16_t ep0_pending[2];               // Index determines direction as t
 
 // TX FIFO RAM allocation so far in words - RX FIFO size is readily available from dwc2->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)
 
 // SOF enabling flag - required for SOF to not get disabled in ISR when SOF was enabled by
 static bool _sof_en;
 
-// Calculate the RX FIFO size according to recommendations from reference manual
+// Calculate the RX FIFO size according to minimum recommendations from reference manual
+// RxFIFO = (5 * number of control endpoints + 8) +
+//          ((largest USB packet used / 4) + 1 for status information) +
+//          (2 * number of OUT endpoints) + 1 for Global NAK
+// with number of control endpoints = 1 we have
+// RxFIFO = 15 + (largest USB packet used / 4) + 2 * number of OUT endpoints
+// we double the largest USB packet size to be able to hold up to 2 packets
 static inline uint16_t calc_grxfsiz(uint16_t max_ep_size, uint8_t ep_count) {
   return 15 + 2 * (max_ep_size / 4) + 2 * ep_count;
 }
 
-static void update_grxfsiz(uint8_t rhport) {
+TU_ATTR_ALWAYS_INLINE static inline void fifo_flush_tx(dwc2_regs_t* dwc2, uint8_t epnum) {
+  // flush TX fifo and wait for it cleared
+  dwc2->grstctl = GRSTCTL_TXFFLSH | (epnum << GRSTCTL_TXFNUM_Pos);
+  while (dwc2->grstctl & GRSTCTL_TXFFLSH_Msk) {}
+}
+TU_ATTR_ALWAYS_INLINE static inline void fifo_flush_rx(dwc2_regs_t* dwc2) {
+  // flush RX fifo and wait for it cleared
+  dwc2->grstctl = GRSTCTL_RXFFLSH;
+  while (dwc2->grstctl & GRSTCTL_RXFFLSH_Msk) {}
+}
+
+static bool fifo_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t packet_size) {
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
   uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
-  // Determine largest EP size for RX FIFO
-  uint16_t max_epsize = 0;
-  for (uint8_t epnum = 0; epnum < ep_count; epnum++) {
-    max_epsize = tu_max16(max_epsize, xfer_status[epnum][TUSB_DIR_OUT].max_size);
+  TU_ASSERT(epnum < ep_count);
+
+  uint16_t fifo_size = tu_div_ceil(packet_size, 4);
+
+  // "USB Data FIFOs" section in reference manual
+  // 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 |
+  // ---------------
+  // |    FREE     |
+  // --------------- GRXFSIZ
+  // | OUT FIFO    |
+  // | ( Shared )  |
+  // --------------- 0
+  //
+  // In FIFO is allocated by following rules:
+  // - IN EP 1 gets FIFO 1, IN EP "n" gets FIFO "n".
+  if (dir == TUSB_DIR_OUT) {
+    // Calculate required size of RX FIFO
+    uint16_t const sz = calc_grxfsiz(4 * fifo_size, ep_count);
+
+    // If size_rx needs to be extended check if possible and if so enlarge it
+    if (dwc2->grxfsiz < sz) {
+      TU_ASSERT(sz + _allocated_fifo_words_tx <= _dwc2_controller[rhport].ep_fifo_size / 4);
+
+      // Enlarge RX FIFO
+      dwc2->grxfsiz = sz;
+    }
+  } else {
+    // Note if The TXFELVL is configured as half empty. In order
+    // to be able to write a packet at that point, the fifo must be twice the max_size.
+    if ((dwc2->gahbcfg & GAHBCFG_TXFELVL) == 0) {
+      fifo_size *= 2;
+    }
+
+    // Check if free space is available
+    TU_ASSERT(_allocated_fifo_words_tx + fifo_size + dwc2->grxfsiz <= _dwc2_controller[rhport].ep_fifo_size / 4);
+    _allocated_fifo_words_tx += fifo_size;
+    TU_LOG(DWC2_DEBUG, "    Allocated %u bytes at offset %" PRIu32, fifo_size * 4,
+           _dwc2_controller[rhport].ep_fifo_size - _allocated_fifo_words_tx * 4);
+
+    // DIEPTXF starts at FIFO #1.
+    // Both TXFD and TXSA are in unit of 32-bit words.
+    dwc2->dieptxf[epnum - 1] = (fifo_size << DIEPTXF_INEPTXFD_Pos) |
+                               (_dwc2_controller[rhport].ep_fifo_size / 4 - _allocated_fifo_words_tx);
+  }
+
+  return true;
+}
+
+static void edpt_activate(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc) {
+  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
+  uint8_t const epnum = tu_edpt_number(p_endpoint_desc->bEndpointAddress);
+  uint8_t const dir = tu_edpt_dir(p_endpoint_desc->bEndpointAddress);
+
+  xfer_ctl_t* xfer = XFER_CTL_BASE(epnum, dir);
+  xfer->max_size = tu_edpt_packet_size(p_endpoint_desc);
+  xfer->interval = p_endpoint_desc->bInterval;
+
+  // USBAEP, EPTYP, SD0PID_SEVNFRM, MPSIZ are the same for IN and OUT endpoints.
+  uint32_t const dxepctl = (1 << DOEPCTL_USBAEP_Pos) |
+                           (p_endpoint_desc->bmAttributes.xfer << DOEPCTL_EPTYP_Pos) |
+                           (p_endpoint_desc->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS ? DOEPCTL_SD0PID_SEVNFRM : 0) |
+                           (xfer->max_size << DOEPCTL_MPSIZ_Pos);
+
+  if (dir == TUSB_DIR_OUT) {
+    dwc2->epout[epnum].doepctl = dxepctl;
+    dwc2->daintmsk |= TU_BIT(DAINTMSK_OEPM_Pos + epnum);
+  } else {
+    dwc2->epin[epnum].diepctl = dxepctl | (epnum << DIEPCTL_TXFNUM_Pos);
+    dwc2->daintmsk |= (1 << (DAINTMSK_IEPM_Pos + epnum));
   }
+}
+
+static void edpt_disable(uint8_t rhport, uint8_t ep_addr, bool stall) {
+  (void) rhport;
+
+  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
 
-  // Update size of RX FIFO
-  dwc2->grxfsiz = calc_grxfsiz(max_epsize, ep_count);
+  if (dir == TUSB_DIR_IN) {
+    dwc2_epin_t* epin = dwc2->epin;
+
+    // Only disable currently enabled non-control endpoint
+    if ((epnum == 0) || !(epin[epnum].diepctl & DIEPCTL_EPENA)) {
+      epin[epnum].diepctl |= DIEPCTL_SNAK | (stall ? DIEPCTL_STALL : 0);
+    } else {
+      // Stop transmitting packets and NAK IN xfers.
+      epin[epnum].diepctl |= DIEPCTL_SNAK;
+      while ((epin[epnum].diepint & DIEPINT_INEPNE) == 0) {}
+
+      // Disable the endpoint.
+      epin[epnum].diepctl |= DIEPCTL_EPDIS | (stall ? DIEPCTL_STALL : 0);
+      while ((epin[epnum].diepint & DIEPINT_EPDISD_Msk) == 0) {}
+
+      epin[epnum].diepint = DIEPINT_EPDISD;
+    }
+
+    // Flush the FIFO, and wait until we have confirmed it cleared.
+    fifo_flush_tx(dwc2, epnum);
+  } else {
+    dwc2_epout_t* epout = dwc2->epout;
+
+    // Only disable currently enabled non-control endpoint
+    if ((epnum == 0) || !(epout[epnum].doepctl & DOEPCTL_EPENA)) {
+      epout[epnum].doepctl |= stall ? DOEPCTL_STALL : 0;
+    } else {
+      // Asserting GONAK is required to STALL an OUT endpoint.
+      // Simpler to use polling here, we don't use the "B"OUTNAKEFF interrupt
+      // anyway, and it can't be cleared by user code. If this while loop never
+      // finishes, we have bigger problems than just the stack.
+      dwc2->dctl |= DCTL_SGONAK;
+      while ((dwc2->gintsts & GINTSTS_BOUTNAKEFF_Msk) == 0) {}
+
+      // Ditto here- disable the endpoint.
+      epout[epnum].doepctl |= DOEPCTL_EPDIS | (stall ? DOEPCTL_STALL : 0);
+      while ((epout[epnum].doepint & DOEPINT_EPDISD_Msk) == 0) {}
+
+      epout[epnum].doepint = DOEPINT_EPDISD;
+
+      // Allow other OUT endpoints to keep receiving.
+      dwc2->dctl |= DCTL_CGONAK;
+    }
+  }
 }
 
 // Start of Bus Reset
@@ -127,7 +269,6 @@ static void bus_reset(uint8_t rhport) {
   uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
 
   tu_memclr(xfer_status, sizeof(xfer_status));
-  _out_ep_closed = false;
 
   _sof_en = false;
 
@@ -139,7 +280,17 @@ static void bus_reset(uint8_t rhport) {
     dwc2->epout[n].doepctl |= DOEPCTL_SNAK;
   }
 
-  // 2. Set up interrupt mask
+  // 2. Disable all IN endpoints
+  for (uint8_t n = 0; n < ep_count; n++) {
+    if (dwc2->epin[n].diepctl & DIEPCTL_EPENA) {
+      dwc2->epin[n].diepctl |= DIEPCTL_SNAK | DIEPCTL_EPDIS;
+    }
+  }
+
+  fifo_flush_tx(dwc2, 0x10); // all tx fifo
+  fifo_flush_rx(dwc2);
+
+  // 3. Set up interrupt mask
   dwc2->daintmsk = TU_BIT(DAINTMSK_OEPM_Pos) | TU_BIT(DAINTMSK_IEPM_Pos);
   dwc2->doepmsk = DOEPMSK_STUPM | DOEPMSK_XFRCM;
   dwc2->diepmsk = DIEPMSK_TOM | DIEPMSK_XFRCM;
@@ -269,18 +420,16 @@ static void edpt_schedule_packets(uint8_t rhport, uint8_t const epnum, uint8_t c
 /* Controller API
  *------------------------------------------------------------------*/
 #if CFG_TUSB_DEBUG >= DWC2_DEBUG
-
 void print_dwc2_info(dwc2_regs_t* dwc2) {
   // print guid, gsnpsid, ghwcfg1, ghwcfg2, ghwcfg3, ghwcfg4
   // use dwc2_info.py/md for bit-field value and comparison with other ports
   volatile uint32_t const* p = (volatile uint32_t const*) &dwc2->guid;
   TU_LOG(DWC2_DEBUG, "guid, gsnpsid, ghwcfg1, ghwcfg2, ghwcfg3, ghwcfg4\r\n");
   for (size_t i = 0; i < 5; i++) {
-    TU_LOG(DWC2_DEBUG, "0x%08lX, ", p[i]);
+    TU_LOG(DWC2_DEBUG, "0x%08" PRIX32 ", ", p[i]);
   }
-  TU_LOG(DWC2_DEBUG, "0x%08lX\r\n", p[5]);
+  TU_LOG(DWC2_DEBUG, "0x%08" PRIX32 "\r\n", p[5]);
 }
-
 #endif
 
 static void reset_core(dwc2_regs_t* dwc2) {
@@ -298,11 +447,15 @@ static void reset_core(dwc2_regs_t* dwc2) {
 }
 
 static bool phy_hs_supported(dwc2_regs_t* dwc2) {
-  // note: esp32 incorrect report its hs_phy_type as utmi
+  (void) dwc2;
+
 #if TU_CHECK_MCU(OPT_MCU_ESP32S2, OPT_MCU_ESP32S3)
+  // note: esp32 incorrect report its hs_phy_type as utmi
+  return false;
+#elif !TUD_OPT_HIGH_SPEED
   return false;
 #else
-  return TUD_OPT_HIGH_SPEED && dwc2->ghwcfg2_bm.hs_phy_type != HS_PHY_TYPE_NONE;
+  return dwc2->ghwcfg2_bm.hs_phy_type != HS_PHY_TYPE_NONE;
 #endif
 }
 
@@ -447,13 +600,8 @@ void dcd_init(uint8_t rhport) {
   // (non zero-length packet), send STALL back and discard.
   dwc2->dcfg |= DCFG_NZLSOHSK;
 
-  // flush all TX fifo and wait for it cleared
-  dwc2->grstctl = GRSTCTL_TXFFLSH | (0x10u << GRSTCTL_TXFNUM_Pos);
-  while (dwc2->grstctl & GRSTCTL_TXFFLSH_Msk) {}
-
-  // flush RX fifo and wait for it cleared
-  dwc2->grstctl = GRSTCTL_RXFFLSH;
-  while (dwc2->grstctl & GRSTCTL_RXFFLSH_Msk) {}
+  fifo_flush_tx(dwc2, 0x10); // all tx fifo
+  fifo_flush_rx(dwc2);
 
   // Clear all interrupts
   uint32_t int_mask = dwc2->gintsts;
@@ -462,11 +610,12 @@ void dcd_init(uint8_t rhport) {
   dwc2->gotgint |= int_mask;
 
   // Required as part of core initialization.
-  // TODO: How should mode mismatch be handled? It will cause
-  // the core to stop working/require reset.
-  dwc2->gintmsk = GINTMSK_OTGINT | GINTMSK_MMISM | GINTMSK_RXFLVLM |
+  dwc2->gintmsk = GINTMSK_OTGINT | GINTMSK_RXFLVLM |
                   GINTMSK_USBSUSPM | GINTMSK_USBRST | GINTMSK_ENUMDNEM | GINTMSK_WUIM;
 
+  // Configure TX FIFO empty level for interrupt. Default is complete empty
+  dwc2->gahbcfg |= GAHBCFG_TXFELVL;
+
   // Enable global interrupt
   dwc2->gahbcfg |= GAHBCFG_GINT;
 
@@ -547,84 +696,8 @@ void dcd_sof_enable(uint8_t rhport, bool en) {
  *------------------------------------------------------------------*/
 
 bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const* desc_edpt) {
-  (void) rhport;
-
-  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-  uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
-
-  uint8_t const epnum = tu_edpt_number(desc_edpt->bEndpointAddress);
-  uint8_t const dir = tu_edpt_dir(desc_edpt->bEndpointAddress);
-
-  TU_ASSERT(epnum < ep_count);
-
-  xfer_ctl_t* xfer = XFER_CTL_BASE(epnum, dir);
-  xfer->max_size = tu_edpt_packet_size(desc_edpt);
-  xfer->interval = desc_edpt->bInterval;
-
-  uint16_t const fifo_size = tu_div_ceil(xfer->max_size, 4);
-
-  if (dir == TUSB_DIR_OUT) {
-    // Calculate required size of RX FIFO
-    uint16_t const sz = calc_grxfsiz(4 * fifo_size, ep_count);
-
-    // If size_rx needs to be extended check if possible and if so enlarge it
-    if (dwc2->grxfsiz < sz) {
-      TU_ASSERT(sz + _allocated_fifo_words_tx <= _dwc2_controller[rhport].ep_fifo_size / 4);
-
-      // Enlarge RX FIFO
-      dwc2->grxfsiz = sz;
-    }
-
-    dwc2->epout[epnum].doepctl |= (1 << DOEPCTL_USBAEP_Pos) |
-                                  (desc_edpt->bmAttributes.xfer << DOEPCTL_EPTYP_Pos) |
-                                  (desc_edpt->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS ? DOEPCTL_SD0PID_SEVNFRM : 0) |
-                                  (xfer->max_size << DOEPCTL_MPSIZ_Pos);
-
-    dwc2->daintmsk |= TU_BIT(DAINTMSK_OEPM_Pos + epnum);
-  } else {
-    // "USB Data FIFOs" section in reference manual
-    // 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 |
-    // ---------------
-    // |    FREE     |
-    // --------------- GRXFSIZ
-    // | OUT FIFO    |
-    // | ( Shared )  |
-    // --------------- 0
-    //
-    // In FIFO is allocated by following rules:
-    // - IN EP 1 gets FIFO 1, IN EP "n" gets FIFO "n".
-
-    // Check if free space is available
-    TU_ASSERT(_allocated_fifo_words_tx + fifo_size + dwc2->grxfsiz <= _dwc2_controller[rhport].ep_fifo_size / 4);
-
-    _allocated_fifo_words_tx += fifo_size;
-
-    TU_LOG(DWC2_DEBUG, "    Allocated %u bytes at offset %lu", fifo_size * 4,
-           _dwc2_controller[rhport].ep_fifo_size - _allocated_fifo_words_tx * 4);
-
-    // DIEPTXF starts at FIFO #1.
-    // Both TXFD and TXSA are in unit of 32-bit words.
-    dwc2->dieptxf[epnum - 1] = (fifo_size << DIEPTXF_INEPTXFD_Pos) |
-                               (_dwc2_controller[rhport].ep_fifo_size / 4 - _allocated_fifo_words_tx);
-
-    dwc2->epin[epnum].diepctl |= (1 << DIEPCTL_USBAEP_Pos) |
-                                 (epnum << DIEPCTL_TXFNUM_Pos) |
-                                 (desc_edpt->bmAttributes.xfer << DIEPCTL_EPTYP_Pos) |
-                                 (desc_edpt->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS ? DIEPCTL_SD0PID_SEVNFRM : 0) |
-                                 (xfer->max_size << DIEPCTL_MPSIZ_Pos);
-
-    dwc2->daintmsk |= (1 << (DAINTMSK_IEPM_Pos + epnum));
-  }
-
+  TU_ASSERT(fifo_alloc(rhport, desc_edpt->bEndpointAddress, tu_edpt_packet_size(desc_edpt)));
+  edpt_activate(rhport, desc_edpt);
   return true;
 }
 
@@ -638,16 +711,39 @@ void dcd_edpt_close_all(uint8_t rhport) {
 
   for (uint8_t n = 1; n < ep_count; n++) {
     // disable OUT endpoint
-    dwc2->epout[n].doepctl = 0;
+    if (dwc2->epout[n].doepctl & DOEPCTL_EPENA) {
+      dwc2->epout[n].doepctl |= DOEPCTL_SNAK | DOEPCTL_EPDIS;
+    }
     xfer_status[n][TUSB_DIR_OUT].max_size = 0;
 
     // disable IN endpoint
-    dwc2->epin[n].diepctl = 0;
+    if (dwc2->epin[n].diepctl & DIEPCTL_EPENA) {
+      dwc2->epin[n].diepctl |= DIEPCTL_SNAK | DIEPCTL_EPDIS;
+    }
     xfer_status[n][TUSB_DIR_IN].max_size = 0;
   }
 
+  // reset allocated fifo OUT
+  dwc2->grxfsiz = calc_grxfsiz(64, ep_count);
   // reset allocated fifo IN
   _allocated_fifo_words_tx = 16;
+
+  fifo_flush_tx(dwc2, 0x10); // all tx fifo
+  fifo_flush_rx(dwc2);
+}
+
+bool dcd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size) {
+  TU_ASSERT(fifo_alloc(rhport, ep_addr, largest_packet_size));
+  return true;
+}
+
+bool dcd_edpt_iso_activate(uint8_t rhport,  tusb_desc_endpoint_t const * p_endpoint_desc) {
+  // Disable EP to clear potential incomplete transfers
+  edpt_disable(rhport, p_endpoint_desc->bEndpointAddress, false);
+
+  edpt_activate(rhport, p_endpoint_desc);
+
+  return true;
 }
 
 bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes) {
@@ -707,89 +803,12 @@ bool dcd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t* ff, uint16_t
   return true;
 }
 
-static void dcd_edpt_disable(uint8_t rhport, uint8_t ep_addr, bool stall) {
-  (void) rhport;
-
-  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir = tu_edpt_dir(ep_addr);
-
-  if (dir == TUSB_DIR_IN) {
-    dwc2_epin_t* epin = dwc2->epin;
-
-    // Only disable currently enabled non-control endpoint
-    if ((epnum == 0) || !(epin[epnum].diepctl & DIEPCTL_EPENA)) {
-      epin[epnum].diepctl |= DIEPCTL_SNAK | (stall ? DIEPCTL_STALL : 0);
-    } else {
-      // Stop transmitting packets and NAK IN xfers.
-      epin[epnum].diepctl |= DIEPCTL_SNAK;
-      while ((epin[epnum].diepint & DIEPINT_INEPNE) == 0) {}
-
-      // Disable the endpoint.
-      epin[epnum].diepctl |= DIEPCTL_EPDIS | (stall ? DIEPCTL_STALL : 0);
-      while ((epin[epnum].diepint & DIEPINT_EPDISD_Msk) == 0) {}
-
-      epin[epnum].diepint = DIEPINT_EPDISD;
-    }
-
-    // Flush the FIFO, and wait until we have confirmed it cleared.
-    dwc2->grstctl = ((epnum << GRSTCTL_TXFNUM_Pos) | GRSTCTL_TXFFLSH);
-    while ((dwc2->grstctl & GRSTCTL_TXFFLSH_Msk) != 0) {}
-  } else {
-    dwc2_epout_t* epout = dwc2->epout;
-
-    // Only disable currently enabled non-control endpoint
-    if ((epnum == 0) || !(epout[epnum].doepctl & DOEPCTL_EPENA)) {
-      epout[epnum].doepctl |= stall ? DOEPCTL_STALL : 0;
-    } else {
-      // Asserting GONAK is required to STALL an OUT endpoint.
-      // Simpler to use polling here, we don't use the "B"OUTNAKEFF interrupt
-      // anyway, and it can't be cleared by user code. If this while loop never
-      // finishes, we have bigger problems than just the stack.
-      dwc2->dctl |= DCTL_SGONAK;
-      while ((dwc2->gintsts & GINTSTS_BOUTNAKEFF_Msk) == 0) {}
-
-      // Ditto here- disable the endpoint.
-      epout[epnum].doepctl |= DOEPCTL_EPDIS | (stall ? DOEPCTL_STALL : 0);
-      while ((epout[epnum].doepint & DOEPINT_EPDISD_Msk) == 0) {}
-
-      epout[epnum].doepint = DOEPINT_EPDISD;
-
-      // Allow other OUT endpoints to keep receiving.
-      dwc2->dctl |= DCTL_CGONAK;
-    }
-  }
-}
-
-/**
- * Close an endpoint.
- */
 void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr) {
-  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-
-  uint8_t const epnum = tu_edpt_number(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 = (dwc2->dieptxf[epnum - 1] & DIEPTXF_INEPTXFD_Msk) >> DIEPTXF_INEPTXFD_Pos;
-    uint16_t const fifo_start = (dwc2->dieptxf[epnum - 1] & DIEPTXF_INEPTXSA_Msk) >> DIEPTXF_INEPTXSA_Pos;
-
-    // For now only the last opened endpoint can be closed without fuss.
-    TU_ASSERT(fifo_start == _dwc2_controller[rhport].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
-  }
+  edpt_disable(rhport, ep_addr, false);
 }
 
 void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr) {
-  dcd_edpt_disable(rhport, ep_addr, true);
+  edpt_disable(rhport, ep_addr, true);
 }
 
 void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr) {
@@ -1130,18 +1149,16 @@ void dcd_int_handler(uint8_t rhport) {
     dwc2->gotgint = otg_int;
   }
 
-  if (int_status & GINTSTS_SOF) {
-    dwc2->gotgint = GINTSTS_SOF;
+  if(int_status & GINTSTS_SOF) {
+    dwc2->gintsts = GINTSTS_SOF;
+    const uint32_t frame = (dwc2->dsts & DSTS_FNSOF) >> DSTS_FNSOF_Pos;
 
-    if (_sof_en) {
-      uint32_t frame = (dwc2->dsts & (DSTS_FNSOF)) >> 8;
-      dcd_event_sof(rhport, frame, true);
-    } else {
-      // Disable SOF interrupt if SOF was not explicitly enabled. SOF was used for remote wakeup detection
+    // Disable SOF interrupt if SOF was not explicitly enabled since SOF was used for remote wakeup detection
+    if (!_sof_en) {
       dwc2->gintmsk &= ~GINTMSK_SOFM;
     }
 
-    dcd_event_bus_signal(rhport, DCD_EVENT_SOF, true);
+    dcd_event_sof(rhport, frame, true);
   }
 
   // RxFIFO non-empty interrupt handling.
@@ -1154,15 +1171,7 @@ void dcd_int_handler(uint8_t rhport) {
     // Loop until all available packets were handled
     do {
       handle_rxflvl_irq(rhport);
-    } while (dwc2->gotgint & GINTSTS_RXFLVL);
-
-    // Manage RX FIFO size
-    if (_out_ep_closed) {
-      update_grxfsiz(rhport);
-
-      // Disable flag
-      _out_ep_closed = false;
-    }
+    } while(dwc2->gintsts & GINTSTS_RXFLVL);
 
     dwc2->gintmsk |= GINTMSK_RXFLVLM;
   }
diff --git a/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c b/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
index b4dfda575..8005f5f7b 100644
--- a/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
+++ b/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
@@ -94,7 +94,8 @@ static void bus_reset(void)
   USBOEPCNT_0 &= ~NAK;
   USBIEPCNT_0 &= ~NAK;
 
-  USBCTL |= FEN; // Enable responding to packets.
+  // Enable responding to packets.
+  USBCTL |= FEN;
 
   // Dedicated buffers in hardware for SETUP and EP0, no setup needed.
   // Now safe to respond to SETUP packets.
@@ -103,6 +104,28 @@ static void bus_reset(void)
   USBKEYPID = 0;
 }
 
+// Controls reset behavior of the USB module on receipt of a bus reset event.
+// - enable: When true, bus reset events will cause a reset the USB module.
+static void enable_functional_reset(const bool enable)
+{
+  // Check whether or not the USB configuration registers were
+  // locked prior to this function being called so that, if
+  // necessary, the lock state can be restored on exit.
+  bool unlocked = (USBKEYPID == 0xA528) ? true : false;
+
+  if(!unlocked) USBKEYPID = USBKEY;
+
+  if(enable)
+  {
+    USBCTL |= FRSTE;
+  }
+  else
+  {
+    USBCTL &= ~FRSTE;
+  }
+
+  if(!unlocked) USBKEYPID = 0;
+}
 
 /*------------------------------------------------------------------*/
 /* Controller API
@@ -131,11 +154,14 @@ void dcd_init (uint8_t rhport)
 
   USBVECINT = 0;
 
-  // Enable reset and wait for it before continuing.
-  USBIE |= RSTRIE;
-
-  // Enable pullup.
-  USBCNF |= PUR_EN;
+  if(USBPWRCTL & USBBGVBV) {// Bus power detected?
+    USBPWRCTL |= VBOFFIE;   // Enable bus-power-removed interrupt.
+    USBIE |= RSTRIE;        // Enable reset and wait for it before continuing.
+    USBCNF |= PUR_EN;       // Enable pullup.
+  } else {
+    USBPWRCTL |= VBONIE;    // Enable bus-power-applied interrupt.
+    USBCNF &= ~USB_EN;      // Disable USB module until bus power is detected.
+  }
 
   USBKEYPID = 0;
 }
@@ -610,14 +636,76 @@ static void handle_setup_packet(void)
     _setup_packet[i] = setup_buf[i];
   }
 
-  // Clearing SETUPIFG by reading USBVECINT does not set NAK, so now that we
-  // have a SETUP packet, force NAKs until tinyusb can handle the SETUP
-  // packet and prepare for a new xfer.
+  // Force NAKs until tinyusb can handle the SETUP packet and prepare for a new xfer.
   USBIEPCNT_0 |= NAK;
   USBOEPCNT_0 |= NAK;
+
+  // Clear SETUPIFG to avoid handling in the USBVECINT switch statement.
+  // When handled there the NAKs applied to the endpoints above are
+  // cleared by hardware and the host will receive stale/duplicate data.
+  //
+  // Excerpt from MSP430x5xx and MSP430x6xx Family User's Guide:
+  //
+  // "...the SETUPIFG is cleared upon reading USBIV. In addition, the NAK on
+  // input endpoint 0 and output endpoint 0 is also cleared."
+  USBIEPCNF_0 &= ~UBME; // Errata USB10 workaround.
+  USBOEPCNF_0 &= ~UBME; // Errata USB10 workaround.
+  USBIFG &= ~SETUPIFG;
+  USBIEPCNF_0 |= UBME;  // Errata USB10 workaround.
+  USBOEPCNF_0 |= UBME;  // Errata USB10 workaround.
   dcd_event_setup_received(0, (uint8_t*) &_setup_packet[0], true);
 }
 
+#if CFG_TUSB_OS == OPT_OS_NONE
+TU_ATTR_ALWAYS_INLINE static inline void tu_delay(uint32_t ms) {
+  // msp430 can run up to 25Mhz -> 40ns per cycle. 1 ms = 25000 cycles
+  // each loop need 4 cycle: 1 sub, 1 cmp, 1 jump, 1 nop
+  volatile uint32_t cycles = (25000 * ms) >> 2;
+  while (cycles > 0) {
+    cycles--;
+    asm("nop");
+  }
+}
+#else
+#define tu_delay(ms) osal_task_delay(ms)
+#endif
+
+static void handle_bus_power_event(void *param) {
+  (void) param;
+
+  tu_delay(5);                 // Bus power settling delay.
+
+  USBKEYPID = USBKEY;
+
+  if(USBPWRCTL & USBBGVBV) {          // Event caused by application of bus power.
+    USBPWRCTL |= VBOFFIE;             // Enable bus-power-removed interrupt.
+    USBPLLDIVB = USBPLLDIVB;          // For some reason the PLL will *NOT* lock unless the divider
+                                      // register is re-written. The assumption here is that this
+                                      // register was already properly configured during board-level
+                                      // initialization.
+    USBPLLCTL |= (UPLLEN | UPFDEN);   // Enable the PLL.
+
+    uint16_t attempts = 0;
+    do {                              // Poll the PLL, checking for a successful lock.
+      USBPLLIR = 0;
+      tu_delay(1);
+      attempts++;
+    } while ((attempts < 10) && (USBPLLIR != 0));
+
+    // A successful lock is indicated by all PLL-related interrupt flags being cleared.
+    if(!USBPLLIR) {
+      dcd_init(0);                    // Re-initialize the USB module.
+    }
+  } else {                            // Event caused by removal of bus power.
+    USBPWRCTL |= VBONIE;              // Enable bus-power-applied interrupt.
+    USBPLLCTL &= ~(UPLLEN | UPFDEN);  // Disable the PLL.
+    USBCNF = 0;                       // Disable the USB module.
+    dcd_event_bus_signal(0, DCD_EVENT_UNPLUGGED, false);
+  }
+
+  USBKEYPID = 0;
+}
+
 void dcd_int_handler(uint8_t rhport)
 {
   (void) rhport;
@@ -628,6 +716,7 @@ void dcd_int_handler(uint8_t rhport)
 
   if(setup_status)
   {
+    enable_functional_reset(true);
     handle_setup_packet();
   }
 
@@ -646,11 +735,32 @@ void dcd_int_handler(uint8_t rhport)
 
   switch(curr_vector)
   {
+    case USBVECINT_NONE:
+      break;
+
     case USBVECINT_RSTR:
+      enable_functional_reset(false); // Errata USB4 workaround.
       bus_reset();
       dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
       break;
 
+    case USBVECINT_PWR_VBUSOn:
+    case USBVECINT_PWR_VBUSOff: {
+      USBKEYPID = USBKEY;
+      // Prevent (possibly) unstable power from generating spurious interrupts.
+      USBPWRCTL &= ~(VBONIE | VBOFFIE);
+      USBKEYPID = 0;
+
+      dcd_event_t event;
+
+      event.rhport = 0;
+      event.event_id = USBD_EVENT_FUNC_CALL;
+      event.func_call.func = handle_bus_power_event;
+
+      dcd_event_handler(&event, true);
+      }
+      break;
+
     // Clear the (hardware-enforced) NAK on EP 0 after a SETUP packet
     // is received. At this point, even though the hardware is no longer
     // forcing NAKs, the EP0 NAK bits should still be set to avoid
@@ -675,10 +785,12 @@ void dcd_int_handler(uint8_t rhport)
       break;
 
     case USBVECINT_INPUT_ENDPOINT0:
+      enable_functional_reset(true);
       transmit_packet(0);
       break;
 
     case USBVECINT_OUTPUT_ENDPOINT0:
+      enable_functional_reset(true);
       receive_packet(0);
       break;
 
@@ -710,7 +822,6 @@ void dcd_int_handler(uint8_t rhport)
 
     default:
       while(true);
-      break;
   }
 
 }
diff --git a/src/portable/wch/dcd_ch32_usbhs.c b/src/portable/wch/dcd_ch32_usbhs.c
index 1f1c0b876..68e2179e9 100644
--- a/src/portable/wch/dcd_ch32_usbhs.c
+++ b/src/portable/wch/dcd_ch32_usbhs.c
@@ -135,6 +135,16 @@ void dcd_remote_wakeup(uint8_t rhport)
   (void) rhport;
 }
 
+void dcd_sof_enable(uint8_t rhport, bool en)
+{
+    (void) rhport;
+    if (en) {
+        USBHSD->INT_EN |= USBHS_SOF_ACT_EN;
+    } else {
+        USBHSD->INT_EN &= ~(USBHS_SOF_ACT_EN);
+    }
+}
+
 void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const *request) {
     (void)rhport;
 
@@ -332,7 +342,10 @@ void dcd_int_handler(uint8_t rhport) {
 
         xfer_ctl_t *xfer = XFER_CTL_BASE(end_num, tu_edpt_dir(endp));
 
-        if (rx_token == PID_OUT) {
+        if (rx_token == PID_SOF) {
+            dcd_event_sof(rhport, USBHSD->FRAME_NO, true);
+
+        } else if (rx_token == PID_OUT) {
             uint16_t rx_len = USBHSD->RX_LEN;
 
             receive_packet(xfer, rx_len);
diff --git a/src/tusb.c b/src/tusb.c
index 7943ea5ef..0092267a1 100644
--- a/src/tusb.c
+++ b/src/tusb.c
@@ -43,32 +43,30 @@
 // Public API
 //--------------------------------------------------------------------+
 
-bool tusb_init(void)
-{
-#if CFG_TUD_ENABLED && defined(TUD_OPT_RHPORT)
+bool tusb_init(void) {
+  #if CFG_TUD_ENABLED && defined(TUD_OPT_RHPORT)
   // init device stack CFG_TUSB_RHPORTx_MODE must be defined
   TU_ASSERT ( tud_init(TUD_OPT_RHPORT) );
-#endif
+  #endif
 
-#if CFG_TUH_ENABLED && defined(TUH_OPT_RHPORT)
+  #if CFG_TUH_ENABLED && defined(TUH_OPT_RHPORT)
   // init host stack CFG_TUSB_RHPORTx_MODE must be defined
   TU_ASSERT( tuh_init(TUH_OPT_RHPORT) );
-#endif
+  #endif
 
   return true;
 }
 
-bool tusb_inited(void)
-{
+bool tusb_inited(void) {
   bool ret = false;
 
-#if CFG_TUD_ENABLED
+  #if CFG_TUD_ENABLED
   ret = ret || tud_inited();
-#endif
+  #endif
 
-#if CFG_TUH_ENABLED
+  #if CFG_TUH_ENABLED
   ret = ret || tuh_inited();
-#endif
+  #endif
 
   return ret;
 }
@@ -77,43 +75,35 @@ bool tusb_inited(void)
 // Descriptor helper
 //--------------------------------------------------------------------+
 
-uint8_t const * tu_desc_find(uint8_t const* desc, uint8_t const* end, uint8_t byte1)
-{
-  while(desc+1 < end)
-  {
-    if ( desc[1] == byte1 ) return desc;
+uint8_t const* tu_desc_find(uint8_t const* desc, uint8_t const* end, uint8_t byte1) {
+  while (desc + 1 < end) {
+    if (desc[1] == byte1) return desc;
     desc += desc[DESC_OFFSET_LEN];
   }
   return NULL;
 }
 
-uint8_t const * tu_desc_find2(uint8_t const* desc, uint8_t const* end, uint8_t byte1, uint8_t byte2)
-{
-  while(desc+2 < end)
-  {
-    if ( desc[1] == byte1 && desc[2] == byte2) return desc;
+uint8_t const* tu_desc_find2(uint8_t const* desc, uint8_t const* end, uint8_t byte1, uint8_t byte2) {
+  while (desc + 2 < end) {
+    if (desc[1] == byte1 && desc[2] == byte2) return desc;
     desc += desc[DESC_OFFSET_LEN];
   }
   return NULL;
 }
 
-uint8_t const * tu_desc_find3(uint8_t const* desc, uint8_t const* end, uint8_t byte1, uint8_t byte2, uint8_t byte3)
-{
-  while(desc+3 < end)
-  {
+uint8_t const* tu_desc_find3(uint8_t const* desc, uint8_t const* end, uint8_t byte1, uint8_t byte2, uint8_t byte3) {
+  while (desc + 3 < end) {
     if (desc[1] == byte1 && desc[2] == byte2 && desc[3] == byte3) return desc;
     desc += desc[DESC_OFFSET_LEN];
   }
   return NULL;
 }
 
-
 //--------------------------------------------------------------------+
 // Endpoint Helper for both Host and Device stack
 //--------------------------------------------------------------------+
 
-bool tu_edpt_claim(tu_edpt_state_t* ep_state, osal_mutex_t mutex)
-{
+bool tu_edpt_claim(tu_edpt_state_t* ep_state, osal_mutex_t mutex) {
   (void) mutex;
 
   // pre-check to help reducing mutex lock
@@ -122,111 +112,93 @@ bool tu_edpt_claim(tu_edpt_state_t* ep_state, osal_mutex_t mutex)
 
   // can only claim the endpoint if it is not busy and not claimed yet.
   bool const available = (ep_state->busy == 0) && (ep_state->claimed == 0);
-  if (available)
-  {
+  if (available) {
     ep_state->claimed = 1;
   }
 
   (void) osal_mutex_unlock(mutex);
-
   return available;
 }
 
-bool tu_edpt_release(tu_edpt_state_t* ep_state, osal_mutex_t mutex)
-{
+bool tu_edpt_release(tu_edpt_state_t* ep_state, osal_mutex_t mutex) {
   (void) mutex;
-
   (void) osal_mutex_lock(mutex, OSAL_TIMEOUT_WAIT_FOREVER);
 
   // can only release the endpoint if it is claimed and not busy
   bool const ret = (ep_state->claimed == 1) && (ep_state->busy == 0);
-  if (ret)
-  {
+  if (ret) {
     ep_state->claimed = 0;
   }
 
   (void) osal_mutex_unlock(mutex);
-
   return ret;
 }
 
-bool tu_edpt_validate(tusb_desc_endpoint_t const * desc_ep, tusb_speed_t speed)
-{
+bool tu_edpt_validate(tusb_desc_endpoint_t const* desc_ep, tusb_speed_t speed) {
   uint16_t const max_packet_size = tu_edpt_packet_size(desc_ep);
   TU_LOG2("  Open EP %02X with Size = %u\r\n", desc_ep->bEndpointAddress, max_packet_size);
 
-  switch (desc_ep->bmAttributes.xfer)
-  {
-    case TUSB_XFER_ISOCHRONOUS:
-    {
+  switch (desc_ep->bmAttributes.xfer) {
+    case TUSB_XFER_ISOCHRONOUS: {
       uint16_t const spec_size = (speed == TUSB_SPEED_HIGH ? 1024 : 1023);
       TU_ASSERT(max_packet_size <= spec_size);
+      break;
     }
-    break;
 
     case TUSB_XFER_BULK:
-      if (speed == TUSB_SPEED_HIGH)
-      {
+      if (speed == TUSB_SPEED_HIGH) {
         // Bulk highspeed must be EXACTLY 512
         TU_ASSERT(max_packet_size == 512);
-      }else
-      {
+      } else {
         // TODO Bulk fullspeed can only be 8, 16, 32, 64
         TU_ASSERT(max_packet_size <= 64);
       }
-    break;
+      break;
 
-    case TUSB_XFER_INTERRUPT:
-    {
+    case TUSB_XFER_INTERRUPT: {
       uint16_t const spec_size = (speed == TUSB_SPEED_HIGH ? 1024 : 64);
       TU_ASSERT(max_packet_size <= spec_size);
+      break;
     }
-    break;
 
-    default: return false;
+    default:
+      return false;
   }
 
   return true;
 }
 
-void tu_edpt_bind_driver(uint8_t ep2drv[][2], tusb_desc_interface_t const* desc_itf, uint16_t desc_len, uint8_t driver_id)
-{
+void tu_edpt_bind_driver(uint8_t ep2drv[][2], tusb_desc_interface_t const* desc_itf, uint16_t desc_len,
+                         uint8_t driver_id) {
   uint8_t const* p_desc = (uint8_t const*) desc_itf;
   uint8_t const* desc_end = p_desc + desc_len;
 
-  while( p_desc < desc_end )
-  {
-    if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) )
-    {
+  while (p_desc < desc_end) {
+    if (TUSB_DESC_ENDPOINT == tu_desc_type(p_desc)) {
       uint8_t const ep_addr = ((tusb_desc_endpoint_t const*) p_desc)->bEndpointAddress;
-
       TU_LOG(2, "  Bind EP %02x to driver id %u\r\n", ep_addr, driver_id);
       ep2drv[tu_edpt_number(ep_addr)][tu_edpt_dir(ep_addr)] = driver_id;
     }
-
     p_desc = tu_desc_next(p_desc);
   }
 }
 
-uint16_t tu_desc_get_interface_total_len(tusb_desc_interface_t const* desc_itf, uint8_t itf_count, uint16_t max_len)
-{
+uint16_t tu_desc_get_interface_total_len(tusb_desc_interface_t const* desc_itf, uint8_t itf_count, uint16_t max_len) {
   uint8_t const* p_desc = (uint8_t const*) desc_itf;
   uint16_t len = 0;
 
-  while (itf_count--)
-  {
+  while (itf_count--) {
     // Next on interface desc
     len += tu_desc_len(desc_itf);
     p_desc = tu_desc_next(p_desc);
 
-    while (len < max_len)
-    {
+    while (len < max_len) {
       // return on IAD regardless of itf count
-      if ( tu_desc_type(p_desc) == TUSB_DESC_INTERFACE_ASSOCIATION ) return len;
-
-      if ( (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE) &&
-           ((tusb_desc_interface_t const*) p_desc)->bAlternateSetting == 0 )
-      {
+      if (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE_ASSOCIATION) {
+        return len;
+      }
+      if ((tu_desc_type(p_desc) == TUSB_DESC_INTERFACE) &&
+          ((tusb_desc_interface_t const*) p_desc)->bAlternateSetting == 0) {
         break;
       }
 
@@ -243,9 +215,8 @@ uint16_t tu_desc_get_interface_total_len(tusb_desc_interface_t const* desc_itf,
 //--------------------------------------------------------------------+
 
 bool tu_edpt_stream_init(tu_edpt_stream_t* s, bool is_host, bool is_tx, bool overwritable,
-                         void* ff_buf, uint16_t ff_bufsize, uint8_t* ep_buf, uint16_t ep_bufsize)
-{
-  osal_mutex_t new_mutex = osal_mutex_create(&s->ff_mutex);
+                         void* ff_buf, uint16_t ff_bufsize, uint8_t* ep_buf, uint16_t ep_bufsize) {
+  osal_mutex_t new_mutex = osal_mutex_create(&s->ff_mutexdef);
   (void) new_mutex;
   (void) is_tx;
 
@@ -259,92 +230,82 @@ bool tu_edpt_stream_init(tu_edpt_stream_t* s, bool is_host, bool is_tx, bool ove
   return true;
 }
 
+bool tu_edpt_stream_deinit(tu_edpt_stream_t* s) {
+  (void) s;
+  #if OSAL_MUTEX_REQUIRED
+  if (s->ff.mutex_wr) osal_mutex_delete(s->ff.mutex_wr);
+  if (s->ff.mutex_rd) osal_mutex_delete(s->ff.mutex_rd);
+  #endif
+  return true;
+}
+
 TU_ATTR_ALWAYS_INLINE static inline
-bool stream_claim(tu_edpt_stream_t* s)
-{
-  if (s->is_host)
-  {
+bool stream_claim(tu_edpt_stream_t* s) {
+  if (s->is_host) {
     #if CFG_TUH_ENABLED
     return usbh_edpt_claim(s->daddr, s->ep_addr);
     #endif
-  }else
-  {
+  } else {
     #if CFG_TUD_ENABLED
     return usbd_edpt_claim(s->rhport, s->ep_addr);
     #endif
   }
-
   return false;
 }
 
 TU_ATTR_ALWAYS_INLINE static inline
-bool stream_xfer(tu_edpt_stream_t* s, uint16_t count)
-{
-  if (s->is_host)
-  {
+bool stream_xfer(tu_edpt_stream_t* s, uint16_t count) {
+  if (s->is_host) {
     #if CFG_TUH_ENABLED
     return usbh_edpt_xfer(s->daddr, s->ep_addr, count ? s->ep_buf : NULL, count);
     #endif
-  }else
-  {
+  } else {
     #if CFG_TUD_ENABLED
     return usbd_edpt_xfer(s->rhport, s->ep_addr, count ? s->ep_buf : NULL, count);
     #endif
   }
-
   return false;
 }
 
 TU_ATTR_ALWAYS_INLINE static inline
-bool stream_release(tu_edpt_stream_t* s)
-{
-  if (s->is_host)
-  {
+bool stream_release(tu_edpt_stream_t* s) {
+  if (s->is_host) {
     #if CFG_TUH_ENABLED
     return usbh_edpt_release(s->daddr, s->ep_addr);
     #endif
-  }else
-  {
+  } else {
     #if CFG_TUD_ENABLED
     return usbd_edpt_release(s->rhport, s->ep_addr);
     #endif
   }
-
   return false;
 }
 
 //--------------------------------------------------------------------+
 // Stream Write
 //--------------------------------------------------------------------+
-
-bool tu_edpt_stream_write_zlp_if_needed(tu_edpt_stream_t* s, uint32_t last_xferred_bytes)
-{
+bool tu_edpt_stream_write_zlp_if_needed(tu_edpt_stream_t* s, uint32_t last_xferred_bytes) {
   // ZLP condition: no pending data, last transferred bytes is multiple of packet size
-  TU_VERIFY( !tu_fifo_count(&s->ff) && last_xferred_bytes && (0 == (last_xferred_bytes & (s->ep_packetsize-1))) );
-
-  TU_VERIFY( stream_claim(s) );
-  TU_ASSERT( stream_xfer(s, 0) );
-
+  TU_VERIFY(!tu_fifo_count(&s->ff) && last_xferred_bytes && (0 == (last_xferred_bytes & (s->ep_packetsize - 1))));
+  TU_VERIFY(stream_claim(s));
+  TU_ASSERT(stream_xfer(s, 0));
   return true;
 }
 
-uint32_t tu_edpt_stream_write_xfer(tu_edpt_stream_t* s)
-{
+uint32_t tu_edpt_stream_write_xfer(tu_edpt_stream_t* s) {
   // skip if no data
-  TU_VERIFY( tu_fifo_count(&s->ff), 0 );
+  TU_VERIFY(tu_fifo_count(&s->ff), 0);
 
   // Claim the endpoint
-  TU_VERIFY( stream_claim(s), 0 );
+  TU_VERIFY(stream_claim(s), 0);
 
   // Pull data from FIFO -> EP buf
   uint16_t const count = tu_fifo_read_n(&s->ff, s->ep_buf, s->ep_bufsize);
 
-  if ( count )
-  {
-    TU_ASSERT( stream_xfer(s, count), 0 );
+  if (count) {
+    TU_ASSERT(stream_xfer(s, count), 0);
     return count;
-  }else
-  {
+  } else {
     // Release endpoint since we don't make any transfer
     // Note: data is dropped if terminal is not connected
     stream_release(s);
@@ -352,16 +313,13 @@ uint32_t tu_edpt_stream_write_xfer(tu_edpt_stream_t* s)
   }
 }
 
-uint32_t tu_edpt_stream_write(tu_edpt_stream_t* s, void const *buffer, uint32_t bufsize)
-{
+uint32_t tu_edpt_stream_write(tu_edpt_stream_t* s, void const* buffer, uint32_t bufsize) {
   TU_VERIFY(bufsize); // TODO support ZLP
-
   uint16_t ret = tu_fifo_write_n(&s->ff, buffer, (uint16_t) bufsize);
 
   // flush if fifo has more than packet size or
   // in rare case: fifo depth is configured too small (which never reach packet size)
-  if ( (tu_fifo_count(&s->ff) >= s->ep_packetsize) || (tu_fifo_depth(&s->ff) < s->ep_packetsize) )
-  {
+  if ((tu_fifo_count(&s->ff) >= s->ep_packetsize) || (tu_fifo_depth(&s->ff) < s->ep_packetsize)) {
     tu_edpt_stream_write_xfer(s);
   }
 
@@ -371,9 +329,7 @@ uint32_t tu_edpt_stream_write(tu_edpt_stream_t* s, void const *buffer, uint32_t
 //--------------------------------------------------------------------+
 // Stream Read
 //--------------------------------------------------------------------+
-
-uint32_t tu_edpt_stream_read_xfer(tu_edpt_stream_t* s)
-{
+uint32_t tu_edpt_stream_read_xfer(tu_edpt_stream_t* s) {
   uint16_t available = tu_fifo_remaining(&s->ff);
 
   // Prepare for incoming data but only allow what we can store in the ring buffer.
@@ -388,25 +344,21 @@ uint32_t tu_edpt_stream_read_xfer(tu_edpt_stream_t* s)
   // get available again since fifo can be changed before endpoint is claimed
   available = tu_fifo_remaining(&s->ff);
 
-  if ( available >= s->ep_packetsize )
-  {
+  if (available >= s->ep_packetsize) {
     // multiple of packet size limit by ep bufsize
-    uint16_t count = (uint16_t) (available & ~(s->ep_packetsize -1));
+    uint16_t count = (uint16_t) (available & ~(s->ep_packetsize - 1));
     count = tu_min16(count, s->ep_bufsize);
 
-    TU_ASSERT( stream_xfer(s, count), 0 );
-
+    TU_ASSERT(stream_xfer(s, count), 0);
     return count;
-  }else
-  {
+  } else {
     // Release endpoint since we don't make any transfer
     stream_release(s);
     return 0;
   }
 }
 
-uint32_t tu_edpt_stream_read(tu_edpt_stream_t* s, void* buffer, uint32_t bufsize)
-{
+uint32_t tu_edpt_stream_read(tu_edpt_stream_t* s, void* buffer, uint32_t bufsize) {
   uint32_t num_read = tu_fifo_read_n(&s->ff, buffer, (uint16_t) bufsize);
   tu_edpt_stream_read_xfer(s);
   return num_read;
@@ -420,42 +372,35 @@ uint32_t tu_edpt_stream_read(tu_edpt_stream_t* s, void* buffer, uint32_t bufsize
 #include <ctype.h>
 
 #if CFG_TUSB_DEBUG >= CFG_TUH_LOG_LEVEL || CFG_TUSB_DEBUG >= CFG_TUD_LOG_LEVEL
-
-char const* const tu_str_speed[] = { "Full", "Low", "High" };
-char const* const tu_str_std_request[] =
-{
-  "Get Status"        ,
-  "Clear Feature"     ,
-  "Reserved"          ,
-  "Set Feature"       ,
-  "Reserved"          ,
-  "Set Address"       ,
-  "Get Descriptor"    ,
-  "Set Descriptor"    ,
-  "Get Configuration" ,
-  "Set Configuration" ,
-  "Get Interface"     ,
-  "Set Interface"     ,
-  "Synch Frame"
+char const* const tu_str_speed[] = {"Full", "Low", "High"};
+char const* const tu_str_std_request[] = {
+    "Get Status",
+    "Clear Feature",
+    "Reserved",
+    "Set Feature",
+    "Reserved",
+    "Set Address",
+    "Get Descriptor",
+    "Set Descriptor",
+    "Get Configuration",
+    "Set Configuration",
+    "Get Interface",
+    "Set Interface",
+    "Synch Frame"
 };
 
 char const* const tu_str_xfer_result[] = {
     "OK", "FAILED", "STALLED", "TIMEOUT"
 };
-
 #endif
 
-static void dump_str_line(uint8_t const* buf, uint16_t count)
-{
+static void dump_str_line(uint8_t const* buf, uint16_t count) {
   tu_printf("  |");
-
   // each line is 16 bytes
-  for(uint16_t i=0; i<count; i++)
-  {
+  for (uint16_t i = 0; i < count; i++) {
     const char ch = buf[i];
     tu_printf("%c", isprint(ch) ? ch : '.');
   }
-
   tu_printf("|\r\n");
 }
 
@@ -464,39 +409,27 @@ static void dump_str_line(uint8_t const* buf, uint16_t count)
  *  - count : number of item
  *  - indent: prefix spaces on every line
  */
-void tu_print_mem(void const *buf, uint32_t count, uint8_t indent)
-{
+void tu_print_mem(void const* buf, uint32_t count, uint8_t indent) {
   uint8_t const size = 1; // fixed 1 byte for now
-
-  if ( !buf || !count )
-  {
+  if (!buf || !count) {
     tu_printf("NULL\r\n");
     return;
   }
 
-  uint8_t const *buf8 = (uint8_t const *) buf;
-
+  uint8_t const* buf8 = (uint8_t const*) buf;
   char format[] = "%00X";
-  format[2] += 2*size;
-
-  const uint8_t item_per_line  = 16 / size;
+  format[2] += (uint8_t) (2 * size); // 1 byte = 2 hex digits
+  const uint8_t item_per_line = 16 / size;
 
-  for(unsigned int i=0; i<count; i++)
-  {
-    unsigned int value=0;
+  for (unsigned int i = 0; i < count; i++) {
+    unsigned int value = 0;
 
-    if ( i%item_per_line == 0 )
-    {
+    if (i % item_per_line == 0) {
       // Print Ascii
-      if ( i != 0 )
-      {
-        dump_str_line(buf8-16, 16);
-      }
-
-      for(uint8_t s=0; s < indent; s++) tu_printf(" ");
-
+      if (i != 0) dump_str_line(buf8 - 16, 16);
+      for (uint8_t s = 0; s < indent; s++) tu_printf(" ");
       // print offset or absolute address
-      tu_printf("%04X: ", 16*i/item_per_line);
+      tu_printf("%04X: ", 16 * i / item_per_line);
     }
 
     tu_memcpy_s(&value, sizeof(value), buf8, size);
@@ -507,19 +440,16 @@ void tu_print_mem(void const *buf, uint32_t count, uint8_t indent)
   }
 
   // fill up last row to 16 for printing ascii
-  const uint32_t remain = count%16;
-  uint8_t nback = (uint8_t)(remain ? remain : 16);
-
-  if ( remain )
-  {
-    for(uint32_t i=0; i< 16-remain; i++)
-    {
+  const uint32_t remain = count % 16;
+  uint8_t nback = (uint8_t) (remain ? remain : 16);
+  if (remain) {
+    for (uint32_t i = 0; i < 16 - remain; i++) {
       tu_printf(" ");
-      for(int j=0; j<2*size; j++) tu_printf(" ");
+      for (int j = 0; j < 2 * size; j++) tu_printf(" ");
     }
   }
 
-  dump_str_line(buf8-nback, nback);
+  dump_str_line(buf8 - nback, nback);
 }
 
 #endif
diff --git a/src/tusb.h b/src/tusb.h
index c9d56d3c3..4f69a1414 100644
--- a/src/tusb.h
+++ b/src/tusb.h
@@ -38,8 +38,6 @@
 #include "osal/osal.h"
 #include "common/tusb_fifo.h"
 
-#include "class/hid/hid.h"
-
 //------------- TypeC -------------//
 #if CFG_TUC_ENABLED
   #include "typec/usbc.h"
diff --git a/src/tusb_option.h b/src/tusb_option.h
index a76281c0c..8d5527936 100644
--- a/src/tusb_option.h
+++ b/src/tusb_option.h
@@ -29,9 +29,14 @@
 
 #include "common/tusb_compiler.h"
 
+// Version is release as major.minor.revision eg 1.0.0. though there could be notable APIs before a new release.
+// For notable API changes within a release, we increase the build number.
 #define TUSB_VERSION_MAJOR     0
 #define TUSB_VERSION_MINOR     16
 #define TUSB_VERSION_REVISION  0
+#define TUSB_VERSION_BUILD     3
+
+#define TUSB_VERSION_NUMBER    (TUSB_VERSION_MAJOR << 24 | TUSB_VERSION_MINOR << 16 | TUSB_VERSION_REVISION << 8 | TUSB_VERSION_BUILD)
 #define TUSB_VERSION_STRING    TU_STRING(TUSB_VERSION_MAJOR) "." TU_STRING(TUSB_VERSION_MINOR) "." TU_STRING(TUSB_VERSION_REVISION)
 
 //--------------------------------------------------------------------+
@@ -50,7 +55,8 @@
 #define OPT_MCU_LPC18XX             6 ///< NXP LPC18xx
 #define OPT_MCU_LPC40XX             7 ///< NXP LPC40xx
 #define OPT_MCU_LPC43XX             8 ///< NXP LPC43xx
-#define OPT_MCU_LPC51UXX            9 ///< NXP LPC51U6x
+#define OPT_MCU_LPC51               9 ///< NXP LPC51
+#define OPT_MCU_LPC51UXX            OPT_MCU_LPC51 ///< NXP LPC51
 #define OPT_MCU_LPC54              10 ///< NXP LPC54
 #define OPT_MCU_LPC55              11 ///< NXP LPC55
 // legacy naming
@@ -114,6 +120,10 @@
 // Espressif
 #define OPT_MCU_ESP32S2           900 ///< Espressif ESP32-S2
 #define OPT_MCU_ESP32S3           901 ///< Espressif ESP32-S3
+#define OPT_MCU_ESP32             902 ///< Espressif ESP32 (for host max3421e)
+#define OPT_MCU_ESP32C3           903 ///< Espressif ESP32-C3
+#define OPT_MCU_ESP32C6           904 ///< Espressif ESP32-C6
+#define TUP_MCU_ESPRESSIF         (CFG_TUSB_MCU >= 900 && CFG_TUSB_MCU < 1000) // check if Espressif MCU
 
 // Dialog
 #define OPT_MCU_DA1469X          1000 ///< Dialog Semiconductor DA1469x
@@ -125,6 +135,7 @@
 #define OPT_MCU_KINETIS_KL       1200 ///< NXP KL series
 #define OPT_MCU_KINETIS_K32L     1201 ///< NXP K32L series
 #define OPT_MCU_KINETIS_K32      1201 ///< Alias to K32L
+#define OPT_MCU_KINETIS_K        1202 ///< NXP K series
 
 #define OPT_MCU_MKL25ZXX         1200 ///< Alias to KL (obsolete)
 #define OPT_MCU_K32L2BXX         1201 ///< Alias to K32 (obsolete)
@@ -138,7 +149,6 @@
 #define OPT_MCU_RX72N            1402 ///< Renesas RX72N
 #define OPT_MCU_RAXXX            1403 ///< Renesas RAxxx families
 
-
 // Mind Motion
 #define OPT_MCU_MM32F327X        1500 ///< Mind Motion MM32F327
 
@@ -175,6 +185,7 @@
 
 // NXP LPC MCX
 #define OPT_MCU_MCXN9            2300  ///< NXP MCX N9 Series
+#define OPT_MCU_MCXA15           2301  ///< NXP MCX A15 Series
 
 // Check if configured MCU is one of listed
 // Apply _TU_CHECK_MCU with || as separator to list of input
@@ -453,10 +464,12 @@
   #define CFG_TUH_CDC_FTDI 0
 #endif
 
-#ifndef CFG_TUH_CDC_FTDI_PID_LIST
-  // List of product IDs that can use the FTDI CDC driver
-  #define CFG_TUH_CDC_FTDI_PID_LIST \
-    0x6001, 0x6006, 0x6010, 0x6011, 0x6014, 0x6015, 0x8372, 0xFBFA, 0xCD18
+#ifndef CFG_TUH_CDC_FTDI_VID_PID_LIST
+  // List of product IDs that can use the FTDI CDC driver. 0x0403 is FTDI's VID
+  #define CFG_TUH_CDC_FTDI_VID_PID_LIST \
+    {0x0403, 0x6001}, {0x0403, 0x6006}, {0x0403, 0x6010}, {0x0403, 0x6011}, \
+    {0x0403, 0x6014}, {0x0403, 0x6015}, {0x0403, 0x8372}, {0x0403, 0xFBFA}, \
+    {0x0403, 0xCD18}
 #endif
 
 #ifndef CFG_TUH_CDC_CP210X
@@ -464,10 +477,27 @@
   #define CFG_TUH_CDC_CP210X 0
 #endif
 
-#ifndef CFG_TUH_CDC_CP210X_PID_LIST
-  // List of product IDs that can use the CP210X CDC driver
-  #define CFG_TUH_CDC_CP210X_PID_LIST \
-    0xEA60, 0xEA70
+#ifndef CFG_TUH_CDC_CP210X_VID_PID_LIST
+  // List of product IDs that can use the CP210X CDC driver. 0x10C4 is Silicon Labs' VID
+  #define CFG_TUH_CDC_CP210X_VID_PID_LIST \
+    {0x10C4, 0xEA60}, {0x10C4, 0xEA70}
+#endif
+
+#ifndef CFG_TUH_CDC_CH34X
+  // CH34X is not part of CDC class, only to re-use CDC driver API
+  #define CFG_TUH_CDC_CH34X 0
+#endif
+
+#ifndef CFG_TUH_CDC_CH34X_VID_PID_LIST
+  // List of product IDs that can use the CH34X CDC driver
+  #define CFG_TUH_CDC_CH34X_VID_PID_LIST \
+    { 0x1a86, 0x5523 }, /* ch341 chip */ \
+    { 0x1a86, 0x7522 }, /* ch340k chip */ \
+    { 0x1a86, 0x7523 }, /* ch340 chip */ \
+    { 0x1a86, 0xe523 }, /* ch330 chip */ \
+    { 0x4348, 0x5523 }, /* ch340 custom chip */ \
+    { 0x2184, 0x0057 }, /* overtaken from Linux Kernel driver /drivers/usb/serial/ch341.c */ \
+    { 0x9986, 0x7523 }  /* overtaken from Linux Kernel driver /drivers/usb/serial/ch341.c */
 #endif
 
 #ifndef CFG_TUH_HID