Merge pull request #2739 from hathach/enhance-fsdev

Enhance fsdev

7 files changed, 667 insertions, 905 deletions

diff --git a/src/device/dcd.h b/src/device/dcd.h
index 5356e9be1..41e0fbee3 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -40,19 +40,15 @@
 //--------------------------------------------------------------------+
 
 typedef enum {
-  DCD_EVENT_INVALID = 0,
-  DCD_EVENT_BUS_RESET,
-  DCD_EVENT_UNPLUGGED,
-  DCD_EVENT_SOF,
-  DCD_EVENT_SUSPEND, // TODO LPM Sleep L1 support
-  DCD_EVENT_RESUME,
-
-  DCD_EVENT_SETUP_RECEIVED,
-  DCD_EVENT_XFER_COMPLETE,
-
-  // Not an DCD event, just a convenient way to defer ISR function
-  USBD_EVENT_FUNC_CALL,
-
+  DCD_EVENT_INVALID = 0,    // 0
+  DCD_EVENT_BUS_RESET,      // 1
+  DCD_EVENT_UNPLUGGED,      // 2
+  DCD_EVENT_SOF,            // 3
+  DCD_EVENT_SUSPEND,        // 4 TODO LPM Sleep L1 support
+  DCD_EVENT_RESUME,         // 5
+  DCD_EVENT_SETUP_RECEIVED, // 6
+  DCD_EVENT_XFER_COMPLETE,  // 7
+  USBD_EVENT_FUNC_CALL,     // 8 Not an DCD event, just a convenient way to defer ISR function
   DCD_EVENT_COUNT
 } dcd_eventid_t;
 
@@ -184,7 +180,7 @@ void dcd_edpt_clear_stall     (uint8_t rhport, uint8_t ep_addr);
 TU_ATTR_WEAK bool dcd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size);
 
 // Configure and enable an ISO endpoint according to descriptor
-TU_ATTR_WEAK bool dcd_edpt_iso_activate(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc);
+TU_ATTR_WEAK bool dcd_edpt_iso_activate(uint8_t rhport, tusb_desc_endpoint_t const * desc_ep);
 
 //--------------------------------------------------------------------+
 // Event API (implemented by stack)
diff --git a/src/device/usbd.c b/src/device/usbd.c
index b93216006..d9b20d26d 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -120,7 +120,6 @@ typedef struct {
   };
   volatile uint8_t cfg_num; // current active configuration (0x00 is not configured)
   uint8_t speed;
-  volatile uint8_t setup_count;
   volatile uint8_t sof_consumer;
 
   uint8_t itf2drv[CFG_TUD_INTERFACE_MAX];   // map interface number to driver (0xff is invalid)
@@ -131,6 +130,7 @@ typedef struct {
 }usbd_device_t;
 
 tu_static usbd_device_t _usbd_dev;
+static volatile uint8_t _usbd_queued_setup;
 
 //--------------------------------------------------------------------+
 // Class Driver
@@ -459,6 +459,7 @@ bool tud_init(uint8_t rhport) {
   TU_LOG_INT(CFG_TUD_LOG_LEVEL, sizeof(tu_edpt_stream_t));
 
   tu_varclr(&_usbd_dev);
+  _usbd_queued_setup = 0;
 
 #if OSAL_MUTEX_REQUIRED
   // Init device mutex
@@ -594,9 +595,10 @@ void tud_task_ext(uint32_t timeout_ms, bool in_isr) {
         break;
 
       case DCD_EVENT_SETUP_RECEIVED:
-        _usbd_dev.setup_count--;
+        TU_ASSERT(_usbd_queued_setup > 0,);
+        _usbd_queued_setup--;
         TU_LOG_BUF(CFG_TUD_LOG_LEVEL, &event.setup_received, 8);
-        if (_usbd_dev.setup_count) {
+        if (_usbd_queued_setup) {
           TU_LOG_USBD("  Skipped since there is other SETUP in queue\r\n");
           break;
         }
@@ -1199,7 +1201,8 @@ TU_ATTR_FAST_FUNC void dcd_event_handler(dcd_event_t const* event, bool in_isr)
       break;
 
     case DCD_EVENT_SETUP_RECEIVED:
-      _usbd_dev.setup_count++;
+      // TU_ASSERT(event->setup_received.bRequest != 0,);
+      _usbd_queued_setup++;
       send = true;
       break;
 
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index 39f33bc34..cff328418 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -123,27 +123,13 @@
   #error "Unknown USB IP"
 #endif
 
-#include "fsdev_common.h"
+#include "fsdev_type.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");
 
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
@@ -168,11 +154,8 @@ 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
 
 //--------------------------------------------------------------------+
@@ -180,99 +163,85 @@ 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, uint8_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);
+
 //--------------------------------------------------------------------+
 // 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. */
-
+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;
+  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 (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) && !defined(STM32U5) // 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);
   }
 
-  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;
+  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
@@ -282,35 +251,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
@@ -321,35 +270,25 @@ 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;
-  }
+static void handle_ctr_tx(uint32_t ep_id) {
+  uint32_t ep_reg = ep_read(ep_id) & USB_EPREG_MASK;
 
-  /* clear int flag */
-  pcd_clear_tx_ep_ctr(USB, EPindex);
+  // Verify the CTR bit is set. This was in the ST Micro code, but I'm not sure it's actually necessary?
+  TU_VERIFY(ep_reg & USB_EP_CTR_TX, );
 
-  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
+  uint8_t const ep_num = ep_reg & USB_EPADDR_FIELD;
+  uint8_t ep_addr = (ep_reg & USB_EPADDR_FIELD) | TUSB_DIR_IN_MASK;
+  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)
@@ -357,25 +296,24 @@ 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); // also clear CTR bit
   } else {
     dcd_event_xfer_complete(0, ep_addr, xfer->total_len, XFER_RESULT_SUCCESS, true);
+
+    // Clear CTR TX and reserved CTR RX
+    ep_reg = (ep_reg & ~USB_EP_CTR_TX) | USB_EP_CTR_RX;
+
+    ep_write(ep_id, ep_reg);
   }
 }
 
 // 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)
-{
+static void handle_ctr_rx(uint32_t ep_id) {
 #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
@@ -397,116 +335,82 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
   }
 #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;
+  uint32_t ep_reg = ep_read(ep_id);
 
-  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
+  // Verify the CTR bit is set. This was in the ST Micro code, but I'm not sure it's actually necessary?
+  TU_VERIFY(ep_reg & USB_EP_CTR_RX, );
+  ep_reg = (ep_reg & ~USB_EP_CTR_RX) | USB_EP_CTR_TX; // Clear CTR RX and reserved CTR TX
 
-  // 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;
-  }
+  uint8_t const ep_num = ep_reg & USB_EPADDR_FIELD;
 
-  if ((ep_addr == 0U) && ((wEPRegVal & USB_EP_SETUP) != 0U)) {
-    /* Setup packet */
-    uint32_t count = pcd_get_ep_rx_cnt(USB, EPindex);
+  if (ep_reg & USB_EP_SETUP) {
+    uint32_t count = btable_get_count(ep_id, BTABLE_BUF_RX);
     // 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
+      uint16_t rx_addr = btable_get_addr(ep_id, BTABLE_BUF_RX);
+      uint32_t setup_packet[2];
+      dcd_read_packet_memory(setup_packet, rx_addr, 8);
+      dcd_event_setup_received(0, (uint8_t*) setup_packet, true);
+
+      // Reset EP to NAK (in case it had been stalling)
+      ep_reg = ep_add_status(ep_reg, TUSB_DIR_IN, EP_STAT_NAK);
+      ep_reg = ep_add_status(ep_reg, TUSB_DIR_OUT, EP_STAT_NAK);
+
+      ep_reg = ep_add_dtog(ep_reg, TUSB_DIR_IN, 1);
+      ep_reg = ep_add_dtog(ep_reg, TUSB_DIR_OUT, 1);
+    } else {
+      ep_reg &= USB_EPREG_MASK; // reversed all toggle
     }
   } else {
-    // Clear RX CTR interrupt flag
-    if (ep_addr != 0u) {
-      pcd_clear_rx_ep_ctr(USB, EPindex);
-    }
+    ep_reg &= USB_EPRX_STAT | USB_EPREG_MASK; // reversed all toggle except RX Status
 
-    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);
-      }
+    bool const is_iso = ep_is_iso(ep_reg);
+    xfer_ctl_t *xfer = xfer_ctl_ptr(ep_num, TUSB_DIR_OUT);
+
+    uint8_t buf_id;
+    if (is_iso) {
+      buf_id = (ep_reg & USB_EP_DTOG_RX) ? 0 : 1; // ISO are double buffered
     } else {
-      count = pcd_get_ep_rx_cnt(USB, EPindex);
-      addr = pcd_get_ep_rx_address(USB, EPindex);
+      buf_id = BTABLE_BUF_RX;
     }
+    uint32_t rx_count = btable_get_count(ep_id, buf_id);
+    uint16_t pma_addr = (uint16_t) btable_get_addr(ep_id, buf_id);
 
-    TU_ASSERT(count <= xfer->max_packet_size, /**/);
-
-    if (count != 0U) {
+    if (rx_count != 0) {
       if (xfer->ff) {
-        dcd_read_packet_memory_ff(xfer->ff, addr, count);
+        dcd_read_packet_memory_ff(xfer->ff, pma_addr, rx_count);
       } else {
-        dcd_read_packet_memory(&(xfer->buffer[xfer->queued_len]), addr, count);
+        dcd_read_packet_memory(xfer->buffer + xfer->queued_len, pma_addr, rx_count);
       }
 
-      xfer->queued_len = (uint16_t)(xfer->queued_len + count);
+      xfer->queued_len = (uint16_t)(xfer->queued_len + rx_count);
     }
 
-    if ((count < xfer->max_packet_size) || (xfer->queued_len == xfer->total_len)) {
+    if ((rx_count < xfer->max_packet_size) || (xfer->queued_len == xfer->total_len)) {
+      uint8_t const ep_addr = ep_num;
       // all bytes received or short packet
       dcd_event_xfer_complete(0, ep_addr, xfer->queued_len, XFER_RESULT_SUCCESS, true);
+
+      if (ep_num == 0) {
+        // prepared for status packet
+        btable_set_rx_bufsize(ep_id, BTABLE_BUF_RX, CFG_TUD_ENDPOINT0_SIZE);
+      }
+      ep_reg = ep_add_status(ep_reg, TUSB_DIR_OUT, EP_STAT_NAK);
     } 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);
+      // 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);
       }
-      pcd_set_ep_rx_status(USB, EPindex, USB_EP_RX_VALID);
+      ep_reg = ep_add_status(ep_reg, TUSB_DIR_OUT, EP_STAT_VALID);
     }
   }
 
-  // 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);
-  }
-}
-
-static void dcd_ep_ctr_handler(void)
-{
-  uint32_t wIstr;
-
-  /* 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);
-    }
-  }
+  ep_write(ep_id, ep_reg);
 }
 
-void dcd_int_handler(uint8_t rhport)
-{
-  (void)rhport;
-
-  uint32_t int_status = USB->ISTR;
+void dcd_int_handler(uint8_t rhport) {
+  uint32_t int_status = FSDEV_REG->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 )
@@ -517,29 +421,23 @@ void dcd_int_handler(uint8_t rhport)
 
   /* Put SOF flag at the beginning of ISR in case to get least amount of jitter if it is used for timing purposes */
   if (int_status & USB_ISTR_SOF) {
-    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);
   }
 
@@ -548,22 +446,35 @@ 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 (int_status & USB_ISTR_CTR) {
+    uint32_t const ep_id = int_status & USB_ISTR_EP_ID;
+
+    if ((int_status & USB_ISTR_DIR) == 0U) {
+      handle_ctr_tx(ep_id); // TX/IN
+    } else {
+      handle_ctr_rx(ep_id); // RX/OUT or both (RX/TX !!)
+    }
+
+    int_status = FSDEV_REG->ISTR;
   }
 }
 
@@ -573,18 +484,14 @@ 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);
   }
 }
 
@@ -593,21 +500,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
@@ -654,34 +559,55 @@ 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)
-{
+  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 = FSDEV_REG->ep[0].reg & ~USB_EPREG_MASK;
+  ep_reg |= USB_EP_CONTROL;
+  ep_reg = ep_add_status(ep_reg, TUSB_DIR_IN, EP_STAT_NAK);
+  ep_reg = ep_add_status(ep_reg, TUSB_DIR_OUT, EP_STAT_NAK);
+  // no need to explicitly set DTOG bits since we aren't masked DTOG bit
+
+  // prepare for setup packet
+  btable_set_rx_bufsize(0, BTABLE_BUF_RX, CFG_TUD_ENDPOINT0_SIZE);
+
+  ep_write(0, ep_reg);
+}
+
+bool dcd_edpt_open(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 = dcd_ep_alloc(ep_addr, p_endpoint_desc->bmAttributes.xfer);
+  uint8_t const ep_addr = desc_ep->bEndpointAddress;
+  uint8_t const ep_num = tu_edpt_number(ep_addr);
   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;
-
+  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 = FSDEV_REG->ep[ep_idx].reg & ~USB_EPREG_MASK;
+  ep_reg |= tu_edpt_number(ep_addr) | USB_EP_CTR_RX | USB_EP_CTR_TX;
 
   // 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:
@@ -689,24 +615,25 @@ 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_reg = ep_add_status(ep_reg, dir, EP_STAT_NAK);
+  ep_reg = ep_add_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);
 
   return true;
 }
@@ -717,7 +644,7 @@ void dcd_edpt_close_all(uint8_t rhport)
 
   for (uint32_t i = 1; i < FSDEV_EP_COUNT; i++) {
     // Reset endpoint
-    pcd_set_endpoint(USB, i, 0);
+    ep_write(i, 0);
     // Clear EP allocation status
     ep_alloc_status[i].ep_num = 0xFF;
     ep_alloc_status[i].ep_type = 0xFF;
@@ -726,109 +653,71 @@ void dcd_edpt_close_all(uint8_t 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. */
+  uint32_t pma_addr = dcd_pma_alloc(largest_packet_size, true);
 #if FSDEV_PMA_SIZE > 1024u
-  uint32_t pma_addr = dcd_pma_alloc(buffer_size, true);
   uint16_t pma_addr2 = pma_addr >> 16;
 #else
-  uint32_t pma_addr = dcd_pma_alloc(buffer_size, true);
   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 ep_addr = desc_ep->bEndpointAddress;
+  uint8_t const ep_num = tu_edpt_number(ep_addr);
   uint8_t const dir = tu_edpt_dir(ep_addr);
-  const uint16_t packet_size = tu_edpt_packet_size(p_endpoint_desc);
-
-  pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_DIS);
-  pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_DIS);
+  xfer_ctl_t* xfer = xfer_ctl_ptr(ep_num, dir);
 
-  pcd_set_ep_address(USB, ep_idx, tu_edpt_number(ep_addr));
+  uint8_t const ep_idx = xfer->ep_idx;
 
-  pcd_clear_tx_dtog(USB, ep_idx);
-  pcd_clear_rx_dtog(USB, ep_idx);
+  xfer->max_packet_size = tu_edpt_packet_size(desc_ep);
 
-  if (dir == TUSB_DIR_IN) {
-    pcd_rx_dtog(USB, ep_idx);
-  } else {
-    pcd_tx_dtog(USB, ep_idx);
-  }
+  uint32_t ep_reg = FSDEV_REG->ep[0].reg & ~USB_EPREG_MASK;
+  ep_reg |= tu_edpt_number(ep_addr) | USB_EP_ISOCHRONOUS | USB_EP_CTR_RX | USB_EP_CTR_TX;
+  ep_reg = ep_add_status(ep_reg, TUSB_DIR_IN, EP_STAT_DISABLED);
+  ep_reg = ep_add_status(ep_reg, TUSB_DIR_OUT, EP_STAT_DISABLED);
+  ep_reg = ep_add_dtog(ep_reg, dir, 0);
+  ep_reg = ep_add_dtog(ep_reg, 1-dir, 1);
 
-  xfer_ctl_ptr(ep_addr)->max_packet_size = packet_size;
+  ep_write(ep_idx, ep_reg);
 
   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);
+  uint16_t ep_reg = ep_read(ep_ix) | EP_CTR_TXRX;
+  bool const is_iso = ep_is_iso(ep_reg);
 
-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;
-
+  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);
+  btable_set_count(ep_ix, buf_id, len);
 
   if (xfer->ff) {
     dcd_write_packet_memory_ff(xfer->ff, addr_ptr, len);
@@ -837,180 +726,218 @@ static void dcd_transmit_packet(xfer_ctl_t *xfer, uint16_t ep_ix)
   }
   xfer->queued_len = (uint16_t)(xfer->queued_len + len);
 
+  ep_reg &= USB_EPREG_MASK | EP_STAT_MASK(TUSB_DIR_IN);
+  ep_reg = ep_add_status(ep_reg, TUSB_DIR_IN, EP_STAT_VALID);
+  ep_reg = ep_clear_ctr(ep_reg, TUSB_DIR_IN);
+
   dcd_int_disable(0);
-  pcd_set_ep_tx_status(USB, ep_ix, USB_EP_TX_VALID);
+  ep_write(ep_ix, ep_reg);
   if (is_iso) {
     xfer->iso_in_sending = true;
   }
   dcd_int_enable(0);
 }
 
-static bool edpt_xfer(uint8_t rhport, uint8_t ep_addr)
-{
-  (void)rhport;
+static bool edpt_xfer(uint8_t rhport, uint8_t ep_num, uint8_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 cnt = (uint32_t) tu_min16(xfer->total_len, xfer->max_packet_size);
+    uint16_t ep_reg = ep_read(ep_idx) | USB_EP_CTR_TX;
+    ep_reg &= USB_EPREG_MASK | EP_STAT_MASK(dir); // keep CTR TX, clear CTR RX
+    ep_reg = ep_add_status(ep_reg, dir, EP_STAT_VALID);
 
-    uint32_t cnt = (uint32_t ) tu_min16(xfer->total_len, xfer->max_packet_size);
-    uint16_t ep_reg = pcd_get_endpoint(USB, ep_idx);
-
-    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_write(ep_idx, ep_reg);
   }
 
   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);
+  uint8_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);
+  uint8_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_idx = xfer->ep_idx;
+  uint8_t const ep_num = tu_edpt_number(ep_addr);
   uint8_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;
 
-  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);
+  ep_reg |= USB_EP_CTR_RX | USB_EP_CTR_TX; // reserve CTR bits
+  ep_reg &= USB_EPREG_MASK | EP_STAT_MASK(dir);
+  ep_reg = ep_add_status(ep_reg, dir, EP_STAT_STALL);
+
+  ep_write(ep_idx, ep_reg);
 }
 
-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_idx = xfer->ep_idx;
+  uint8_t const ep_num = tu_edpt_number(ep_addr);
   uint8_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;
 
-  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) | EP_CTR_TXRX;
+  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_reg = ep_add_status(ep_reg, dir, EP_STAT_NAK);
   }
+  ep_reg = ep_add_dtog(ep_reg, dir, 0); // Reset to DATA0
+
+  ep_write(ep_idx, ep_reg);
 }
 
 #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);
+static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t wNBytes) {
+  const uint8_t *src8 = src;
+  volatile uint32_t *pma32 = (volatile uint32_t *)(USB_PMAADDR + dst);
 
   for (uint32_t n = wNBytes / 4; n > 0; --n) {
-    *dst32++ = tu_unaligned_read32(srcVal);
-    srcVal += 4;
+    *pma32++ = tu_unaligned_read32(src8);
+    src8 += 4;
   }
 
-  wNBytes = wNBytes & 0x03;
-  if (wNBytes) {
-    uint32_t wrVal = *srcVal;
-    wNBytes--;
+  uint16_t odd = wNBytes & 0x03;
+  if (odd) {
+    uint32_t wrVal = *src8;
+    odd--;
 
-    if (wNBytes) {
-      wrVal |= *++srcVal << 8;
-      wNBytes--;
+    if (odd) {
+      wrVal |= *++src8 << 8;
+      odd--;
 
-      if (wNBytes) {
-        wrVal |= *++srcVal << 16;
+      if (odd) {
+        wrVal |= *++src8 << 16;
       }
     }
 
-    *dst32 = wrVal;
+    *pma32 = wrVal;
+  }
+
+  return true;
+}
+
+static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t wNBytes) {
+  uint8_t *dst8 = dst;
+  volatile uint32_t *src32 = (volatile uint32_t *)(USB_PMAADDR + src);
+
+  for (uint32_t n = wNBytes / 4; n > 0; --n) {
+    tu_unaligned_write32(dst8, *src32++);
+    dst8 += 4;
+  }
+
+  uint16_t odd = wNBytes & 0x03;
+  if (odd) {
+    uint32_t rdVal = *src32;
+
+    *dst8 = tu_u32_byte0(rdVal);
+    odd--;
+
+    if (odd) {
+      *++dst8 = tu_u32_byte1(rdVal);
+      odd--;
+
+      if (odd) {
+        *++dst8 = tu_u32_byte2(rdVal);
+      }
+    }
   }
 
   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.
+ *        This uses un-aligned for user memory 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.
+ * @param   nbytes 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;
+static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, uint16_t nbytes) {
+  uint32_t n16 = (uint32_t)nbytes >> 1U;
+  const uint8_t *src8 = src;
+  fsdev_pma16_t* pma16 = (fsdev_pma16_t*) (USB_PMAADDR + FSDEV_PMA_STRIDE * dst);
 
-  // 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;
+  while (n16--) {
+    pma16->u16 = tu_unaligned_read16(src8);
+    src8 += 2;
+    pma16++;
+  }
 
-  srcVal = src;
-  pdwVal = &pma[FSDEV_PMA_STRIDE * (dst >> 1)];
+  if (nbytes & 0x01) {
+    pma16->u16 = (uint16_t) *src8;
+  }
+
+  return true;
+}
+
+/**
+ * @brief Copy a buffer from packet memory area (PMA) to user memory area.
+ *        Uses unaligned for system memory and 16-bit access of packet memory
+ * @param   nbytes no. of bytes to be copied.
+ * @retval None
+ */
+static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, uint16_t nbytes) {
+  uint32_t n16 = (uint32_t)nbytes >> 1U;
+  fsdev_pma16_t* pma16 = (fsdev_pma16_t*) (USB_PMAADDR + FSDEV_PMA_STRIDE * 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 (n16--) {
+    uint16_t temp16 = pma16->u16;
+    tu_unaligned_write16(dst8, temp16);
+    dst8 += 2;
+    pma16++;
   }
 
-  if (wNBytes & 0x01) {
-    temp1 = (uint16_t) *srcVal;
-    *pdwVal = temp1;
+  if (nbytes & 0x01) {
+    *dst8++ = tu_u16_low(pma16->u16);
   }
 
   return true;
 }
+
 #endif
 
 /**
@@ -1019,8 +946,7 @@ static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, ui
  * @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)
-{
+static bool dcd_write_packet_memory_ff(tu_fifo_t *ff, uint16_t dst, uint16_t wNBytes) {
   // 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);
@@ -1054,8 +980,9 @@ static bool dcd_write_packet_memory_ff(tu_fifo_t *ff, uint16_t dst, uint16_t wNB
     dst += 4;
 
     // Copy rest of wrapped byte
-    if (wCnt)
+    if (wCnt) {
       dcd_write_packet_memory(dst, info.ptr_wrap, wCnt);
+    }
   }
 #else
   if ((cnt_lin & 0x01) && cnt_wrap) {
@@ -1088,78 +1015,13 @@ static bool dcd_write_packet_memory_ff(tu_fifo_t *ff, uint16_t dst, uint16_t wNB
   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);
-
-  for (uint32_t n = wNBytes / 4; n > 0; --n) {
-    tu_unaligned_write32(dstVal, *src32++);
-    dstVal += 4;
-  }
-
-  wNBytes = wNBytes & 0x03;
-  if (wNBytes) {
-    uint32_t rdVal = *src32;
-
-    *dstVal = tu_u32_byte0(rdVal);
-    wNBytes--;
-
-    if (wNBytes) {
-      *++dstVal = tu_u32_byte1(rdVal);
-      wNBytes--;
-
-      if (wNBytes) {
-        *++dstVal = tu_u32_byte2(rdVal);
-      }
-    }
-  }
-
-  return true;
-}
-#else
-/**
- * @brief Copy a buffer from packet memory area (PMA) to user memory area.
- *        Uses byte-access of system memory and 16-bit access of packet memory
- * @param   wNBytes no. of bytes to be copied.
- * @retval None
- */
-static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, 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
-
 /**
  * @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)
-{
+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;
@@ -1193,8 +1055,9 @@ static bool dcd_read_packet_memory_ff(tu_fifo_t *ff, uint16_t src, uint16_t wNBy
     }
 
     // Copy rest of wrapped byte
-    if (wCnt)
+    if (wCnt) {
       dcd_read_packet_memory(info.ptr_wrap, src, wCnt);
+    }
   }
 #else
   if ((cnt_lin & 0x01) && cnt_wrap) {
diff --git a/src/portable/st/stm32_fsdev/fsdev_ch32.h b/src/portable/st/stm32_fsdev/fsdev_ch32.h
index f63a80d56..8c0961bb9 100644
--- a/src/portable/st/stm32_fsdev/fsdev_ch32.h
+++ b/src/portable/st/stm32_fsdev/fsdev_ch32.h
@@ -54,29 +54,7 @@
 #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 FSDEV_REG_BASE  0x40005C00UL
 
 #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 */
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 e8a6af8cb..000000000
--- a/src/portable/st/stm32_fsdev/fsdev_common.h
+++ /dev/null
@@ -1,371 +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;
-  regVal ^= wState;
-  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;
-  regVal ^= wState;
-  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 a8f61a35f..bb2c72fd1 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
@@ -193,6 +194,16 @@
   // This includes U0
 #endif
 
+#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
+
 // This checks if the device has "LPM"
 #if defined(USB_ISTR_L1REQ)
 #define USB_ISTR_L1REQ_FORCED (USB_ISTR_L1REQ)
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..e4a0f3f28
--- /dev/null
+++ b/src/portable/st/stm32_fsdev/fsdev_type.h
@@ -0,0 +1,282 @@
+/*
+ * 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_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
+  #define FSDEV_PMA_STRIDE  (2u) // 1x16 bit access scheme
+  #define pma_aligned TU_ATTR_ALIGNED(4)
+#elif FSDEV_PMA_SIZE == 1024
+  #define FSDEV_PMA_STRIDE  (1u) // 2x16 bit access scheme
+  #define pma_aligned
+#elif FSDEV_PMA_SIZE == 2048
+  #ifndef FSDEV_BUS_32BIT
+    #warning "FSDEV_PMA_SIZE is 2048, but FSDEV_BUS_32BIT is not defined"
+  #endif
+  #define FSDEV_PMA_STRIDE  (1u) // 32 bit access scheme
+  #define pma_aligned
+#endif
+
+//--------------------------------------------------------------------+
+// 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.
+typedef union {
+  // 0: TX (IN), 1: RX (OUT)
+
+  // strictly 16-bit access (could be 32-bit aligned)
+  struct {
+    volatile pma_aligned uint16_t addr;
+    volatile pma_aligned 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*) (USB_PMAADDR+FSDEV_BTABLE_BASE))
+
+typedef struct {
+  volatile pma_aligned uint16_t u16;
+} fsdev_pma16_t;
+
+//--------------------------------------------------------------------+
+// Registers Typedef
+//--------------------------------------------------------------------+
+
+// volatile 32-bit aligned
+#define _va32     volatile TU_ATTR_ALIGNED(4)
+
+// 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;
+#else
+typedef uint16_t fsdev_bus_t;
+#endif
+
+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
+
+
+#define EP_CTR_TXRX (USB_EP_CTR_TX | USB_EP_CTR_RX)
+
+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 void ep_write(uint32_t ep_id, uint32_t value) {
+  FSDEV_REG->ep[ep_id].reg = (fsdev_bus_t) value;
+}
+
+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 uint32_t ep_add_status(uint32_t reg, tusb_dir_t dir, ep_stat_t state) {
+  return reg ^ (state << (USB_EPTX_STAT_Pos + (dir == TUSB_DIR_IN ? 0 : 8)));
+}
+
+TU_ATTR_ALWAYS_INLINE static inline uint32_t ep_add_dtog(uint32_t reg, tusb_dir_t dir, uint8_t state) {
+  return reg ^ (state << (USB_EP_DTOG_TX_Pos + (dir == TUSB_DIR_IN ? 0 : 8)));
+}
+
+TU_ATTR_ALWAYS_INLINE static inline uint32_t ep_clear_ctr(uint32_t reg, tusb_dir_t dir) {
+  return reg & ~(1 << (USB_EP_CTR_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 uint32_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;
+  } else {
+    block_in_bytes = 2;
+    *blsize = 0;
+  }
+
+  *num_block = tu_div_ceil(size, block_in_bytes);
+
+  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, uint32_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);
+
+#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