Merge branch 'master' into MCX

41 files changed, 2075 insertions, 2154 deletions

diff --git a/src/class/audio/audio_device.c b/src/class/audio/audio_device.c
index 4adc558a6..293defd96 100644
--- a/src/class/audio/audio_device.c
+++ b/src/class/audio/audio_device.c
@@ -96,8 +96,8 @@
   #define  USE_LINEAR_BUFFER     1
 #endif
 
-// Temporarily put the check here for stm32_fsdev
-#ifdef TUP_USBIP_FSDEV
+// Temporarily put the check here
+#if defined(TUP_USBIP_FSDEV) || defined(TUP_USBIP_DWC2)
   #define  USE_ISO_EP_ALLOCATION   1
 #else
   #define  USE_ISO_EP_ALLOCATION   0
diff --git a/src/class/cdc/cdc_host.c b/src/class/cdc/cdc_host.c
index 2fb37d835..5bdd41f1f 100644
--- a/src/class/cdc/cdc_host.c
+++ b/src/class/cdc/cdc_host.c
@@ -621,12 +621,11 @@ 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++) {
     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);
@@ -635,6 +634,17 @@ void cdch_init(void) {
                         p_cdc->stream.rx_ff_buf, CFG_TUH_CDC_RX_BUFSIZE,
                         p_cdc->stream.rx_ep_buf, CFG_TUH_CDC_RX_EPSIZE);
   }
+
+  return true;
+}
+
+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) {
diff --git a/src/class/cdc/cdc_host.h b/src/class/cdc/cdc_host.h
index d512a23a5..b63dd1530 100644
--- a/src/class/cdc/cdc_host.h
+++ b/src/class/cdc/cdc_host.h
@@ -192,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/hid/hid_host.c b/src/class/hid/hid_host.c
index 709fa3dd8..621fb2a55 100644
--- a/src/class/hid/hid_host.c
+++ b/src/class/hid/hid_host.c
@@ -39,12 +39,11 @@
 #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;
@@ -55,7 +54,7 @@ typedef struct
   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;
@@ -73,79 +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);
   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);
 
@@ -153,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;
 }
@@ -188,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);
   }
 }
@@ -219,123 +187,109 @@ 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 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);
@@ -346,68 +300,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
@@ -415,8 +361,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;
   }
@@ -427,13 +372,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);
@@ -442,30 +391,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;
-      p_hid->mounted = false;
+      if (tuh_hid_umount_cb) tuh_hid_umount_cb(daddr, i);
+      tu_memclr(p_hid, sizeof(hidh_interface_t));
     }
   }
 }
@@ -474,25 +419,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();
@@ -501,38 +443,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;
   }
 
@@ -553,15 +490,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
@@ -570,71 +506,67 @@ 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
@@ -648,21 +580,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;
@@ -672,114 +602,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..0902bf1af 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
@@ -107,7 +107,8 @@ bool tuh_hid_set_protocol(uint8_t dev_addr, uint8_t idx, uint8_t protocol);
 
 // 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 +122,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);
 
@@ -159,11 +163,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/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/video/video.h b/src/class/video/video.h
index 6319c6536..3b511af96 100644
--- a/src/class/video/video.h
+++ b/src/class/video/video.h
@@ -160,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 */
diff --git a/src/class/video/video_device.c b/src/class/video/video_device.c
index 1affd615e..5306356ba 100644
--- a/src/class/video/video_device.c
+++ b/src/class/video/video_device.c
@@ -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;
@@ -456,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  */
@@ -488,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;
@@ -512,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;
@@ -530,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;
@@ -653,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);
 }
@@ -736,6 +817,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;
@@ -752,6 +834,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)
@@ -789,7 +872,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:
@@ -853,19 +939,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;
@@ -876,6 +962,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:
@@ -891,8 +978,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);
       }
@@ -906,26 +992,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;
@@ -937,25 +1023,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;
 
@@ -963,19 +1047,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);
@@ -983,8 +1064,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);
           }
@@ -998,10 +1078,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;
@@ -1023,16 +1102,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);
@@ -1040,8 +1117,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);
           }
@@ -1142,8 +1218,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));
@@ -1154,8 +1229,7 @@ void videod_init(void)
   }
 }
 
-void videod_reset(uint8_t rhport)
-{
+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];
@@ -1167,8 +1241,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);
@@ -1215,7 +1288,7 @@ uint16_t videod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
        * 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);
       }
     }
@@ -1227,8 +1300,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);
@@ -1241,6 +1313,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;
@@ -1256,6 +1329,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;
@@ -1264,8 +1338,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/common/tusb_fifo.h b/src/common/tusb_fifo.h
index 2f60ec2f4..2d9f5e667 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,23 +140,18 @@ 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)
-{
-  f->mutex_wr = wr_mutex;
-  f->mutex_rd = rd_mutex;
-}
-
+  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) {
+    f->mutex_wr = wr_mutex;
+    f->mutex_rd = rd_mutex;
+  }
 #else
-
-#define tu_fifo_config_mutex(_f, _wr_mutex, _rd_mutex)
-
+  #define tu_fifo_config_mutex(_f, _wr_mutex, _rd_mutex)
 #endif
 
 bool     tu_fifo_write                  (tu_fifo_t* f, void const * p_data);
@@ -182,8 +173,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 +188,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 7b6da5f59..688b9a352 100644
--- a/src/common/tusb_mcu.h
+++ b/src/common/tusb_mcu.h
@@ -114,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
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/device/usbd.c b/src/device/usbd.c
index 5c94ebcc5..96d0b698b 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -68,9 +68,9 @@ 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;
 
   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
@@ -319,7 +319,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;
     }
   }
@@ -372,13 +372,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));
 
@@ -395,15 +395,13 @@ 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);
+  for (uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++) {
+    usbd_class_driver_t const* driver = get_driver(i);
     TU_ASSERT(driver);
     TU_LOG_USBD("%s init\r\n", driver->name);
     driver->init();
@@ -418,31 +416,26 @@ 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, );
+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);
 }
 
@@ -450,57 +443,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
@@ -509,81 +497,72 @@ 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:
       default:
         TU_BREAKPOINT();
-      break;
+        break;
     }
 
 #if CFG_TUSB_OS != OPT_OS_NONE && CFG_TUSB_OS != OPT_OS_PICO
@@ -598,8 +577,7 @@ 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);
@@ -607,15 +585,12 @@ static bool invoke_class_control(uint8_t rhport, usbd_class_driver_t const * dri
 
 // 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)
-{
+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);
@@ -623,19 +598,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));
 
@@ -646,15 +618,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
@@ -665,22 +635,18 @@ 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);
 
@@ -695,15 +661,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();
             }
           }
@@ -739,15 +701,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;
@@ -755,8 +716,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));
 
@@ -765,25 +725,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;
@@ -791,54 +747,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
 
@@ -854,14 +798,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;
@@ -1121,6 +1069,11 @@ TU_ATTR_FAST_FUNC void dcd_event_handler(dcd_event_t const* event, bool in_isr)
       // skip osal queue for SOF in usbd task
       break;
 
+    case DCD_EVENT_SETUP_RECEIVED:
+      _usbd_dev.setup_count++;
+      send = true;
+      break;
+
     default:
       send = true;
       break;
@@ -1186,8 +1139,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);
@@ -1196,37 +1148,34 @@ 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());
@@ -1240,11 +1189,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;
@@ -1258,12 +1205,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);
 
@@ -1274,12 +1220,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;
@@ -1289,26 +1233,23 @@ 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 )
-  {
+  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;
@@ -1316,16 +1257,14 @@ void usbd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
   }
 }
 
-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 )
-  {
+  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;
@@ -1333,31 +1272,27 @@ void usbd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
   }
 }
 
-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;
@@ -1367,8 +1302,7 @@ 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, 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.
@@ -1376,8 +1310,7 @@ void usbd_sof_enable(uint8_t rhport, bool en)
   dcd_sof_enable(rhport, en);
 }
 
-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);
@@ -1386,12 +1319,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..cf500143a 100644
--- a/src/device/usbd.h
+++ b/src/device/usbd.h
@@ -421,7 +421,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
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..aa0603d47 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
     }
@@ -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", 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));
+  TU_LOG_USBH("USBH init on controller %u\r\n", rhport);
 
-  // Event queue
-  _usbh_q = osal_queue_create( &_usbh_qdef );
-  TU_ASSERT(_usbh_q != NULL);
+  // 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);
 
 #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;
 
-  TU_ASSERT(hcd_init(controller_id));
-  hcd_int_enable(controller_id);
+  // "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) {
+        TU_LOG_USBH("%s deinit\r\n", driver->name);
+        driver->deinit();
+      }
+    }
+
+    osal_queue_delete(_usbh_q);
+    _usbh_q = NULL;
+
+    #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 = %lu\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..57362c778 100644
--- a/src/host/usbh.h
+++ b/src/host/usbh.h
@@ -73,11 +73,21 @@ 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 union {
+  // For TUH_CFGID_RPI_PIO_USB_CONFIGURATION use pio_usb_configuration_t
+
+  struct {
+    uint8_t max_nak;
+  } max3421;
+} tuh_configure_param_t;
+
 //--------------------------------------------------------------------+
 // APPLICATION CALLBACK
 //--------------------------------------------------------------------+
@@ -109,7 +119,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..46d13a907 100644
--- a/src/host/usbh_pvt.h
+++ b/src/host/usbh_pvt.h
@@ -46,11 +46,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..c954fcfe8 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
 
diff --git a/src/osal/osal_pico.h b/src/osal/osal_pico.h
index e6efa0968..00c589ef9 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,54 @@ 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);
+  critical_section_exit(&qhdl->critsec);
 
   TU_ASSERT(success);
-
   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 +159,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..61a7e2703 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,20 @@ 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 uint8_t _max_nak = MAX_NAK_DEFAULT;
+
 //--------------------------------------------------------------------+
 // API: SPI transfer with MAX3421E
 // - spi_cs_api(), spi_xfer_api(), int_api(): must be implemented by application
@@ -304,7 +323,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 +339,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 +377,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 +412,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) &&
+         (_max_nak == 0 || state < EP_STATE_ATTEMPT_1 + _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 +436,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 +451,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);
 
-  return false;
+  tuh_configure_param_t const* cfg = (tuh_configure_param_t const*) cfg_param;
+  _max_nak = cfg->max3421.max_nak;
+  return true;
 }
 
 // Initialize controller to host mode
@@ -438,6 +466,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
@@ -449,7 +478,7 @@ bool hcd_init(uint8_t rhport) {
   // full duplex, interrupt negative edge
   reg_write(rhport, PINCTL_ADDR, 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);
@@ -481,6 +510,24 @@ bool hcd_init(uint8_t rhport) {
   return true;
 }
 
+bool hcd_deinit(uint8_t rhport) {
+  (void) rhport;
+
+  // disable interrupt
+  tuh_max3421_int_api(rhport, false);
+
+  // reset max3421
+  reg_write(rhport, USBCTL_ADDR, USBCTL_CHIPRES, false);
+  reg_write(rhport, USBCTL_ADDR, 0, false);
+
+  #if OSAL_MUTEX_REQUIRED
+  osal_mutex_delete(_hcd_data.spi_mutex);
+  _hcd_data.spi_mutex = NULL;
+  #endif
+
+  return true;
+}
+
 // Enable USB interrupt
 // Not actually enable GPIO interrupt, just set variable to prevent handler to process
 void hcd_int_enable (uint8_t rhport) {
@@ -539,7 +586,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 +597,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 +607,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 +623,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 +635,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 = HXFR_HS | (ep->hxfr_bm.is_out ? HXFR_OUT_NIN : 0);
       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);
   }
 }
 
@@ -633,24 +677,21 @@ bool hcd_edpt_xfer(uint8_t rhport, uint8_t daddr, uint8_t ep_addr, uint8_t * buf
   TU_VERIFY(ep);
 
   // control transfer can switch direction
-  ep->ep_dir = ep_dir ? 1u : 0u;
+  ep->hxfr_bm.is_out = ep_dir ? 0u : 1u;
 
   ep->buf = buffer;
   ep->total_len = buflen;
   ep->xferred_len = 0;
-  ep->xfer_complete = 0;
-  ep->xfer_pending = 1;
+  ep->state = EP_STATE_ATTEMPT_1;
 
-  if ( ep_num == 0 ) {
-    ep->is_setup = 0;
+  if (ep_num == 0) {
+    ep->hxfr_bm.is_setup = 0;
     ep->data_toggle = 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 +714,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 +788,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 +834,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 +872,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 +913,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 +936,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 +963,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 +984,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/dcd_ci_fs.c b/src/portable/chipidea/ci_fs/dcd_ci_fs.c
index 9327e09d8..a68ecf8c9 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);
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/nordic/nrf5x/dcd_nrf5x.c b/src/portable/nordic/nrf5x/dcd_nrf5x.c
index 4e702aed4..cf9e5923b 100644
--- a/src/portable/nordic/nrf5x/dcd_nrf5x.c
+++ b/src/portable/nordic/nrf5x/dcd_nrf5x.c
@@ -246,7 +246,7 @@ static void xact_in_dma(uint8_t epnum)
 //--------------------------------------------------------------------+
 void dcd_init (uint8_t rhport)
 {
-  TU_LOG1("dcd init\r\n");
+  TU_LOG2("dcd init\r\n");
   (void) rhport;
 }
 
@@ -681,7 +681,7 @@ void dcd_int_handler(uint8_t rhport)
 
   if ( int_status & USBD_INTEN_USBEVENT_Msk )
   {
-    TU_LOG(2, "EVENTCAUSE = 0x%04lX\r\n", NRF_USBD->EVENTCAUSE);
+    TU_LOG(3, "EVENTCAUSE = 0x%04lX\r\n", NRF_USBD->EVENTCAUSE);
 
     enum { EVT_CAUSE_MASK = USBD_EVENTCAUSE_SUSPEND_Msk | USBD_EVENTCAUSE_RESUME_Msk | USBD_EVENTCAUSE_USBWUALLOWED_Msk };
     uint32_t const evt_cause = NRF_USBD->EVENTCAUSE & EVT_CAUSE_MASK;
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/raspberrypi/rp2040/dcd_rp2040.c b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
index e8cee73fd..5c564cb1c 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,24 @@ 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);
+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 +425,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 +436,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 +525,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..b351a9a07 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 )
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/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index 9c37f1f98..7bf726f3f 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -109,7 +109,7 @@
 #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"
+  #include "portable/st/stm32_fsdev/dcd_stm32_fsdev.h"
 #endif
 
 /*****************************************************
@@ -200,8 +200,7 @@ static bool dcd_read_packet_memory_ff(tu_fifo_t * ff, uint16_t src, uint16_t wNB
 // 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);
   // Fix -Werror=null-dereference
@@ -524,7 +523,7 @@ static void dcd_ep_ctr_tx_handler(uint32_t wIstr)
   xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
   if((xfer->total_len != xfer->queued_len)) /* TX not complete */
   {
-      dcd_transmit_packet(xfer, EPindex);
+    dcd_transmit_packet(xfer, EPindex);
   }
   else /* TX Complete */
   {
@@ -533,10 +532,29 @@ static void dcd_ep_ctr_tx_handler(uint32_t wIstr)
 }
 
 // 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)
-{
+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 40 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 2 cycle (1 cycle for sub, 1 cycle for compare/jump)
+  }
+  #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,8 +563,7 @@ 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;
   }
 
@@ -633,26 +650,22 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
   // (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...
+    // Always be prepared for a status packet...
     pcd_set_ep_rx_bufsize(USB, EPindex, CFG_TUD_ENDPOINT0_SIZE);
     pcd_clear_rx_ep_ctr(USB, EPindex);
   }
 }
 
-static void dcd_ep_ctr_handler(void)
-{
+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);
     }
   }
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..946ad2c7c 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;
@@ -310,8 +305,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 +313,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 +322,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 +331,7 @@ 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)
-{
+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 +340,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 +349,7 @@ 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)
-{
+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 +359,7 @@ 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)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_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 +369,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 +381,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,13 +395,11 @@ 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_tx_bufsize(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_bufsize(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount) {
   pcd_set_ep_bufsize(USBx, 2*bEpIdx + 1, wCount);
 }
 
@@ -425,8 +410,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 +427,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 +437,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 +466,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 +486,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 +506,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/synopsys/dwc2/dcd_dwc2.c b/src/portable/synopsys/dwc2/dcd_dwc2.c
index 3e15d51c6..cf8f3be50 100644
--- a/src/portable/synopsys/dwc2/dcd_dwc2.c
+++ b/src/portable/synopsys/dwc2/dcd_dwc2.c
@@ -102,7 +102,13 @@ static bool _out_ep_closed;                   // Flag to check if RX FIFO size n
 // 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;
 }
@@ -121,6 +127,141 @@ static void update_grxfsiz(uint8_t rhport) {
   dwc2->grxfsiz = calc_grxfsiz(max_epsize, ep_count);
 }
 
+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);
+
+  TU_ASSERT(epnum < ep_count);
+
+  uint16_t const 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 {
+    // 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);
+  }
+
+  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);
+
+  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;
+    }
+  }
+}
+
 // Start of Bus Reset
 static void bus_reset(uint8_t rhport) {
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
@@ -139,6 +280,14 @@ static void bus_reset(uint8_t rhport) {
     dwc2->epout[n].doepctl |= DOEPCTL_SNAK;
   }
 
+  // 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) {}
+
   // 2. Set up interrupt mask
   dwc2->daintmsk = TU_BIT(DAINTMSK_OEPM_Pos) | TU_BIT(DAINTMSK_IEPM_Pos);
   dwc2->doepmsk = DOEPMSK_STUPM | DOEPMSK_XFRCM;
@@ -269,7 +418,6 @@ 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
@@ -280,7 +428,6 @@ void print_dwc2_info(dwc2_regs_t* dwc2) {
   }
   TU_LOG(DWC2_DEBUG, "0x%08lX\r\n", p[5]);
 }
-
 #endif
 
 static void reset_core(dwc2_regs_t* dwc2) {
@@ -462,9 +609,7 @@ 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;
 
   // Enable global interrupt
@@ -547,84 +692,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;
 }
 
@@ -650,6 +719,20 @@ void dcd_edpt_close_all(uint8_t rhport) {
   _allocated_fifo_words_tx = 16;
 }
 
+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) {
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir = tu_edpt_dir(ep_addr);
@@ -707,71 +790,13 @@ 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);
+  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
@@ -789,7 +814,7 @@ void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr) {
 }
 
 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) {
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_option.h b/src/tusb_option.h
index e46e641f3..2921d3ae0 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     2
+
+#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)
 
 //--------------------------------------------------------------------+
@@ -125,6 +130,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 +144,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