Merge pull request #1035 from hathach/improve-host-stack

Improve host stack

14 files changed, 425 insertions, 334 deletions

diff --git a/src/class/cdc/cdc_host.c b/src/class/cdc/cdc_host.c
index fc7100e81..35b8de3b1 100644
--- a/src/class/cdc/cdc_host.c
+++ b/src/class/cdc/cdc_host.c
@@ -149,7 +149,7 @@ void cdch_init(void)
   tu_memclr(cdch_data, sizeof(cdch_data_t)*CFG_TUSB_HOST_DEVICE_MAX);
 }
 
-uint16_t cdch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
+bool cdch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
 {
   (void) max_len;
 
@@ -157,7 +157,7 @@ uint16_t cdch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const
   // Protocol 0xFF can be RNDIS device for windows XP
   TU_VERIFY( TUSB_CLASS_CDC                           == itf_desc->bInterfaceClass &&
              CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL == itf_desc->bInterfaceSubClass &&
-             0xFF                                     != itf_desc->bInterfaceProtocol, 0);
+             0xFF                                     != itf_desc->bInterfaceProtocol);
 
   cdch_data_t * p_cdc = get_itf(dev_addr);
 
@@ -186,7 +186,7 @@ uint16_t cdch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const
     // notification endpoint
     tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) p_desc;
 
-    TU_ASSERT( usbh_edpt_open(rhport, dev_addr, desc_ep), 0 );
+    TU_ASSERT( usbh_edpt_open(rhport, dev_addr, desc_ep) );
     p_cdc->ep_notif = desc_ep->bEndpointAddress;
 
     drv_len += tu_desc_len(p_desc);
@@ -205,9 +205,9 @@ uint16_t cdch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const
     for(uint32_t i=0; i<2; i++)
     {
       tusb_desc_endpoint_t const *desc_ep = (tusb_desc_endpoint_t const *) p_desc;
-      TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType && TUSB_XFER_BULK == desc_ep->bmAttributes.xfer, 0);
+      TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType && TUSB_XFER_BULK == desc_ep->bmAttributes.xfer);
 
-      TU_ASSERT(usbh_edpt_open(rhport, dev_addr, desc_ep), 0);
+      TU_ASSERT(usbh_edpt_open(rhport, dev_addr, desc_ep));
 
       if ( tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN )
       {
@@ -222,7 +222,7 @@ uint16_t cdch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const
     }
   }
 
-  return drv_len;
+  return true;
 }
 
 bool cdch_set_config(uint8_t dev_addr, uint8_t itf_num)
diff --git a/src/class/cdc/cdc_host.h b/src/class/cdc/cdc_host.h
index edcd258a8..0d435138b 100644
--- a/src/class/cdc/cdc_host.h
+++ b/src/class/cdc/cdc_host.h
@@ -121,11 +121,11 @@ void tuh_cdc_xfer_isr(uint8_t dev_addr, xfer_result_t event, cdc_pipeid_t pipe_i
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-void     cdch_init       (void);
-uint16_t cdch_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len);
-bool     cdch_set_config (uint8_t dev_addr, uint8_t itf_num);
-bool     cdch_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
-void     cdch_close      (uint8_t dev_addr);
+void cdch_init       (void);
+bool cdch_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len);
+bool cdch_set_config (uint8_t dev_addr, uint8_t itf_num);
+bool cdch_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
+void cdch_close      (uint8_t dev_addr);
 
 #ifdef __cplusplus
  }
diff --git a/src/class/hid/hid_host.c b/src/class/hid/hid_host.c
index 40b6f5631..4dd198369 100644
--- a/src/class/hid/hid_host.c
+++ b/src/class/hid/hid_host.c
@@ -247,19 +247,24 @@ static bool config_set_protocol             (uint8_t dev_addr, tusb_control_requ
 static bool config_get_report_desc          (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result);
 static bool config_get_report_desc_complete (uint8_t dev_addr, tusb_control_request_t const * request, xfer_result_t result);
 
-uint16_t hidh_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len)
+static void config_driver_mount_complete(uint8_t dev_addr, uint8_t instance, uint8_t const* desc_report, uint16_t desc_len);
+
+bool hidh_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len)
 {
   (void) max_len;
 
-  TU_VERIFY(TUSB_CLASS_HID == desc_itf->bInterfaceClass, 0);
+  TU_VERIFY(TUSB_CLASS_HID == desc_itf->bInterfaceClass);
+
+  // len = interface + hid + n*endpoints
+  uint16_t const drv_len = sizeof(tusb_desc_interface_t) + sizeof(tusb_hid_descriptor_hid_t) + desc_itf->bNumEndpoints*sizeof(tusb_desc_endpoint_t);
+  TU_ASSERT(max_len >= drv_len);
 
-  uint16_t drv_len = sizeof(tusb_desc_interface_t);
   uint8_t const *p_desc = (uint8_t const *) desc_itf;
 
   //------------- HID descriptor -------------//
   p_desc = tu_desc_next(p_desc);
   tusb_hid_descriptor_hid_t const *desc_hid = (tusb_hid_descriptor_hid_t const *) p_desc;
-  TU_ASSERT(HID_DESC_TYPE_HID == desc_hid->bDescriptorType, 0);
+  TU_ASSERT(HID_DESC_TYPE_HID == desc_hid->bDescriptorType);
 
   // not enough interface, try to increase CFG_TUH_HID
   // TODO multiple devices
@@ -267,13 +272,12 @@ uint16_t hidh_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const
   TU_ASSERT(hid_dev->inst_count < CFG_TUH_HID, 0);
 
   //------------- Endpoint Descriptor -------------//
-  drv_len += tu_desc_len(p_desc);
   p_desc = tu_desc_next(p_desc);
   tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) p_desc;
-  TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType, 0);
+  TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType);
 
   // TODO also open endpoint OUT
-  TU_ASSERT( usbh_edpt_open(rhport, dev_addr, desc_ep), 0 );
+  TU_ASSERT( usbh_edpt_open(rhport, dev_addr, desc_ep) );
 
   hidh_interface_t* hid_itf = get_instance(dev_addr, hid_dev->inst_count);
   hid_dev->inst_count++;
@@ -290,9 +294,7 @@ uint16_t hidh_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const
   hid_itf->protocol_mode = HID_PROTOCOL_BOOT;
   if ( HID_SUBCLASS_BOOT == desc_itf->bInterfaceSubClass ) hid_itf->itf_protocol = desc_itf->bInterfaceProtocol;
 
-  drv_len += desc_itf->bNumEndpoints*sizeof(tusb_desc_endpoint_t);
-
-  return drv_len;
+  return true;
 }
 
 bool hidh_set_config(uint8_t dev_addr, uint8_t itf_num)
@@ -367,26 +369,34 @@ static bool config_get_report_desc(uint8_t dev_addr, tusb_control_request_t cons
   uint8_t const instance    = get_instance_id_by_itfnum(dev_addr, itf_num);
   hidh_interface_t* hid_itf = get_instance(dev_addr, instance);
 
-  // Get Report Descriptor
+  // Get Report Descriptor if possible
   // using usbh enumeration buffer since report descriptor can be very long
-  TU_ASSERT( hid_itf->report_desc_len <= CFG_TUH_ENUMERATION_BUFSIZE );
+  if( hid_itf->report_desc_len > CFG_TUH_ENUMERATION_BUFSIZE )
+  {
+    TU_LOG2("HID Skip Report Descriptor since it is too large %u bytes\r\n", hid_itf->report_desc_len);
 
-  TU_LOG2("HID Get Report Descriptor\r\n");
-  tusb_control_request_t const new_request =
+    // Driver is mounted without report descriptor
+    config_driver_mount_complete(dev_addr, instance, NULL, 0);
+  }else
   {
-    .bmRequestType_bit =
+    TU_LOG2("HID Get Report Descriptor\r\n");
+    tusb_control_request_t const new_request =
     {
-      .recipient = TUSB_REQ_RCPT_INTERFACE,
-      .type      = TUSB_REQ_TYPE_STANDARD,
-      .direction = TUSB_DIR_IN
-    },
-    .bRequest = TUSB_REQ_GET_DESCRIPTOR,
-    .wValue   = tu_u16(hid_itf->report_desc_type, 0),
-    .wIndex   = itf_num,
-    .wLength  = hid_itf->report_desc_len
-  };
+      .bmRequestType_bit =
+      {
+        .recipient = TUSB_REQ_RCPT_INTERFACE,
+        .type      = TUSB_REQ_TYPE_STANDARD,
+        .direction = TUSB_DIR_IN
+      },
+      .bRequest = TUSB_REQ_GET_DESCRIPTOR,
+      .wValue   = tu_u16(hid_itf->report_desc_type, 0),
+      .wIndex   = itf_num,
+      .wLength  = hid_itf->report_desc_len
+    };
+
+    TU_ASSERT(tuh_control_xfer(dev_addr, &new_request, usbh_get_enum_buf(), config_get_report_desc_complete));
+  }
 
-  TU_ASSERT(tuh_control_xfer(dev_addr, &new_request, usbh_get_enum_buf(), config_get_report_desc_complete));
   return true;
 }
 
@@ -394,13 +404,21 @@ static bool config_get_report_desc_complete(uint8_t dev_addr, tusb_control_reque
 {
   TU_ASSERT(XFER_RESULT_SUCCESS == result);
 
-  uint8_t const itf_num     = (uint8_t) request->wIndex;
-  uint8_t const instance    = get_instance_id_by_itfnum(dev_addr, itf_num);
-  hidh_interface_t* hid_itf = get_instance(dev_addr, instance);
+  uint8_t const itf_num      = (uint8_t) request->wIndex;
+  uint8_t const instance     = get_instance_id_by_itfnum(dev_addr, itf_num);
 
   uint8_t const* desc_report = usbh_get_enum_buf();
   uint16_t const desc_len    = request->wLength;
 
+  config_driver_mount_complete(dev_addr, instance, desc_report, desc_len);
+
+  return true;
+}
+
+static void config_driver_mount_complete(uint8_t dev_addr, uint8_t instance, uint8_t const* desc_report, uint16_t desc_len)
+{
+  hidh_interface_t* hid_itf = get_instance(dev_addr, instance);
+
   // enumeration is complete
   tuh_hid_mount_cb(dev_addr, instance, desc_report, desc_len);
 
@@ -408,9 +426,7 @@ static bool config_get_report_desc_complete(uint8_t dev_addr, tusb_control_reque
   hidh_get_report(dev_addr, hid_itf);
 
   // notify usbh that driver enumeration is complete
-  usbh_driver_set_config_complete(dev_addr, itf_num);
-
-  return true;
+  usbh_driver_set_config_complete(dev_addr, hid_itf->itf_num);
 }
 
 //--------------------------------------------------------------------+
diff --git a/src/class/hid/hid_host.h b/src/class/hid/hid_host.h
index ef203123d..9a498afec 100644
--- a/src/class/hid/hid_host.h
+++ b/src/class/hid/hid_host.h
@@ -97,6 +97,8 @@ uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* reports_info_arr,
 // Invoked when device with hid interface is mounted
 // Report descriptor is also available for use. tuh_hid_parse_report_descriptor()
 // can be used to parse common/simple enough descriptor.
+// Note: if report descriptor length > CFG_TUH_ENUMERATION_BUFSIZE, it will be skipped
+// therefore report_desc = NULL, desc_len = 0
 void tuh_hid_mount_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* report_desc, uint16_t desc_len);
 
 // Invoked when device with hid interface is un-mounted
@@ -119,11 +121,11 @@ TU_ATTR_WEAK void tuh_hid_set_protocol_complete_cb(uint8_t dev_addr, uint8_t ins
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-void     hidh_init       (void);
-uint16_t hidh_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len);
-bool     hidh_set_config (uint8_t dev_addr, uint8_t itf_num);
-bool     hidh_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
-void     hidh_close      (uint8_t dev_addr);
+void hidh_init       (void);
+bool hidh_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len);
+bool hidh_set_config (uint8_t dev_addr, uint8_t itf_num);
+bool hidh_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
+void hidh_close      (uint8_t dev_addr);
 
 #ifdef __cplusplus
 }
diff --git a/src/class/msc/msc_host.c b/src/class/msc/msc_host.c
index 2c027deff..f02f4550a 100644
--- a/src/class/msc/msc_host.c
+++ b/src/class/msc/msc_host.c
@@ -360,22 +360,22 @@ static bool config_test_unit_ready_complete(uint8_t dev_addr, msc_cbw_t const* c
 static bool config_request_sense_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw);
 static bool config_read_capacity_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw);
 
-uint16_t msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len)
+bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len)
 {
   TU_VERIFY (MSC_SUBCLASS_SCSI == desc_itf->bInterfaceSubClass &&
              MSC_PROTOCOL_BOT  == desc_itf->bInterfaceProtocol);
 
   // msc driver length is fixed
   uint16_t const drv_len = sizeof(tusb_desc_interface_t) + desc_itf->bNumEndpoints*sizeof(tusb_desc_endpoint_t);
-  TU_ASSERT(drv_len <= max_len, 0);
+  TU_ASSERT(drv_len <= max_len);
 
   msch_interface_t* p_msc = get_itf(dev_addr);
   tusb_desc_endpoint_t const * ep_desc = (tusb_desc_endpoint_t const *) tu_desc_next(desc_itf);
 
   for(uint32_t i=0; i<2; i++)
   {
-    TU_ASSERT(TUSB_DESC_ENDPOINT == ep_desc->bDescriptorType && TUSB_XFER_BULK == ep_desc->bmAttributes.xfer, 0);
-    TU_ASSERT(usbh_edpt_open(rhport, dev_addr, ep_desc), 0);
+    TU_ASSERT(TUSB_DESC_ENDPOINT == ep_desc->bDescriptorType && TUSB_XFER_BULK == ep_desc->bmAttributes.xfer);
+    TU_ASSERT(usbh_edpt_open(rhport, dev_addr, ep_desc));
 
     if ( tu_edpt_dir(ep_desc->bEndpointAddress) == TUSB_DIR_IN )
     {
@@ -390,7 +390,7 @@ uint16_t msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const
 
   p_msc->itf_num = desc_itf->bInterfaceNumber;
 
-  return drv_len;
+  return true;
 }
 
 bool msch_set_config(uint8_t dev_addr, uint8_t itf_num)
diff --git a/src/class/msc/msc_host.h b/src/class/msc/msc_host.h
index 9e4217ba5..7718ad4fe 100644
--- a/src/class/msc/msc_host.h
+++ b/src/class/msc/msc_host.h
@@ -106,11 +106,11 @@ TU_ATTR_WEAK void tuh_msc_umount_cb(uint8_t dev_addr);
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 
-void     msch_init       (void);
-uint16_t msch_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len);
-bool     msch_set_config (uint8_t dev_addr, uint8_t itf_num);
-bool     msch_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
-void     msch_close      (uint8_t dev_addr);
+void msch_init       (void);
+bool msch_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len);
+bool msch_set_config (uint8_t dev_addr, uint8_t itf_num);
+void msch_close      (uint8_t dev_addr);
+bool msch_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
 
 #ifdef __cplusplus
  }
diff --git a/src/common/tusb_common.h b/src/common/tusb_common.h
index c8a66a879..c2356ffee 100644
--- a/src/common/tusb_common.h
+++ b/src/common/tusb_common.h
@@ -73,6 +73,23 @@
 #include "tusb_error.h"   // TODO remove
 #include "tusb_timeout.h" // TODO remove
 
+//--------------------------------------------------------------------+
+// Internal Helper used by Host and Device Stack
+//--------------------------------------------------------------------+
+
+// Check if endpoint descriptor is valid per USB specs
+bool tu_edpt_validate(tusb_desc_endpoint_t const * desc_ep, tusb_speed_t speed);
+
+// Bind all endpoint of a interface descriptor to class driver
+void tu_edpt_bind_driver(uint8_t ep2drv[][2], tusb_desc_interface_t const* p_desc, uint16_t desc_len, uint8_t driver_id);
+
+// Calculate total length of n interfaces (depending on IAD)
+uint16_t tu_desc_get_interface_total_len(tusb_desc_interface_t const* desc_itf, uint8_t itf_count, uint16_t max_len);
+
+//--------------------------------------------------------------------+
+// Internal Inline Functions
+//--------------------------------------------------------------------+
+
 //------------- Mem -------------//
 #define tu_memclr(buffer, size)  memset((buffer), 0, (size))
 #define tu_varclr(_var)          tu_memclr(_var, sizeof(*(_var)))
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 8b4f4e7e3..e12dafcfd 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -274,7 +274,6 @@ static osal_mutex_t _usbd_mutex;
 //--------------------------------------------------------------------+
 // Prototypes
 //--------------------------------------------------------------------+
-static void mark_interface_endpoint(uint8_t ep2drv[][2], uint8_t const* p_desc, uint16_t desc_len, uint8_t driver_id);
 static bool process_control_request(uint8_t rhport, tusb_control_request_t const * p_request);
 static bool process_set_config(uint8_t rhport, uint8_t cfg_num);
 static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const * p_request);
@@ -857,12 +856,12 @@ static bool process_set_config(uint8_t rhport, uint8_t cfg_num)
 
   while( p_desc < desc_end )
   {
-    tusb_desc_interface_assoc_t const * desc_itf_assoc = NULL;
+    tusb_desc_interface_assoc_t const * desc_iad = NULL;
 
     // Class will always starts with Interface Association (if any) and then Interface descriptor
     if ( TUSB_DESC_INTERFACE_ASSOCIATION == tu_desc_type(p_desc) )
     {
-      desc_itf_assoc = (tusb_desc_interface_assoc_t const *) p_desc;
+      desc_iad = (tusb_desc_interface_assoc_t const *) p_desc;
       p_desc = tu_desc_next(p_desc); // next to Interface
     }
 
@@ -871,6 +870,13 @@ static bool process_set_config(uint8_t rhport, uint8_t cfg_num)
     tusb_desc_interface_t const * desc_itf = (tusb_desc_interface_t const*) p_desc;
     uint16_t const remaining_len = desc_end-p_desc;
 
+    // Interface number must not be used already
+    TU_ASSERT(DRVID_INVALID == _usbd_dev.itf2drv[desc_itf->bInterfaceNumber]);
+
+    // TODO usbd can calculate the total length used for driver --> driver open() does not need to calculate it
+    // uint16_t const drv_len = tu_desc_get_interface_total_len(desc_itf, desc_iad ? desc_iad->bInterfaceCount : 1, desc_end-p_desc);
+
+    // Find driver for this interface
     uint8_t drv_id;
     for (drv_id = 0; drv_id < TOTAL_DRIVER_COUNT; drv_id++)
     {
@@ -882,30 +888,30 @@ static bool process_set_config(uint8_t rhport, uint8_t cfg_num)
         // Open successfully, check if length is correct
         TU_ASSERT( sizeof(tusb_desc_interface_t) <= drv_len && drv_len <= remaining_len);
 
-        // Interface number must not be used already
-        TU_ASSERT(DRVID_INVALID == _usbd_dev.itf2drv[desc_itf->bInterfaceNumber]);
-
         TU_LOG2("  %s opened\r\n", driver->name);
+
+        // bind interface to found driver
         _usbd_dev.itf2drv[desc_itf->bInterfaceNumber] = drv_id;
 
-        // If IAD exist, assign all interfaces to the same driver
-        if (desc_itf_assoc)
+        // If using IAD, bind all interfaces to the same driver
+        if (desc_iad)
         {
           // IAD's first interface number and class should match with opened interface
-          TU_ASSERT(desc_itf_assoc->bFirstInterface == desc_itf->bInterfaceNumber &&
-                    desc_itf_assoc->bFunctionClass  == desc_itf->bInterfaceClass);
+          TU_ASSERT(desc_iad->bFirstInterface == desc_itf->bInterfaceNumber &&
+                    desc_iad->bFunctionClass  == desc_itf->bInterfaceClass);
 
-          for(uint8_t i=1; i<desc_itf_assoc->bInterfaceCount; i++)
+          for(uint8_t i=1; i<desc_iad->bInterfaceCount; i++)
           {
             _usbd_dev.itf2drv[desc_itf->bInterfaceNumber+i] = drv_id;
           }
         }
 
-        mark_interface_endpoint(_usbd_dev.ep2drv, p_desc, drv_len, drv_id); // TODO refactor
+        // bind all endpoints to found driver
+        tu_edpt_bind_driver(_usbd_dev.ep2drv, desc_itf, drv_len, drv_id);
 
         p_desc += drv_len; // next interface
 
-        break;
+        break; // exit driver find loop
       }
     }
 
@@ -919,25 +925,6 @@ static bool process_set_config(uint8_t rhport, uint8_t cfg_num)
   return true;
 }
 
-// Helper marking endpoint of interface belongs to class driver
-static void mark_interface_endpoint(uint8_t ep2drv[][2], uint8_t const* p_desc, uint16_t desc_len, uint8_t driver_id)
-{
-  uint16_t len = 0;
-
-  while( len < desc_len )
-  {
-    if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) )
-    {
-      uint8_t const ep_addr = ((tusb_desc_endpoint_t const*) p_desc)->bEndpointAddress;
-
-      ep2drv[tu_edpt_number(ep_addr)][tu_edpt_dir(ep_addr)] = driver_id;
-    }
-
-    len   = (uint16_t)(len + tu_desc_len(p_desc));
-    p_desc = tu_desc_next(p_desc);
-  }
-}
-
 // return descriptor's buffer and update desc_len
 static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const * p_request)
 {
@@ -1177,41 +1164,8 @@ void usbd_defer_func(osal_task_func_t func, void* param, bool in_isr)
 
 bool usbd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * desc_ep)
 {
-  uint16_t const max_packet_size = tu_le16toh(desc_ep->wMaxPacketSize.size);
-
-  TU_LOG2("  Open EP %02X with Size = %u\r\n", desc_ep->bEndpointAddress, max_packet_size);
   TU_ASSERT(tu_edpt_number(desc_ep->bEndpointAddress) < CFG_TUD_ENDPPOINT_MAX);
-
-  switch (desc_ep->bmAttributes.xfer)
-  {
-    case TUSB_XFER_ISOCHRONOUS:
-    {
-      uint16_t const spec_size = (_usbd_dev.speed == TUSB_SPEED_HIGH ? 1024 : 1023);
-      TU_ASSERT(max_packet_size <= spec_size);
-    }
-    break;
-
-    case TUSB_XFER_BULK:
-      if (_usbd_dev.speed == TUSB_SPEED_HIGH)
-      {
-        // Bulk highspeed must be EXACTLY 512
-        TU_ASSERT(max_packet_size == 512);
-      }else
-      {
-        // TODO Bulk fullspeed can only be 8, 16, 32, 64
-        TU_ASSERT(max_packet_size <= 64);
-      }
-    break;
-
-    case TUSB_XFER_INTERRUPT:
-    {
-      uint16_t const spec_size = (_usbd_dev.speed == TUSB_SPEED_HIGH ? 1024 : 64);
-      TU_ASSERT(max_packet_size <= spec_size);
-    }
-    break;
-
-    default: return false;
-  }
+  TU_ASSERT(tu_edpt_validate(desc_ep, (tusb_speed_t) _usbd_dev.speed));
 
   return dcd_edpt_open(rhport, desc_ep);
 }
diff --git a/src/host/hub.c b/src/host/hub.c
index 2ead5bed1..00dce4e65 100644
--- a/src/host/hub.c
+++ b/src/host/hub.c
@@ -147,16 +147,17 @@ void hub_init(void)
   tu_memclr(hub_data, CFG_TUSB_HOST_DEVICE_MAX*sizeof(hub_interface_t));
 }
 
-uint16_t hub_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
+bool hub_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
 {
-  // hub driver does not support multiple TT yet
   TU_VERIFY(TUSB_CLASS_HUB == itf_desc->bInterfaceClass &&
-            0              == itf_desc->bInterfaceSubClass &&
-            1              <= itf_desc->bInterfaceProtocol, 0);
+            0              == itf_desc->bInterfaceSubClass);
+
+  // hub driver does not support multiple TT yet
+  TU_VERIFY(itf_desc->bInterfaceProtocol <= 1);
 
   // msc driver length is fixed
   uint16_t const drv_len = sizeof(tusb_desc_interface_t) + sizeof(tusb_desc_endpoint_t);
-  TU_ASSERT(drv_len <= max_len, 0);
+  TU_ASSERT(drv_len <= max_len);
 
   //------------- Interrupt Status endpoint -------------//
   tusb_desc_endpoint_t const *desc_ep = (tusb_desc_endpoint_t const *) tu_desc_next(itf_desc);
@@ -169,7 +170,7 @@ uint16_t hub_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const
   hub_data[dev_addr-1].itf_num = itf_desc->bInterfaceNumber;
   hub_data[dev_addr-1].ep_in   = desc_ep->bEndpointAddress;
 
-  return drv_len;
+  return true;
 }
 
 void hub_close(uint8_t dev_addr)
diff --git a/src/host/hub.h b/src/host/hub.h
index c9ffe4985..c4d544193 100644
--- a/src/host/hub.h
+++ b/src/host/hub.h
@@ -181,11 +181,11 @@ bool hub_status_pipe_queue(uint8_t dev_addr);
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-void     hub_init       (void);
-uint16_t 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);
-void     hub_close      (uint8_t dev_addr);
+void hub_init       (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);
+void hub_close      (uint8_t dev_addr);
 
 #ifdef __cplusplus
  }
diff --git a/src/host/usbh.c b/src/host/usbh.c
index 93621c42a..662ce2673 100644
--- a/src/host/usbh.c
+++ b/src/host/usbh.c
@@ -28,10 +28,6 @@
 
 #if TUSB_OPT_HOST_ENABLED
 
-#ifndef CFG_TUH_TASK_QUEUE_SZ
-#define CFG_TUH_TASK_QUEUE_SZ   16
-#endif
-
 #include "tusb.h"
 #include "host/usbh.h"
 #include "host/usbh_classdriver.h"
@@ -39,8 +35,23 @@
 #include "usbh_hcd.h"
 
 //--------------------------------------------------------------------+
+// USBH Configuration
+//--------------------------------------------------------------------+
+
+#ifndef CFG_TUH_TASK_QUEUE_SZ
+#define CFG_TUH_TASK_QUEUE_SZ   16
+#endif
+
+// Debug level of USBD
+#define USBH_DBG_LVL   2
+
+//--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
+
+// Invalid driver ID in itf2drv[] ep2drv[][] mapping
+enum { DRVID_INVALID = 0xFFu };
+
 #if CFG_TUSB_DEBUG >= 2
   #define DRIVER_NAME(_name)    .name = _name,
 #else
@@ -52,7 +63,6 @@ static usbh_class_driver_t const usbh_class_drivers[] =
   #if CFG_TUH_CDC
     {
       DRIVER_NAME("CDC")
-      .class_code = TUSB_CLASS_CDC,
       .init       = cdch_init,
       .open       = cdch_open,
       .set_config = cdch_set_config,
@@ -64,7 +74,6 @@ static usbh_class_driver_t const usbh_class_drivers[] =
   #if CFG_TUH_MSC
     {
       DRIVER_NAME("MSC")
-      .class_code = TUSB_CLASS_MSC,
       .init       = msch_init,
       .open       = msch_open,
       .set_config = msch_set_config,
@@ -76,7 +85,6 @@ static usbh_class_driver_t const usbh_class_drivers[] =
   #if CFG_TUH_HID
     {
       DRIVER_NAME("HID")
-      .class_code = TUSB_CLASS_HID,
       .init       = hidh_init,
       .open       = hidh_open,
       .set_config = hidh_set_config,
@@ -88,7 +96,6 @@ static usbh_class_driver_t const usbh_class_drivers[] =
   #if CFG_TUH_HUB
     {
       DRIVER_NAME("HUB")
-      .class_code = TUSB_CLASS_HUB,
       .init       = hub_init,
       .open       = hub_open,
       .set_config = hub_set_config,
@@ -100,7 +107,6 @@ static usbh_class_driver_t const usbh_class_drivers[] =
   #if CFG_TUH_VENDOR
     {
       DRIVER_NAME("VENDOR")
-      .class_code = TUSB_CLASS_VENDOR_SPECIFIC,
       .init       = cush_init,
       .open       = cush_open_subtask,
       .xfer_cb    = cush_isr,
@@ -135,6 +141,7 @@ CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t _usbh_ctrl_buf[CFG_TUH_EN
 //------------- Helper Function Prototypes -------------//
 static bool enum_new_device(hcd_event_t* event);
 static void process_device_unplugged(uint8_t rhport, uint8_t hub_addr, uint8_t hub_port);
+static bool usbh_edpt_control_open(uint8_t dev_addr, uint8_t max_packet_size);
 
 // from usbh_control.c
 extern bool usbh_control_xfer_cb (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
@@ -195,8 +202,8 @@ bool tuh_init(uint8_t rhport)
     TU_ASSERT(dev->mutex);
 #endif
 
-    memset(dev->itf2drv, 0xff, sizeof(dev->itf2drv)); // invalid mapping
-    memset(dev->ep2drv , 0xff, sizeof(dev->ep2drv )); // invalid mapping
+    memset(dev->itf2drv, DRVID_INVALID, sizeof(dev->itf2drv)); // invalid mapping
+    memset(dev->ep2drv , DRVID_INVALID, sizeof(dev->ep2drv )); // invalid mapping
   }
 
   // Class drivers init
@@ -315,151 +322,6 @@ uint8_t* usbh_get_enum_buf(void)
 }
 
 //--------------------------------------------------------------------+
-// Endpoint API
-//--------------------------------------------------------------------+
-
-// TODO has some duplication code with device, refactor later
-bool usbh_edpt_claim(uint8_t dev_addr, uint8_t ep_addr)
-{
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
-
-  usbh_device_t* dev = &_usbh_devices[dev_addr];
-
-#if CFG_TUSB_OS != OPT_OS_NONE
-  // pre-check to help reducing mutex lock
-  TU_VERIFY((dev->ep_status[epnum][dir].busy == 0) && (dev->ep_status[epnum][dir].claimed == 0));
-  osal_mutex_lock(dev->mutex, OSAL_TIMEOUT_WAIT_FOREVER);
-#endif
-
-  // can only claim the endpoint if it is not busy and not claimed yet.
-  bool const ret = (dev->ep_status[epnum][dir].busy == 0) && (dev->ep_status[epnum][dir].claimed == 0);
-  if (ret)
-  {
-    dev->ep_status[epnum][dir].claimed = 1;
-  }
-
-#if CFG_TUSB_OS != OPT_OS_NONE
-  osal_mutex_unlock(dev->mutex);
-#endif
-
-  return ret;
-}
-
-// TODO has some duplication code with device, refactor later
-bool usbh_edpt_release(uint8_t dev_addr, uint8_t ep_addr)
-{
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
-
-  usbh_device_t* dev = &_usbh_devices[dev_addr];
-
-#if CFG_TUSB_OS != OPT_OS_NONE
-  osal_mutex_lock(dev->mutex, OSAL_TIMEOUT_WAIT_FOREVER);
-#endif
-
-  // can only release the endpoint if it is claimed and not busy
-  bool const ret = (dev->ep_status[epnum][dir].busy == 0) && (dev->ep_status[epnum][dir].claimed == 1);
-  if (ret)
-  {
-    dev->ep_status[epnum][dir].claimed = 0;
-  }
-
-#if CFG_TUSB_OS != OPT_OS_NONE
-  osal_mutex_unlock(dev->mutex);
-#endif
-
-  return ret;
-}
-
-// TODO has some duplication code with device, refactor later
-bool usbh_edpt_xfer(uint8_t dev_addr, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
-{
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
-
-  usbh_device_t* dev = &_usbh_devices[dev_addr];
-
-  TU_LOG2("  Queue EP %02X with %u bytes ... ", ep_addr, total_bytes);
-
-  // Attempt to transfer on a busy endpoint, sound like an race condition !
-  TU_ASSERT(dev->ep_status[epnum][dir].busy == 0);
-
-  // Set busy first since the actual transfer can be complete before hcd_edpt_xfer()
-  // could return and USBH task can preempt and clear the busy
-  dev->ep_status[epnum][dir].busy = true;
-
-  if ( hcd_edpt_xfer(dev->rhport, dev_addr, ep_addr, buffer, total_bytes) )
-  {
-    TU_LOG2("OK\r\n");
-    return true;
-  }else
-  {
-    // HCD error, mark endpoint as ready to allow next transfer
-    dev->ep_status[epnum][dir].busy = false;
-    dev->ep_status[epnum][dir].claimed = 0;
-    TU_LOG2("failed\r\n");
-    TU_BREAKPOINT();
-    return false;
-  }
-}
-
-bool usbh_edpt_control_open(uint8_t dev_addr, uint8_t max_packet_size)
-{
-  TU_LOG2("Open EP Control with Size = %u\r\n", max_packet_size);
-
-  tusb_desc_endpoint_t ep0_desc =
-  {
-    .bLength          = sizeof(tusb_desc_endpoint_t),
-    .bDescriptorType  = TUSB_DESC_ENDPOINT,
-    .bEndpointAddress = 0,
-    .bmAttributes     = { .xfer = TUSB_XFER_CONTROL },
-    .wMaxPacketSize   = { .size = max_packet_size },
-    .bInterval        = 0
-  };
-
-  return hcd_edpt_open(_usbh_devices[dev_addr].rhport, dev_addr, &ep0_desc);
-}
-
-bool usbh_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const * desc_ep)
-{
-  TU_LOG2("  Open EP %02X with Size = %u\r\n", desc_ep->bEndpointAddress, desc_ep->wMaxPacketSize.size);
-
-  bool ret = hcd_edpt_open(rhport, dev_addr, desc_ep);
-
-  if (ret)
-  {
-    usbh_device_t* dev = &_usbh_devices[dev_addr];
-
-    // new endpoints belongs to latest interface (last valid value)
-    // TODO FIXME not true with ISO
-    uint8_t drvid = 0xff;
-    for(uint8_t i=0; i < sizeof(dev->itf2drv); i++)
-    {
-      if ( dev->itf2drv[i] == 0xff ) break;
-      drvid = dev->itf2drv[i];
-    }
-    TU_ASSERT(drvid < USBH_CLASS_DRIVER_COUNT);
-
-    uint8_t const ep_addr = desc_ep->bEndpointAddress;
-    dev->ep2drv[tu_edpt_number(ep_addr)][tu_edpt_dir(ep_addr)] = drvid;
-  }
-
-  return ret;
-}
-
-bool usbh_edpt_busy(uint8_t dev_addr, uint8_t ep_addr)
-{
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
-
-  usbh_device_t* dev = &_usbh_devices[dev_addr];
-
-  return dev->ep_status[epnum][dir].busy;
-}
-
-
-//--------------------------------------------------------------------+
 // HCD Event Handler
 //--------------------------------------------------------------------+
 
@@ -473,7 +335,6 @@ void hcd_event_handler(hcd_event_t const* event, bool in_isr)
   }
 }
 
-// interrupt caused by a TD (with IOC=1) in pipe of class class_code
 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 =
@@ -546,8 +407,8 @@ void process_device_unplugged(uint8_t rhport, uint8_t hub_addr, uint8_t hub_port
         usbh_class_drivers[drv_id].close(dev_addr);
       }
 
-      memset(dev->itf2drv, 0xff, sizeof(dev->itf2drv)); // invalid mapping
-      memset(dev->ep2drv , 0xff, sizeof(dev->ep2drv )); // invalid mapping
+      memset(dev->itf2drv, DRVID_INVALID, sizeof(dev->itf2drv)); // invalid mapping
+      memset(dev->ep2drv , DRVID_INVALID, sizeof(dev->ep2drv )); // invalid mapping
 
       hcd_device_close(rhport, dev_addr);
 
@@ -576,7 +437,7 @@ void usbh_driver_set_config_complete(uint8_t dev_addr, uint8_t itf_num)
   {
     // continue with next valid interface
     uint8_t const drv_id = dev->itf2drv[itf_num];
-    if (drv_id != 0xff)
+    if (drv_id != DRVID_INVALID)
     {
       usbh_class_driver_t const * driver = &usbh_class_drivers[drv_id];
       TU_LOG2("%s set config: itf = %u\r\n", driver->name, itf_num);
@@ -841,7 +702,7 @@ static bool enum_set_address_complete(uint8_t dev_addr, tusb_control_request_t c
   dev0->state = TUSB_DEVICE_STATE_UNPLUG;
 
   // open control pipe for new address
-  TU_ASSERT ( usbh_edpt_control_open(new_addr, new_dev->ep0_packet_size) );
+  TU_ASSERT( usbh_edpt_control_open(new_addr, new_dev->ep0_packet_size) );
 
   // Get full device descriptor
   TU_LOG2("Get Device Descriptor\r\n");
@@ -972,11 +833,11 @@ static bool enum_set_config_complete(uint8_t dev_addr, tusb_control_request_t co
   dev->configured = 1;
   dev->state = TUSB_DEVICE_STATE_CONFIGURED;
 
-  // Start the Set Configuration process for interfaces (itf = 0xff)
+  // Start the Set Configuration process for interfaces (itf = DRVID_INVALID)
   // 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 usig 0xff
-  usbh_driver_set_config_complete(dev_addr, 0xff);
+  // TODO use separated API instead of using DRVID_INVALID
+  usbh_driver_set_config_complete(dev_addr, DRVID_INVALID);
 
   return true;
 }
@@ -992,58 +853,206 @@ static bool parse_configuration_descriptor(uint8_t dev_addr, tusb_desc_configura
   while( p_desc < desc_end )
   {
     // TODO Do we need to use IAD
-    // tusb_desc_interface_assoc_t const * desc_itf_assoc = NULL;
+     tusb_desc_interface_assoc_t const * desc_iad = NULL;
 
     // Class will always starts with Interface Association (if any) and then Interface descriptor
     if ( TUSB_DESC_INTERFACE_ASSOCIATION == tu_desc_type(p_desc) )
     {
-      // desc_itf_assoc = (tusb_desc_interface_assoc_t const *) p_desc;
+      desc_iad = (tusb_desc_interface_assoc_t const *) p_desc;
       p_desc = tu_desc_next(p_desc);
     }
 
     TU_ASSERT( TUSB_DESC_INTERFACE == tu_desc_type(p_desc) );
 
     tusb_desc_interface_t const* desc_itf = (tusb_desc_interface_t const*) p_desc;
-    uint16_t const remaining_len = desc_end-p_desc;
 
-    // Check if class is supported TODO drop class_code
-    uint8_t drv_id;
-    for (drv_id = 0; drv_id < USBH_CLASS_DRIVER_COUNT; drv_id++)
-    {
-      if ( usbh_class_drivers[drv_id].class_code == desc_itf->bInterfaceClass ) break;
-    }
+    // Interface number must not be used already
+    TU_ASSERT( dev->itf2drv[desc_itf->bInterfaceNumber] == DRVID_INVALID );
 
-    if( drv_id >= USBH_CLASS_DRIVER_COUNT )
+    uint16_t const drv_len = tu_desc_get_interface_total_len(desc_itf, desc_iad ? desc_iad->bInterfaceCount : 1, desc_end-p_desc);
+    TU_ASSERT(drv_len);
+
+    if (desc_itf->bInterfaceClass == TUSB_CLASS_HUB && dev->hub_addr != 0)
     {
-      // skip unsupported class
-      p_desc = tu_desc_next(p_desc);
+      // TODO Attach hub to Hub is not currently supported
+      // skip this interface
+      TU_LOG(USBH_DBG_LVL, "Only 1 level of HUB is supported\r\n");
     }
     else
     {
-      usbh_class_driver_t const * driver = &usbh_class_drivers[drv_id];
+      // Find driver for this interface
+      uint8_t drv_id;
+      for (drv_id = 0; drv_id < USBH_CLASS_DRIVER_COUNT; drv_id++)
+      {
+        usbh_class_driver_t const * driver = &usbh_class_drivers[drv_id];
 
-      // Interface number must not be used already TODO alternate interface
-      TU_ASSERT( dev->itf2drv[desc_itf->bInterfaceNumber] == 0xff );
-      dev->itf2drv[desc_itf->bInterfaceNumber] = drv_id;
+        if ( driver->open(dev->rhport, dev_addr, desc_itf, drv_len) )
+        {
+          // open successfully
+          TU_LOG2("%s opened\r\n", driver->name);
 
-      if (desc_itf->bInterfaceClass == TUSB_CLASS_HUB && dev->hub_addr != 0)
-      {
-        // TODO Attach hub to Hub is not currently supported
-        // skip this interface
-        p_desc = tu_desc_next(p_desc);
+          // bind interface to found driver
+          dev->itf2drv[desc_itf->bInterfaceNumber] = drv_id;
+
+          // If using IAD, bind all interfaces to the same driver
+          if (desc_iad)
+          {
+            // IAD's first interface number and class should match with opened interface
+            TU_ASSERT(desc_iad->bFirstInterface == desc_itf->bInterfaceNumber &&
+                      desc_iad->bFunctionClass  == desc_itf->bInterfaceClass);
+
+            for(uint8_t i=1; i<desc_iad->bInterfaceCount; i++)
+            {
+              dev->itf2drv[desc_itf->bInterfaceNumber+i] = drv_id;
+            }
+          }
+
+          // bind all endpoints to found driver
+          tu_edpt_bind_driver(dev->ep2drv, desc_itf, drv_len, drv_id);
+
+          break; // exit driver find loop
+        }
       }
-      else
-      {
-        TU_LOG2("%s open\r\n", driver->name);
 
-        uint16_t const itf_len = driver->open(dev->rhport, dev_addr, desc_itf, remaining_len);
-        TU_ASSERT( sizeof(tusb_desc_interface_t) <= itf_len && itf_len <= remaining_len);
-        p_desc += itf_len;
+      if( drv_id >= USBH_CLASS_DRIVER_COUNT )
+      {
+        TU_LOG(USBH_DBG_LVL, "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);
       }
     }
+
+    // next Interface or IAD descriptor
+    p_desc += drv_len;
   }
 
   return true;
 }
 
+//--------------------------------------------------------------------+
+// Endpoint API
+//--------------------------------------------------------------------+
+
+// TODO has some duplication code with device, refactor later
+bool usbh_edpt_claim(uint8_t dev_addr, uint8_t ep_addr)
+{
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir   = tu_edpt_dir(ep_addr);
+
+  usbh_device_t* dev = &_usbh_devices[dev_addr];
+
+#if CFG_TUSB_OS != OPT_OS_NONE
+  // pre-check to help reducing mutex lock
+  TU_VERIFY((dev->ep_status[epnum][dir].busy == 0) && (dev->ep_status[epnum][dir].claimed == 0));
+  osal_mutex_lock(dev->mutex, OSAL_TIMEOUT_WAIT_FOREVER);
+#endif
+
+  // can only claim the endpoint if it is not busy and not claimed yet.
+  bool const ret = (dev->ep_status[epnum][dir].busy == 0) && (dev->ep_status[epnum][dir].claimed == 0);
+  if (ret)
+  {
+    dev->ep_status[epnum][dir].claimed = 1;
+  }
+
+#if CFG_TUSB_OS != OPT_OS_NONE
+  osal_mutex_unlock(dev->mutex);
+#endif
+
+  return ret;
+}
+
+// TODO has some duplication code with device, refactor later
+bool usbh_edpt_release(uint8_t dev_addr, uint8_t ep_addr)
+{
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir   = tu_edpt_dir(ep_addr);
+
+  usbh_device_t* dev = &_usbh_devices[dev_addr];
+
+#if CFG_TUSB_OS != OPT_OS_NONE
+  osal_mutex_lock(dev->mutex, OSAL_TIMEOUT_WAIT_FOREVER);
+#endif
+
+  // can only release the endpoint if it is claimed and not busy
+  bool const ret = (dev->ep_status[epnum][dir].busy == 0) && (dev->ep_status[epnum][dir].claimed == 1);
+  if (ret)
+  {
+    dev->ep_status[epnum][dir].claimed = 0;
+  }
+
+#if CFG_TUSB_OS != OPT_OS_NONE
+  osal_mutex_unlock(dev->mutex);
+#endif
+
+  return ret;
+}
+
+// TODO has some duplication code with device, refactor later
+bool usbh_edpt_xfer(uint8_t dev_addr, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
+{
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir   = tu_edpt_dir(ep_addr);
+
+  usbh_device_t* dev = &_usbh_devices[dev_addr];
+
+  TU_LOG2("  Queue EP %02X with %u bytes ... ", ep_addr, total_bytes);
+
+  // Attempt to transfer on a busy endpoint, sound like an race condition !
+  TU_ASSERT(dev->ep_status[epnum][dir].busy == 0);
+
+  // Set busy first since the actual transfer can be complete before hcd_edpt_xfer()
+  // could return and USBH task can preempt and clear the busy
+  dev->ep_status[epnum][dir].busy = true;
+
+  if ( hcd_edpt_xfer(dev->rhport, dev_addr, ep_addr, buffer, total_bytes) )
+  {
+    TU_LOG2("OK\r\n");
+    return true;
+  }else
+  {
+    // HCD error, mark endpoint as ready to allow next transfer
+    dev->ep_status[epnum][dir].busy = false;
+    dev->ep_status[epnum][dir].claimed = 0;
+    TU_LOG2("failed\r\n");
+    TU_BREAKPOINT();
+    return false;
+  }
+}
+
+static bool usbh_edpt_control_open(uint8_t dev_addr, uint8_t max_packet_size)
+{
+  TU_LOG2("Open EP Control with Size = %u\r\n", max_packet_size);
+
+  tusb_desc_endpoint_t ep0_desc =
+  {
+    .bLength          = sizeof(tusb_desc_endpoint_t),
+    .bDescriptorType  = TUSB_DESC_ENDPOINT,
+    .bEndpointAddress = 0,
+    .bmAttributes     = { .xfer = TUSB_XFER_CONTROL },
+    .wMaxPacketSize   = { .size = max_packet_size },
+    .bInterval        = 0
+  };
+
+  return hcd_edpt_open(_usbh_devices[dev_addr].rhport, dev_addr, &ep0_desc);
+}
+
+bool usbh_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const * desc_ep)
+{
+  usbh_device_t* dev = &_usbh_devices[dev_addr];
+  TU_ASSERT(tu_edpt_validate(desc_ep, (tusb_speed_t) dev->speed));
+
+  return hcd_edpt_open(rhport, dev_addr, desc_ep);
+}
+
+bool usbh_edpt_busy(uint8_t dev_addr, uint8_t ep_addr)
+{
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir   = tu_edpt_dir(ep_addr);
+
+  usbh_device_t* dev = &_usbh_devices[dev_addr];
+
+  return dev->ep_status[epnum][dir].busy;
+}
+
+
+
 #endif
diff --git a/src/host/usbh_classdriver.h b/src/host/usbh_classdriver.h
index 0736fefa1..8bc2622aa 100644
--- a/src/host/usbh_classdriver.h
+++ b/src/host/usbh_classdriver.h
@@ -43,13 +43,11 @@ typedef struct {
   char const* name;
   #endif
 
-  uint8_t class_code;
-
-  void     (* const init       )(void);
-  uint16_t (* 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);
+  void (* const init       )(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;
 
 // Call by class driver to tell USBH that it has complete the enumeration
@@ -73,6 +71,8 @@ bool usbh_edpt_xfer(uint8_t dev_addr, uint8_t ep_addr, uint8_t * buffer, uint16_
 // 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);
 
+bool usbh_edpt_release(uint8_t dev_addr, uint8_t ep_addr);
+
 // Check if endpoint transferring is complete
 bool usbh_edpt_busy(uint8_t dev_addr, uint8_t ep_addr);
 
diff --git a/src/host/usbh_hcd.h b/src/host/usbh_hcd.h
index b3856d6b7..450b3e447 100644
--- a/src/host/usbh_hcd.h
+++ b/src/host/usbh_hcd.h
@@ -73,9 +73,9 @@ typedef struct {
     uint8_t suspended    : 1;
   };
 
-  volatile uint8_t state;             // device state, value from enum tusbh_device_state_t
+  volatile uint8_t state;            // device state, value from enum tusbh_device_state_t
 
-  uint8_t itf2drv[16];  // map interface number to driver (0xff is invalid)
+  uint8_t itf2drv[16];               // map interface number to driver (0xff is invalid)
   uint8_t ep2drv[CFG_TUH_EP_MAX][2]; // map endpoint to driver ( 0xff is invalid )
 
   struct TU_ATTR_PACKED
diff --git a/src/tusb.c b/src/tusb.c
index 0350fa1de..1fc775399 100644
--- a/src/tusb.c
+++ b/src/tusb.c
@@ -63,6 +63,98 @@ bool tusb_inited(void)
   return ret;
 }
 
+//--------------------------------------------------------------------+
+// Internal Helper for both Host and Device stack
+//--------------------------------------------------------------------+
+
+bool tu_edpt_validate(tusb_desc_endpoint_t const * desc_ep, tusb_speed_t speed)
+{
+  uint16_t const max_packet_size = tu_le16toh(desc_ep->wMaxPacketSize.size);
+  TU_LOG2("  Open EP %02X with Size = %u\r\n", desc_ep->bEndpointAddress, max_packet_size);
+
+  switch (desc_ep->bmAttributes.xfer)
+  {
+    case TUSB_XFER_ISOCHRONOUS:
+    {
+      uint16_t const spec_size = (speed == TUSB_SPEED_HIGH ? 1024 : 1023);
+      TU_ASSERT(max_packet_size <= spec_size);
+    }
+    break;
+
+    case TUSB_XFER_BULK:
+      if (speed == TUSB_SPEED_HIGH)
+      {
+        // Bulk highspeed must be EXACTLY 512
+        TU_ASSERT(max_packet_size == 512);
+      }else
+      {
+        // TODO Bulk fullspeed can only be 8, 16, 32, 64
+        TU_ASSERT(max_packet_size <= 64);
+      }
+    break;
+
+    case TUSB_XFER_INTERRUPT:
+    {
+      uint16_t const spec_size = (speed == TUSB_SPEED_HIGH ? 1024 : 64);
+      TU_ASSERT(max_packet_size <= spec_size);
+    }
+    break;
+
+    default: return false;
+  }
+
+  return true;
+}
+
+void tu_edpt_bind_driver(uint8_t ep2drv[][2], tusb_desc_interface_t const* desc_itf, uint16_t desc_len, uint8_t driver_id)
+{
+  uint8_t const* p_desc = (uint8_t const*) desc_itf;
+  uint16_t len = 0;
+
+  while( len < desc_len )
+  {
+    if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) )
+    {
+      uint8_t const ep_addr = ((tusb_desc_endpoint_t const*) p_desc)->bEndpointAddress;
+
+      ep2drv[tu_edpt_number(ep_addr)][tu_edpt_dir(ep_addr)] = driver_id;
+    }
+
+    len   = (uint16_t)(len + tu_desc_len(p_desc));
+    p_desc = tu_desc_next(p_desc);
+  }
+}
+
+uint16_t tu_desc_get_interface_total_len(tusb_desc_interface_t const* desc_itf, uint8_t itf_count, uint16_t max_len)
+{
+  uint8_t const* p_desc = (uint8_t const*) desc_itf;
+  uint16_t len = 0;
+
+  while (itf_count--)
+  {
+    // Next on interface desc
+    len += tu_desc_len(desc_itf);
+    p_desc = tu_desc_next(p_desc);
+
+    while (len < max_len)
+    {
+      // return on IAD regardless of itf count
+      if ( tu_desc_type(p_desc) == TUSB_DESC_INTERFACE_ASSOCIATION ) return len;
+
+      if ( (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE) &&
+           ((tusb_desc_interface_t const*) p_desc)->bAlternateSetting == 0 )
+      {
+        break;
+      }
+
+      len += tu_desc_len(p_desc);
+      p_desc = tu_desc_next(p_desc);
+    }
+  }
+
+  return len;
+}
+
 /*------------------------------------------------------------------*/
 /* Debug
  *------------------------------------------------------------------*/