Merge remote-tracking branch 'mainline/master' into HEAD

Update fsp to 4.0.0

28 files changed, 1752 insertions, 1011 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index a270ea8b7..09f41c9ab 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,12 +1,16 @@
-# TODO make tinyusb as library that depends on 'tusb_config' interface that exposes the tusb_config.h file
-# This file is WIP and should not used yet
+# 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} PUBLIC
+  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
@@ -21,15 +25,80 @@ function(add_tinyusb TARGET)
     ${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
     )
 
-  # enable all possible warnings
-  target_compile_options(${TARGET} PUBLIC
-
-    )
+  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
+      )
+  elseif (CMAKE_C_COMPILER_ID STREQUAL "IAR")
 
+  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}
+  )
+
+# export target name
+# set(TINYUSB_TARGET ${TINYUSB_TARGET} PARENT_SCOPE)
+# set(TINYUSB_CONFIG_TARGET ${TINYUSB_CONFIG_TARGET} PARENT_SCOPE)
diff --git a/src/class/cdc/cdc_host.c b/src/class/cdc/cdc_host.c
index e2447d033..ce9f27c33 100644
--- a/src/class/cdc/cdc_host.c
+++ b/src/class/cdc/cdc_host.c
@@ -35,26 +35,19 @@
 
 // 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
 //--------------------------------------------------------------------+
 
-enum {
-  SERIAL_PROTOCOL_ACM = 0,
-  SERIAL_PROTOCOL_FTDI,
-  SERIAL_PROTOCOL_CP210X,
-};
-
 typedef struct {
   uint8_t daddr;
   uint8_t bInterfaceNumber;
   uint8_t bInterfaceSubClass;
   uint8_t bInterfaceProtocol;
 
-  uint8_t serial_protocol;
+  uint8_t serial_drid; // Serial Driver ID
   cdc_acm_capability_t acm_capability;
   uint8_t ep_notif;
 
@@ -76,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)
 {
@@ -130,39 +213,6 @@ static bool open_ep_stream_pair(cdch_interface_t* p_cdc , tusb_desc_endpoint_t c
 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);
 
-//------------- 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])
-};
-
-static bool ftdi_open(uint8_t daddr, const tusb_desc_interface_t *itf_desc, uint16_t max_len);
-static void process_ftdi_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 process_cp210x_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
-
-
 //--------------------------------------------------------------------+
 // APPLICATION API
 //--------------------------------------------------------------------+
@@ -322,174 +372,138 @@ static void cdch_internal_control_complete(tuh_xfer_t* xfer)
 
   if (xfer->result == XFER_RESULT_SUCCESS)
   {
-    if (p_cdc->serial_protocol == SERIAL_PROTOCOL_ACM) {
-      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);
+          default: break;
         }
-          break;
+        break;
 
-        default: break;
-      }
-    }
-    #if CFG_TUH_CDC_FTDI
-    else if (p_cdc->serial_protocol == SERIAL_PROTOCOL_FTDI) {
-      switch (xfer->setup->bRequest) {
-        case FTDI_SIO_MODEM_CTRL:
-          p_cdc->line_state = (uint8_t) (tu_le16toh(xfer->setup->wValue) & 0x00ff);
-          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;
 
-        default: break;
-      }
-    }
-    #endif
-  }
+          case FTDI_SIO_SET_BAUD_RATE:
+            // convert from divisor to baudrate is not supported
+            p_cdc->line_coding.bit_rate = _ftdi_requested_baud;
+            break;
 
-  xfer->complete_cb = p_cdc->user_control_cb;
-  xfer->complete_cb(xfer);
-}
+          default: break;
+        }
+        break;
+      #endif
 
-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_LOG_CDCH("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
-  };
+      #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;
 
-  p_cdc->user_control_cb = complete_cb;
+          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
 
-  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
-  };
+      default: break;
+    }
+  }
 
-  TU_ASSERT(tuh_control_xfer(&xfer));
-  return true;
+  xfer->complete_cb = p_cdc->user_control_cb;
+  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);
+  TU_VERIFY(p_cdc && p_cdc->serial_drid < SERIAL_DRIVER_COUNT);
+  cdch_serial_driver_t const* driver = &serial_drivers[p_cdc->serial_drid];
 
-  switch(p_cdc->serial_protocol) {
-    case SERIAL_PROTOCOL_ACM:
-      TU_VERIFY(p_cdc->acm_capability.support_line_request);
-      return acm_set_control_line_state(p_cdc, line_state, complete_cb, user_data);
+  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);
 
-    #if CFG_TUH_CDC_FTDI
-    case SERIAL_PROTOCOL_FTDI:
-      return ftdi_sio_set_modem_ctrl(p_cdc, line_state, complete_cb, user_data);
-    #endif
+    if (user_data) {
+      // user_data is not NULL, return result via user_data
+      *((xfer_result_t*) user_data) = result;
+    }
 
-    #if CFG_TUH_CDC_CP210X
-    case SERIAL_PROTOCOL_CP210X:
-      return ftdi_sio_set_modem_ctrl(p_cdc, line_state, complete_cb, user_data);
-    #endif
+    TU_VERIFY(ret && result == XFER_RESULT_SUCCESS);
 
-    default:
-      return false;
+    p_cdc->line_state = (uint8_t) line_state;
+    return true;
   }
 }
 
-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_CDCH("CDC ACM Set Line Conding\r\n");
+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];
 
-  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 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);
 
-  // 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));
+    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.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);
-
-  switch(p_cdc->serial_protocol) {
-    case SERIAL_PROTOCOL_ACM:
-      TU_VERIFY(p_cdc->acm_capability.support_line_request);
-      return acm_set_line_coding(p_cdc, line_coding, complete_cb, user_data);
-
-    #if CFG_TUH_CDC_FTDI
-    case SERIAL_PROTOCOL_FTDI:
-      // FTDI need to set baud rate and data bits, parity, stop bits separately
-      return ftdi_sio_set_baudrate(p_cdc, line_coding->bit_rate, complete_cb, user_data);
-    #endif
-
-    #if CFG_TUH_CDC_CP210X
-    case SERIAL_PROTOCOL_CP210X:
-      return cp210x_set_baudrate(p_cdc, line_coding->bit_rate, complete_cb, user_data);
-    #endif
+  // 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);
 
-    default: return false;
-  }
-}
+  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);
 
-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);
-
-  switch(p_cdc->serial_protocol) {
-    case SERIAL_PROTOCOL_ACM: {
-      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);
+    if (user_data) {
+      // user_data is not NULL, return result via user_data
+      *((xfer_result_t*) user_data) = result;
     }
 
-    #if CFG_TUH_CDC_FTDI
-    case SERIAL_PROTOCOL_FTDI:
-      // FTDI need to set baud rate and data bits, parity, stop bits separately
-      return ftdi_sio_set_baudrate(p_cdc, baudrate, complete_cb, user_data);
-    #endif
-
-    #if CFG_TUH_CDC_CP210X
-    case SERIAL_PROTOCOL_CP210X:
-      return cp210x_set_baudrate(p_cdc, baudrate, complete_cb, user_data);
-    #endif
+    TU_VERIFY(ret && result == XFER_RESULT_SUCCESS);
 
-    default: return false;
+    p_cdc->line_coding = *line_coding;
+    return true;
   }
 }
 
@@ -522,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);
 
@@ -534,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);
 
@@ -543,41 +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
-    // FTDI reserve 2 bytes for status
-    if (p_cdc->serial_protocol == SERIAL_PROTOCOL_FTDI) {
-      uint8_t status[2];
-      tu_edpt_stream_read(&p_cdc->stream.rx, status, 2);
-      (void) status; // TODO handle status
-    }
+    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);
 
     // 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);
   }
 
@@ -587,15 +601,10 @@ bool cdch_xfer_cb(uint8_t daddr, uint8_t ep_addr, xfer_result_t event, uint32_t
 //--------------------------------------------------------------------+
 // Enumeration
 //--------------------------------------------------------------------+
-enum
-{
-  // ACM
-  CONFIG_ACM_SET_CONTROL_LINE_STATE = 0,
-  CONFIG_ACM_SET_LINE_CODING,
-  CONFIG_ACM_COMPLETE,
-};
 
-static bool open_ep_stream_pair(cdch_interface_t* p_cdc , tusb_desc_endpoint_t const *desc_ep)
+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++)
   {
@@ -618,57 +627,6 @@ static bool open_ep_stream_pair(cdch_interface_t* p_cdc , tusb_desc_endpoint_t c
   return true;
 }
 
-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 = make_new_itf(daddr, itf_desc);
-  TU_VERIFY(p_cdc);
-
-  p_cdc->serial_protocol = SERIAL_PROTOCOL_ACM;
-
-  //------------- Control Interface -------------//
-  uint8_t const * p_desc = tu_desc_next(itf_desc);
-
-  // Communication Functional Descriptors
-  while( (p_desc < p_desc_end) && (TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc)) )
-  {
-    if ( CDC_FUNC_DESC_ABSTRACT_CONTROL_MANAGEMENT == cdc_functional_desc_typeof(p_desc) )
-    {
-      // save ACM bmCapabilities
-      p_cdc->acm_capability = ((cdc_desc_func_acm_t const *) p_desc)->bmCapabilities;
-    }
-
-    p_desc = tu_desc_next(p_desc);
-  }
-
-  // Open notification endpoint of control interface if any
-  if (itf_desc->bNumEndpoints == 1)
-  {
-    TU_ASSERT(TUSB_DESC_ENDPOINT == tu_desc_type(p_desc));
-    tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) p_desc;
-
-    TU_ASSERT( tuh_edpt_open(daddr, desc_ep) );
-    p_cdc->ep_notif = desc_ep->bEndpointAddress;
-
-    p_desc = tu_desc_next(p_desc);
-  }
-
-  //------------- Data Interface (if any) -------------//
-  if ( (TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) &&
-       (TUSB_CLASS_CDC_DATA == ((tusb_desc_interface_t const *) p_desc)->bInterfaceClass) )
-  {
-    // next to endpoint descriptor
-    p_desc = tu_desc_next(p_desc);
-
-    // data endpoints expected to be in pairs
-    TU_ASSERT(open_ep_stream_pair(p_cdc, (tusb_desc_endpoint_t const *) p_desc));
-  }
-
-  return true;
-}
-
-
 bool cdch_open(uint8_t rhport, uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
 {
   (void) rhport;
@@ -721,7 +679,88 @@ static void set_config_complete(cdch_interface_t * p_cdc, uint8_t idx, uint8_t i
   usbh_driver_set_config_complete(p_cdc->daddr, itf_num);
 }
 
-static void process_acm_config(tuh_xfer_t* xfer)
+
+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 = make_new_itf(daddr, itf_desc);
+  TU_VERIFY(p_cdc);
+
+  p_cdc->serial_drid = SERIAL_DRIVER_ACM;
+
+  //------------- Control Interface -------------//
+  uint8_t const * p_desc = tu_desc_next(itf_desc);
+
+  // Communication Functional Descriptors
+  while( (p_desc < p_desc_end) && (TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc)) )
+  {
+    if ( CDC_FUNC_DESC_ABSTRACT_CONTROL_MANAGEMENT == cdc_functional_desc_typeof(p_desc) )
+    {
+      // save ACM bmCapabilities
+      p_cdc->acm_capability = ((cdc_desc_func_acm_t const *) p_desc)->bmCapabilities;
+    }
+
+    p_desc = tu_desc_next(p_desc);
+  }
+
+  // Open notification endpoint of control interface if any
+  if (itf_desc->bNumEndpoints == 1)
+  {
+    TU_ASSERT(TUSB_DESC_ENDPOINT == tu_desc_type(p_desc));
+    tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+
+    TU_ASSERT( tuh_edpt_open(daddr, desc_ep) );
+    p_cdc->ep_notif = desc_ep->bEndpointAddress;
+
+    p_desc = tu_desc_next(p_desc);
+  }
+
+  //------------- Data Interface (if any) -------------//
+  if ( (TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) &&
+       (TUSB_CLASS_CDC_DATA == ((tusb_desc_interface_t const *) p_desc)->bInterfaceClass) )
+  {
+    // next to endpoint descriptor
+    p_desc = tu_desc_next(p_desc);
+
+    // data endpoints expected to be in pairs
+    TU_ASSERT(open_ep_stream_pair(p_cdc, (tusb_desc_endpoint_t const *) p_desc));
+  }
+
+  return true;
+}
+
+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);
@@ -735,72 +774,104 @@ static void process_acm_config(tuh_xfer_t* xfer)
       #if CFG_TUH_CDC_LINE_CONTROL_ON_ENUM
       if (p_cdc->acm_capability.support_line_request)
       {
-        TU_ASSERT(acm_set_control_line_state(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, process_acm_config,
-                                                 CONFIG_ACM_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_ACM_SET_LINE_CODING:
-      #ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
+        #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(acm_set_line_coding(p_cdc, &line_coding, process_acm_config, CONFIG_ACM_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_ACM_COMPLETE:
       // itf_num+1 to account for data interface as well
       set_config_complete(p_cdc, idx, itf_num+1);
-    break;
+      break;
 
     default: break;
   }
 }
 
-bool cdch_set_config(uint8_t daddr, uint8_t itf_num)
-{
-  tusb_control_request_t request;
-  request.wIndex = tu_htole16((uint16_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");
 
-  tuh_xfer_t xfer;
-  xfer.daddr  = daddr;
-  xfer.result = XFER_RESULT_SUCCESS;
-  xfer.setup  = &request;
-  xfer.user_data = 0;
+  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
+  };
 
-  // fake transfer to kick-off process
-  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->user_control_cb = complete_cb;
 
-  switch (p_cdc->serial_protocol) {
-    case SERIAL_PROTOCOL_ACM:
-      process_acm_config(&xfer);
-      break;
+  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
+  };
 
-    #if CFG_TUH_CDC_FTDI
-    case SERIAL_PROTOCOL_FTDI:
-      process_ftdi_config(&xfer);
-      break;
-    #endif
+  TU_ASSERT(tuh_control_xfer(&xfer));
+  return true;
+}
 
-    #if CFG_TUH_CDC_CP210X
-    case SERIAL_PROTOCOL_CP210X:
-      process_cp210x_config(&xfer);
-      break;
-    #endif
+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");
 
-      default: return false;
-  }
+  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
 //--------------------------------------------------------------------+
@@ -822,9 +893,9 @@ static bool ftdi_open(uint8_t daddr, const tusb_desc_interface_t *itf_desc, uint
   cdch_interface_t * p_cdc = make_new_itf(daddr, itf_desc);
   TU_VERIFY(p_cdc);
 
-  TU_LOG_CDCH("FTDI opened\r\n");
+  TU_LOG_DRV("FTDI opened\r\n");
 
-  p_cdc->serial_protocol = SERIAL_PROTOCOL_FTDI;
+  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);
@@ -866,9 +937,10 @@ static bool ftdi_sio_reset(cdch_interface_t* p_cdc, tuh_xfer_cb_t complete_cb, u
 
 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_CDCH("CDC FTDI Set Control Line State\r\n");
+  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, cdch_internal_control_complete, user_data));
+  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;
 }
 
@@ -901,14 +973,17 @@ static uint32_t ftdi_232bm_baud_to_divisor(uint32_t baud)
 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_CDCH("CDC FTDI Set BaudRate = %lu, divisor = 0x%04x\n", baudrate, divisor);
+  TU_LOG_DRV("CDC FTDI Set BaudRate = %lu, divisor = 0x%04x\n", baudrate, divisor);
 
   p_cdc->user_control_cb = complete_cb;
-  TU_ASSERT(ftdi_sio_set_request(p_cdc, FTDI_SIO_SET_BAUD_RATE, divisor, cdch_internal_control_complete, user_data));
+  _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 process_ftdi_config(tuh_xfer_t* xfer) {
+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);
@@ -918,13 +993,13 @@ static void process_ftdi_config(tuh_xfer_t* xfer) {
   switch(state) {
     // Note may need to read FTDI eeprom
     case CONFIG_FTDI_RESET:
-      TU_ASSERT(ftdi_sio_reset(p_cdc, process_ftdi_config, CONFIG_FTDI_MODEM_CTRL),);
+      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, process_ftdi_config, CONFIG_FTDI_SET_BAUDRATE),);
+        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;
@@ -933,7 +1008,7 @@ static void process_ftdi_config(tuh_xfer_t* xfer) {
     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, process_ftdi_config, CONFIG_FTDI_SET_DATA),);
+      TU_ASSERT(ftdi_sio_set_baudrate(p_cdc, line_coding.bit_rate, ftdi_process_config, CONFIG_FTDI_SET_DATA),);
       break;
       #else
       TU_ATTR_FALLTHROUGH;
@@ -985,8 +1060,8 @@ static bool cp210x_open(uint8_t daddr, tusb_desc_interface_t const *itf_desc, ui
   cdch_interface_t * p_cdc = make_new_itf(daddr, itf_desc);
   TU_VERIFY(p_cdc);
 
-  TU_LOG_CDCH("CP210x opened\r\n");
-  p_cdc->serial_protocol = SERIAL_PROTOCOL_CP210X;
+  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);
@@ -1033,19 +1108,22 @@ static bool cp210x_ifc_enable(cdch_interface_t* p_cdc, uint16_t enabled, tuh_xfe
 }
 
 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_CDCH("CDC CP210x Set BaudRate = %lu\n", baudrate);
-  baudrate = tu_htole32(baudrate);
-  return cp210x_set_request(p_cdc, CP210X_SET_BAUDRATE, 0, (uint8_t *) &baudrate, 4, complete_cb, 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_CDCH("CDC CP210x Set Control Line State\r\n");
+  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, cdch_internal_control_complete, user_data);
+  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 process_cp210x_config(tuh_xfer_t* xfer) {
+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);
@@ -1054,13 +1132,13 @@ static void process_cp210x_config(tuh_xfer_t* xfer) {
 
   switch (state) {
     case CONFIG_CP210X_IFC_ENABLE:
-      TU_ASSERT(cp210x_ifc_enable(p_cdc, 1, process_cp210x_config, CONFIG_CP210X_SET_BAUDRATE),);
+      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, process_cp210x_config, CONFIG_CP210X_SET_LINE_CTL),);
+      TU_ASSERT(cp210x_set_baudrate(p_cdc, line_coding.bit_rate, cp210x_process_config, CONFIG_CP210X_SET_LINE_CTL),);
       break;
       #else
       TU_ATTR_FALLTHROUGH;
@@ -1078,7 +1156,8 @@ static void process_cp210x_config(tuh_xfer_t* xfer) {
 
     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, process_cp210x_config, CONFIG_CP210X_COMPLETE),);
+      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;
diff --git a/src/class/cdc/cdc_host.h b/src/class/cdc/cdc_host.h
index 971ebe2ae..19552f1ee 100644
--- a/src/class/cdc/cdc_host.h
+++ b/src/class/cdc/cdc_host.h
@@ -134,7 +134,12 @@ 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)
@@ -144,6 +149,7 @@ bool tuh_cdc_set_control_line_state(uint8_t idx, uint16_t line_state, tuh_xfer_c
 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 (ACM only)
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/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_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 ba8976a8c..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
@@ -193,6 +201,11 @@
   #define TUP_USBIP_FSDEV_STM32
   #define TUP_DCD_ENDPOINT_MAX    8
 
+#elif TU_CHECK_MCU(OPT_MCU_STM32G0)
+  #define TUP_USBIP_FSDEV
+  #define TUP_USBIP_FSDEV_STM32
+  #define TUP_DCD_ENDPOINT_MAX    8
+
 #elif TU_CHECK_MCU(OPT_MCU_STM32L0, OPT_MCU_STM32L1)
   #define TUP_USBIP_FSDEV
   #define TUP_USBIP_FSDEV_STM32
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 3953d5b21..6159044ab 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);
 
@@ -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 %u\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 607926a65..ceff893bd 100644
--- a/src/portable/chipidea/ci_hs/ci_hs_imxrt.h
+++ b/src/portable/chipidea/ci_hs/ci_hs_imxrt.h
@@ -46,18 +46,44 @@ 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/renesas/rusb2/hcd_rusb2.c b/src/portable/renesas/rusb2/hcd_rusb2.c
index b97ffd7e9..3a38bef58 100644
--- a/src/portable/renesas/rusb2/hcd_rusb2.c
+++ b/src/portable/renesas/rusb2/hcd_rusb2.c
@@ -417,7 +417,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 a1c58abe5..6e150a4b0 100644
--- a/src/portable/renesas/rusb2/rusb2_ra.h
+++ b/src/portable/renesas/rusb2/rusb2_ra.h
@@ -43,6 +43,10 @@ extern IRQn_Type _usb_hs_irqn;
 #define CFG_TUSB_RHPORT1_MODE   0
 #endif
 
+#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)
 {
 #ifdef CFG_TUSB_RHPORT1_MODE
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index 98d3d0829..14cabaf8d 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -44,6 +44,7 @@
  * L0x2, L0x3                     1024 byte buffer
  * L1                              512 byte buffer
  * L4x2, L4x3                     1024 byte buffer
+ * G0                             2048 byte buffer
  *
  * To use this driver, you must:
  * - If you are using a device with crystal-less USB, set up the clock recovery system (CRS)
@@ -185,12 +186,12 @@ static void dcd_ep_ctr_handler(void);
 static uint8_t open_ep_count;
 static uint16_t ep_buf_ptr; ///< Points to first free memory location
 static void dcd_pma_alloc_reset(void);
-static uint16_t dcd_pma_alloc(uint8_t ep_addr, size_t length);
+static uint16_t dcd_pma_alloc(uint8_t ep_addr, uint16_t length);
 static void dcd_pma_free(uint8_t ep_addr);
 static void dcd_ep_free(uint8_t ep_addr);
 static uint8_t dcd_ep_alloc(uint8_t ep_addr, uint8_t ep_type);
-static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, size_t wNBytes);
-static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, size_t wNBytes);
+static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t wNBytes);
+static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t wNBytes);
 
 static bool dcd_write_packet_memory_ff(tu_fifo_t * ff, uint16_t dst, uint16_t wNBytes);
 static bool dcd_read_packet_memory_ff(tu_fifo_t * ff, uint16_t src, uint16_t wNBytes);
@@ -209,17 +210,6 @@ TU_ATTR_ALWAYS_INLINE static inline xfer_ctl_t* xfer_ctl_ptr(uint32_t ep_addr)
   return &xfer_status[epnum][dir];
 }
 
-// Using a function due to better type checks
-// This seems better than having to do type casts everywhere else
-TU_ATTR_ALWAYS_INLINE static inline void reg16_clear_bits(__IO uint16_t *reg, uint16_t mask) {
-  *reg = (uint16_t)(*reg & ~mask);
-}
-
-// Bits in ISTR are cleared upon writing 0
-TU_ATTR_ALWAYS_INLINE static inline void clear_istr_bits(uint16_t mask) {
-  USB->ISTR = ~mask;
-}
-
 //--------------------------------------------------------------------+
 // Controller API
 //--------------------------------------------------------------------+
@@ -242,7 +232,9 @@ void dcd_init (uint8_t rhport)
   {
     asm("NOP");
   }
-  reg16_clear_bits(&USB->CNTR, USB_CNTR_PDWN);// Remove powerdown
+
+  USB->CNTR &= ~USB_CNTR_PDWN;
+
   // Wait startup time, for F042 and F070, this is <= 1 us.
   for(uint32_t i = 0; i<200; i++) // should be a few us
   {
@@ -250,8 +242,9 @@ void dcd_init (uint8_t rhport)
   }
   USB->CNTR = 0; // Enable USB
 
+#ifndef STM32G0 // BTABLE register does not exist any more on STM32G0, it is fixed to USB SRAM base address
   USB->BTABLE = DCD_STM32_BTABLE_BASE;
-
+#endif
   USB->ISTR = 0; // Clear pending interrupts
 
   // Reset endpoints to disabled
@@ -312,7 +305,7 @@ void dcd_sof_enable(uint8_t rhport, bool en)
   }
   else
   {
-    USB->CNTR &= (uint16_t) ~USB_CNTR_SOFM;
+    USB->CNTR &= ~USB_CNTR_SOFM;
   }
 }
 
@@ -358,6 +351,13 @@ void dcd_int_enable (uint8_t rhport)
   NVIC_EnableIRQ(USB_LP_IRQn);
   NVIC_EnableIRQ(USBWakeUp_IRQn);
 
+#elif CFG_TUSB_MCU == OPT_MCU_STM32G0
+  #ifdef STM32G0B0xx
+    NVIC_EnableIRQ(USB_IRQn);
+  #else
+    NVIC_EnableIRQ(USB_UCPD1_2_IRQn);
+  #endif
+
 #elif CFG_TUSB_MCU == OPT_MCU_STM32WB
   NVIC_EnableIRQ(USB_HP_IRQn);
   NVIC_EnableIRQ(USB_LP_IRQn);
@@ -408,6 +408,13 @@ void dcd_int_disable(uint8_t rhport)
   NVIC_DisableIRQ(USB_LP_IRQn);
   NVIC_DisableIRQ(USBWakeUp_IRQn);
 
+#elif CFG_TUSB_MCU == OPT_MCU_STM32G0
+  #ifdef STM32G0B0xx
+    NVIC_DisableIRQ(USB_IRQn);
+  #else
+    NVIC_DisableIRQ(USB_UCPD1_2_IRQn);
+  #endif
+
 #elif CFG_TUSB_MCU == OPT_MCU_STM32WB
   NVIC_DisableIRQ(USB_HP_IRQn);
   NVIC_DisableIRQ(USB_LP_IRQn);
@@ -439,7 +446,7 @@ void dcd_remote_wakeup(uint8_t rhport)
 {
   (void) rhport;
 
-  USB->CNTR |= (uint16_t) USB_CNTR_RESUME;
+  USB->CNTR |= USB_CNTR_RESUME;
   remoteWakeCountdown = 4u; // required to be 1 to 15 ms, ESOF should trigger every 1ms.
 }
 
@@ -470,7 +477,6 @@ static void dcd_handle_bus_reset(void)
   //__IO uint16_t * const epreg = &(EPREG(0));
   USB->DADDR = 0u; // disable USB peripheral by clearing the EF flag
 
-
   for(uint32_t i=0; i<STFSDEV_EP_COUNT; i++)
   {
     // Clear all EPREG (or maybe this is automatic? I'm not sure)
@@ -540,9 +546,6 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
 
   if((ep_addr == 0U) && ((wEPRegVal & USB_EP_SETUP) != 0U)) /* Setup packet */
   {
-    // The setup_received function uses memcpy, so this must first copy the setup data into
-    // user memory, to allow for the 32-bit access that memcpy performs.
-    uint8_t userMemBuf[8];
     uint32_t count = pcd_get_ep_rx_cnt(USB, EPindex);
     /* Get SETUP Packet*/
     if(count == 8) // Setup packet should always be 8 bytes. If not, ignore it, and try again.
@@ -550,8 +553,15 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
       // Must reset EP to NAK (in case it had been stalling) (though, maybe too late here)
       pcd_set_ep_rx_status(USB,0u,USB_EP_RX_NAK);
       pcd_set_ep_tx_status(USB,0u,USB_EP_TX_NAK);
-      dcd_read_packet_memory(userMemBuf, *pcd_ep_rx_address_ptr(USB,EPindex), 8);
+#ifdef PMA_32BIT_ACCESS
+      dcd_event_setup_received(0, (uint8_t*)(USB_PMAADDR + pcd_get_ep_rx_address(USB, EPindex)), true);
+#else
+      // The setup_received function uses memcpy, so this must first copy the setup data into
+      // user memory, to allow for the 32-bit access that memcpy performs.
+      uint8_t userMemBuf[8];
+      dcd_read_packet_memory(userMemBuf, pcd_get_ep_rx_address(USB,EPindex), 8);
       dcd_event_setup_received(0, (uint8_t*)userMemBuf, true);
+#endif
     }
   }
   else
@@ -574,7 +584,7 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
 
     if (count != 0U)
     {
-      uint16_t addr = *pcd_ep_rx_address_ptr(USB, EPindex);
+      uint16_t addr = pcd_get_ep_rx_address(USB, EPindex);
 
       if (xfer->ff)
       {
@@ -657,13 +667,13 @@ void dcd_int_handler(uint8_t rhport) {
 
   /* Put SOF flag at the beginning of ISR in case to get least amount of jitter if it is used for timing purposes */
   if(int_status & USB_ISTR_SOF) {
-    clear_istr_bits(USB_ISTR_SOF);
+    USB->ISTR &=~USB_ISTR_SOF;
     dcd_event_sof(0, USB->FNR & USB_FNR_FN, true);
   }
 
   if(int_status & USB_ISTR_RESET) {
     // USBRST is start of reset.
-    clear_istr_bits(USB_ISTR_RESET);
+    USB->ISTR &=~USB_ISTR_RESET;
     dcd_handle_bus_reset();
     dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
     return; // Don't do the rest of the things here; perhaps they've been cleared?
@@ -678,9 +688,10 @@ void dcd_int_handler(uint8_t rhport) {
 
   if (int_status & USB_ISTR_WKUP)
   {
-    reg16_clear_bits(&USB->CNTR, USB_CNTR_LPMODE);
-    reg16_clear_bits(&USB->CNTR, USB_CNTR_FSUSP);
-    clear_istr_bits(USB_ISTR_WKUP);
+    USB->CNTR &= ~USB_CNTR_LPMODE;
+    USB->CNTR &= ~USB_CNTR_FSUSP;
+
+    USB->ISTR &=~USB_ISTR_WKUP;
     dcd_event_bus_signal(0, DCD_EVENT_RESUME, true);
   }
 
@@ -694,20 +705,20 @@ void dcd_int_handler(uint8_t rhport) {
     USB->CNTR |= USB_CNTR_LPMODE;
 
     /* clear of the ISTR bit must be done after setting of CNTR_FSUSP */
-    clear_istr_bits(USB_ISTR_SUSP);
+    USB->ISTR &=~USB_ISTR_SUSP;
     dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
   }
 
   if(int_status & USB_ISTR_ESOF) {
     if(remoteWakeCountdown == 1u)
     {
-      USB->CNTR &= (uint16_t)(~USB_CNTR_RESUME);
+      USB->CNTR &= ~USB_CNTR_RESUME;
     }
     if(remoteWakeCountdown > 0u)
     {
       remoteWakeCountdown--;
     }
-    clear_istr_bits(USB_ISTR_ESOF);
+    USB->ISTR &=~USB_ISTR_ESOF;
   }
 }
 
@@ -728,8 +739,8 @@ void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const * re
     uint8_t const dev_addr = (uint8_t) request->wValue;
 
     // Setting new address after the whole request is complete
-    reg16_clear_bits(&USB->DADDR, USB_DADDR_ADD);
-    USB->DADDR = (uint16_t)(USB->DADDR | dev_addr); // leave the enable bit set
+    USB->DADDR &= ~USB_DADDR_ADD;
+    USB->DADDR |= dev_addr; // leave the enable bit set
   }
 }
 
@@ -756,7 +767,7 @@ static void dcd_pma_alloc_reset(void)
  *
  * During failure, TU_ASSERT is used. If this happens, rework/reallocate memory manually.
  */
-static uint16_t dcd_pma_alloc(uint8_t ep_addr, size_t length)
+static uint16_t dcd_pma_alloc(uint8_t ep_addr, uint16_t length)
 {
   xfer_ctl_t* epXferCtl = xfer_ctl_ptr(ep_addr);
 
@@ -768,6 +779,13 @@ static uint16_t dcd_pma_alloc(uint8_t ep_addr, size_t length)
     return epXferCtl->pma_ptr;
   }
 
+  // Ensure allocated buffer is aligned
+#ifdef PMA_32BIT_ACCESS
+  length = (length + 3) & ~0x03;
+#else
+  length = (length + 1) & ~0x01;
+#endif
+
   open_ep_count++;
 
   uint16_t addr = ep_buf_ptr;
@@ -778,7 +796,7 @@ static uint16_t dcd_pma_alloc(uint8_t ep_addr, size_t length)
 
   epXferCtl->pma_ptr = addr;
   epXferCtl->pma_alloc_size = length;
-  //TU_LOG2("dcd_pma_alloc(%x,%x)=%x\r\n",ep_addr,length,addr);
+  //TU_LOG1("dcd_pma_alloc(%x,%x)=%x\r\n",ep_addr,length,addr);
 
   return addr;
 }
@@ -931,14 +949,14 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc
 
   if( (dir == TUSB_DIR_IN) || (wType == USB_EP_ISOCHRONOUS) )
   {
-    *pcd_ep_tx_address_ptr(USB, ep_idx) = pma_addr;
+    pcd_set_ep_tx_address(USB, ep_idx, pma_addr);
     pcd_set_ep_tx_bufsize(USB, ep_idx, buffer_size);
     pcd_clear_tx_dtog(USB, ep_idx);
   }
 
   if( (dir == TUSB_DIR_OUT) || (wType == USB_EP_ISOCHRONOUS) )
   {
-    *pcd_ep_rx_address_ptr(USB, ep_idx) = pma_addr;
+    pcd_set_ep_rx_address(USB, ep_idx, pma_addr);
     pcd_set_ep_rx_bufsize(USB, ep_idx, buffer_size);
     pcd_clear_rx_dtog(USB, ep_idx);
   }
@@ -1018,8 +1036,8 @@ bool dcd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet
 
   pcd_set_eptype(USB, ep_idx, USB_EP_ISOCHRONOUS);
 
-  *pcd_ep_tx_address_ptr(USB, ep_idx) = pma_addr;
-  *pcd_ep_rx_address_ptr(USB, ep_idx) = pma_addr;
+  pcd_set_ep_tx_address(USB, ep_idx, pma_addr);
+  pcd_set_ep_rx_address(USB, ep_idx, pma_addr);
 
   return true;
 }
@@ -1069,7 +1087,7 @@ static void dcd_transmit_packet(xfer_ctl_t * xfer, uint16_t ep_ix)
   }
 
   uint16_t ep_reg = pcd_get_endpoint(USB, ep_ix);
-  uint16_t addr_ptr = *pcd_ep_tx_address_ptr(USB,ep_ix);
+  uint16_t addr_ptr = pcd_get_ep_tx_address(USB, ep_ix);
 
   if (xfer->ff)
   {
@@ -1203,6 +1221,40 @@ void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
   }
 }
 
+#ifdef PMA_32BIT_ACCESS
+static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t wNBytes)
+{
+  const uint8_t* srcVal = src;
+  volatile uint32_t* dst32 = (volatile uint32_t*)(USB_PMAADDR + dst);
+
+  for (uint32_t n = wNBytes / 4; n > 0; --n) {
+    *dst32++ = tu_unaligned_read32(srcVal);
+    srcVal += 4;
+  }
+
+  wNBytes = wNBytes & 0x03;
+  if (wNBytes)
+  {
+    uint32_t wrVal = *srcVal;
+    wNBytes--;
+
+    if (wNBytes)
+    {
+      wrVal |= *++srcVal << 8;
+      wNBytes--;
+
+      if (wNBytes)
+      {
+        wrVal |= *++srcVal << 16;
+      }
+    }
+
+    *dst32 = wrVal;
+  }
+
+  return true;
+}
+#else
 // Packet buffer access can only be 8- or 16-bit.
 /**
   * @brief Copy a buffer from user memory area to packet memory area (PMA).
@@ -1214,7 +1266,7 @@ void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
   * @param   wNBytes no. of bytes to be copied.
   * @retval None
   */
-static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, size_t wNBytes)
+static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t wNBytes)
 {
   uint32_t n = (uint32_t)wNBytes >> 1U;
   uint16_t temp1, temp2;
@@ -1237,7 +1289,7 @@ static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, si
     srcVal++;
   }
 
-  if (wNBytes & 0x01)
+  if (wNBytes)
   {
     temp1 = *srcVal;
     *pdwVal = temp1;
@@ -1245,6 +1297,7 @@ static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, si
 
   return true;
 }
+#endif
 
 /**
   * @brief Copy from FIFO to packet memory area (PMA).
@@ -1263,7 +1316,37 @@ static bool dcd_write_packet_memory_ff(tu_fifo_t * ff, uint16_t dst, uint16_t wN
 
   // We want to read from the FIFO and write it into the PMA, if LIN part is ODD and has WRAPPED part,
   // last lin byte will be combined with wrapped part
-  // To ensure PMA is always access 16bit aligned (dst aligned to 16 bit)
+  // To ensure PMA is always access aligned (dst aligned to 16 or 32 bit)
+#ifdef PMA_32BIT_ACCESS
+  if((cnt_lin & 0x03) && cnt_wrap)
+  {
+    // Copy first linear part
+    dcd_write_packet_memory(dst, info.ptr_lin, cnt_lin &~0x03);
+    dst += cnt_lin &~0x03;
+
+    // Copy last linear bytes & first wrapped bytes to buffer
+    uint32_t i;
+    uint8_t tmp[4];
+    for (i = 0; i < (cnt_lin & 0x03); i++)
+    {
+      tmp[i] = ((uint8_t*)info.ptr_lin)[(cnt_lin &~0x03) + i];
+    }
+    uint32_t wCnt = cnt_wrap;
+    for (; i < 4 && wCnt > 0; i++, wCnt--)
+    {
+      tmp[i] = *(uint8_t*)info.ptr_wrap;
+      info.ptr_wrap = (uint8_t*)info.ptr_wrap + 1;
+    }
+
+    // Write unaligned buffer
+    dcd_write_packet_memory(dst, &tmp, 4);
+    dst += 4;
+
+    // Copy rest of wrapped byte
+    if (wCnt)
+        dcd_write_packet_memory(dst, info.ptr_wrap, wCnt);
+  }
+#else
   if((cnt_lin & 0x01) && cnt_wrap)
   {
     // Copy first linear part
@@ -1278,6 +1361,7 @@ static bool dcd_write_packet_memory_ff(tu_fifo_t * ff, uint16_t dst, uint16_t wN
     // Copy rest of wrapped byte
     dcd_write_packet_memory(dst, ((uint8_t*)info.ptr_wrap) + 1, cnt_wrap - 1);
   }
+#endif
   else
   {
     // Copy linear part
@@ -1296,13 +1380,47 @@ static bool dcd_write_packet_memory_ff(tu_fifo_t * ff, uint16_t dst, uint16_t wN
   return true;
 }
 
+#ifdef PMA_32BIT_ACCESS
+static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t wNBytes)
+{
+  uint8_t* dstVal = dst;
+  volatile uint32_t* src32 = (volatile uint32_t*)(USB_PMAADDR + src);
+
+  for (uint32_t n = wNBytes / 4; n > 0; --n) {
+    tu_unaligned_write32(dstVal, *src32++);
+    dstVal += 4;
+  }
+
+  wNBytes = wNBytes & 0x03;
+  if (wNBytes)
+  {
+    uint32_t rdVal = *src32;
+
+    *dstVal = tu_u32_byte0(rdVal);
+    wNBytes--;
+
+    if (wNBytes)
+    {
+      *++dstVal = tu_u32_byte1(rdVal);
+      wNBytes--;
+
+      if (wNBytes)
+      {
+        *++dstVal = tu_u32_byte2(rdVal);
+      }
+    }
+  }
+
+  return true;
+}
+#else
 /**
   * @brief Copy a buffer from packet memory area (PMA) to user memory area.
   *        Uses byte-access of system memory and 16-bit access of packet memory
   * @param   wNBytes no. of bytes to be copied.
   * @retval None
   */
-static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, size_t wNBytes)
+static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t wNBytes)
 {
   uint32_t n = (uint32_t)wNBytes >> 1U;
   // The GCC optimizer will combine access to 32-bit sizes if we let it. Force
@@ -1329,6 +1447,7 @@ static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, size_t wN
   }
   return true;
 }
+#endif
 
 /**
   * @brief Copy a buffer from user packet memory area (PMA) to FIFO.
@@ -1346,9 +1465,39 @@ static bool dcd_read_packet_memory_ff(tu_fifo_t * ff, uint16_t src, uint16_t wNB
   uint16_t cnt_lin =  TU_MIN(wNBytes, info.len_lin);
   uint16_t cnt_wrap = TU_MIN(wNBytes - cnt_lin, info.len_wrap);
 
+
   // We want to read from PMA and write it into the FIFO, if LIN part is ODD and has WRAPPED part,
   // last lin byte will be combined with wrapped part
-  // To ensure PMA is always access 16bit aligned (src aligned to 16 bit)
+  // To ensure PMA is always access aligned (src aligned to 16 or 32 bit)
+#ifdef PMA_32BIT_ACCESS
+  if((cnt_lin & 0x03) && cnt_wrap)
+  {
+    // Copy first linear part
+    dcd_read_packet_memory(info.ptr_lin, src, cnt_lin &~0x03);
+    src += cnt_lin &~0x03;
+
+    // Copy last linear bytes & first wrapped bytes
+    uint8_t tmp[4];
+    dcd_read_packet_memory(tmp, src, 4);
+    src += 4;
+
+    uint32_t i;
+    for (i = 0; i < (cnt_lin & 0x03); i++)
+    {
+      ((uint8_t*)info.ptr_lin)[(cnt_lin &~0x03) + i] = tmp[i];
+    }
+    uint32_t wCnt = cnt_wrap;
+    for (; i < 4 && wCnt > 0; i++, wCnt--)
+    {
+      *(uint8_t*)info.ptr_wrap = tmp[i];
+      info.ptr_wrap = (uint8_t*)info.ptr_wrap + 1;
+    }
+
+    // Copy rest of wrapped byte
+    if (wCnt)
+      dcd_read_packet_memory(info.ptr_wrap, src, wCnt);
+  }
+#else
   if((cnt_lin & 0x01) && cnt_wrap)
   {
     // Copy first linear part
@@ -1356,16 +1505,17 @@ static bool dcd_read_packet_memory_ff(tu_fifo_t * ff, uint16_t src, uint16_t wNB
     src += cnt_lin &~0x01;
 
     // Copy last linear byte & first wrapped byte
-    uint16_t tmp;
-    dcd_read_packet_memory(&tmp, src, 2);
-
-    ((uint8_t*)info.ptr_lin)[cnt_lin - 1] = (uint8_t)tmp;
-    ((uint8_t*)info.ptr_wrap)[0] = (uint8_t)(tmp >> 8U);
+    uint8_t tmp[2];
+    dcd_read_packet_memory(tmp, src, 2);
     src += 2;
 
+    ((uint8_t*)info.ptr_lin)[cnt_lin - 1] = tmp[0];
+    ((uint8_t*)info.ptr_wrap)[0] = tmp[1];
+
     // Copy rest of wrapped byte
     dcd_read_packet_memory(((uint8_t*)info.ptr_wrap) + 1, src, cnt_wrap - 1);
   }
+#endif
   else
   {
     // Copy linear part
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
index e3fc8aeb5..e6649de5a 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
@@ -82,6 +82,34 @@
   #include "stm32g4xx.h"
   #define PMA_LENGTH (1024u)
 
+#elif CFG_TUSB_MCU == OPT_MCU_STM32G0
+  #include "stm32g0xx.h"
+  #define PMA_32BIT_ACCESS
+  #define PMA_LENGTH (2048u)
+  #undef USB_PMAADDR
+  #define USB_PMAADDR USB_DRD_PMAADDR
+  #define USB_TypeDef USB_DRD_TypeDef
+  #define EP0R CHEP0R
+  #define USB_EP_CTR_RX USB_EP_VTRX
+  #define USB_EP_CTR_TX USB_EP_VTTX
+  #define USB_EP_T_FIELD USB_CHEP_UTYPE
+  #define USB_EPREG_MASK USB_CHEP_REG_MASK
+  #define USB_EPTX_DTOGMASK USB_CHEP_TX_DTOGMASK
+  #define USB_EPRX_DTOGMASK USB_CHEP_RX_DTOGMASK
+  #define USB_EPTX_DTOG1 USB_CHEP_TX_DTOG1
+  #define USB_EPTX_DTOG2 USB_CHEP_TX_DTOG2
+  #define USB_EPRX_DTOG1 USB_CHEP_RX_DTOG1
+  #define USB_EPRX_DTOG2 USB_CHEP_RX_DTOG2
+  #define USB_EPRX_STAT USB_CH_RX_VALID
+  #define USB_EPKIND_MASK USB_EP_KIND_MASK
+  #define USB USB_DRD_FS
+  #define USB_CNTR_FRES USB_CNTR_USBRST
+  #define USB_CNTR_RESUME USB_CNTR_L2RES
+  #define USB_ISTR_EP_ID USB_ISTR_IDN
+  #define USB_EPADDR_FIELD USB_CHEP_ADDR
+  #define USB_CNTR_LPMODE USB_CNTR_SUSPRDY
+  #define USB_CNTR_FSUSP USB_CNTR_SUSPEN
+
 #elif CFG_TUSB_MCU == OPT_MCU_STM32WB
   #include "stm32wbxx.h"
   #define PMA_LENGTH (1024u)
@@ -113,15 +141,31 @@
   #define PMA_STRIDE  (1u)
 #endif
 
-// And for type-safety create a new macro for the volatile address of PMAADDR
+// For type-safety create a new macro for the volatile address of PMAADDR
 // The compiler should warn us if we cast it to a non-volatile type?
+#ifdef PMA_32BIT_ACCESS
+static __IO uint32_t * const pma32 = (__IO uint32_t*)USB_PMAADDR;
+#else
 // Volatile is also needed to prevent the optimizer from changing access to 32-bit (as 32-bit access is forbidden)
 static __IO uint16_t * const pma = (__IO uint16_t*)USB_PMAADDR;
 
-// prototypes
-TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx);
-TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx);
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wRegValue);
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t * pcd_btable_word_ptr(USB_TypeDef * USBx, size_t x)
+{
+  size_t total_word_offset = (((USBx)->BTABLE)>>1) + x;
+  total_word_offset *= PMA_STRIDE;
+  return &(pma[total_word_offset]);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
+{
+  return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 1u);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
+{
+  return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 3u);
+}
+#endif
 
 /* Aligned buffer size according to hardware */
 TU_ATTR_ALWAYS_INLINE static inline uint16_t pcd_aligned_buffer_size(uint16_t size)
@@ -139,13 +183,24 @@ TU_ATTR_ALWAYS_INLINE static inline uint16_t pcd_aligned_buffer_size(uint16_t si
 /* SetENDPOINT */
 TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wRegValue)
 {
+#ifdef PMA_32BIT_ACCESS
+  (void) USBx;
+  __O uint32_t *reg = (__O uint32_t *)(USB_DRD_BASE + bEpIdx*4);
+  *reg = wRegValue;
+#else
   __O uint16_t *reg = (__O uint16_t *)((&USBx->EP0R) + bEpIdx*2u);
   *reg = (uint16_t)wRegValue;
+#endif
 }
 
 /* GetENDPOINT */
-TU_ATTR_ALWAYS_INLINE static inline uint16_t pcd_get_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx) {
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx) {
+#ifdef PMA_32BIT_ACCESS
+  (void) USBx;
+  __I uint32_t *reg = (__I uint32_t *)(USB_DRD_BASE + bEpIdx*4);
+#else
   __I uint16_t *reg = (__I uint16_t *)((&USBx->EP0R) + bEpIdx*2u);
+#endif
   return *reg;
 }
 
@@ -195,34 +250,24 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_ep_ctr(USB_TypeDef * USBx,
   */
 TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
+#ifdef PMA_32BIT_ACCESS
+  (void) USBx;
+  return (pma32[2*bEpIdx] & 0x03FF0000) >> 16;
+#else
   __I uint16_t *regPtr = pcd_ep_tx_cnt_ptr(USBx, bEpIdx);
   return *regPtr & 0x3ffU;
+#endif
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
+#ifdef PMA_32BIT_ACCESS
+  (void) USBx;
+  return (pma32[2*bEpIdx + 1] & 0x03FF0000) >> 16;
+#else
   __I uint16_t *regPtr = pcd_ep_rx_cnt_ptr(USBx, bEpIdx);
   return *regPtr & 0x3ffU;
-}
-
-/**
-  * @brief  Sets counter of rx buffer with no. of blocks.
-  * @param  dwReg Register
-  * @param  wCount Counter.
-  * @param  wNBlocks no. of Blocks.
-  * @retval None
-  */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_cnt_reg(__O uint16_t * pdwReg, size_t wCount)
-{
-  /* We assume that the buffer size is already aligned to hardware requirements. */
-  uint16_t blocksize = (wCount > 62) ? 1 : 0;
-  uint16_t numblocks = wCount / (blocksize ? 32 : 2);
-
-  /* There should be no remainder in the above calculation */
-  TU_ASSERT((wCount - (numblocks * (blocksize ? 32 : 2))) == 0, /**/);
-
-  /* Encode into register. When BLSIZE==1, we need to subtract 1 block count */
-  *pdwReg = (blocksize << 15) | ((numblocks - blocksize) << 10);
+#endif
 }
 
 /**
@@ -241,57 +286,103 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_address(USB_TypeDef * USBx,
   pcd_set_endpoint(USBx, bEpIdx,regVal);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t * pcd_btable_word_ptr(USB_TypeDef * USBx, size_t x)
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_address(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  size_t total_word_offset = (((USBx)->BTABLE)>>1) + x;
-  total_word_offset *= PMA_STRIDE;
-  return &(pma[total_word_offset]);
+#ifdef PMA_32BIT_ACCESS
+  (void) USBx;
+  return pma32[2*bEpIdx] & 0x0000FFFFu ;
+#else
+  return *pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 0u);
+#endif
 }
 
-// Pointers to the PMA table entries (using the ARM address space)
-TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_address_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_address(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 0u);
-}
-TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
-  return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 1u);
+#ifdef PMA_32BIT_ACCESS
+  (void) USBx;
+  return pma32[2*bEpIdx + 1] & 0x0000FFFFu;
+#else
+  return *pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 2u);
+#endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_address_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_address(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t addr)
 {
-  return  pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 2u);
+#ifdef PMA_32BIT_ACCESS
+  (void) USBx;
+  pma32[2*bEpIdx] = (pma32[2*bEpIdx] & 0xFFFF0000u) | (addr & 0x0000FFFCu);
+#else
+  *pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 0u) = addr;
+#endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_address(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t addr)
 {
-  return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 3u);
+#ifdef PMA_32BIT_ACCESS
+  (void) USBx;
+  pma32[2*bEpIdx + 1] = (pma32[2*bEpIdx + 1] & 0xFFFF0000u) | (addr & 0x0000FFFCu);
+#else
+  *pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 2u) = addr;
+#endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_cnt(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wCount)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount)
 {
+#ifdef PMA_32BIT_ACCESS
+  (void) USBx;
+  pma32[2*bEpIdx] = (pma32[2*bEpIdx] & ~0x03FF0000u) | ((wCount & 0x3FFu) << 16);
+#else
   __IO uint16_t * reg = pcd_ep_tx_cnt_ptr(USBx, bEpIdx);
   *reg = (uint16_t) (*reg & (uint16_t) ~0x3FFU) | (wCount & 0x3FFU);
+#endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_cnt(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wCount)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount)
 {
+#ifdef PMA_32BIT_ACCESS
+  (void) USBx;
+  pma32[2*bEpIdx + 1] = (pma32[2*bEpIdx + 1] & ~0x03FF0000u) | ((wCount & 0x3FFu) << 16);
+#else
   __IO uint16_t * reg = pcd_ep_rx_cnt_ptr(USBx, bEpIdx);
   *reg = (uint16_t) (*reg & (uint16_t) ~0x3FFU) | (wCount & 0x3FFU);
+#endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_bufsize(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wCount)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_blsize_num_blocks(USB_TypeDef * USBx, uint32_t rxtx_idx, uint32_t blocksize, uint32_t numblocks)
 {
-  __IO uint16_t *pdwReg = pcd_ep_tx_cnt_ptr((USBx),(bEpIdx));
-  wCount = pcd_aligned_buffer_size(wCount);
-  pcd_set_ep_cnt_reg(pdwReg, wCount);
+  /* Encode into register. When BLSIZE==1, we need to subtract 1 block count */
+#ifdef PMA_32BIT_ACCESS
+  (void) USBx;
+  pma32[rxtx_idx] = (pma32[rxtx_idx] & 0x0000FFFFu) | (blocksize << 31) | ((numblocks - blocksize) << 26);
+#else
+  __IO uint16_t *pdwReg = pcd_btable_word_ptr(USBx, rxtx_idx*2u + 1u);
+  *pdwReg = (blocksize << 15) | ((numblocks - blocksize) << 10);
+#endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_bufsize(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wCount)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_bufsize(USB_TypeDef * USBx, uint32_t rxtx_idx, uint32_t wCount)
 {
-  __IO uint16_t *pdwReg = pcd_ep_rx_cnt_ptr((USBx),(bEpIdx));
   wCount = pcd_aligned_buffer_size(wCount);
-  pcd_set_ep_cnt_reg(pdwReg, wCount);
+
+  /* We assume that the buffer size is already aligned to hardware requirements. */
+  uint16_t blocksize = (wCount > 62) ? 1 : 0;
+  uint16_t numblocks = wCount / (blocksize ? 32 : 2);
+
+  /* There should be no remainder in the above calculation */
+  TU_ASSERT((wCount - (numblocks * (blocksize ? 32 : 2))) == 0, /**/);
+
+  /* Encode into register. When BLSIZE==1, we need to subtract 1 block count */
+  pcd_set_ep_blsize_num_blocks(USBx, rxtx_idx, blocksize, numblocks);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_bufsize(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount)
+{
+  pcd_set_ep_bufsize(USBx, 2*bEpIdx, wCount);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_bufsize(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount)
+{
+  pcd_set_ep_bufsize(USBx, 2*bEpIdx + 1, wCount);
 }
 
 /**
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 4f3f3a985..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