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

Update fsp to 4.0.0

12 files changed, 1011 insertions, 520 deletions

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);
 }
 
 /**