Merge remote-tracking branch 'upstream/master'

40 files changed, 507 insertions, 283 deletions

diff --git a/src/class/bth/bth_device.c b/src/class/bth/bth_device.c
index 04011bcdb..252e20e37 100755
--- a/src/class/bth/bth_device.c
+++ b/src/class/bth/bth_device.c
@@ -121,7 +121,7 @@ uint16_t btd_open(uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_
     desc_ep = (tusb_desc_endpoint_t const *) tu_desc_next(itf_desc);
 
     TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType && TUSB_XFER_INTERRUPT == desc_ep->bmAttributes.xfer, 0);
-    TU_ASSERT(dcd_edpt_open(rhport, desc_ep), 0);
+    TU_ASSERT(usbd_edpt_open(rhport, desc_ep), 0);
     _btd_itf.ep_ev = desc_ep->bEndpointAddress;
 
     // Open endpoint pair
diff --git a/src/class/cdc/cdc_device.c b/src/class/cdc/cdc_device.c
index 89326f71d..b4ebfc92e 100644
--- a/src/class/cdc/cdc_device.c
+++ b/src/class/cdc/cdc_device.c
@@ -164,7 +164,7 @@ uint32_t tud_cdc_n_write_flush (uint8_t itf)
   // skip if previous transfer not complete yet
   TU_VERIFY( !usbd_edpt_busy(TUD_OPT_RHPORT, p_cdc->ep_in), 0 );
 
-  uint16_t count = tu_fifo_read_n(&_cdcd_itf[itf].tx_ff, p_cdc->epin_buf, sizeof(p_cdc->epin_buf));
+  uint16_t count = tu_fifo_read_n(&p_cdc->tx_ff, p_cdc->epin_buf, sizeof(p_cdc->epin_buf));
   if ( count )
   {
     TU_VERIFY( tud_cdc_n_connected(itf), 0 ); // fifo is empty if not connected
@@ -376,7 +376,6 @@ bool cdcd_control_request(uint8_t rhport, tusb_control_request_t const * request
 
 bool cdcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
 {
-  (void) rhport;
   (void) result;
 
   uint8_t itf;
@@ -393,6 +392,7 @@ bool cdcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_
   // Received new data
   if ( ep_addr == p_cdc->ep_out )
   {
+    // TODO search for wanted char first for better performance
     for(uint32_t i=0; i<xferred_bytes; i++)
     {
       tu_fifo_write(&p_cdc->rx_ff, &p_cdc->epout_buf[i]);
@@ -416,13 +416,17 @@ bool cdcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_
   //       Though maybe the baudrate is not really important !!!
   if ( ep_addr == p_cdc->ep_in )
   {
+    // invoke transmit callback to possibly refill tx fifo
+    if ( tud_cdc_tx_complete_cb ) tud_cdc_tx_complete_cb(itf);
+
     if ( 0 == tud_cdc_n_write_flush(itf) )
     {
       // There is no data left, a ZLP should be sent if
       // xferred_bytes is multiple of EP size and not zero
+      // FIXME CFG_TUD_CDC_EP_BUFSIZE is not Endpoint packet size
       if ( xferred_bytes && (0 == (xferred_bytes % CFG_TUD_CDC_EP_BUFSIZE)) )
       {
-        usbd_edpt_xfer(TUD_OPT_RHPORT, p_cdc->ep_in, NULL, 0);
+        usbd_edpt_xfer(rhport, p_cdc->ep_in, NULL, 0);
       }
     }
   }
diff --git a/src/class/cdc/cdc_device.h b/src/class/cdc/cdc_device.h
index 9d257bbe9..145381c42 100644
--- a/src/class/cdc/cdc_device.h
+++ b/src/class/cdc/cdc_device.h
@@ -57,7 +57,6 @@
 // CFG_TUD_CDC > 1
 //--------------------------------------------------------------------+
 
-
 // Check if terminal is connected to this port
 bool     tud_cdc_n_connected       (uint8_t itf);
 
@@ -133,6 +132,9 @@ TU_ATTR_WEAK void tud_cdc_rx_cb(uint8_t itf);
 // Invoked when received `wanted_char`
 TU_ATTR_WEAK void tud_cdc_rx_wanted_cb(uint8_t itf, char wanted_char);
 
+// Invoked when space becomes available in TX buffer
+TU_ATTR_WEAK void tud_cdc_tx_complete_cb(uint8_t itf);
+
 // Invoked when line state DTR & RTS are changed via SET_CONTROL_LINE_STATE
 TU_ATTR_WEAK void tud_cdc_line_state_cb(uint8_t itf, bool dtr, bool rts);
 
diff --git a/src/class/cdc/cdc_host.c b/src/class/cdc/cdc_host.c
index 5e45e8f6b..e62f47b72 100644
--- a/src/class/cdc/cdc_host.c
+++ b/src/class/cdc/cdc_host.c
@@ -86,7 +86,6 @@ bool tuh_cdc_serial_is_mounted(uint8_t dev_addr)
 {
   // TODO consider all AT Command as serial candidate
   return tuh_cdc_mounted(dev_addr)                                         &&
-      (CDC_COMM_PROTOCOL_NONE <= cdch_data[dev_addr-1].itf_protocol) &&
       (cdch_data[dev_addr-1].itf_protocol <= CDC_COMM_PROTOCOL_ATCOMMAND_CDMA);
 }
 
@@ -159,7 +158,7 @@ bool cdch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *it
     // notification endpoint
     tusb_desc_endpoint_t const * ep_desc = (tusb_desc_endpoint_t const *) p_desc;
 
-    TU_ASSERT( hcd_edpt_open(rhport, dev_addr, ep_desc) );
+    TU_ASSERT( usbh_edpt_open(rhport, dev_addr, ep_desc) );
     p_cdc->ep_notif = ep_desc->bEndpointAddress;
 
     (*p_length) += p_desc[DESC_OFFSET_LEN];
@@ -180,7 +179,7 @@ bool cdch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *it
       TU_ASSERT(TUSB_DESC_ENDPOINT == ep_desc->bDescriptorType);
       TU_ASSERT(TUSB_XFER_BULK == ep_desc->bmAttributes.xfer);
 
-      TU_ASSERT(hcd_edpt_open(rhport, dev_addr, ep_desc));
+      TU_ASSERT(usbh_edpt_open(rhport, dev_addr, ep_desc));
 
       if ( tu_edpt_dir(ep_desc->bEndpointAddress) ==  TUSB_DIR_IN )
       {
diff --git a/src/class/hid/hid_host.c b/src/class/hid/hid_host.c
index 69c49b012..378278cc3 100644
--- a/src/class/hid/hid_host.c
+++ b/src/class/hid/hid_host.c
@@ -35,35 +35,43 @@
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
 
+typedef struct {
+  uint8_t  itf_num;
+  uint8_t  ep_in;
+  uint8_t  ep_out;
+
+  uint16_t report_size;
+}hidh_interface_t;
+
 //--------------------------------------------------------------------+
 // HID Interface common functions
 //--------------------------------------------------------------------+
-static inline bool hidh_interface_open(uint8_t dev_addr, uint8_t interface_number, tusb_desc_endpoint_t const *p_endpoint_desc, hidh_interface_info_t *p_hid)
+static inline bool hidh_interface_open(uint8_t rhport, uint8_t dev_addr, uint8_t interface_number, tusb_desc_endpoint_t const *p_endpoint_desc, hidh_interface_t *p_hid)
 {
-  p_hid->pipe_hdl         = hcd_edpt_open(dev_addr, p_endpoint_desc, TUSB_CLASS_HID);
-  p_hid->report_size      = p_endpoint_desc->wMaxPacketSize.size; // TODO get size from report descriptor
-  p_hid->interface_number = interface_number;
+  TU_ASSERT( usbh_edpt_open(rhport, dev_addr, p_endpoint_desc) );
 
-  TU_ASSERT (pipehandle_is_valid(p_hid->pipe_hdl));
+  p_hid->ep_in       = p_endpoint_desc->bEndpointAddress;
+  p_hid->report_size = p_endpoint_desc->wMaxPacketSize.size; // TODO get size from report descriptor
+  p_hid->itf_num     = interface_number;
 
   return true;
 }
 
-static inline void hidh_interface_close(hidh_interface_info_t *p_hid)
+static inline void hidh_interface_close(hidh_interface_t *p_hid)
 {
-  tu_memclr(p_hid, sizeof(hidh_interface_info_t));
+  tu_memclr(p_hid, sizeof(hidh_interface_t));
 }
 
 // called from public API need to validate parameters
-tusb_error_t hidh_interface_get_report(uint8_t dev_addr, void * report, hidh_interface_info_t *p_hid)
+tusb_error_t hidh_interface_get_report(uint8_t dev_addr, void * report, hidh_interface_t *p_hid)
 {
   //------------- parameters validation -------------//
   // TODO change to use is configured function
-  TU_ASSERT (TUSB_DEVICE_STATE_CONFIGURED == tuh_device_get_state(dev_addr), TUSB_ERROR_DEVICE_NOT_READY);
-  TU_VERIFY (report, TUSB_ERROR_INVALID_PARA);
-  TU_ASSERT (!hcd_edpt_busy(p_hid->pipe_hdl), TUSB_ERROR_INTERFACE_IS_BUSY);
+  TU_ASSERT(TUSB_DEVICE_STATE_CONFIGURED == tuh_device_get_state(dev_addr), TUSB_ERROR_DEVICE_NOT_READY);
+  TU_VERIFY(report, TUSB_ERROR_INVALID_PARA);
+  TU_ASSERT(!hcd_edpt_busy(dev_addr, p_hid->ep_in), TUSB_ERROR_INTERFACE_IS_BUSY);
 
-  TU_ASSERT_ERR( hcd_pipe_xfer(p_hid->pipe_hdl, report, p_hid->report_size, true) ) ;
+  TU_ASSERT( hcd_pipe_xfer(dev_addr, p_hid->ep_in, report, p_hid->report_size, true) ) ;
 
   return TUSB_ERROR_NONE;
 }
@@ -73,24 +81,12 @@ tusb_error_t hidh_interface_get_report(uint8_t dev_addr, void * report, hidh_int
 //--------------------------------------------------------------------+
 #if CFG_TUH_HID_KEYBOARD
 
-#if 0
-#define EXPAND_KEYCODE_TO_ASCII(keycode, ascii, shift_modified)  \
-  [0][keycode] = ascii,\
-  [1][keycode] = shift_modified,\
-
-// TODO size of table should be a macro for application to check boundary
-uint8_t const hid_keycode_to_ascii_tbl[2][128] =
-{
-    HID_KEYCODE_TABLE(EXPAND_KEYCODE_TO_ASCII)
-};
-#endif
-
-static hidh_interface_info_t keyboardh_data[CFG_TUSB_HOST_DEVICE_MAX]; // does not have addr0, index = dev_address-1
+static hidh_interface_t keyboardh_data[CFG_TUSB_HOST_DEVICE_MAX]; // does not have addr0, index = dev_address-1
 
 //------------- KEYBOARD PUBLIC API (parameter validation required) -------------//
 bool  tuh_hid_keyboard_is_mounted(uint8_t dev_addr)
 {
-  return tuh_device_is_configured(dev_addr) && pipehandle_is_valid(keyboardh_data[dev_addr-1].pipe_hdl);
+  return tuh_device_is_configured(dev_addr) && (keyboardh_data[dev_addr-1].ep_in != 0);
 }
 
 tusb_error_t tuh_hid_keyboard_get_report(uint8_t dev_addr, void* p_report)
@@ -100,8 +96,7 @@ tusb_error_t tuh_hid_keyboard_get_report(uint8_t dev_addr, void* p_report)
 
 bool tuh_hid_keyboard_is_busy(uint8_t dev_addr)
 {
-  return  tuh_hid_keyboard_is_mounted(dev_addr) &&
-          hcd_edpt_busy( keyboardh_data[dev_addr-1].pipe_hdl );
+  return  tuh_hid_keyboard_is_mounted(dev_addr) && hcd_edpt_busy(dev_addr, keyboardh_data[dev_addr-1].ep_in);
 }
 
 #endif
@@ -111,18 +106,17 @@ bool tuh_hid_keyboard_is_busy(uint8_t dev_addr)
 //--------------------------------------------------------------------+
 #if CFG_TUH_HID_MOUSE
 
-static hidh_interface_info_t mouseh_data[CFG_TUSB_HOST_DEVICE_MAX]; // does not have addr0, index = dev_address-1
+static hidh_interface_t mouseh_data[CFG_TUSB_HOST_DEVICE_MAX]; // does not have addr0, index = dev_address-1
 
 //------------- Public API -------------//
 bool tuh_hid_mouse_is_mounted(uint8_t dev_addr)
 {
-  return tuh_device_is_configured(dev_addr) && pipehandle_is_valid(mouseh_data[dev_addr-1].pipe_hdl);
+  return tuh_device_is_configured(dev_addr) && (mouseh_data[dev_addr-1].ep_in != 0);
 }
 
 bool tuh_hid_mouse_is_busy(uint8_t dev_addr)
 {
-  return  tuh_hid_mouse_is_mounted(dev_addr) &&
-          hcd_edpt_busy( mouseh_data[dev_addr-1].pipe_hdl );
+  return  tuh_hid_mouse_is_mounted(dev_addr) && hcd_edpt_busy(dev_addr, mouseh_data[dev_addr-1].ep_in);
 }
 
 tusb_error_t tuh_hid_mouse_get_report(uint8_t dev_addr, void * report)
@@ -149,11 +143,11 @@ tusb_error_t tuh_hid_mouse_get_report(uint8_t dev_addr, void * report)
 void hidh_init(void)
 {
 #if CFG_TUH_HID_KEYBOARD
-  tu_memclr(&keyboardh_data, sizeof(hidh_interface_info_t)*CFG_TUSB_HOST_DEVICE_MAX);
+  tu_memclr(&keyboardh_data, sizeof(hidh_interface_t)*CFG_TUSB_HOST_DEVICE_MAX);
 #endif
 
 #if CFG_TUH_HID_MOUSE
-  tu_memclr(&mouseh_data, sizeof(hidh_interface_info_t)*CFG_TUSB_HOST_DEVICE_MAX);
+  tu_memclr(&mouseh_data, sizeof(hidh_interface_t)*CFG_TUSB_HOST_DEVICE_MAX);
 #endif
 
 #if CFG_TUSB_HOST_HID_GENERIC
@@ -165,7 +159,7 @@ void hidh_init(void)
 CFG_TUSB_MEM_SECTION uint8_t report_descriptor[256];
 #endif
 
-bool hidh_open_subtask(uint8_t dev_addr, tusb_desc_interface_t const *p_interface_desc, uint16_t *p_length)
+bool hidh_open_subtask(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *p_interface_desc, uint16_t *p_length)
 {
   uint8_t const *p_desc = (uint8_t const *) p_interface_desc;
 
@@ -208,7 +202,8 @@ bool hidh_open_subtask(uint8_t dev_addr, tusb_desc_interface_t const *p_interfac
     #if CFG_TUH_HID_KEYBOARD
     if ( HID_PROTOCOL_KEYBOARD == p_interface_desc->bInterfaceProtocol)
     {
-      TU_ASSERT( hidh_interface_open(dev_addr, p_interface_desc->bInterfaceNumber, p_endpoint_desc, &keyboardh_data[dev_addr-1]) );
+      TU_ASSERT( hidh_interface_open(rhport, dev_addr, p_interface_desc->bInterfaceNumber, p_endpoint_desc, &keyboardh_data[dev_addr-1]) );
+      TU_LOG2_HEX(keyboardh_data[dev_addr-1].ep_in);
       tuh_hid_keyboard_mounted_cb(dev_addr);
     } else
     #endif
@@ -216,7 +211,8 @@ bool hidh_open_subtask(uint8_t dev_addr, tusb_desc_interface_t const *p_interfac
     #if CFG_TUH_HID_MOUSE
     if ( HID_PROTOCOL_MOUSE == p_interface_desc->bInterfaceProtocol)
     {
-      TU_ASSERT ( hidh_interface_open(dev_addr, p_interface_desc->bInterfaceNumber, p_endpoint_desc, &mouseh_data[dev_addr-1]) );
+      TU_ASSERT ( hidh_interface_open(rhport, dev_addr, p_interface_desc->bInterfaceNumber, p_endpoint_desc, &mouseh_data[dev_addr-1]) );
+      TU_LOG2_HEX(mouseh_data[dev_addr-1].ep_in);
       tuh_hid_mouse_mounted_cb(dev_addr);
     } else
     #endif
@@ -236,22 +232,22 @@ bool hidh_open_subtask(uint8_t dev_addr, tusb_desc_interface_t const *p_interfac
   return true;
 }
 
-void hidh_isr(pipe_handle_t pipe_hdl, xfer_result_t event, uint32_t xferred_bytes)
+void hidh_isr(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
 {
   (void) xferred_bytes; // TODO may need to use this para later
 
 #if CFG_TUH_HID_KEYBOARD
-  if ( pipehandle_is_equal(pipe_hdl, keyboardh_data[pipe_hdl.dev_addr-1].pipe_hdl) )
+  if ( ep_addr == keyboardh_data[dev_addr-1].ep_in )
   {
-    tuh_hid_keyboard_isr(pipe_hdl.dev_addr, event);
+    tuh_hid_keyboard_isr(dev_addr, event);
     return;
   }
 #endif
 
 #if CFG_TUH_HID_MOUSE
-  if ( pipehandle_is_equal(pipe_hdl, mouseh_data[pipe_hdl.dev_addr-1].pipe_hdl) )
+  if ( ep_addr == mouseh_data[dev_addr-1].ep_in )
   {
-    tuh_hid_mouse_isr(pipe_hdl.dev_addr, event);
+    tuh_hid_mouse_isr(dev_addr, event);
     return;
   }
 #endif
@@ -264,7 +260,7 @@ void hidh_isr(pipe_handle_t pipe_hdl, xfer_result_t event, uint32_t xferred_byte
 void hidh_close(uint8_t dev_addr)
 {
 #if CFG_TUH_HID_KEYBOARD
-  if ( pipehandle_is_valid( keyboardh_data[dev_addr-1].pipe_hdl ) )
+  if ( keyboardh_data[dev_addr-1].ep_in != 0 )
   {
     hidh_interface_close(&keyboardh_data[dev_addr-1]);
     tuh_hid_keyboard_unmounted_cb(dev_addr);
@@ -272,7 +268,7 @@ void hidh_close(uint8_t dev_addr)
 #endif
 
 #if CFG_TUH_HID_MOUSE
-  if( pipehandle_is_valid( mouseh_data[dev_addr-1].pipe_hdl ) )
+  if( mouseh_data[dev_addr-1].ep_in != 0 )
   {
     hidh_interface_close(&mouseh_data[dev_addr-1]);
     tuh_hid_mouse_unmounted_cb( dev_addr );
diff --git a/src/class/hid/hid_host.h b/src/class/hid/hid_host.h
index a3b614cd7..2e7f52674 100644
--- a/src/class/hid/hid_host.h
+++ b/src/class/hid/hid_host.h
@@ -195,15 +195,9 @@ void tuh_hid_generic_isr(uint8_t dev_addr, xfer_result_t event);
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-typedef struct {
-  pipe_handle_t pipe_hdl;
-  uint16_t report_size;
-  uint8_t interface_number;
-}hidh_interface_info_t;
-
 void hidh_init(void);
-bool hidh_open_subtask(uint8_t dev_addr, tusb_desc_interface_t const *p_interface_desc, uint16_t *p_length);
-void hidh_isr(pipe_handle_t pipe_hdl, xfer_result_t event, uint32_t xferred_bytes);
+bool hidh_open_subtask(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *p_interface_desc, uint16_t *p_length);
+void hidh_isr(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
 void hidh_close(uint8_t dev_addr);
 
 #ifdef __cplusplus
diff --git a/src/class/midi/midi_device.c b/src/class/midi/midi_device.c
index d4ed21c78..376b3670a 100644
--- a/src/class/midi/midi_device.c
+++ b/src/class/midi/midi_device.c
@@ -101,8 +101,7 @@ uint32_t tud_midi_n_read(uint8_t itf, uint8_t jack_id, void* buffer, uint32_t bu
 
   // Fill empty buffer
   if (midi->read_buffer_length == 0) {
-    if (!tud_midi_n_receive(itf, midi->read_buffer))
-      return 0;
+    if (!tud_midi_n_receive(itf, midi->read_buffer)) return 0;
 
     uint8_t code_index = midi->read_buffer[0] & 0x0f;
     // We always copy over the first byte.
@@ -119,8 +118,7 @@ uint32_t tud_midi_n_read(uint8_t itf, uint8_t jack_id, void* buffer, uint32_t bu
   }
 
   uint32_t n = midi->read_buffer_length - midi->read_target_length;
-  if (bufsize < n)
-    n = bufsize;
+  if (bufsize < n) n = bufsize;
 
   // Skip the header in the buffer
   memcpy(buffer, midi->read_buffer + 1 + midi->read_target_length, n);
@@ -153,10 +151,8 @@ void midi_rx_done_cb(midid_interface_t* midi, uint8_t const* buffer, uint32_t bu
 // WRITE API
 //--------------------------------------------------------------------+
 
-static bool maybe_transmit(midid_interface_t* midi, uint8_t itf_index)
+static uint32_t write_flush(midid_interface_t* midi)
 {
-  (void) itf_index;
-
   // skip if previous transfer not complete
   TU_VERIFY( !usbd_edpt_busy(TUD_OPT_RHPORT, midi->ep_in) );
 
@@ -165,7 +161,7 @@ static bool maybe_transmit(midid_interface_t* midi, uint8_t itf_index)
   {
     TU_ASSERT( usbd_edpt_xfer(TUD_OPT_RHPORT, midi->ep_in, midi->epin_buf, count) );
   }
-  return true;
+  return count;
 }
 
 uint32_t tud_midi_n_write(uint8_t itf, uint8_t jack_id, uint8_t const* buffer, uint32_t bufsize)
@@ -234,7 +230,8 @@ uint32_t tud_midi_n_write(uint8_t itf, uint8_t jack_id, uint8_t const* buffer, u
     }
     i++;
   }
-  maybe_transmit(midi, itf);
+
+  write_flush(midi);
 
   return i;
 }
@@ -250,7 +247,7 @@ bool tud_midi_n_send (uint8_t itf, uint8_t const packet[4])
     return false;
 
   tu_fifo_write_n(&midi->tx_ff, packet, 4);
-  maybe_transmit(midi, itf);
+  write_flush(midi);
 
   return true;
 }
@@ -388,28 +385,40 @@ bool midid_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32
 {
   (void) result;
 
-  uint8_t itf = 0;
-  midid_interface_t* p_midi = _midid_itf;
+  uint8_t itf;
+  midid_interface_t* p_midi;
 
-  for ( ; ; itf++, p_midi++)
+  // Identify which interface to use
+  for (itf = 0; itf < CFG_TUD_MIDI; itf++)
   {
-    if (itf >= TU_ARRAY_SIZE(_midid_itf)) return false;
-
-    if ( ep_addr == p_midi->ep_out ) break;
+    p_midi = &_midid_itf[itf];
+    if ( ( ep_addr == p_midi->ep_out ) || ( ep_addr == p_midi->ep_in ) ) break;
   }
+  TU_ASSERT(itf < CFG_TUD_MIDI);
 
   // receive new data
   if ( ep_addr == p_midi->ep_out )
   {
-    midi_rx_done_cb(p_midi, p_midi->epout_buf, xferred_bytes);
+    tu_fifo_write_n(&p_midi->rx_ff, p_midi->epout_buf, xferred_bytes);
+
+    // invoke receive callback if available
+    if (tud_midi_rx_cb) tud_midi_rx_cb(itf);
 
     // prepare for next
-    TU_ASSERT( usbd_edpt_xfer(rhport, p_midi->ep_out, p_midi->epout_buf, CFG_TUD_MIDI_EP_BUFSIZE), false );
-  } else if ( ep_addr == p_midi->ep_in ) {
-    maybe_transmit(p_midi, itf);
+    TU_ASSERT(usbd_edpt_xfer(rhport, p_midi->ep_out, p_midi->epout_buf, CFG_TUD_MIDI_EP_BUFSIZE), false);
+  }
+  else if ( ep_addr == p_midi->ep_in )
+  {
+    if (0 == write_flush(p_midi))
+    {
+      // There is no data left, a ZLP should be sent if
+      // xferred_bytes is multiple of EP size and not zero
+      if ( xferred_bytes && (0 == (xferred_bytes % CFG_TUD_MIDI_EP_BUFSIZE)) )
+      {
+        usbd_edpt_xfer(rhport, p_midi->ep_in, NULL, 0);
+      }
+    }
   }
-
-  // nothing to do with in and notif endpoint
 
   return true;
 }
diff --git a/src/class/midi/midi_device.h b/src/class/midi/midi_device.h
index 1828a21a3..b8fb55cc2 100644
--- a/src/class/midi/midi_device.h
+++ b/src/class/midi/midi_device.h
@@ -72,7 +72,7 @@ bool tud_midi_n_receive        (uint8_t itf, uint8_t packet[4]);
 bool tud_midi_n_send           (uint8_t itf, uint8_t const packet[4]);
 
 //--------------------------------------------------------------------+
-// Application API (Interface0)
+// Application API (Single Interface)
 //--------------------------------------------------------------------+
 static inline bool     tud_midi_mounted    (void);
 static inline uint32_t tud_midi_available  (void);
diff --git a/src/class/msc/msc_device.c b/src/class/msc/msc_device.c
index 692fd2441..f3f2e536e 100644
--- a/src/class/msc/msc_device.c
+++ b/src/class/msc/msc_device.c
@@ -31,6 +31,7 @@
 #include "common/tusb_common.h"
 #include "msc_device.h"
 #include "device/usbd_pvt.h"
+#include "device/dcd.h"         // for faking dcd_event_xfer_complete
 
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
@@ -105,7 +106,7 @@ static inline uint16_t rdwr10_get_blockcount(uint8_t const command[])
 //--------------------------------------------------------------------+
 #if CFG_TUSB_DEBUG >= 2
 
-static lookup_entry_t const _msc_scsi_cmd_lookup[] =
+static tu_lookup_entry_t const _msc_scsi_cmd_lookup[] =
 {
   { .key = SCSI_CMD_TEST_UNIT_READY              , .data = "Test Unit Ready" },
   { .key = SCSI_CMD_INQUIRY                      , .data = "Inquiry" },
@@ -120,7 +121,7 @@ static lookup_entry_t const _msc_scsi_cmd_lookup[] =
   { .key = SCSI_CMD_WRITE_10                     , .data = "Write10" }
 };
 
-static lookup_table_t const _msc_scsi_cmd_table =
+static tu_lookup_table_t const _msc_scsi_cmd_table =
 {
   .count = TU_ARRAY_SIZE(_msc_scsi_cmd_lookup),
   .items = _msc_scsi_cmd_lookup
@@ -418,7 +419,7 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
       TU_ASSERT( event == XFER_RESULT_SUCCESS &&
                  xferred_bytes == sizeof(msc_cbw_t) && p_cbw->signature == MSC_CBW_SIGNATURE );
 
-      TU_LOG2("  SCSI Command: %s\r\n", lookup_find(&_msc_scsi_cmd_table, p_cbw->command[0]));
+      TU_LOG2("  SCSI Command: %s\r\n", tu_lookup_find(&_msc_scsi_cmd_table, p_cbw->command[0]));
       // TU_LOG2_MEM(p_cbw, xferred_bytes, 2);
 
       p_csw->signature    = MSC_CSW_SIGNATURE;
diff --git a/src/class/msc/msc_host.c b/src/class/msc/msc_host.c
index e20969f57..539e98376 100644
--- a/src/class/msc/msc_host.c
+++ b/src/class/msc/msc_host.c
@@ -293,7 +293,7 @@ bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *it
     TU_ASSERT(TUSB_DESC_ENDPOINT == ep_desc->bDescriptorType);
     TU_ASSERT(TUSB_XFER_BULK == ep_desc->bmAttributes.xfer);
 
-    TU_ASSERT(hcd_edpt_open(rhport, dev_addr, ep_desc));
+    TU_ASSERT(usbh_edpt_open(rhport, dev_addr, ep_desc));
 
     if ( tu_edpt_dir(ep_desc->bEndpointAddress) == TUSB_DIR_IN )
     {
@@ -306,15 +306,16 @@ bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *it
     ep_desc = (tusb_desc_endpoint_t const *) tu_desc_next(ep_desc);
   }
 
-  p_msc->itf_numr = itf_desc->bInterfaceNumber;
+  p_msc->itf_num = itf_desc->bInterfaceNumber;
   (*p_length) += sizeof(tusb_desc_interface_t) + 2*sizeof(tusb_desc_endpoint_t);
 
   //------------- Get Max Lun -------------//
+  TU_LOG2("MSC Get Max Lun\r\n");
   tusb_control_request_t request = {
         .bmRequestType_bit = { .recipient = TUSB_REQ_RCPT_INTERFACE, .type = TUSB_REQ_TYPE_CLASS, .direction = TUSB_DIR_IN },
         .bRequest = MSC_REQ_GET_MAX_LUN,
         .wValue = 0,
-        .wIndex = p_msc->itf_numr,
+        .wIndex = p_msc->itf_num,
         .wLength = 1
   };
   // TODO STALL means zero
@@ -327,7 +328,7 @@ bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *it
         .bmRequestType_bit = { .recipient = TUSB_REQ_RCPT_INTERFACE, .type = TUSB_REQ_TYPE_CLASS, .direction = TUSB_DIR_OUT },
         .bRequest = MSC_REQ_RESET,
         .wValue = 0,
-        .wIndex = p_msc->itf_numr,
+        .wIndex = p_msc->itf_num,
         .wLength = 0
   };
   TU_ASSERT( usbh_control_xfer( dev_addr, &request, NULL ) );
diff --git a/src/class/msc/msc_host.h b/src/class/msc/msc_host.h
index 76cf86853..959294adf 100644
--- a/src/class/msc/msc_host.h
+++ b/src/class/msc/msc_host.h
@@ -175,7 +175,7 @@ void tuh_msc_isr(uint8_t dev_addr, xfer_result_t event, uint32_t xferred_bytes);
 //--------------------------------------------------------------------+
 typedef struct
 {
-  uint8_t itf_numr;
+  uint8_t  itf_num;
   uint8_t  ep_in;
   uint8_t  ep_out;
 
diff --git a/src/class/net/net_device.c b/src/class/net/net_device.c
index 2d2a0b46d..1d7f9ce3a 100644
--- a/src/class/net/net_device.c
+++ b/src/class/net/net_device.c
@@ -326,7 +326,7 @@ bool netd_control_request(uint8_t rhport, tusb_control_request_t const * request
       {
         if (request->bmRequestType_bit.direction == TUSB_DIR_IN)
         {
-          rndis_generic_msg_t *rndis_msg = (rndis_generic_msg_t *)notify.rndis_buf;
+          rndis_generic_msg_t *rndis_msg = (rndis_generic_msg_t *) ((void*) notify.rndis_buf);
           uint32_t msglen = tu_le32toh(rndis_msg->MessageLength);
           TU_ASSERT(msglen <= sizeof(notify.rndis_buf));
           tud_control_xfer(rhport, request, notify.rndis_buf, msglen);
@@ -356,7 +356,7 @@ static void handle_incoming_packet(uint32_t len)
   }
   else
   {
-    rndis_data_packet_t *r = (rndis_data_packet_t *)pnt;
+    rndis_data_packet_t *r = (rndis_data_packet_t *) ((void*) pnt);
     if (len >= sizeof(rndis_data_packet_t))
       if ( (r->MessageType == REMOTE_NDIS_PACKET_MSG) && (r->MessageLength <= len))
         if ( (r->DataOffset + offsetof(rndis_data_packet_t, DataOffset) + r->DataLength) <= len)
@@ -450,7 +450,7 @@ void tud_network_xmit(struct pbuf *p)
 
   if (!_netd_itf.ecm_mode)
   {
-    rndis_data_packet_t *hdr = (rndis_data_packet_t *)transmitted;
+    rndis_data_packet_t *hdr = (rndis_data_packet_t *) ((void*) transmitted);
     memset(hdr, 0, sizeof(rndis_data_packet_t));
     hdr->MessageType = REMOTE_NDIS_PACKET_MSG;
     hdr->MessageLength = len;
diff --git a/src/class/usbtmc/usbtmc_device.c b/src/class/usbtmc/usbtmc_device.c
index a3ff76079..bc5f23f42 100644
--- a/src/class/usbtmc/usbtmc_device.c
+++ b/src/class/usbtmc/usbtmc_device.c
@@ -80,7 +80,6 @@
 #include <string.h>
 #include "usbtmc.h"
 #include "usbtmc_device.h"
-#include "device/dcd.h"
 #include "device/usbd.h"
 #include "osal/osal.h"
 
diff --git a/src/class/vendor/vendor_host.c b/src/class/vendor/vendor_host.c
index 3e8dac0f6..0524cb981 100644
--- a/src/class/vendor/vendor_host.c
+++ b/src/class/vendor/vendor_host.c
@@ -107,7 +107,7 @@ tusb_error_t cush_open_subtask(uint8_t dev_addr, tusb_desc_interface_t const *p_
 
     pipe_handle_t * p_pipe_hdl =  ( p_endpoint->bEndpointAddress &  TUSB_DIR_IN_MASK ) ?
                          &custom_interface[dev_addr-1].pipe_in : &custom_interface[dev_addr-1].pipe_out;
-    *p_pipe_hdl = hcd_edpt_open(dev_addr, p_endpoint, TUSB_CLASS_VENDOR_SPECIFIC);
+    *p_pipe_hdl = usbh_edpt_open(dev_addr, p_endpoint, TUSB_CLASS_VENDOR_SPECIFIC);
     TU_ASSERT ( pipehandle_is_valid(*p_pipe_hdl), TUSB_ERROR_HCD_OPEN_PIPE_FAILED );
 
     p_desc = tu_desc_next(p_desc);
diff --git a/src/common/tusb_common.h b/src/common/tusb_common.h
index 1dba6c914..d95c0ffc4 100644
--- a/src/common/tusb_common.h
+++ b/src/common/tusb_common.h
@@ -251,16 +251,16 @@ void tu_print_var(uint8_t const* buf, uint32_t bufsize)
 typedef struct
 {
   uint32_t key;
-  char const * data;
-}lookup_entry_t;
+  const char* data;
+} tu_lookup_entry_t;
 
 typedef struct
 {
   uint16_t count;
-  lookup_entry_t const* items;
-} lookup_table_t;
+  tu_lookup_entry_t const* items;
+} tu_lookup_table_t;
 
-static inline char const* lookup_find(lookup_table_t const* p_table, uint32_t key)
+static inline const char* tu_lookup_find(tu_lookup_table_t const* p_table, uint32_t key)
 {
   for(uint16_t i=0; i<p_table->count; i++)
   {
diff --git a/src/device/dcd.h b/src/device/dcd.h
index a4635346b..776f782b8 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -87,9 +87,9 @@ typedef struct TU_ATTR_ALIGNED(4)
 
 //TU_VERIFY_STATIC(sizeof(dcd_event_t) <= 12, "size is not correct");
 
-/*------------------------------------------------------------------*/
-/* Device API
- *------------------------------------------------------------------*/
+//--------------------------------------------------------------------+
+// Controller API
+//--------------------------------------------------------------------+
 
 // Initialize controller to device mode
 void dcd_init       (uint8_t rhport);
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 9faa3549f..6b91048b6 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -82,21 +82,7 @@ enum { DRVID_INVALID = 0xFFu };
   #define DRIVER_NAME(_name)
 #endif
 
-typedef struct
-{
-  #if CFG_TUSB_DEBUG >= 2
-  char const* name;
-  #endif
-
-  void     (* init             ) (void);
-  void     (* reset            ) (uint8_t rhport);
-  uint16_t (* open             ) (uint8_t rhport, tusb_desc_interface_t const * desc_intf, uint16_t max_len);
-  bool     (* control_request  ) (uint8_t rhport, tusb_control_request_t const * request);
-  bool     (* control_complete ) (uint8_t rhport, tusb_control_request_t const * request);
-  bool     (* xfer_cb          ) (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
-  void     (* sof              ) (uint8_t rhport); /* optional */
-} usbd_class_driver_t;
-
+// Built-in class drivers
 static usbd_class_driver_t const _usbd_driver[] =
 {
   #if CFG_TUD_CDC
@@ -217,7 +203,30 @@ static usbd_class_driver_t const _usbd_driver[] =
   #endif
 };
 
-enum { USBD_CLASS_DRIVER_COUNT = TU_ARRAY_SIZE(_usbd_driver) };
+enum { BUILTIN_DRIVER_COUNT = TU_ARRAY_SIZE(_usbd_driver) };
+
+// Additional class drivers implemented by application
+static usbd_class_driver_t const * _app_driver = NULL;
+static uint8_t _app_driver_count = 0;
+
+// virtually joins built-in and application drivers together.
+// Application is positioned first to allow overwriting built-in ones.
+static inline usbd_class_driver_t const * get_driver(uint8_t drvid)
+{
+  // Application drivers
+  if ( usbd_app_driver_get_cb )
+  {
+    if ( drvid < _app_driver_count ) return &_app_driver[drvid];
+    drvid -= _app_driver_count;
+  }
+
+  // Built-in drivers
+  if (drvid < BUILTIN_DRIVER_COUNT) return &_usbd_driver[drvid];
+
+  return NULL;
+}
+
+#define TOTAL_DRIVER_COUNT    (_app_driver_count + BUILTIN_DRIVER_COUNT)
 
 //--------------------------------------------------------------------+
 // DCD Event
@@ -236,6 +245,7 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
 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);
 
+// from usbd_control.c
 void usbd_control_reset(void);
 void usbd_control_set_request(tusb_control_request_t const *request);
 void usbd_control_set_complete_callback( bool (*fp) (uint8_t, tusb_control_request_t const * ) );
@@ -243,7 +253,7 @@ bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t event,
 
 
 //--------------------------------------------------------------------+
-// Debugging
+// Debug
 //--------------------------------------------------------------------+
 #if CFG_TUSB_DEBUG >= 2
 static char const* const _usbd_event_str[DCD_EVENT_COUNT] =
@@ -279,11 +289,12 @@ static char const* const _tusb_std_request_str[] =
 // for usbd_control to print the name of control complete driver
 void usbd_driver_print_control_complete_name(bool (*control_complete) (uint8_t, tusb_control_request_t const * ))
 {
-  for (uint8_t i = 0; i < USBD_CLASS_DRIVER_COUNT; i++)
+  for (uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++)
   {
-    if (_usbd_driver[i].control_complete == control_complete )
+    usbd_class_driver_t const * driver = get_driver(i);
+    if ( driver->control_complete == control_complete )
     {
-      TU_LOG2("  %s control complete\r\n", _usbd_driver[i].name);
+      TU_LOG2("  %s control complete\r\n", driver->name);
       return;
     }
   }
@@ -317,6 +328,20 @@ bool tud_remote_wakeup(void)
   return true;
 }
 
+bool tud_disconnect(void)
+{
+  TU_VERIFY(dcd_disconnect);
+  dcd_disconnect(TUD_OPT_RHPORT);
+  return true;
+}
+
+bool tud_connect(void)
+{
+  TU_VERIFY(dcd_connect);
+  dcd_connect(TUD_OPT_RHPORT);
+  return true;
+}
+
 //--------------------------------------------------------------------+
 // USBD Task
 //--------------------------------------------------------------------+
@@ -330,16 +355,22 @@ bool tud_init (void)
   _usbd_q = osal_queue_create(&_usbd_qdef);
   TU_ASSERT(_usbd_q != NULL);
 
+  // Get application driver if available
+  if ( usbd_app_driver_get_cb )
+  {
+    _app_driver = usbd_app_driver_get_cb(&_app_driver_count);
+  }
+
   // Init class drivers
-  for (uint8_t i = 0; i < USBD_CLASS_DRIVER_COUNT; i++)
+  for (uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++)
   {
-    TU_LOG2("%s init\r\n", _usbd_driver[i].name);
-    _usbd_driver[i].init();
+    usbd_class_driver_t const * driver = get_driver(i);
+    TU_LOG2("%s init\r\n", driver->name);
+    driver->init();
   }
 
   // Init device controller driver
   dcd_init(TUD_OPT_RHPORT);
-  tud_connect();
   dcd_int_enable(TUD_OPT_RHPORT);
 
   return true;
@@ -354,9 +385,9 @@ static void usbd_reset(uint8_t rhport)
 
   usbd_control_reset();
 
-  for (uint8_t i = 0; i < USBD_CLASS_DRIVER_COUNT; i++)
+  for ( uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++ )
   {
-    if ( _usbd_driver[i].reset ) _usbd_driver[i].reset( rhport );
+    get_driver(i)->reset(rhport);
   }
 }
 
@@ -454,11 +485,11 @@ void tud_task (void)
         }
         else
         {
-          uint8_t const drv_id = _usbd_dev.ep2drv[epnum][ep_dir];
-          TU_ASSERT(drv_id < USBD_CLASS_DRIVER_COUNT,);
+          usbd_class_driver_t const * driver = get_driver( _usbd_dev.ep2drv[epnum][ep_dir] );
+          TU_ASSERT(driver, );
 
-          TU_LOG2("  %s xfer callback\r\n", _usbd_driver[drv_id].name);
-          _usbd_driver[drv_id].xfer_cb(event.rhport, ep_addr, (xfer_result_t)event.xfer_complete.result, event.xfer_complete.len);
+          TU_LOG2("  %s xfer callback\r\n", driver->name);
+          driver->xfer_cb(event.rhport, ep_addr, event.xfer_complete.result, event.xfer_complete.len);
         }
       }
       break;
@@ -475,12 +506,10 @@ void tud_task (void)
 
       case DCD_EVENT_SOF:
         TU_LOG2("\r\n");
-        for ( uint8_t i = 0; i < USBD_CLASS_DRIVER_COUNT; i++ )
+        for ( uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++ )
         {
-          if ( _usbd_driver[i].sof )
-          {
-            _usbd_driver[i].sof(event.rhport);
-          }
+          usbd_class_driver_t const * driver = get_driver(i);
+          if ( driver->sof ) driver->sof(event.rhport);
         }
       break;
 
@@ -501,11 +530,11 @@ void tud_task (void)
 //--------------------------------------------------------------------+
 
 // Helper to invoke class driver control request handler
-static bool invoke_class_control(uint8_t rhport, uint8_t drvid, tusb_control_request_t const * request)
+static bool invoke_class_control(uint8_t rhport, usbd_class_driver_t const * driver, tusb_control_request_t const * request)
 {
-  usbd_control_set_complete_callback(_usbd_driver[drvid].control_complete);
-  TU_LOG2("  %s control request\r\n", _usbd_driver[drvid].name);
-  return _usbd_driver[drvid].control_request(rhport, request);
+  usbd_control_set_complete_callback(driver->control_complete);
+  TU_LOG2("  %s control request\r\n", driver->name);
+  return driver->control_request(rhport, request);
 }
 
 // This handles the actual request and its response.
@@ -539,15 +568,14 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
     case TUSB_REQ_RCPT_DEVICE:
       if ( TUSB_REQ_TYPE_CLASS == p_request->bmRequestType_bit.type )
       {
-          uint8_t const itf = tu_u16_low(p_request->wIndex);
-          TU_VERIFY(itf < TU_ARRAY_SIZE(_usbd_dev.itf2drv));
+        uint8_t const itf = tu_u16_low(p_request->wIndex);
+        TU_VERIFY(itf < TU_ARRAY_SIZE(_usbd_dev.itf2drv));
 
-          uint8_t const drvid = _usbd_dev.itf2drv[itf];
-          TU_VERIFY(drvid < USBD_CLASS_DRIVER_COUNT);
+        usbd_class_driver_t const * driver = get_driver(_usbd_dev.itf2drv[itf]);
+        TU_VERIFY(driver);
 
-          // forward to class driver: "non-STD request to Interface"
-          TU_VERIFY(invoke_class_control(rhport, drvid, p_request));
-          return true;
+        // forward to class driver: "non-STD request to Interface"
+        return invoke_class_control(rhport, driver, p_request);
       }
       if ( TUSB_REQ_TYPE_STANDARD != p_request->bmRequestType_bit.type )
       {
@@ -580,7 +608,6 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
           uint8_t const cfg_num = (uint8_t) p_request->wValue;
 
           if ( !_usbd_dev.configured && cfg_num ) TU_ASSERT( process_set_config(rhport, cfg_num) );
-
           _usbd_dev.configured = cfg_num ? 1 : 0;
 
           tud_control_status(rhport, p_request);
@@ -630,12 +657,12 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
       uint8_t const itf = tu_u16_low(p_request->wIndex);
       TU_VERIFY(itf < TU_ARRAY_SIZE(_usbd_dev.itf2drv));
 
-      uint8_t const drvid = _usbd_dev.itf2drv[itf];
-      TU_VERIFY(drvid < USBD_CLASS_DRIVER_COUNT);
+      usbd_class_driver_t const * driver = get_driver(_usbd_dev.itf2drv[itf]);
+      TU_VERIFY(driver);
 
       // all requests to Interface (STD or Class) is forwarded to class driver.
       // notable requests are: GET HID REPORT DESCRIPTOR, SET_INTERFACE, GET_INTERFACE
-      if ( !invoke_class_control(rhport, drvid, p_request) )
+      if ( !invoke_class_control(rhport, driver, p_request) )
       {
         // For GET_INTERFACE, it is mandatory to respond even if the class
         // driver doesn't use alternate settings.
@@ -657,8 +684,6 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
 
       TU_ASSERT(ep_num < TU_ARRAY_SIZE(_usbd_dev.ep2drv) );
 
-      uint8_t const drvid = _usbd_dev.ep2drv[ep_num][ep_dir];
-
       bool ret = false;
 
       // Handle STD request to endpoint
@@ -677,18 +702,12 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
           break;
 
           case TUSB_REQ_CLEAR_FEATURE:
-            if ( TUSB_REQ_FEATURE_EDPT_HALT == p_request->wValue )
-            {
-              usbd_edpt_clear_stall(rhport, ep_addr);
-            }
+            if ( TUSB_REQ_FEATURE_EDPT_HALT == p_request->wValue ) usbd_edpt_clear_stall(rhport, ep_addr);
             tud_control_status(rhport, p_request);
           break;
 
           case TUSB_REQ_SET_FEATURE:
-            if ( TUSB_REQ_FEATURE_EDPT_HALT == p_request->wValue )
-            {
-              usbd_edpt_stall(rhport, ep_addr);
-            }
+            if ( TUSB_REQ_FEATURE_EDPT_HALT == p_request->wValue ) usbd_edpt_stall(rhport, ep_addr);
             tud_control_status(rhport, p_request);
           break;
 
@@ -697,16 +716,17 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
         }
       }
 
-      if (drvid < 0xFF) {
-        TU_ASSERT(drvid < USBD_CLASS_DRIVER_COUNT);
-        
+      usbd_class_driver_t const * driver = get_driver(_usbd_dev.ep2drv[ep_num][ep_dir]);
+
+      if (driver)
+      {
         // Some classes such as USBTMC needs to clear/re-init its buffer when receiving CLEAR_FEATURE request
         // We will forward all request targeted endpoint to class drivers after
         // - For class-type requests: driver is fully responsible to reply to host
         // - For std-type requests  : driver init/re-init internal variable/buffer only, and
         //                            must not call tud_control_status(), driver's return value will have no effect.
         //                            EP state has already affected (stalled/cleared)
-        if ( invoke_class_control(rhport, drvid, p_request) ) ret = true;
+        if ( invoke_class_control(rhport, driver, p_request) ) ret = true;
       }
 
       if ( TUSB_REQ_TYPE_STANDARD == p_request->bmRequestType_bit.type )
@@ -758,13 +778,10 @@ static bool process_set_config(uint8_t rhport, uint8_t cfg_num)
     uint16_t const remaining_len = desc_end-p_desc;
 
     uint8_t drv_id;
-    uint16_t drv_len;
-
-    for (drv_id = 0; drv_id < USBD_CLASS_DRIVER_COUNT; drv_id++)
+    for (drv_id = 0; drv_id < TOTAL_DRIVER_COUNT; drv_id++)
     {
-      usbd_class_driver_t const *driver = &_usbd_driver[drv_id];
-
-      drv_len = driver->open(rhport, desc_itf, remaining_len);
+      usbd_class_driver_t const *driver = get_driver(drv_id);
+      uint16_t const drv_len = driver->open(rhport, desc_itf, remaining_len);
 
       if ( drv_len > 0 )
       {
@@ -772,7 +789,7 @@ static bool process_set_config(uint8_t rhport, uint8_t cfg_num)
         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_ASSERT(DRVID_INVALID == _usbd_dev.itf2drv[desc_itf->bInterfaceNumber]);
 
         TU_LOG2("  %s opened\r\n", driver->name);
         _usbd_dev.itf2drv[desc_itf->bInterfaceNumber] = drv_id;
@@ -790,16 +807,16 @@ static bool process_set_config(uint8_t rhport, uint8_t cfg_num)
           }
         }
 
+        mark_interface_endpoint(_usbd_dev.ep2drv, p_desc, drv_len, drv_id); // TODO refactor
+
+        p_desc += drv_len; // next interface
+
         break;
       }
     }
 
     // Failed if cannot find supported driver
-    TU_ASSERT(drv_id < USBD_CLASS_DRIVER_COUNT);
-
-    mark_interface_endpoint(_usbd_dev.ep2drv, p_desc, drv_len, drv_id); // TODO refactor
-
-    p_desc += drv_len; // next interface
+    TU_ASSERT(drv_id < TOTAL_DRIVER_COUNT);
   }
 
   // invoke callback
diff --git a/src/device/usbd.h b/src/device/usbd.h
index 098e94075..5338be157 100644
--- a/src/device/usbd.h
+++ b/src/device/usbd.h
@@ -35,7 +35,6 @@
 #endif
 
 #include "common/tusb_common.h"
-#include "dcd.h"
 
 //--------------------------------------------------------------------+
 // Application API
@@ -53,6 +52,7 @@ void tud_task (void);
 bool tud_task_event_ready(void);
 
 // Interrupt handler, name alias to DCD
+extern void dcd_int_handler(uint8_t rhport);
 #define tud_int_handler   dcd_int_handler
 
 // Get current bus speed
@@ -75,21 +75,11 @@ bool tud_remote_wakeup(void);
 
 // Enable pull-up resistor on D+ D-
 // Return false on unsupported MCUs
-static inline bool tud_disconnect(void)
-{
-  TU_VERIFY(dcd_disconnect);
-  dcd_disconnect(TUD_OPT_RHPORT);
-  return true;
-}
+bool tud_disconnect(void);
 
 // Disable pull-up resistor on D+ D-
 // Return false on unsupported MCUs
-static inline bool tud_connect(void)
-{
-  TU_VERIFY(dcd_connect);
-  dcd_connect(TUD_OPT_RHPORT);
-  return true;
-}
+bool tud_connect(void);
 
 // Carry out Data and Status stage of control transfer
 // - If len = 0, it is equivalent to sending status only
diff --git a/src/device/usbd_pvt.h b/src/device/usbd_pvt.h
index 48188ec99..a5d223329 100644
--- a/src/device/usbd_pvt.h
+++ b/src/device/usbd_pvt.h
@@ -34,6 +34,30 @@
 #endif
 
 //--------------------------------------------------------------------+
+// Class Drivers
+//--------------------------------------------------------------------+
+
+typedef struct
+{
+  #if CFG_TUSB_DEBUG >= 2
+  char const* name;
+  #endif
+
+  void     (* init             ) (void);
+  void     (* reset            ) (uint8_t rhport);
+  uint16_t (* open             ) (uint8_t rhport, tusb_desc_interface_t const * desc_intf, uint16_t max_len);
+  bool     (* control_request  ) (uint8_t rhport, tusb_control_request_t const * request);
+  bool     (* control_complete ) (uint8_t rhport, tusb_control_request_t const * request);
+  bool     (* xfer_cb          ) (uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
+  void     (* sof              ) (uint8_t rhport); /* optional */
+} usbd_class_driver_t;
+
+// Invoked when initializing device stack to get additional class drivers.
+// Can optionally implemented by application to extend/overwrite class driver support.
+// Note: The drivers array must be accessible at all time when stack is active
+usbd_class_driver_t const* usbd_app_driver_get_cb(uint8_t* driver_count) TU_ATTR_WEAK;
+
+//--------------------------------------------------------------------+
 // USBD Endpoint API
 //--------------------------------------------------------------------+
 
diff --git a/src/host/ehci/ehci.c b/src/host/ehci/ehci.c
index 9ddffaf2c..295da4566 100644
--- a/src/host/ehci/ehci.c
+++ b/src/host/ehci/ehci.c
@@ -108,16 +108,39 @@ bool hcd_init(void)
   return ehci_init(TUH_OPT_RHPORT);
 }
 
+uint32_t hcd_uframe_number(uint8_t rhport)
+{
+  (void) rhport;
+  return ehci_data.uframe_number + ehci_data.regs->frame_index;
+}
+
 void hcd_port_reset(uint8_t rhport)
 {
   (void) rhport;
 
   ehci_registers_t* regs = ehci_data.regs;
 
-  regs->portsc_bm.port_enabled = 0; // disable port before reset
-  regs->portsc_bm.port_reset = 1;
+//  regs->portsc_bm.port_enabled = 0; // disable port before reset
+//  regs->portsc_bm.port_reset = 1;
+
+  uint32_t portsc = regs->portsc;
+
+  portsc &= ~(EHCI_PORTSC_MASK_PORT_EANBLED);
+  portsc |= EHCI_PORTSC_MASK_PORT_RESET;
+
+  regs->portsc = portsc;
 }
 
+#if 0
+void hcd_port_reset_end(uint8_t rhport)
+{
+  (void) rhport;
+
+  ehci_registers_t* regs = ehci_data.regs;
+  regs->portsc_bm.port_reset = 0;
+}
+#endif
+
 bool hcd_port_connect_status(uint8_t rhport)
 {
   (void) rhport;
@@ -192,7 +215,7 @@ static bool ehci_init(uint8_t rhport)
   regs->status = EHCI_INT_MASK_ALL; // 2. clear all status
 
   regs->inten  = EHCI_INT_MASK_ERROR | EHCI_INT_MASK_PORT_CHANGE | EHCI_INT_MASK_ASYNC_ADVANCE |
-                 EHCI_INT_MASK_NXP_PERIODIC | EHCI_INT_MASK_NXP_ASYNC ;
+                 EHCI_INT_MASK_NXP_PERIODIC | EHCI_INT_MASK_NXP_ASYNC | EHCI_INT_MASK_FRAMELIST_ROLLOVER;
 
   //------------- Asynchronous List -------------//
   ehci_qhd_t * const async_head = qhd_async_head(rhport);
@@ -358,7 +381,7 @@ bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const
   if ( dev_addr == 0 ) return true;
 
   // Insert to list
-  ehci_link_t * list_head;
+  ehci_link_t * list_head = NULL;
 
   switch (ep_desc->bmAttributes.xfer)
   {
@@ -378,8 +401,10 @@ bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const
     default: break;
   }
 
+  TU_ASSERT(list_head);
+
   // TODO might need to disable async/period list
-  list_insert( list_head, (ehci_link_t*) p_qhd, EHCI_QTYPE_QHD);
+  list_insert(list_head, (ehci_link_t*) p_qhd, EHCI_QTYPE_QHD);
 
   return true;
 }
@@ -623,7 +648,7 @@ static void xfer_error_isr(uint8_t hostid)
 }
 
 //------------- Host Controller Driver's Interrupt Handler -------------//
-void hcd_isr(uint8_t rhport)
+void hcd_int_handler(uint8_t rhport)
 {
   ehci_registers_t* regs = ehci_data.regs;
 
@@ -634,6 +659,11 @@ void hcd_isr(uint8_t rhport)
 
   if (int_status == 0) return;
 
+  if (int_status & EHCI_INT_MASK_FRAMELIST_ROLLOVER)
+  {
+    ehci_data.uframe_number += (EHCI_FRAMELIST_SIZE << 3);
+  }
+
   if (int_status & EHCI_INT_MASK_PORT_CHANGE)
   {
     uint32_t port_status = regs->portsc & EHCI_PORTSC_MASK_ALL;
diff --git a/src/host/ehci/ehci.h b/src/host/ehci/ehci.h
index ce2f56771..a6342b2d2 100644
--- a/src/host/ehci/ehci.h
+++ b/src/host/ehci/ehci.h
@@ -54,8 +54,8 @@
 //--------------------------------------------------------------------+
 // EHCI CONFIGURATION & CONSTANTS
 //--------------------------------------------------------------------+
-#define	EHCI_CFG_FRAMELIST_SIZE_BITS			7			/// Framelist Size (NXP specific) (0:1024) - (1:512) - (2:256) - (3:128) - (4:64) - (5:32) - (6:16) - (7:8)
-#define EHCI_FRAMELIST_SIZE  (1024 >> EHCI_CFG_FRAMELIST_SIZE_BITS)
+#define	EHCI_CFG_FRAMELIST_SIZE_BITS		7			/// Framelist Size (NXP specific) (0:1024) - (1:512) - (2:256) - (3:128) - (4:64) - (5:32) - (6:16) - (7:8)
+#define EHCI_FRAMELIST_SIZE             (1024 >> EHCI_CFG_FRAMELIST_SIZE_BITS)
 
 // TODO merge OHCI with EHCI
 enum {
@@ -311,10 +311,14 @@ enum ehci_usbcmd_pos_ {
 };
 
 enum ehci_portsc_change_mask_{
+  EHCI_PORTSC_MASK_CURRENT_CONNECT_STATUS = TU_BIT(0),
   EHCI_PORTSC_MASK_CONNECT_STATUS_CHANGE = TU_BIT(1),
+  EHCI_PORTSC_MASK_PORT_EANBLED = TU_BIT(2),
   EHCI_PORTSC_MASK_PORT_ENABLE_CHAGNE = TU_BIT(3),
   EHCI_PORTSC_MASK_OVER_CURRENT_CHANGE = TU_BIT(5),
 
+  EHCI_PORTSC_MASK_PORT_RESET = TU_BIT(8),
+
   EHCI_PORTSC_MASK_ALL =
       EHCI_PORTSC_MASK_CONNECT_STATUS_CHANGE |
       EHCI_PORTSC_MASK_PORT_ENABLE_CHAGNE |
@@ -445,6 +449,8 @@ typedef struct
   ehci_qtd_t qtd_pool[HCD_MAX_XFER] TU_ATTR_ALIGNED(32);
 
   ehci_registers_t* regs;
+
+  volatile uint32_t uframe_number;
 }ehci_data_t;
 
 #ifdef __cplusplus
diff --git a/src/host/hcd.h b/src/host/hcd.h
index a39e7fe16..24cc62501 100644
--- a/src/host/hcd.h
+++ b/src/host/hcd.h
@@ -84,17 +84,26 @@ enum {
 #endif
 
 //--------------------------------------------------------------------+
-// HCD API
+// Controller & Port API
 //--------------------------------------------------------------------+
 bool hcd_init(void);
-void hcd_isr(uint8_t hostid);
+void hcd_int_handler(uint8_t rhport);
 void hcd_int_enable (uint8_t rhport);
 void hcd_int_disable(uint8_t rhport);
 
-// PORT API
+// Get micro frame number (125 us)
+uint32_t hcd_uframe_number(uint8_t rhport);
+
+// Get frame number (1ms)
+static inline uint32_t hcd_frame_number(uint8_t rhport)
+{
+  return hcd_uframe_number(rhport) >> 3;
+}
+
 /// return the current connect status of roothub port
 bool hcd_port_connect_status(uint8_t hostid);
 void hcd_port_reset(uint8_t hostid);
+void hcd_port_reset_end(uint8_t rhport);
 tusb_speed_t hcd_port_speed_get(uint8_t hostid);
 
 // HCD closes all opened endpoints belong to this device
@@ -134,9 +143,7 @@ bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t *
 bool hcd_pipe_queue_xfer(uint8_t dev_addr, uint8_t ep_addr, uint8_t buffer[], uint16_t total_bytes); // only queue, not transferring yet
 bool hcd_pipe_xfer(uint8_t dev_addr, uint8_t ep_addr, uint8_t buffer[], uint16_t total_bytes, bool int_on_complete);
 
-#if 0
-tusb_error_t hcd_pipe_cancel();
-#endif
+// tusb_error_t hcd_pipe_cancel();
 
 #ifdef __cplusplus
  }
diff --git a/src/host/hub.c b/src/host/hub.c
index 00450b704..eb85dfa42 100644
--- a/src/host/hub.c
+++ b/src/host/hub.c
@@ -154,7 +154,7 @@ bool hub_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf
   TU_ASSERT(TUSB_DESC_ENDPOINT == ep_desc->bDescriptorType);
   TU_ASSERT(TUSB_XFER_INTERRUPT == ep_desc->bmAttributes.xfer);
   
-  TU_ASSERT(hcd_edpt_open(rhport, dev_addr, ep_desc));
+  TU_ASSERT(usbh_edpt_open(rhport, dev_addr, ep_desc));
 
   hub_data[dev_addr-1].itf_num = itf_desc->bInterfaceNumber;
   hub_data[dev_addr-1].ep_status = ep_desc->bEndpointAddress;
diff --git a/src/host/ohci/ohci.c b/src/host/ohci/ohci.c
index 547622edf..114f52e7d 100644
--- a/src/host/ohci/ohci.c
+++ b/src/host/ohci/ohci.c
@@ -166,7 +166,7 @@ bool hcd_init(void)
   OHCI_REG->interrupt_disable = OHCI_REG->interrupt_enable; // disable all interrupts
   OHCI_REG->interrupt_status  = OHCI_REG->interrupt_status; // clear current set bits
   OHCI_REG->interrupt_enable  = OHCI_INT_WRITEBACK_DONEHEAD_MASK | OHCI_INT_RESUME_DETECTED_MASK |
-      OHCI_INT_UNRECOVERABLE_ERROR_MASK | /*OHCI_INT_FRAME_OVERFLOW_MASK |*/ OHCI_INT_RHPORT_STATUS_CHANGE_MASK |
+      OHCI_INT_UNRECOVERABLE_ERROR_MASK | OHCI_INT_FRAME_OVERFLOW_MASK | OHCI_INT_RHPORT_STATUS_CHANGE_MASK |
       OHCI_INT_MASTER_ENABLE_MASK;
 
   OHCI_REG->control |= OHCI_CONTROL_CONTROL_BULK_RATIO | OHCI_CONTROL_LIST_CONTROL_ENABLE_MASK |
@@ -181,6 +181,13 @@ bool hcd_init(void)
   return true;
 }
 
+uint32_t hcd_uframe_number(uint8_t rhport)
+{
+  (void) rhport;
+  return (ohci_data.frame_number_hi << 16 | OHCI_REG->frame_number) << 3;
+}
+
+
 //--------------------------------------------------------------------+
 // PORT API
 //--------------------------------------------------------------------+
@@ -599,13 +606,19 @@ static void done_queue_isr(uint8_t hostid)
   }
 }
 
-void hcd_isr(uint8_t hostid)
+void hcd_int_handler(uint8_t hostid)
 {
   uint32_t const int_en     = OHCI_REG->interrupt_enable;
   uint32_t const int_status = OHCI_REG->interrupt_status & int_en;
 
   if (int_status == 0) return;
 
+  // Frame number overflow
+  if ( int_status & OHCI_INT_FRAME_OVERFLOW_MASK )
+  {
+    ohci_data.frame_number_hi++;
+  }
+
   //------------- RootHub status -------------//
   if ( int_status & OHCI_INT_RHPORT_STATUS_CHANGE_MASK )
   {
diff --git a/src/host/ohci/ohci.h b/src/host/ohci/ohci.h
index 6f6ef9676..bfcdaf9a3 100644
--- a/src/host/ohci/ohci.h
+++ b/src/host/ohci/ohci.h
@@ -180,6 +180,8 @@ typedef struct TU_ATTR_ALIGNED(256)
   ohci_ed_t ed_pool[HCD_MAX_ENDPOINT];
   ohci_gtd_t gtd_pool[HCD_MAX_XFER];
 
+  volatile uint16_t frame_number_hi;
+
 } ohci_data_t;
 
 //--------------------------------------------------------------------+
diff --git a/src/host/usbh.c b/src/host/usbh.c
index f1b88259c..58550c93b 100644
--- a/src/host/usbh.c
+++ b/src/host/usbh.c
@@ -42,10 +42,17 @@
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
+#if CFG_TUSB_DEBUG >= 2
+  #define DRIVER_NAME(_name)    .name = _name,
+#else
+  #define DRIVER_NAME(_name)
+#endif
+
 static host_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,
@@ -56,6 +63,7 @@ static host_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,
@@ -66,6 +74,7 @@ static host_class_driver_t const usbh_class_drivers[] =
 
   #if HOST_CLASS_HID
     {
+      DRIVER_NAME("HID")
       .class_code = TUSB_CLASS_HID,
       .init       = hidh_init,
       .open       = hidh_open_subtask,
@@ -76,6 +85,7 @@ static host_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,
@@ -86,6 +96,7 @@ static host_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,
@@ -116,7 +127,6 @@ CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(4) static uint8_t _usbh_ctrl_buf[CFG_TUSB_H
 //------------- Helper Function Prototypes -------------//
 static inline uint8_t get_new_address(void);
 static inline uint8_t get_configure_number_for_device(tusb_desc_device_t* dev_desc);
-static void mark_interface_endpoint(uint8_t ep2drv[8][2], uint8_t const* p_desc, uint16_t desc_len, uint8_t driver_id);
 
 //--------------------------------------------------------------------+
 // PUBLIC API (Parameter Verification is required)
@@ -127,6 +137,15 @@ tusb_device_state_t tuh_device_get_state (uint8_t const dev_addr)
   return (tusb_device_state_t) _usbh_devices[dev_addr].state;
 }
 
+
+static inline void osal_task_delay(uint32_t msec)
+{
+  (void) msec;
+
+  const uint32_t start = hcd_frame_number(TUH_OPT_RHPORT);
+  while ( ( hcd_frame_number(TUH_OPT_RHPORT) - start ) < msec ) {}
+}
+
 //--------------------------------------------------------------------+
 // CLASS-USBD API (don't require to verify parameters)
 //--------------------------------------------------------------------+
@@ -154,7 +173,11 @@ bool usbh_init(void)
   }
 
   // Class drivers init
-  for (uint8_t drv_id = 0; drv_id < USBH_CLASS_DRIVER_COUNT; drv_id++) usbh_class_drivers[drv_id].init();
+  for (uint8_t drv_id = 0; drv_id < USBH_CLASS_DRIVER_COUNT; drv_id++)
+  {
+    TU_LOG2("%s init\r\n", usbh_class_drivers[drv_id].name);
+    usbh_class_drivers[drv_id].init();
+  }
 
   TU_ASSERT(hcd_init());
   hcd_int_enable(TUH_OPT_RHPORT);
@@ -216,6 +239,30 @@ tusb_error_t usbh_pipe_control_open(uint8_t dev_addr, uint8_t max_packet_size)
   return TUSB_ERROR_NONE;
 }
 
+bool usbh_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const * ep_desc)
+{
+  bool ret = hcd_edpt_open(rhport, dev_addr, ep_desc);
+
+  if (ret)
+  {
+    usbh_device_t* dev = &_usbh_devices[dev_addr];
+
+    // new endpoints belongs to latest interface (last valid value)
+    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 = ep_desc->bEndpointAddress;
+    dev->ep2drv[tu_edpt_number(ep_addr)][tu_edpt_dir(ep_addr)] = drvid;
+  }
+
+  return ret;
+}
+
 //--------------------------------------------------------------------+
 // USBH-HCD ISR/Callback API
 //--------------------------------------------------------------------+
@@ -237,6 +284,7 @@ void hcd_event_xfer_complete(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t ev
 
     if (usbh_class_drivers[drv_id].isr)
     {
+      //TU_LOG2("%s isr\r\n", usbh_class_drivers[drv_id].name);
       usbh_class_drivers[drv_id].isr(dev_addr, ep_addr, event, xferred_bytes);
     }
     else
@@ -304,7 +352,11 @@ static void usbh_device_unplugged(uint8_t rhport, uint8_t hub_addr, uint8_t hub_
       if (tuh_umount_cb) tuh_umount_cb(dev_addr);
 
       // Close class driver
-      for (uint8_t drv_id = 0; drv_id < USBH_CLASS_DRIVER_COUNT; drv_id++) usbh_class_drivers[drv_id].close(dev_addr);
+      for (uint8_t drv_id = 0; drv_id < USBH_CLASS_DRIVER_COUNT; drv_id++)
+      {
+        TU_LOG2("%s close\r\n", usbh_class_drivers[drv_id].name);
+        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
@@ -349,6 +401,8 @@ bool enum_task(hcd_event_t* event)
   {
     if( hcd_port_connect_status(dev0->rhport) )
     {
+      TU_LOG2("Device connect \r\n");
+
       // connection event
       osal_task_delay(POWER_STABLE_DELAY); // wait until device is stable. Increase this if the first 8 bytes is failed to get
 
@@ -362,6 +416,8 @@ bool enum_task(hcd_event_t* event)
     }
     else
     {
+      TU_LOG2("Device disconnect \r\n");
+
       // disconnection event
       usbh_device_unplugged(dev0->rhport, 0, 0);
       return true; // restart task
@@ -415,6 +471,7 @@ bool enum_task(hcd_event_t* event)
   TU_ASSERT_ERR( usbh_pipe_control_open(0, 8) );
 
   //------------- Get first 8 bytes of device descriptor to get Control Endpoint Size -------------//
+  TU_LOG2("Get 8 byte of Device Descriptor\r\n");
   request = (tusb_control_request_t ) {
         .bmRequestType_bit = { .recipient = TUSB_REQ_RCPT_DEVICE, .type = TUSB_REQ_TYPE_STANDARD, .direction = TUSB_DIR_IN },
         .bRequest = TUSB_REQ_GET_DESCRIPTOR,
@@ -425,12 +482,16 @@ bool enum_task(hcd_event_t* event)
   bool is_ok = usbh_control_xfer(0, &request, _usbh_ctrl_buf);
 
   //------------- Reset device again before Set Address -------------//
+  TU_LOG2("Port reset \r\n");
+
   if (dev0->hub_addr == 0)
   {
     // connected directly to roothub
     TU_ASSERT(is_ok); // TODO some slow device is observed to fail the very fist controller xfer, can try more times
     hcd_port_reset( dev0->rhport ); // reset port after 8 byte descriptor
     osal_task_delay(RESET_DELAY);
+//    hcd_port_reset_end(dev0->rhport);
+//    osal_task_delay(RESET_DELAY);
   }
   #if CFG_TUH_HUB
   else
@@ -449,6 +510,7 @@ bool enum_task(hcd_event_t* event)
   #endif
 
   //------------- Set new address -------------//
+  TU_LOG2("Set Address \r\n");
   uint8_t const new_addr = get_new_address();
   TU_ASSERT(new_addr <= CFG_TUSB_HOST_DEVICE_MAX); // TODO notify application we reach max devices
 
@@ -475,6 +537,7 @@ bool enum_task(hcd_event_t* event)
   TU_ASSERT_ERR ( usbh_pipe_control_open(new_addr, ((tusb_desc_device_t*) _usbh_ctrl_buf)->bMaxPacketSize0 ) );
 
   //------------- Get full device descriptor -------------//
+  TU_LOG2("Get Device Descriptor \r\n");
   request = (tusb_control_request_t ) {
         .bmRequestType_bit = { .recipient = TUSB_REQ_RCPT_DEVICE, .type = TUSB_REQ_TYPE_STANDARD, .direction = TUSB_DIR_IN },
         .bRequest = TUSB_REQ_GET_DESCRIPTOR,
@@ -493,6 +556,7 @@ bool enum_task(hcd_event_t* event)
   TU_ASSERT(configure_selected <= new_dev->configure_count); // TODO notify application when invalid configuration
 
   //------------- Get 9 bytes of configuration descriptor -------------//
+  TU_LOG2("Get 9 bytes of Configuration Descriptor\r\n");
   request = (tusb_control_request_t ) {
         .bmRequestType_bit = { .recipient = TUSB_REQ_RCPT_DEVICE, .type = TUSB_REQ_TYPE_STANDARD, .direction = TUSB_DIR_IN },
         .bRequest = TUSB_REQ_GET_DESCRIPTOR,
@@ -506,6 +570,7 @@ bool enum_task(hcd_event_t* event)
   TU_ASSERT( CFG_TUSB_HOST_ENUM_BUFFER_SIZE >= ((tusb_desc_configuration_t*)_usbh_ctrl_buf)->wTotalLength );
 
   //------------- Get full configuration descriptor -------------//
+  TU_LOG2("Get full Configuration Descriptor\r\n");
   request.wLength = ((tusb_desc_configuration_t*)_usbh_ctrl_buf)->wTotalLength; // full length
   TU_ASSERT( usbh_control_xfer( new_addr, &request, _usbh_ctrl_buf ) );
 
@@ -513,6 +578,7 @@ bool enum_task(hcd_event_t* event)
   new_dev->interface_count = ((tusb_desc_configuration_t*) _usbh_ctrl_buf)->bNumInterfaces;
 
   //------------- Set Configure -------------//
+  TU_LOG2("Set Configuration Descriptor\r\n");
   request = (tusb_control_request_t ) {
         .bmRequestType_bit = { .recipient = TUSB_REQ_RCPT_DEVICE, .type = TUSB_REQ_TYPE_STANDARD, .direction = TUSB_DIR_OUT },
         .bRequest = TUSB_REQ_SET_CONFIGURATION,
@@ -522,6 +588,7 @@ bool enum_task(hcd_event_t* event)
   };
   TU_ASSERT(usbh_control_xfer( new_addr, &request, NULL ));
 
+  TU_LOG2("Device configured\r\n");
   new_dev->state = TUSB_DEVICE_STATE_CONFIGURED;
 
   //------------- TODO Get String Descriptors -------------//
@@ -529,6 +596,8 @@ bool enum_task(hcd_event_t* event)
   //------------- parse configuration & install drivers -------------//
   uint8_t const* p_desc = _usbh_ctrl_buf + sizeof(tusb_desc_configuration_t);
 
+  // TU_LOG2_MEM(_usbh_ctrl_buf, ((tusb_desc_configuration_t*)_usbh_ctrl_buf)->wTotalLength, 0);
+
   // parse each interfaces
   while( p_desc < _usbh_ctrl_buf + ((tusb_desc_configuration_t*)_usbh_ctrl_buf)->wTotalLength )
   {
@@ -538,7 +607,7 @@ bool enum_task(hcd_event_t* event)
       p_desc = tu_desc_next(p_desc); // skip the descriptor, increase by the descriptor's length
     }else
     {
-      tusb_desc_interface_t* desc_itf = (tusb_desc_interface_t*) p_desc;
+      tusb_desc_interface_t const* desc_itf = (tusb_desc_interface_t const*) p_desc;
 
       // Check if class is supported
       uint8_t drv_id;
@@ -568,11 +637,8 @@ bool enum_task(hcd_event_t* event)
         {
           uint16_t itf_len = 0;
 
-          if ( usbh_class_drivers[drv_id].open(new_dev->rhport, new_addr, desc_itf, &itf_len) )
-          {
-            mark_interface_endpoint(new_dev->ep2drv, p_desc, itf_len, drv_id);
-          }
-
+          TU_LOG2("%s open\r\n", usbh_class_drivers[drv_id].name);
+          TU_ASSERT( usbh_class_drivers[drv_id].open(new_dev->rhport, new_addr, desc_itf, &itf_len) );
           TU_ASSERT( itf_len >= sizeof(tusb_desc_interface_t) );
           p_desc += itf_len;
         }
@@ -588,7 +654,7 @@ bool enum_task(hcd_event_t* event)
 /* USB Host Driver task
  * This top level thread manages all host controller event and delegates events to class-specific drivers.
  * This should be called periodically within the mainloop or rtos thread.
- *
+ *_usbh_devices[dev_addr].
    @code
     int main(void)
     {
@@ -652,25 +718,4 @@ static inline uint8_t get_configure_number_for_device(tusb_desc_device_t* dev_de
   return config_num;
 }
 
-// Helper marking endpoint of interface belongs to class driver
-// TODO merge with usbd
-static void mark_interface_endpoint(uint8_t ep2drv[8][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   += tu_desc_len(p_desc);
-    p_desc = tu_desc_next(p_desc);
-  }
-}
-
-
 #endif
diff --git a/src/host/usbh.h b/src/host/usbh.h
index fe39a19f4..27193d378 100644
--- a/src/host/usbh.h
+++ b/src/host/usbh.h
@@ -52,6 +52,10 @@ typedef enum tusb_interface_status_{
 } tusb_interface_status_t;
 
 typedef struct {
+  #if CFG_TUSB_DEBUG >= 2
+  char const* name;
+  #endif
+
   uint8_t class_code;
 
   void (* const init) (void);
@@ -69,7 +73,8 @@ typedef struct {
 void tuh_task(void);
 
 // Interrupt handler, name alias to HCD
-#define tuh_isr   hcd_isr
+extern void hcd_int_handler(uint8_t rhport);
+#define tuh_int_handler   hcd_int_handler
 
 tusb_device_state_t tuh_device_get_state (uint8_t dev_addr);
 static inline bool tuh_device_is_configured(uint8_t dev_addr)
@@ -97,6 +102,8 @@ TU_ATTR_WEAK void tuh_umount_cb(uint8_t dev_addr);
 bool usbh_init(void);
 bool usbh_control_xfer (uint8_t dev_addr, tusb_control_request_t* request, uint8_t* data);
 
+bool usbh_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const * ep_desc);
+
 #ifdef __cplusplus
  }
 #endif
diff --git a/src/osal/osal.h b/src/osal/osal.h
index 2e7a3539f..b5057ff4c 100644
--- a/src/osal/osal.h
+++ b/src/osal/osal.h
@@ -37,9 +37,9 @@
 #include "common/tusb_common.h"
 
 // Return immediately
-#define OSAL_TIMEOUT_NOTIMEOUT (0)
+#define OSAL_TIMEOUT_NOTIMEOUT     (0)
 // Default timeout
-#define OSAL_TIMEOUT_NORMAL       (10)
+#define OSAL_TIMEOUT_NORMAL        (10)
 // Wait forever
 #define OSAL_TIMEOUT_WAIT_FOREVER  (UINT32_MAX)
 
@@ -62,7 +62,7 @@ typedef void (*osal_task_func_t)( void * );
 //--------------------------------------------------------------------+
 // OSAL Porting API
 //--------------------------------------------------------------------+
-static inline void osal_task_delay(uint32_t msec);
+//static inline void osal_task_delay(uint32_t msec);
 
 //------------- Semaphore -------------//
 static inline osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semdef);
diff --git a/src/osal/osal_none.h b/src/osal/osal_none.h
index 0ac5e8aac..4a5843f8e 100644
--- a/src/osal/osal_none.h
+++ b/src/osal/osal_none.h
@@ -34,14 +34,14 @@
 //--------------------------------------------------------------------+
 // TASK API
 //--------------------------------------------------------------------+
-static inline void osal_task_delay(uint32_t msec)
-{
-  (void) msec;
-  // TODO only used by Host stack, will implement using SOF
-
-//  uint32_t start = tusb_hal_millis();
-//  while ( ( tusb_hal_millis() - start ) < msec ) {}
-}
+//static inline void osal_task_delay(uint32_t msec)
+//{
+//  (void) msec;
+//  // TODO only used by Host stack, will implement using SOF
+//
+////  uint32_t start = tusb_hal_millis();
+////  while ( ( tusb_hal_millis() - start ) < msec ) {}
+//}
 
 //--------------------------------------------------------------------+
 // Binary Semaphore API
diff --git a/src/portable/dialog/da146xx/dcd_da146xx.c b/src/portable/dialog/da146xx/dcd_da146xx.c
index 3a3459b7d..602836e3a 100644
--- a/src/portable/dialog/da146xx/dcd_da146xx.c
+++ b/src/portable/dialog/da146xx/dcd_da146xx.c
@@ -555,10 +555,10 @@ static void handle_ep0_nak(void)
  *------------------------------------------------------------------*/
 void dcd_init(uint8_t rhport)
 {
-  (void)rhport;
-
   USB->USB_MCTRL_REG = USB_USB_MCTRL_REG_USBEN_Msk;
   tusb_vbus_changed((CRG_TOP->ANA_STATUS_REG & CRG_TOP_ANA_STATUS_REG_VBUS_AVAILABLE_Msk) != 0);
+
+  dcd_connect(rhport);
 }
 
 void dcd_int_enable(uint8_t rhport)
diff --git a/src/portable/espressif/esp32s2/dcd_esp32s2.c b/src/portable/espressif/esp32s2/dcd_esp32s2.c
index 3d30b6016..58d92e728 100644
--- a/src/portable/espressif/esp32s2/dcd_esp32s2.c
+++ b/src/portable/espressif/esp32s2/dcd_esp32s2.c
@@ -54,6 +54,9 @@
 // FIFO size in bytes
 #define EP_FIFO_SIZE      1024
 
+// Max number of IN EP FIFOs
+#define EP_FIFO_NUM 5
+
 typedef struct {
     uint8_t *buffer;
     uint16_t total_len;
@@ -71,6 +74,16 @@ static uint32_t _setup_packet[2];
 #define XFER_CTL_BASE(_ep, _dir) &xfer_status[_ep][_dir]
 static xfer_ctl_t xfer_status[EP_MAX][2];
 
+// Keep count of how many FIFOs are in use
+static uint8_t _allocated_fifos = 1; //FIFO0 is always in use
+
+// Will either return an unused FIFO number, or 0 if all are used.
+static uint8_t get_free_fifo(void)
+{
+  if (_allocated_fifos < EP_FIFO_NUM) return _allocated_fifos++;
+  return 0;
+}
+
 // Setup the control endpoint 0.
 static void bus_reset(void)
 {
@@ -152,8 +165,6 @@ static void enum_done_processing(void)
  *------------------------------------------------------------------*/
 void dcd_init(uint8_t rhport)
 {
-  (void)rhport;
-
   ESP_LOGV(TAG, "DCD init - Start");
 
   // A. Disconnect
@@ -191,6 +202,8 @@ void dcd_init(uint8_t rhport)
                  USB_ENUMDONEMSK_M |
                  USB_RESETDETMSK_M |
                  USB_DISCONNINTMSK_M; // host most only
+
+  dcd_connect(rhport);
 }
 
 void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
@@ -271,8 +284,12 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *desc_edpt)
     // - Offset: GRXFSIZ + 16 + Size*(epnum-1)
     // - IN EP 1 gets FIFO 1, IN EP "n" gets FIFO "n".
 
+    uint8_t fifo_num = get_free_fifo();
+    TU_ASSERT(fifo_num != 0);
+
+    in_ep[epnum].diepctl &= ~(USB_D_TXFNUM1_M | USB_D_EPTYPE1_M | USB_DI_SETD0PID1 | USB_D_MPS1_M);
     in_ep[epnum].diepctl |= USB_D_USBACTEP1_M |
-                            epnum << USB_D_TXFNUM1_S |
+                            fifo_num << USB_D_TXFNUM1_S |
                             desc_edpt->bmAttributes.xfer << USB_D_EPTYPE1_S |
                             (desc_edpt->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS ? (1 << USB_DI_SETD0PID1_S) : 0) |
                             desc_edpt->wMaxPacketSize.size << 0;
@@ -282,8 +299,8 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *desc_edpt)
     // Both TXFD and TXSA are in unit of 32-bit words.
     // IN FIFO 0 was configured during enumeration, hence the "+ 16".
     uint16_t const allocated_size = (USB0.grxfsiz & 0x0000ffff) + 16;
-    uint16_t const fifo_size = (EP_FIFO_SIZE/4 - allocated_size) / (EP_MAX-1);
-    uint32_t const fifo_offset = allocated_size + fifo_size*(epnum-1);
+    uint16_t const fifo_size = (EP_FIFO_SIZE/4 - allocated_size) / (EP_FIFO_NUM-1);
+    uint32_t const fifo_offset = allocated_size + fifo_size*(fifo_num-1);
 
     // DIEPTXF starts at FIFO #1.
     USB0.dieptxf[epnum - 1] = (fifo_size << USB_NPTXFDEP_S) | fifo_offset;
@@ -361,7 +378,8 @@ void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
     }
 
     // Flush the FIFO, and wait until we have confirmed it cleared.
-    USB0.grstctl |= ((epnum - 1) << USB_TXFNUM_S);
+    uint8_t const fifo_num = ((in_ep[epnum].diepctl >> USB_D_TXFNUM1_S) & USB_D_TXFNUM1_V);
+    USB0.grstctl |= (fifo_num << USB_TXFNUM_S);
     USB0.grstctl |= USB_TXFFLSH_M;
     while ((USB0.grstctl & USB_TXFFLSH_M) != 0) ;
   } else {
@@ -660,6 +678,8 @@ static void _dcd_int_handler(void* arg)
     // start of reset
     ESP_EARLY_LOGV(TAG, "dcd_int_handler - reset");
     USB0.gintsts = USB_USBRST_M;
+    // FIFOs will be reassigned when the endpoints are reopen
+    _allocated_fifos = 1;
     bus_reset();
   }
 
diff --git a/src/portable/microchip/samd/dcd_samd.c b/src/portable/microchip/samd/dcd_samd.c
index d0cc8560a..f1dca88a5 100644
--- a/src/portable/microchip/samd/dcd_samd.c
+++ b/src/portable/microchip/samd/dcd_samd.c
@@ -27,7 +27,8 @@
 #include "tusb_option.h"
 
 #if TUSB_OPT_DEVICE_ENABLED && \
-    (CFG_TUSB_MCU == OPT_MCU_SAMD21 || CFG_TUSB_MCU == OPT_MCU_SAMD51 || CFG_TUSB_MCU == OPT_MCU_SAME5X)
+    (CFG_TUSB_MCU == OPT_MCU_SAMD11 || CFG_TUSB_MCU == OPT_MCU_SAMD21 || \
+     CFG_TUSB_MCU == OPT_MCU_SAMD51 || CFG_TUSB_MCU == OPT_MCU_SAME5X)
 
 #include "sam.h"
 #include "device/dcd.h"
@@ -113,7 +114,7 @@ void dcd_int_disable(uint8_t rhport)
   NVIC_DisableIRQ(USB_0_IRQn);
 }
 
-#elif CFG_TUSB_MCU == OPT_MCU_SAMD21
+#elif CFG_TUSB_MCU == OPT_MCU_SAMD11 || CFG_TUSB_MCU == OPT_MCU_SAMD21
 
 void dcd_int_enable(uint8_t rhport)
 {
diff --git a/src/portable/microchip/samg/dcd_samg.c b/src/portable/microchip/samg/dcd_samg.c
index 4a8ed53f3..2b64df50b 100644
--- a/src/portable/microchip/samg/dcd_samg.c
+++ b/src/portable/microchip/samg/dcd_samg.c
@@ -154,9 +154,8 @@ static void bus_reset(void)
 // Initialize controller to device mode
 void dcd_init (uint8_t rhport)
 {
-  (void) rhport;
-
   tu_memclr(_dcd_xfer, sizeof(_dcd_xfer));
+  dcd_connect(rhport);
 }
 
 // Enable device interrupt
diff --git a/src/portable/nordic/nrf5x/dcd_nrf5x.c b/src/portable/nordic/nrf5x/dcd_nrf5x.c
index 7ca32c4f6..cd35f2e5b 100644
--- a/src/portable/nordic/nrf5x/dcd_nrf5x.c
+++ b/src/portable/nordic/nrf5x/dcd_nrf5x.c
@@ -271,6 +271,10 @@ void dcd_disconnect(uint8_t rhport)
 {
   (void) rhport;
   NRF_USBD->USBPULLUP = 0;
+
+  // Disable Pull-up does not trigger Power USB Removed, in fact it have no
+  // impact on the USB Power status at all -> need to submit unplugged event to the stack.
+  dcd_event_bus_signal(0, DCD_EVENT_UNPLUGGED, false);
 }
 
 // connect by enabling internal pull-up resistor on D+/D-
@@ -693,6 +697,8 @@ void tusb_hal_nrf_power_event (uint32_t event)
   switch ( event )
   {
     case USB_EVT_DETECTED:
+      TU_LOG2("Power USB Detect\r\n");
+
       if ( !NRF_USBD->ENABLE )
       {
         /* Prepare for READY event receiving */
@@ -743,6 +749,12 @@ void tusb_hal_nrf_power_event (uint32_t event)
     break;
 
     case USB_EVT_READY:
+      TU_LOG2("Power USB Ready\r\n");
+
+      // Skip if pull-up is enabled and HCLK is already running.
+      // Application probably call this more than necessary.
+      if ( NRF_USBD->USBPULLUP && hfclk_running() ) break;
+
       /* Waiting for USBD peripheral enabled */
       while ( !(USBD_EVENTCAUSE_READY_Msk & NRF_USBD->EVENTCAUSE) ) { }
 
@@ -810,6 +822,7 @@ void tusb_hal_nrf_power_event (uint32_t event)
     break;
 
     case USB_EVT_REMOVED:
+      TU_LOG2("Power USB Removed\r\n");
       if ( NRF_USBD->ENABLE )
       {
         // Abort all transfers
@@ -829,7 +842,7 @@ void tusb_hal_nrf_power_event (uint32_t event)
 
         hfclk_disable();
 
-        dcd_event_bus_signal(0, DCD_EVENT_UNPLUGGED, true);
+        dcd_event_bus_signal(0, DCD_EVENT_UNPLUGGED, (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) ? true : false);
       }
     break;
 
diff --git a/src/portable/nxp/lpc17_40/dcd_lpc17_40.c b/src/portable/nxp/lpc17_40/dcd_lpc17_40.c
index f7a59a808..39c5e6638 100644
--- a/src/portable/nxp/lpc17_40/dcd_lpc17_40.c
+++ b/src/portable/nxp/lpc17_40/dcd_lpc17_40.c
@@ -181,6 +181,8 @@ void dcd_init(uint8_t rhport)
   LPC_USB->UDCAH = (uint32_t) _dcd.udca;
   LPC_USB->DMAIntEn = (DMA_INT_END_OF_XFER_MASK /*| DMA_INT_NEW_DD_REQUEST_MASK*/ | DMA_INT_ERROR_MASK);
 
+  dcd_connect(rhport);
+
   // Clear pending IRQ
   NVIC_ClearPendingIRQ(USB_IRQn);
 }
diff --git a/src/portable/nxp/transdimension/dcd_transdimension.c b/src/portable/nxp/transdimension/dcd_transdimension.c
index 46751cf5d..755e7635e 100644
--- a/src/portable/nxp/transdimension/dcd_transdimension.c
+++ b/src/portable/nxp/transdimension/dcd_transdimension.c
@@ -345,7 +345,8 @@ void dcd_init(uint8_t rhport)
   dcd_reg->USBSTS  = dcd_reg->USBSTS;
   dcd_reg->USBINTR = INTR_USB | INTR_ERROR | INTR_PORT_CHANGE | INTR_RESET | INTR_SUSPEND /*| INTR_SOF*/;
 
-  dcd_reg->USBCMD &= ~0x00FF0000; // Interrupt Threshold Interval = 0
+  dcd_reg->USBCMD &= ~0x00FF0000;     // Interrupt Threshold Interval = 0
+  dcd_reg->USBCMD |= USBCMD_RUN_STOP; // Connect
 }
 
 void dcd_int_enable(uint8_t rhport)
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index da184abe7..32179874e 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -205,7 +205,6 @@ static inline void reg16_clear_bits(__IO uint16_t *reg, uint16_t mask) {
 
 void dcd_init (uint8_t rhport)
 {
-  (void)rhport;
   /* Clocks should already be enabled */
   /* Use __HAL_RCC_USB_CLK_ENABLE(); to enable the clocks before calling this function */
 
@@ -244,7 +243,8 @@ void dcd_init (uint8_t rhport)
   USB->CNTR |= USB_CNTR_RESETM | (USE_SOF ? USB_CNTR_SOFM : 0) | USB_CNTR_ESOFM | USB_CNTR_CTRM | USB_CNTR_SUSPM | USB_CNTR_WKUPM;
   dcd_handle_bus_reset();
   
-  // Data-line pull-up is left disconnected.
+  // Enable pull-up if supported
+  if ( dcd_connect ) dcd_connect(rhport);
 }
 
 // Define only on MCU with internal pull-up. BSP can define on MCU without internal PU.
diff --git a/src/portable/st/synopsys/dcd_synopsys.c b/src/portable/st/synopsys/dcd_synopsys.c
index 7eb63e52f..cc7181c3d 100644
--- a/src/portable/st/synopsys/dcd_synopsys.c
+++ b/src/portable/st/synopsys/dcd_synopsys.c
@@ -231,6 +231,7 @@ static void bus_reset(uint8_t rhport)
 }
 
 // Set turn-around timeout according to link speed
+extern uint32_t SystemCoreClock;
 static void set_turnaround(USB_OTG_GlobalTypeDef * usb_otg, tusb_speed_t speed)
 {
   usb_otg->GUSBCFG &= ~USB_OTG_GUSBCFG_TRDT;
@@ -243,7 +244,6 @@ static void set_turnaround(USB_OTG_GlobalTypeDef * usb_otg, tusb_speed_t speed)
   else
   {
     // Turnaround timeout depends on the MCU clock
-    extern uint32_t SystemCoreClock;
     uint32_t turnaround;
 
     if ( SystemCoreClock >= 32000000U )
@@ -365,6 +365,13 @@ static void edpt_schedule_packets(uint8_t rhport, uint8_t const epnum, uint8_t c
         ((total_bytes << USB_OTG_DIEPTSIZ_XFRSIZ_Pos) & USB_OTG_DIEPTSIZ_XFRSIZ_Msk);
 
     in_ep[epnum].DIEPCTL |= USB_OTG_DIEPCTL_EPENA | USB_OTG_DIEPCTL_CNAK;
+    // For ISO endpoint set correct odd/even bit for next frame.
+    if ((in_ep[epnum].DIEPCTL & USB_OTG_DIEPCTL_EPTYP) == USB_OTG_DIEPCTL_EPTYP_0)
+    {
+      // Take odd/even bit from frame counter.
+      uint32_t const odd_frame_now = (dev->DSTS & (1u << USB_OTG_DSTS_FNSOF_Pos));
+      in_ep[epnum].DIEPCTL |= (odd_frame_now ? USB_OTG_DIEPCTL_SD0PID_SEVNFRM_Msk : USB_OTG_DIEPCTL_SODDFRM_Msk);
+    }
     // Enable fifo empty interrupt only if there are something to put in the fifo.
     if(total_bytes != 0) {
       dev->DIEPEMPMSK |= (1 << epnum);
@@ -453,6 +460,8 @@ void dcd_init (uint8_t rhport)
 
   // Enable global interrupt
   usb_otg->GAHBCFG |= USB_OTG_GAHBCFG_GINT;
+
+  dcd_connect(rhport);
 }
 
 void dcd_int_enable (uint8_t rhport)
@@ -514,8 +523,14 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
 
   TU_ASSERT(epnum < EP_MAX);
 
-  // TODO ISO endpoint can be up to 1024 bytes
-  TU_ASSERT(desc_edpt->wMaxPacketSize.size <= (get_speed(rhport) == TUSB_SPEED_HIGH ? 512 : 64));
+  if (desc_edpt->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS)
+  {
+    TU_ASSERT(desc_edpt->wMaxPacketSize.size <= (get_speed(rhport) == TUSB_SPEED_HIGH ? 1024 : 1023));
+  }
+  else
+  {
+    TU_ASSERT(desc_edpt->wMaxPacketSize.size <= (get_speed(rhport) == TUSB_SPEED_HIGH ? 512 : 64));
+  }
 
   xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir);
   xfer->max_size = desc_edpt->wMaxPacketSize.size;
@@ -622,9 +637,7 @@ bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
   return true;
 }
 
-// TODO: The logic for STALLing and disabling an endpoint is very similar
-// (send STALL versus NAK handshakes back). Refactor into resuable function.
-void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
+static void dcd_edpt_disable (uint8_t rhport, uint8_t ep_addr, bool stall)
 {
   (void) rhport;
 
@@ -639,26 +652,26 @@ void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
   if(dir == TUSB_DIR_IN) {
     // Only disable currently enabled non-control endpoint
     if ( (epnum == 0) || !(in_ep[epnum].DIEPCTL & USB_OTG_DIEPCTL_EPENA) ){
-      in_ep[epnum].DIEPCTL |= (USB_OTG_DIEPCTL_SNAK | USB_OTG_DIEPCTL_STALL);
+      in_ep[epnum].DIEPCTL |= USB_OTG_DIEPCTL_SNAK | (stall ? USB_OTG_DIEPCTL_STALL : 0);
     } else {
       // Stop transmitting packets and NAK IN xfers.
       in_ep[epnum].DIEPCTL |= USB_OTG_DIEPCTL_SNAK;
       while((in_ep[epnum].DIEPINT & USB_OTG_DIEPINT_INEPNE) == 0);
 
       // Disable the endpoint.
-      in_ep[epnum].DIEPCTL |= (USB_OTG_DIEPCTL_STALL | USB_OTG_DIEPCTL_EPDIS);
+      in_ep[epnum].DIEPCTL |= USB_OTG_DIEPCTL_EPDIS | (stall ? USB_OTG_DIEPCTL_STALL : 0);
       while((in_ep[epnum].DIEPINT & USB_OTG_DIEPINT_EPDISD_Msk) == 0);
       in_ep[epnum].DIEPINT = USB_OTG_DIEPINT_EPDISD;
     }
 
     // Flush the FIFO, and wait until we have confirmed it cleared.
-    usb_otg->GRSTCTL |= ((epnum - 1) << USB_OTG_GRSTCTL_TXFNUM_Pos);
+    usb_otg->GRSTCTL |= (epnum << USB_OTG_GRSTCTL_TXFNUM_Pos);
     usb_otg->GRSTCTL |= USB_OTG_GRSTCTL_TXFFLSH;
     while((usb_otg->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH_Msk) != 0);
   } else {
     // Only disable currently enabled non-control endpoint
     if ( (epnum == 0) || !(out_ep[epnum].DOEPCTL & USB_OTG_DOEPCTL_EPENA) ){
-      out_ep[epnum].DOEPCTL |= USB_OTG_DOEPCTL_STALL;
+      out_ep[epnum].DOEPCTL |= stall ? USB_OTG_DOEPCTL_STALL : 0;
     } else {
       // Asserting GONAK is required to STALL an OUT endpoint.
       // Simpler to use polling here, we don't use the "B"OUTNAKEFF interrupt
@@ -668,7 +681,7 @@ void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
       while((usb_otg->GINTSTS & USB_OTG_GINTSTS_BOUTNAKEFF_Msk) == 0);
 
       // Ditto here- disable the endpoint.
-      out_ep[epnum].DOEPCTL |= (USB_OTG_DOEPCTL_STALL | USB_OTG_DOEPCTL_EPDIS);
+      out_ep[epnum].DOEPCTL |= USB_OTG_DOEPCTL_EPDIS | (stall ? USB_OTG_DOEPCTL_STALL : 0);
       while((out_ep[epnum].DOEPINT & USB_OTG_DOEPINT_EPDISD_Msk) == 0);
       out_ep[epnum].DOEPINT = USB_OTG_DOEPINT_EPDISD;
 
@@ -678,6 +691,32 @@ void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
   }
 }
 
+/**
+ * Close an endpoint.
+ */
+void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr)
+{
+  USB_OTG_GlobalTypeDef * usb_otg = GLOBAL_BASE(rhport);
+
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir   = tu_edpt_dir(ep_addr);
+
+  dcd_edpt_disable(rhport, ep_addr, false);
+  if (dir == TUSB_DIR_IN)
+  {
+    uint16_t const fifo_size = (usb_otg->DIEPTXF[epnum - 1] & USB_OTG_DIEPTXF_INEPTXFD_Msk) >> USB_OTG_DIEPTXF_INEPTXFD_Pos;
+    uint16_t const fifo_start = (usb_otg->DIEPTXF[epnum - 1] & USB_OTG_DIEPTXF_INEPTXSA_Msk) >> USB_OTG_DIEPTXF_INEPTXSA_Pos;
+    // For now only endpoint that has FIFO at the end of FIFO memory can be closed without fuss.
+    TU_ASSERT(fifo_start + fifo_size == _allocated_fifo_words,);
+    _allocated_fifo_words -= fifo_size;
+  }
+}
+
+void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
+{
+  dcd_edpt_disable(rhport, ep_addr, true);
+}
+
 void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
 {
   (void) rhport;
diff --git a/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c b/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
index 8672050f0..ad71e1168 100644
--- a/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
+++ b/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
@@ -70,8 +70,7 @@ typedef enum
   SIZXY = 7
 } ep_regs_index_t;
 
-#define EP_REGS(epnum, dir) &USBOEPCNF_1 + 64*dir + 8*(epnum - 1)
-
+#define EP_REGS(epnum, dir) ((ep_regs_t) ((uintptr_t)&USBOEPCNF_1 + 64*dir + 8*(epnum - 1)))
 
 static void bus_reset(void)
 {
@@ -134,6 +133,9 @@ void dcd_init (uint8_t rhport)
   // Enable reset and wait for it before continuing.
   USBIE |= RSTRIE;
 
+  // Enable pullup.
+  USBCNF |= PUR_EN;
+
   USBKEYPID = 0;
 }
 
diff --git a/src/tusb_option.h b/src/tusb_option.h
index abf6450ca..7e486bff0 100644
--- a/src/tusb_option.h
+++ b/src/tusb_option.h
@@ -55,10 +55,11 @@
 #define OPT_MCU_NRF5X             100 ///< Nordic nRF5x series
 
 // SAM
+#define OPT_MCU_SAMD11            204 ///< MicroChip SAMD11
 #define OPT_MCU_SAMD21            200 ///< MicroChip SAMD21
 #define OPT_MCU_SAMD51            201 ///< MicroChip SAMD51
-#define OPT_MCU_SAMG              202 ///< MicroChip SAMDG series
 #define OPT_MCU_SAME5X            203 ///< MicroChip SAM E5x
+#define OPT_MCU_SAMG              202 ///< MicroChip SAMDG series
 
 // STM32
 #define OPT_MCU_STM32F0           300 ///< ST STM32F0