Merge branch 'refs/heads/master' into fork/BrentK-ADI/max32_port

12 files changed, 910 insertions, 1162 deletions

diff --git a/src/portable/nordic/nrf5x/dcd_nrf5x.c b/src/portable/nordic/nrf5x/dcd_nrf5x.c
index abce35245..1cc5c1836 100644
--- a/src/portable/nordic/nrf5x/dcd_nrf5x.c
+++ b/src/portable/nordic/nrf5x/dcd_nrf5x.c
@@ -907,9 +907,9 @@ void tusb_hal_nrf_power_event(uint32_t event) {
     USB_EVT_READY = 2
   };
 
-#if CFG_TUSB_DEBUG >= 2
+#if CFG_TUSB_DEBUG >= 3
   const char* const power_evt_str[] = {"Detected", "Removed", "Ready"};
-  TU_LOG(2, "Power USB event: %s\r\n", power_evt_str[event]);
+  TU_LOG(3, "Power USB event: %s\r\n", power_evt_str[event]);
 #endif
 
   switch (event) {
diff --git a/src/portable/nxp/khci/dcd_khci.c b/src/portable/nxp/khci/dcd_khci.c
index dc71117b3..ef61146aa 100644
--- a/src/portable/nxp/khci/dcd_khci.c
+++ b/src/portable/nxp/khci/dcd_khci.c
@@ -540,7 +540,7 @@ void dcd_int_handler(uint8_t rhport)
   }
 
   if (is & USB_ISTAT_SLEEP_MASK) {
-    // TU_LOG2("Suspend: "); TU_LOG2_HEX(is);
+    // TU_LOG3("Suspend: "); TU_LOG2_HEX(is);
 
     // Note Host usually has extra delay after bus reset (without SOF), which could falsely
     // detected as Sleep event. Though usbd has debouncing logic so we are good
diff --git a/src/portable/raspberrypi/rp2040/rp2040_usb.c b/src/portable/raspberrypi/rp2040/rp2040_usb.c
index 1ca711c77..43f48da39 100644
--- a/src/portable/raspberrypi/rp2040/rp2040_usb.c
+++ b/src/portable/raspberrypi/rp2040/rp2040_usb.c
@@ -53,6 +53,14 @@ TU_ATTR_ALWAYS_INLINE static inline bool is_host_mode(void) {
 //--------------------------------------------------------------------+
 // Implementation
 //--------------------------------------------------------------------+
+// Provide own byte by byte memcpy as not all copies are aligned
+static void unaligned_memcpy(void *dst, const void *src, size_t n) {
+  uint8_t *dst_byte = (uint8_t*)dst;
+  const uint8_t *src_byte = (const uint8_t*)src;
+  while (n--) {
+    *dst_byte++ = *src_byte++;
+  }
+}
 
 void rp2040_usb_init(void) {
   // Reset usb controller
@@ -67,7 +75,6 @@ void rp2040_usb_init(void) {
 #pragma GCC diagnostic ignored "-Wstringop-overflow"
 #endif
 #endif
-  memset(usb_hw, 0, sizeof(*usb_hw));
   memset(usb_dpram, 0, sizeof(*usb_dpram));
 #ifdef __GNUC__
 #pragma GCC diagnostic pop
@@ -125,7 +132,7 @@ static uint32_t __tusb_irq_path_func(prepare_ep_buffer)(struct hw_endpoint* ep,
 
   if (!ep->rx) {
     // Copy data from user buffer to hw buffer
-    memcpy(ep->hw_data_buf + buf_id * 64, ep->user_buf, buflen);
+    unaligned_memcpy(ep->hw_data_buf + buf_id * 64, ep->user_buf, buflen);
     ep->user_buf += buflen;
 
     // Mark as full
@@ -230,7 +237,7 @@ static uint16_t __tusb_irq_path_func(sync_ep_buffer)(struct hw_endpoint* ep, uin
     // we have received AFTER we have copied it to the user buffer at the appropriate offset
     assert(buf_ctrl & USB_BUF_CTRL_FULL);
 
-    memcpy(ep->user_buf, ep->hw_data_buf + buf_id * 64, xferred_bytes);
+    unaligned_memcpy(ep->user_buf, ep->hw_data_buf + buf_id * 64, xferred_bytes);
     ep->xferred_len = (uint16_t) (ep->xferred_len + xferred_bytes);
     ep->user_buf += xferred_bytes;
   }
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index 9ce37f992..6eea1ab32 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -4,7 +4,6 @@
  * Copyright (c) 2019 Nathan Conrad
  *
  * Portions:
- * Copyright (c) 2016 STMicroelectronics
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
  * Copyright (c) 2022 Simon Küppers (skuep)
  * Copyright (c) 2022 HiFiPhile
@@ -37,14 +36,20 @@
  * It also should work with minimal changes for any ST MCU with an "USB A"/"PCD"/"HCD" peripheral. This
  *  covers:
  *
- * F04x, F072, F078, 070x6/B      1024 byte buffer
+ * F04x, F072, F078, F070x6/B     1024 byte buffer
  * F102, F103                      512 byte buffer; no internal D+ pull-up (maybe many more changes?)
  * F302xB/C, F303xB/C, F373        512 byte buffer; no internal D+ pull-up
  * F302x6/8, F302xD/E2, F303xD/E  1024 byte buffer; no internal D+ pull-up
+ * G0                             2048 byte buffer; 32-bit bus; host mode
+ * G4                             1024 byte buffer
+ * H5                             2048 byte buffer; 32-bit bus; host mode
  * L0x2, L0x3                     1024 byte buffer
  * L1                              512 byte buffer
  * L4x2, L4x3                     1024 byte buffer
- * G0                             2048 byte buffer
+ * L5                             1024 byte buffer
+ * U0                             1024 byte buffer; 32-bit bus
+ * U535, U545                     2048 byte buffer; 32-bit bus; host mode
+ * WB35, WB55                     1024 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)
@@ -108,8 +113,6 @@
 #include "device/dcd.h"
 
 #if defined(TUP_USBIP_FSDEV_STM32)
-  // Undefine to reduce the dependence on HAL
-  #undef USE_HAL_DRIVER
   #include "fsdev_stm32.h"
 #elif defined(TUP_USBIP_FSDEV_CH32)
   #include "fsdev_ch32.h"
@@ -117,27 +120,7 @@
   #error "Unknown USB IP"
 #endif
 
-#include "fsdev_common.h"
-
-//--------------------------------------------------------------------+
-// Configuration
-//--------------------------------------------------------------------+
-
-// hardware limit endpoint
-#define FSDEV_EP_COUNT 8
-
-// If sharing with CAN, one can set this to be non-zero to give CAN space where it wants it
-// Both of these MUST be a multiple of 2, and are in byte units.
-#ifndef DCD_STM32_BTABLE_BASE
-#define DCD_STM32_BTABLE_BASE 0U
-#endif
-
-#ifndef DCD_STM32_BTABLE_SIZE
-#define DCD_STM32_BTABLE_SIZE (FSDEV_PMA_SIZE - DCD_STM32_BTABLE_BASE)
-#endif
-
-TU_VERIFY_STATIC(((DCD_STM32_BTABLE_BASE) + (DCD_STM32_BTABLE_SIZE)) <= (FSDEV_PMA_SIZE), "BTABLE does not fit in PMA RAM");
-TU_VERIFY_STATIC(((DCD_STM32_BTABLE_BASE) % 8) == 0, "BTABLE base must be aligned to 8 bytes");
+#include "fsdev_type.h"
 
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
@@ -162,11 +145,7 @@ typedef struct {
 } ep_alloc_t;
 
 static xfer_ctl_t xfer_status[CFG_TUD_ENDPPOINT_MAX][2];
-
 static ep_alloc_t ep_alloc_status[FSDEV_EP_COUNT];
-
-static TU_ATTR_ALIGNED(4) uint32_t _setup_packet[6];
-
 static uint8_t remoteWakeCountdown; // When wake is requested
 
 //--------------------------------------------------------------------+
@@ -174,99 +153,90 @@ static uint8_t remoteWakeCountdown; // When wake is requested
 //--------------------------------------------------------------------+
 
 // into the stack.
-static void dcd_handle_bus_reset(void);
+static void handle_bus_reset(uint8_t rhport);
 static void dcd_transmit_packet(xfer_ctl_t *xfer, uint16_t ep_ix);
-static bool edpt_xfer(uint8_t rhport, uint8_t ep_addr);
-static void dcd_ep_ctr_handler(void);
+static bool edpt_xfer(uint8_t rhport, uint8_t ep_num, tusb_dir_t dir);
 
 // PMA allocation/access
 static uint16_t ep_buf_ptr; ///< Points to first free memory location
-static uint32_t dcd_pma_alloc(uint16_t length, bool dbuf);
+static uint32_t dcd_pma_alloc(uint16_t len, bool dbuf);
 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, uint16_t wNBytes);
-static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t wNBytes);
+static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t nbytes);
+static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t nbytes);
 
 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);
 
+static void edpt0_open(uint8_t rhport);
+
+TU_ATTR_ALWAYS_INLINE static inline void edpt0_prepare_setup(void) {
+   btable_set_rx_bufsize(0, BTABLE_BUF_RX, 8);
+}
+
 //--------------------------------------------------------------------+
 // Inline helper
 //--------------------------------------------------------------------+
 
-TU_ATTR_ALWAYS_INLINE static inline xfer_ctl_t *xfer_ctl_ptr(uint32_t ep_addr)
-{
-  uint8_t epnum = tu_edpt_number(ep_addr);
-  uint8_t dir = tu_edpt_dir(ep_addr);
-  // Fix -Werror=null-dereference
-  TU_ASSERT(epnum < CFG_TUD_ENDPPOINT_MAX, &xfer_status[0][0]);
-
+TU_ATTR_ALWAYS_INLINE static inline xfer_ctl_t *xfer_ctl_ptr(uint8_t epnum, uint8_t dir) {
   return &xfer_status[epnum][dir];
 }
 
 //--------------------------------------------------------------------+
 // Controller API
 //--------------------------------------------------------------------+
-
-void dcd_init(uint8_t rhport)
-{
-  /* Clocks should already be enabled */
-  /* Use __HAL_RCC_USB_CLK_ENABLE(); to enable the clocks before calling this function */
-
-  /* The RM mentions to use a special ordering of PDWN and FRES, but this isn't done in HAL.
-   * Here, the RM is followed. */
-
-  for (uint32_t i = 0; i < 200; i++) { // should be a few us
+void dcd_init(uint8_t rhport) {
+  // Follow the RM mentions to use a special ordering of PDWN and FRES
+  for (volatile uint32_t i = 0; i < 200; i++) { // should be a few us
     asm("NOP");
   }
+
   // Perform USB peripheral reset
-  USB->CNTR = USB_CNTR_FRES | USB_CNTR_PDWN;
-  for (uint32_t i = 0; i < 200; i++) { // should be a few us
+  FSDEV_REG->CNTR = USB_CNTR_FRES | USB_CNTR_PDWN;
+  for (volatile uint32_t i = 0; i < 200; i++) { // should be a few us
     asm("NOP");
   }
 
-  USB->CNTR &= ~USB_CNTR_PDWN;
+  FSDEV_REG->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
+  for (volatile uint32_t i = 0; i < 200; i++) { // should be a few us
     asm("NOP");
   }
-  USB->CNTR = 0; // Enable USB
+  FSDEV_REG->CNTR = 0; // Enable USB
 
-#if !defined(STM32G0) && !defined(STM32H5) // BTABLE register does not exist any more on STM32G0, it is fixed to USB SRAM base address
-  USB->BTABLE = DCD_STM32_BTABLE_BASE;
+#if !defined(FSDEV_BUS_32BIT)
+  // BTABLE register does not exist any more on 32-bit bus devices
+  FSDEV_REG->BTABLE = FSDEV_BTABLE_BASE;
 #endif
-  USB->ISTR = 0; // Clear pending interrupts
+
+  FSDEV_REG->ISTR = 0; // Clear pending interrupts
 
   // Reset endpoints to disabled
   for (uint32_t i = 0; i < FSDEV_EP_COUNT; i++) {
     // This doesn't clear all bits since some bits are "toggle", but does set the type to DISABLED.
-    pcd_set_endpoint(USB, i, 0u);
+    ep_write(i, 0u, false);
   }
 
-  USB->CNTR |= USB_CNTR_RESETM | USB_CNTR_ESOFM | USB_CNTR_CTRM | USB_CNTR_SUSPM | USB_CNTR_WKUPM;
-  dcd_handle_bus_reset();
+  FSDEV_REG->CNTR |= USB_CNTR_RESETM | USB_CNTR_ESOFM | USB_CNTR_CTRM |
+      USB_CNTR_SUSPM | USB_CNTR_WKUPM | USB_CNTR_PMAOVRM;
+  handle_bus_reset(rhport);
 
   // Enable pull-up if supported
   dcd_connect(rhport);
 }
 
-
-void dcd_sof_enable(uint8_t rhport, bool en)
-{
+void dcd_sof_enable(uint8_t rhport, bool en) {
   (void)rhport;
-  (void)en;
 
   if (en) {
-    USB->CNTR |= USB_CNTR_SOFM;
+    FSDEV_REG->CNTR |= USB_CNTR_SOFM;
   } else {
-    USB->CNTR &= ~USB_CNTR_SOFM;
+    FSDEV_REG->CNTR &= ~USB_CNTR_SOFM;
   }
 }
 
 // Receive Set Address request, mcu port must also include status IN response
-void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
-{
-  (void)rhport;
+void dcd_set_address(uint8_t rhport, uint8_t dev_addr) {
   (void)dev_addr;
 
   // Respond with status
@@ -276,35 +246,15 @@ void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
   // do it at dcd_edpt0_status_complete()
 }
 
-void dcd_remote_wakeup(uint8_t rhport)
-{
+void dcd_remote_wakeup(uint8_t rhport) {
   (void)rhport;
 
-  USB->CNTR |= USB_CNTR_RESUME;
+  FSDEV_REG->CNTR |= USB_CNTR_RESUME;
   remoteWakeCountdown = 4u; // required to be 1 to 15 ms, ESOF should trigger every 1ms.
 }
 
-static const tusb_desc_endpoint_t ep0OUT_desc = {
-    .bLength = sizeof(tusb_desc_endpoint_t),
-    .bDescriptorType = TUSB_DESC_ENDPOINT,
-    .bEndpointAddress = 0x00,
-    .bmAttributes = {.xfer = TUSB_XFER_CONTROL},
-    .wMaxPacketSize = CFG_TUD_ENDPOINT0_SIZE,
-    .bInterval = 0
-};
-
-static const tusb_desc_endpoint_t ep0IN_desc = {
-    .bLength = sizeof(tusb_desc_endpoint_t),
-    .bDescriptorType = TUSB_DESC_ENDPOINT,
-    .bEndpointAddress = 0x80,
-    .bmAttributes = {.xfer = TUSB_XFER_CONTROL},
-    .wMaxPacketSize = CFG_TUD_ENDPOINT0_SIZE,
-    .bInterval = 0
-};
-
-static void dcd_handle_bus_reset(void)
-{
-  USB->DADDR = 0u; // disable USB peripheral by clearing the EF flag
+static void handle_bus_reset(uint8_t rhport) {
+  FSDEV_REG->DADDR = 0u; // disable USB Function
 
   for (uint32_t i = 0; i < FSDEV_EP_COUNT; i++) {
     // Clear EP allocation status
@@ -315,35 +265,21 @@ static void dcd_handle_bus_reset(void)
   }
 
   // Reset PMA allocation
-  ep_buf_ptr = DCD_STM32_BTABLE_BASE + 8 * CFG_TUD_ENDPPOINT_MAX;
+  ep_buf_ptr = FSDEV_BTABLE_BASE + 8 * FSDEV_EP_COUNT;
 
-  dcd_edpt_open(0, &ep0OUT_desc);
-  dcd_edpt_open(0, &ep0IN_desc);
+  edpt0_open(rhport); // open control endpoint (both IN & OUT)
 
-  USB->DADDR = USB_DADDR_EF; // Set enable flag, and leaving the device address as zero.
+  FSDEV_REG->DADDR = USB_DADDR_EF; // Enable USB Function
 }
 
 // Handle CTR interrupt for the TX/IN direction
-//
-// Upon call, (wIstr & USB_ISTR_DIR) == 0U
-static void dcd_ep_ctr_tx_handler(uint32_t wIstr)
-{
-  uint32_t EPindex = wIstr & USB_ISTR_EP_ID;
-  uint32_t wEPRegVal = pcd_get_endpoint(USB, EPindex);
-  uint8_t ep_addr = (wEPRegVal & USB_EPADDR_FIELD) | TUSB_DIR_IN_MASK;
-
-  // Verify the CTR_TX bit is set. This was in the ST Micro code,
-  // but I'm not sure it's actually necessary?
-  if ((wEPRegVal & USB_EP_CTR_TX) == 0U) {
-    return;
-  }
-
-  /* clear int flag */
-  pcd_clear_tx_ep_ctr(USB, EPindex);
+static void handle_ctr_tx(uint32_t ep_id) {
+  uint32_t ep_reg = ep_read(ep_id) | USB_EP_CTR_TX | USB_EP_CTR_RX;
 
-  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
+  uint8_t const ep_num = ep_reg & USB_EPADDR_FIELD;
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_num, TUSB_DIR_IN);
 
-  if ((wEPRegVal & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS) {
+  if (ep_is_iso(ep_reg)) {
     // Ignore spurious interrupts that we don't schedule
     // host can send IN token while there is no data to send, since ISO does not have NAK
     // this will result to zero length packet --> trigger interrupt (which cannot be masked)
@@ -351,189 +287,106 @@ static void dcd_ep_ctr_tx_handler(uint32_t wIstr)
       return;
     }
     xfer->iso_in_sending = false;
-
-    if (wEPRegVal & USB_EP_DTOG_TX) {
-      pcd_set_ep_tx_dbuf0_cnt(USB, EPindex, 0);
-    } else {
-      pcd_set_ep_tx_dbuf1_cnt(USB, EPindex, 0);
-    }
+    uint8_t buf_id = (ep_reg & USB_EP_DTOG_TX) ? 0 : 1;
+    btable_set_count(ep_id, buf_id, 0);
   }
 
-  if ((xfer->total_len != xfer->queued_len)) {
-    dcd_transmit_packet(xfer, EPindex);
+  if (xfer->total_len != xfer->queued_len) {
+    dcd_transmit_packet(xfer, ep_id);
   } else {
-    dcd_event_xfer_complete(0, ep_addr, xfer->total_len, XFER_RESULT_SUCCESS, true);
+    dcd_event_xfer_complete(0, ep_num | TUSB_DIR_IN_MASK, xfer->queued_len, XFER_RESULT_SUCCESS, true);
   }
 }
 
-// Handle CTR interrupt for the RX/OUT direction
-// Upon call, (wIstr & USB_ISTR_DIR) == 0U
-static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
-{
-#ifdef FSDEV_BUS_32BIT
-  /* https://www.st.com/resource/en/errata_sheet/es0561-stm32h503cbebkbrb-device-errata-stmicroelectronics.pdf
-   * From STM32H503 errata 2.15.1: Buffer description table update completes after CTR interrupt triggers
-   * Description:
-   * - During OUT transfers, the correct transfer interrupt (CTR) is triggered a little before the last USB SRAM accesses
-   * have completed. If the software responds quickly to the interrupt, the full buffer contents may not be correct.
-   * Workaround:
-   * - Software should ensure that a small delay is included before accessing the SRAM contents. This delay
-   * should be 800 ns in Full Speed mode and 6.4 μs in Low Speed mode
-   * - Since H5 can run up to 250Mhz -> 1 cycle = 4ns. Per errata, we need to wait 200 cycles. Though executing code
-   * also takes time, so we'll wait 60 cycles (count = 20).
-   * - Since Low Speed mode is not supported/popular, we will ignore it for now.
-   *
-   * Note: this errata also seems to apply to G0, U5, H5 etc.
-   */
-  volatile uint32_t cycle_count = 20; // defined as PCD_RX_PMA_CNT in stm32 hal_driver
-  while (cycle_count > 0U) {
-    cycle_count--; // each count take 3 cycles (1 for sub, jump, and compare)
-  }
-#endif
-
-  uint32_t EPindex = wIstr & USB_ISTR_EP_ID;
-  uint32_t wEPRegVal = pcd_get_endpoint(USB, EPindex);
-  uint8_t ep_addr = wEPRegVal & USB_EPADDR_FIELD;
+static void handle_ctr_setup(uint32_t ep_id) {
+  uint16_t rx_count = btable_get_count(ep_id, BTABLE_BUF_RX);
+  uint16_t rx_addr = btable_get_addr(ep_id, BTABLE_BUF_RX);
+  uint8_t setup_packet[8] TU_ATTR_ALIGNED(4);
 
-  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
+  dcd_read_packet_memory(setup_packet, rx_addr, rx_count);
 
-  // Verify the CTR_RX bit is set. This was in the ST Micro code,
-  // but I'm not sure it's actually necessary?
-  if ((wEPRegVal & USB_EP_CTR_RX) == 0U) {
-    return;
-  }
+  // Clear CTR RX if another setup packet arrived before this, it will be discarded
+  ep_write_clear_ctr(ep_id, TUSB_DIR_OUT);
 
-  if ((ep_addr == 0U) && ((wEPRegVal & USB_EP_SETUP) != 0U)) {
-    /* Setup packet */
-    uint32_t count = pcd_get_ep_rx_cnt(USB, EPindex);
-    // Setup packet should always be 8 bytes. If not, ignore it, and try again.
-    if (count == 8) {
-      // 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);
-#ifdef FSDEV_BUS_32BIT
-      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
-    }
+  // Setup packet should always be 8 bytes. If not, we probably missed the packet
+  if (rx_count == 8) {
+    dcd_event_setup_received(0, (uint8_t*) setup_packet, true);
+    // Hardware should reset EP0 RX/TX to NAK and both toggle to 1
   } else {
-    // Clear RX CTR interrupt flag
-    if (ep_addr != 0u) {
-      pcd_clear_rx_ep_ctr(USB, EPindex);
-    }
-
-    uint32_t count;
-    uint16_t addr;
-    /* Read from correct register when ISOCHRONOUS (double buffered) */
-    if ((wEPRegVal & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS) {
-      if (wEPRegVal & USB_EP_DTOG_RX) {
-        count = pcd_get_ep_dbuf0_cnt(USB, EPindex);
-        addr = pcd_get_ep_dbuf0_address(USB, EPindex);
-      } else {
-        count = pcd_get_ep_dbuf1_cnt(USB, EPindex);
-        addr = pcd_get_ep_dbuf1_address(USB, EPindex);
-      }
-    } else {
-      count = pcd_get_ep_rx_cnt(USB, EPindex);
-      addr = pcd_get_ep_rx_address(USB, EPindex);
-    }
-
-    TU_ASSERT(count <= xfer->max_packet_size, /**/);
-
-    if (count != 0U) {
-      if (xfer->ff) {
-        dcd_read_packet_memory_ff(xfer->ff, addr, count);
-      } else {
-        dcd_read_packet_memory(&(xfer->buffer[xfer->queued_len]), addr, count);
-      }
+    // Missed setup packet !!!
+    TU_BREAKPOINT();
+    edpt0_prepare_setup();
+  }
+}
 
-      xfer->queued_len = (uint16_t)(xfer->queued_len + count);
-    }
+// Handle CTR interrupt for the RX/OUT direction
+static void handle_ctr_rx(uint32_t ep_id) {
+  uint32_t ep_reg = ep_read(ep_id) | USB_EP_CTR_TX | USB_EP_CTR_RX;
+  uint8_t const ep_num = ep_reg & USB_EPADDR_FIELD;
+  bool const is_iso = ep_is_iso(ep_reg);
+  xfer_ctl_t* xfer = xfer_ctl_ptr(ep_num, TUSB_DIR_OUT);
 
-    if ((count < xfer->max_packet_size) || (xfer->queued_len == xfer->total_len)) {
-      // all bytes received or short packet
-      dcd_event_xfer_complete(0, ep_addr, xfer->queued_len, XFER_RESULT_SUCCESS, true);
-    } else {
-      /* Set endpoint active again for receiving more data.
-       * Note that isochronous endpoints stay active always */
-      if ((wEPRegVal & USB_EP_TYPE_MASK) != USB_EP_ISOCHRONOUS) {
-        uint16_t remaining = xfer->total_len - xfer->queued_len;
-        uint16_t cnt = tu_min16(remaining, xfer->max_packet_size);
-        pcd_set_ep_rx_cnt(USB, EPindex, cnt);
-      }
-      pcd_set_ep_rx_status(USB, EPindex, USB_EP_RX_VALID);
-    }
+  uint8_t buf_id;
+  if (is_iso) {
+    buf_id = (ep_reg & USB_EP_DTOG_RX) ? 0 : 1; // ISO are double buffered
+  } else {
+    buf_id = BTABLE_BUF_RX;
   }
+  uint16_t const rx_count = btable_get_count(ep_id, buf_id);
+  uint16_t pma_addr = (uint16_t) btable_get_addr(ep_id, buf_id);
 
-  // For EP0, prepare to receive another SETUP packet.
-  // Clear CTR last so that a new packet does not overwrite the packing being read.
-  // (Based on the docs, it seems SETUP will always be accepted after CTR is cleared)
-  if (ep_addr == 0u) {
-    // Always be prepared for a status packet...
-    pcd_set_ep_rx_cnt(USB, EPindex, CFG_TUD_ENDPOINT0_SIZE);
-    pcd_clear_rx_ep_ctr(USB, EPindex);
+  if (xfer->ff) {
+    dcd_read_packet_memory_ff(xfer->ff, pma_addr, rx_count);
+  } else {
+    dcd_read_packet_memory(xfer->buffer + xfer->queued_len, pma_addr, rx_count);
   }
-}
+  xfer->queued_len += rx_count;
 
-static void dcd_ep_ctr_handler(void)
-{
-  uint32_t wIstr;
+  if ((rx_count < xfer->max_packet_size) || (xfer->queued_len >= xfer->total_len)) {
+    // all bytes received or short packet
 
-  /* stay in loop while pending interrupts */
-  while (((wIstr = USB->ISTR) & USB_ISTR_CTR) != 0U) {
-    if ((wIstr & USB_ISTR_DIR) == 0U) {
-      /* TX/IN */
-      dcd_ep_ctr_tx_handler(wIstr);
-    } else {
-      /* RX/OUT*/
-      dcd_ep_ctr_rx_handler(wIstr);
+    // For ch32v203: reset rx bufsize to mps to prevent race condition to cause PMAOVR (occurs with msc write10)
+    btable_set_rx_bufsize(ep_id, BTABLE_BUF_RX, xfer->max_packet_size);
+
+    dcd_event_xfer_complete(0, ep_num, xfer->queued_len, XFER_RESULT_SUCCESS, true);
+
+    // ch32 seems to unconditionally accept ZLP on EP0 OUT, which can incorrectly use queued_len of previous
+    // transfer. So reset total_len and queued_len to 0.
+    xfer->total_len = xfer->queued_len = 0;
+  } else {
+    // Set endpoint active again for receiving more data. Note that isochronous endpoints stay active always
+    if (!is_iso) {
+      uint16_t const cnt = tu_min16(xfer->total_len - xfer->queued_len, xfer->max_packet_size);
+      btable_set_rx_bufsize(ep_id, BTABLE_BUF_RX, cnt);
     }
+    ep_reg &= USB_EPREG_MASK | EP_STAT_MASK(TUSB_DIR_OUT); // will change RX Status, reserved other toggle bits
+    ep_change_status(&ep_reg, TUSB_DIR_OUT, EP_STAT_VALID);
+    ep_write(ep_id, ep_reg, false);
   }
 }
 
-void dcd_int_handler(uint8_t rhport)
-{
-  (void)rhport;
-
-  uint32_t int_status = USB->ISTR;
-  // const uint32_t handled_ints = USB_ISTR_CTR | USB_ISTR_RESET | USB_ISTR_WKUP
-  //     | USB_ISTR_SUSP | USB_ISTR_SOF | USB_ISTR_ESOF;
-  //  unused IRQs: (USB_ISTR_PMAOVR | USB_ISTR_ERR | USB_ISTR_L1REQ )
-
-  // The ST driver loops here on the CTR bit, but that loop has been moved into the
-  // dcd_ep_ctr_handler(), so less need to loop here. The other interrupts shouldn't
-  // be triggered repeatedly.
+void dcd_int_handler(uint8_t rhport) {
+  uint32_t int_status = FSDEV_REG->ISTR;
 
   /* 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) {
-    USB->ISTR = (fsdev_bus_t)~USB_ISTR_SOF;
-    dcd_event_sof(0, USB->FNR & USB_FNR_FN, true);
+    FSDEV_REG->ISTR = (fsdev_bus_t)~USB_ISTR_SOF;
+    dcd_event_sof(0, FSDEV_REG->FNR & USB_FNR_FN, true);
   }
 
   if (int_status & USB_ISTR_RESET) {
     // USBRST is start of reset.
-    USB->ISTR = (fsdev_bus_t)~USB_ISTR_RESET;
-    dcd_handle_bus_reset();
+    FSDEV_REG->ISTR = (fsdev_bus_t)~USB_ISTR_RESET;
+    handle_bus_reset(rhport);
     dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
     return; // Don't do the rest of the things here; perhaps they've been cleared?
   }
 
-  if (int_status & USB_ISTR_CTR) {
-    /* servicing of the endpoint correct transfer interrupt */
-    /* clear of the CTR flag into the sub */
-    dcd_ep_ctr_handler();
-  }
-
   if (int_status & USB_ISTR_WKUP) {
-    USB->CNTR &= ~USB_CNTR_LPMODE;
-    USB->CNTR &= ~USB_CNTR_FSUSP;
+    FSDEV_REG->CNTR &= ~USB_CNTR_LPMODE;
+    FSDEV_REG->CNTR &= ~USB_CNTR_FSUSP;
 
-    USB->ISTR = (fsdev_bus_t)~USB_ISTR_WKUP;
+    FSDEV_REG->ISTR = (fsdev_bus_t)~USB_ISTR_WKUP;
     dcd_event_bus_signal(0, DCD_EVENT_RESUME, true);
   }
 
@@ -542,22 +395,70 @@ void dcd_int_handler(uint8_t rhport)
      * these events cannot be differentiated, so we only trigger suspend. */
 
     /* Force low-power mode in the macrocell */
-    USB->CNTR |= USB_CNTR_FSUSP;
-    USB->CNTR |= USB_CNTR_LPMODE;
+    FSDEV_REG->CNTR |= USB_CNTR_FSUSP;
+    FSDEV_REG->CNTR |= USB_CNTR_LPMODE;
 
     /* clear of the ISTR bit must be done after setting of CNTR_FSUSP */
-    USB->ISTR = (fsdev_bus_t)~USB_ISTR_SUSP;
+    FSDEV_REG->ISTR = (fsdev_bus_t)~USB_ISTR_SUSP;
     dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
   }
 
   if (int_status & USB_ISTR_ESOF) {
     if (remoteWakeCountdown == 1u) {
-      USB->CNTR &= ~USB_CNTR_RESUME;
+      FSDEV_REG->CNTR &= ~USB_CNTR_RESUME;
     }
     if (remoteWakeCountdown > 0u) {
       remoteWakeCountdown--;
     }
-    USB->ISTR = (fsdev_bus_t)~USB_ISTR_ESOF;
+    FSDEV_REG->ISTR = (fsdev_bus_t)~USB_ISTR_ESOF;
+  }
+
+  // loop to handle all pending CTR interrupts
+  while (FSDEV_REG->ISTR & USB_ISTR_CTR) {
+    // skip DIR bit, and use CTR TX/RX instead, since there is chance we have both TX/RX completed in one interrupt
+    uint32_t const ep_id = FSDEV_REG->ISTR & USB_ISTR_EP_ID;
+    uint32_t const ep_reg = ep_read(ep_id);
+
+    if (ep_reg & USB_EP_CTR_RX) {
+      #ifdef FSDEV_BUS_32BIT
+      /* https://www.st.com/resource/en/errata_sheet/es0561-stm32h503cbebkbrb-device-errata-stmicroelectronics.pdf
+       * https://www.st.com/resource/en/errata_sheet/es0587-stm32u535xx-and-stm32u545xx-device-errata-stmicroelectronics.pdf
+       * From H503/U535 errata: Buffer description table update completes after CTR interrupt triggers
+       * Description:
+       * - During OUT transfers, the correct transfer interrupt (CTR) is triggered a little before the last USB SRAM accesses
+       * have completed. If the software responds quickly to the interrupt, the full buffer contents may not be correct.
+       * Workaround:
+       * - Software should ensure that a small delay is included before accessing the SRAM contents. This delay
+       * should be 800 ns in Full Speed mode and 6.4 μs in Low Speed mode
+       * - Since H5 can run up to 250Mhz -> 1 cycle = 4ns. Per errata, we need to wait 200 cycles. Though executing code
+       * also takes time, so we'll wait 60 cycles (count = 20).
+       * - Since Low Speed mode is not supported/popular, we will ignore it for now.
+       *
+       * Note: this errata may also apply to G0, U5, H5 etc.
+       */
+      volatile uint32_t cycle_count = 20; // defined as PCD_RX_PMA_CNT in stm32 hal_driver
+      while (cycle_count > 0U) {
+        cycle_count--; // each count take 3 cycles (1 for sub, jump, and compare)
+      }
+      #endif
+
+      if (ep_reg & USB_EP_SETUP) {
+        handle_ctr_setup(ep_id); // CTR will be clear after copied setup packet
+      } else {
+        ep_write_clear_ctr(ep_id, TUSB_DIR_OUT);
+        handle_ctr_rx(ep_id);
+      }
+    }
+
+    if (ep_reg & USB_EP_CTR_TX) {
+      ep_write_clear_ctr(ep_id, TUSB_DIR_IN);
+      handle_ctr_tx(ep_id);
+    }
+  }
+
+  if (int_status & USB_ISTR_PMAOVR) {
+    TU_BREAKPOINT();
+    FSDEV_REG->ISTR = (fsdev_bus_t)~USB_ISTR_PMAOVR;
   }
 }
 
@@ -567,19 +468,17 @@ void dcd_int_handler(uint8_t rhport)
 
 // Invoked when a control transfer's status stage is complete.
 // May help DCD to prepare for next control transfer, this API is optional.
-void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const *request)
-{
+void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const *request) {
   (void)rhport;
 
   if (request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_DEVICE &&
       request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD &&
       request->bRequest == TUSB_REQ_SET_ADDRESS) {
     uint8_t const dev_addr = (uint8_t)request->wValue;
-
-    // Setting new address after the whole request is complete
-    USB->DADDR &= ~USB_DADDR_ADD;
-    USB->DADDR |= dev_addr; // leave the enable bit set
+    FSDEV_REG->DADDR = (USB_DADDR_EF | dev_addr);
   }
+
+  edpt0_prepare_setup();
 }
 
 /***
@@ -587,21 +486,19 @@ void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const *req
  * In case of double buffering, high 16bit is the address of 2nd buffer
  * During failure, TU_ASSERT is used. If this happens, rework/reallocate memory manually.
  */
-static uint32_t dcd_pma_alloc(uint16_t length, bool dbuf)
+static uint32_t dcd_pma_alloc(uint16_t len, bool dbuf)
 {
-  // Ensure allocated buffer is aligned
-#ifdef FSDEV_BUS_32BIT
-  length = (length + 3) & ~0x03;
-#else
-  length = (length + 1) & ~0x01;
-#endif
+  uint8_t blsize, num_block;
+  uint16_t aligned_len = pma_align_buffer_size(len, &blsize, &num_block);
+  (void) blsize;
+  (void) num_block;
 
   uint32_t addr = ep_buf_ptr;
-  ep_buf_ptr = (uint16_t)(ep_buf_ptr + length); // increment buffer pointer
+  ep_buf_ptr = (uint16_t)(ep_buf_ptr + aligned_len); // increment buffer pointer
 
   if (dbuf) {
     addr |= ((uint32_t)ep_buf_ptr) << 16;
-    ep_buf_ptr = (uint16_t)(ep_buf_ptr + length); // increment buffer pointer
+    ep_buf_ptr = (uint16_t)(ep_buf_ptr + aligned_len); // increment buffer pointer
   }
 
   // Verify packet buffer is not overflowed
@@ -648,34 +545,53 @@ static uint8_t dcd_ep_alloc(uint8_t ep_addr, uint8_t ep_type)
   TU_ASSERT(0);
 }
 
-// The STM32F0 doesn't seem to like |= or &= to manipulate the EP#R registers,
-// so I'm using the #define from HAL here, instead.
+void edpt0_open(uint8_t rhport) {
+  (void) rhport;
 
-bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *p_endpoint_desc)
-{
-  (void)rhport;
-  uint8_t const ep_addr = p_endpoint_desc->bEndpointAddress;
-  uint8_t const ep_idx = dcd_ep_alloc(ep_addr, p_endpoint_desc->bmAttributes.xfer);
-  uint8_t const dir = tu_edpt_dir(ep_addr);
-  const uint16_t packet_size = tu_edpt_packet_size(p_endpoint_desc);
-  const uint16_t buffer_size = pcd_aligned_buffer_size(packet_size);
-  uint16_t pma_addr;
-  uint32_t wType;
+  dcd_ep_alloc(0x0, TUSB_XFER_CONTROL);
+  dcd_ep_alloc(0x80, TUSB_XFER_CONTROL);
+
+  xfer_status[0][0].max_packet_size = CFG_TUD_ENDPOINT0_SIZE;
+  xfer_status[0][0].ep_idx = 0;
+
+  xfer_status[0][1].max_packet_size = CFG_TUD_ENDPOINT0_SIZE;
+  xfer_status[0][1].ep_idx = 0;
+
+  uint16_t pma_addr0 = dcd_pma_alloc(CFG_TUD_ENDPOINT0_SIZE, false);
+  uint16_t pma_addr1 = dcd_pma_alloc(CFG_TUD_ENDPOINT0_SIZE, false);
+
+  btable_set_addr(0, BTABLE_BUF_RX, pma_addr0);
+  btable_set_addr(0, BTABLE_BUF_TX, pma_addr1);
+
+  uint32_t ep_reg = ep_read(0) & ~USB_EPREG_MASK; // only get toggle bits
+  ep_reg |= USB_EP_CONTROL;
+  ep_change_status(&ep_reg, TUSB_DIR_IN, EP_STAT_NAK);
+  ep_change_status(&ep_reg, TUSB_DIR_OUT, EP_STAT_NAK);
+  // no need to explicitly set DTOG bits since we aren't masked DTOG bit
 
+  edpt0_prepare_setup(); // prepare for setup packet
+  ep_write(0, ep_reg, false);
+}
+
+bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *desc_ep) {
+  (void)rhport;
+  uint8_t const ep_addr = desc_ep->bEndpointAddress;
+  uint8_t const ep_num = tu_edpt_number(ep_addr);
+  tusb_dir_t const dir = tu_edpt_dir(ep_addr);
+  const uint16_t packet_size = tu_edpt_packet_size(desc_ep);
+  uint8_t const ep_idx = dcd_ep_alloc(ep_addr, desc_ep->bmAttributes.xfer);
   TU_ASSERT(ep_idx < FSDEV_EP_COUNT);
-  TU_ASSERT(buffer_size <= 64);
+
+  uint32_t ep_reg = ep_read(ep_idx) & ~USB_EPREG_MASK;
+  ep_reg |= tu_edpt_number(ep_addr) | USB_EP_CTR_TX | USB_EP_CTR_RX;
 
   // Set type
-  switch (p_endpoint_desc->bmAttributes.xfer) {
-    case TUSB_XFER_CONTROL:
-      wType = USB_EP_CONTROL;
-      break;
+  switch (desc_ep->bmAttributes.xfer) {
     case TUSB_XFER_BULK:
-      wType = USB_EP_CONTROL;
+      ep_reg |= USB_EP_BULK;
       break;
-
     case TUSB_XFER_INTERRUPT:
-      wType = USB_EP_INTERRUPT;
+      ep_reg |= USB_EP_INTERRUPT;
       break;
 
     default:
@@ -683,35 +599,35 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *p_endpoint_desc)
       TU_ASSERT(false);
   }
 
-  pcd_set_eptype(USB, ep_idx, wType);
-  pcd_set_ep_address(USB, ep_idx, tu_edpt_number(ep_addr));
-
   /* Create a packet memory buffer area. */
-  pma_addr = dcd_pma_alloc(buffer_size, false);
+  uint16_t pma_addr = dcd_pma_alloc(packet_size, false);
+  btable_set_addr(ep_idx, dir == TUSB_DIR_IN ? BTABLE_BUF_TX : BTABLE_BUF_RX, pma_addr);
 
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_num, dir);
+  xfer->max_packet_size = packet_size;
+  xfer->ep_idx = ep_idx;
+
+  ep_change_status(&ep_reg, dir, EP_STAT_NAK);
+  ep_change_dtog(&ep_reg, dir, 0);
+
+  // reserve other direction toggle bits
   if (dir == TUSB_DIR_IN) {
-    pcd_set_ep_tx_address(USB, ep_idx, pma_addr);
-    pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_NAK);
-    pcd_clear_tx_dtog(USB, ep_idx);
+    ep_reg &= ~(USB_EPRX_STAT | USB_EP_DTOG_RX);
   } else {
-    pcd_set_ep_rx_address(USB, ep_idx, pma_addr);
-    pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_NAK);
-    pcd_clear_rx_dtog(USB, ep_idx);
+    ep_reg &= ~(USB_EPTX_STAT | USB_EP_DTOG_TX);
   }
 
-  xfer_ctl_ptr(ep_addr)->max_packet_size = packet_size;
-  xfer_ctl_ptr(ep_addr)->ep_idx = ep_idx;
+  ep_write(ep_idx, ep_reg, true);
 
   return true;
 }
 
-void dcd_edpt_close_all(uint8_t rhport)
-{
-  (void)rhport;
+void dcd_edpt_close_all(uint8_t rhport) {
+  dcd_int_disable(rhport);
 
   for (uint32_t i = 1; i < FSDEV_EP_COUNT; i++) {
     // Reset endpoint
-    pcd_set_endpoint(USB, i, 0);
+    ep_write(i, 0, false);
     // Clear EP allocation status
     ep_alloc_status[i].ep_num = 0xFF;
     ep_alloc_status[i].ep_type = 0xFF;
@@ -719,508 +635,340 @@ void dcd_edpt_close_all(uint8_t rhport)
     ep_alloc_status[i].allocated[1] = false;
   }
 
+  dcd_int_enable(rhport);
+
   // Reset PMA allocation
-  ep_buf_ptr = DCD_STM32_BTABLE_BASE + 8 * CFG_TUD_ENDPPOINT_MAX + 2 * CFG_TUD_ENDPOINT0_SIZE;
+  ep_buf_ptr = FSDEV_BTABLE_BASE + 8 * CFG_TUD_ENDPPOINT_MAX + 2 * CFG_TUD_ENDPOINT0_SIZE;
 }
 
-/**
- * Close an endpoint.
- *
- * This function may be called with interrupts enabled or disabled.
- *
- * This also clears transfers in progress, should there be any.
- */
-void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr)
-{
+bool dcd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size) {
   (void)rhport;
 
-  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
-  uint8_t const ep_idx = xfer->ep_idx;
+  uint8_t const ep_num = tu_edpt_number(ep_addr);
   uint8_t const dir = tu_edpt_dir(ep_addr);
-
-  if (dir == TUSB_DIR_IN) {
-    pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_DIS);
-  } else {
-    pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_DIS);
-  }
-}
-
-bool dcd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size)
-{
-  (void)rhport;
-
   uint8_t const ep_idx = dcd_ep_alloc(ep_addr, TUSB_XFER_ISOCHRONOUS);
-  const uint16_t buffer_size = pcd_aligned_buffer_size(largest_packet_size);
 
   /* Create a packet memory buffer area. Enable double buffering for devices with 2048 bytes PMA,
      for smaller devices double buffering occupy too much space. */
 #if FSDEV_PMA_SIZE > 1024u
-  uint32_t pma_addr = dcd_pma_alloc(buffer_size, true);
+  uint32_t pma_addr = dcd_pma_alloc(largest_packet_size, true);
   uint16_t pma_addr2 = pma_addr >> 16;
 #else
-  uint32_t pma_addr = dcd_pma_alloc(buffer_size, true);
+  uint32_t pma_addr = dcd_pma_alloc(largest_packet_size, false);
   uint16_t pma_addr2 = pma_addr;
 #endif
-  pcd_set_ep_tx_address(USB, ep_idx, pma_addr);
-  pcd_set_ep_rx_address(USB, ep_idx, pma_addr2);
 
-  pcd_set_eptype(USB, ep_idx, USB_EP_ISOCHRONOUS);
+  btable_set_addr(ep_idx, 0, pma_addr);
+  btable_set_addr(ep_idx, 1, pma_addr2);
 
-  xfer_ctl_ptr(ep_addr)->ep_idx = ep_idx;
+  xfer_ctl_t* xfer = xfer_ctl_ptr(ep_num, dir);
+  xfer->ep_idx = ep_idx;
 
   return true;
 }
 
-bool dcd_edpt_iso_activate(uint8_t rhport, tusb_desc_endpoint_t const *p_endpoint_desc)
-{
+bool dcd_edpt_iso_activate(uint8_t rhport, tusb_desc_endpoint_t const *desc_ep) {
   (void)rhport;
-  uint8_t const ep_addr = p_endpoint_desc->bEndpointAddress;
-  uint8_t const ep_idx = xfer_ctl_ptr(ep_addr)->ep_idx;
-  uint8_t const dir = tu_edpt_dir(ep_addr);
-  const uint16_t packet_size = tu_edpt_packet_size(p_endpoint_desc);
+  uint8_t const ep_addr = desc_ep->bEndpointAddress;
+  uint8_t const ep_num = tu_edpt_number(ep_addr);
+  tusb_dir_t const dir = tu_edpt_dir(ep_addr);
+  xfer_ctl_t* xfer = xfer_ctl_ptr(ep_num, dir);
 
-  pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_DIS);
-  pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_DIS);
+  uint8_t const ep_idx = xfer->ep_idx;
 
-  pcd_set_ep_address(USB, ep_idx, tu_edpt_number(ep_addr));
+  xfer->max_packet_size = tu_edpt_packet_size(desc_ep);
 
-  pcd_clear_tx_dtog(USB, ep_idx);
-  pcd_clear_rx_dtog(USB, ep_idx);
+  uint32_t ep_reg = ep_read(ep_idx) & ~USB_EPREG_MASK;
+  ep_reg |= tu_edpt_number(ep_addr) | USB_EP_ISOCHRONOUS | USB_EP_CTR_TX | USB_EP_CTR_RX;
+  ep_change_status(&ep_reg, TUSB_DIR_IN, EP_STAT_DISABLED);
+  ep_change_status(&ep_reg, TUSB_DIR_OUT, EP_STAT_DISABLED);
+  ep_change_dtog(&ep_reg, dir, 0);
+  ep_change_dtog(&ep_reg, (tusb_dir_t)(1 - dir), 1);
 
-  if (dir == TUSB_DIR_IN) {
-    pcd_rx_dtog(USB, ep_idx);
-  } else {
-    pcd_tx_dtog(USB, ep_idx);
-  }
-
-  xfer_ctl_ptr(ep_addr)->max_packet_size = packet_size;
+  ep_write(ep_idx, ep_reg, true);
 
   return true;
 }
 
 // Currently, single-buffered, and only 64 bytes at a time (max)
+static void dcd_transmit_packet(xfer_ctl_t *xfer, uint16_t ep_ix) {
+  uint16_t len = tu_min16(xfer->total_len - xfer->queued_len, xfer->max_packet_size);
+  uint32_t ep_reg = ep_read(ep_ix) | USB_EP_CTR_TX | USB_EP_CTR_RX; // reserve CTR
 
-static void dcd_transmit_packet(xfer_ctl_t *xfer, uint16_t ep_ix)
-{
-  uint16_t len = (uint16_t)(xfer->total_len - xfer->queued_len);
-  if (len > xfer->max_packet_size) {
-    len = xfer->max_packet_size;
-  }
-
-  uint16_t ep_reg = pcd_get_endpoint(USB, ep_ix);
-  bool const is_iso = (ep_reg & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS;
-  uint16_t addr_ptr;
+  bool const is_iso = ep_is_iso(ep_reg);
 
+  uint8_t buf_id;
   if (is_iso) {
-    if (ep_reg & USB_EP_DTOG_TX) {
-      addr_ptr = pcd_get_ep_dbuf1_address(USB, ep_ix);
-      pcd_set_ep_tx_dbuf1_cnt(USB, ep_ix, len);
-    } else {
-      addr_ptr = pcd_get_ep_dbuf0_address(USB, ep_ix);
-      pcd_set_ep_tx_dbuf0_cnt(USB, ep_ix, len);
-    }
+    buf_id = (ep_reg & USB_EP_DTOG_TX) ? 1 : 0;
   } else {
-    addr_ptr = pcd_get_ep_tx_address(USB, ep_ix);
-    pcd_set_ep_tx_cnt(USB, ep_ix, len);
+    buf_id = BTABLE_BUF_TX;
   }
+  uint16_t addr_ptr = (uint16_t) btable_get_addr(ep_ix, buf_id);
 
   if (xfer->ff) {
     dcd_write_packet_memory_ff(xfer->ff, addr_ptr, len);
   } else {
     dcd_write_packet_memory(addr_ptr, &(xfer->buffer[xfer->queued_len]), len);
   }
-  xfer->queued_len = (uint16_t)(xfer->queued_len + len);
+  xfer->queued_len += len;
+
+  btable_set_count(ep_ix, buf_id, len);
+  ep_change_status(&ep_reg, TUSB_DIR_IN, EP_STAT_VALID);
 
-  dcd_int_disable(0);
-  pcd_set_ep_tx_status(USB, ep_ix, USB_EP_TX_VALID);
   if (is_iso) {
     xfer->iso_in_sending = true;
   }
-  dcd_int_enable(0);
+  ep_reg &= USB_EPREG_MASK | EP_STAT_MASK(TUSB_DIR_IN); // only change TX Status, reserve other toggle bits
+  ep_write(ep_ix, ep_reg, true);
 }
 
-static bool edpt_xfer(uint8_t rhport, uint8_t ep_addr)
-{
-  (void)rhport;
+static bool edpt_xfer(uint8_t rhport, uint8_t ep_num, tusb_dir_t dir) {
+  (void) rhport;
 
-  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_num, dir);
   uint8_t const ep_idx = xfer->ep_idx;
-  uint8_t const dir = tu_edpt_dir(ep_addr);
 
   if (dir == TUSB_DIR_IN) {
     dcd_transmit_packet(xfer, ep_idx);
   } else {
-    // A setup token can occur immediately after an OUT STATUS packet so make sure we have a valid
-    // buffer for the control endpoint.
-    if (ep_idx == 0 && xfer->buffer == NULL) {
-      xfer->buffer = (uint8_t *)_setup_packet;
-    }
+    uint32_t ep_reg = ep_read(ep_idx) | USB_EP_CTR_TX | USB_EP_CTR_RX; // reserve CTR
+    ep_reg &= USB_EPREG_MASK | EP_STAT_MASK(dir);
 
-    uint32_t cnt = (uint32_t ) tu_min16(xfer->total_len, xfer->max_packet_size);
-    uint16_t ep_reg = pcd_get_endpoint(USB, ep_idx);
+    uint16_t cnt = tu_min16(xfer->total_len, xfer->max_packet_size);
 
-    if ((ep_reg & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS) {
-      pcd_set_ep_rx_dbuf0_cnt(USB, ep_idx, cnt);
-      pcd_set_ep_rx_dbuf1_cnt(USB, ep_idx, cnt);
+    if (ep_is_iso(ep_reg)) {
+      btable_set_rx_bufsize(ep_idx, 0, cnt);
+      btable_set_rx_bufsize(ep_idx, 1, cnt);
     } else {
-      pcd_set_ep_rx_cnt(USB, ep_idx, cnt);
+      btable_set_rx_bufsize(ep_idx, BTABLE_BUF_RX, cnt);
     }
 
-    pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_VALID);
+    ep_change_status(&ep_reg, dir, EP_STAT_VALID);
+    ep_write(ep_idx, ep_reg, true);
   }
 
   return true;
 }
 
-bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t *buffer, uint16_t total_bytes)
-{
-  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
+bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t *buffer, uint16_t total_bytes) {
+  uint8_t const ep_num = tu_edpt_number(ep_addr);
+  tusb_dir_t const dir = tu_edpt_dir(ep_addr);
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_num, dir);
 
   xfer->buffer = buffer;
   xfer->ff = NULL;
   xfer->total_len = total_bytes;
   xfer->queued_len = 0;
 
-  return edpt_xfer(rhport, ep_addr);
+  return edpt_xfer(rhport, ep_num, dir);
 }
 
-bool dcd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t *ff, uint16_t total_bytes)
-{
-  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
+bool dcd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t *ff, uint16_t total_bytes) {
+  uint8_t const ep_num = tu_edpt_number(ep_addr);
+  tusb_dir_t const dir = tu_edpt_dir(ep_addr);
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_num, dir);
+
   xfer->buffer = NULL;
   xfer->ff = ff;
   xfer->total_len = total_bytes;
   xfer->queued_len = 0;
 
-  return edpt_xfer(rhport, ep_addr);
+  return edpt_xfer(rhport, ep_num, dir);
 }
 
-void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
-{
+void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr) {
   (void)rhport;
-
-  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
+  uint8_t const ep_num = tu_edpt_number(ep_addr);
+  tusb_dir_t const dir = tu_edpt_dir(ep_addr);
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_num, dir);
   uint8_t const ep_idx = xfer->ep_idx;
-  uint8_t const dir = tu_edpt_dir(ep_addr);
 
-  if (dir == TUSB_DIR_IN) {
-    pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_STALL);
-  } else {
-    pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_STALL);
-  }
+  uint32_t ep_reg = ep_read(ep_idx) | USB_EP_CTR_TX | USB_EP_CTR_RX; // reserve CTR bits
+  ep_reg &= USB_EPREG_MASK | EP_STAT_MASK(dir);
+  ep_change_status(&ep_reg, dir, EP_STAT_STALL);
+
+  ep_write(ep_idx, ep_reg, true);
 }
 
-void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
-{
+void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr) {
   (void)rhport;
 
-  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
+  uint8_t const ep_num = tu_edpt_number(ep_addr);
+  tusb_dir_t const dir = tu_edpt_dir(ep_addr);
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_num, dir);
   uint8_t const ep_idx = xfer->ep_idx;
-  uint8_t const dir = tu_edpt_dir(ep_addr);
 
-  if (dir == TUSB_DIR_IN) { // IN
-    if (pcd_get_eptype(USB, ep_idx) != USB_EP_ISOCHRONOUS) {
-      pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_NAK);
-    }
+  uint32_t ep_reg = ep_read(ep_idx) | USB_EP_CTR_TX | USB_EP_CTR_RX; // reserve CTR bits
+  ep_reg &= USB_EPREG_MASK | EP_STAT_MASK(dir) | EP_DTOG_MASK(dir);
 
-    /* Reset to DATA0 if clearing stall condition. */
-    pcd_clear_tx_dtog(USB, ep_idx);
-  } else { // OUT
-    if (pcd_get_eptype(USB, ep_idx) != USB_EP_ISOCHRONOUS) {
-      pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_NAK);
-    }
-    /* Reset to DATA0 if clearing stall condition. */
-    pcd_clear_rx_dtog(USB, ep_idx);
+  if (!ep_is_iso(ep_reg)) {
+    ep_change_status(&ep_reg, dir, EP_STAT_NAK);
   }
+  ep_change_dtog(&ep_reg, dir, 0); // Reset to DATA0
+  ep_write(ep_idx, ep_reg, true);
 }
 
-#ifdef FSDEV_BUS_32BIT
-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);
+//--------------------------------------------------------------------+
+// PMA read/write
+//--------------------------------------------------------------------+
 
-  for (uint32_t n = wNBytes / 4; n > 0; --n) {
-    *dst32++ = tu_unaligned_read32(srcVal);
-    srcVal += 4;
-  }
+// Write to packet memory area (PMA) from user memory
+// - Packet memory must be either strictly 16-bit or 32-bit depending on FSDEV_BUS_32BIT
+// - Uses unaligned for RAM (since M0 cannot access unaligned address)
+static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t nbytes) {
+  if (nbytes == 0) return true;
+  uint32_t n_write = nbytes / FSDEV_BUS_SIZE;
 
-  wNBytes = wNBytes & 0x03;
-  if (wNBytes) {
-    uint32_t wrVal = *srcVal;
-    wNBytes--;
+  fsdev_pma_buf_t* pma_buf = PMA_BUF_AT(dst);
+  const uint8_t *src8 = src;
 
-    if (wNBytes) {
-      wrVal |= *++srcVal << 8;
-      wNBytes--;
+  while (n_write--) {
+    pma_buf->value = fsdevbus_unaligned_read(src8);
+    src8 += FSDEV_BUS_SIZE;
+    pma_buf++;
+  }
 
-      if (wNBytes) {
-        wrVal |= *++srcVal << 16;
-      }
+  // odd bytes e.g 1 for 16-bit or 1-3 for 32-bit
+  uint16_t odd = nbytes & (FSDEV_BUS_SIZE - 1);
+  if (odd) {
+    fsdev_bus_t temp = 0;
+    for(uint16_t i = 0; i < odd; i++) {
+      temp |= *src8++ << (i * 8);
     }
-
-    *dst32 = wrVal;
+    pma_buf->value = temp;
   }
 
   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).
- *        This uses byte-access for user memory (so support non-aligned buffers)
- *        and 16-bit access for packet memory.
- * @param   dst, byte address in PMA; must be 16-bit aligned
- * @param   src pointer to user memory area.
- * @param   wPMABufAddr address into PMA.
- * @param   wNBytes no. of bytes to be copied.
- * @retval None
- */
-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;
-  const uint8_t *srcVal;
 
-  // The GCC optimizer will combine access to 32-bit sizes if we let it. Force
-  // it volatile so that it won't do that.
-  __IO uint16_t *pdwVal;
+// Read from packet memory area (PMA) to user memory.
+// - Packet memory must be either strictly 16-bit or 32-bit depending on FSDEV_BUS_32BIT
+// - Uses unaligned for RAM (since M0 cannot access unaligned address)
+static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t nbytes) {
+  if (nbytes == 0) return true;
+  uint32_t n_read = nbytes / FSDEV_BUS_SIZE;
 
-  srcVal = src;
-  pdwVal = &pma[FSDEV_PMA_STRIDE * (dst >> 1)];
+  fsdev_pma_buf_t* pma_buf = PMA_BUF_AT(src);
+  uint8_t *dst8 = (uint8_t *)dst;
 
-  while (n--) {
-    temp1 = (uint16_t)*srcVal;
-    srcVal++;
-    temp2 = temp1 | ((uint16_t)(((uint16_t)(*srcVal)) << 8U));
-    *pdwVal = temp2;
-    pdwVal += FSDEV_PMA_STRIDE;
-    srcVal++;
+  while (n_read--) {
+    fsdevbus_unaligned_write(dst8, (fsdev_bus_t ) pma_buf->value);
+    dst8 += FSDEV_BUS_SIZE;
+    pma_buf++;
   }
 
-  if (wNBytes) {
-    temp1 = (uint16_t) *srcVal;
-    *pdwVal = temp1;
+  // odd bytes e.g 1 for 16-bit or 1-3 for 32-bit
+  uint16_t odd = nbytes & (FSDEV_BUS_SIZE - 1);
+  if (odd) {
+    fsdev_bus_t temp = pma_buf->value;
+    while (odd--) {
+      *dst8++ = (uint8_t) (temp & 0xfful);
+      temp >>= 8;
+    }
   }
 
   return true;
 }
-#endif
 
-/**
- * @brief Copy from FIFO to packet memory area (PMA).
- *        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_write_packet_memory_ff(tu_fifo_t *ff, uint16_t dst, uint16_t wNBytes)
-{
+// Write to PMA from FIFO
+static bool dcd_write_packet_memory_ff(tu_fifo_t *ff, uint16_t dst, uint16_t wNBytes) {
+  if (wNBytes == 0) return true;
+
   // Since we copy from a ring buffer FIFO, a wrap might occur making it necessary to conduct two copies
   tu_fifo_buffer_info_t info;
   tu_fifo_get_read_info(ff, &info);
 
-  uint16_t cnt_lin = TU_MIN(wNBytes, info.len_lin);
-  uint16_t cnt_wrap = TU_MIN(wNBytes - cnt_lin, info.len_wrap);
+  uint16_t cnt_lin = tu_min16(wNBytes, info.len_lin);
+  uint16_t cnt_wrap = tu_min16(wNBytes - cnt_lin, info.len_wrap);
+  uint16_t const cnt_total = cnt_lin + cnt_wrap;
 
   // 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 aligned (dst aligned to 16 or 32 bit)
-#ifdef FSDEV_BUS_32BIT
-  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
-    dcd_write_packet_memory(dst, info.ptr_lin, cnt_lin & ~0x01);
-    dst += cnt_lin & ~0x01;
+  // last lin byte will be combined with wrapped part To ensure PMA is always access aligned
+  uint16_t lin_even = cnt_lin & ~(FSDEV_BUS_SIZE - 1);
+  uint16_t lin_odd = cnt_lin & (FSDEV_BUS_SIZE - 1);
+  uint8_t const *src8 = (uint8_t const*) info.ptr_lin;
 
-    // Copy last linear byte & first wrapped byte
-    uint16_t tmp = ((uint8_t *)info.ptr_lin)[cnt_lin - 1] | ((uint16_t)(((uint8_t *)info.ptr_wrap)[0]) << 8U);
-    dcd_write_packet_memory(dst, &tmp, 2);
-    dst += 2;
+  // write even linear part
+  dcd_write_packet_memory(dst, src8, lin_even);
+  dst += lin_even;
+  src8 += lin_even;
 
-    // Copy rest of wrapped byte
-    dcd_write_packet_memory(dst, ((uint8_t *)info.ptr_wrap) + 1, cnt_wrap - 1);
-  }
-#endif
-  else {
-    // Copy linear part
-    dcd_write_packet_memory(dst, info.ptr_lin, cnt_lin);
-    dst += info.len_lin;
-
-    if (info.len_wrap) {
-      // Copy wrapped byte
-      dcd_write_packet_memory(dst, info.ptr_wrap, cnt_wrap);
+  if (lin_odd == 0) {
+    src8 = (uint8_t const*) info.ptr_wrap;
+  } else {
+    // Combine last linear bytes + first wrapped bytes to form fsdev bus width data
+    fsdev_bus_t temp = 0;
+    uint16_t i;
+    for(i = 0; i < lin_odd; i++) {
+      temp |= *src8++ << (i * 8);
     }
-  }
 
-  tu_fifo_advance_read_pointer(ff, cnt_lin + cnt_wrap);
-
-  return true;
-}
-
-#ifdef FSDEV_BUS_32BIT
-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);
+    src8 = (uint8_t const*) info.ptr_wrap;
+    for(; i < FSDEV_BUS_SIZE && cnt_wrap > 0; i++, cnt_wrap--) {
+      temp |= *src8++ << (i * 8);
+    }
 
-  for (uint32_t n = wNBytes / 4; n > 0; --n) {
-    tu_unaligned_write32(dstVal, *src32++);
-    dstVal += 4;
+    dcd_write_packet_memory(dst, &temp, FSDEV_BUS_SIZE);
+    dst += FSDEV_BUS_SIZE;
   }
 
-  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);
-      }
-    }
-  }
+  // write the rest of the wrapped part
+  dcd_write_packet_memory(dst, src8, cnt_wrap);
 
+  tu_fifo_advance_read_pointer(ff, cnt_total);
   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, 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
-  // it volatile so that it won't do that.
-  __IO const uint16_t *pdwVal;
-  uint32_t temp;
-
-  pdwVal = &pma[FSDEV_PMA_STRIDE * (src >> 1)];
-  uint8_t *dstVal = (uint8_t *)dst;
-
-  while (n--) {
-    temp = *pdwVal;
-    pdwVal += FSDEV_PMA_STRIDE;
-    *dstVal++ = ((temp >> 0) & 0xFF);
-    *dstVal++ = ((temp >> 8) & 0xFF);
-  }
 
-  if (wNBytes & 0x01) {
-    temp = *pdwVal;
-    pdwVal += FSDEV_PMA_STRIDE;
-    *dstVal++ = ((temp >> 0) & 0xFF);
-  }
-  return true;
-}
-#endif
+// Read from PMA to FIFO
+static bool dcd_read_packet_memory_ff(tu_fifo_t *ff, uint16_t src, uint16_t wNBytes) {
+  if (wNBytes == 0) return true;
 
-/**
- * @brief Copy a buffer from user packet memory area (PMA) to FIFO.
- *        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_ff(tu_fifo_t *ff, uint16_t src, uint16_t wNBytes)
-{
   // Since we copy into a ring buffer FIFO, a wrap might occur making it necessary to conduct two copies
   // Check for first linear part
   tu_fifo_buffer_info_t info;
   tu_fifo_get_write_info(ff, &info); // We want to read from the FIFO
 
-  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 aligned (src aligned to 16 or 32 bit)
-#ifdef FSDEV_BUS_32BIT
-  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;
+  uint16_t cnt_lin = tu_min16(wNBytes, info.len_lin);
+  uint16_t cnt_wrap = tu_min16(wNBytes - cnt_lin, info.len_wrap);
+  uint16_t cnt_total = cnt_lin + cnt_wrap;
 
-    // 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;
-    }
+  // 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 aligned
 
-    // 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
-    dcd_read_packet_memory(info.ptr_lin, src, cnt_lin & ~0x01);
-    src += cnt_lin & ~0x01;
+  uint16_t lin_even = cnt_lin & ~(FSDEV_BUS_SIZE - 1);
+  uint16_t lin_odd = cnt_lin & (FSDEV_BUS_SIZE - 1);
+  uint8_t *dst8 = (uint8_t *) info.ptr_lin;
 
-    // Copy last linear byte & first wrapped byte
-    uint8_t tmp[2];
-    dcd_read_packet_memory(tmp, src, 2);
-    src += 2;
+  // read even linear part
+  dcd_read_packet_memory(dst8, src, lin_even);
+  dst8 += lin_even;
+  src += lin_even;
 
-    ((uint8_t *)info.ptr_lin)[cnt_lin - 1] = tmp[0];
-    ((uint8_t *)info.ptr_wrap)[0] = tmp[1];
+  if (lin_odd == 0) {
+    dst8 = (uint8_t *) info.ptr_wrap;
+  } else {
+    // Combine last linear bytes + first wrapped bytes to form fsdev bus width data
+    fsdev_bus_t temp;
+    dcd_read_packet_memory(&temp, src, FSDEV_BUS_SIZE);
+    src += FSDEV_BUS_SIZE;
 
-    // Copy rest of wrapped byte
-    dcd_read_packet_memory(((uint8_t *)info.ptr_wrap) + 1, src, cnt_wrap - 1);
-  }
-#endif
-  else {
-    // Copy linear part
-    dcd_read_packet_memory(info.ptr_lin, src, cnt_lin);
-    src += cnt_lin;
+    uint16_t i;
+    for (i = 0; i < lin_odd; i++) {
+      *dst8++ = (uint8_t) (temp & 0xfful);
+      temp >>= 8;
+    }
 
-    if (info.len_wrap) {
-      // Copy wrapped byte
-      dcd_read_packet_memory(info.ptr_wrap, src, cnt_wrap);
+    dst8 = (uint8_t *) info.ptr_wrap;
+    for (; i < FSDEV_BUS_SIZE && cnt_wrap > 0; i++, cnt_wrap--) {
+      *dst8++ = (uint8_t) (temp & 0xfful);
+      temp >>= 8;
     }
   }
 
-  tu_fifo_advance_write_pointer(ff, cnt_lin + cnt_wrap);
+  // read the rest of the wrapped part
+  dcd_read_packet_memory(dst8, src, cnt_wrap);
 
+  tu_fifo_advance_write_pointer(ff, cnt_total);
   return true;
 }
 
diff --git a/src/portable/st/stm32_fsdev/fsdev_ch32.h b/src/portable/st/stm32_fsdev/fsdev_ch32.h
index 85fe3266c..518197c47 100644
--- a/src/portable/st/stm32_fsdev/fsdev_ch32.h
+++ b/src/portable/st/stm32_fsdev/fsdev_ch32.h
@@ -36,52 +36,26 @@
 
 #include "common/tusb_compiler.h"
 
-#if CFG_TUSB_MCU == OPT_MCU_CH32V20X
-  #include <ch32v20x.h>
+// https://github.com/openwch/ch32v307/pull/90
+// https://github.com/openwch/ch32v20x/pull/12
+#ifdef __GNUC__
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wstrict-prototypes"
+#endif
 
-#elif CFG_TUSB_MCU == OPT_MCU_CH32F20X
+#if CFG_TUSB_MCU == OPT_MCU_CH32F20X
   #include <ch32f20x.h>
+#elif CFG_TUSB_MCU == OPT_MCU_CH32V20X
+  #include <ch32v20x.h>
 #endif
 
-#define FSDEV_PMA_SIZE (512u)
-
-// volatile 32-bit aligned
-#define _va32     volatile TU_ATTR_ALIGNED(4)
-
-typedef struct {
-  _va32 uint16_t EP0R;            // 00: USB Endpoint 0 register
-  _va32 uint16_t EP1R;            // 04: USB Endpoint 1 register
-  _va32 uint16_t EP2R;            // 08: USB Endpoint 2 register
-  _va32 uint16_t EP3R;            // 0C: USB Endpoint 3 register
-  _va32 uint16_t EP4R;            // 10: USB Endpoint 4 register
-  _va32 uint16_t EP5R;            // 14: USB Endpoint 5 register
-  _va32 uint16_t EP6R;            // 18: USB Endpoint 6 register
-  _va32 uint16_t EP7R;            // 1C: USB Endpoint 7 register
-  _va32 uint16_t RESERVED7[16];   // Reserved
-  _va32 uint16_t CNTR;            // 40: Control register
-  _va32 uint16_t ISTR;            // 44: Interrupt status register
-  _va32 uint16_t FNR;             // 48: Frame number register
-  _va32 uint16_t DADDR;           // 4C: Device address register
-  _va32 uint16_t BTABLE;          // 50: Buffer Table address register
-} USB_TypeDef;
-
-TU_VERIFY_STATIC(sizeof(USB_TypeDef) == 0x54, "Size is not correct");
-TU_VERIFY_STATIC(offsetof(USB_TypeDef, CNTR) == 0x40, "Wrong offset");
-
-#define USB_BASE            (APB1PERIPH_BASE + 0x00005C00UL) /*!< USB_IP Peripheral Registers base address */
-#define USB_PMAADDR         (APB1PERIPH_BASE + 0x00006000UL) /*!< USB_IP Packet Memory Area base address */
-#define USB                 ((USB_TypeDef *)USB_BASE)
+#ifdef __GNUC__
+#pragma GCC diagnostic pop
+#endif
 
-/******************************************************************************/
-/*                                                                            */
-/*                         USB Device General registers                       */
-/*                                                                            */
-/******************************************************************************/
-#define USB_CNTR                             (USB_BASE + 0x40U)             /*!< Control register */
-#define USB_ISTR                             (USB_BASE + 0x44U)             /*!< Interrupt status register */
-#define USB_FNR                              (USB_BASE + 0x48U)             /*!< Frame number register */
-#define USB_DADDR                            (USB_BASE + 0x4CU)             /*!< Device address register */
-#define USB_BTABLE                           (USB_BASE + 0x50U)             /*!< Buffer Table address register */
+#define FSDEV_PMA_SIZE (512u)
+#define FSDEV_REG_BASE  (APB1PERIPH_BASE + 0x00005C00UL)
+#define FSDEV_PMA_BASE  (APB1PERIPH_BASE + 0x00006000UL)
 
 /****************************  ISTR interrupt events  *************************/
 #define USB_ISTR_CTR                         ((uint16_t)0x8000U)               /*!< Correct TRansfer (clear-only bit) */
diff --git a/src/portable/st/stm32_fsdev/fsdev_common.h b/src/portable/st/stm32_fsdev/fsdev_common.h
deleted file mode 100644
index af5d8afab..000000000
--- a/src/portable/st/stm32_fsdev/fsdev_common.h
+++ /dev/null
@@ -1,391 +0,0 @@
-/*
- * The MIT License (MIT)
- *
- * Copyright(c) 2016 STMicroelectronics
- * Copyright(c) N Conrad
- * Copyright (c) 2024, hathach (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.
- *
- */
-
-#ifndef TUSB_FSDEV_COMMON_H
-#define TUSB_FSDEV_COMMON_H
-
-#ifdef __cplusplus
- extern "C" {
-#endif
-
-#include "stdint.h"
-
-// FSDEV_PMA_SIZE is PMA buffer size in bytes.
-// On 512-byte devices, access with a stride of two words (use every other 16-bit address)
-// On 1024-byte devices, access with a stride of one word (use every 16-bit address)
-
-// For purposes of accessing the packet
-#if ((FSDEV_PMA_SIZE) == 512u)
-  #define FSDEV_PMA_STRIDE  (2u)
-#elif ((FSDEV_PMA_SIZE) == 1024u)
-  #define FSDEV_PMA_STRIDE  (1u)
-#endif
-
-// The fsdev_bus_t type can be used for both register and PMA access necessities
-// 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 FSDEV_BUS_32BIT
-typedef uint32_t fsdev_bus_t;
-static volatile uint32_t * const pma32 = (volatile uint32_t*)USB_PMAADDR;
-
-#else
-typedef uint16_t fsdev_bus_t;
-// Volatile is also needed to prevent the optimizer from changing access to 32-bit (as 32-bit access is forbidden)
-static volatile uint16_t * const pma = (volatile uint16_t*)USB_PMAADDR;
-
-TU_ATTR_ALWAYS_INLINE static inline volatile uint16_t * pcd_btable_word_ptr(USB_TypeDef * USBx, size_t x) {
-  size_t total_word_offset = (((USBx)->BTABLE)>>1) + x;
-  total_word_offset *= FSDEV_PMA_STRIDE;
-  return &(pma[total_word_offset]);
-}
-
-TU_ATTR_ALWAYS_INLINE static inline volatile 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 volatile 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) {
-  /* The STM32 full speed USB peripheral supports only a limited set of
-   * buffer sizes given by the RX buffer entry format in the USB_BTABLE. */
-  uint16_t blocksize = (size > 62) ? 32 : 2;
-
-  // Round up while dividing requested size by blocksize
-  uint16_t numblocks = (size + blocksize - 1) / blocksize ;
-
-  return numblocks * blocksize;
-}
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wRegValue) {
-#ifdef FSDEV_BUS_32BIT
-  (void) USBx;
-  volatile uint32_t *reg = (volatile uint32_t *)(USB_DRD_BASE + bEpIdx*4);
-  *reg = wRegValue;
-#else
-  volatile uint16_t *reg = (volatile uint16_t *)((&USBx->EP0R) + bEpIdx*2u);
-  *reg = (uint16_t)wRegValue;
-#endif
-}
-
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx) {
-#ifdef FSDEV_BUS_32BIT
-  (void) USBx;
-  volatile const uint32_t *reg = (volatile const uint32_t *)(USB_DRD_BASE + bEpIdx*4);
-#else
-  volatile const uint16_t *reg = (volatile const uint16_t *)((&USBx->EP0R) + bEpIdx*2u);
-#endif
-  return *reg;
-}
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_eptype(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wType) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  regVal &= (uint32_t)USB_EP_T_MASK;
-  regVal |= wType;
-  regVal |= USB_EP_CTR_RX | USB_EP_CTR_TX; // These clear on write0, so must set high
-  pcd_set_endpoint(USBx, bEpIdx, regVal);
-}
-
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_eptype(USB_TypeDef * USBx, uint32_t bEpIdx) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  regVal &= USB_EP_T_FIELD;
-  return regVal;
-}
-
-/**
-  * @brief  Clears bit CTR_RX / CTR_TX in the endpoint register.
-  * @param  USBx USB peripheral instance register address.
-  * @param  bEpIdx Endpoint Number.
-  * @retval None
-  */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpIdx) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  regVal &= USB_EPREG_MASK;
-  regVal &= ~USB_EP_CTR_RX;
-  regVal |= USB_EP_CTR_TX; // preserve CTR_TX (clears on writing 0)
-  pcd_set_endpoint(USBx, bEpIdx, regVal);
-}
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpIdx) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  regVal &= USB_EPREG_MASK;
-  regVal &= ~USB_EP_CTR_TX;
-  regVal |= USB_EP_CTR_RX; // preserve CTR_RX (clears on writing 0)
-  pcd_set_endpoint(USBx, bEpIdx,regVal);
-}
-
-/**
-  * @brief  gets counter of the tx buffer.
-  * @param  USBx USB peripheral instance register address.
-  * @param  bEpIdx Endpoint Number.
-  * @retval Counter value
-  */
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx) {
-#ifdef FSDEV_BUS_32BIT
-  (void) USBx;
-  return (pma32[2*bEpIdx] & 0x03FF0000) >> 16;
-#else
-  volatile const 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 FSDEV_BUS_32BIT
-  (void) USBx;
-  return (pma32[2*bEpIdx + 1] & 0x03FF0000) >> 16;
-#else
-  volatile const uint16_t *regPtr = pcd_ep_rx_cnt_ptr(USBx, bEpIdx);
-  return *regPtr & 0x3ffU;
-#endif
-}
-
-#define pcd_get_ep_dbuf0_cnt pcd_get_ep_tx_cnt
-#define pcd_get_ep_dbuf1_cnt pcd_get_ep_rx_cnt
-
-/**
-  * @brief  Sets address in an endpoint register.
-  * @param  USBx USB peripheral instance register address.
-  * @param  bEpIdx Endpoint Number.
-  * @param  bAddr Address.
-  * @retval None
-  */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_address(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t bAddr) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  regVal &= USB_EPREG_MASK;
-  regVal |= bAddr;
-  regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
-  pcd_set_endpoint(USBx, bEpIdx,regVal);
-}
-
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_address(USB_TypeDef * USBx, uint32_t bEpIdx) {
-#ifdef FSDEV_BUS_32BIT
-  (void) USBx;
-  return pma32[2*bEpIdx] & 0x0000FFFFu ;
-#else
-  return *pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 0u);
-#endif
-}
-
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_address(USB_TypeDef * USBx, uint32_t bEpIdx) {
-#ifdef FSDEV_BUS_32BIT
-  (void) USBx;
-  return pma32[2*bEpIdx + 1] & 0x0000FFFFu;
-#else
-  return *pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 2u);
-#endif
-}
-
-#define pcd_get_ep_dbuf0_address pcd_get_ep_tx_address
-#define pcd_get_ep_dbuf1_address pcd_get_ep_rx_address
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_address(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t addr) {
-#ifdef FSDEV_BUS_32BIT
-  (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 void pcd_set_ep_rx_address(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t addr) {
-#ifdef FSDEV_BUS_32BIT
-  (void) USBx;
-  pma32[2*bEpIdx + 1] = (pma32[2*bEpIdx + 1] & 0xFFFF0000u) | (addr & 0x0000FFFCu);
-#else
-  *pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 2u) = addr;
-#endif
-}
-
-#define pcd_set_ep_dbuf0_address pcd_set_ep_tx_address
-#define pcd_set_ep_dbuf1_address pcd_set_ep_rx_address
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount) {
-#ifdef FSDEV_BUS_32BIT
-  (void) USBx;
-  pma32[2*bEpIdx] = (pma32[2*bEpIdx] & ~0x03FF0000u) | ((wCount & 0x3FFu) << 16);
-#else
-  volatile uint16_t * reg = pcd_ep_tx_cnt_ptr(USBx, bEpIdx);
-  *reg = (uint16_t) (*reg & (uint16_t) ~0x3FFU) | (wCount & 0x3FFU);
-#endif
-}
-
-#define pcd_set_ep_tx_dbuf0_cnt pcd_set_ep_tx_cnt
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_dbuf1_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount) {
-#ifdef FSDEV_BUS_32BIT
-  (void) USBx;
-  pma32[2*bEpIdx + 1] = (pma32[2*bEpIdx + 1] & ~0x03FF0000u) | ((wCount & 0x3FFu) << 16);
-#else
-  volatile 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_blsize_num_blocks(USB_TypeDef * USBx, uint32_t rxtx_idx,
-                                                                      uint32_t blocksize, uint32_t numblocks) {
-  /* Encode into register. When BLSIZE==1, we need to subtract 1 block count */
-#ifdef FSDEV_BUS_32BIT
-  (void) USBx;
-  pma32[rxtx_idx] = (pma32[rxtx_idx] & 0x0000FFFFu) | (blocksize << 31) | ((numblocks - blocksize) << 26);
-#else
-  volatile 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_bufsize(USB_TypeDef * USBx, uint32_t rxtx_idx, uint32_t wCount) {
-  wCount = pcd_aligned_buffer_size(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_rx_dbuf0_cnt(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_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount) {
-  pcd_set_ep_bufsize(USBx, 2*bEpIdx + 1, wCount);
-}
-
-#define pcd_set_ep_rx_dbuf1_cnt pcd_set_ep_rx_cnt
-
-/**
-  * @brief  sets the status for tx transfer (bits STAT_TX[1:0]).
-  * @param  USBx USB peripheral instance register address.
-  * @param  bEpIdx Endpoint Number.
-  * @param  wState new state
-  * @retval None
-  */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wState) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  regVal &= USB_EPTX_DTOGMASK;
-
-  /* toggle first bit ? */
-  if((USB_EPTX_DTOG1 & (wState))!= 0U)
-  {
-    regVal ^= USB_EPTX_DTOG1;
-  }
-  /* toggle second bit ?  */
-  if((USB_EPTX_DTOG2 & ((uint32_t)(wState)))!= 0U)
-  {
-    regVal ^= USB_EPTX_DTOG2;
-  }
-
-  regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
-  pcd_set_endpoint(USBx, bEpIdx, regVal);
-}
-
-/**
-  * @brief  sets the status for rx transfer (bits STAT_TX[1:0])
-  * @param  USBx USB peripheral instance register address.
-  * @param  bEpIdx Endpoint Number.
-  * @param  wState new state
-  * @retval None
-  */
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wState) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  regVal &= USB_EPRX_DTOGMASK;
-
-  /* toggle first bit ? */
-  if((USB_EPRX_DTOG1 & wState)!= 0U) {
-    regVal ^= USB_EPRX_DTOG1;
-  }
-  /* toggle second bit ? */
-  if((USB_EPRX_DTOG2 & wState)!= 0U) {
-    regVal ^= USB_EPRX_DTOG2;
-  }
-
-  regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
-  pcd_set_endpoint(USBx, bEpIdx, regVal);
-}
-
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpIdx) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  return (regVal & USB_EPRX_STAT) >> (12u);
-}
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  regVal &= USB_EPREG_MASK;
-  regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX|USB_EP_DTOG_RX;
-  pcd_set_endpoint(USBx, bEpIdx, regVal);
-}
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  regVal &= USB_EPREG_MASK;
-  regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX|USB_EP_DTOG_TX;
-  pcd_set_endpoint(USBx, bEpIdx, regVal);
-}
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  if((regVal & USB_EP_DTOG_RX) != 0) {
-    pcd_rx_dtog(USBx,bEpIdx);
-  }
-}
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  if((regVal & USB_EP_DTOG_TX) != 0) {
-    pcd_tx_dtog(USBx,bEpIdx);
-  }
-}
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_kind(USB_TypeDef * USBx,  uint32_t bEpIdx) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  regVal |= USB_EP_KIND;
-  regVal &= USB_EPREG_MASK;
-  regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
-  pcd_set_endpoint(USBx, bEpIdx, regVal);
-}
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_ep_kind(USB_TypeDef * USBx, uint32_t bEpIdx) {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  regVal &= USB_EPKIND_MASK;
-  regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
-  pcd_set_endpoint(USBx, bEpIdx, regVal);
-}
-
-#ifdef __cplusplus
- }
-#endif
-
-#endif
diff --git a/src/portable/st/stm32_fsdev/fsdev_stm32.h b/src/portable/st/stm32_fsdev/fsdev_stm32.h
index b3fa11b88..99fe8d55f 100644
--- a/src/portable/st/stm32_fsdev/fsdev_stm32.h
+++ b/src/portable/st/stm32_fsdev/fsdev_stm32.h
@@ -35,6 +35,7 @@
 #if CFG_TUSB_MCU == OPT_MCU_STM32F0
   #include "stm32f0xx.h"
   #define FSDEV_PMA_SIZE (1024u)
+  #define FSDEV_REG_BASE USB_BASE
   // F0x2 models are crystal-less
   // All have internal D+ pull-up
   // 070RB:    2 x 16 bits/word memory     LPM Support, BCD Support
@@ -81,12 +82,9 @@
 
 #elif CFG_TUSB_MCU == OPT_MCU_STM32G0
   #include "stm32g0xx.h"
-  #define FSDEV_BUS_32BIT
   #define FSDEV_PMA_SIZE (2048u)
-  #undef USB_PMAADDR
-  #define USB_PMAADDR USB_DRD_PMAADDR
-  #define USB_TypeDef USB_DRD_TypeDef
-  #define EP0R CHEP0R
+  #define USB USB_DRD_FS
+
   #define USB_EP_CTR_RX USB_EP_VTRX
   #define USB_EP_CTR_TX USB_EP_VTTX
   #define USB_EP_T_FIELD USB_CHEP_UTYPE
@@ -99,7 +97,6 @@
   #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
@@ -109,17 +106,9 @@
 
 #elif CFG_TUSB_MCU == OPT_MCU_STM32H5
   #include "stm32h5xx.h"
-  #define FSDEV_BUS_32BIT
-
-  #if !defined(USB_DRD_BASE) && defined(USB_DRD_FS_BASE)
-  #define USB_DRD_BASE USB_DRD_FS_BASE
-  #endif
-
   #define FSDEV_PMA_SIZE (2048u)
-  #undef USB_PMAADDR
-  #define USB_PMAADDR USB_DRD_PMAADDR
-  #define USB_TypeDef USB_DRD_TypeDef
-  #define EP0R CHEP0R
+  #define USB USB_DRD_FS
+
   #define USB_EP_CTR_RX USB_EP_VTRX
   #define USB_EP_CTR_TX USB_EP_VTTX
   #define USB_EP_T_FIELD USB_CHEP_UTYPE
@@ -132,7 +121,6 @@
   #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
@@ -143,9 +131,8 @@
 #elif CFG_TUSB_MCU == OPT_MCU_STM32WB
   #include "stm32wbxx.h"
   #define FSDEV_PMA_SIZE (1024u)
-  /* ST provided header has incorrect value */
-  #undef USB_PMAADDR
-  #define USB_PMAADDR USB1_PMAADDR
+  /* ST provided header has incorrect value of USB_PMAADDR */
+  #define FSDEV_PMA_BASE USB1_PMAADDR
 
 #elif CFG_TUSB_MCU == OPT_MCU_STM32L4
   #include "stm32l4xx.h"
@@ -159,9 +146,58 @@
     #define USB_PMAADDR (USB_BASE + (USB_PMAADDR_NS - USB_BASE_NS))
   #endif
 
+#elif CFG_TUSB_MCU == OPT_MCU_STM32U5
+  #include "stm32u5xx.h"
+  #define FSDEV_PMA_SIZE (2048u)
+  #define USB USB_DRD_FS
+
+  #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_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
+
 #else
   #error You are using an untested or unimplemented STM32 variant. Please update the driver.
-  // This includes L1x0, L1x1, L1x2, L4x2 and L4x3, G1x1, G1x3, and G1x4
+  // This includes U0
+#endif
+
+//--------------------------------------------------------------------+
+// Register and PMA Base Address
+//--------------------------------------------------------------------+
+#ifndef FSDEV_REG_BASE
+#if defined(USB_BASE)
+  #define FSDEV_REG_BASE USB_BASE
+#elif defined(USB_DRD_BASE)
+  #define FSDEV_REG_BASE USB_DRD_BASE
+#elif defined(USB_DRD_FS_BASE)
+  #define FSDEV_REG_BASE USB_DRD_FS_BASE
+#else
+  #error "FSDEV_REG_BASE not defined"
+#endif
+#endif
+
+#ifndef FSDEV_PMA_BASE
+#if defined(USB_PMAADDR)
+  #define FSDEV_PMA_BASE USB_PMAADDR
+#elif defined(USB_DRD_PMAADDR)
+  #define FSDEV_PMA_BASE USB_DRD_PMAADDR
+#else
+  #error "FSDEV_PMA_BASE not defined"
+#endif
 #endif
 
 // This checks if the device has "LPM"
@@ -211,6 +247,8 @@ static const IRQn_Type fsdev_irq[] = {
   #elif CFG_TUSB_MCU == OPT_MCU_STM32WB
     USB_HP_IRQn,
     USB_LP_IRQn,
+  #elif CFG_TUSB_MCU == OPT_MCU_STM32U5
+    USB_IRQn,
   #else
     #error Unknown arch in USB driver
   #endif
diff --git a/src/portable/st/stm32_fsdev/fsdev_type.h b/src/portable/st/stm32_fsdev/fsdev_type.h
new file mode 100644
index 000000000..cf36576bb
--- /dev/null
+++ b/src/portable/st/stm32_fsdev/fsdev_type.h
@@ -0,0 +1,307 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright(c) N Conrad
+ * Copyright(c) 2024, hathach (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 TUSB_FSDEV_TYPE_H
+#define TUSB_FSDEV_TYPE_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#include "stdint.h"
+
+// If sharing with CAN, one can set this to be non-zero to give CAN space where it wants it
+// Both of these MUST be a multiple of 2, and are in byte units.
+#ifndef FSDEV_BTABLE_BASE
+#define FSDEV_BTABLE_BASE 0U
+#endif
+
+TU_VERIFY_STATIC(FSDEV_BTABLE_BASE % 8 == 0, "BTABLE base must be aligned to 8 bytes");
+
+// FSDEV_PMA_SIZE is PMA buffer size in bytes.
+// - 512-byte devices, access with a stride of two words (use every other 16-bit address)
+// - 1024-byte devices, access with a stride of one word (use every 16-bit address)
+// - 2048-byte devices, access with 32-bit address
+
+// For purposes of accessing the packet
+#if FSDEV_PMA_SIZE == 512
+  // 1x16 bit / word access scheme
+  #define FSDEV_PMA_STRIDE  2
+  #define pma_access_scheme TU_ATTR_ALIGNED(4)
+#elif FSDEV_PMA_SIZE == 1024
+  // 2x16 bit / word access scheme
+  #define FSDEV_PMA_STRIDE  1
+  #define pma_access_scheme
+#elif FSDEV_PMA_SIZE == 2048
+  // 32 bit access scheme
+  #define FSDEV_BUS_32BIT
+  #define FSDEV_PMA_STRIDE  1
+  #define pma_access_scheme
+#endif
+
+// The fsdev_bus_t type can be used for both register and PMA access necessities
+#ifdef FSDEV_BUS_32BIT
+  typedef uint32_t fsdev_bus_t;
+  #define fsdevbus_unaligned_read(_addr)           tu_unaligned_read32(_addr)
+  #define fsdevbus_unaligned_write(_addr, _value)  tu_unaligned_write32(_addr, _value)
+#else
+  typedef uint16_t fsdev_bus_t;
+  #define fsdevbus_unaligned_read(_addr)           tu_unaligned_read16(_addr)
+  #define fsdevbus_unaligned_write(_addr, _value)  tu_unaligned_write16(_addr, _value)
+#endif
+
+enum {
+  FSDEV_BUS_SIZE = sizeof(fsdev_bus_t),
+};
+
+//--------------------------------------------------------------------+
+// BTable Typedef
+//--------------------------------------------------------------------+
+enum {
+  BTABLE_BUF_TX = 0,
+  BTABLE_BUF_RX = 1
+};
+
+// hardware limit endpoint
+#define FSDEV_EP_COUNT 8
+
+// Buffer Table is located in Packet Memory Area (PMA) and therefore its address access is forced to either
+// 16-bit or 32-bit depending on FSDEV_BUS_32BIT.
+// 0: TX (IN), 1: RX (OUT)
+typedef union {
+  // data is strictly 16-bit access (address could be 32-bit aligned)
+  struct {
+    volatile pma_access_scheme uint16_t addr;
+    volatile pma_access_scheme uint16_t count;
+  } ep16[FSDEV_EP_COUNT][2];
+
+  // strictly 32-bit access
+  struct {
+    volatile uint32_t count_addr;
+  } ep32[FSDEV_EP_COUNT][2];
+} fsdev_btable_t;
+
+TU_VERIFY_STATIC(sizeof(fsdev_btable_t) == FSDEV_EP_COUNT*8*FSDEV_PMA_STRIDE, "size is not correct");
+TU_VERIFY_STATIC(FSDEV_BTABLE_BASE + FSDEV_EP_COUNT*8 <= FSDEV_PMA_SIZE, "BTABLE does not fit in PMA RAM");
+
+#define FSDEV_BTABLE ((volatile fsdev_btable_t*) (FSDEV_PMA_BASE + FSDEV_PMA_STRIDE*(FSDEV_BTABLE_BASE)))
+
+typedef struct {
+  volatile pma_access_scheme fsdev_bus_t value;
+} fsdev_pma_buf_t;
+
+#define PMA_BUF_AT(_addr) ((fsdev_pma_buf_t*) (FSDEV_PMA_BASE + FSDEV_PMA_STRIDE*(_addr)))
+
+//--------------------------------------------------------------------+
+// Registers Typedef
+//--------------------------------------------------------------------+
+
+// volatile 32-bit aligned
+#define _va32     volatile TU_ATTR_ALIGNED(4)
+
+typedef struct {
+  struct {
+    _va32 fsdev_bus_t reg;
+  }ep[FSDEV_EP_COUNT];
+
+  _va32 uint32_t RESERVED7[8];       // Reserved
+  _va32 fsdev_bus_t CNTR;            // 40: Control register
+  _va32 fsdev_bus_t ISTR;            // 44: Interrupt status register
+  _va32 fsdev_bus_t FNR;             // 48: Frame number register
+  _va32 fsdev_bus_t DADDR;           // 4C: Device address register
+  _va32 fsdev_bus_t BTABLE;          // 50: Buffer Table address register (16-bit only)
+  _va32 fsdev_bus_t LPMCSR;          // 54: LPM Control and Status Register (32-bit only)
+  _va32 fsdev_bus_t BCDR;            // 58: Battery Charging Detector Register (32-bit only)
+} fsdev_regs_t;
+
+TU_VERIFY_STATIC(offsetof(fsdev_regs_t, CNTR) == 0x40, "Wrong offset");
+TU_VERIFY_STATIC(sizeof(fsdev_regs_t) == 0x5C, "Size is not correct");
+
+#define FSDEV_REG ((fsdev_regs_t*) FSDEV_REG_BASE)
+
+
+#ifndef USB_EPTX_STAT
+#define USB_EPTX_STAT 0x0030U
+#endif
+
+#ifndef USB_EPRX_STAT
+#define USB_EPRX_STAT 0x3000U
+#endif
+
+#ifndef USB_EPTX_STAT_Pos
+#define USB_EPTX_STAT_Pos    4u
+#endif
+
+#ifndef USB_EP_DTOG_TX_Pos
+#define USB_EP_DTOG_TX_Pos   6u
+#endif
+
+#ifndef USB_EP_CTR_TX_Pos
+#define USB_EP_CTR_TX_Pos    7u
+#endif
+
+typedef enum {
+  EP_STAT_DISABLED = 0,
+  EP_STAT_STALL = 1,
+  EP_STAT_NAK = 2,
+  EP_STAT_VALID = 3
+}ep_stat_t;
+
+#define EP_STAT_MASK(_dir)  (3u << (USB_EPTX_STAT_Pos + ((_dir) == TUSB_DIR_IN ? 0 : 8)))
+#define EP_DTOG_MASK(_dir)  (1u << (USB_EP_DTOG_TX_Pos + ((_dir) == TUSB_DIR_IN ? 0 : 8)))
+
+//--------------------------------------------------------------------+
+// Endpoint Helper
+// - CTR is write 0 to clear
+// - DTOG and STAT are write 1 to toggle
+//--------------------------------------------------------------------+
+
+TU_ATTR_ALWAYS_INLINE static inline uint32_t ep_read(uint32_t ep_id) {
+  return FSDEV_REG->ep[ep_id].reg;
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void ep_write(uint32_t ep_id, uint32_t value, bool need_exclusive) {
+  if (need_exclusive) {
+    dcd_int_disable(0);
+  }
+
+  FSDEV_REG->ep[ep_id].reg = (fsdev_bus_t) value;
+
+  if (need_exclusive) {
+    dcd_int_enable(0);
+  }
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void ep_write_clear_ctr(uint32_t ep_id, tusb_dir_t dir) {
+  uint32_t reg = FSDEV_REG->ep[ep_id].reg;
+  reg |= USB_EP_CTR_TX | USB_EP_CTR_RX;
+  reg &= USB_EPREG_MASK;
+  reg &= ~(1 << (USB_EP_CTR_TX_Pos + (dir == TUSB_DIR_IN ? 0 : 8)));
+  ep_write(ep_id, reg, false);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void ep_change_status(uint32_t* reg, tusb_dir_t dir, ep_stat_t state) {
+  *reg ^= (state << (USB_EPTX_STAT_Pos + (dir == TUSB_DIR_IN ? 0 : 8)));
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void ep_change_dtog(uint32_t* reg, tusb_dir_t dir, uint8_t state) {
+  *reg ^= (state << (USB_EP_DTOG_TX_Pos + (dir == TUSB_DIR_IN ? 0 : 8)));
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool ep_is_iso(uint32_t reg) {
+  return (reg & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS;
+}
+
+//--------------------------------------------------------------------+
+// BTable Helper
+//--------------------------------------------------------------------+
+
+TU_ATTR_ALWAYS_INLINE static inline uint32_t btable_get_addr(uint32_t ep_id, uint8_t buf_id) {
+#ifdef FSDEV_BUS_32BIT
+  return FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr & 0x0000FFFFu;
+#else
+  return FSDEV_BTABLE->ep16[ep_id][buf_id].addr;
+#endif
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void btable_set_addr(uint32_t ep_id, uint8_t buf_id, uint16_t addr) {
+#ifdef FSDEV_BUS_32BIT
+  uint32_t count_addr = FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr;
+  count_addr = (count_addr & 0xFFFF0000u) | (addr & 0x0000FFFCu);
+  FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr = count_addr;
+#else
+  FSDEV_BTABLE->ep16[ep_id][buf_id].addr = addr;
+#endif
+}
+
+TU_ATTR_ALWAYS_INLINE static inline uint16_t btable_get_count(uint32_t ep_id, uint8_t buf_id) {
+  uint16_t count;
+#ifdef FSDEV_BUS_32BIT
+  count = (FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr >> 16);
+#else
+  count = FSDEV_BTABLE->ep16[ep_id][buf_id].count;
+#endif
+  return count & 0x3FFU;
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void btable_set_count(uint32_t ep_id, uint8_t buf_id, uint16_t byte_count) {
+#ifdef FSDEV_BUS_32BIT
+  uint32_t count_addr = FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr;
+  count_addr = (count_addr & ~0x03FF0000u) | ((byte_count & 0x3FFu) << 16);
+  FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr = count_addr;
+#else
+  uint16_t cnt = FSDEV_BTABLE->ep16[ep_id][buf_id].count;
+  cnt = (cnt & ~0x3FFU) | (byte_count & 0x3FFU);
+  FSDEV_BTABLE->ep16[ep_id][buf_id].count = cnt;
+#endif
+}
+
+/* Aligned buffer size according to hardware */
+TU_ATTR_ALWAYS_INLINE static inline uint16_t pma_align_buffer_size(uint16_t size, uint8_t* blsize, uint8_t* num_block) {
+  /* The STM32 full speed USB peripheral supports only a limited set of
+   * buffer sizes given by the RX buffer entry format in the USB_BTABLE. */
+  uint16_t block_in_bytes;
+  if (size > 62) {
+    block_in_bytes = 32;
+    *blsize = 1;
+    *num_block = tu_div_ceil(size, 32);
+  } else {
+    block_in_bytes = 2;
+    *blsize = 0;
+    *num_block = tu_div_ceil(size, 2);
+  }
+
+  return (*num_block) * block_in_bytes;
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void btable_set_rx_bufsize(uint32_t ep_id, uint8_t buf_id, uint16_t wCount) {
+  uint8_t blsize, num_block;
+  (void) pma_align_buffer_size(wCount, &blsize, &num_block);
+
+  /* Encode into register. When BLSIZE==1, we need to subtract 1 block count */
+  uint16_t bl_nb = (blsize << 15) | ((num_block - blsize) << 10);
+  if (bl_nb == 0) {
+    // zlp but 0 is invalid value, set blsize to 1 (32 bytes)
+    // Note: lower value can cause PMAOVR on setup with ch32v203
+    bl_nb = 1 << 15;
+  }
+
+#ifdef FSDEV_BUS_32BIT
+  uint32_t count_addr = FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr;
+  count_addr = (bl_nb << 16) | (count_addr & 0x0000FFFFu);
+  FSDEV_BTABLE->ep32[ep_id][buf_id].count_addr = count_addr;
+#else
+  FSDEV_BTABLE->ep16[ep_id][buf_id].count = bl_nb;
+#endif
+
+}
+
+#ifdef __cplusplus
+ }
+#endif
+
+#endif
diff --git a/src/portable/synopsys/dwc2/dcd_dwc2.c b/src/portable/synopsys/dwc2/dcd_dwc2.c
index dbcd586c5..9a38b46dc 100644
--- a/src/portable/synopsys/dwc2/dcd_dwc2.c
+++ b/src/portable/synopsys/dwc2/dcd_dwc2.c
@@ -1195,25 +1195,13 @@ void dcd_int_handler(uint8_t rhport) {
   //  }
 }
 
-#if defined(TUP_USBIP_DWC2_TEST_MODE) && CFG_TUD_TEST_MODE
-
-bool dcd_check_test_mode_support(test_mode_t test_selector) {
-  // Check if test mode selector is unsupported
-  if (TEST_FORCE_ENABLE < test_selector || TEST_J > test_selector) {
-    return false;
-  }
-
-  return true;
-}
-
-void dcd_enter_test_mode(uint8_t rhport, test_mode_t test_selector) {
-  // Get port address...
+#if CFG_TUD_TEST_MODE
+void dcd_enter_test_mode(uint8_t rhport, tusb_feature_test_mode_t test_selector) {
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
 
   // Enable the test mode
-  dwc2->dctl = (dwc2->dctl & ~DCTL_TCTL_Msk) | (test_selector << DCTL_TCTL_Pos);
+  dwc2->dctl = (dwc2->dctl & ~DCTL_TCTL_Msk) | (((uint8_t) test_selector) << DCTL_TCTL_Pos);
 }
-
-#endif /* TUP_USBIP_DWC2_TEST_MODE && CFG_TUD_TEST_MODE */
+#endif
 
 #endif
diff --git a/src/portable/synopsys/dwc2/dwc2_esp32.h b/src/portable/synopsys/dwc2/dwc2_esp32.h
index c50dd66b8..932f52f40 100644
--- a/src/portable/synopsys/dwc2/dwc2_esp32.h
+++ b/src/portable/synopsys/dwc2/dwc2_esp32.h
@@ -35,6 +35,7 @@
 #include "esp_intr_alloc.h"
 #include "soc/periph_defs.h"
 //#include "soc/usb_periph.h"
+#include "freertos/task.h"
 
 #define DWC2_REG_BASE       0x60080000UL
 #define DWC2_EP_MAX         6             // USB_OUT_EP_NUM. TODO ESP32Sx only has 5 tx fifo (5 endpoint IN)
diff --git a/src/portable/wch/ch32_usbfs_reg.h b/src/portable/wch/ch32_usbfs_reg.h
index 0a50a6169..68be64f5e 100644
--- a/src/portable/wch/ch32_usbfs_reg.h
+++ b/src/portable/wch/ch32_usbfs_reg.h
@@ -28,15 +28,79 @@
 #ifndef USB_CH32_USBFS_REG_H
 #define USB_CH32_USBFS_REG_H
 
-#if CFG_TUSB_MCU == OPT_MCU_CH32V307
-  #include <ch32v30x.h>
-  #define USBHD_IRQn OTG_FS_IRQn
+// https://github.com/openwch/ch32v307/pull/90
+// https://github.com/openwch/ch32v20x/pull/12
+#ifdef __GNUC__
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wstrict-prototypes"
+#endif
+
+#if CFG_TUSB_MCU == OPT_MCU_CH32F20X
+  #include <ch32f20x.h>
+#elif CFG_TUSB_MCU == OPT_MCU_CH32V103
+  #include <ch32v10x.h>
+  typedef struct
+  {
+    __IO uint8_t  BASE_CTRL;
+    __IO uint8_t  UDEV_CTRL;
+    __IO uint8_t  INT_EN;
+    __IO uint8_t  DEV_ADDR;
+    __IO uint8_t  Reserve0;
+    __IO uint8_t  MIS_ST;
+    __IO uint8_t  INT_FG;
+    __IO uint8_t  INT_ST;
+    __IO uint32_t RX_LEN;
+    __IO uint8_t  UEP4_1_MOD;
+    __IO uint8_t  UEP2_3_MOD;
+    __IO uint8_t  UEP5_6_MOD;
+    __IO uint8_t  UEP7_MOD;
+    __IO uint32_t UEP0_DMA;
+    __IO uint32_t UEP1_DMA;
+    __IO uint32_t UEP2_DMA;
+    __IO uint32_t UEP3_DMA;
+    __IO uint32_t UEP4_DMA;
+    __IO uint32_t UEP5_DMA;
+    __IO uint32_t UEP6_DMA;
+    __IO uint32_t UEP7_DMA;
+    __IO uint16_t UEP0_TX_LEN;
+    __IO uint8_t  UEP0_TX_CTRL;
+    __IO uint8_t  UEP0_RX_CTRL;
+    __IO uint16_t UEP1_TX_LEN;
+    __IO uint8_t  UEP1_TX_CTRL;
+    __IO uint8_t  UEP1_RX_CTRL;
+    __IO uint16_t UEP2_TX_LEN;
+    __IO uint8_t  UEP2_TX_CTRL;
+    __IO uint8_t  UEP2_RX_CTRL;
+    __IO uint16_t UEP3_TX_LEN;
+    __IO uint8_t  UEP3_TX_CTRL;
+    __IO uint8_t  UEP3_RX_CTRL;
+    __IO uint16_t UEP4_TX_LEN;
+    __IO uint8_t  UEP4_TX_CTRL;
+    __IO uint8_t  UEP4_RX_CTRL;
+    __IO uint16_t UEP5_TX_LEN;
+    __IO uint8_t  UEP5_TX_CTRL;
+    __IO uint8_t  UEP5_RX_CTRL;
+    __IO uint16_t UEP6_TX_LEN;
+    __IO uint8_t  UEP6_TX_CTRL;
+    __IO uint8_t  UEP6_RX_CTRL;
+    __IO uint16_t UEP7_TX_LEN;
+    __IO uint8_t  UEP7_TX_CTRL;
+    __IO uint8_t  UEP7_RX_CTRL;
+    __IO uint32_t Reserve1;
+    __IO uint32_t OTG_CR;
+    __IO uint32_t OTG_SR;
+  } USBOTG_FS_TypeDef;
 
+  #define USBOTG_FS ((USBOTG_FS_TypeDef *) 0x40023400)
 #elif CFG_TUSB_MCU == OPT_MCU_CH32V20X
   #include <ch32v20x.h>
+#elif CFG_TUSB_MCU == OPT_MCU_CH32V307
+  #include <ch32v30x.h>
+  #define USBHD_IRQn OTG_FS_IRQn
+#endif
 
-#elif CFG_TUSB_MCU == OPT_MCU_CH32F20X
-  #include <ch32f20x.h>
+#ifdef __GNUC__
+#pragma GCC diagnostic pop
 #endif
 
 // CTRL
diff --git a/src/portable/wch/ch32_usbhs_reg.h b/src/portable/wch/ch32_usbhs_reg.h
index 130e4f223..87300b497 100644
--- a/src/portable/wch/ch32_usbhs_reg.h
+++ b/src/portable/wch/ch32_usbhs_reg.h
@@ -28,12 +28,24 @@
 #ifndef USB_CH32_USBHS_REG_H
 #define USB_CH32_USBHS_REG_H
 
+// https://github.com/openwch/ch32v307/pull/90
+// https://github.com/openwch/ch32v20x/pull/12
+#ifdef __GNUC__
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wstrict-prototypes"
+#endif
+
 #if CFG_TUSB_MCU == OPT_MCU_CH32V307
   #include <ch32v30x.h>
 #elif CFG_TUSB_MCU == OPT_MCU_CH32F20X
   #include <ch32f20x.h>
 #endif
 
+#ifdef __GNUC__
+#pragma GCC diagnostic pop
+#endif
+
+
 /******************* GLOBAL ******************/
 
 // USB CONTROL