Merge branch 'master' into feature/STM32G0

32 files changed, 2128 insertions, 757 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 000000000..396737ed5
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,98 @@
+# TODO more docs and example on how to use this file
+# Usage: requires target tinyusb_config which expose tusb_config.h file
+# TINYUSB_TARGET_PREFIX and TINYUSB_TARGET_SUFFIX can be used to change the name of the target
+
+cmake_minimum_required(VERSION 3.17)
+
+# Add tinyusb to a target, if user don't want to compile tinyusb as a library
+function(add_tinyusb TARGET)
+  target_sources(${TARGET} PRIVATE
+    # common
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/tusb.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/common/tusb_fifo.c
+    # device
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/device/usbd.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/device/usbd_control.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/audio/audio_device.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/cdc/cdc_device.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/dfu/dfu_device.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/dfu/dfu_rt_device.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/hid/hid_device.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/midi/midi_device.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/msc/msc_device.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/net/ecm_rndis_device.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/net/ncm_device.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/usbtmc/usbtmc_device.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/vendor/vendor_device.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/video/video_device.c
+    # host
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/host/usbh.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/host/hub.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/cdc/cdc_host.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/hid/hid_host.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/msc/msc_host.c
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/class/vendor/vendor_host.c
+    )
+  target_include_directories(${TARGET} PUBLIC
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}
+    # TODO for net driver, should be removed/changed
+    ${CMAKE_CURRENT_FUNCTION_LIST_DIR}/../lib/networking
+    )
+
+  if (CMAKE_C_COMPILER_ID STREQUAL "GNU")
+    target_compile_options(${TARGET} PRIVATE
+      -Wall
+      -Wextra
+      -Werror
+      -Wfatal-errors
+      -Wdouble-promotion
+      -Wstrict-prototypes
+      -Wstrict-overflow
+      -Werror-implicit-function-declaration
+      -Wfloat-equal
+      -Wundef
+      -Wshadow
+      -Wwrite-strings
+      -Wsign-compare
+      -Wmissing-format-attribute
+      -Wunreachable-code
+      -Wcast-align
+      -Wcast-function-type
+      -Wcast-qual
+      -Wnull-dereference
+      -Wuninitialized
+      -Wunused
+      -Wreturn-type
+      -Wredundant-decls
+      )
+  endif ()
+endfunction()
+
+#------------------------------------
+# TinyUSB as library target
+#------------------------------------
+set(TINYUSB_TARGET "tinyusb")
+set(TINYUSB_CONFIG_TARGET "tinyusb_config")
+
+if (DEFINED TINYUSB_TARGET_PREFIX)
+  set(TINYUSB_TARGET "${TINYUSB_TARGET_PREFIX}${TINYUSB_TARGET}")
+  set(TINYUSB_CONFIG_TARGET "${TINYUSB_TARGET_PREFIX}${TINYUSB_CONFIG_TARGET}")
+endif ()
+
+if (DEFINED TINYUSB_TARGET_SUFFIX)
+  set(TINYUSB_TARGET "${TINYUSB_TARGET}${TINYUSB_TARGET_SUFFIX}")
+  set(TINYUSB_CONFIG_TARGET "${TINYUSB_CONFIG_TARGET}${TINYUSB_TARGET_SUFFIX}")
+endif ()
+
+add_library(${TINYUSB_TARGET} STATIC)
+add_tinyusb(${TINYUSB_TARGET})
+
+# Check if tinyusb_config target is defined
+if (NOT TARGET ${TINYUSB_CONFIG_TARGET})
+  message(FATAL_ERROR "${TINYUSB_CONFIG_TARGET} target is not defined")
+endif()
+
+# Link with tinyusb_config target
+target_link_libraries(${TINYUSB_TARGET} PUBLIC
+  ${TINYUSB_CONFIG_TARGET}
+  )
diff --git a/src/class/cdc/cdc_device.c b/src/class/cdc/cdc_device.c
index a82ef1d62..5adce521d 100644
--- a/src/class/cdc/cdc_device.c
+++ b/src/class/cdc/cdc_device.c
@@ -53,7 +53,7 @@ typedef struct
 
   /*------------- From this point, data is not cleared by bus reset -------------*/
   char    wanted_char;
-  cdc_line_coding_t line_coding;
+  TU_ATTR_ALIGNED(4) cdc_line_coding_t line_coding;
 
   // FIFO
   tu_fifo_t rx_ff;
diff --git a/src/class/cdc/cdc_host.c b/src/class/cdc/cdc_host.c
index c0cc41adf..ce9f27c33 100644
--- a/src/class/cdc/cdc_host.c
+++ b/src/class/cdc/cdc_host.c
@@ -33,14 +33,12 @@
 
 #include "cdc_host.h"
 
-
 // Debug level, TUSB_CFG_DEBUG must be at least this level for debug message
 #define CDCH_DEBUG   2
-
-#define TU_LOG_CDCH(...)   TU_LOG(CDCH_DEBUG, __VA_ARGS__)
+#define TU_LOG_DRV(...)   TU_LOG(CDCH_DEBUG, __VA_ARGS__)
 
 //--------------------------------------------------------------------+
-// MACRO CONSTANT TYPEDEF
+// Host CDC Interface
 //--------------------------------------------------------------------+
 
 typedef struct {
@@ -49,11 +47,12 @@ typedef struct {
   uint8_t bInterfaceSubClass;
   uint8_t bInterfaceProtocol;
 
+  uint8_t serial_drid; // Serial Driver ID
   cdc_acm_capability_t acm_capability;
   uint8_t ep_notif;
 
-  cdc_line_coding_t line_coding;  // Baudrate, stop bits, parity, data width
-  uint8_t line_state;             // DTR (bit0), RTS (bit1)
+  uint8_t line_state;                               // DTR (bit0), RTS (bit1)
+  TU_ATTR_ALIGNED(4) cdc_line_coding_t line_coding; // Baudrate, stop bits, parity, data width
 
   tuh_xfer_cb_t user_control_cb;
 
@@ -70,12 +69,102 @@ typedef struct {
 
 } cdch_interface_t;
 
+CFG_TUH_MEM_SECTION
+static cdch_interface_t cdch_data[CFG_TUH_CDC];
+
 //--------------------------------------------------------------------+
-// INTERNAL OBJECT & FUNCTION DECLARATION
+// Serial Driver
 //--------------------------------------------------------------------+
 
-CFG_TUH_MEM_SECTION
-static cdch_interface_t cdch_data[CFG_TUH_CDC];
+//------------- ACM prototypes -------------//
+static void acm_process_config(tuh_xfer_t* xfer);
+
+static bool acm_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool acm_set_control_line_state(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool acm_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+
+//------------- FTDI prototypes -------------//
+#if CFG_TUH_CDC_FTDI
+#include "serial/ftdi_sio.h"
+
+static uint16_t const ftdi_pids[] = { TU_FTDI_PID_LIST };
+enum {
+  FTDI_PID_COUNT = sizeof(ftdi_pids) / sizeof(ftdi_pids[0])
+};
+
+// Store last request baudrate since divisor to baudrate is not easy
+static uint32_t _ftdi_requested_baud;
+
+static bool ftdi_open(uint8_t daddr, const tusb_desc_interface_t *itf_desc, uint16_t max_len);
+static void ftdi_process_config(tuh_xfer_t* xfer);
+
+static bool ftdi_sio_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool ftdi_sio_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+#endif
+
+//------------- CP210X prototypes -------------//
+#if CFG_TUH_CDC_CP210X
+#include "serial/cp210x.h"
+
+static uint16_t const cp210x_pids[] = { TU_CP210X_PID_LIST };
+enum {
+  CP210X_PID_COUNT = sizeof(cp210x_pids) / sizeof(cp210x_pids[0])
+};
+
+static bool cp210x_open(uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len);
+static void cp210x_process_config(tuh_xfer_t* xfer);
+
+static bool cp210x_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool cp210x_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+#endif
+
+enum {
+  SERIAL_DRIVER_ACM = 0,
+
+#if CFG_TUH_CDC_FTDI
+  SERIAL_DRIVER_FTDI,
+#endif
+
+#if CFG_TUH_CDC_CP210X
+  SERIAL_DRIVER_CP210X,
+#endif
+};
+
+typedef struct {
+  void (*const process_set_config)(tuh_xfer_t* xfer);
+  bool (*const set_control_line_state)(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+  bool (*const set_baudrate)(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+} cdch_serial_driver_t;
+
+// Note driver list must be in the same order as SERIAL_DRIVER enum
+static const cdch_serial_driver_t serial_drivers[] = {
+  { .process_set_config     = acm_process_config,
+    .set_control_line_state = acm_set_control_line_state,
+    .set_baudrate           = acm_set_baudrate
+  },
+
+  #if CFG_TUH_CDC_FTDI
+  { .process_set_config     = ftdi_process_config,
+    .set_control_line_state = ftdi_sio_set_modem_ctrl,
+    .set_baudrate           = ftdi_sio_set_baudrate
+  },
+  #endif
+
+  #if CFG_TUH_CDC_CP210X
+  { .process_set_config     = cp210x_process_config,
+    .set_control_line_state = cp210x_set_modem_ctrl,
+    .set_baudrate           = cp210x_set_baudrate
+  },
+  #endif
+};
+
+enum {
+  SERIAL_DRIVER_COUNT = sizeof(serial_drivers) / sizeof(serial_drivers[0])
+};
+
+//--------------------------------------------------------------------+
+// INTERNAL OBJECT & FUNCTION DECLARATION
+//--------------------------------------------------------------------+
 
 static inline cdch_interface_t* get_itf(uint8_t idx)
 {
@@ -101,16 +190,29 @@ static inline uint8_t get_idx_by_ep_addr(uint8_t daddr, uint8_t ep_addr)
 }
 
 
-static cdch_interface_t* find_new_itf(void)
+static cdch_interface_t* make_new_itf(uint8_t daddr, tusb_desc_interface_t const *itf_desc)
 {
   for(uint8_t i=0; i<CFG_TUH_CDC; i++)
   {
-    if (cdch_data[i].daddr == 0) return &cdch_data[i];
+    if (cdch_data[i].daddr == 0) {
+      cdch_interface_t* p_cdc = &cdch_data[i];
+
+      p_cdc->daddr              = daddr;
+      p_cdc->bInterfaceNumber   = itf_desc->bInterfaceNumber;
+      p_cdc->bInterfaceSubClass = itf_desc->bInterfaceSubClass;
+      p_cdc->bInterfaceProtocol = itf_desc->bInterfaceProtocol;
+      p_cdc->line_state         = 0;
+      return p_cdc;
+    }
   }
 
   return NULL;
 }
 
+static bool open_ep_stream_pair(cdch_interface_t* p_cdc , tusb_desc_endpoint_t const *desc_ep);
+static void set_config_complete(cdch_interface_t * p_cdc, uint8_t idx, uint8_t itf_num);
+static void cdch_internal_control_complete(tuh_xfer_t* xfer);
+
 //--------------------------------------------------------------------+
 // APPLICATION API
 //--------------------------------------------------------------------+
@@ -270,102 +372,139 @@ static void cdch_internal_control_complete(tuh_xfer_t* xfer)
 
   if (xfer->result == XFER_RESULT_SUCCESS)
   {
-    switch(xfer->setup->bRequest)
-    {
-      case CDC_REQUEST_SET_CONTROL_LINE_STATE:
-        p_cdc->line_state = (uint8_t) tu_le16toh(xfer->setup->wValue);
-      break;
+    switch (p_cdc->serial_drid) {
+      case SERIAL_DRIVER_ACM:
+        switch (xfer->setup->bRequest) {
+          case CDC_REQUEST_SET_CONTROL_LINE_STATE:
+            p_cdc->line_state = (uint8_t) tu_le16toh(xfer->setup->wValue);
+            break;
 
-      case CDC_REQUEST_SET_LINE_CODING:
-      {
-        uint16_t const len = tu_min16(sizeof(cdc_line_coding_t), tu_le16toh(xfer->setup->wLength));
-        memcpy(&p_cdc->line_coding, xfer->buffer, len);
-      }
-      break;
+          case CDC_REQUEST_SET_LINE_CODING: {
+            uint16_t const len = tu_min16(sizeof(cdc_line_coding_t), tu_le16toh(xfer->setup->wLength));
+            memcpy(&p_cdc->line_coding, xfer->buffer, len);
+          }
+            break;
+
+          default: break;
+        }
+        break;
+
+      #if CFG_TUH_CDC_FTDI
+      case SERIAL_DRIVER_FTDI:
+        switch (xfer->setup->bRequest) {
+          case FTDI_SIO_MODEM_CTRL:
+            p_cdc->line_state = (uint8_t) (tu_le16toh(xfer->setup->wValue) & 0x00ff);
+            break;
+
+          case FTDI_SIO_SET_BAUD_RATE:
+            // convert from divisor to baudrate is not supported
+            p_cdc->line_coding.bit_rate = _ftdi_requested_baud;
+            break;
+
+          default: break;
+        }
+        break;
+      #endif
+
+      #if CFG_TUH_CDC_CP210X
+      case SERIAL_DRIVER_CP210X:
+        switch(xfer->setup->bRequest) {
+          case CP210X_SET_MHS:
+            p_cdc->line_state = (uint8_t) (tu_le16toh(xfer->setup->wValue) & 0x00ff);
+            break;
+
+          case CP210X_SET_BAUDRATE: {
+            uint32_t baudrate;
+            memcpy(&baudrate, xfer->buffer, sizeof(uint32_t));
+            p_cdc->line_coding.bit_rate = tu_le32toh(baudrate);
+          }
+            break;
+        }
+        break;
+      #endif
 
       default: break;
     }
   }
 
   xfer->complete_cb = p_cdc->user_control_cb;
-  xfer->complete_cb(xfer);
+  if (xfer->complete_cb) {
+    xfer->complete_cb(xfer);
+  }
 }
 
-bool tuh_cdc_set_control_line_state(uint8_t idx, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
+bool tuh_cdc_set_control_line_state(uint8_t idx, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
   cdch_interface_t* p_cdc = get_itf(idx);
-  TU_VERIFY(p_cdc && p_cdc->acm_capability.support_line_request);
+  TU_VERIFY(p_cdc && p_cdc->serial_drid < SERIAL_DRIVER_COUNT);
+  cdch_serial_driver_t const* driver = &serial_drivers[p_cdc->serial_drid];
 
-  TU_LOG_CDCH("CDC Set Control Line State\r\n");
+  if ( complete_cb ) {
+    return driver->set_control_line_state(p_cdc, line_state, complete_cb, user_data);
+  }else {
+    // blocking
+    xfer_result_t result = XFER_RESULT_INVALID;
+    bool ret = driver->set_control_line_state(p_cdc, line_state, complete_cb, (uintptr_t) &result);
 
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_INTERFACE,
-      .type      = TUSB_REQ_TYPE_CLASS,
-      .direction = TUSB_DIR_OUT
-    },
-    .bRequest = CDC_REQUEST_SET_CONTROL_LINE_STATE,
-    .wValue   = tu_htole16(line_state),
-    .wIndex   = tu_htole16((uint16_t) p_cdc->bInterfaceNumber),
-    .wLength  = 0
-  };
+    if (user_data) {
+      // user_data is not NULL, return result via user_data
+      *((xfer_result_t*) user_data) = result;
+    }
 
-  p_cdc->user_control_cb = complete_cb;
-  tuh_xfer_t xfer =
-  {
-    .daddr       = p_cdc->daddr,
-    .ep_addr     = 0,
-    .setup       = &request,
-    .buffer      = NULL,
-    .complete_cb = cdch_internal_control_complete,
-    .user_data   = user_data
-  };
+    TU_VERIFY(ret && result == XFER_RESULT_SUCCESS);
 
-  TU_ASSERT(tuh_control_xfer(&xfer));
-  return true;
+    p_cdc->line_state = (uint8_t) line_state;
+    return true;
+  }
+}
+
+bool tuh_cdc_set_baudrate(uint8_t idx, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc && p_cdc->serial_drid < SERIAL_DRIVER_COUNT);
+  cdch_serial_driver_t const* driver = &serial_drivers[p_cdc->serial_drid];
+
+  if ( complete_cb ) {
+    return driver->set_baudrate(p_cdc, baudrate, complete_cb, user_data);
+  }else {
+    // blocking
+    xfer_result_t result = XFER_RESULT_INVALID;
+    bool ret = driver->set_baudrate(p_cdc, baudrate, complete_cb, (uintptr_t) &result);
+
+    if (user_data) {
+      // user_data is not NULL, return result via user_data
+      *((xfer_result_t*) user_data) = result;
+    }
+
+    TU_VERIFY(ret && result == XFER_RESULT_SUCCESS);
+
+    p_cdc->line_coding.bit_rate = baudrate;
+    return true;
+  }
 }
 
 bool tuh_cdc_set_line_coding(uint8_t idx, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
 {
   cdch_interface_t* p_cdc = get_itf(idx);
-  TU_VERIFY(p_cdc && p_cdc->acm_capability.support_line_request);
-
-  TU_LOG_CDCH("CDC Set Line Conding\r\n");
+  // only ACM support this set line coding request
+  TU_VERIFY(p_cdc && p_cdc->serial_drid == SERIAL_DRIVER_ACM);
+  TU_VERIFY(p_cdc->acm_capability.support_line_request);
 
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_INTERFACE,
-      .type      = TUSB_REQ_TYPE_CLASS,
-      .direction = TUSB_DIR_OUT
-    },
-    .bRequest = CDC_REQUEST_SET_LINE_CODING,
-    .wValue   = 0,
-    .wIndex   = tu_htole16(p_cdc->bInterfaceNumber),
-    .wLength  = tu_htole16(sizeof(cdc_line_coding_t))
-  };
+  if ( complete_cb ) {
+    return acm_set_line_coding(p_cdc, line_coding, complete_cb, user_data);
+  }else {
+    // blocking
+    xfer_result_t result = XFER_RESULT_INVALID;
+    bool ret = acm_set_line_coding(p_cdc, line_coding, complete_cb, (uintptr_t) &result);
 
-  // use usbh enum buf to hold line coding since user line_coding variable may not live long enough
-  // for the transfer to complete
-  uint8_t* enum_buf = usbh_get_enum_buf();
-  memcpy(enum_buf, line_coding, sizeof(cdc_line_coding_t));
+    if (user_data) {
+      // user_data is not NULL, return result via user_data
+      *((xfer_result_t*) user_data) = result;
+    }
 
-  p_cdc->user_control_cb = complete_cb;
-  tuh_xfer_t xfer =
-  {
-    .daddr       = p_cdc->daddr,
-    .ep_addr     = 0,
-    .setup       = &request,
-    .buffer      = enum_buf,
-    .complete_cb = cdch_internal_control_complete,
-    .user_data   = user_data
-  };
+    TU_VERIFY(ret && result == XFER_RESULT_SUCCESS);
 
-  TU_ASSERT(tuh_control_xfer(&xfer));
-  return true;
+    p_cdc->line_coding = *line_coding;
+    return true;
+  }
 }
 
 //--------------------------------------------------------------------+
@@ -397,6 +536,8 @@ void cdch_close(uint8_t daddr)
     cdch_interface_t* p_cdc = &cdch_data[idx];
     if (p_cdc->daddr == daddr)
     {
+      TU_LOG_DRV("  CDCh close addr = %u index = %u\r\n", daddr, idx);
+
       // Invoke application callback
       if (tuh_cdc_umount_cb) tuh_cdc_umount_cb(idx);
 
@@ -409,8 +550,7 @@ void cdch_close(uint8_t daddr)
   }
 }
 
-bool cdch_xfer_cb(uint8_t daddr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
-{
+bool cdch_xfer_cb(uint8_t daddr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes) {
   // TODO handle stall response, retry failed transfer ...
   TU_ASSERT(event == XFER_RESULT_SUCCESS);
 
@@ -418,32 +558,40 @@ bool cdch_xfer_cb(uint8_t daddr, uint8_t ep_addr, xfer_result_t event, uint32_t
   cdch_interface_t * p_cdc = get_itf(idx);
   TU_ASSERT(p_cdc);
 
-  if ( ep_addr == p_cdc->stream.tx.ep_addr )
-  {
+  if ( ep_addr == p_cdc->stream.tx.ep_addr ) {
     // invoke tx complete callback to possibly refill tx fifo
     if (tuh_cdc_tx_complete_cb) tuh_cdc_tx_complete_cb(idx);
 
-    if ( 0 == tu_edpt_stream_write_xfer(&p_cdc->stream.tx) )
-    {
+    if ( 0 == tu_edpt_stream_write_xfer(&p_cdc->stream.tx) ) {
       // If there is no data left, a ZLP should be sent if:
       // - xferred_bytes is multiple of EP Packet size and not zero
       tu_edpt_stream_write_zlp_if_needed(&p_cdc->stream.tx, xferred_bytes);
     }
   }
-  else if ( ep_addr == p_cdc->stream.rx.ep_addr )
-  {
-    tu_edpt_stream_read_xfer_complete(&p_cdc->stream.rx, xferred_bytes);
+  else if ( ep_addr == p_cdc->stream.rx.ep_addr ) {
+    #if CFG_TUH_CDC_FTDI
+    if (p_cdc->serial_drid == SERIAL_DRIVER_FTDI) {
+      // FTDI reserve 2 bytes for status
+      // FTDI status
+//      uint8_t status[2] = {
+//        p_cdc->stream.rx.ep_buf[0],
+//        p_cdc->stream.rx.ep_buf[1]
+//      };
+      tu_edpt_stream_read_xfer_complete_offset(&p_cdc->stream.rx, xferred_bytes, 2);
+    }else
+    #endif
+    {
+      tu_edpt_stream_read_xfer_complete(&p_cdc->stream.rx, xferred_bytes);
+    }
 
     // invoke receive callback
-    if (tuh_cdc_rx_cb)  tuh_cdc_rx_cb(idx);
+    if (tuh_cdc_rx_cb) tuh_cdc_rx_cb(idx);
 
     // prepare for next transfer if needed
     tu_edpt_stream_read_xfer(&p_cdc->stream.rx);
-  }else if ( ep_addr == p_cdc->ep_notif )
-  {
+  }else if ( ep_addr == p_cdc->ep_notif ) {
     // TODO handle notification endpoint
-  }else
-  {
+  }else {
     TU_ASSERT(false);
   }
 
@@ -454,26 +602,122 @@ bool cdch_xfer_cb(uint8_t daddr, uint8_t ep_addr, xfer_result_t event, uint32_t
 // Enumeration
 //--------------------------------------------------------------------+
 
+static bool acm_open(uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len);
+
+static bool open_ep_stream_pair(cdch_interface_t* p_cdc, tusb_desc_endpoint_t const *desc_ep)
+{
+  for(size_t i=0; i<2; i++)
+  {
+    TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType &&
+              TUSB_XFER_BULK     == desc_ep->bmAttributes.xfer);
+
+    TU_ASSERT(tuh_edpt_open(p_cdc->daddr, desc_ep));
+
+    if ( tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN )
+    {
+      tu_edpt_stream_open(&p_cdc->stream.rx, p_cdc->daddr, desc_ep);
+    }else
+    {
+      tu_edpt_stream_open(&p_cdc->stream.tx, p_cdc->daddr, desc_ep);
+    }
+
+    desc_ep = (tusb_desc_endpoint_t const*) tu_desc_next(desc_ep);
+  }
+
+  return true;
+}
+
 bool cdch_open(uint8_t rhport, uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
 {
   (void) rhport;
 
   // Only support ACM subclass
-  // 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);
+  // Note: Protocol 0xFF can be RNDIS device
+  if ( TUSB_CLASS_CDC                           == itf_desc->bInterfaceClass &&
+       CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL == itf_desc->bInterfaceSubClass)
+  {
+    return acm_open(daddr, itf_desc, max_len);
+  }
+  #if CFG_TUH_CDC_FTDI || CFG_TUH_CDC_CP210X
+  else if ( 0xff == itf_desc->bInterfaceClass )
+  {
+    uint16_t vid, pid;
+    TU_VERIFY(tuh_vid_pid_get(daddr, &vid, &pid));
+
+    #if CFG_TUH_CDC_FTDI
+    if (TU_FTDI_VID == vid) {
+      for (size_t i = 0; i < FTDI_PID_COUNT; i++) {
+        if (ftdi_pids[i] == pid) {
+          return ftdi_open(daddr, itf_desc, max_len);
+        }
+      }
+    }
+    #endif
+
+    #if CFG_TUH_CDC_CP210X
+    if (TU_CP210X_VID == vid) {
+      for (size_t i = 0; i < CP210X_PID_COUNT; i++) {
+        if (cp210x_pids[i] == pid) {
+          return cp210x_open(daddr, itf_desc, max_len);
+        }
+      }
+    }
+    #endif
+  }
+  #endif
+
+  return false;
+}
+
+static void set_config_complete(cdch_interface_t * p_cdc, uint8_t idx, uint8_t itf_num) {
+  if (tuh_cdc_mount_cb) tuh_cdc_mount_cb(idx);
+
+  // Prepare for incoming data
+  tu_edpt_stream_read_xfer(&p_cdc->stream.rx);
+
+  // notify usbh that driver enumeration is complete
+  usbh_driver_set_config_complete(p_cdc->daddr, itf_num);
+}
+
+
+bool cdch_set_config(uint8_t daddr, uint8_t itf_num)
+{
+  tusb_control_request_t request;
+  request.wIndex = tu_htole16((uint16_t) itf_num);
+
+  // fake transfer to kick-off process
+  tuh_xfer_t xfer;
+  xfer.daddr  = daddr;
+  xfer.result = XFER_RESULT_SUCCESS;
+  xfer.setup  = &request;
+  xfer.user_data = 0; // initial state
+
+  uint8_t const idx = tuh_cdc_itf_get_index(daddr, itf_num);
+  cdch_interface_t * p_cdc = get_itf(idx);
+  TU_ASSERT(p_cdc && p_cdc->serial_drid < SERIAL_DRIVER_COUNT);
+
+  serial_drivers[p_cdc->serial_drid].process_set_config(&xfer);
+  return true;
+}
+
+//--------------------------------------------------------------------+
+// ACM
+//--------------------------------------------------------------------+
+
+enum {
+  CONFIG_ACM_SET_CONTROL_LINE_STATE = 0,
+  CONFIG_ACM_SET_LINE_CODING,
+  CONFIG_ACM_COMPLETE,
+};
 
+static bool acm_open(uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
+{
   uint8_t const * p_desc_end = ((uint8_t const*) itf_desc) + max_len;
 
-  cdch_interface_t * p_cdc = find_new_itf();
+  cdch_interface_t * p_cdc = make_new_itf(daddr, itf_desc);
   TU_VERIFY(p_cdc);
 
-  p_cdc->daddr              = daddr;
-  p_cdc->bInterfaceNumber   = itf_desc->bInterfaceNumber;
-  p_cdc->bInterfaceSubClass = itf_desc->bInterfaceSubClass;
-  p_cdc->bInterfaceProtocol = itf_desc->bInterfaceProtocol;
-  p_cdc->line_state         = 0;
+  p_cdc->serial_drid = SERIAL_DRIVER_ACM;
 
   //------------- Control Interface -------------//
   uint8_t const * p_desc = tu_desc_next(itf_desc);
@@ -510,37 +754,13 @@ bool cdch_open(uint8_t rhport, uint8_t daddr, tusb_desc_interface_t const *itf_d
     p_desc = tu_desc_next(p_desc);
 
     // data endpoints expected to be in pairs
-    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);
-
-      TU_ASSERT(tuh_edpt_open(daddr, desc_ep));
-
-      if ( tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN )
-      {
-        tu_edpt_stream_open(&p_cdc->stream.rx, daddr, desc_ep);
-      }else
-      {
-        tu_edpt_stream_open(&p_cdc->stream.tx, daddr, desc_ep);
-      }
-
-      p_desc = tu_desc_next(p_desc);
-    }
+    TU_ASSERT(open_ep_stream_pair(p_cdc, (tusb_desc_endpoint_t const *) p_desc));
   }
 
   return true;
 }
 
-enum
-{
-  CONFIG_SET_CONTROL_LINE_STATE,
-  CONFIG_SET_LINE_CODING,
-  CONFIG_COMPLETE
-};
-
-static void process_cdc_config(tuh_xfer_t* xfer)
+static void acm_process_config(tuh_xfer_t* xfer)
 {
   uintptr_t const state = xfer->user_data;
   uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
@@ -550,57 +770,407 @@ static void process_cdc_config(tuh_xfer_t* xfer)
 
   switch(state)
   {
-    case CONFIG_SET_CONTROL_LINE_STATE:
+    case CONFIG_ACM_SET_CONTROL_LINE_STATE:
       #if CFG_TUH_CDC_LINE_CONTROL_ON_ENUM
       if (p_cdc->acm_capability.support_line_request)
       {
-        TU_ASSERT( tuh_cdc_set_control_line_state(idx, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, process_cdc_config, CONFIG_SET_LINE_CODING), );
+        TU_ASSERT(acm_set_control_line_state(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, acm_process_config,
+                                             CONFIG_ACM_SET_LINE_CODING), );
         break;
       }
-      #endif
+          #endif
       TU_ATTR_FALLTHROUGH;
 
-    case CONFIG_SET_LINE_CODING:
-      #ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
+    case CONFIG_ACM_SET_LINE_CODING:
+        #ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
       if (p_cdc->acm_capability.support_line_request)
       {
         cdc_line_coding_t line_coding = CFG_TUH_CDC_LINE_CODING_ON_ENUM;
-        TU_ASSERT( tuh_cdc_set_line_coding(idx, &line_coding, process_cdc_config, CONFIG_COMPLETE), );
+        TU_ASSERT(acm_set_line_coding(p_cdc, &line_coding, acm_process_config, CONFIG_ACM_COMPLETE), );
         break;
       }
-      #endif
+        #endif
       TU_ATTR_FALLTHROUGH;
 
-    case CONFIG_COMPLETE:
-      if (tuh_cdc_mount_cb) tuh_cdc_mount_cb(idx);
-
-      // Prepare for incoming data
-      tu_edpt_stream_read_xfer(&p_cdc->stream.rx);
-
-      // notify usbh that driver enumeration is complete
+    case CONFIG_ACM_COMPLETE:
       // itf_num+1 to account for data interface as well
-      usbh_driver_set_config_complete(xfer->daddr, itf_num+1);
-    break;
+      set_config_complete(p_cdc, idx, itf_num+1);
+      break;
 
     default: break;
   }
 }
 
-bool cdch_set_config(uint8_t daddr, uint8_t itf_num)
+static bool acm_set_control_line_state(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  TU_VERIFY(p_cdc->acm_capability.support_line_request);
+  TU_LOG_DRV("CDC ACM Set Control Line State\r\n");
+
+  tusb_control_request_t const request = {
+    .bmRequestType_bit = {
+      .recipient = TUSB_REQ_RCPT_INTERFACE,
+      .type      = TUSB_REQ_TYPE_CLASS,
+      .direction = TUSB_DIR_OUT
+    },
+    .bRequest = CDC_REQUEST_SET_CONTROL_LINE_STATE,
+    .wValue   = tu_htole16(line_state),
+    .wIndex   = tu_htole16((uint16_t) p_cdc->bInterfaceNumber),
+    .wLength  = 0
+  };
+
+  p_cdc->user_control_cb = complete_cb;
+
+  tuh_xfer_t xfer = {
+    .daddr       = p_cdc->daddr,
+    .ep_addr     = 0,
+    .setup       = &request,
+    .buffer      = NULL,
+    .complete_cb = complete_cb ? cdch_internal_control_complete : NULL, // complete_cb is NULL for sync call
+    .user_data   = user_data
+  };
+
+  TU_ASSERT(tuh_control_xfer(&xfer));
+  return true;
+}
+
+static bool acm_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  TU_LOG_DRV("CDC ACM Set Line Conding\r\n");
+
+  tusb_control_request_t const request = {
+    .bmRequestType_bit = {
+      .recipient = TUSB_REQ_RCPT_INTERFACE,
+      .type      = TUSB_REQ_TYPE_CLASS,
+      .direction = TUSB_DIR_OUT
+    },
+    .bRequest = CDC_REQUEST_SET_LINE_CODING,
+    .wValue   = 0,
+    .wIndex   = tu_htole16(p_cdc->bInterfaceNumber),
+    .wLength  = tu_htole16(sizeof(cdc_line_coding_t))
+  };
+
+  // use usbh enum buf to hold line coding since user line_coding variable does not live long enough
+  uint8_t* enum_buf = usbh_get_enum_buf();
+  memcpy(enum_buf, line_coding, sizeof(cdc_line_coding_t));
+
+  p_cdc->user_control_cb = complete_cb;
+  tuh_xfer_t xfer = {
+    .daddr       = p_cdc->daddr,
+    .ep_addr     = 0,
+    .setup       = &request,
+    .buffer      = enum_buf,
+    .complete_cb = complete_cb ? cdch_internal_control_complete : NULL, // complete_cb is NULL for sync call
+    .user_data   = user_data
+  };
+
+  TU_ASSERT(tuh_control_xfer(&xfer));
+  return true;
+}
+
+static bool acm_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  TU_VERIFY(p_cdc->acm_capability.support_line_request);
+  cdc_line_coding_t line_coding = p_cdc->line_coding;
+  line_coding.bit_rate = baudrate;
+  return acm_set_line_coding(p_cdc, &line_coding, complete_cb, user_data);
+}
+
+//--------------------------------------------------------------------+
+// FTDI
+//--------------------------------------------------------------------+
+#if CFG_TUH_CDC_FTDI
+
+enum {
+  CONFIG_FTDI_RESET = 0,
+  CONFIG_FTDI_MODEM_CTRL,
+  CONFIG_FTDI_SET_BAUDRATE,
+  CONFIG_FTDI_SET_DATA,
+  CONFIG_FTDI_COMPLETE
+};
+
+static bool ftdi_open(uint8_t daddr, const tusb_desc_interface_t *itf_desc, uint16_t max_len) {
+  // FTDI Interface includes 1 vendor interface + 2 bulk endpoints
+  TU_VERIFY(itf_desc->bInterfaceSubClass == 0xff && itf_desc->bInterfaceProtocol == 0xff && itf_desc->bNumEndpoints == 2);
+  TU_VERIFY(sizeof(tusb_desc_interface_t) + 2*sizeof(tusb_desc_endpoint_t) <= max_len);
+
+  cdch_interface_t * p_cdc = make_new_itf(daddr, itf_desc);
+  TU_VERIFY(p_cdc);
+
+  TU_LOG_DRV("FTDI opened\r\n");
+
+  p_cdc->serial_drid = SERIAL_DRIVER_FTDI;
+
+  // endpoint pair
+  tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) tu_desc_next(itf_desc);
+
+  // data endpoints expected to be in pairs
+  return open_ep_stream_pair(p_cdc, desc_ep);
+}
+
+// set request without data
+static bool ftdi_sio_set_request(cdch_interface_t* p_cdc, uint8_t command, uint16_t value, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  tusb_control_request_t const request = {
+    .bmRequestType_bit = {
+      .recipient = TUSB_REQ_RCPT_DEVICE,
+      .type      = TUSB_REQ_TYPE_VENDOR,
+      .direction = TUSB_DIR_OUT
+    },
+    .bRequest = command,
+    .wValue   = tu_htole16(value),
+    .wIndex   = 0,
+    .wLength  = 0
+  };
+
+  tuh_xfer_t xfer = {
+    .daddr       = p_cdc->daddr,
+    .ep_addr     = 0,
+    .setup       = &request,
+    .buffer      = NULL,
+    .complete_cb = complete_cb,
+    .user_data   = user_data
+  };
+
+  return tuh_control_xfer(&xfer);
+}
+
+static bool ftdi_sio_reset(cdch_interface_t* p_cdc, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
 {
-  // fake transfer to kick-off process
-  tusb_control_request_t request;
-  request.wIndex = tu_htole16((uint16_t) itf_num);
+  return ftdi_sio_set_request(p_cdc, FTDI_SIO_RESET, FTDI_SIO_RESET_SIO, complete_cb, user_data);
+}
 
-  tuh_xfer_t xfer;
-  xfer.daddr     = daddr;
-  xfer.result    = XFER_RESULT_SUCCESS;
-  xfer.setup     = &request;
-  xfer.user_data = CONFIG_SET_CONTROL_LINE_STATE;
+static bool ftdi_sio_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
+{
+  TU_LOG_DRV("CDC FTDI Set Control Line State\r\n");
+  p_cdc->user_control_cb = complete_cb;
+  TU_ASSERT(ftdi_sio_set_request(p_cdc, FTDI_SIO_MODEM_CTRL, 0x0300 | line_state,
+                                 complete_cb ? cdch_internal_control_complete : NULL, user_data));
+  return true;
+}
 
-  process_cdc_config(&xfer);
+static uint32_t ftdi_232bm_baud_base_to_divisor(uint32_t baud, uint32_t base)
+{
+  const uint8_t divfrac[8] = { 0, 3, 2, 4, 1, 5, 6, 7 };
+  uint32_t divisor;
+
+  /* divisor shifted 3 bits to the left */
+  uint32_t divisor3 = base / (2 * baud);
+  divisor = (divisor3 >> 3);
+  divisor |= (uint32_t) divfrac[divisor3 & 0x7] << 14;
+
+  /* Deal with special cases for highest baud rates. */
+  if (divisor == 1) { /* 1.0 */
+    divisor = 0;
+  }
+  else if (divisor == 0x4001) { /* 1.5 */
+    divisor = 1;
+  }
+
+  return divisor;
+}
+
+static uint32_t ftdi_232bm_baud_to_divisor(uint32_t baud)
+{
+  return ftdi_232bm_baud_base_to_divisor(baud, 48000000u);
+}
+
+static bool ftdi_sio_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
+{
+  uint16_t const divisor = (uint16_t) ftdi_232bm_baud_to_divisor(baudrate);
+  TU_LOG_DRV("CDC FTDI Set BaudRate = %lu, divisor = 0x%04x\n", baudrate, divisor);
+
+  p_cdc->user_control_cb = complete_cb;
+  _ftdi_requested_baud = baudrate;
+  TU_ASSERT(ftdi_sio_set_request(p_cdc, FTDI_SIO_SET_BAUD_RATE, divisor,
+                                 complete_cb ? cdch_internal_control_complete : NULL, user_data));
 
   return true;
 }
 
+static void ftdi_process_config(tuh_xfer_t* xfer) {
+  uintptr_t const state = xfer->user_data;
+  uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
+  uint8_t const idx = tuh_cdc_itf_get_index(xfer->daddr, itf_num);
+  cdch_interface_t * p_cdc = get_itf(idx);
+  TU_ASSERT(p_cdc, );
+
+  switch(state) {
+    // Note may need to read FTDI eeprom
+    case CONFIG_FTDI_RESET:
+      TU_ASSERT(ftdi_sio_reset(p_cdc, ftdi_process_config, CONFIG_FTDI_MODEM_CTRL),);
+      break;
+
+    case CONFIG_FTDI_MODEM_CTRL:
+      #if CFG_TUH_CDC_LINE_CONTROL_ON_ENUM
+      TU_ASSERT(
+        ftdi_sio_set_modem_ctrl(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, ftdi_process_config, CONFIG_FTDI_SET_BAUDRATE),);
+      break;
+      #else
+      TU_ATTR_FALLTHROUGH;
+      #endif
+
+    case CONFIG_FTDI_SET_BAUDRATE: {
+      #ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
+      cdc_line_coding_t line_coding = CFG_TUH_CDC_LINE_CODING_ON_ENUM;
+      TU_ASSERT(ftdi_sio_set_baudrate(p_cdc, line_coding.bit_rate, ftdi_process_config, CONFIG_FTDI_SET_DATA),);
+      break;
+      #else
+      TU_ATTR_FALLTHROUGH;
+      #endif
+    }
+
+    case CONFIG_FTDI_SET_DATA: {
+      #if 0 // TODO set data format
+      #ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
+      cdc_line_coding_t line_coding = CFG_TUH_CDC_LINE_CODING_ON_ENUM;
+      TU_ASSERT(ftdi_sio_set_data(p_cdc, process_ftdi_config, CONFIG_FTDI_COMPLETE),);
+      break;
+      #endif
+      #endif
+
+      TU_ATTR_FALLTHROUGH;
+    }
+
+    case CONFIG_FTDI_COMPLETE:
+      set_config_complete(p_cdc, idx, itf_num);
+      break;
+
+    default:
+      break;
+  }
+}
+
+#endif
+
+//--------------------------------------------------------------------+
+// CP210x
+//--------------------------------------------------------------------+
+
+#if CFG_TUH_CDC_CP210X
+
+enum {
+  CONFIG_CP210X_IFC_ENABLE = 0,
+  CONFIG_CP210X_SET_BAUDRATE,
+  CONFIG_CP210X_SET_LINE_CTL,
+  CONFIG_CP210X_SET_DTR_RTS,
+  CONFIG_CP210X_COMPLETE
+};
+
+static bool cp210x_open(uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len) {
+  // CP210x Interface includes 1 vendor interface + 2 bulk endpoints
+  TU_VERIFY(itf_desc->bInterfaceSubClass == 0 && itf_desc->bInterfaceProtocol == 0 && itf_desc->bNumEndpoints == 2);
+  TU_VERIFY(sizeof(tusb_desc_interface_t) + 2*sizeof(tusb_desc_endpoint_t) <= max_len);
+
+  cdch_interface_t * p_cdc = make_new_itf(daddr, itf_desc);
+  TU_VERIFY(p_cdc);
+
+  TU_LOG_DRV("CP210x opened\r\n");
+  p_cdc->serial_drid = SERIAL_DRIVER_CP210X;
+
+  // endpoint pair
+  tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) tu_desc_next(itf_desc);
+
+  // data endpoints expected to be in pairs
+  return open_ep_stream_pair(p_cdc, desc_ep);
+}
+
+static bool cp210x_set_request(cdch_interface_t* p_cdc, uint8_t command, uint16_t value, uint8_t* buffer, uint16_t length, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  tusb_control_request_t const request = {
+    .bmRequestType_bit = {
+      .recipient = TUSB_REQ_RCPT_INTERFACE,
+      .type      = TUSB_REQ_TYPE_VENDOR,
+      .direction = TUSB_DIR_OUT
+    },
+    .bRequest = command,
+    .wValue   = tu_htole16(value),
+    .wIndex   = p_cdc->bInterfaceNumber,
+    .wLength  = tu_htole16(length)
+  };
+
+  // use usbh enum buf since application variable does not live long enough
+  uint8_t* enum_buf = NULL;
+
+  if (buffer && length > 0) {
+    enum_buf = usbh_get_enum_buf();
+    tu_memcpy_s(enum_buf, CFG_TUH_ENUMERATION_BUFSIZE, buffer, length);
+  }
+
+  tuh_xfer_t xfer = {
+    .daddr       = p_cdc->daddr,
+    .ep_addr     = 0,
+    .setup       = &request,
+    .buffer      = enum_buf,
+    .complete_cb = complete_cb,
+    .user_data   = user_data
+  };
+
+  return tuh_control_xfer(&xfer);
+}
+
+static bool cp210x_ifc_enable(cdch_interface_t* p_cdc, uint16_t enabled, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  return cp210x_set_request(p_cdc, CP210X_IFC_ENABLE, enabled, NULL, 0, complete_cb, user_data);
+}
+
+static bool cp210x_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  TU_LOG_DRV("CDC CP210x Set BaudRate = %lu\n", baudrate);
+  uint32_t baud_le = tu_htole32(baudrate);
+  p_cdc->user_control_cb = complete_cb;
+  return cp210x_set_request(p_cdc, CP210X_SET_BAUDRATE, 0, (uint8_t *) &baud_le, 4,
+                            complete_cb ? cdch_internal_control_complete : NULL, user_data);
+}
+
+static bool cp210x_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
+{
+  TU_LOG_DRV("CDC CP210x Set Control Line State\r\n");
+  p_cdc->user_control_cb = complete_cb;
+  return cp210x_set_request(p_cdc, CP210X_SET_MHS, 0x0300 | line_state, NULL, 0,
+                            complete_cb ? cdch_internal_control_complete : NULL, user_data);
+}
+
+static void cp210x_process_config(tuh_xfer_t* xfer) {
+  uintptr_t const state   = xfer->user_data;
+  uint8_t const   itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
+  uint8_t const   idx     = tuh_cdc_itf_get_index(xfer->daddr, itf_num);
+  cdch_interface_t *p_cdc = get_itf(idx);
+  TU_ASSERT(p_cdc,);
+
+  switch (state) {
+    case CONFIG_CP210X_IFC_ENABLE:
+      TU_ASSERT(cp210x_ifc_enable(p_cdc, 1, cp210x_process_config, CONFIG_CP210X_SET_BAUDRATE),);
+      break;
+
+    case CONFIG_CP210X_SET_BAUDRATE: {
+      #ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
+      cdc_line_coding_t line_coding = CFG_TUH_CDC_LINE_CODING_ON_ENUM;
+      TU_ASSERT(cp210x_set_baudrate(p_cdc, line_coding.bit_rate, cp210x_process_config, CONFIG_CP210X_SET_LINE_CTL),);
+      break;
+      #else
+      TU_ATTR_FALLTHROUGH;
+      #endif
+    }
+
+    case CONFIG_CP210X_SET_LINE_CTL: {
+      #if defined(CFG_TUH_CDC_LINE_CODING_ON_ENUM) && 0 // skip for now
+      cdc_line_coding_t line_coding = CFG_TUH_CDC_LINE_CODING_ON_ENUM;
+      break;
+      #else
+      TU_ATTR_FALLTHROUGH;
+      #endif
+    }
+
+    case CONFIG_CP210X_SET_DTR_RTS:
+      #if CFG_TUH_CDC_LINE_CONTROL_ON_ENUM
+      TU_ASSERT(
+        cp210x_set_modem_ctrl(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, cp210x_process_config, CONFIG_CP210X_COMPLETE),);
+      break;
+      #else
+      TU_ATTR_FALLTHROUGH;
+      #endif
+
+    case CONFIG_CP210X_COMPLETE:
+      set_config_complete(p_cdc, idx, itf_num);
+      break;
+
+    default: break;
+  }
+}
+
+#endif
+
 #endif
diff --git a/src/class/cdc/cdc_host.h b/src/class/cdc/cdc_host.h
index a1e78f158..19552f1ee 100644
--- a/src/class/cdc/cdc_host.h
+++ b/src/class/cdc/cdc_host.h
@@ -134,28 +134,39 @@ bool tuh_cdc_read_clear (uint8_t idx);
 
 //--------------------------------------------------------------------+
 // Control Endpoint (Request) API
-// Each Function will make a USB transfer request to/from device
+// Each Function will make a USB control transfer request to/from device
+// - If complete_cb is provided, the function will return immediately and invoke
+// the callback when request is complete.
+// - If complete_cb is NULL, the function will block until request is complete.
+//   - In this case, user_data should be pointed to xfer_result_t to hold the transfer result.
+//   - The function will return true if transfer is successful, false otherwise.
 //--------------------------------------------------------------------+
 
 // Request to Set Control Line State: DTR (bit 0), RTS (bit 1)
 bool tuh_cdc_set_control_line_state(uint8_t idx, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
 
-// Request to Set Line Coding
+// Request to set baudrate
+bool tuh_cdc_set_baudrate(uint8_t idx, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+
+// Request to Set Line Coding (ACM only)
+// Should only use if you don't work with serial devices such as FTDI/CP210x
 bool tuh_cdc_set_line_coding(uint8_t idx, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
 
-// Request to Get Line Coding
+// Request to Get Line Coding (ACM only)
 // Should only use if tuh_cdc_set_line_coding() / tuh_cdc_get_line_coding() never got invoked and
 // CFG_TUH_CDC_LINE_CODING_ON_ENUM is not defined
 // bool tuh_cdc_get_line_coding(uint8_t idx, cdc_line_coding_t* coding);
 
 // Connect by set both DTR, RTS
-static inline bool tuh_cdc_connect(uint8_t idx, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
+TU_ATTR_ALWAYS_INLINE static inline
+bool tuh_cdc_connect(uint8_t idx, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
 {
   return tuh_cdc_set_control_line_state(idx, CDC_CONTROL_LINE_STATE_DTR | CDC_CONTROL_LINE_STATE_RTS, complete_cb, user_data);
 }
 
 // Disconnect by clear both DTR, RTS
-static inline bool tuh_cdc_disconnect(uint8_t idx, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
+TU_ATTR_ALWAYS_INLINE static inline
+bool tuh_cdc_disconnect(uint8_t idx, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
 {
   return tuh_cdc_set_control_line_state(idx, 0x00, complete_cb, user_data);
 }
diff --git a/src/class/cdc/serial/cp210x.h b/src/class/cdc/serial/cp210x.h
new file mode 100644
index 000000000..b01417092
--- /dev/null
+++ b/src/class/cdc/serial/cp210x.h
@@ -0,0 +1,64 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2023 Ha Thach ([email protected]) for Adafruit Industries
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef TUSB_CP210X_H
+#define TUSB_CP210X_H
+
+// Protocol details can be found at AN571: CP210x Virtual COM Port Interface
+// https://www.silabs.com/documents/public/application-notes/AN571.pdf
+
+#define TU_CP210X_VID 0x10C4
+#define TU_CP210X_PID_LIST \
+  0xEA60, 0xEA70
+
+/* Config request codes */
+#define CP210X_IFC_ENABLE      0x00
+#define CP210X_SET_BAUDDIV     0x01
+#define CP210X_GET_BAUDDIV     0x02
+#define CP210X_SET_LINE_CTL    0x03 // Set parity, data bits, stop bits
+#define CP210X_GET_LINE_CTL    0x04
+#define CP210X_SET_BREAK       0x05
+#define CP210X_IMM_CHAR        0x06
+#define CP210X_SET_MHS         0x07 // Set DTR, RTS
+#define CP210X_GET_MDMSTS      0x08 // Get modem status (DTR, RTS, CTS, DSR, RI, DCD)
+#define CP210X_SET_XON         0x09
+#define CP210X_SET_XOFF        0x0A
+#define CP210X_SET_EVENTMASK   0x0B
+#define CP210X_GET_EVENTMASK   0x0C
+#define CP210X_SET_CHAR        0x0D
+#define CP210X_GET_CHARS       0x0E
+#define CP210X_GET_PROPS       0x0F
+#define CP210X_GET_COMM_STATUS 0x10
+#define CP210X_RESET           0x11
+#define CP210X_PURGE           0x12
+#define CP210X_SET_FLOW        0x13
+#define CP210X_GET_FLOW        0x14
+#define CP210X_EMBED_EVENTS    0x15
+#define CP210X_GET_EVENTSTATE  0x16
+#define CP210X_SET_CHARS       0x19
+#define CP210X_GET_BAUDRATE    0x1D
+#define CP210X_SET_BAUDRATE    0x1E
+#define CP210X_VENDOR_SPECIFIC 0xFF // GPIO, Recipient must be Device
+
+#endif //TUSB_CP210X_H
diff --git a/src/class/cdc/serial/ftdi_sio.h b/src/class/cdc/serial/ftdi_sio.h
new file mode 100644
index 000000000..6916e4031
--- /dev/null
+++ b/src/class/cdc/serial/ftdi_sio.h
@@ -0,0 +1,249 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2023 Ha Thach ([email protected]) for Adafruit Industries
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef TUSB_FTDI_SIO_H
+#define TUSB_FTDI_SIO_H
+
+// VID/PID for matching FTDI devices
+#define TU_FTDI_VID 0x0403
+#define TU_FTDI_PID_LIST \
+  0x6001, 0x6006, 0x6010, 0x6011, 0x6014, 0x6015, 0x8372, 0xFBFA, \
+  0xcd18
+
+// Commands
+#define FTDI_SIO_RESET             	0    /* Reset the port */
+#define FTDI_SIO_MODEM_CTRL        	1    /* Set the modem control register */
+#define FTDI_SIO_SET_FLOW_CTRL     	2    /* Set flow control register */
+#define FTDI_SIO_SET_BAUD_RATE     	3    /* Set baud rate */
+#define FTDI_SIO_SET_DATA          	4    /* Set the data characteristics of the port */
+#define FTDI_SIO_GET_MODEM_STATUS  	5    /* Retrieve current value of modem status register */
+#define FTDI_SIO_SET_EVENT_CHAR    	6    /* Set the event character */
+#define FTDI_SIO_SET_ERROR_CHAR    	7    /* Set the error character */
+#define FTDI_SIO_SET_LATENCY_TIMER 	9    /* Set the latency timer */
+#define FTDI_SIO_GET_LATENCY_TIMER 	0x0a /* Get the latency timer */
+#define FTDI_SIO_SET_BITMODE       	0x0b /* Set bitbang mode */
+#define FTDI_SIO_READ_PINS         	0x0c /* Read immediate value of pins */
+#define FTDI_SIO_READ_EEPROM       	0x90 /* Read EEPROM */
+
+/* FTDI_SIO_RESET */
+#define FTDI_SIO_RESET_SIO 0
+#define FTDI_SIO_RESET_PURGE_RX 1
+#define FTDI_SIO_RESET_PURGE_TX 2
+
+/*
+ * BmRequestType:  0100 0000B
+ * bRequest:       FTDI_SIO_RESET
+ * wValue:         Control Value
+ *                   0 = Reset SIO
+ *                   1 = Purge RX buffer
+ *                   2 = Purge TX buffer
+ * wIndex:         Port
+ * wLength:        0
+ * Data:           None
+ *
+ * The Reset SIO command has this effect:
+ *
+ *    Sets flow control set to 'none'
+ *    Event char = $0D
+ *    Event trigger = disabled
+ *    Purge RX buffer
+ *    Purge TX buffer
+ *    Clear DTR
+ *    Clear RTS
+ *    baud and data format not reset
+ *
+ * The Purge RX and TX buffer commands affect nothing except the buffers
+ *
+   */
+
+/* FTDI_SIO_MODEM_CTRL */
+/*
+ * BmRequestType:   0100 0000B
+ * bRequest:        FTDI_SIO_MODEM_CTRL
+ * wValue:          ControlValue (see below)
+ * wIndex:          Port
+ * wLength:         0
+ * Data:            None
+ *
+ * NOTE: If the device is in RTS/CTS flow control, the RTS set by this
+ * command will be IGNORED without an error being returned
+ * Also - you can not set DTR and RTS with one control message
+ */
+
+#define FTDI_SIO_SET_DTR_MASK 0x1
+#define FTDI_SIO_SET_DTR_HIGH ((FTDI_SIO_SET_DTR_MASK << 8) | 1)
+#define FTDI_SIO_SET_DTR_LOW  ((FTDI_SIO_SET_DTR_MASK << 8) | 0)
+#define FTDI_SIO_SET_RTS_MASK 0x2
+#define FTDI_SIO_SET_RTS_HIGH ((FTDI_SIO_SET_RTS_MASK << 8) | 2)
+#define FTDI_SIO_SET_RTS_LOW  ((FTDI_SIO_SET_RTS_MASK << 8) | 0)
+
+/*
+ * ControlValue
+ * B0    DTR state
+ *          0 = reset
+ *          1 = set
+ * B1    RTS state
+ *          0 = reset
+ *          1 = set
+ * B2..7 Reserved
+ * B8    DTR state enable
+ *          0 = ignore
+ *          1 = use DTR state
+ * B9    RTS state enable
+ *          0 = ignore
+ *          1 = use RTS state
+ * B10..15 Reserved
+ */
+
+/* FTDI_SIO_SET_FLOW_CTRL */
+#define FTDI_SIO_DISABLE_FLOW_CTRL 0x0
+#define FTDI_SIO_RTS_CTS_HS (0x1 << 8)
+#define FTDI_SIO_DTR_DSR_HS (0x2 << 8)
+#define FTDI_SIO_XON_XOFF_HS (0x4 << 8)
+
+/*
+ *   BmRequestType:  0100 0000b
+ *   bRequest:       FTDI_SIO_SET_FLOW_CTRL
+ *   wValue:         Xoff/Xon
+ *   wIndex:         Protocol/Port - hIndex is protocol / lIndex is port
+ *   wLength:        0
+ *   Data:           None
+ *
+ * hIndex protocol is:
+ *   B0 Output handshaking using RTS/CTS
+ *       0 = disabled
+ *       1 = enabled
+ *   B1 Output handshaking using DTR/DSR
+ *       0 = disabled
+ *       1 = enabled
+ *   B2 Xon/Xoff handshaking
+ *       0 = disabled
+ *       1 = enabled
+ *
+ * A value of zero in the hIndex field disables handshaking
+ *
+ * If Xon/Xoff handshaking is specified, the hValue field should contain the
+ * XOFF character and the lValue field contains the XON character.
+ */
+
+/* FTDI_SIO_SET_BAUD_RATE */
+/*
+ * BmRequestType:  0100 0000B
+ * bRequest:       FTDI_SIO_SET_BAUDRATE
+ * wValue:         BaudDivisor value - see below
+ * wIndex:         Port
+ * wLength:        0
+ * Data:           None
+ * The BaudDivisor values are calculated as follows (too complicated):
+ */
+
+/* FTDI_SIO_SET_DATA */
+#define FTDI_SIO_SET_DATA_PARITY_NONE	(0x0 << 8)
+#define FTDI_SIO_SET_DATA_PARITY_ODD	(0x1 << 8)
+#define FTDI_SIO_SET_DATA_PARITY_EVEN	(0x2 << 8)
+#define FTDI_SIO_SET_DATA_PARITY_MARK	(0x3 << 8)
+#define FTDI_SIO_SET_DATA_PARITY_SPACE	(0x4 << 8)
+#define FTDI_SIO_SET_DATA_STOP_BITS_1	(0x0 << 11)
+#define FTDI_SIO_SET_DATA_STOP_BITS_15	(0x1 << 11)
+#define FTDI_SIO_SET_DATA_STOP_BITS_2	(0x2 << 11)
+#define FTDI_SIO_SET_BREAK		(0x1 << 14)
+
+/*
+ * BmRequestType:  0100 0000B
+ * bRequest:       FTDI_SIO_SET_DATA
+ * wValue:         Data characteristics (see below)
+ * wIndex:         Port
+ * wLength:        0
+ * Data:           No
+ *
+ * Data characteristics
+ *
+ *   B0..7   Number of data bits
+ *   B8..10  Parity
+ *           0 = None
+ *           1 = Odd
+ *           2 = Even
+ *           3 = Mark
+ *           4 = Space
+ *   B11..13 Stop Bits
+ *           0 = 1
+ *           1 = 1.5
+ *           2 = 2
+ *   B14
+ *           1 = TX ON (break)
+ *           0 = TX OFF (normal state)
+ *   B15 Reserved
+ *
+ */
+
+/*
+* DATA FORMAT
+*
+* IN Endpoint
+*
+* The device reserves the first two bytes of data on this endpoint to contain
+* the current values of the modem and line status registers. In the absence of
+* data, the device generates a message consisting of these two status bytes
+  * every 40 ms
+  *
+  * Byte 0: Modem Status
+*
+* Offset	Description
+* B0	Reserved - must be 1
+* B1	Reserved - must be 0
+* B2	Reserved - must be 0
+* B3	Reserved - must be 0
+* B4	Clear to Send (CTS)
+* B5	Data Set Ready (DSR)
+* B6	Ring Indicator (RI)
+* B7	Receive Line Signal Detect (RLSD)
+*
+* Byte 1: Line Status
+*
+* Offset	Description
+* B0	Data Ready (DR)
+* B1	Overrun Error (OE)
+* B2	Parity Error (PE)
+* B3	Framing Error (FE)
+* B4	Break Interrupt (BI)
+* B5	Transmitter Holding Register (THRE)
+* B6	Transmitter Empty (TEMT)
+* B7	Error in RCVR FIFO
+*
+*/
+#define FTDI_RS0_CTS	(1 << 4)
+#define FTDI_RS0_DSR	(1 << 5)
+#define FTDI_RS0_RI	(1 << 6)
+#define FTDI_RS0_RLSD	(1 << 7)
+
+#define FTDI_RS_DR	1
+#define FTDI_RS_OE	(1<<1)
+#define FTDI_RS_PE	(1<<2)
+#define FTDI_RS_FE	(1<<3)
+#define FTDI_RS_BI	(1<<4)
+#define FTDI_RS_THRE	(1<<5)
+#define FTDI_RS_TEMT	(1<<6)
+#define FTDI_RS_FIFO	(1<<7)
+
+#endif //TUSB_FTDI_SIO_H
diff --git a/src/class/hid/hid_host.c b/src/class/hid/hid_host.c
index d95d3ef35..6abe298e5 100644
--- a/src/class/hid/hid_host.c
+++ b/src/class/hid/hid_host.c
@@ -33,6 +33,10 @@
 
 #include "hid_host.h"
 
+// Debug level, TUSB_CFG_DEBUG must be at least this level for debug message
+#define HIDH_DEBUG   2
+#define TU_LOG_DRV(...)   TU_LOG(HIDH_DEBUG, __VA_ARGS__)
+
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
@@ -68,7 +72,7 @@ tu_static hidh_interface_t _hidh_itf[CFG_TUH_HID];
 TU_ATTR_ALWAYS_INLINE static inline
 hidh_interface_t* get_hid_itf(uint8_t daddr, uint8_t idx)
 {
-  TU_ASSERT(daddr && idx < CFG_TUH_HID, NULL);
+  TU_ASSERT(daddr > 0 && idx < CFG_TUH_HID, NULL);
   hidh_interface_t* p_hid = &_hidh_itf[idx];
   return (p_hid->daddr == daddr) ? p_hid : NULL;
 }
@@ -207,7 +211,7 @@ static void set_protocol_complete(tuh_xfer_t* xfer)
 
 static bool _hidh_set_protocol(uint8_t daddr, uint8_t itf_num, uint8_t protocol, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
 {
-  TU_LOG2("HID Set Protocol = %d\r\n", protocol);
+  TU_LOG_DRV("HID Set Protocol = %d\r\n", protocol);
 
   tusb_control_request_t const request =
   {
@@ -246,7 +250,7 @@ bool tuh_hid_set_protocol(uint8_t daddr, uint8_t idx, uint8_t protocol)
 
 static void set_report_complete(tuh_xfer_t* xfer)
 {
-  TU_LOG2("HID Set Report complete\r\n");
+  TU_LOG_DRV("HID Set Report complete\r\n");
 
   if (tuh_hid_set_report_complete_cb)
   {
@@ -266,7 +270,7 @@ bool tuh_hid_set_report(uint8_t daddr, uint8_t idx, uint8_t report_id, uint8_t r
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
   TU_VERIFY(p_hid);
 
-  TU_LOG2("HID Set Report: id = %u, type = %u, len = %u\r\n", report_id, report_type, len);
+  TU_LOG_DRV("HID Set Report: id = %u, type = %u, len = %u\r\n", report_id, report_type, len);
 
   tusb_control_request_t const request =
   {
@@ -298,7 +302,7 @@ bool tuh_hid_set_report(uint8_t daddr, uint8_t idx, uint8_t report_id, uint8_t r
 static bool _hidh_set_idle(uint8_t daddr, uint8_t itf_num, uint16_t idle_rate, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
 {
   // SET IDLE request, device can stall if not support this request
-  TU_LOG2("HID Set Idle \r\n");
+  TU_LOG_DRV("HID Set Idle \r\n");
 
   tusb_control_request_t const request =
   {
@@ -367,7 +371,7 @@ bool tuh_hid_send_ready(uint8_t dev_addr, uint8_t idx)
 
 bool tuh_hid_send_report(uint8_t daddr, uint8_t idx, uint8_t report_id, const void* report, uint16_t len)
 {
-  TU_LOG2("HID Send Report %d\r\n", report_id);
+  TU_LOG_DRV("HID Send Report %d\r\n", report_id);
 
   hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
   TU_VERIFY(p_hid);
@@ -430,7 +434,7 @@ bool hidh_xfer_cb(uint8_t daddr, uint8_t ep_addr, xfer_result_t result, uint32_t
 
   if ( dir == TUSB_DIR_IN )
   {
-    TU_LOG2("  Get Report callback (%u, %u)\r\n", daddr, idx);
+    TU_LOG_DRV("  Get Report callback (%u, %u)\r\n", daddr, idx);
     TU_LOG3_MEM(p_hid->epin_buf, xferred_bytes, 2);
     tuh_hid_report_received_cb(daddr, idx, p_hid->epin_buf, (uint16_t) xferred_bytes);
   }else
@@ -448,8 +452,9 @@ void hidh_close(uint8_t daddr)
     hidh_interface_t* p_hid = &_hidh_itf[i];
     if (p_hid->daddr == daddr)
     {
-       if(tuh_hid_umount_cb) tuh_hid_umount_cb(daddr, i);
-       p_hid->daddr = 0;
+      TU_LOG_DRV("  HIDh close addr = %u index = %u\r\n", daddr, i);
+      if(tuh_hid_umount_cb) tuh_hid_umount_cb(daddr, i);
+      p_hid->daddr = 0;
     }
   }
 }
@@ -465,7 +470,7 @@ bool hidh_open(uint8_t rhport, uint8_t daddr, tusb_desc_interface_t const *desc_
 
   TU_VERIFY(TUSB_CLASS_HID == desc_itf->bInterfaceClass);
 
-  TU_LOG2("[%u] HID opening Interface %u\r\n", daddr, desc_itf->bInterfaceNumber);
+  TU_LOG_DRV("[%u] HID opening Interface %u\r\n", daddr, desc_itf->bInterfaceNumber);
 
   // len = interface + hid + n*endpoints
   uint16_t const drv_len = (uint16_t) (sizeof(tusb_desc_interface_t) + sizeof(tusb_hid_descriptor_hid_t) +
@@ -592,7 +597,7 @@ static void process_set_config(tuh_xfer_t* xfer)
       // using usbh enumeration buffer since report descriptor can be very long
       if( p_hid->report_desc_len > CFG_TUH_ENUMERATION_BUFSIZE )
       {
-        TU_LOG2("HID Skip Report Descriptor since it is too large %u bytes\r\n", p_hid->report_desc_len);
+        TU_LOG_DRV("HID Skip Report Descriptor since it is too large %u bytes\r\n", p_hid->report_desc_len);
 
         // Driver is mounted without report descriptor
         config_driver_mount_complete(daddr, idx, NULL, 0);
@@ -763,7 +768,7 @@ uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* report_info_arr,
   for ( uint8_t i = 0; i < report_num; i++ )
   {
     info = report_info_arr+i;
-    TU_LOG2("%u: id = %u, usage_page = %u, usage = %u\r\n", i, info->report_id, info->usage_page, info->usage);
+    TU_LOG_DRV("%u: id = %u, usage_page = %u, usage = %u\r\n", i, info->report_id, info->usage_page, info->usage);
   }
 
   return report_num;
diff --git a/src/class/midi/midi.h b/src/class/midi/midi.h
index 8b78c6555..8ddcdfda2 100644
--- a/src/class/midi/midi.h
+++ b/src/class/midi/midi.h
@@ -71,8 +71,8 @@ typedef enum
   MIDI_CIN_SYSEX_END_1BYTE   = 5, // SysEx ends with 1 data, or 1 byte system common message
   MIDI_CIN_SYSEX_END_2BYTE   = 6, // SysEx ends with 2 data
   MIDI_CIN_SYSEX_END_3BYTE   = 7, // SysEx ends with 3 data
-  MIDI_CIN_NOTE_ON           = 8,
-  MIDI_CIN_NOTE_OFF          = 9,
+  MIDI_CIN_NOTE_OFF          = 8,
+  MIDI_CIN_NOTE_ON           = 9,
   MIDI_CIN_POLY_KEYPRESS     = 10,
   MIDI_CIN_CONTROL_CHANGE    = 11,
   MIDI_CIN_PROGRAM_CHANGE    = 12,
diff --git a/src/class/msc/msc_host.c b/src/class/msc/msc_host.c
index 1b48813ec..138443de4 100644
--- a/src/class/msc/msc_host.c
+++ b/src/class/msc/msc_host.c
@@ -35,7 +35,6 @@
 
 // Debug level, TUSB_CFG_DEBUG must be at least this level for debug message
 #define MSCH_DEBUG   2
-
 #define TU_LOG_MSCH(...)   TU_LOG(MSCH_DEBUG, __VA_ARGS__)
 
 //--------------------------------------------------------------------+
@@ -82,6 +81,7 @@ CFG_TUH_MEM_SECTION static msch_interface_t _msch_itf[CFG_TUH_DEVICE_MAX];
 CFG_TUH_MEM_SECTION CFG_TUH_MEM_ALIGN
 static uint8_t _msch_buffer[sizeof(scsi_inquiry_resp_t)];
 
+// FIXME potential nul reference
 TU_ATTR_ALWAYS_INLINE
 static inline msch_interface_t* get_itf(uint8_t dev_addr)
 {
@@ -305,11 +305,15 @@ void msch_init(void)
 void msch_close(uint8_t dev_addr)
 {
   TU_VERIFY(dev_addr <= CFG_TUH_DEVICE_MAX, );
-
   msch_interface_t* p_msc = get_itf(dev_addr);
+  TU_VERIFY(p_msc->configured, );
+
+  TU_LOG_MSCH("  MSCh close addr = %d\r\n", dev_addr);
 
   // invoke Application Callback
-  if (p_msc->mounted && tuh_msc_umount_cb) tuh_msc_umount_cb(dev_addr);
+  if (p_msc->mounted) {
+    if(tuh_msc_umount_cb) tuh_msc_umount_cb(dev_addr);
+  }
 
   tu_memclr(p_msc, sizeof(msch_interface_t));
 }
diff --git a/src/common/tusb_compiler.h b/src/common/tusb_compiler.h
index 713bbb8d4..5ab56e145 100644
--- a/src/common/tusb_compiler.h
+++ b/src/common/tusb_compiler.h
@@ -138,10 +138,14 @@
   #define TU_ATTR_BIT_FIELD_ORDER_BEGIN
   #define TU_ATTR_BIT_FIELD_ORDER_END
 
-  #if __has_attribute(__fallthrough__)
-    #define TU_ATTR_FALLTHROUGH         __attribute__((fallthrough))
-  #else
+  #if __GNUC__ < 5
     #define TU_ATTR_FALLTHROUGH         do {} while (0)  /* fallthrough */
+  #else
+    #if __has_attribute(__fallthrough__)
+      #define TU_ATTR_FALLTHROUGH         __attribute__((fallthrough))
+    #else
+      #define TU_ATTR_FALLTHROUGH         do {} while (0)  /* fallthrough */
+    #endif
   #endif
 
   // Endian conversion use well-known host to network (big endian) naming
@@ -151,8 +155,17 @@
     #define TU_BYTE_ORDER TU_BIG_ENDIAN
   #endif
 
-  #define TU_BSWAP16(u16) (__builtin_bswap16(u16))
-  #define TU_BSWAP32(u32) (__builtin_bswap32(u32))
+  // Unfortunately XC16 doesn't provide builtins for 32bit endian conversion
+  #if defined(__XC16)
+    #define TU_BSWAP16(u16) (__builtin_swap(u16))
+    #define TU_BSWAP32(u32) ((((u32) & 0xff000000) >> 24) |  \
+                            (((u32) & 0x00ff0000) >> 8)  |  \
+                            (((u32) & 0x0000ff00) << 8)  |  \
+                            (((u32) & 0x000000ff) << 24))
+  #else
+    #define TU_BSWAP16(u16) (__builtin_bswap16(u16))
+    #define TU_BSWAP32(u32) (__builtin_bswap32(u32))
+  #endif
 
 	#ifndef __ARMCC_VERSION
   // List of obsolete callback function that is renamed and should not be defined.
diff --git a/src/common/tusb_debug.h b/src/common/tusb_debug.h
index 82f682043..36507041f 100644
--- a/src/common/tusb_debug.h
+++ b/src/common/tusb_debug.h
@@ -46,6 +46,7 @@
 #if CFG_TUSB_DEBUG >= 2
 extern char const* const tu_str_speed[];
 extern char const* const tu_str_std_request[];
+extern char const* const tu_str_xfer_result[];
 #endif
 
 void tu_print_mem(void const *buf, uint32_t count, uint8_t indent);
diff --git a/src/common/tusb_mcu.h b/src/common/tusb_mcu.h
index 95f90f957..48f765674 100644
--- a/src/common/tusb_mcu.h
+++ b/src/common/tusb_mcu.h
@@ -59,14 +59,6 @@
   #define TUP_USBIP_OHCI
   #define TUP_OHCI_RHPORTS        2
 
-#elif TU_CHECK_MCU(OPT_MCU_LPC18XX, OPT_MCU_LPC43XX)
-  // TODO USB0 has 6, USB1 has 4
-  #define TUP_USBIP_CHIPIDEA_HS
-  #define TUP_USBIP_EHCI
-
-  #define TUP_DCD_ENDPOINT_MAX    6
-  #define TUP_RHPORT_HIGHSPEED    1 // Port0 HS, Port1 FS
-
 #elif TU_CHECK_MCU(OPT_MCU_LPC51UXX)
    #define TUP_DCD_ENDPOINT_MAX   5
 
@@ -78,12 +70,28 @@
   // TODO USB0 has 5, USB1 has 6
   #define TUP_DCD_ENDPOINT_MAX    6
 
+#elif TU_CHECK_MCU(OPT_MCU_LPC18XX, OPT_MCU_LPC43XX)
+  // USB0 has 6 with HS PHY, USB1 has 4 only FS
+  #define TUP_USBIP_CHIPIDEA_HS
+  #define TUP_USBIP_EHCI
+
+  #define TUP_DCD_ENDPOINT_MAX    6
+  #define TUP_RHPORT_HIGHSPEED    1
+
+#elif TU_CHECK_MCU(OPT_MCU_MCXN9)
+  // NOTE: MCXN943 port 1 use chipidea HS, port 0 use chipidea FS
+  #define TUP_USBIP_CHIPIDEA_HS
+  #define TUP_USBIP_EHCI
+
+  #define TUP_DCD_ENDPOINT_MAX    8
+  #define TUP_RHPORT_HIGHSPEED    1
+
 #elif TU_CHECK_MCU(OPT_MCU_MIMXRT)
   #define TUP_USBIP_CHIPIDEA_HS
   #define TUP_USBIP_EHCI
 
   #define TUP_DCD_ENDPOINT_MAX    8
-  #define TUP_RHPORT_HIGHSPEED    1 // Port0 HS, Port1 HS
+  #define TUP_RHPORT_HIGHSPEED    1
 
 #elif TU_CHECK_MCU(OPT_MCU_KINETIS_KL, OPT_MCU_KINETIS_K32)
   #define TUP_USBIP_CHIPIDEA_FS
diff --git a/src/common/tusb_private.h b/src/common/tusb_private.h
index d5541856c..db1ba974d 100644
--- a/src/common/tusb_private.h
+++ b/src/common/tusb_private.h
@@ -148,21 +148,26 @@ uint32_t tu_edpt_stream_read_xfer(tu_edpt_stream_t* s);
 
 // Must be called in the transfer complete callback
 TU_ATTR_ALWAYS_INLINE static inline
-void tu_edpt_stream_read_xfer_complete(tu_edpt_stream_t* s, uint32_t xferred_bytes)
-{
+void tu_edpt_stream_read_xfer_complete(tu_edpt_stream_t* s, uint32_t xferred_bytes) {
   tu_fifo_write_n(&s->ff, s->ep_buf, (uint16_t) xferred_bytes);
 }
 
+// Same as tu_edpt_stream_read_xfer_complete but skip the first n bytes
+TU_ATTR_ALWAYS_INLINE static inline
+void tu_edpt_stream_read_xfer_complete_offset(tu_edpt_stream_t* s, uint32_t xferred_bytes, uint32_t skip_offset) {
+  if (skip_offset < xferred_bytes) {
+    tu_fifo_write_n(&s->ff, s->ep_buf + skip_offset, (uint16_t) (xferred_bytes - skip_offset));
+  }
+}
+
 // Get the number of bytes available for reading
 TU_ATTR_ALWAYS_INLINE static inline
-uint32_t tu_edpt_stream_read_available(tu_edpt_stream_t* s)
-{
+uint32_t tu_edpt_stream_read_available(tu_edpt_stream_t* s) {
   return (uint32_t) tu_fifo_count(&s->ff);
 }
 
 TU_ATTR_ALWAYS_INLINE static inline
-bool tu_edpt_stream_peek(tu_edpt_stream_t* s, uint8_t* ch)
-{
+bool tu_edpt_stream_peek(tu_edpt_stream_t* s, uint8_t* ch) {
   return tu_fifo_peek(&s->ff, ch);
 }
 
diff --git a/src/device/dcd.h b/src/device/dcd.h
index 00419ff05..f82b8633d 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -103,6 +103,22 @@ typedef struct TU_ATTR_ALIGNED(4)
 //TU_VERIFY_STATIC(sizeof(dcd_event_t) <= 12, "size is not correct");
 
 //--------------------------------------------------------------------+
+// Memory API
+//--------------------------------------------------------------------+
+
+// clean/flush data cache: write cache -> memory.
+// Required before an DMA TX transfer to make sure data is in memory
+void dcd_dcache_clean(void* addr, uint32_t data_size) TU_ATTR_WEAK;
+
+// invalidate data cache: mark cache as invalid, next read will read from memory
+// Required BOTH before and after an DMA RX transfer
+void dcd_dcache_invalidate(void* addr, uint32_t data_size) TU_ATTR_WEAK;
+
+// clean and invalidate data cache
+// Required before an DMA transfer where memory is both read/write by DMA
+void dcd_dcache_clean_invalidate(void* addr, uint32_t data_size) TU_ATTR_WEAK;
+
+//--------------------------------------------------------------------+
 // Controller API
 //--------------------------------------------------------------------+
 
diff --git a/src/device/usbd.c b/src/device/usbd.c
index cee56af60..409a5ec10 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -516,9 +516,9 @@ void tud_task_ext(uint32_t timeout_ms, bool in_isr)
         _usbd_dev.connected = 1;
 
         // mark both in & out control as free
-        _usbd_dev.ep_status[0][TUSB_DIR_OUT].busy = false;
+        _usbd_dev.ep_status[0][TUSB_DIR_OUT].busy = 0;
         _usbd_dev.ep_status[0][TUSB_DIR_OUT].claimed = 0;
-        _usbd_dev.ep_status[0][TUSB_DIR_IN ].busy = false;
+        _usbd_dev.ep_status[0][TUSB_DIR_IN ].busy = 0;
         _usbd_dev.ep_status[0][TUSB_DIR_IN ].claimed = 0;
 
         // Process control request
@@ -540,12 +540,13 @@ void tud_task_ext(uint32_t timeout_ms, bool in_isr)
 
         TU_LOG(USBD_DBG, "on EP %02X with %u bytes\r\n", ep_addr, (unsigned int) event.xfer_complete.len);
 
-        _usbd_dev.ep_status[epnum][ep_dir].busy = false;
+        _usbd_dev.ep_status[epnum][ep_dir].busy = 0;
         _usbd_dev.ep_status[epnum][ep_dir].claimed = 0;
 
         if ( 0 == epnum )
         {
-          usbd_control_xfer_cb(event.rhport, ep_addr, (xfer_result_t)event.xfer_complete.result, event.xfer_complete.len);
+          usbd_control_xfer_cb(event.rhport, ep_addr, (xfer_result_t) event.xfer_complete.result, event.xfer_complete
+          .len);
         }
         else
         {
@@ -553,7 +554,7 @@ void tud_task_ext(uint32_t timeout_ms, bool in_isr)
           TU_ASSERT(driver, );
 
           TU_LOG(USBD_DBG, "  %s xfer callback\r\n", driver->name);
-          driver->xfer_cb(event.rhport, ep_addr, (xfer_result_t)event.xfer_complete.result, event.xfer_complete.len);
+          driver->xfer_cb(event.rhport, ep_addr, (xfer_result_t) event.xfer_complete.result, event.xfer_complete.len);
         }
       }
       break;
@@ -1244,7 +1245,7 @@ bool usbd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
 
   // Set busy first since the actual transfer can be complete before dcd_edpt_xfer()
   // could return and USBD task can preempt and clear the busy
-  _usbd_dev.ep_status[epnum][dir].busy = true;
+  _usbd_dev.ep_status[epnum][dir].busy = 1;
 
   if ( dcd_edpt_xfer(rhport, ep_addr, buffer, total_bytes) )
   {
@@ -1252,7 +1253,7 @@ bool usbd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
   }else
   {
     // DCD error, mark endpoint as ready to allow next transfer
-    _usbd_dev.ep_status[epnum][dir].busy = false;
+    _usbd_dev.ep_status[epnum][dir].busy = 0;
     _usbd_dev.ep_status[epnum][dir].claimed = 0;
     TU_LOG(USBD_DBG, "FAILED\r\n");
     TU_BREAKPOINT();
@@ -1278,7 +1279,7 @@ bool usbd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16
 
   // Set busy first since the actual transfer can be complete before dcd_edpt_xfer() could return
   // and usbd task can preempt and clear the busy
-  _usbd_dev.ep_status[epnum][dir].busy = true;
+  _usbd_dev.ep_status[epnum][dir].busy = 1;
 
   if (dcd_edpt_xfer_fifo(rhport, ep_addr, ff, total_bytes))
   {
@@ -1287,7 +1288,7 @@ bool usbd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16
   }else
   {
     // DCD error, mark endpoint as ready to allow next transfer
-    _usbd_dev.ep_status[epnum][dir].busy = false;
+    _usbd_dev.ep_status[epnum][dir].busy = 0;
     _usbd_dev.ep_status[epnum][dir].claimed = 0;
     TU_LOG(USBD_DBG, "failed\r\n");
     TU_BREAKPOINT();
@@ -1317,8 +1318,8 @@ void usbd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
   {
     TU_LOG(USBD_DBG, "    Stall EP %02X\r\n", ep_addr);
     dcd_edpt_stall(rhport, ep_addr);
-    _usbd_dev.ep_status[epnum][dir].stalled = true;
-    _usbd_dev.ep_status[epnum][dir].busy = true;
+    _usbd_dev.ep_status[epnum][dir].stalled = 1;
+    _usbd_dev.ep_status[epnum][dir].busy = 1;
   }
 }
 
@@ -1334,8 +1335,8 @@ void usbd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
   {
     TU_LOG(USBD_DBG, "    Clear Stall EP %02X\r\n", ep_addr);
     dcd_edpt_clear_stall(rhport, ep_addr);
-    _usbd_dev.ep_status[epnum][dir].stalled = false;
-    _usbd_dev.ep_status[epnum][dir].busy = false;
+    _usbd_dev.ep_status[epnum][dir].stalled = 0;
+    _usbd_dev.ep_status[epnum][dir].busy = 0;
   }
 }
 
@@ -1366,9 +1367,9 @@ void usbd_edpt_close(uint8_t rhport, uint8_t ep_addr)
   uint8_t const dir   = tu_edpt_dir(ep_addr);
 
   dcd_edpt_close(rhport, ep_addr);
-  _usbd_dev.ep_status[epnum][dir].stalled = false;
-  _usbd_dev.ep_status[epnum][dir].busy = false;
-  _usbd_dev.ep_status[epnum][dir].claimed = false;
+  _usbd_dev.ep_status[epnum][dir].stalled = 0;
+  _usbd_dev.ep_status[epnum][dir].busy = 0;
+  _usbd_dev.ep_status[epnum][dir].claimed = 0;
 
   return;
 }
@@ -1403,9 +1404,9 @@ bool usbd_edpt_iso_activate(uint8_t rhport, tusb_desc_endpoint_t const * desc_ep
   TU_ASSERT(epnum < CFG_TUD_ENDPPOINT_MAX);
   TU_ASSERT(tu_edpt_validate(desc_ep, (tusb_speed_t) _usbd_dev.speed));
 
-  _usbd_dev.ep_status[epnum][dir].stalled = false;
-  _usbd_dev.ep_status[epnum][dir].busy = false;
-  _usbd_dev.ep_status[epnum][dir].claimed = false;
+  _usbd_dev.ep_status[epnum][dir].stalled = 0;
+  _usbd_dev.ep_status[epnum][dir].busy = 0;
+  _usbd_dev.ep_status[epnum][dir].claimed = 0;
   return dcd_edpt_iso_activate(rhport, desc_ep);
 }
 
diff --git a/src/device/usbd_control.c b/src/device/usbd_control.c
index ea8eef285..2afe967b5 100644
--- a/src/device/usbd_control.c
+++ b/src/device/usbd_control.c
@@ -32,7 +32,10 @@
 #include "tusb.h"
 #include "device/usbd_pvt.h"
 
-#if CFG_TUSB_DEBUG >= 2
+// Debug level of USBD Control
+#define USBD_CONTROL_DEBUG   2
+
+#if CFG_TUSB_DEBUG >= USBD_CONTROL_DEBUG
 extern void usbd_driver_print_control_complete_name(usbd_control_xfer_cb_t callback);
 #endif
 
@@ -188,7 +191,7 @@ bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t result
   {
     TU_VERIFY(_ctrl_xfer.buffer);
     memcpy(_ctrl_xfer.buffer, _usbd_ctrl_buf, xferred_bytes);
-    TU_LOG_MEM(2, _usbd_ctrl_buf, xferred_bytes, 2);
+    TU_LOG_MEM(USBD_CONTROL_DEBUG, _usbd_ctrl_buf, xferred_bytes, 2);
   }
 
   _ctrl_xfer.total_xferred += (uint16_t) xferred_bytes;
@@ -205,7 +208,7 @@ bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t result
     // callback can still stall control in status phase e.g out data does not make sense
     if ( _ctrl_xfer.complete_cb )
     {
-      #if CFG_TUSB_DEBUG >= 2
+      #if CFG_TUSB_DEBUG >= USBD_CONTROL_DEBUG
       usbd_driver_print_control_complete_name(_ctrl_xfer.complete_cb);
       #endif
 
diff --git a/src/host/hcd.h b/src/host/hcd.h
index 623c12a12..5a3b0a087 100644
--- a/src/host/hcd.h
+++ b/src/host/hcd.h
@@ -39,8 +39,10 @@
 // Configuration
 //--------------------------------------------------------------------+
 
+// Max number of endpoints per device
+// TODO optimize memory usage
 #ifndef CFG_TUH_ENDPOINT_MAX
-  #define CFG_TUH_ENDPOINT_MAX   (CFG_TUH_HUB + CFG_TUH_HID*2 + CFG_TUH_MSC*2 + CFG_TUH_CDC*3)
+  #define CFG_TUH_ENDPOINT_MAX   16
 //  #ifdef TUP_HCD_ENDPOINT_MAX
 //    #define CFG_TUH_ENDPPOINT_MAX   TUP_HCD_ENDPOINT_MAX
 //  #else
@@ -103,6 +105,22 @@ typedef struct
 } hcd_devtree_info_t;
 
 //--------------------------------------------------------------------+
+// Memory API
+//--------------------------------------------------------------------+
+
+// clean/flush data cache: write cache -> memory.
+// Required before an DMA TX transfer to make sure data is in memory
+void hcd_dcache_clean(void* addr, uint32_t data_size) TU_ATTR_WEAK;
+
+// invalidate data cache: mark cache as invalid, next read will read from memory
+// Required BOTH before and after an DMA RX transfer
+void hcd_dcache_invalidate(void* addr, uint32_t data_size) TU_ATTR_WEAK;
+
+// clean and invalidate data cache
+// Required before an DMA transfer where memory is both read/write by DMA
+void hcd_dcache_clean_invalidate(void* addr, uint32_t data_size) TU_ATTR_WEAK;
+
+//--------------------------------------------------------------------+
 // Controller API
 //--------------------------------------------------------------------+
 
@@ -157,7 +175,7 @@ bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t *
 bool hcd_setup_send(uint8_t rhport, uint8_t dev_addr, uint8_t const setup_packet[8]);
 
 // clear stall, data toggle is also reset to DATA0
-bool hcd_edpt_clear_stall(uint8_t dev_addr, uint8_t ep_addr);
+bool hcd_edpt_clear_stall(uint8_t daddr, uint8_t ep_addr);
 
 //--------------------------------------------------------------------+
 // USBH implemented API
@@ -182,6 +200,7 @@ void hcd_event_device_attach(uint8_t rhport, bool in_isr)
   event.event_id            = HCD_EVENT_DEVICE_ATTACH;
   event.connection.hub_addr = 0;
   event.connection.hub_port = 0;
+
   hcd_event_handler(&event, in_isr);
 }
 
@@ -212,7 +231,6 @@ void hcd_event_xfer_complete(uint8_t dev_addr, uint8_t ep_addr, uint32_t xferred
   event.xfer_complete.result = result;
   event.xfer_complete.len = xferred_bytes;
 
-
   hcd_event_handler(&event, in_isr);
 }
 
diff --git a/src/host/hub.c b/src/host/hub.c
index 386ad6aae..182bd6ce8 100644
--- a/src/host/hub.c
+++ b/src/host/hub.c
@@ -33,6 +33,10 @@
 #include "usbh_classdriver.h"
 #include "hub.h"
 
+// Debug level, TUSB_CFG_DEBUG must be at least this level for debug message
+#define HUB_DEBUG   2
+#define TU_LOG_DRV(...)   TU_LOG(HUB_DEBUG, __VA_ARGS__)
+
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
@@ -218,7 +222,10 @@ void hub_close(uint8_t dev_addr)
   TU_VERIFY(dev_addr > CFG_TUH_DEVICE_MAX, );
   hub_interface_t* p_hub = get_itf(dev_addr);
 
-  if (p_hub->ep_in) tu_memclr(p_hub, sizeof( hub_interface_t));
+  if (p_hub->ep_in) {
+    TU_LOG_DRV("  HUB close addr = %d\r\n", dev_addr);
+    tu_memclr(p_hub, sizeof( hub_interface_t));
+  }
 }
 
 bool hub_edpt_status_xfer(uint8_t dev_addr)
@@ -320,34 +327,35 @@ static void connection_clear_conn_change_complete (tuh_xfer_t* xfer);
 static void connection_port_reset_complete (tuh_xfer_t* xfer);
 
 // callback as response of interrupt endpoint polling
-bool hub_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
+bool hub_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
   (void) xferred_bytes; // TODO can be more than 1 for hub with lots of ports
   (void) ep_addr;
   TU_ASSERT(result == XFER_RESULT_SUCCESS);
 
   hub_interface_t* p_hub = get_itf(dev_addr);
 
-  TU_LOG2("  Hub Status Change = 0x%02X\r\n", p_hub->status_change);
+  uint8_t const status_change = p_hub->status_change;
+  TU_LOG2("  Hub Status Change = 0x%02X\r\n", status_change);
 
-  // Hub bit 0 is for the hub device events
-  if (tu_bit_test(p_hub->status_change, 0))
-  {
-    if (hub_port_get_status(dev_addr, 0, &p_hub->hub_status, hub_get_status_complete, 0) == false)
-    {
+  if ( status_change == 0 ) {
+    // The status change event was neither for the hub, nor for any of its ports.
+    // This shouldn't happen, but it does with some devices.
+    // Initiate the next interrupt poll here.
+    return hub_edpt_status_xfer(dev_addr);
+  }
+
+  if (tu_bit_test(status_change, 0)) {
+    // Hub bit 0 is for the hub device events
+    if (hub_port_get_status(dev_addr, 0, &p_hub->hub_status, hub_get_status_complete, 0) == false) {
       //Hub status control transfer failed, retry
       hub_edpt_status_xfer(dev_addr);
     }
   }
-  else
-  {
+  else {
     // Hub bits 1 to n are hub port events
-    for (uint8_t port=1; port <= p_hub->port_count; port++)
-    {
-      if ( tu_bit_test(p_hub->status_change, port) )
-      {
-        if (hub_port_get_status(dev_addr, port, &p_hub->port_status, hub_port_get_status_complete, 0) == false)
-        {
+    for (uint8_t port=1; port <= p_hub->port_count; port++) {
+      if ( tu_bit_test(status_change, port) ) {
+        if (hub_port_get_status(dev_addr, port, &p_hub->port_status, hub_port_get_status_complete, 0) == false) {
           //Hub status control transfer failed, retry
           hub_edpt_status_xfer(dev_addr);
         }
@@ -357,7 +365,6 @@ bool hub_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32
   }
 
   // NOTE: next status transfer is queued by usbh.c after handling this request
-
   return true;
 }
 
diff --git a/src/host/usbh.c b/src/host/usbh.c
index 3be662c63..7b265c742 100644
--- a/src/host/usbh.c
+++ b/src/host/usbh.c
@@ -61,6 +61,8 @@ typedef struct
   uint8_t hub_addr;
   uint8_t hub_port;
   uint8_t speed;
+
+  // enumeration is in progress, done when all interfaces are configured
   volatile uint8_t enumerating;
 
 //  struct TU_ATTR_PACKED {
@@ -79,10 +81,12 @@ typedef struct {
 
   // Device State
   struct TU_ATTR_PACKED {
-    volatile uint8_t connected  : 1;
-    volatile uint8_t addressed  : 1;
-    volatile uint8_t configured : 1;
-    volatile uint8_t suspended  : 1;
+    volatile uint8_t connected  : 1; // After 1st transfer
+    volatile uint8_t addressed  : 1; // After SET_ADDR
+    volatile uint8_t configured : 1; // After SET_CONFIG and all drivers are configured
+    volatile uint8_t suspended  : 1; // Bus suspended
+
+    // volatile uint8_t removing : 1; // Physically disconnected, waiting to be processed by usbh
   };
 
   // Device Descriptor
@@ -246,7 +250,7 @@ static inline usbh_device_t* get_device(uint8_t dev_addr)
 }
 
 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 void process_removing_device(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);
 static bool usbh_control_xfer_cb (uint8_t daddr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
 
@@ -286,7 +290,7 @@ bool tuh_vid_pid_get(uint8_t dev_addr, uint16_t* vid, uint16_t* pid)
   *vid = *pid = 0;
 
   usbh_device_t const* dev = get_device(dev_addr);
-  TU_VERIFY(dev && dev->configured);
+  TU_VERIFY(dev && dev->addressed && dev->vid != 0);
 
   *vid = dev->vid;
   *pid = dev->pid;
@@ -418,7 +422,7 @@ void tuh_task_ext(uint32_t timeout_ms, bool in_isr)
 
       case HCD_EVENT_DEVICE_REMOVE:
         TU_LOG_USBH("[%u:%u:%u] USBH DEVICE REMOVED\r\n", event.rhport, event.connection.hub_addr, event.connection.hub_port);
-        process_device_unplugged(event.rhport, event.connection.hub_addr, event.connection.hub_port);
+        process_removing_device(event.rhport, event.connection.hub_addr, event.connection.hub_port);
 
         #if CFG_TUH_HUB
         // TODO remove
@@ -436,7 +440,8 @@ void tuh_task_ext(uint32_t timeout_ms, bool in_isr)
         uint8_t const epnum   = tu_edpt_number(ep_addr);
         uint8_t const ep_dir  = tu_edpt_dir(ep_addr);
 
-        TU_LOG_USBH("on EP %02X with %u bytes\r\n", ep_addr, (unsigned int) event.xfer_complete.len);
+        TU_LOG_USBH("on EP %02X with %u bytes: %s\r\n", ep_addr, (unsigned int) event.xfer_complete.len,
+                    tu_str_xfer_result[event.xfer_complete.result]);
 
         if (event.dev_addr == 0)
         {
@@ -447,7 +452,7 @@ void tuh_task_ext(uint32_t timeout_ms, bool in_isr)
         else
         {
           usbh_device_t* dev = get_device(event.dev_addr);
-          TU_ASSERT(dev, );
+          TU_VERIFY(dev && dev->connected, );
 
           dev->ep_status[epnum][ep_dir].busy    = 0;
           dev->ep_status[epnum][ep_dir].claimed = 0;
@@ -571,19 +576,19 @@ bool tuh_control_xfer (tuh_xfer_t* xfer)
 
     TU_ASSERT( hcd_setup_send(rhport, daddr, (uint8_t*) &_ctrl_xfer.request) );
 
-    while (result == XFER_RESULT_INVALID)
-    {
+    while (result == XFER_RESULT_INVALID) {
       // Note: this can be called within an callback ie. part of tuh_task()
       // therefore event with RTOS tuh_task() still need to be invoked
-      if (tuh_task_event_ready())
-      {
+      if (tuh_task_event_ready()) {
         tuh_task();
       }
-
       // TODO probably some timeout to prevent hanged
     }
 
-    // update transfer result
+    // update transfer result, user_data is expected to point to xfer_result_t
+    if (xfer->user_data != 0) {
+      *((xfer_result_t*) xfer->user_data) = result;
+    }
     xfer->result     = result;
     xfer->actual_len = _ctrl_xfer.actual_len;
   }
@@ -736,29 +741,33 @@ void usbh_int_set(bool enabled)
 // TODO has some duplication code with device, refactor later
 bool usbh_edpt_claim(uint8_t dev_addr, uint8_t ep_addr)
 {
+  // Note: addr0 only use tuh_control_xfer
   usbh_device_t* dev = get_device(dev_addr);
-
-  // addr0 only use tuh_control_xfer
-  TU_ASSERT(dev);
+  TU_ASSERT(dev && dev->connected);
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir   = tu_edpt_dir(ep_addr);
 
-  return tu_edpt_claim(&dev->ep_status[epnum][dir], _usbh_mutex);
+  TU_VERIFY(tu_edpt_claim(&dev->ep_status[epnum][dir], _usbh_mutex));
+  TU_LOG_USBH("[%u] Claimed EP 0x%02x\r\n", dev_addr, ep_addr);
+
+  return true;
 }
 
 // TODO has some duplication code with device, refactor later
 bool usbh_edpt_release(uint8_t dev_addr, uint8_t ep_addr)
 {
+  // Note: addr0 only use tuh_control_xfer
   usbh_device_t* dev = get_device(dev_addr);
-
-  // addr0 only use tuh_control_xfer
-  TU_ASSERT(dev);
+  TU_VERIFY(dev && dev->connected);
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir   = tu_edpt_dir(ep_addr);
 
-  return tu_edpt_release(&dev->ep_status[epnum][dir], _usbh_mutex);
+  TU_VERIFY(tu_edpt_release(&dev->ep_status[epnum][dir], _usbh_mutex));
+  TU_LOG_USBH("[%u] Released EP 0x%02x\r\n", dev_addr, ep_addr);
+
+  return true;
 }
 
 // TODO has some duplication code with device, refactor later
@@ -866,6 +875,10 @@ TU_ATTR_FAST_FUNC void hcd_event_handler(hcd_event_t const* event, bool in_isr)
 {
   switch (event->event_id)
   {
+//    case HCD_EVENT_DEVICE_REMOVE:
+//      // mark device as removing to prevent further xfer before the event is processed in usbh task
+//      break;
+
     default:
       osal_queue_send(_usbh_q, event, in_isr);
     break;
@@ -877,7 +890,7 @@ TU_ATTR_FAST_FUNC void hcd_event_handler(hcd_event_t const* event, bool in_isr)
 //--------------------------------------------------------------------+
 
 // generic helper to get a descriptor
-// if blocking, user_data could be pointed to xfer_result
+// if blocking, user_data is pointed to xfer_result
 static bool _get_descriptor(uint8_t daddr, uint8_t type, uint8_t index, uint16_t language_id, void* buffer, uint16_t len,
                             tuh_xfer_cb_t complete_cb, uintptr_t user_data)
 {
@@ -905,15 +918,7 @@ static bool _get_descriptor(uint8_t daddr, uint8_t type, uint8_t index, uint16_t
     .user_data   = user_data
   };
 
-  bool const ret = tuh_control_xfer(&xfer);
-
-  // if blocking, user_data could be pointed to xfer_result
-  if ( !complete_cb && user_data )
-  {
-    *((xfer_result_t*) user_data) = xfer.result;
-  }
-
-  return ret;
+  return tuh_control_xfer(&xfer);
 }
 
 bool tuh_descriptor_get(uint8_t daddr, uint8_t type, uint8_t index, void* buffer, uint16_t len,
@@ -971,7 +976,7 @@ bool tuh_descriptor_get_serial_string(uint8_t daddr, uint16_t language_id, void*
 }
 
 // Get HID report descriptor
-// if blocking, user_data could be pointed to xfer_result
+// if blocking, user_data is pointed to xfer_result
 bool tuh_descriptor_get_hid_report(uint8_t daddr, uint8_t itf_num, uint8_t desc_type, uint8_t index, void* buffer, uint16_t len,
                                    tuh_xfer_cb_t complete_cb, uintptr_t user_data)
 {
@@ -1000,15 +1005,7 @@ bool tuh_descriptor_get_hid_report(uint8_t daddr, uint8_t itf_num, uint8_t desc_
     .user_data   = user_data
   };
 
-  bool const ret = tuh_control_xfer(&xfer);
-
-  // if blocking, user_data could be pointed to xfer_result
-  if ( !complete_cb && user_data )
-  {
-    *((xfer_result_t*) user_data) = xfer.result;
-  }
-
-  return ret;
+  return tuh_control_xfer(&xfer);
 }
 
 bool tuh_configuration_set(uint8_t daddr, uint8_t config_num,
@@ -1040,15 +1037,7 @@ bool tuh_configuration_set(uint8_t daddr, uint8_t config_num,
     .user_data   = user_data
   };
 
-  bool ret = tuh_control_xfer(&xfer);
-
-  // if blocking, user_data could be pointed to xfer_result
-  if ( !complete_cb && user_data )
-  {
-    *((xfer_result_t*) user_data) = xfer.result;
-  }
-
-  return ret;
+  return tuh_control_xfer(&xfer);
 }
 
 bool tuh_interface_set(uint8_t daddr, uint8_t itf_num, uint8_t itf_alt,
@@ -1080,15 +1069,7 @@ bool tuh_interface_set(uint8_t daddr, uint8_t itf_num, uint8_t itf_alt,
     .user_data   = user_data
   };
 
-  bool ret = tuh_control_xfer(&xfer);
-
-  // if blocking, user_data could be pointed to xfer_result
-  if ( !complete_cb && user_data )
-  {
-    *((xfer_result_t*) user_data) = xfer.result;
-  }
-
-  return ret;
+  return tuh_control_xfer(&xfer);
 }
 
 //--------------------------------------------------------------------+
@@ -1141,7 +1122,7 @@ uint8_t tuh_descriptor_get_serial_string_sync(uint8_t daddr, uint16_t language_i
 }
 
 //--------------------------------------------------------------------+
-//
+// Detaching
 //--------------------------------------------------------------------+
 
 TU_ATTR_ALWAYS_INLINE
@@ -1150,47 +1131,79 @@ static inline bool is_hub_addr(uint8_t daddr)
   return (CFG_TUH_HUB > 0) && (daddr > CFG_TUH_DEVICE_MAX);
 }
 
+//static void mark_removing_device_isr(uint8_t rhport, uint8_t hub_addr, uint8_t hub_port) {
+//  for (uint8_t dev_id = 0; dev_id < TOTAL_DEVICES; dev_id++) {
+//    usbh_device_t *dev = &_usbh_devices[dev_id];
+//    uint8_t const daddr = dev_id + 1;
+//
+//    // hub_addr = 0 means roothub, hub_port = 0 means all devices of downstream hub
+//    if (dev->rhport == rhport && dev->connected &&
+//        (hub_addr == 0 || dev->hub_addr == hub_addr) &&
+//        (hub_port == 0 || dev->hub_port == hub_port)) {
+//      if (is_hub_addr(daddr)) {
+//        // If the device itself is a usb hub, mark all downstream devices.
+//        // FIXME recursive calls
+//        mark_removing_device_isr(rhport, daddr, 0);
+//      }
+//
+//      dev->removing = 1;
+//    }
+//  }
+//}
+
 // a device unplugged from rhport:hub_addr:hub_port
-static void process_device_unplugged(uint8_t rhport, uint8_t hub_addr, uint8_t hub_port)
+static void process_removing_device(uint8_t rhport, uint8_t hub_addr, uint8_t hub_port)
 {
   //------------- find the all devices (star-network) under port that is unplugged -------------//
   // TODO mark as disconnected in ISR, also handle dev0
-  for ( uint8_t dev_id = 0; dev_id < TU_ARRAY_SIZE(_usbh_devices); dev_id++ )
-  {
-    usbh_device_t* dev = &_usbh_devices[dev_id];
-    uint8_t const dev_addr = dev_id+1;
 
-    // TODO Hub multiple level
-    if (dev->rhport == rhport   &&
-        (hub_addr == 0 || dev->hub_addr == hub_addr) && // hub_addr = 0 means roothub
-        (hub_port == 0 || dev->hub_port == hub_port) && // hub_port = 0 means all devices of downstream hub
-        dev->connected)
-    {
-      TU_LOG_USBH("  Address = %u\r\n", dev_addr);
+#if 0
+  // index as hub addr, value is hub port (0xFF for invalid)
+  uint8_t removing_hubs[CFG_TUH_HUB];
+  memset(removing_hubs, TUSB_INDEX_INVALID_8, sizeof(removing_hubs));
 
-      if (is_hub_addr(dev_addr))
-      {
-        TU_LOG(USBH_DEBUG, "HUB address = %u is unmounted\r\n", dev_addr);
-        // If the device itself is a usb hub, unplug downstream devices.
-        // FIXME un-roll recursive calls to prevent potential stack overflow
-        process_device_unplugged(rhport, dev_addr, 0);
-      }else
-      {
-        // Invoke callback before closing driver
-        if (tuh_umount_cb) tuh_umount_cb(dev_addr);
+  removing_hubs[hub_addr-CFG_TUH_DEVICE_MAX] = hub_port;
+
+  // consecutive non-removing hub
+  uint8_t nop_count = 0;
+#endif
+
+  for (uint8_t dev_id = 0; dev_id < TOTAL_DEVICES; dev_id++) {
+    usbh_device_t *dev = &_usbh_devices[dev_id];
+    uint8_t const daddr = dev_id + 1;
+
+    // hub_addr = 0 means roothub, hub_port = 0 means all devices of downstream hub
+    if (dev->rhport == rhport && dev->connected &&
+        (hub_addr == 0 || dev->hub_addr == hub_addr) &&
+        (hub_port == 0 || dev->hub_port == hub_port)) {
+      TU_LOG_USBH("Device unplugged address = %u\r\n", daddr);
+
+      if (is_hub_addr(daddr)) {
+        TU_LOG(USBH_DEBUG, "  is a HUB device\r\n", daddr);
+
+        // Submit removed event If the device itself is a hub (un-rolled recursive)
+        // TODO a better to unroll recursrive is using array of removing_hubs and mark it here
+        hcd_event_t event;
+        event.rhport = rhport;
+        event.event_id = HCD_EVENT_DEVICE_REMOVE;
+        event.connection.hub_addr = daddr;
+        event.connection.hub_port = 0;
+
+        hcd_event_handler(&event, false);
+      } else {
+        // Invoke callback before closing driver (maybe call it later ?)
+        if (tuh_umount_cb) tuh_umount_cb(daddr);
       }
 
       // Close class driver
-      for (uint8_t drv_id = 0; drv_id < USBH_CLASS_DRIVER_COUNT; drv_id++)
-      {
-        TU_LOG_USBH("%s close\r\n", usbh_class_drivers[drv_id].name);
-        usbh_class_drivers[drv_id].close(dev_addr);
+      for (uint8_t drv_id = 0; drv_id < USBH_CLASS_DRIVER_COUNT; drv_id++) {
+        usbh_class_drivers[drv_id].close(daddr);
       }
 
-      hcd_device_close(rhport, dev_addr);
+      hcd_device_close(rhport, daddr);
       clear_device(dev);
       // abort on-going control xfer if any
-      if (_ctrl_xfer.daddr == dev_addr) _set_control_xfer_stage(CONTROL_STAGE_IDLE);
+      if (_ctrl_xfer.daddr == daddr) _set_control_xfer_stage(CONTROL_STAGE_IDLE);
     }
   }
 }
@@ -1242,6 +1255,7 @@ static void process_enumeration(tuh_xfer_t* xfer)
     {
       failed_count++;
       osal_task_delay(ATTEMPT_DELAY_MS); // delay a bit
+      TU_LOG1("Enumeration attempt %u\r\n", failed_count);
       TU_ASSERT(tuh_control_xfer(xfer), );
     }else
     {
@@ -1481,6 +1495,7 @@ static uint8_t get_new_address(bool is_hub)
 {
   uint8_t start;
   uint8_t end;
+
   if ( is_hub )
   {
     start = CFG_TUH_DEVICE_MAX;
@@ -1491,7 +1506,7 @@ static uint8_t get_new_address(bool is_hub)
     end   = start + CFG_TUH_DEVICE_MAX;
   }
 
-  for ( uint8_t idx = start; idx < end; idx++)
+  for (uint8_t idx = start; idx < end; idx++)
   {
     if (!_usbh_devices[idx].connected) return (idx+1);
   }
diff --git a/src/portable/chipidea/ci_hs/ci_hs_imxrt.h b/src/portable/chipidea/ci_hs/ci_hs_imxrt.h
index 2de0d9cb4..ceff893bd 100644
--- a/src/portable/chipidea/ci_hs/ci_hs_imxrt.h
+++ b/src/portable/chipidea/ci_hs/ci_hs_imxrt.h
@@ -37,22 +37,53 @@
 #define USB2_BASE USB_OTG2_BASE
 #endif
 
+// RT1040 calls its only USB USB_OTG (no 1)
+#if defined(MIMXRT1042_SERIES)
+#define USB_OTG1_IRQn USB_OTG_IRQn
+#endif
+
 static const ci_hs_controller_t _ci_controller[] =
 {
   // RT1010 and RT1020 only has 1 USB controller
   #if FSL_FEATURE_SOC_USBHS_COUNT == 1
-    { .reg_base = USB_BASE , .irqnum = USB_OTG1_IRQn, .ep_count = 8 }
+    { .reg_base = USB_BASE , .irqnum = USB_OTG1_IRQn }
   #else
-    { .reg_base = USB1_BASE, .irqnum = USB_OTG1_IRQn, .ep_count = 8 },
-    { .reg_base = USB2_BASE, .irqnum = USB_OTG2_IRQn, .ep_count = 8 }
+    { .reg_base = USB1_BASE, .irqnum = USB_OTG1_IRQn},
+    { .reg_base = USB2_BASE, .irqnum = USB_OTG2_IRQn}
   #endif
 };
 
+#define CI_HS_REG(_port)        ((ci_hs_regs_t*) _ci_controller[_port].reg_base)
+
+//------------- DCD -------------//
 #define CI_DCD_INT_ENABLE(_p)   NVIC_EnableIRQ (_ci_controller[_p].irqnum)
 #define CI_DCD_INT_DISABLE(_p)  NVIC_DisableIRQ(_ci_controller[_p].irqnum)
 
+//------------- HCD -------------//
 #define CI_HCD_INT_ENABLE(_p)   NVIC_EnableIRQ (_ci_controller[_p].irqnum)
 #define CI_HCD_INT_DISABLE(_p)  NVIC_DisableIRQ(_ci_controller[_p].irqnum)
 
+//------------- DCache -------------//
+TU_ATTR_ALWAYS_INLINE static inline bool imxrt_is_cache_mem(uint32_t addr) {
+  return !(0x20000000 <= addr && addr < 0x20100000);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void imxrt_dcache_clean(void* addr, uint32_t data_size) {
+  if (imxrt_is_cache_mem((uint32_t) addr)) {
+    SCB_CleanDCache_by_Addr((uint32_t *) addr, (int32_t) data_size);
+  }
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void imxrt_dcache_invalidate(void* addr, uint32_t data_size) {
+  if (imxrt_is_cache_mem((uint32_t) addr)) {
+    SCB_InvalidateDCache_by_Addr(addr, (int32_t) data_size);
+  }
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void imxrt_dcache_clean_invalidate(void* addr, uint32_t data_size) {
+  if (imxrt_is_cache_mem((uint32_t) addr)) {
+    SCB_CleanInvalidateDCache_by_Addr(addr, (int32_t) data_size);
+  }
+}
 
 #endif
diff --git a/src/portable/chipidea/ci_hs/ci_hs_lpc18_43.h b/src/portable/chipidea/ci_hs/ci_hs_lpc18_43.h
index 8c2e7dfa6..2e84c93e7 100644
--- a/src/portable/chipidea/ci_hs/ci_hs_lpc18_43.h
+++ b/src/portable/chipidea/ci_hs/ci_hs_lpc18_43.h
@@ -32,10 +32,12 @@
 
 static const ci_hs_controller_t _ci_controller[] =
 {
-  { .reg_base = LPC_USB0_BASE, .irqnum = USB0_IRQn, .ep_count = 6 },
-  { .reg_base = LPC_USB1_BASE, .irqnum = USB1_IRQn, .ep_count = 4 }
+  { .reg_base = LPC_USB0_BASE, .irqnum = USB0_IRQn },
+  { .reg_base = LPC_USB1_BASE, .irqnum = USB1_IRQn }
 };
 
+#define CI_HS_REG(_port)        ((ci_hs_regs_t*) _ci_controller[_port].reg_base)
+
 #define CI_DCD_INT_ENABLE(_p)   NVIC_EnableIRQ (_ci_controller[_p].irqnum)
 #define CI_DCD_INT_DISABLE(_p)  NVIC_DisableIRQ(_ci_controller[_p].irqnum)
 
diff --git a/src/portable/chipidea/ci_hs/ci_hs_mcx.h b/src/portable/chipidea/ci_hs/ci_hs_mcx.h
new file mode 100644
index 000000000..f940f4a9d
--- /dev/null
+++ b/src/portable/chipidea/ci_hs/ci_hs_mcx.h
@@ -0,0 +1,52 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2021, Ha Thach (tinyusb.org)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * This file is part of the TinyUSB stack.
+ */
+
+#ifndef _CI_HS_MCX_H_
+#define _CI_HS_MCX_H_
+
+#include "fsl_device_registers.h"
+
+// NOTE: MCX N9 has 2 different USB Controller
+// - USB0 is KHCI FullSpeed
+// - USB1 is ChipIdea HighSpeed, therefore rhport = 1 is actually index 0
+
+static const ci_hs_controller_t _ci_controller[] = {
+    {.reg_base = USBHS1__USBC_BASE, .irqnum = USB1_HS_IRQn}
+};
+
+TU_ATTR_ALWAYS_INLINE static inline ci_hs_regs_t* CI_HS_REG(uint8_t port) {
+  (void) port;
+  return ((ci_hs_regs_t*) _ci_controller[0].reg_base);
+}
+
+#define CI_DCD_INT_ENABLE(_p)   do { (void) _p; NVIC_EnableIRQ (_ci_controller[0].irqnum); } while (0)
+#define CI_DCD_INT_DISABLE(_p)  do { (void) _p; NVIC_DisableIRQ(_ci_controller[0].irqnum); } while (0)
+
+#define CI_HCD_INT_ENABLE(_p)   NVIC_EnableIRQ (_ci_controller[_p].irqnum)
+#define CI_HCD_INT_DISABLE(_p)  NVIC_DisableIRQ(_ci_controller[_p].irqnum)
+
+
+#endif
diff --git a/src/portable/chipidea/ci_hs/ci_hs_type.h b/src/portable/chipidea/ci_hs/ci_hs_type.h
index 31b5a012d..2f3aa3694 100644
--- a/src/portable/chipidea/ci_hs/ci_hs_type.h
+++ b/src/portable/chipidea/ci_hs/ci_hs_type.h
@@ -31,13 +31,21 @@
  extern "C" {
 #endif
 
+// DCCPARAMS
+enum {
+  DCCPARAMS_DEN_MASK = 0x1Fu, ///< DEN bit 4:0
+};
+
 // USBCMD
 enum {
   USBCMD_RUN_STOP         = TU_BIT(0),
   USBCMD_RESET            = TU_BIT(1),
   USBCMD_SETUP_TRIPWIRE   = TU_BIT(13),
-  USBCMD_ADD_QTD_TRIPWIRE = TU_BIT(14)  ///< This bit is used as a semaphore to ensure the to proper addition of a new dTD to an active (primed) endpoint’s linked list. This bit is set and cleared by software during the process of adding a new dTD
-// Interrupt Threshold bit 23:16
+  USBCMD_ADD_QTD_TRIPWIRE = TU_BIT(14), // This bit is used as a semaphore to ensure the to proper addition of a
+                                        // new dTD to an active (primed) endpoint’s linked list. This bit is set and
+                                        // cleared by software during the process of adding a new dTD
+
+  USBCMD_INTR_THRESHOLD_MASK = 0x00FF0000u, // Interrupt Threshold bit 23:16
 };
 
 // PORTSC1
@@ -72,6 +80,7 @@ enum {
 
 // USBMode
 enum {
+  USBMOD_CM_MASK    = TU_BIT(0) | TU_BIT(1),
   USBMODE_CM_DEVICE = 2,
   USBMODE_CM_HOST   = 3,
 
@@ -134,7 +143,6 @@ typedef struct
 {
   uint32_t reg_base;
   uint32_t irqnum;
-  uint8_t  ep_count; // Max bi-directional Endpoints
 }ci_hs_controller_t;
 
 #ifdef __cplusplus
diff --git a/src/portable/chipidea/ci_hs/dcd_ci_hs.c b/src/portable/chipidea/ci_hs/dcd_ci_hs.c
index bc6736cf2..850c82e43 100644
--- a/src/portable/chipidea/ci_hs/dcd_ci_hs.c
+++ b/src/portable/chipidea/ci_hs/dcd_ci_hs.c
@@ -28,34 +28,52 @@
 
 #if CFG_TUD_ENABLED && defined(TUP_USBIP_CHIPIDEA_HS)
 
-//--------------------------------------------------------------------+
-// INCLUDE
-//--------------------------------------------------------------------+
 #include "device/dcd.h"
 #include "ci_hs_type.h"
 
 #if CFG_TUSB_MCU == OPT_MCU_MIMXRT
   #include "ci_hs_imxrt.h"
-#elif TU_CHECK_MCU(OPT_MCU_LPC18XX, OPT_MCU_LPC43XX)
+
+  void dcd_dcache_clean(void* addr, uint32_t data_size) {
+    imxrt_dcache_clean(addr, data_size);
+  }
+
+  void dcd_dcache_invalidate(void* addr, uint32_t data_size) {
+    imxrt_dcache_invalidate(addr, data_size);
+  }
+
+  void dcd_dcache_clean_invalidate(void* addr, uint32_t data_size) {
+    imxrt_dcache_clean_invalidate(addr, data_size);
+  }
+
+#else
+
+#if TU_CHECK_MCU(OPT_MCU_LPC18XX, OPT_MCU_LPC43XX)
   #include "ci_hs_lpc18_43.h"
+
+#elif TU_CHECK_MCU(OPT_MCU_MCXN9)
+  // MCX N9 only port 1 use this controller
+  #include "ci_hs_mcx.h"
 #else
   #error "Unsupported MCUs"
 #endif
 
-//--------------------------------------------------------------------+
-// MACRO CONSTANT TYPEDEF
-//--------------------------------------------------------------------+
+  TU_ATTR_WEAK void dcd_dcache_clean(void* addr, uint32_t data_size) {
+    (void) addr; (void) data_size;
+  }
 
-#define CI_HS_REG(_port)      ((ci_hs_regs_t*) _ci_controller[_port].reg_base)
+  TU_ATTR_WEAK void dcd_dcache_invalidate(void* addr, uint32_t data_size) {
+    (void) addr; (void) data_size;
+  }
 
-// Clean means to push any cached changes to RAM and invalidate "removes" the
-// entry from the cache.
-#if defined(__CORTEX_M) && __CORTEX_M == 7 && __DCACHE_PRESENT == 1
-  #define CleanInvalidateDCache_by_Addr   SCB_CleanInvalidateDCache_by_Addr
-#else
-  #define CleanInvalidateDCache_by_Addr(_addr, _dsize)
+  TU_ATTR_WEAK void dcd_dcache_clean_invalidate(void* addr, uint32_t data_size) {
+    (void) addr; (void) data_size;
+  }
 #endif
 
+//--------------------------------------------------------------------+
+// MACRO CONSTANT TYPEDEF
+//--------------------------------------------------------------------+
 
 // ENDPTCTRL
 enum {
@@ -161,6 +179,16 @@ CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(2048)
 static dcd_data_t _dcd_data;
 
 //--------------------------------------------------------------------+
+// Prototypes and Helper Functions
+//--------------------------------------------------------------------+
+
+TU_ATTR_ALWAYS_INLINE
+static inline uint8_t ci_ep_count(ci_hs_regs_t const* dcd_reg)
+{
+  return dcd_reg->DCCPARAMS & DCCPARAMS_DEN_MASK;
+}
+
+//--------------------------------------------------------------------+
 // Controller API
 //--------------------------------------------------------------------+
 
@@ -174,7 +202,8 @@ static void bus_reset(uint8_t rhport)
   // endpoint type of the unused direction must be changed from the control type to any other
   // type (e.g. bulk). Leaving an un-configured endpoint control will cause undefined behavior
   // for the data PID tracking on the active endpoint.
-  for( uint8_t i=1; i < _ci_controller[rhport].ep_count; i++)
+  uint8_t const ep_count = ci_ep_count(dcd_reg);
+  for( uint8_t i=1; i < ep_count; i++)
   {
     dcd_reg->ENDPTCTRL[i] = (TUSB_XFER_BULK << ENDPTCTRL_TYPE_POS) | (TUSB_XFER_BULK << (16+ENDPTCTRL_TYPE_POS));
   }
@@ -202,7 +231,7 @@ static void bus_reset(uint8_t rhport)
 
   _dcd_data.qhd[0][0].int_on_setup = 1; // OUT only
 
-  CleanInvalidateDCache_by_Addr((uint32_t*) &_dcd_data, sizeof(dcd_data_t));
+  dcd_dcache_clean_invalidate(&_dcd_data, sizeof(dcd_data_t));
 }
 
 void dcd_init(uint8_t rhport)
@@ -211,26 +240,34 @@ void dcd_init(uint8_t rhport)
 
   ci_hs_regs_t* dcd_reg = CI_HS_REG(rhport);
 
+  TU_ASSERT(ci_ep_count(dcd_reg) <= TUP_DCD_ENDPOINT_MAX, );
+
   // Reset controller
   dcd_reg->USBCMD |= USBCMD_RESET;
   while( dcd_reg->USBCMD & USBCMD_RESET ) {}
 
   // Set mode to device, must be set immediately after reset
-  dcd_reg->USBMODE = USBMODE_CM_DEVICE;
+  uint32_t usbmode = dcd_reg->USBMODE & ~USBMOD_CM_MASK;
+  usbmode |= USBMODE_CM_DEVICE;
+  dcd_reg->USBMODE = usbmode;
+
   dcd_reg->OTGSC = OTGSC_VBUS_DISCHARGE | OTGSC_OTG_TERMINATION;
 
 #if !TUD_OPT_HIGH_SPEED
   dcd_reg->PORTSC1 = PORTSC1_FORCE_FULL_SPEED;
 #endif
 
-  CleanInvalidateDCache_by_Addr((uint32_t*) &_dcd_data, sizeof(dcd_data_t));
+  dcd_dcache_clean_invalidate(&_dcd_data, sizeof(dcd_data_t));
 
   dcd_reg->ENDPTLISTADDR = (uint32_t) _dcd_data.qhd; // Endpoint List Address has to be 2K alignment
   dcd_reg->USBSTS  = dcd_reg->USBSTS;
   dcd_reg->USBINTR = INTR_USB | INTR_ERROR | INTR_PORT_CHANGE | INTR_SUSPEND;
 
-  dcd_reg->USBCMD &= ~0x00FF0000;     // Interrupt Threshold Interval = 0
-  dcd_reg->USBCMD |= USBCMD_RUN_STOP; // Connect
+  uint32_t usbcmd = dcd_reg->USBCMD;
+  usbcmd &= ~USBCMD_INTR_THRESHOLD_MASK; // Interrupt Threshold Interval = 0
+  usbcmd |= USBCMD_RUN_STOP; // run
+
+  dcd_reg->USBCMD = usbcmd;
 }
 
 void dcd_int_enable(uint8_t rhport)
@@ -286,7 +323,7 @@ static void qtd_init(dcd_qtd_t* p_qtd, void * data_ptr, uint16_t total_bytes)
 {
   // Force the CPU to flush the buffer. We increase the size by 31 because the call aligns the
   // address to 32-byte boundaries. Buffer must be word aligned
-  CleanInvalidateDCache_by_Addr((uint32_t*) tu_align((uint32_t) data_ptr, 4), total_bytes + 31);
+  dcd_dcache_clean_invalidate((uint32_t*) tu_align((uint32_t) data_ptr, 4), total_bytes + 31);
 
   tu_memclr(p_qtd, sizeof(dcd_qtd_t));
 
@@ -343,8 +380,10 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc)
   uint8_t const epnum = tu_edpt_number(p_endpoint_desc->bEndpointAddress);
   uint8_t const dir   = tu_edpt_dir(p_endpoint_desc->bEndpointAddress);
 
+  ci_hs_regs_t* dcd_reg = CI_HS_REG(rhport);
+
   // Must not exceed max endpoint number
-  TU_ASSERT( epnum < _ci_controller[rhport].ep_count );
+  TU_ASSERT(epnum < ci_ep_count(dcd_reg));
 
   //------------- Prepare Queue Head -------------//
   dcd_qhd_t * p_qhd = &_dcd_data.qhd[epnum][dir];
@@ -359,11 +398,9 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc)
 
   p_qhd->qtd_overlay.next        = QTD_NEXT_INVALID;
 
-  CleanInvalidateDCache_by_Addr((uint32_t*) &_dcd_data, sizeof(dcd_data_t));
+  dcd_dcache_clean_invalidate(&_dcd_data, sizeof(dcd_data_t));
 
   // Enable EP Control
-  ci_hs_regs_t* dcd_reg = CI_HS_REG(rhport);
-
   uint32_t const epctrl = (p_endpoint_desc->bmAttributes.xfer << ENDPTCTRL_TYPE_POS) | ENDPTCTRL_ENABLE | ENDPTCTRL_TOGGLE_RESET;
 
   if ( dir == TUSB_DIR_OUT )
@@ -382,7 +419,8 @@ void dcd_edpt_close_all (uint8_t rhport)
   ci_hs_regs_t* dcd_reg = CI_HS_REG(rhport);
 
   // Disable all non-control endpoints
-  for( uint8_t epnum=1; epnum < _ci_controller[rhport].ep_count; epnum++)
+  uint8_t const ep_count = ci_ep_count(dcd_reg);
+  for (uint8_t epnum = 1; epnum < ep_count; epnum++)
   {
     _dcd_data.qhd[epnum][TUSB_DIR_OUT].qtd_overlay.halted = 1;
     _dcd_data.qhd[epnum][TUSB_DIR_IN ].qtd_overlay.halted = 1;
@@ -420,7 +458,7 @@ static void qhd_start_xfer(uint8_t rhport, uint8_t epnum, uint8_t dir)
   p_qhd->qtd_overlay.next   = (uint32_t) p_qtd; // link qtd to qhd
 
   // flush cache
-  CleanInvalidateDCache_by_Addr((uint32_t*) &_dcd_data, sizeof(dcd_data_t));
+  dcd_dcache_clean_invalidate(&_dcd_data, sizeof(dcd_data_t));
 
   if ( epnum == 0 )
   {
@@ -498,7 +536,7 @@ bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16
         }
       }
 
-      CleanInvalidateDCache_by_Addr((uint32_t*) tu_align((uint32_t) fifo_info.ptr_wrap, 4), total_bytes - fifo_info.len_wrap + 31);
+      dcd_dcache_clean_invalidate((uint32_t*) tu_align((uint32_t) fifo_info.ptr_wrap, 4), total_bytes - fifo_info.len_wrap + 31);
     }
     else
     {
@@ -611,20 +649,11 @@ void dcd_int_handler(uint8_t rhport)
   if (int_status & INTR_USB)
   {
     // Make sure we read the latest version of _dcd_data.
-    CleanInvalidateDCache_by_Addr((uint32_t*) &_dcd_data, sizeof(dcd_data_t));
+    dcd_dcache_clean_invalidate(&_dcd_data, sizeof(dcd_data_t));
 
     uint32_t const edpt_complete = dcd_reg->ENDPTCOMPLETE;
     dcd_reg->ENDPTCOMPLETE = edpt_complete; // acknowledge
 
-    if (dcd_reg->ENDPTSETUPSTAT)
-    {
-      //------------- Set up Received -------------//
-      // 23.10.10.2 Operational model for setup transfers
-      dcd_reg->ENDPTSETUPSTAT = dcd_reg->ENDPTSETUPSTAT;
-
-      dcd_event_setup_received(rhport, (uint8_t*)(uintptr_t) &_dcd_data.qhd[0][0].setup_request, true);
-    }
-
     // 23.10.12.3 Failed QTD also get ENDPTCOMPLETE set
     // nothing to do, we will submit xfer as error to usbd
     // if (int_status & INTR_ERROR) { }
@@ -637,6 +666,15 @@ void dcd_int_handler(uint8_t rhport)
         if ( tu_bit_test(edpt_complete, epnum+16) ) process_edpt_complete_isr(rhport, epnum, TUSB_DIR_IN);
       }
     }
+
+    // Set up Received
+    // 23.10.10.2 Operational model for setup transfers
+    // Must be after normal transfer complete since it is possible to have both previous control status + new setup
+    // in the same frame and we should handle previous status first.
+    if (dcd_reg->ENDPTSETUPSTAT) {
+      dcd_reg->ENDPTSETUPSTAT = dcd_reg->ENDPTSETUPSTAT;
+      dcd_event_setup_received(rhport, (uint8_t *) (uintptr_t) &_dcd_data.qhd[0][0].setup_request, true);
+    }
   }
 
   if (int_status & INTR_SOF)
diff --git a/src/portable/chipidea/ci_hs/hcd_ci_hs.c b/src/portable/chipidea/ci_hs/hcd_ci_hs.c
index d607627b4..8c27abbf6 100644
--- a/src/portable/chipidea/ci_hs/hcd_ci_hs.c
+++ b/src/portable/chipidea/ci_hs/hcd_ci_hs.c
@@ -41,6 +41,19 @@
 
 #if CFG_TUSB_MCU == OPT_MCU_MIMXRT
   #include "ci_hs_imxrt.h"
+
+  void hcd_dcache_clean(void* addr, uint32_t data_size) {
+    imxrt_dcache_clean(addr, data_size);
+  }
+
+  void hcd_dcache_invalidate(void* addr, uint32_t data_size) {
+    imxrt_dcache_invalidate(addr, data_size);
+  }
+
+  void hcd_dcache_clean_invalidate(void* addr, uint32_t data_size) {
+    imxrt_dcache_clean_invalidate(addr, data_size);
+  }
+
 #elif TU_CHECK_MCU(OPT_MCU_LPC18XX, OPT_MCU_LPC43XX)
   #include "ci_hs_lpc18_43.h"
 #else
@@ -51,8 +64,6 @@
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
 
-#define CI_HS_REG(_port)      ((ci_hs_regs_t*) _ci_controller[_port].reg_base)
-
 //--------------------------------------------------------------------+
 // Controller API
 //--------------------------------------------------------------------+
@@ -76,6 +87,8 @@ bool hcd_init(uint8_t rhport)
 #endif
 
   // FIXME force full speed, still have issue with Highspeed enumeration
+  // 1. Have issue when plug/unplug devices, maybe the port is not reset properly
+  // 2. Also does not seems to detect disconnection
   hcd_reg->PORTSC1 |= PORTSC1_FORCE_FULL_SPEED;
 
   return ehci_init(rhport, (uint32_t) &hcd_reg->CAPLENGTH, (uint32_t) &hcd_reg->USBCMD);
diff --git a/src/portable/ehci/ehci.c b/src/portable/ehci/ehci.c
index 9951aa5da..2b25eee9d 100644
--- a/src/portable/ehci/ehci.c
+++ b/src/portable/ehci/ehci.c
@@ -58,7 +58,8 @@
 
 #define FRAMELIST_SIZE                  (1024 >> FRAMELIST_SIZE_BIT_VALUE)
 
-#define QHD_MAX      (CFG_TUH_DEVICE_MAX*CFG_TUH_ENDPOINT_MAX)
+// Total queue head pool. TODO should be user configurable and more optimize memory usage in the future
+#define QHD_MAX      (CFG_TUH_DEVICE_MAX*CFG_TUH_ENDPOINT_MAX + CFG_TUH_HUB)
 #define QTD_MAX      QHD_MAX
 
 typedef struct
@@ -79,7 +80,8 @@ typedef struct
   ehci_qhd_t qhd_pool[QHD_MAX];
   ehci_qtd_t qtd_pool[QTD_MAX] TU_ATTR_ALIGNED(32);
 
-  ehci_registers_t* regs;
+  ehci_registers_t* regs;         // operational register
+  ehci_cap_registers_t* cap_regs; // capability register
 
   volatile uint32_t uframe_number;
 }ehci_data_t;
@@ -88,6 +90,40 @@ typedef struct
 CFG_TUH_MEM_SECTION TU_ATTR_ALIGNED(4096) static ehci_data_t ehci_data;
 
 //--------------------------------------------------------------------+
+// Debug
+//--------------------------------------------------------------------+
+#if CFG_TUSB_DEBUG >= (EHCI_DBG + 1)
+static inline void print_portsc(ehci_registers_t* regs) {
+  TU_LOG_HEX(EHCI_DBG, regs->portsc);
+  TU_LOG(EHCI_DBG, "  Connect Status : %u\r\n", regs->portsc_bm.current_connect_status);
+  TU_LOG(EHCI_DBG, "  Connect Change : %u\r\n", regs->portsc_bm.connect_status_change);
+  TU_LOG(EHCI_DBG, "  Enabled        : %u\r\n", regs->portsc_bm.port_enabled);
+  TU_LOG(EHCI_DBG, "  Enabled Change : %u\r\n", regs->portsc_bm.port_enable_change);
+
+  TU_LOG(EHCI_DBG, "  OverCurr Change: %u\r\n", regs->portsc_bm.over_current_change);
+  TU_LOG(EHCI_DBG, "  Force Resume   : %u\r\n", regs->portsc_bm.force_port_resume);
+  TU_LOG(EHCI_DBG, "  Suspend        : %u\r\n", regs->portsc_bm.suspend);
+  TU_LOG(EHCI_DBG, "  Reset          : %u\r\n", regs->portsc_bm.port_reset);
+  TU_LOG(EHCI_DBG, "  Power          : %u\r\n", regs->portsc_bm.port_power);
+}
+
+static inline void print_intr(uint32_t intr) {
+  TU_LOG_HEX(EHCI_DBG, intr);
+  TU_LOG(EHCI_DBG, "  USB Interrupt      : %u\r\n", (intr & EHCI_INT_MASK_USB) ? 1 : 0);
+  TU_LOG(EHCI_DBG, "  USB Error          : %u\r\n", (intr & EHCI_INT_MASK_ERROR) ? 1 : 0);
+  TU_LOG(EHCI_DBG, "  Port Change Detect : %u\r\n", (intr & EHCI_INT_MASK_PORT_CHANGE) ? 1 : 0);
+  TU_LOG(EHCI_DBG, "  Frame List Rollover: %u\r\n", (intr & EHCI_INT_MASK_FRAMELIST_ROLLOVER) ? 1 : 0);
+  TU_LOG(EHCI_DBG, "  Host System Error  : %u\r\n", (intr & EHCI_INT_MASK_PCI_HOST_SYSTEM_ERROR) ? 1 : 0);
+  TU_LOG(EHCI_DBG, "  Async Advance      : %u\r\n", (intr & EHCI_INT_MASK_ASYNC_ADVANCE) ? 1 : 0);
+//  TU_LOG(EHCI_DBG, "  Interrupt on Async: %u\r\n", (intr & EHCI_INT_MASK_NXP_ASYNC));
+//  TU_LOG(EHCI_DBG, "  Periodic Schedule : %u\r\n", (intr & EHCI_INT_MASK_NXP_PERIODIC));
+}
+
+#else
+#define print_portsc(_reg)
+#endif
+
+//--------------------------------------------------------------------+
 // PROTOTYPE
 //--------------------------------------------------------------------+
 static inline ehci_link_t* get_period_head(uint8_t rhport, uint32_t interval_ms)
@@ -117,24 +153,26 @@ static inline ehci_qtd_t* qtd_control(uint8_t dev_addr)
 static inline ehci_qhd_t* qhd_next (ehci_qhd_t const * p_qhd);
 static inline ehci_qhd_t* qhd_find_free (void);
 static inline ehci_qhd_t* qhd_get_from_addr (uint8_t dev_addr, uint8_t ep_addr);
-
-// determine if a queue head has bus-related error
-static inline bool qhd_has_xact_error (ehci_qhd_t * p_qhd)
-{
-  return (p_qhd->qtd_overlay.buffer_err || p_qhd->qtd_overlay.babble_err || p_qhd->qtd_overlay.xact_err);
-  //p_qhd->qtd_overlay.non_hs_period_missed_uframe || p_qhd->qtd_overlay.pingstate_err TODO split transaction error
-}
-
 static void qhd_init(ehci_qhd_t *p_qhd, uint8_t dev_addr, tusb_desc_endpoint_t const * ep_desc);
+static void qhd_attach_qtd(ehci_qhd_t *qhd, ehci_qtd_t *qtd);
 
 static inline ehci_qtd_t* qtd_find_free (void);
-static inline ehci_qtd_t* qtd_next (ehci_qtd_t const * p_qtd);
-static inline void qtd_insert_to_qhd (ehci_qhd_t *p_qhd, ehci_qtd_t *p_qtd_new);
-static inline void qtd_remove_1st_from_qhd (ehci_qhd_t *p_qhd);
-static void qtd_init (ehci_qtd_t* p_qtd, void const* buffer, uint16_t total_bytes);
+static void qtd_init (ehci_qtd_t* qtd, void const* buffer, uint16_t total_bytes);
 
 static inline void list_insert (ehci_link_t *current, ehci_link_t *new, uint8_t new_type);
-static inline ehci_link_t* list_next (ehci_link_t *p_link_pointer);
+static inline ehci_link_t* list_next (ehci_link_t const *p_link);
+
+TU_ATTR_WEAK void hcd_dcache_clean(void* addr, uint32_t data_size) {
+  (void) addr; (void) data_size;
+}
+
+TU_ATTR_WEAK void hcd_dcache_invalidate(void* addr, uint32_t data_size) {
+  (void) addr; (void) data_size;
+}
+
+TU_ATTR_WEAK void hcd_dcache_clean_invalidate(void* addr, uint32_t data_size) {
+  (void) addr; (void) data_size;
+}
 
 //--------------------------------------------------------------------+
 // HCD API
@@ -152,11 +190,11 @@ void hcd_port_reset(uint8_t rhport)
 
   ehci_registers_t* regs = ehci_data.regs;
 
-//  regs->portsc_bm.port_enabled = 0; // disable port before reset
-//  regs->portsc_bm.port_reset = 1;
-
-  uint32_t portsc = regs->portsc;
+  // mask out Write-1-to-Clear bits
+  uint32_t portsc = regs->portsc & ~EHCI_PORTSC_MASK_W1C;
 
+  // EHCI Table 2-16 PortSC
+  // when software writes Port Reset bit to a one, it must also write a zero to the Port Enable bit.
   portsc &= ~(EHCI_PORTSC_MASK_PORT_EANBLED);
   portsc |= EHCI_PORTSC_MASK_PORT_RESET;
 
@@ -167,9 +205,14 @@ void hcd_port_reset_end(uint8_t rhport)
 {
   (void) rhport;
 
-#if 0
+#if 0 // TODO check if this is necessary
   ehci_registers_t* regs = ehci_data.regs;
-  regs->portsc_bm.port_reset = 0;
+
+  // mask out all change bits since they are Write 1 to clear
+  uint32_t portsc = regs->portsc & ~EHCI_PORTSC_MASK_CHANGE_ALL;
+  portsc &= ~(EHCI_PORTSC_MASK_PORT_RESET);
+
+  regs->portsc = portsc;
 #endif
 }
 
@@ -185,24 +228,22 @@ tusb_speed_t hcd_port_speed_get(uint8_t rhport)
   return (tusb_speed_t) ehci_data.regs->portsc_bm.nxp_port_speed; // NXP specific port speed
 }
 
-static void list_remove_qhd_by_addr(ehci_link_t* list_head, uint8_t dev_addr)
-{
-  for(ehci_link_t* prev = list_head;
-      !prev->terminate && (tu_align32(prev->address) != (uint32_t) list_head) && prev != NULL;
-      prev = list_next(prev) )
-  {
-    // TODO check type for ISO iTD and siTD
-    // TODO Suppress cast-align warning
-    #pragma GCC diagnostic push
-    #pragma GCC diagnostic ignored "-Wcast-align"
-    ehci_qhd_t* qhd = (ehci_qhd_t*) list_next(prev);
-    #pragma GCC diagnostic pop
-    if ( qhd->dev_addr == dev_addr )
-    {
+static void list_remove_qhd_by_daddr(ehci_link_t* list_head, uint8_t dev_addr) {
+  ehci_link_t* prev = list_head;
+
+  while (prev && !prev->terminate) {
+    ehci_qhd_t* qhd = (ehci_qhd_t*) (uintptr_t) list_next(prev);
+
+    // done if loop back to head
+    if ( (uintptr_t) qhd == (uintptr_t) list_head) {
+      break;
+    }
+
+    if ( qhd->dev_addr == dev_addr ) {
       // TODO deactivate all TD, wait for QHD to inactive before removal
       prev->address = qhd->next.address;
 
-      // EHCI 4.8.2 link the removed qhd to async head (which always reachable by Host Controller)
+      // EHCI 4.8.2 link the removed qhd's next to async head (which always reachable by Host Controller)
       qhd->next.address = ((uint32_t) list_head) | (EHCI_QTYPE_QHD << 1);
 
       if ( qhd->int_smask )
@@ -215,44 +256,91 @@ static void list_remove_qhd_by_addr(ehci_link_t* list_head, uint8_t dev_addr)
         // mark as removing, will completely re-usable when async advance isr occurs
         qhd->removing = 1;
       }
+
+      hcd_dcache_clean(qhd, sizeof(ehci_qhd_t));
+      hcd_dcache_clean(prev, sizeof(ehci_qhd_t));
+    }else {
+      prev = list_next(prev);
     }
   }
 }
 
 // Close all opened endpoint belong to this device
-void hcd_device_close(uint8_t rhport, uint8_t dev_addr)
+void hcd_device_close(uint8_t rhport, uint8_t daddr)
 {
   // skip dev0
-  if (dev_addr == 0) return;
+  if (daddr == 0) {
+    return;
+  }
 
   // Remove from async list
-  list_remove_qhd_by_addr( (ehci_link_t*) qhd_async_head(rhport), dev_addr );
+  list_remove_qhd_by_daddr((ehci_link_t *) qhd_async_head(rhport), daddr);
 
   // Remove from all interval period list
-  for(uint8_t i = 0; i < TU_ARRAY_SIZE(ehci_data.period_head_arr); i++)
-  {
-    list_remove_qhd_by_addr( (ehci_link_t*) &ehci_data.period_head_arr[i], dev_addr);
+  for(uint8_t i = 0; i < TU_ARRAY_SIZE(ehci_data.period_head_arr); i++) {
+    list_remove_qhd_by_daddr((ehci_link_t *) &ehci_data.period_head_arr[i], daddr);
   }
 
   // Async doorbell (EHCI 4.8.2 for operational details)
   ehci_data.regs->command_bm.async_adv_doorbell = 1;
 }
 
+static void init_periodic_list(uint8_t rhport) {
+  // Build the polling interval tree with 1 ms, 2 ms, 4 ms and 8 ms (framesize) only
+  for ( uint32_t i = 0; i < TU_ARRAY_SIZE(ehci_data.period_head_arr); i++ ) {
+    ehci_data.period_head_arr[i].int_smask          = 1; // queue head in period list must have smask non-zero
+    ehci_data.period_head_arr[i].qtd_overlay.halted = 1; // dummy node, always inactive
+  }
+
+  ehci_link_t * const framelist  = ehci_data.period_framelist;
+  ehci_link_t * const period_1ms = get_period_head(rhport, 1u);
+
+  // all links --> period_head_arr[0] (1ms)
+  // 0, 2, 4, 6 etc --> period_head_arr[1] (2ms)
+  // 1, 5 --> period_head_arr[2] (4ms)
+  // 3 --> period_head_arr[3] (8ms)
+
+  // TODO EHCI_FRAMELIST_SIZE with other size than 8
+  for (uint32_t i = 0; i < FRAMELIST_SIZE; i++) {
+    framelist[i].address = (uint32_t) period_1ms;
+    framelist[i].type = EHCI_QTYPE_QHD;
+  }
+
+  for (uint32_t i = 0; i < FRAMELIST_SIZE; i += 2) {
+    list_insert(framelist + i, get_period_head(rhport, 2u), EHCI_QTYPE_QHD);
+  }
+
+  for (uint32_t i = 1; i < FRAMELIST_SIZE; i += 4) {
+    list_insert(framelist + i, get_period_head(rhport, 4u), EHCI_QTYPE_QHD);
+  }
+
+  list_insert(framelist + 3, get_period_head(rhport, 8u), EHCI_QTYPE_QHD);
+
+  period_1ms->terminate    = 1;
+}
+
 bool ehci_init(uint8_t rhport, uint32_t capability_reg, uint32_t operatial_reg)
 {
-  (void) capability_reg; // not used yet
-
   tu_memclr(&ehci_data, sizeof(ehci_data_t));
 
-  ehci_data.regs = (ehci_registers_t* ) operatial_reg;
+  ehci_data.regs = (ehci_registers_t*) operatial_reg;
+  ehci_data.cap_regs = (ehci_cap_registers_t*) capability_reg;
 
   ehci_registers_t* regs = ehci_data.regs;
 
+  // EHCI 4.1 Host Controller Initialization
+
   //------------- CTRLDSSEGMENT Register (skip) -------------//
+
   //------------- USB INT Register -------------//
-  regs->inten  = 0;                 // 1. disable all the interrupt
-  regs->status = EHCI_INT_MASK_ALL; // 2. clear all status
 
+  // disable all the interrupt
+  regs->inten  = 0;
+
+  // clear all status except port change since device maybe connected before this driver is initialized
+  regs->status = (EHCI_INT_MASK_ALL & ~EHCI_INT_MASK_PORT_CHANGE);
+
+  // Enable interrupts
   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_FRAMELIST_ROLLOVER;
 
@@ -269,43 +357,10 @@ bool ehci_init(uint8_t rhport, uint32_t capability_reg, uint32_t operatial_reg)
   regs->async_list_addr = (uint32_t) async_head;
 
   //------------- Periodic List -------------//
-  // Build the polling interval tree with 1 ms, 2 ms, 4 ms and 8 ms (framesize) only
-  for ( uint32_t i = 0; i < TU_ARRAY_SIZE(ehci_data.period_head_arr); i++ )
-  {
-    ehci_data.period_head_arr[i].int_smask          = 1; // queue head in period list must have smask non-zero
-    ehci_data.period_head_arr[i].qtd_overlay.halted = 1; // dummy node, always inactive
-  }
-
-  ehci_link_t * const framelist  = ehci_data.period_framelist;
-  ehci_link_t * const period_1ms = get_period_head(rhport, 1u);
+  init_periodic_list(rhport);
+  regs->periodic_list_base = (uint32_t) ehci_data.period_framelist;
 
-  // all links --> period_head_arr[0] (1ms)
-  // 0, 2, 4, 6 etc --> period_head_arr[1] (2ms)
-  // 1, 5 --> period_head_arr[2] (4ms)
-  // 3 --> period_head_arr[3] (8ms)
-
-  // TODO EHCI_FRAMELIST_SIZE with other size than 8
-  for(uint32_t i=0; i<FRAMELIST_SIZE; i++)
-  {
-    framelist[i].address = (uint32_t) period_1ms;
-    framelist[i].type    = EHCI_QTYPE_QHD;
-  }
-
-  for(uint32_t i=0; i<FRAMELIST_SIZE; i+=2)
-  {
-    list_insert(framelist + i, get_period_head(rhport, 2u), EHCI_QTYPE_QHD);
-  }
-
-  for(uint32_t i=1; i<FRAMELIST_SIZE; i+=4)
-  {
-    list_insert(framelist + i, get_period_head(rhport, 4u), EHCI_QTYPE_QHD);
-  }
-
-  list_insert(framelist+3, get_period_head(rhport, 8u), EHCI_QTYPE_QHD);
-
-  period_1ms->terminate    = 1;
-
-  regs->periodic_list_base = (uint32_t) framelist;
+  hcd_dcache_clean(&ehci_data, sizeof(ehci_data_t));
 
   //------------- TT Control (NXP only) -------------//
   regs->nxp_tt_control = 0;
@@ -316,7 +371,16 @@ bool ehci_init(uint8_t rhport, uint32_t capability_reg, uint32_t operatial_reg)
                    FRAMELIST_SIZE_USBCMD_VALUE;
 
   //------------- ConfigFlag Register (skip) -------------//
-  regs->portsc_bm.port_power = 1; // enable port power
+
+  // enable port power bit in portsc. The function of this bit depends on the value of the Port
+  // Power Control (PPC) field in the HCSPARAMS register.
+  if (ehci_data.cap_regs->hcsparams_bm.port_power_control) {
+    // mask out all change bits since they are Write 1 to clear
+    uint32_t portsc = (regs->portsc & ~EHCI_PORTSC_MASK_W1C);
+    portsc |= ECHI_PORTSC_MASK_PORT_POWER;
+
+    regs->portsc = portsc;
+  }
 
   return true;
 }
@@ -347,15 +411,7 @@ bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const
   TU_ASSERT (ep_desc->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS);
 
   //------------- Prepare Queue Head -------------//
-  ehci_qhd_t * p_qhd;
-
-  if ( ep_desc->bEndpointAddress == 0 )
-  {
-    p_qhd = qhd_control(dev_addr);
-  }else
-  {
-    p_qhd = qhd_find_free();
-  }
+  ehci_qhd_t *p_qhd = (ep_desc->bEndpointAddress == 0) ? qhd_control(dev_addr) : qhd_find_free();
   TU_ASSERT(p_qhd);
 
   qhd_init(p_qhd, dev_addr, ep_desc);
@@ -389,6 +445,9 @@ bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const
   // TODO might need to disable async/period list
   list_insert(list_head, (ehci_link_t*) p_qhd, EHCI_QTYPE_QHD);
 
+  hcd_dcache_clean(p_qhd, sizeof(ehci_qhd_t));
+  hcd_dcache_clean(list_head, sizeof(ehci_qhd_t));
+
   return true;
 }
 
@@ -400,16 +459,12 @@ bool hcd_setup_send(uint8_t rhport, uint8_t dev_addr, uint8_t const setup_packet
   ehci_qtd_t* td  = &ehci_data.control[dev_addr].qtd;
 
   qtd_init(td, setup_packet, 8);
-  td->pid          = EHCI_PID_SETUP;
-  td->int_on_complete = 1;
-  td->next.terminate  = 1;
+  td->pid = EHCI_PID_SETUP;
 
-  // sw region
-  qhd->p_qtd_list_head = td;
-  qhd->p_qtd_list_tail = td;
+  hcd_dcache_clean((void *) setup_packet, 8);
 
-  // attach TD
-  qhd->qtd_overlay.next.address = (uint32_t) td;
+  // attach TD to QHD -> start transferring
+  qhd_attach_qtd(qhd, td);
 
   return true;
 }
@@ -421,50 +476,45 @@ bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t *
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir   = tu_edpt_dir(ep_addr);
 
-  if ( epnum == 0 )
-  {
-    ehci_qhd_t* qhd = qhd_control(dev_addr);
-    ehci_qtd_t* qtd = qtd_control(dev_addr);
+  ehci_qhd_t* qhd;
+  ehci_qtd_t* qtd;
+
+  if (epnum == 0) {
+    qhd = qhd_control(dev_addr);
+    qtd = qtd_control(dev_addr);
 
     qtd_init(qtd, buffer, buflen);
 
-    // first first data toggle is always 1 (data & setup stage)
+    // first data toggle is always 1 (data & setup stage)
     qtd->data_toggle = 1;
     qtd->pid = dir ? EHCI_PID_IN : EHCI_PID_OUT;
-    qtd->int_on_complete = 1;
-    qtd->next.terminate  = 1;
-
-    // sw region
-    qhd->p_qtd_list_head = qtd;
-    qhd->p_qtd_list_tail = qtd;
+  } else {
+    qhd = qhd_get_from_addr(dev_addr, ep_addr);
+    qtd = qtd_find_free();
+    TU_ASSERT(qtd);
 
-    // attach TD
-    qhd->qtd_overlay.next.address = (uint32_t) qtd;
-  }else
-  {
-    ehci_qhd_t *p_qhd = qhd_get_from_addr(dev_addr, ep_addr);
-    ehci_qtd_t *p_qtd = qtd_find_free();
-    TU_ASSERT(p_qtd);
-
-    qtd_init(p_qtd, buffer, buflen);
-    p_qtd->pid = p_qhd->pid;
-
-    // Insert TD to QH
-    qtd_insert_to_qhd(p_qhd, p_qtd);
-
-    p_qhd->p_qtd_list_tail->int_on_complete = 1;
+    qtd_init(qtd, buffer, buflen);
+    qtd->pid = qhd->pid;
+  }
 
-    // attach head QTD to QHD start transferring
-    p_qhd->qtd_overlay.next.address = (uint32_t) p_qhd->p_qtd_list_head;
+  // IN transfer: invalidate buffer, OUT transfer: clean buffer
+  if (dir) {
+    hcd_dcache_invalidate(buffer, buflen);
+  }else {
+    hcd_dcache_clean(buffer, buflen);
   }
 
+  // attach TD to QHD -> start transferring
+  qhd_attach_qtd(qhd, qtd);
+
   return true;
 }
 
-bool hcd_edpt_clear_stall(uint8_t dev_addr, uint8_t ep_addr)
+bool hcd_edpt_clear_stall(uint8_t daddr, uint8_t ep_addr)
 {
-  ehci_qhd_t *p_qhd = qhd_get_from_addr(dev_addr, ep_addr);
-  p_qhd->qtd_overlay.halted = 0;
+  ehci_qhd_t *qhd = qhd_get_from_addr(daddr, ep_addr);
+  qhd->qtd_overlay.halted = 0;
+  hcd_dcache_clean_invalidate(qhd, sizeof(ehci_qhd_t));
   // TODO reset data toggle ?
   return true;
 }
@@ -476,22 +526,22 @@ bool hcd_edpt_clear_stall(uint8_t dev_addr, uint8_t ep_addr)
 // async_advance is handshake between usb stack & ehci controller.
 // This isr mean it is safe to modify previously removed queue head from async list.
 // In tinyusb, queue head is only removed when device is unplugged.
-static void async_advance_isr(uint8_t rhport)
+TU_ATTR_ALWAYS_INLINE static inline
+void async_advance_isr(uint8_t rhport)
 {
   (void) rhport;
 
-  ehci_qhd_t* qhd_pool = ehci_data.qhd_pool;
-  for(uint32_t i = 0; i < QHD_MAX; i++)
-  {
-    if ( qhd_pool[i].removing )
-    {
+  ehci_qhd_t *qhd_pool = ehci_data.qhd_pool;
+  for (uint32_t i = 0; i < QHD_MAX; i++) {
+    if (qhd_pool[i].removing) {
       qhd_pool[i].removing = 0;
-      qhd_pool[i].used     = 0;
+      qhd_pool[i].used = 0;
     }
   }
 }
 
-static void port_connect_status_change_isr(uint8_t rhport)
+TU_ATTR_ALWAYS_INLINE static inline
+void port_connect_status_change_isr(uint8_t rhport)
 {
   // NOTE There is an sequence plug->unplug->…..-> plug if device is powering with pre-plugged device
   if (ehci_data.regs->portsc_bm.current_connect_status)
@@ -504,132 +554,169 @@ static void port_connect_status_change_isr(uint8_t rhport)
   }
 }
 
-static void qhd_xfer_complete_isr(ehci_qhd_t * p_qhd)
-{
-  // free all TDs from the head td to the first active TD
-  while(p_qhd->p_qtd_list_head != NULL && !p_qhd->p_qtd_list_head->active)
-  {
-    ehci_qtd_t * volatile qtd = (ehci_qtd_t * volatile) p_qhd->p_qtd_list_head;
-    bool const is_ioc = (qtd->int_on_complete != 0);
-    uint8_t const ep_addr = tu_edpt_addr(p_qhd->ep_number, qtd->pid == EHCI_PID_IN ? 1 : 0);
+TU_ATTR_ALWAYS_INLINE static inline
+void qhd_xfer_complete_isr(ehci_qhd_t * qhd) {
+  // examine TD attached to queue head
+  ehci_qtd_t * volatile qtd = (ehci_qtd_t * volatile) qhd->attached_qtd;
+  if (qtd == NULL) return; // no TD attached
+  hcd_dcache_invalidate(qtd, sizeof(ehci_qtd_t));
 
-    p_qhd->total_xferred_bytes += qtd->expected_bytes - qtd->total_bytes;
+  // TD is still active, no need to process
+  if (qtd->active) {
+    return;
+  }
 
-    // TD need to be freed and removed from qhd, before invoking callback
-    qtd->used = 0; // free QTD
-    qtd_remove_1st_from_qhd(p_qhd);
+  uint8_t dir = (qtd->pid == EHCI_PID_IN) ? 1 : 0;
+  uint32_t const xferred_bytes = qtd->expected_bytes - qtd->total_bytes;
 
-    if (is_ioc)
-    {
-      hcd_event_xfer_complete(p_qhd->dev_addr, ep_addr, p_qhd->total_xferred_bytes, XFER_RESULT_SUCCESS, true);
-      p_qhd->total_xferred_bytes = 0;
-    }
+  // invalidate dcache if IN transfer
+  if (dir == 1 && qhd->attached_buffer != 0 && xferred_bytes > 0) {
+    hcd_dcache_invalidate((void*) qhd->attached_buffer, xferred_bytes);
   }
+
+  // remove and free TD before invoking callback
+  qhd->attached_qtd = NULL;
+  qhd->attached_buffer = 0;
+  qtd->used = 0; // free QTD
+
+  // notify usbh
+  uint8_t const ep_addr = tu_edpt_addr(qhd->ep_number, dir);
+  hcd_event_xfer_complete(qhd->dev_addr, ep_addr, xferred_bytes, XFER_RESULT_SUCCESS, true);
 }
 
-static void async_list_xfer_complete_isr(ehci_qhd_t * const async_head)
+TU_ATTR_ALWAYS_INLINE static inline
+void async_list_xfer_complete_isr(ehci_qhd_t * const async_head)
 {
   ehci_qhd_t *p_qhd = async_head;
   do
   {
-    if ( !p_qhd->qtd_overlay.halted ) // halted or error is processed in error isr
-    {
+    hcd_dcache_invalidate(p_qhd, sizeof(ehci_qhd_t));
+
+    // halted or error is processed in error isr
+    if ( !p_qhd->qtd_overlay.halted ) {
       qhd_xfer_complete_isr(p_qhd);
     }
+
     p_qhd = qhd_next(p_qhd);
   }while(p_qhd != async_head); // async list traversal, stop if loop around
 }
 
-static void period_list_xfer_complete_isr(uint8_t hostid, uint32_t interval_ms)
+TU_ATTR_ALWAYS_INLINE static inline
+void period_list_xfer_complete_isr(uint8_t rhport, uint32_t interval_ms)
 {
-  uint16_t max_loop = 0;
-  uint32_t const period_1ms_addr = (uint32_t) get_period_head(hostid, 1u);
-  ehci_link_t next_item = * get_period_head(hostid, interval_ms);
+  uint32_t const period_1ms_addr = (uint32_t) get_period_head(rhport, 1u);
+  ehci_link_t next_link = * get_period_head(rhport, interval_ms);
 
-  // TODO abstract max loop guard for period
-  while( !next_item.terminate &&
-      !(interval_ms > 1 && period_1ms_addr == tu_align32(next_item.address)) &&
-      max_loop < (QHD_MAX + EHCI_MAX_ITD + EHCI_MAX_SITD)*CFG_TUH_DEVICE_MAX)
-  {
-    switch ( next_item.type )
-    {
-      case EHCI_QTYPE_QHD:
-      {
-        ehci_qhd_t *p_qhd_int = (ehci_qhd_t *) tu_align32(next_item.address);
-        if ( !p_qhd_int->qtd_overlay.halted )
-        {
-          qhd_xfer_complete_isr(p_qhd_int);
+  while (!next_link.terminate) {
+    if (interval_ms > 1 && period_1ms_addr == tu_align32(next_link.address)) {
+      // 1ms period list is end of list for all larger interval
+      break;
+    }
+
+    uintptr_t const entry_addr = tu_align32(next_link.address);
+
+    switch (next_link.type) {
+      case EHCI_QTYPE_QHD: {
+        ehci_qhd_t *qhd = (ehci_qhd_t *) entry_addr;
+        hcd_dcache_invalidate(qhd, sizeof(ehci_qhd_t));
+
+        if (!qhd->qtd_overlay.halted) {
+          qhd_xfer_complete_isr(qhd);
         }
       }
-      break;
+        break;
+
+      case EHCI_QTYPE_ITD:
+        // TODO support hs ISO
+        break;
 
-      case EHCI_QTYPE_ITD: // TODO support hs/fs ISO
       case EHCI_QTYPE_SITD:
-      case EHCI_QTYPE_FSTN:
+        // TODO support split ISO
+        break;
 
-      default: break;
+      case EHCI_QTYPE_FSTN:
+      default:
+        break;
     }
 
-    next_item = *list_next(&next_item);
-    max_loop++;
+    next_link = *list_next(&next_link);
   }
 }
 
-static void qhd_xfer_error_isr(ehci_qhd_t * p_qhd)
+// TODO merge with qhd_xfer_complete_isr()
+TU_ATTR_ALWAYS_INLINE static inline
+void qhd_xfer_error_isr(ehci_qhd_t * qhd)
 {
-  if ( (p_qhd->dev_addr != 0 && p_qhd->qtd_overlay.halted) || // addr0 cannot be protocol STALL
-        qhd_has_xact_error(p_qhd) )
-  {
-    // current qhd has error in transaction
-    xfer_result_t error_event;
+  volatile ehci_qtd_t *qtd_overlay = &qhd->qtd_overlay;
 
-    // no error bits are set, endpoint is halted due to STALL
-    error_event = qhd_has_xact_error(p_qhd) ? XFER_RESULT_FAILED : XFER_RESULT_STALLED;
+  // TD has error
+  if (qtd_overlay->halted) {
+    xfer_result_t xfer_result;
 
-    p_qhd->total_xferred_bytes += p_qhd->p_qtd_list_head->expected_bytes - p_qhd->p_qtd_list_head->total_bytes;
+    if (qtd_overlay->xact_err || qtd_overlay->err_count == 0 || qtd_overlay->buffer_err || qtd_overlay->babble_err) {
+      // Error count = 0 often occurs when device disconnected, or other bus-related error
+      xfer_result = XFER_RESULT_FAILED;
+    }else {
+      // no error bits are set, endpoint is halted due to STALL
+      xfer_result = XFER_RESULT_STALLED;
+    }
 
-//    if ( XFER_RESULT_FAILED == error_event )    TU_BREAKPOINT(); // TODO skip unplugged device
+//    if (XFER_RESULT_FAILED == xfer_result ) {
+//      TU_LOG1("  QHD xfer err count: %d\n", qtd_overlay->err_count);
+//      TU_BREAKPOINT(); // TODO skip unplugged device
+//    }
 
-    p_qhd->p_qtd_list_head->used = 0; // free QTD
-    qtd_remove_1st_from_qhd(p_qhd);
+    ehci_qtd_t * volatile qtd = (ehci_qtd_t * volatile) qhd->attached_qtd;
+    TU_ASSERT(qtd, ); // No TD yet, probably a race condition or cache issue !?
 
-    if ( 0 == p_qhd->ep_number )
-    {
-      // control cannot be halted --> clear all qtd list
-      p_qhd->p_qtd_list_head = NULL;
-      p_qhd->p_qtd_list_tail = NULL;
+    hcd_dcache_invalidate(qtd, sizeof(ehci_qtd_t));
 
-      p_qhd->qtd_overlay.next.terminate      = 1;
-      p_qhd->qtd_overlay.alternate.terminate = 1;
-      p_qhd->qtd_overlay.halted              = 0;
+    uint8_t dir = (qtd->pid == EHCI_PID_IN) ? 1 : 0;
+    uint32_t const xferred_bytes = qtd->expected_bytes - qtd->total_bytes;
 
-      ehci_qtd_t *p_setup = qtd_control(p_qhd->dev_addr);
-      p_setup->used = 0;
+    // invalidate dcache if IN transfer
+    if (dir == 1 && qhd->attached_buffer != 0 && xferred_bytes > 0) {
+      hcd_dcache_invalidate((void*) qhd->attached_buffer, xferred_bytes);
     }
 
-    // call USBH callback
-    hcd_event_xfer_complete(p_qhd->dev_addr, tu_edpt_addr(p_qhd->ep_number, p_qhd->pid == EHCI_PID_IN ? 1 : 0), p_qhd->total_xferred_bytes, error_event, true);
+    // remove and free TD before invoking callback
+    qhd->attached_qtd = NULL;
+    qhd->attached_buffer = 0;
+    qtd->used = 0; // free QTD
+
+    if (0 == qhd->ep_number ) {
+      // control cannot be halted
+      qhd->qtd_overlay.next.terminate      = 1;
+      qhd->qtd_overlay.alternate.terminate = 1;
+      qhd->qtd_overlay.halted              = 0;
+
+      hcd_dcache_clean(qhd, sizeof(ehci_qhd_t));
+    }
 
-    p_qhd->total_xferred_bytes = 0;
+    // notify usbh
+    uint8_t const ep_addr = tu_edpt_addr(qhd->ep_number, dir);
+    hcd_event_xfer_complete(qhd->dev_addr, ep_addr, xferred_bytes, xfer_result, true);
   }
 }
 
-static void xfer_error_isr(uint8_t hostid)
+TU_ATTR_ALWAYS_INLINE static inline
+void xfer_error_isr(uint8_t rhport)
 {
   //------------- async list -------------//
-  ehci_qhd_t * const async_head = qhd_async_head(hostid);
+  ehci_qhd_t * const async_head = qhd_async_head(rhport);
   ehci_qhd_t *p_qhd = async_head;
   do
   {
+    hcd_dcache_invalidate(p_qhd, sizeof(ehci_qhd_t));
     qhd_xfer_error_isr( p_qhd );
     p_qhd = qhd_next(p_qhd);
   }while(p_qhd != async_head); // async list traversal, stop if loop around
 
   //------------- TODO refractor period list -------------//
-  uint32_t const period_1ms_addr = (uint32_t) get_period_head(hostid, 1u);
+  uint32_t const period_1ms_addr = (uint32_t) get_period_head(rhport, 1u);
   for (uint32_t interval_ms=1; interval_ms <= FRAMELIST_SIZE; interval_ms *= 2)
   {
-    ehci_link_t next_item = * get_period_head(hostid, interval_ms);
+    ehci_link_t next_item = * get_period_head(rhport, interval_ms);
 
     // TODO abstract max loop guard for period
     while( !next_item.terminate &&
@@ -640,6 +727,8 @@ static void xfer_error_isr(uint8_t hostid)
         case EHCI_QTYPE_QHD:
         {
           ehci_qhd_t *p_qhd_int = (ehci_qhd_t *) tu_align32(next_item.address);
+          hcd_dcache_invalidate(p_qhd_int, sizeof(ehci_qhd_t));
+
           qhd_xfer_error_isr(p_qhd_int);
         }
         break;
@@ -656,79 +745,67 @@ static void xfer_error_isr(uint8_t hostid)
   }
 }
 
-#if CFG_TUSB_DEBUG >= EHCI_DBG
-
-static inline void print_portsc(ehci_registers_t* regs)
-{
-  TU_LOG_HEX(EHCI_DBG, regs->portsc);
-  TU_LOG(EHCI_DBG, "  Current Connect Status: %u\r\n", regs->portsc_bm.current_connect_status);
-  TU_LOG(EHCI_DBG, "  Connect Status Change : %u\r\n", regs->portsc_bm.connect_status_change);
-  TU_LOG(EHCI_DBG, "  Port Enabled          : %u\r\n", regs->portsc_bm.port_enabled);
-  TU_LOG(EHCI_DBG, "  Port Enabled Change   : %u\r\n", regs->portsc_bm.port_enable_change);
-
-  TU_LOG(EHCI_DBG, "  Port Reset            : %u\r\n", regs->portsc_bm.port_reset);
-  TU_LOG(EHCI_DBG, "  Port Power            : %u\r\n", regs->portsc_bm.port_power);
-}
-
-#else
-
-#define print_portsc(_reg)
-
-#endif
-
 //------------- Host Controller Driver's Interrupt Handler -------------//
 void hcd_int_handler(uint8_t rhport)
 {
   ehci_registers_t* regs = ehci_data.regs;
+  uint32_t const int_status = regs->status;
 
-  uint32_t int_status = regs->status;
-  int_status &= regs->inten;
-
-  regs->status = int_status; // Acknowledge handled interrupt
-
-  if (int_status == 0) return;
+  if (int_status & EHCI_INT_MASK_HC_HALTED) {
+    // something seriously wrong, maybe forget to flush/invalidate cache
+    TU_BREAKPOINT();
+    TU_LOG1("  HC halted\n");
+    return;
+  }
 
-  if (int_status & EHCI_INT_MASK_FRAMELIST_ROLLOVER)
-  {
+  if (int_status & EHCI_INT_MASK_FRAMELIST_ROLLOVER) {
     ehci_data.uframe_number += (FRAMELIST_SIZE << 3);
+    regs->status = EHCI_INT_MASK_FRAMELIST_ROLLOVER; // Acknowledge
   }
 
-  if (int_status & EHCI_INT_MASK_PORT_CHANGE)
-  {
-    uint32_t const port_status = regs->portsc & EHCI_PORTSC_MASK_ALL;
+  if (int_status & EHCI_INT_MASK_PORT_CHANGE) {
+    // Including: Force port resume, over-current change, enable/disable change and connect status change.
+    uint32_t const port_status = regs->portsc & EHCI_PORTSC_MASK_W1C;
     print_portsc(regs);
 
-    if (regs->portsc_bm.connect_status_change)
-    {
+    if (regs->portsc_bm.connect_status_change) {
       port_connect_status_change_isr(rhport);
     }
 
     regs->portsc |= port_status; // Acknowledge change bits in portsc
+    regs->status = EHCI_INT_MASK_PORT_CHANGE; // Acknowledge
   }
 
-  if (int_status & EHCI_INT_MASK_ERROR)
-  {
+  if (int_status & EHCI_INT_MASK_ERROR) {
     xfer_error_isr(rhport);
+    regs->status = EHCI_INT_MASK_ERROR; // Acknowledge
   }
 
   //------------- some QTD/SITD/ITD with IOC set is completed -------------//
-  if (int_status & EHCI_INT_MASK_NXP_ASYNC)
-  {
-    async_list_xfer_complete_isr( qhd_async_head(rhport) );
+  if (int_status & EHCI_INT_MASK_NXP_ASYNC) {
+    async_list_xfer_complete_isr(qhd_async_head(rhport));
+    regs->status = EHCI_INT_MASK_NXP_ASYNC; // Acknowledge
   }
 
   if (int_status & EHCI_INT_MASK_NXP_PERIODIC)
   {
     for (uint32_t i=1; i <= FRAMELIST_SIZE; i *= 2)
     {
-      period_list_xfer_complete_isr( rhport, i );
+      period_list_xfer_complete_isr(rhport, i);
     }
+    regs->status = EHCI_INT_MASK_NXP_PERIODIC; // Acknowledge
+  }
+
+  if (int_status & EHCI_INT_MASK_USB) {
+    // TODO standard EHCI xfer complete
+    regs->status = EHCI_INT_MASK_USB; // Acknowledge
   }
 
   //------------- There is some removed async previously -------------//
-  if (int_status & EHCI_INT_MASK_ASYNC_ADVANCE) // need to place after EHCI_INT_MASK_NXP_ASYNC
-  {
+  // need to place after EHCI_INT_MASK_NXP_ASYNC
+  if (int_status & EHCI_INT_MASK_ASYNC_ADVANCE) {
     async_advance_isr(rhport);
+    regs->status = EHCI_INT_MASK_ASYNC_ADVANCE; // Acknowledge
   }
 }
 
@@ -769,50 +846,10 @@ static inline ehci_qhd_t* qhd_get_from_addr(uint8_t dev_addr, uint8_t ep_addr)
   return NULL;
 }
 
-//------------- TD helper -------------//
-static inline ehci_qtd_t* qtd_find_free(void)
-{
-  for (uint32_t i=0; i<QTD_MAX; i++)
-  {
-    if ( !ehci_data.qtd_pool[i].used ) return &ehci_data.qtd_pool[i];
-  }
-
-  return NULL;
-}
-
-static inline ehci_qtd_t* qtd_next(ehci_qtd_t const * p_qtd )
-{
-  return (ehci_qtd_t*) tu_align32(p_qtd->next.address);
-}
-
-static inline void qtd_remove_1st_from_qhd(ehci_qhd_t *p_qhd)
-{
-  if (p_qhd->p_qtd_list_head == p_qhd->p_qtd_list_tail) // last TD --> make it NULL
-  {
-    p_qhd->p_qtd_list_head = p_qhd->p_qtd_list_tail = NULL;
-  }else
-  {
-    p_qhd->p_qtd_list_head = qtd_next( p_qhd->p_qtd_list_head );
-  }
-}
-
-static inline void qtd_insert_to_qhd(ehci_qhd_t *p_qhd, ehci_qtd_t *p_qtd_new)
-{
-  if (p_qhd->p_qtd_list_head == NULL) // empty list
-  {
-    p_qhd->p_qtd_list_head               = p_qhd->p_qtd_list_tail = p_qtd_new;
-  }else
-  {
-    p_qhd->p_qtd_list_tail->next.address = (uint32_t) p_qtd_new;
-    p_qhd->p_qtd_list_tail               = p_qtd_new;
-  }
-}
-
 static void qhd_init(ehci_qhd_t *p_qhd, uint8_t dev_addr, tusb_desc_endpoint_t const * ep_desc)
 {
   // address 0 is used as async head, which always on the list --> cannot be cleared (ehci halted otherwise)
-  if (dev_addr != 0)
-  {
+  if (dev_addr != 0) {
     tu_memclr(p_qhd, sizeof(ehci_qhd_t));
   }
 
@@ -862,58 +899,80 @@ static void qhd_init(ehci_qhd_t *p_qhd, uint8_t dev_addr, tusb_desc_endpoint_t c
     p_qhd->int_smask = p_qhd->fl_int_cmask = 0;
   }
 
-  p_qhd->fl_hub_addr     = devtree_info.hub_addr;
-  p_qhd->fl_hub_port     = devtree_info.hub_port;
-  p_qhd->mult            = 1; // TODO not use high bandwidth/park mode yet
+  p_qhd->fl_hub_addr  = devtree_info.hub_addr;
+  p_qhd->fl_hub_port  = devtree_info.hub_port;
+  p_qhd->mult         = 1; // TODO not use high bandwidth/park mode yet
 
   //------------- HCD Management Data -------------//
-  p_qhd->used            = 1;
-  p_qhd->removing        = 0;
-  p_qhd->p_qtd_list_head = NULL;
-  p_qhd->p_qtd_list_tail = NULL;
+  p_qhd->used         = 1;
+  p_qhd->removing     = 0;
+  p_qhd->attached_qtd = NULL;
   p_qhd->pid = tu_edpt_dir(ep_desc->bEndpointAddress) ? EHCI_PID_IN : EHCI_PID_OUT; // PID for TD under this endpoint
 
   //------------- active, but no TD list -------------//
   p_qhd->qtd_overlay.halted              = 0;
   p_qhd->qtd_overlay.next.terminate      = 1;
   p_qhd->qtd_overlay.alternate.terminate = 1;
+
   if (TUSB_XFER_BULK == xfer_type && p_qhd->ep_speed == TUSB_SPEED_HIGH && p_qhd->pid == EHCI_PID_OUT)
   {
     p_qhd->qtd_overlay.ping_err = 1; // do PING for Highspeed Bulk OUT, EHCI section 4.11
   }
 }
 
-static void qtd_init(ehci_qtd_t* p_qtd, void const* buffer, uint16_t total_bytes)
-{
-  tu_memclr(p_qtd, sizeof(ehci_qtd_t));
+static void qhd_attach_qtd(ehci_qhd_t *qhd, ehci_qtd_t *qtd) {
+  qhd->attached_qtd = qtd;
+  qhd->attached_buffer = qtd->buffer[0];
 
-  p_qtd->used                = 1;
+  // clean and invalidate cache before physically write
+  hcd_dcache_clean_invalidate(qtd, sizeof(ehci_qtd_t));
+
+  qhd->qtd_overlay.next.address = (uint32_t) qtd;
+  hcd_dcache_clean_invalidate(qhd, sizeof(ehci_qhd_t));
+}
 
-  p_qtd->next.terminate      = 1; // init to null
-  p_qtd->alternate.terminate = 1; // not used, always set to terminated
-  p_qtd->active              = 1;
-  p_qtd->err_count           = 3; // TODO 3 consecutive errors tolerance
-  p_qtd->data_toggle         = 0;
-  p_qtd->total_bytes         = total_bytes;
-  p_qtd->expected_bytes      = total_bytes;
 
-  p_qtd->buffer[0] = (uint32_t) buffer;
+//------------- TD helper -------------//
+static inline ehci_qtd_t *qtd_find_free(void) {
+  for (uint32_t i = 0; i < QTD_MAX; i++) {
+    if (!ehci_data.qtd_pool[i].used) return &ehci_data.qtd_pool[i];
+  }
+  return NULL;
+}
+
+static void qtd_init(ehci_qtd_t* qtd, void const* buffer, uint16_t total_bytes)
+{
+  tu_memclr(qtd, sizeof(ehci_qtd_t));
+  qtd->used                = 1;
+
+  qtd->next.terminate      = 1; // init to null
+  qtd->alternate.terminate = 1; // not used, always set to terminated
+  qtd->active              = 1;
+  qtd->err_count           = 3; // TODO 3 consecutive errors tolerance
+  qtd->data_toggle         = 0;
+  qtd->int_on_complete     = 1;
+  qtd->total_bytes         = total_bytes;
+  qtd->expected_bytes      = total_bytes;
+
+  qtd->buffer[0] = (uint32_t) buffer;
   for(uint8_t i=1; i<5; i++)
   {
-    p_qtd->buffer[i] |= tu_align4k( p_qtd->buffer[i-1] ) + 4096;
+    qtd->buffer[i] |= tu_align4k(qtd->buffer[i - 1] ) + 4096;
   }
 }
 
 //------------- List Managing Helper -------------//
+
+// insert at head
 static inline void list_insert(ehci_link_t *current, ehci_link_t *new, uint8_t new_type)
 {
   new->address = current->address;
   current->address = ((uint32_t) new) | (new_type << 1);
 }
 
-static inline ehci_link_t* list_next(ehci_link_t *p_link_pointer)
+static inline ehci_link_t* list_next(ehci_link_t const *p_link)
 {
-  return (ehci_link_t*) tu_align32(p_link_pointer->address);
+  return (ehci_link_t*) tu_align32(p_link->address);
 }
 
 #endif
diff --git a/src/portable/ehci/ehci.h b/src/portable/ehci/ehci.h
index 36f8649be..8338fb419 100644
--- a/src/portable/ehci/ehci.h
+++ b/src/portable/ehci/ehci.h
@@ -81,6 +81,8 @@ typedef union {
 	};
 }ehci_link_t;
 
+TU_VERIFY_STATIC( sizeof(ehci_link_t) == 4, "size is not correct" );
+
 /// Queue Element Transfer Descriptor
 /// Qtd is used to declare overlay in ehci_qhd_t -> cannot be declared with TU_ATTR_ALIGNED(32)
 typedef struct
@@ -162,11 +164,12 @@ typedef struct TU_ATTR_ALIGNED(32)
 	uint8_t pid;
 	uint8_t interval_ms; // polling interval in frames (or millisecond)
 
-	uint16_t total_xferred_bytes; // number of bytes xferred until a qtd with ioc bit set
-	uint8_t reserved2[2];
+	uint8_t TU_RESERVED[4];
 
-	ehci_qtd_t * volatile p_qtd_list_head;	// head of the scheduled TD list
-	ehci_qtd_t * volatile p_qtd_list_tail;	// tail of the scheduled TD list
+  // Attached TD management, note usbh will only queue 1 TD per QHD.
+  // buffer for dcache invalidate since td's buffer is modified by HC and finding initial buffer address is not trivial
+  uint32_t attached_buffer;
+	ehci_qtd_t * volatile attached_qtd;
 } ehci_qhd_t;
 
 TU_VERIFY_STATIC( sizeof(ehci_qhd_t) == 64, "size is not correct" );
@@ -245,14 +248,6 @@ typedef struct TU_ATTR_ALIGNED(32)
 	/// Word 4-5: Buffer Pointer List
 	uint32_t buffer[2];		// buffer[1] TP: Transaction Position - T-Count: Transaction Count
 
-// 	union{
-// 		uint32_t BufferPointer1;
-// 		struct  {
-// 			volatile uint32_t TCount : 3;
-// 			volatile uint32_t TPosition : 2;
-// 		};
-// 	};
-
 	/*---------- Word 6 ----------*/
 	ehci_link_t back;
 
@@ -267,16 +262,22 @@ TU_VERIFY_STATIC( sizeof(ehci_sitd_t) == 32, "size is not correct" );
 //--------------------------------------------------------------------+
 // EHCI Operational Register
 //--------------------------------------------------------------------+
-enum ehci_interrupt_mask_{
+enum {
+  // Bit 0-5 has maskable in interrupt enabled register
   EHCI_INT_MASK_USB                   = TU_BIT(0),
   EHCI_INT_MASK_ERROR                 = TU_BIT(1),
   EHCI_INT_MASK_PORT_CHANGE           = TU_BIT(2),
-
   EHCI_INT_MASK_FRAMELIST_ROLLOVER    = TU_BIT(3),
   EHCI_INT_MASK_PCI_HOST_SYSTEM_ERROR = TU_BIT(4),
   EHCI_INT_MASK_ASYNC_ADVANCE         = TU_BIT(5),
+
   EHCI_INT_MASK_NXP_SOF               = TU_BIT(7),
 
+  EHCI_INT_MASK_HC_HALTED             = TU_BIT(12),
+  EHCI_INT_MASK_RECLAIMATION          = TU_BIT(13),
+  EHCI_INT_MASK_PERIODIC_SCHED_STATUS = TU_BIT(14),
+  EHCI_INT_MASK_ASYNC_SCHED_STATUS    = TU_BIT(15),
+
   EHCI_INT_MASK_NXP_ASYNC             = TU_BIT(18),
   EHCI_INT_MASK_NXP_PERIODIC          = TU_BIT(19),
 
@@ -287,7 +288,7 @@ enum ehci_interrupt_mask_{
       EHCI_INT_MASK_NXP_ASYNC | EHCI_INT_MASK_NXP_PERIODIC
 };
 
-enum ehci_usbcmd_pos_ {
+enum {
   EHCI_USBCMD_POS_RUN_STOP               = 0,
   EHCI_USBCMD_POS_FRAMELIST_SIZE         = 2,
   EHCI_USBCMD_POS_PERIOD_ENABLE          = 4,
@@ -296,24 +297,27 @@ enum ehci_usbcmd_pos_ {
   EHCI_USBCMD_POS_INTERRUPT_THRESHOLD    = 16
 };
 
-enum ehci_portsc_change_mask_{
+enum {
   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_PORT_ENABLE_CHANGE     = TU_BIT(3),
   EHCI_PORTSC_MASK_OVER_CURRENT_CHANGE    = TU_BIT(5),
+  EHCI_PORTSC_MASK_FORCE_RESUME           = TU_BIT(6),
+  EHCI_PORTSC_MASK_PORT_SUSPEND           = TU_BIT(7),
   EHCI_PORTSC_MASK_PORT_RESET             = TU_BIT(8),
+  ECHI_PORTSC_MASK_PORT_POWER             = TU_BIT(12),
 
-  EHCI_PORTSC_MASK_ALL =
-      EHCI_PORTSC_MASK_CONNECT_STATUS_CHANGE |
-      EHCI_PORTSC_MASK_PORT_ENABLE_CHAGNE |
-      EHCI_PORTSC_MASK_OVER_CURRENT_CHANGE
+  EHCI_PORTSC_MASK_W1C =
+    EHCI_PORTSC_MASK_CONNECT_STATUS_CHANGE |
+    EHCI_PORTSC_MASK_PORT_ENABLE_CHANGE |
+    EHCI_PORTSC_MASK_OVER_CURRENT_CHANGE
 };
 
 typedef volatile struct
 {
   union {
-    uint32_t command;
+    uint32_t command; // 0x00
 
     struct {
       uint32_t run_stop               : 1 ; ///< 1=Run. 0=Stop
@@ -333,7 +337,7 @@ typedef volatile struct
   };
 
   union {
-    uint32_t status;
+    uint32_t status; // 0x04
 
     struct {
       uint32_t usb                   : 1  ; ///< qTD with IOC is retired
@@ -357,7 +361,7 @@ typedef volatile struct
   };
 
   union{
-    uint32_t inten;
+    uint32_t inten; // 0x08
 
     struct {
       uint32_t usb                   : 1  ;
@@ -375,43 +379,87 @@ typedef volatile struct
     }inten_bm;
   };
 
-  uint32_t frame_index        ; ///< Micro frame counter
-  uint32_t ctrl_ds_seg        ; ///< Control Data Structure Segment
-  uint32_t periodic_list_base ; ///< Beginning address of perodic frame list
-  uint32_t async_list_addr    ; ///< Address of next async QHD to be executed
+  uint32_t frame_index        ; ///< 0x0C Micro frame counter
+  uint32_t ctrl_ds_seg        ; ///< 0x10 Control Data Structure Segment
+  uint32_t periodic_list_base ; ///< 0x14 Beginning address of perodic frame list
+  uint32_t async_list_addr    ; ///< 0x18 Address of next async QHD to be executed
   uint32_t nxp_tt_control     ; ///< nxp embedded transaction translator (reserved by EHCI specs)
   uint32_t reserved[8]        ;
-  uint32_t config_flag        ; ///< not used by NXP
+  uint32_t config_flag        ; ///< 0x40 not used by NXP
 
   union {
-    uint32_t portsc             ; ///< port status and control
-    struct {
-      uint32_t current_connect_status      : 1; ///< 0: No device, 1: Device is present on port
-      uint32_t connect_status_change       : 1; ///< Change in Current Connect Status
-      uint32_t port_enabled                : 1; ///< Ports can only be enabled by HC as a part of the reset and enable. SW can write 0 to disable
-      uint32_t port_enable_change          : 1; ///< Port Enabled has changed
-      uint32_t over_current_active         : 1; ///< Port has an over-current condition
-      uint32_t over_current_change         : 1; ///< Change to Over-current Active
-      uint32_t force_port_resume           : 1; ///< Resume detected/driven on port. This functionality defined for manipulating this bit depends on the value of the Suspend bit.
-      uint32_t suspend                     : 1; ///< Port in suspend state
-      uint32_t port_reset                  : 1; ///< 1=Port is in Reset. 0=Port is not in Reset
-      uint32_t nxp_highspeed_status        : 1; ///< NXP customized: 0=connected to the port is not in High-speed mode, 1=connected to the port is in High-speed mode
-      uint32_t line_status                 : 2; ///< D+/D- state: 00: SE0, 10: J-state, 01: K-state
-      uint32_t port_power                  : 1; ///< 0= power off, 1= power on
-      uint32_t port_owner                  : 1; ///< not used by NXP
-      uint32_t port_indicator_control      : 2; ///< 00b: off, 01b: Amber, 10b: green, 11b: undefined
-      uint32_t port_test_control           : 4; ///< Port test mode, not used by tinyusb
-      uint32_t wake_on_connect_enable      : 1; ///< Enables device connects as wake-up events
-      uint32_t wake_on_disconnect_enable   : 1; ///< Enables device disconnects as wake-up events
-      uint32_t wake_on_over_current_enable : 1; ///< Enables over-current conditions as wake-up events
-      uint32_t nxp_phy_clock_disable       : 1; ///< NXP customized: the PHY can be put into Low Power Suspend – Clock Disable when the downstream device has been put into suspend mode or when no downstream device is connected. Low power suspend is completely under the control of software. 0: enable PHY clock, 1: disable PHY clock
-      uint32_t nxp_port_force_fullspeed    : 1; ///< NXP customized: Writing this bit to a 1 will force the port to only connect at Full Speed. It disables the chirp sequence that allowsthe port to identify itself as High Speed. This is useful for testing FS configurations with a HS host, hub or device.
-      uint32_t TU_RESERVED                 : 1;
-      uint32_t nxp_port_speed              : 2; ///< NXP customized: This register field indicates the speed atwhich the port is operating. For HS mode operation in the host controllerand HS/FS operation in the device controller the port routing steers data to the Protocol engine. For FS and LS mode operation in the host controller, the port routing steers data to the Protocol Engine w/ Embedded Transaction Translator. 0x0: Fullspeed, 0x1: Lowspeed, 0x2: Highspeed
+    // mixed with RW and R/WC bits, care should be taken when writing to this register
+    uint32_t portsc           ; ///< 0x44 port status and control
+    const struct {
+      uint32_t current_connect_status      : 1; ///< 00: 0: No device, 1: Device is present on port
+      uint32_t connect_status_change       : 1; ///< 01: [R/WC] Change in Current Connect Status
+      uint32_t port_enabled                : 1; ///< 02: Ports can only be enabled by HC as a part of the reset and enable. SW can write 0 to disable
+      uint32_t port_enable_change          : 1; ///< 03: [R/WC] Port Enabled has changed
+      uint32_t over_current_active         : 1; ///< 04: Port has an over-current condition
+      uint32_t over_current_change         : 1; ///< 05: [R/WC] Change to Over-current Active
+      uint32_t force_port_resume           : 1; ///< 06: Resume detected/driven on port. This functionality defined for manipulating this bit depends on the value of the Suspend bit.
+      uint32_t suspend                     : 1; ///< 07: Port in suspend state
+      uint32_t port_reset                  : 1; ///< 08: 1=Port is in Reset. 0=Port is not in Reset
+      uint32_t nxp_highspeed_status        : 1; ///< 09: NXP customized: 0=connected to the port is not in High-speed mode, 1=connected to the port is in High-speed mode
+      uint32_t line_status                 : 2; ///< 10-11: D+/D- state: 00: SE0, 10: J-state, 01: K-state
+      uint32_t port_power                  : 1; ///< 12: 0= power off, 1= power on
+      uint32_t port_owner                  : 1; ///< 13: not used by NXP
+      uint32_t port_indicator_control      : 2; ///< 14-15: 00b: off, 01b: Amber, 10b: green, 11b: undefined
+      uint32_t port_test_control           : 4; ///< 16-19: Port test mode, not used by tinyusb
+      uint32_t wake_on_connect_enable      : 1; ///< 20: Enables device connects as wake-up events
+      uint32_t wake_on_disconnect_enable   : 1; ///< 21: Enables device disconnects as wake-up events
+      uint32_t wake_on_over_current_enable : 1; ///< 22: Enables over-current conditions as wake-up events
+      uint32_t nxp_phy_clock_disable       : 1; ///< 23: NXP customized: the PHY can be put into Low Power Suspend – Clock Disable when the downstream device has been put into suspend mode or when no downstream device is connected. Low power suspend is completely under the control of software. 0: enable PHY clock, 1: disable PHY clock
+      uint32_t nxp_port_force_fullspeed    : 1; ///< 24: NXP customized: Writing this bit to a 1 will force the port to only connect at Full Speed. It disables the chirp sequence that allowsthe port to identify itself as High Speed. This is useful for testing FS configurations with a HS host, hub or device.
+      uint32_t TU_RESERVED                 : 1; ///< 25
+      uint32_t nxp_port_speed              : 2; ///< 26-27: NXP customized: This register field indicates the speed atwhich the port is operating. For HS mode operation in the host controllerand HS/FS operation in the device controller the port routing steers data to the Protocol engine. For FS and LS mode operation in the host controller, the port routing steers data to the Protocol Engine w/ Embedded Transaction Translator. 0x0: Fullspeed, 0x1: Lowspeed, 0x2: Highspeed
       uint32_t TU_RESERVED                 : 4;
     }portsc_bm;
   };
-}ehci_registers_t;
+} ehci_registers_t;
+
+//--------------------------------------------------------------------+
+// Capability Registers
+//--------------------------------------------------------------------+
+typedef volatile struct {
+  uint8_t caplength;   // 0x00
+  uint8_t TU_RESERVED; // 0x01
+  uint16_t hciversion; // 0x02
+
+  union {
+    uint32_t hcsparams; // 0x04
+    struct {
+      uint32_t num_ports          : 4; // [00:03]
+      uint32_t port_power_control : 1; // [04]
+      uint32_t TU_RESERVED        : 2; // [05:06]
+      uint32_t port_route_rule    : 1; // [07]
+      uint32_t n_pcc              : 4; // [08:11] Number of Ports per Companion Controller
+      uint32_t n_cc               : 4; // [12:15] Number of Companion Controllers
+      uint32_t port_ind           : 1; // [16] Port Indicators
+      uint32_t TU_RESERVED        : 3; // [17:19]
+      uint32_t n_ptt              : 4; // [20:23] ChipIdea: Number of Ports per Transaction Translator
+      uint32_t n_tt               : 4; // [24:27] ChipIdea: Number of Transaction Translators
+      uint32_t TU_RESERVED        : 4; // [28:31]
+    } hcsparams_bm;
+  };
+
+  union {
+    uint32_t hccparams; // 0x08
+    struct {
+      uint32_t addr_64bit                   : 1; // [00] 64-bit Addressing Capability
+      uint32_t programmable_frame_list_flag : 1; // [01] Programmable Frame List Flag
+      uint32_t async_park_cap               : 1; // [02] Asynchronous Schedule Park Capability
+      uint32_t TU_RESERVED                  : 1; // [03]
+      uint32_t iso_schedule_threshold       : 4; // [4:7] Isochronous Scheduling Threshold
+      uint32_t eecp                         : 8; // [8:15] EHCI Extended Capabilities Pointer
+      uint32_t TU_RESERVED                  : 16;// [16:31]
+    } hccparams_bm;
+  };
+
+  uint32_t hcsp_portroute; // 0x0C HCSP Port Route Register
+} ehci_cap_registers_t;
+
+TU_VERIFY_STATIC(sizeof(ehci_cap_registers_t) == 16, "size is not correct");
 
 #ifdef __cplusplus
  }
diff --git a/src/portable/raspberrypi/rp2040/hcd_rp2040.c b/src/portable/raspberrypi/rp2040/hcd_rp2040.c
index 661255cf6..02f9968a7 100644
--- a/src/portable/raspberrypi/rp2040/hcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/hcd_rp2040.c
@@ -562,6 +562,11 @@ bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t *
     uint32_t flags = USB_SIE_CTRL_START_TRANS_BITS | SIE_CTRL_BASE |
                      (ep_dir ? USB_SIE_CTRL_RECEIVE_DATA_BITS : USB_SIE_CTRL_SEND_DATA_BITS) |
                      (need_pre(dev_addr) ? USB_SIE_CTRL_PREAMBLE_EN_BITS : 0);
+    // START_TRANS bit on SIE_CTRL seems to exhibit the same behavior as the AVAILABLE bit
+    // described in RP2040 Datasheet, release 2.1, section "4.1.2.5.1. Concurrent access".
+    // We write everything except the START_TRANS bit first, then wait some cycles.
+    usb_hw->sie_ctrl = flags & ~USB_SIE_CTRL_START_TRANS_BITS;
+    busy_wait_at_least_cycles(12);
     usb_hw->sie_ctrl = flags;
   }else
   {
@@ -602,6 +607,11 @@ bool hcd_setup_send(uint8_t rhport, uint8_t dev_addr, uint8_t const setup_packet
   uint32_t const flags = SIE_CTRL_BASE | USB_SIE_CTRL_SEND_SETUP_BITS | USB_SIE_CTRL_START_TRANS_BITS |
                          (need_pre(dev_addr) ? USB_SIE_CTRL_PREAMBLE_EN_BITS : 0);
 
+  // START_TRANS bit on SIE_CTRL seems to exhibit the same behavior as the AVAILABLE bit
+  // described in RP2040 Datasheet, release 2.1, section "4.1.2.5.1. Concurrent access".
+  // We write everything except the START_TRANS bit first, then wait some cycles.
+  usb_hw->sie_ctrl = flags & ~USB_SIE_CTRL_START_TRANS_BITS;
+  busy_wait_at_least_cycles(12);
   usb_hw->sie_ctrl = flags;
 
   return true;
diff --git a/src/portable/renesas/rusb2/hcd_rusb2.c b/src/portable/renesas/rusb2/hcd_rusb2.c
index e4743223e..0e6fa1618 100644
--- a/src/portable/renesas/rusb2/hcd_rusb2.c
+++ b/src/portable/renesas/rusb2/hcd_rusb2.c
@@ -407,7 +407,7 @@ static void process_pipe0_bemp(uint8_t rhport)
 static void process_pipe_nrdy(uint8_t rhport, unsigned num)
 {
   (void)rhport;
-  unsigned result;
+  xfer_result_t result;
   uint16_t volatile *ctr = get_pipectr(num);
   // TU_LOG1("NRDY %d %x\n", num, *ctr);
   switch (*ctr & RUSB2_PIPE_CTR_PID_Msk) {
diff --git a/src/portable/renesas/rusb2/rusb2_ra.h b/src/portable/renesas/rusb2/rusb2_ra.h
index 5785850cc..5be9f11ce 100644
--- a/src/portable/renesas/rusb2/rusb2_ra.h
+++ b/src/portable/renesas/rusb2/rusb2_ra.h
@@ -36,6 +36,10 @@ extern "C" {
 
 #define RUSB2_REG_BASE (0x40090000)
 
+#if defined(__ICCARM__)
+  #define __builtin_ctz(x)             __iar_builtin_CLZ(__iar_builtin_RBIT(x))
+#endif
+
 TU_ATTR_ALWAYS_INLINE static inline void rusb2_int_enable(uint8_t rhport)
 {
   (void) rhport;
diff --git a/src/tusb.c b/src/tusb.c
index 85fe5a3cc..465b608b0 100644
--- a/src/tusb.c
+++ b/src/tusb.c
@@ -439,6 +439,10 @@ char const* const tu_str_std_request[] =
   "Synch Frame"
 };
 
+char const* const tu_str_xfer_result[] = {
+    "OK", "FAILED", "STALLED", "TIMEOUT"
+};
+
 #endif
 
 static void dump_str_line(uint8_t const* buf, uint16_t count)
diff --git a/src/tusb_option.h b/src/tusb_option.h
index 44d036ea4..948c9edf3 100644
--- a/src/tusb_option.h
+++ b/src/tusb_option.h
@@ -166,6 +166,10 @@
 // WCH
 #define OPT_MCU_CH32V307         2200 ///< WCH CH32V307
 
+
+// NXP LPC MCX
+#define OPT_MCU_MCXN9            2300  ///< NXP MCX N9 Series
+
 // Helper to check if configured MCU is one of listed
 // Apply _TU_CHECK_MCU with || as separator to list of input
 #define _TU_CHECK_MCU(_m)   (CFG_TUSB_MCU == _m)
@@ -274,7 +278,7 @@
 // In case TUP_MCU_STRICT_ALIGN = 1 and TUP_ARCH_STRICT_ALIGN =0, we will not reply on compiler
 // to generate unaligned access code.
 // LPC_IP3511 Highspeed cannot access unaligned memory on USB_RAM
-#if TUD_OPT_HIGH_SPEED && (CFG_TUSB_MCU == OPT_MCU_LPC54XXX || CFG_TUSB_MCU == OPT_MCU_LPC55XX)
+#if TUD_OPT_HIGH_SPEED && TU_CHECK_MCU(OPT_MCU_LPC54XXX, OPT_MCU_LPC55XX)
   #define TUP_MCU_STRICT_ALIGN   1
 #else
   #define TUP_MCU_STRICT_ALIGN   0
@@ -439,6 +443,16 @@
 #define CFG_TUH_CDC    0
 #endif
 
+#ifndef CFG_TUH_CDC_FTDI
+  // FTDI is not part of CDC class, only to re-use CDC driver API
+  #define CFG_TUH_CDC_FTDI 0
+#endif
+
+#ifndef CFG_TUH_CDC_CP210X
+  // CP210X is not part of CDC class, only to re-use CDC driver API
+  #define CFG_TUH_CDC_CP210X 0
+#endif
+
 #ifndef CFG_TUH_HID
 #define CFG_TUH_HID    0
 #endif