Merge branch 'master' into pr/2181

6 files changed, 762 insertions, 704 deletions

diff --git a/src/host/hcd.h b/src/host/hcd.h
index 50f1dcdcc..5547c7cc5 100644
--- a/src/host/hcd.h
+++ b/src/host/hcd.h
@@ -39,7 +39,7 @@
 // Configuration
 //--------------------------------------------------------------------+
 
-// Max number of endpoints per device
+// Max number of endpoints pair per device
 // TODO optimize memory usage
 #ifndef CFG_TUH_ENDPOINT_MAX
   #define CFG_TUH_ENDPOINT_MAX   16
@@ -125,13 +125,16 @@ 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);
+void hcd_int_handler(uint8_t rhport, bool in_isr);
 
 // Enable USB interrupt
 void hcd_int_enable (uint8_t rhport);
@@ -149,10 +152,11 @@ uint32_t hcd_frame_number(uint8_t rhport);
 // Get the current connect status of roothub port
 bool hcd_port_connect_status(uint8_t rhport);
 
-// Reset USB bus on the port
+// Reset USB bus on the port. Return immediately, bus reset sequence may not be complete.
+// Some port would require hcd_port_reset_end() to be invoked after 10ms to complete the reset sequence.
 void hcd_port_reset(uint8_t rhport);
 
-// TODO implement later
+// Complete bus reset sequence, may be required by some controllers
 void hcd_port_reset_end(uint8_t rhport);
 
 // Get port link speed
@@ -166,20 +170,20 @@ void hcd_device_close(uint8_t rhport, uint8_t dev_addr);
 //--------------------------------------------------------------------+
 
 // Open an endpoint
-bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const * ep_desc);
+bool hcd_edpt_open(uint8_t rhport, uint8_t daddr, tusb_desc_endpoint_t const * ep_desc);
 
 // Submit a transfer, when complete hcd_event_xfer_complete() must be invoked
-bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t * buffer, uint16_t buflen);
+bool hcd_edpt_xfer(uint8_t rhport, uint8_t daddr, uint8_t ep_addr, uint8_t * buffer, uint16_t buflen);
 
 // Abort a queued transfer. Note: it can only abort transfer that has not been started
 // Return true if a queued transfer is aborted, false if there is no transfer to abort
 bool hcd_edpt_abort_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr);
 
 // Submit a special transfer to send 8-byte Setup Packet, when complete hcd_event_xfer_complete() must be invoked
-bool hcd_setup_send(uint8_t rhport, uint8_t dev_addr, uint8_t const setup_packet[8]);
+bool hcd_setup_send(uint8_t rhport, uint8_t daddr, uint8_t const setup_packet[8]);
 
 // clear stall, data toggle is also reset to DATA0
-bool hcd_edpt_clear_stall(uint8_t daddr, uint8_t ep_addr);
+bool hcd_edpt_clear_stall(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr);
 
 //--------------------------------------------------------------------+
 // USBH implemented API
@@ -197,8 +201,7 @@ extern void hcd_event_handler(hcd_event_t const* event, bool in_isr);
 
 // Helper to send device attach event
 TU_ATTR_ALWAYS_INLINE static inline
-void hcd_event_device_attach(uint8_t rhport, bool in_isr)
-{
+void hcd_event_device_attach(uint8_t rhport, bool in_isr) {
   hcd_event_t event;
   event.rhport              = rhport;
   event.event_id            = HCD_EVENT_DEVICE_ATTACH;
@@ -210,8 +213,7 @@ void hcd_event_device_attach(uint8_t rhport, bool in_isr)
 
 // Helper to send device removal event
 TU_ATTR_ALWAYS_INLINE static inline
-void hcd_event_device_remove(uint8_t rhport, bool in_isr)
-{
+void hcd_event_device_remove(uint8_t rhport, bool in_isr) {
   hcd_event_t event;
   event.rhport              = rhport;
   event.event_id            = HCD_EVENT_DEVICE_REMOVE;
@@ -223,10 +225,8 @@ void hcd_event_device_remove(uint8_t rhport, bool in_isr)
 
 // Helper to send USB transfer event
 TU_ATTR_ALWAYS_INLINE static inline
-void hcd_event_xfer_complete(uint8_t dev_addr, uint8_t ep_addr, uint32_t xferred_bytes, xfer_result_t result, bool in_isr)
-{
-  hcd_event_t event =
-  {
+void hcd_event_xfer_complete(uint8_t dev_addr, uint8_t ep_addr, uint32_t xferred_bytes, xfer_result_t result, bool in_isr) {
+  hcd_event_t event = {
     .rhport   = 0, // TODO correct rhport
     .event_id = HCD_EVENT_XFER_COMPLETE,
     .dev_addr = dev_addr,
diff --git a/src/host/hub.c b/src/host/hub.c
index 182bd6ce8..e97014443 100644
--- a/src/host/hub.c
+++ b/src/host/hub.c
@@ -30,7 +30,7 @@
 
 #include "hcd.h"
 #include "usbh.h"
-#include "usbh_classdriver.h"
+#include "usbh_pvt.h"
 #include "hub.h"
 
 // Debug level, TUSB_CFG_DEBUG must be at least this level for debug message
@@ -45,8 +45,8 @@ typedef struct
   uint8_t itf_num;
   uint8_t ep_in;
   uint8_t port_count;
-  uint8_t status_change; // data from status change interrupt endpoint
 
+  CFG_TUH_MEM_ALIGN uint8_t status_change;
   CFG_TUH_MEM_ALIGN hub_port_status_response_t port_status;
   CFG_TUH_MEM_ALIGN hub_status_response_t hub_status;
 } hub_interface_t;
@@ -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)
@@ -330,7 +334,7 @@ static void connection_port_reset_complete (tuh_xfer_t* xfer);
 bool hub_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
   (void) xferred_bytes; // TODO can be more than 1 for hub with lots of ports
   (void) ep_addr;
-  TU_ASSERT(result == XFER_RESULT_SUCCESS);
+  TU_VERIFY(result == XFER_RESULT_SUCCESS);
 
   hub_interface_t* p_hub = get_itf(dev_addr);
 
@@ -435,9 +439,12 @@ static void hub_port_get_status_complete (tuh_xfer_t* xfer)
     // Other changes are: L1 state
     // TODO clear change
 
-    // prepare for next hub status
-    // TODO continue with status_change, or maybe we can do it again with status
-    hub_edpt_status_xfer(daddr);
+    else
+    {
+      // prepare for next hub status
+      // TODO continue with status_change, or maybe we can do it again with status
+      hub_edpt_status_xfer(daddr);
+    }
   }
 }
 
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 15c48be47..7a47f5056 100644
--- a/src/host/usbh.c
+++ b/src/host/usbh.c
@@ -30,13 +30,12 @@
 
 #include "host/hcd.h"
 #include "tusb.h"
-#include "host/usbh_classdriver.h"
+#include "host/usbh_pvt.h"
 #include "hub.h"
 
 //--------------------------------------------------------------------+
 // USBH Configuration
 //--------------------------------------------------------------------+
-
 #ifndef CFG_TUH_TASK_QUEUE_SZ
   #define CFG_TUH_TASK_QUEUE_SZ   16
 #endif
@@ -45,33 +44,41 @@
   #define CFG_TUH_INTERFACE_MAX   8
 #endif
 
-// Level where CFG_TUSB_DEBUG must be at least for USBH is logged
-#ifndef CFG_TUH_LOG_LEVEL
-  #define CFG_TUH_LOG_LEVEL   2
-#endif
+//--------------------------------------------------------------------+
+// Weak stubs: invoked if no strong implementation is available
+//--------------------------------------------------------------------+
+TU_ATTR_WEAK bool hcd_deinit(uint8_t rhport) {
+  (void) rhport;
+  return false;
+}
 
-#define TU_LOG_USBH(...)   TU_LOG(CFG_TUH_LOG_LEVEL, __VA_ARGS__)
+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;
+  (void) in_isr;
+}
 
 //--------------------------------------------------------------------+
 // USBH-HCD common data structure
 //--------------------------------------------------------------------+
-
-typedef struct
-{
+typedef struct {
   // port
   uint8_t rhport;
   uint8_t hub_addr;
   uint8_t hub_port;
-  uint8_t speed;
-
-  // enumeration is in progress, done when all interfaces are configured
-  volatile uint8_t enumerating;
 
-//  struct TU_ATTR_PACKED {
-//    uint8_t          speed       : 4; // packed speed to save footprint
-//    volatile uint8_t enumerating : 1;
-//    uint8_t TU_RESERVED : 3;
-//  };
+  struct TU_ATTR_PACKED {
+             uint8_t speed       : 4; // packed speed to save footprint
+    volatile uint8_t enumerating : 1; // enumeration is in progress, false if not connected or all interfaces are configured
+    uint8_t TU_RESERVED : 3;
+  };
 } usbh_dev0_t;
 
 typedef struct {
@@ -123,76 +130,94 @@ typedef struct {
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-
-#if CFG_TUSB_DEBUG >= 2
-  #define DRIVER_NAME(_name)    .name = _name,
+#if CFG_TUSB_DEBUG >= CFG_TUH_LOG_LEVEL
+  #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
+static usbh_class_driver_t const usbh_class_drivers[] = {
+    #if CFG_TUH_CDC
     {
-      DRIVER_NAME("CDC")
-      .init       = cdch_init,
-      .open       = cdch_open,
-      .set_config = cdch_set_config,
-      .xfer_cb    = cdch_xfer_cb,
-      .close      = cdch_close
+        .name       = DRIVER_NAME("CDC"),
+        .init       = cdch_init,
+        .deinit     = cdch_deinit,
+        .open       = cdch_open,
+        .set_config = cdch_set_config,
+        .xfer_cb    = cdch_xfer_cb,
+        .close      = cdch_close
     },
-  #endif
+    #endif
 
-  #if CFG_TUH_MSC
+    #if CFG_TUH_MSC
     {
-      DRIVER_NAME("MSC")
-      .init       = msch_init,
-      .open       = msch_open,
-      .set_config = msch_set_config,
-      .xfer_cb    = msch_xfer_cb,
-      .close      = msch_close
+        .name       = DRIVER_NAME("MSC"),
+        .init       = msch_init,
+        .deinit     = msch_deinit,
+        .open       = msch_open,
+        .set_config = msch_set_config,
+        .xfer_cb    = msch_xfer_cb,
+        .close      = msch_close
     },
-  #endif
+    #endif
 
-  #if CFG_TUH_HID
+    #if CFG_TUH_HID
     {
-      DRIVER_NAME("HID")
-      .init       = hidh_init,
-      .open       = hidh_open,
-      .set_config = hidh_set_config,
-      .xfer_cb    = hidh_xfer_cb,
-      .close      = hidh_close
+        .name       = DRIVER_NAME("HID"),
+        .init       = hidh_init,
+        .deinit     = hidh_deinit,
+        .open       = hidh_open,
+        .set_config = hidh_set_config,
+        .xfer_cb    = hidh_xfer_cb,
+        .close      = hidh_close
     },
-  #endif
+    #endif
 
-  #if CFG_TUH_HUB
+    #if CFG_TUH_HUB
     {
-      DRIVER_NAME("HUB")
-      .init       = hub_init,
-      .open       = hub_open,
-      .set_config = hub_set_config,
-      .xfer_cb    = hub_xfer_cb,
-      .close      = hub_close
+        .name       = DRIVER_NAME("HUB"),
+        .init       = hub_init,
+        .deinit     = hub_deinit,
+        .open       = hub_open,
+        .set_config = hub_set_config,
+        .xfer_cb    = hub_xfer_cb,
+        .close      = hub_close
     },
-  #endif
+    #endif
 
-  #if CFG_TUH_VENDOR
+    #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
     }
-  #endif
+    #endif
 };
 
-enum { USBH_CLASS_DRIVER_COUNT = TU_ARRAY_SIZE(usbh_class_drivers) };
+enum { BUILTIN_DRIVER_COUNT = TU_ARRAY_SIZE(usbh_class_drivers) };
+enum { CONFIG_NUM = 1 }; // default to use configuration 1
 
-enum { RESET_DELAY = 500 };  // 200 USB specs say only 50ms but many devices require much longer
+// Additional class drivers implemented by application
+tu_static usbh_class_driver_t const * _app_driver = NULL;
+tu_static uint8_t _app_driver_count = 0;
 
-enum { CONFIG_NUM = 1 }; // default to use configuration 1
+#define TOTAL_DRIVER_COUNT    (_app_driver_count + BUILTIN_DRIVER_COUNT)
+
+static inline usbh_class_driver_t const *get_driver(uint8_t drv_id) {
+  usbh_class_driver_t const *driver = NULL;
 
+  if ( drv_id < _app_driver_count ) {
+    driver = &_app_driver[drv_id];
+  } else if ( drv_id < TOTAL_DRIVER_COUNT && BUILTIN_DRIVER_COUNT > 0) {
+    driver = &usbh_class_drivers[drv_id - _app_driver_count];
+  }
+
+  return driver;
+}
 
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
@@ -230,8 +255,7 @@ static uint8_t _usbh_ctrl_buf[CFG_TUH_ENUMERATION_BUFSIZE];
 // Control transfers: since most controllers do not support multiple control transfers
 // on multiple devices concurrently and control transfers are not used much except for
 // enumeration, we will only execute control transfers one at a time.
-CFG_TUH_MEM_SECTION struct
-{
+CFG_TUH_MEM_SECTION struct {
   CFG_TUH_MEM_ALIGN tusb_control_request_t request;
   uint8_t* buffer;
   tuh_xfer_cb_t complete_cb;
@@ -244,9 +268,7 @@ CFG_TUH_MEM_SECTION struct
 
 //------------- Helper Function -------------//
 
-TU_ATTR_ALWAYS_INLINE
-static inline usbh_device_t* get_device(uint8_t dev_addr)
-{
+TU_ATTR_ALWAYS_INLINE static inline usbh_device_t* get_device(uint8_t dev_addr) {
   TU_VERIFY(dev_addr > 0 && dev_addr <= TOTAL_DEVICES, NULL);
   return &_usbh_devices[dev_addr-1];
 }
@@ -258,40 +280,33 @@ static bool usbh_control_xfer_cb (uint8_t daddr, uint8_t ep_addr, xfer_result_t
 
 #if CFG_TUSB_OS == OPT_OS_NONE
 // TODO rework time-related function later
-void osal_task_delay(uint32_t msec)
-{
+// weak and overridable
+TU_ATTR_WEAK void osal_task_delay(uint32_t msec) {
   const uint32_t start = hcd_frame_number(_usbh_controller);
   while ( ( hcd_frame_number(_usbh_controller) - start ) < msec ) {}
 }
 #endif
 
+TU_ATTR_ALWAYS_INLINE static inline bool queue_event(hcd_event_t const * event, bool in_isr) {
+  TU_ASSERT(osal_queue_send(_usbh_q, event, in_isr));
+  tuh_event_hook_cb(event->rhport, event->event_id, in_isr);
+  return true;
+}
+
 //--------------------------------------------------------------------+
-// PUBLIC API (Parameter Verification is required)
+// Device API
 //--------------------------------------------------------------------+
 
-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;
-  }
-}
-
-bool tuh_mounted(uint8_t dev_addr)
-{
-  usbh_device_t* dev = get_device(dev_addr);
+bool tuh_mounted(uint8_t dev_addr) {
+  usbh_device_t *dev = get_device(dev_addr);
   TU_VERIFY(dev);
   return dev->configured;
 }
 
-bool tuh_vid_pid_get(uint8_t dev_addr, uint16_t* vid, uint16_t* pid)
-{
+bool tuh_vid_pid_get(uint8_t dev_addr, uint16_t *vid, uint16_t *pid) {
   *vid = *pid = 0;
 
-  usbh_device_t const* dev = get_device(dev_addr);
+  usbh_device_t const *dev = get_device(dev_addr);
   TU_VERIFY(dev && dev->addressed && dev->vid != 0);
 
   *vid = dev->vid;
@@ -300,74 +315,136 @@ bool tuh_vid_pid_get(uint8_t dev_addr, uint16_t* vid, uint16_t* pid)
   return true;
 }
 
-tusb_speed_t tuh_speed_get (uint8_t dev_addr)
-{
-  usbh_device_t* dev = get_device(dev_addr);
+tusb_speed_t tuh_speed_get(uint8_t dev_addr) {
+  usbh_device_t *dev = get_device(dev_addr);
   return (tusb_speed_t) (dev ? get_device(dev_addr)->speed : _dev0.speed);
 }
 
-static void clear_device(usbh_device_t* dev)
-{
+bool tuh_rhport_is_active(uint8_t rhport) {
+  return _usbh_controller == rhport;
+}
+
+bool tuh_rhport_reset_bus(uint8_t rhport, bool active) {
+  TU_VERIFY(tuh_rhport_is_active(rhport));
+  if ( active ) {
+    hcd_port_reset(rhport);
+  } else {
+    hcd_port_reset_end(rhport);
+  }
+  return true;
+}
+
+//--------------------------------------------------------------------+
+// PUBLIC API (Parameter Verification is required)
+//--------------------------------------------------------------------+
+
+bool tuh_configure(uint8_t rhport, uint32_t cfg_id, const void *cfg_param) {
+  return hcd_configure(rhport, cfg_id, cfg_param);
+}
+
+static void clear_device(usbh_device_t* dev) {
   tu_memclr(dev, sizeof(usbh_device_t));
   memset(dev->itf2drv, TUSB_INDEX_INVALID_8, sizeof(dev->itf2drv)); // invalid mapping
   memset(dev->ep2drv , TUSB_INDEX_INVALID_8, sizeof(dev->ep2drv )); // invalid mapping
 }
 
-bool tuh_inited(void)
-{
+bool tuh_inited(void) {
   return _usbh_controller != TUSB_INDEX_INVALID_8;
 }
 
-bool tuh_init(uint8_t controller_id)
-{
+bool tuh_init(uint8_t rhport) {
   // skip if already initialized
-  if ( tuh_inited() ) return true;
+  if (tuh_rhport_is_active(rhport)) return true;
+
+  TU_LOG_USBH("USBH init on controller %u\r\n", rhport);
 
-  TU_LOG_USBH("USBH init on controller %u\r\n", controller_id);
-  TU_LOG_INT(CFG_TUH_LOG_LEVEL, sizeof(usbh_device_t));
-  TU_LOG_INT(CFG_TUH_LOG_LEVEL, sizeof(hcd_event_t));
-  TU_LOG_INT(CFG_TUH_LOG_LEVEL, sizeof(_ctrl_xfer));
-  TU_LOG_INT(CFG_TUH_LOG_LEVEL, sizeof(tuh_xfer_t));
-  TU_LOG_INT(CFG_TUH_LOG_LEVEL, sizeof(tu_fifo_t));
-  TU_LOG_INT(CFG_TUH_LOG_LEVEL, sizeof(tu_edpt_stream_t));
+  // Init host stack if not already
+  if (!tuh_inited()) {
+    TU_LOG_INT_USBH(sizeof(usbh_device_t));
+    TU_LOG_INT_USBH(sizeof(hcd_event_t));
+    TU_LOG_INT_USBH(sizeof(_ctrl_xfer));
+    TU_LOG_INT_USBH(sizeof(tuh_xfer_t));
+    TU_LOG_INT_USBH(sizeof(tu_fifo_t));
+    TU_LOG_INT_USBH(sizeof(tu_edpt_stream_t));
 
-  // Event queue
-  _usbh_q = osal_queue_create( &_usbh_qdef );
-  TU_ASSERT(_usbh_q != NULL);
+    // Event queue
+    _usbh_q = osal_queue_create(&_usbh_qdef);
+    TU_ASSERT(_usbh_q != NULL);
 
 #if OSAL_MUTEX_REQUIRED
-  // Init mutex
-  _usbh_mutex = osal_mutex_create(&_usbh_mutexdef);
-  TU_ASSERT(_usbh_mutex);
+    // Init mutex
+    _usbh_mutex = osal_mutex_create(&_usbh_mutexdef);
+    TU_ASSERT(_usbh_mutex);
 #endif
 
-  // Device
-  tu_memclr(&_dev0, sizeof(_dev0));
-  tu_memclr(_usbh_devices, sizeof(_usbh_devices));
-  tu_memclr(&_ctrl_xfer, sizeof(_ctrl_xfer));
+    // Get application driver if available
+    if (usbh_app_driver_get_cb) {
+      _app_driver = usbh_app_driver_get_cb(&_app_driver_count);
+    }
 
-  for(uint8_t i=0; i<TOTAL_DEVICES; i++)
-  {
-    clear_device(&_usbh_devices[i]);
-  }
+    // Device
+    tu_memclr(&_dev0, sizeof(_dev0));
+    tu_memclr(_usbh_devices, sizeof(_usbh_devices));
+    tu_memclr(&_ctrl_xfer, sizeof(_ctrl_xfer));
 
-  // Class drivers
-  for (uint8_t drv_id = 0; drv_id < USBH_CLASS_DRIVER_COUNT; drv_id++)
-  {
-    TU_LOG_USBH("%s init\r\n", usbh_class_drivers[drv_id].name);
-    usbh_class_drivers[drv_id].init();
+    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();
+      }
+    }
   }
 
-  _usbh_controller = controller_id;;
+  // Init host controller
+  _usbh_controller = rhport;;
+  TU_ASSERT(hcd_init(rhport));
+  hcd_int_enable(rhport);
 
-  TU_ASSERT(hcd_init(controller_id));
-  hcd_int_enable(controller_id);
+  return true;
+}
+
+bool tuh_deinit(uint8_t rhport) {
+  if (!tuh_rhport_is_active(rhport)) return true;
+
+  // deinit host controller
+  hcd_int_disable(rhport);
+  hcd_deinit(rhport);
+  _usbh_controller = TUSB_INDEX_INVALID_8;
+
+  // "unplug" all devices on this rhport (hub_addr = 0, hub_port = 0)
+  process_removing_device(rhport, 0, 0);
+
+  // deinit host stack if no controller is active
+  if (!tuh_inited()) {
+    // Class drivers
+    for (uint8_t drv_id = 0; drv_id < TOTAL_DRIVER_COUNT; drv_id++) {
+      usbh_class_driver_t const* driver = get_driver(drv_id);
+      if (driver && driver->deinit) {
+        TU_LOG_USBH("%s deinit\r\n", driver->name);
+        driver->deinit();
+      }
+    }
+
+    osal_queue_delete(_usbh_q);
+    _usbh_q = NULL;
+
+    #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;
 }
 
-bool tuh_task_event_ready(void)
-{
+bool tuh_task_event_ready(void) {
   // Skip if stack is not initialized
   if ( !tuh_inited() ) return false;
 
@@ -392,42 +469,37 @@ bool tuh_task_event_ready(void)
     }
     @endcode
  */
-void tuh_task_ext(uint32_t timeout_ms, bool in_isr)
-{
+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.
-        if ( _dev0.enumerating )
-        {
+        // 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) {
           TU_LOG_USBH("[%u:] USBH Defer Attach until current enumeration complete\r\n", event.rhport);
 
           bool is_empty = osal_queue_empty(_usbh_q);
-          osal_queue_send(_usbh_q, &event, in_isr);
+          queue_event(&event, in_isr);
 
           if (is_empty) {
             // 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);
@@ -435,38 +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)
-        {
+        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
@@ -483,16 +551,16 @@ 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 const drv_id = dev->ep2drv[epnum][ep_dir];
-              if ( drv_id < USBH_CLASS_DRIVER_COUNT ) {
-                TU_LOG_USBH("%s xfer callback\r\n", usbh_class_drivers[drv_id].name);
-                usbh_class_drivers[drv_id].xfer_cb(event.dev_addr, ep_addr, (xfer_result_t) event.xfer_complete.result,
-                                                   event.xfer_complete.len);
+              uint8_t drv_id = dev->ep2drv[epnum][ep_dir];
+              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 {
                 // no driver/callback responsible for this transfer
                 TU_ASSERT(false,);
@@ -500,14 +568,15 @@ void tuh_task_ext(uint32_t timeout_ms, bool in_isr)
             }
           }
         }
+        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
@@ -521,28 +590,31 @@ void tuh_task_ext(uint32_t timeout_ms, bool in_isr)
 // Control transfer
 //--------------------------------------------------------------------+
 
-static void _control_blocking_complete_cb(tuh_xfer_t* xfer)
-{
+static void _control_blocking_complete_cb(tuh_xfer_t* xfer) {
   // update result
   *((xfer_result_t*) xfer->user_data) = xfer->result;
 }
 
 // TODO timeout_ms is not supported yet
-bool tuh_control_xfer (tuh_xfer_t* xfer)
-{
+bool tuh_control_xfer (tuh_xfer_t* xfer) {
   // EP0 with setup packet
   TU_VERIFY(xfer->ep_addr == 0 && xfer->setup);
 
-  // pre-check to help reducing mutex lock
-  TU_VERIFY(_ctrl_xfer.stage == CONTROL_STAGE_IDLE);
-
+  // Check if device is still connected (enumerating for dev0)
   uint8_t const daddr = xfer->daddr;
+  if ( daddr == 0 ) {
+    if (!_dev0.enumerating) return false;
+  } else {
+    usbh_device_t const* dev = get_device(daddr);
+    if (dev && dev->connected == 0) return false;
+  }
 
+  // pre-check to help reducing mutex lock
+  TU_VERIFY(_ctrl_xfer.stage == CONTROL_STAGE_IDLE);
   (void) osal_mutex_lock(_usbh_mutex, OSAL_TIMEOUT_WAIT_FOREVER);
 
   bool const is_idle = (_ctrl_xfer.stage == CONTROL_STAGE_IDLE);
-  if (is_idle)
-  {
+  if (is_idle) {
     _ctrl_xfer.stage       = CONTROL_STAGE_SETUP;
     _ctrl_xfer.daddr       = daddr;
     _ctrl_xfer.actual_len  = 0;
@@ -561,14 +633,11 @@ 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_PTR(CFG_TUH_LOG_LEVEL, xfer->setup);
-  TU_LOG_USBH("\r\n");
+  TU_LOG_BUF_USBH(xfer->setup, 8);
 
-  if (xfer->complete_cb)
-  {
+  if (xfer->complete_cb) {
     TU_ASSERT( hcd_setup_send(rhport, daddr, (uint8_t const*) &_ctrl_xfer.request) );
-  }else
-  {
+  }else {
     // blocking if complete callback is not provided
     // change callback to internal blocking, and result as user argument
     volatile xfer_result_t result = XFER_RESULT_INVALID;
@@ -599,21 +668,18 @@ bool tuh_control_xfer (tuh_xfer_t* xfer)
   return true;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void _set_control_xfer_stage(uint8_t stage)
-{
+TU_ATTR_ALWAYS_INLINE static inline void _set_control_xfer_stage(uint8_t stage) {
   (void) osal_mutex_lock(_usbh_mutex, OSAL_TIMEOUT_WAIT_FOREVER);
   _ctrl_xfer.stage = 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
   tusb_control_request_t const request = _ctrl_xfer.request;
-  tuh_xfer_t xfer_temp =
-  {
+  tuh_xfer_t xfer_temp = {
     .daddr       = daddr,
     .ep_addr     = 0,
     .result      = result,
@@ -626,60 +692,61 @@ static void _xfer_complete(uint8_t daddr, xfer_result_t result)
 
   _set_control_xfer_stage(CONTROL_STAGE_IDLE);
 
-  if (xfer_temp.complete_cb)
-  {
+  if (xfer_temp.complete_cb) {
     xfer_temp.complete_cb(&xfer_temp);
   }
 }
 
-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);
-    #if CFG_TUSB_DEBUG == 1
-    TU_LOG1_PTR(request);
-    TU_LOG1("\r\n");
-    #endif
+  if (XFER_RESULT_SUCCESS != result) {
+    TU_LOG_USBH("[%u:%u] Control %s, xferred_bytes = %" PRIu32 "\r\n", rhport, daddr, result == XFER_RESULT_STALLED ? "STALLED" : "FAILED", xferred_bytes);
+    TU_LOG_BUF_USBH(request, 8);
 
     // terminate transfer if any stage failed
-    _xfer_complete(dev_addr, result);
-  }else
-  {
-    switch(_ctrl_xfer.stage)
-    {
+    _control_xfer_complete(daddr, result);
+  }else {
+    switch(_ctrl_xfer.stage) {
       case CONTROL_STAGE_SETUP:
-        if (request->wLength)
-        {
+        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);
+        if (request->wLength) {
+          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;
     }
@@ -692,17 +759,15 @@ static bool usbh_control_xfer_cb (uint8_t dev_addr, uint8_t ep_addr, xfer_result
 //
 //--------------------------------------------------------------------+
 
-bool tuh_edpt_xfer(tuh_xfer_t* xfer)
-{
-  uint8_t const daddr   = xfer->daddr;
+bool tuh_edpt_xfer(tuh_xfer_t* xfer) {
+  uint8_t const daddr = xfer->daddr;
   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, xfer->complete_cb, xfer->user_data) )
-  {
+  if (!usbh_edpt_xfer_with_callback(daddr, ep_addr, xfer->buffer, (uint16_t) xfer->buflen,
+                                    xfer->complete_cb, xfer->user_data)) {
     usbh_edpt_release(daddr, ep_addr);
     return false;
   }
@@ -714,61 +779,72 @@ bool tuh_edpt_abort_xfer(uint8_t daddr, uint8_t ep_addr) {
   usbh_device_t* dev = get_device(daddr);
   TU_VERIFY(dev);
 
+  TU_LOG_USBH("[%u] Aborted transfer on EP %02X\r\n", daddr, ep_addr);
+
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir   = tu_edpt_dir(ep_addr);
 
-  // skip if not busy
-  TU_VERIFY(dev->ep_status[epnum][dir].busy);
-
-  bool const ret = hcd_edpt_abort_xfer(dev->rhport, daddr, ep_addr);
-  if (ret) {
-    // mark as ready if transfer is aborted
+  if ( epnum == 0 ) {
+    // control transfer: only 1 control at a time, check if we are aborting the current one
+    TU_VERIFY(daddr == _ctrl_xfer.daddr && _ctrl_xfer.stage != CONTROL_STAGE_IDLE);
+    TU_VERIFY(hcd_edpt_abort_xfer(dev->rhport, daddr, ep_addr));
+    // reset control transfer state to idle
+    _set_control_xfer_stage(CONTROL_STAGE_IDLE);
+  } else {
+    // non-control skip if not busy
+    TU_VERIFY(dev->ep_status[epnum][dir].busy);
+    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;
+    tu_edpt_release(&dev->ep_status[epnum][dir], _usbh_mutex);
   }
 
-  return ret;
+  return true;
 }
 
 //--------------------------------------------------------------------+
 // USBH API For Class Driver
 //--------------------------------------------------------------------+
 
-uint8_t usbh_get_rhport(uint8_t dev_addr)
-{
-  usbh_device_t* dev = get_device(dev_addr);
+uint8_t usbh_get_rhport(uint8_t dev_addr) {
+  usbh_device_t *dev = get_device(dev_addr);
   return dev ? dev->rhport : _dev0.rhport;
 }
 
-uint8_t* usbh_get_enum_buf(void)
-{
+uint8_t *usbh_get_enum_buf(void) {
   return _usbh_ctrl_buf;
 }
 
-void usbh_int_set(bool enabled)
-{
+void usbh_int_set(bool enabled) {
   // TODO all host controller if multiple are used since they shared the same event queue
-  if (enabled)
-  {
+  if (enabled) {
     hcd_int_enable(_usbh_controller);
-  }else
-  {
+  } else {
     hcd_int_disable(_usbh_controller);
   }
 }
 
+void usbh_defer_func(osal_task_func_t func, void *param, bool in_isr) {
+  hcd_event_t event = { 0 };
+  event.event_id = USBH_EVENT_FUNC_CALL;
+  event.func_call.func = func;
+  event.func_call.param = param;
+
+  queue_event(&event, in_isr);
+}
+
 //--------------------------------------------------------------------+
 // Endpoint API
 //--------------------------------------------------------------------+
 
 // 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);
@@ -777,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);
@@ -793,9 +868,9 @@ bool usbh_edpt_release(uint8_t dev_addr, uint8_t ep_addr)
 }
 
 // Submit an transfer
-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)
-{
+// 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) {
   (void) complete_cb;
   (void) user_data;
 
@@ -803,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);
@@ -820,27 +895,22 @@ 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();
+//    TU_BREAKPOINT();
     return false;
   }
 }
 
-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,
@@ -852,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);
 }
 
@@ -864,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;
 }
@@ -873,37 +941,38 @@ 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;
   }
 }
 
-TU_ATTR_FAST_FUNC void hcd_event_handler(hcd_event_t const* event, bool in_isr)
-{
-  switch (event->event_id)
-  {
-//    case HCD_EVENT_DEVICE_REMOVE:
-//      // mark device as removing to prevent further xfer before the event is processed in usbh task
-//      break;
+TU_ATTR_FAST_FUNC void hcd_event_handler(hcd_event_t const* event, bool in_isr) {
+  switch (event->event_id) {
+    case HCD_EVENT_DEVICE_REMOVE:
+      // FIXME device remove from a hub need an HCD API for hcd to free up endpoint
+      // mark device as removing to prevent further xfer before the event is processed in usbh task
 
-    default:
-      osal_queue_send(_usbh_q, event, in_isr);
-    break;
+      // Check if dev0 is removed
+      if ((event->rhport == _dev0.rhport) && (event->connection.hub_addr == _dev0.hub_addr) &&
+          (event->connection.hub_port == _dev0.hub_port)) {
+        _dev0.enumerating = 0;
+      }
+      break;
+
+    default: break;
   }
+
+  queue_event(event, in_isr);
 }
 
 //--------------------------------------------------------------------+
@@ -913,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
@@ -928,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,
@@ -943,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);
 }
 
@@ -980,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);
@@ -989,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);
@@ -999,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);
@@ -1102,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);
 }
 
@@ -1146,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);
 }
 
@@ -1173,60 +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));
+      // 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);
 
-  removing_hubs[hub_addr-CFG_TUH_DEVICE_MAX] = hub_port;
-
-  // 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(CFG_TUH_LOG_LEVEL, "  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 < USBH_CLASS_DRIVER_COUNT; drv_id++) {
-        usbh_class_drivers[drv_id].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);
 }
 
 //--------------------------------------------------------------------+
@@ -1238,6 +1279,12 @@ static void process_removing_device(uint8_t rhport, uint8_t hub_addr, uint8_t hu
 //--------------------------------------------------------------------+
 
 enum {
+  ENUM_RESET_DELAY = 50, // USB specs: 10 to 50ms
+  ENUM_CONTACT_DEBOUNCING_DELAY = 450, // when plug/unplug a device, physical connection can be bouncing and may
+                                       // generate a series of attach/detach event. This delay wait for stable connection
+};
+
+enum {
   ENUM_IDLE,
   ENUM_RESET_1,         // 1st reset when attached
   //ENUM_HUB_GET_STATUS_1,
@@ -1260,8 +1307,7 @@ static bool _parse_configuration_descriptor (uint8_t dev_addr, tusb_desc_configu
 static void enum_full_complete(void);
 
 // process device enumeration
-static void process_enumeration(tuh_xfer_t* xfer)
-{
+static void process_enumeration(tuh_xfer_t* xfer) {
   // Retry a few times with transfers in enumeration since device can be unstable when starting up
   enum {
     ATTEMPT_COUNT_MAX = 3,
@@ -1269,19 +1315,20 @@ static void process_enumeration(tuh_xfer_t* xfer)
   };
   static uint8_t failed_count = 0;
 
-  if (XFER_RESULT_SUCCESS != xfer->result)
-  {
+  if (XFER_RESULT_SUCCESS != xfer->result) {
     // retry if not reaching max attempt
-    if ( failed_count < ATTEMPT_COUNT_MAX )
-    {
+    bool retry = _dev0.enumerating && (failed_count < ATTEMPT_COUNT_MAX);
+    if ( retry ) {
       failed_count++;
       osal_task_delay(ATTEMPT_DELAY_MS); // delay a bit
       TU_LOG1("Enumeration attempt %u\r\n", failed_count);
-      TU_ASSERT(tuh_control_xfer(xfer), );
-    }else
-    {
+      retry = tuh_control_xfer(xfer);
+    }
+
+    if (!retry) {
       enum_full_complete();
     }
+
     return;
   }
   failed_count = 0;
@@ -1289,307 +1336,288 @@ 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(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;
+      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;
 
-    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
-    // wait until device is stable TODO non blocking
     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);
+    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);
+
+    // 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 is stable
-    osal_task_delay(RESET_DELAY);
+    // 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
 
   return true;
 }
 
-static uint8_t get_new_address(bool is_hub)
-{
+static uint8_t get_new_address(bool is_hub) {
   uint8_t start;
   uint8_t end;
 
-  if ( is_hub )
-  {
+  if ( is_hub ) {
     start = CFG_TUH_DEVICE_MAX;
     end   = start + CFG_TUH_HUB;
-  }else
-  {
+  }else {
     start = 0;
     end   = start + CFG_TUH_DEVICE_MAX;
   }
 
-  for (uint8_t idx = start; idx < end; idx++)
-  {
+  for (uint8_t idx = start; idx < end; idx++) {
     if (!_usbh_devices[idx].connected) return (idx+1);
   }
 
   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);
@@ -1597,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;
 
@@ -1623,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
@@ -1634,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
@@ -1644,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 < USBH_CLASS_DRIVER_COUNT; drv_id++)
-    {
-      usbh_class_driver_t const * driver = &usbh_class_drivers[drv_id];
-
-      if ( driver->open(dev->rhport, dev_addr, desc_itf, drv_len) )
-      {
+    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) ) {
         // 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
@@ -1669,10 +1689,9 @@ static bool _parse_configuration_descriptor(uint8_t dev_addr, tusb_desc_configur
         break; // exit driver find loop
       }
 
-      if( drv_id >= USBH_CLASS_DRIVER_COUNT )
-      {
-        TU_LOG(CFG_TUH_LOG_LEVEL, "Interface %u: class = %u subclass = %u protocol = %u is not supported\r\n",
-               desc_itf->bInterfaceNumber, desc_itf->bInterfaceClass, desc_itf->bInterfaceSubClass, desc_itf->bInterfaceProtocol);
+      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);
       }
     }
 
@@ -1683,19 +1702,16 @@ static bool _parse_configuration_descriptor(uint8_t dev_addr, tusb_desc_configur
   return true;
 }
 
-void usbh_driver_set_config_complete(uint8_t dev_addr, uint8_t itf_num)
-{
+void usbh_driver_set_config_complete(uint8_t dev_addr, uint8_t itf_num) {
   usbh_device_t* dev = get_device(dev_addr);
 
-  for(itf_num++; itf_num < CFG_TUH_INTERFACE_MAX; itf_num++)
-  {
+  for(itf_num++; itf_num < CFG_TUH_INTERFACE_MAX; itf_num++) {
     // continue with next valid interface
     // IAD binding interface such as CDCs should return itf_num + 1 when complete
     // with usbh_driver_set_config_complete()
     uint8_t const drv_id = dev->itf2drv[itf_num];
-    if (drv_id != TUSB_INDEX_INVALID_8)
-    {
-      usbh_class_driver_t const * driver = &usbh_class_drivers[drv_id];
+    usbh_class_driver_t const * driver = get_driver(drv_id);
+    if (driver) {
       TU_LOG_USBH("%s set config: itf = %u\r\n", driver->name, itf_num);
       driver->set_config(dev_addr, itf_num);
       break;
@@ -1703,23 +1719,19 @@ void usbh_driver_set_config_complete(uint8_t dev_addr, uint8_t itf_num)
   }
 
   // all interface are configured
-  if (itf_num == CFG_TUH_INTERFACE_MAX)
-  {
+  if (itf_num == CFG_TUH_INTERFACE_MAX) {
     enum_full_complete();
 
-    if (is_hub_addr(dev_addr))
-    {
-      TU_LOG(CFG_TUH_LOG_LEVEL, "HUB address = %u is mounted\r\n", dev_addr);
-    }else
-    {
+    if (is_hub_addr(dev_addr)) {
+      TU_LOG_USBH("HUB address = %u is mounted\r\n", dev_addr);
+    }else {
       // Invoke callback if available
       if (tuh_mount_cb) tuh_mount_cb(dev_addr);
     }
   }
 }
 
-static void enum_full_complete(void)
-{
+static void enum_full_complete(void) {
   // mark enumeration as complete
   _dev0.enumerating = 0;
 
diff --git a/src/host/usbh.h b/src/host/usbh.h
index 0d56f40c9..359684169 100644
--- a/src/host/usbh.h
+++ b/src/host/usbh.h
@@ -47,8 +47,7 @@ typedef void (*tuh_xfer_cb_t)(tuh_xfer_t* xfer);
 // it is advised to initialize it using member name
 // Note2: not all field is available/meaningful in callback,
 // some info is not saved by usbh to save SRAM
-struct tuh_xfer_s
-{
+struct tuh_xfer_s {
   uint8_t daddr;
   uint8_t ep_addr;
   uint8_t TU_RESERVED;      // reserved
@@ -56,8 +55,7 @@ struct tuh_xfer_s
 
   uint32_t actual_len;      // excluding setup packet
 
-  union
-  {
+  union {
     tusb_control_request_t const* setup; // setup packet pointer if control transfer
     uint32_t buflen;                     // expected length if not control transfer (not available in callback)
   };
@@ -70,18 +68,30 @@ struct tuh_xfer_s
 };
 
 // Subject to change
-typedef struct
-{
+typedef struct {
   uint8_t daddr;
   tusb_desc_interface_t desc;
 } tuh_itf_info_t;
 
-// ConfigID for tuh_config()
-enum
-{
-  TUH_CFGID_RPI_PIO_USB_CONFIGURATION = OPT_MCU_RP2040 << 8 // cfg_param: pio_usb_configuration_t
+// ConfigID for tuh_configure()
+enum {
+  TUH_CFGID_INVALID = 0,
+  TUH_CFGID_RPI_PIO_USB_CONFIGURATION = 100, // cfg_param: pio_usb_configuration_t
+  TUH_CFGID_MAX3421 = 200,
 };
 
+typedef struct {
+  uint8_t max_nak; // max NAK per endpoint per frame
+  uint8_t cpuctl; // R16: CPU Control Register
+  uint8_t pinctl; // R17: Pin Control Register. FDUPSPI bit is ignored
+} tuh_configure_max3421_t;
+
+typedef union {
+  // For TUH_CFGID_RPI_PIO_USB_CONFIGURATION use pio_usb_configuration_t
+
+  tuh_configure_max3421_t max3421;
+} tuh_configure_param_t;
+
 //--------------------------------------------------------------------+
 // APPLICATION CALLBACK
 //--------------------------------------------------------------------+
@@ -94,9 +104,12 @@ TU_ATTR_WEAK void tuh_mount_cb (uint8_t daddr);
 // Invoked when a device failed to mount during enumeration process
 // TU_ATTR_WEAK void tuh_mount_failed_cb (uint8_t daddr);
 
-/// Invoked when a device is unmounted (detached)
+// Invoked when a device is unmounted (detached)
 TU_ATTR_WEAK void tuh_umount_cb(uint8_t daddr);
 
+// Invoked when there is a new usb event, which need to be processed by tuh_task()/tuh_task_ext()
+void tuh_event_hook_cb(uint8_t rhport, uint32_t eventid, bool in_isr);
+
 //--------------------------------------------------------------------+
 // APPLICATION API
 //--------------------------------------------------------------------+
@@ -105,12 +118,16 @@ TU_ATTR_WEAK void tuh_umount_cb(uint8_t daddr);
 // Should be called before tuh_init()
 // - cfg_id   : configure ID (TBD)
 // - cfg_param: configure data, structure depends on the ID
-bool tuh_configure(uint8_t controller_id, uint32_t cfg_id, const void* cfg_param);
+bool tuh_configure(uint8_t rhport, uint32_t cfg_id, const void* cfg_param);
 
 // Init host stack
-bool tuh_init(uint8_t controller_id);
+bool tuh_init(uint8_t rhport);
+
+// Deinit host stack on rhport
+bool tuh_deinit(uint8_t rhport);
 
-// Check if host stack is already initialized
+// 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()
@@ -120,8 +137,7 @@ void tuh_task_ext(uint32_t timeout_ms, bool in_isr);
 
 // Task function should be called in main/rtos loop
 TU_ATTR_ALWAYS_INLINE static inline
-void tuh_task(void)
-{
+void tuh_task(void) {
   tuh_task_ext(UINT32_MAX, false);
 }
 
@@ -129,14 +145,31 @@ void tuh_task(void)
 bool tuh_task_event_ready(void);
 
 #ifndef _TUSB_HCD_H_
-extern void hcd_int_handler(uint8_t rhport);
+extern void hcd_int_handler(uint8_t rhport, bool in_isr);
 #endif
 
-// Interrupt handler, name alias to HCD
-#define tuh_int_handler   hcd_int_handler
+// Interrupt handler alias to HCD with in_isr as optional parameter
+// - tuh_int_handler(rhport) --> hcd_int_handler(rhport, true)
+// - tuh_int_handler(rhport, in_isr) --> hcd_int_handler(rhport, in_isr)
+// Note: this is similar to TU_VERIFY(), _GET_3RD_ARG() is defined in tusb_verify.h
+#define _tuh_int_handler_1arg(_rhport)            hcd_int_handler(_rhport, true)
+#define _tuh_int_hanlder_2arg(_rhport, _in_isr)   hcd_int_handler(_rhport, _in_isr)
+#define tuh_int_handler(...)   _GET_3RD_ARG(__VA_ARGS__, _tuh_int_hanlder_2arg, _tuh_int_handler_1arg, _dummy)(__VA_ARGS__)
+
+// Check if roothub port is initialized and active as a host
+bool tuh_rhport_is_active(uint8_t rhport);
+
+// Assert/de-assert Bus Reset signal to roothub port. USB specs: it should last 10-50ms
+bool tuh_rhport_reset_bus(uint8_t rhport, bool active);
+
+//--------------------------------------------------------------------+
+// Device API
+//--------------------------------------------------------------------+
 
+// Get VID/PID of device
 bool tuh_vid_pid_get(uint8_t daddr, uint16_t* vid, uint16_t* pid);
 
+// Get speed of device
 tusb_speed_t tuh_speed_get(uint8_t daddr);
 
 // Check if device is connected and configured
@@ -144,8 +177,7 @@ bool tuh_mounted(uint8_t daddr);
 
 // Check if device is suspended
 TU_ATTR_ALWAYS_INLINE static inline
-bool tuh_suspended(uint8_t daddr)
-{
+bool tuh_suspended(uint8_t daddr) {
   // TODO implement suspend & resume on host
   (void) daddr;
   return false;
@@ -153,8 +185,7 @@ bool tuh_suspended(uint8_t daddr)
 
 // Check if device is ready to communicate with
 TU_ATTR_ALWAYS_INLINE static inline
-bool tuh_ready(uint8_t daddr)
-{
+bool tuh_ready(uint8_t daddr) {
   return tuh_mounted(daddr) && !tuh_suspended(daddr);
 }
 
diff --git a/src/host/usbh_classdriver.h b/src/host/usbh_pvt.h
index be9811641..95de915e9 100644
--- a/src/host/usbh_classdriver.h
+++ b/src/host/usbh_pvt.h
@@ -24,8 +24,8 @@
  * This file is part of the TinyUSB stack.
  */
 
-#ifndef _TUSB_USBH_CLASSDRIVER_H_
-#define _TUSB_USBH_CLASSDRIVER_H_
+#ifndef _TUSB_USBH_PVT_H_
+#define _TUSB_USBH_PVT_H_
 
 #include "osal/osal.h"
 #include "common/tusb_fifo.h"
@@ -35,6 +35,12 @@
  extern "C" {
 #endif
 
+#define TU_LOG_USBH(...)      TU_LOG(CFG_TUH_LOG_LEVEL, __VA_ARGS__)
+#define TU_LOG_MEM_USBH(...)  TU_LOG_MEM(CFG_TUH_LOG_LEVEL, __VA_ARGS__)
+#define TU_LOG_BUF_USBH(...)  TU_LOG_BUF(CFG_TUH_LOG_LEVEL, __VA_ARGS__)
+#define TU_LOG_INT_USBH(...)  TU_LOG_INT(CFG_TUH_LOG_LEVEL, __VA_ARGS__)
+#define TU_LOG_HEX_USBH(...)  TU_LOG_HEX(CFG_TUH_LOG_LEVEL, __VA_ARGS__)
+
 enum {
   USBH_EPSIZE_BULK_MAX = (TUH_OPT_HIGH_SPEED ? TUSB_EPSIZE_BULK_HS : TUSB_EPSIZE_BULK_FS)
 };
@@ -44,17 +50,20 @@ enum {
 //--------------------------------------------------------------------+
 
 typedef struct {
-  #if CFG_TUSB_DEBUG >= 2
   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);
   void (* const close      )(uint8_t dev_addr);
 } usbh_class_driver_t;
 
+// Invoked when initializing host stack to get additional class drivers.
+// Can be implemented by application to extend/overwrite class driver support.
+// Note: The drivers array must be accessible at all time when stack is active
+usbh_class_driver_t const* usbh_app_driver_get_cb(uint8_t* driver_count) TU_ATTR_WEAK;
+
 // Call by class driver to tell USBH that it has complete the enumeration
 void usbh_driver_set_config_complete(uint8_t dev_addr, uint8_t itf_num);
 
@@ -64,6 +73,8 @@ uint8_t* usbh_get_enum_buf(void);
 
 void usbh_int_set(bool enabled);
 
+void usbh_defer_func(osal_task_func_t func, void *param, bool in_isr);
+
 //--------------------------------------------------------------------+
 // USBH Endpoint API
 //--------------------------------------------------------------------+
@@ -73,12 +84,10 @@ bool usbh_edpt_xfer_with_callback(uint8_t dev_addr, uint8_t ep_addr, uint8_t * b
                                   tuh_xfer_cb_t complete_cb, uintptr_t user_data);
 
 TU_ATTR_ALWAYS_INLINE
-static inline bool usbh_edpt_xfer(uint8_t dev_addr, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
-{
+static inline bool usbh_edpt_xfer(uint8_t dev_addr, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes) {
   return usbh_edpt_xfer_with_callback(dev_addr, ep_addr, buffer, total_bytes, NULL, 0);
 }
 
-
 // Claim an endpoint before submitting a transfer.
 // If caller does not make any transfer, it must release endpoint for others.
 bool usbh_edpt_claim(uint8_t dev_addr, uint8_t ep_addr);