Merge branch 'master' into renesas-ra

41 files changed, 3734 insertions, 1396 deletions

diff --git a/src/portable/bridgetek/ft9xx/dcd_ft9xx.c b/src/portable/bridgetek/ft9xx/dcd_ft9xx.c
index 3c3cb63b8..efca5bdcb 100644
--- a/src/portable/bridgetek/ft9xx/dcd_ft9xx.c
+++ b/src/portable/bridgetek/ft9xx/dcd_ft9xx.c
@@ -38,9 +38,6 @@
 #include <ft900.h>
 #include <registers/ft900_registers.h>
 
-#include "board.h"
-#include "bsp/board.h"
-
 #define USBD_USE_STREAMS
 
 #include "device/dcd.h"
@@ -50,17 +47,21 @@
 //--------------------------------------------------------------------+
 
 // Board code will determine the state of VBUS from USB host.
-extern int8_t board_ft90x_vbus(void);
+extern int8_t board_ft9xx_vbus(void);
+extern int board_uart_write(void const *buf, int len);
 
 // Static array to store an incoming SETUP request for processing by tinyusb.
-static uint8_t _ft90x_setup_packet[8];
+CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN
+static uint8_t _ft9xx_setup_packet[8];
 
-struct ft90x_xfer_state
+struct ft9xx_xfer_state
 {
+  volatile uint8_t ready; // OUT Transfer has been received and waiting for transfer.
   volatile uint8_t valid; // Transfer is pending and total_size, remain_size, and buff_ptr are valid.
-  volatile int16_t total_size; // Total transfer size in bytes for this transfer.
-  volatile int16_t remain_size; // Total remaining in transfer.
-  volatile uint8_t *buff_ptr; // Pointer to buffer to transmit from or receive to.
+  
+  int16_t total_size; // Total transfer size in bytes for this transfer.
+  int16_t remain_size; // Total remaining in transfer.
+  uint8_t *buff_ptr; // Pointer to buffer to transmit from or receive to.
 
   uint8_t type; // Endpoint type. Of type USBD_ENDPOINT_TYPE from endpoint descriptor.
   uint8_t dir; // Endpoint direction. TUSB_DIR_OUT or TUSB_DIR_IN. For control endpoint this is the current direction.
@@ -68,24 +69,24 @@ struct ft90x_xfer_state
   uint16_t size; // Max packet size for endpoint from endpoint descriptor.
 };
 // Endpoint description array for each endpoint.
-static struct ft90x_xfer_state ep_xfer[USBD_MAX_ENDPOINT_COUNT];
+static struct ft9xx_xfer_state ep_xfer[USBD_MAX_ENDPOINT_COUNT];
 // USB speed.
 static tusb_speed_t _speed;
 
 // Interrupt handlers.
-void _ft90x_usbd_ISR(void); // Interrupt handler for USB device.
-void ft90x_usbd_pm_ISR(void); // Interrupt handler for USB device for power management (called by board).
+void _ft9xx_usbd_ISR(void); // Interrupt handler for USB device.
+void ft9xx_usbd_pm_ISR(void); // Interrupt handler for USB device for power management (called by board).
 
 // Internal functions forward declarations.
-static uint16_t _ft90x_edpt_xfer_out(uint8_t ep_number, uint8_t *buffer, uint16_t xfer_bytes);
-static uint16_t _ft90x_edpt_xfer_in(uint8_t ep_number, uint8_t *buffer, uint16_t xfer_bytes);
-static void _ft90x_reset_edpts(void);
-static inline void _ft90x_phy_enable(bool en);
-static void _ft90x_usb_speed(void);
-static void _dcd_ft90x_attach(void);
-static void _dcd_ft90x_detach(void) __attribute__((unused));
-static uint16_t _ft90x_dusb_in(uint8_t ep_number, const uint8_t *buffer, uint16_t length);
-static uint16_t _ft90x_dusb_out(uint8_t ep_number, uint8_t *buffer, uint16_t length);
+static uint16_t _ft9xx_edpt_xfer_out(uint8_t ep_number, uint8_t *buffer, uint16_t xfer_bytes);
+static uint16_t _ft9xx_edpt_xfer_in(uint8_t ep_number, uint8_t *buffer, uint16_t xfer_bytes);
+static void _ft9xx_reset_edpts(void);
+static inline void _ft9xx_phy_enable(bool en);
+static void _ft9xx_usb_speed(void);
+static void _dcd_ft9xx_attach(void);
+static void _dcd_ft9xx_detach(void) __attribute__((unused));
+static uint16_t _ft9xx_dusb_in(uint8_t ep_number, const uint8_t *buffer, uint16_t length);
+static uint16_t _ft9xx_dusb_out(uint8_t ep_number, uint8_t *buffer, uint16_t length);
 
 // Internal functions.
 
@@ -93,7 +94,7 @@ static uint16_t _ft90x_dusb_out(uint8_t ep_number, uint8_t *buffer, uint16_t len
 // This can be up-to the maximum packet size of the endpoint.
 // Continuation of a transfer beyond the maximum packet size is performed 
 // by the interrupt handler.
-static uint16_t _ft90x_edpt_xfer_out(uint8_t ep_number, uint8_t *buffer, uint16_t xfer_bytes)
+static uint16_t _ft9xx_edpt_xfer_out(uint8_t ep_number, uint8_t *buffer, uint16_t xfer_bytes)
 {
   //Note: this is called from only the interrupt handler when an OUT transfer is called.
   uint16_t ep_size = ep_xfer[ep_number].size;
@@ -108,7 +109,7 @@ static uint16_t _ft90x_edpt_xfer_out(uint8_t ep_number, uint8_t *buffer, uint16_
     //;
 
   // Send the first packet of max packet size
-  xfer_bytes = _ft90x_dusb_out(ep_number, (uint8_t *)buffer, xfer_bytes);
+  xfer_bytes = _ft9xx_dusb_out(ep_number, (uint8_t *)buffer, xfer_bytes);
   if (ep_number == USBD_EP_0)
   {
     // Set flags to indicate data ready.
@@ -126,7 +127,7 @@ static uint16_t _ft90x_edpt_xfer_out(uint8_t ep_number, uint8_t *buffer, uint16_
 // This can be up-to the maximum packet size of the endpoint.
 // Continuation of a transfer beyond the maximum packet size is performed 
 // by the interrupt handler.
-static uint16_t _ft90x_edpt_xfer_in(uint8_t ep_number, uint8_t *buffer, uint16_t xfer_bytes)
+static uint16_t _ft9xx_edpt_xfer_in(uint8_t ep_number, uint8_t *buffer, uint16_t xfer_bytes)
 {
   //Note: this may be called from the interrupt handler or from normal code.
   uint8_t end = 0;
@@ -154,17 +155,24 @@ static uint16_t _ft90x_edpt_xfer_in(uint8_t ep_number, uint8_t *buffer, uint16_t
   }
   else
   {
-    uint8_t sr_reg;
     // If there is data to transmit then wait until the IN buffer
     // for the endpoint is empty.
-    do
+    // This does not apply to interrupt endpoints.
+    if (ep_xfer[ep_number].type != TUSB_XFER_INTERRUPT)
     {
-      sr_reg = USBD_EP_SR_REG(ep_number);
-    } while (sr_reg & MASK_USBD_EPxSR_INPRDY);
-    
+      uint8_t sr_reg;
+      do
+      {
+        sr_reg = USBD_EP_SR_REG(ep_number);
+      } while (sr_reg & MASK_USBD_EPxSR_INPRDY);
+    }
   }
 
-  xfer_bytes = _ft90x_dusb_in(ep_number, (uint8_t *)buffer, xfer_bytes);
+  // Do not send a ZLP for interrupt endpoints.
+  if ((ep_xfer[ep_number].type != TUSB_XFER_INTERRUPT) || (xfer_bytes > 0))
+  {
+    xfer_bytes = _ft9xx_dusb_in(ep_number, (uint8_t *)buffer, xfer_bytes);
+  }
 
   if (ep_number == USBD_EP_0)
   {
@@ -190,13 +198,13 @@ static uint16_t _ft90x_edpt_xfer_in(uint8_t ep_number, uint8_t *buffer, uint16_t
 
 // Reset all non-control endpoints to a default state. 
 // Control endpoint is always enabled and ready. All others disabled.
-static void _ft90x_reset_edpts(void)
+static void _ft9xx_reset_edpts(void)
 {
   // Disable all endpoints and remove configuration values.
   for (int i = 1; i < USBD_MAX_ENDPOINT_COUNT; i++)
   {
     // Clear settings.
-    tu_memclr(&ep_xfer[i], sizeof(struct ft90x_xfer_state));
+    tu_memclr(&ep_xfer[i], sizeof(struct ft9xx_xfer_state));
     // Disable hardware.
     USBD_EP_CR_REG(i) = 0;
   }
@@ -206,7 +214,7 @@ static void _ft90x_reset_edpts(void)
 }
 
 // Enable or disable the USB PHY.
-static inline void _ft90x_phy_enable(bool en)
+static inline void _ft9xx_phy_enable(bool en)
 {
   if (en)
     SYS->PMCFG_L |= MASK_SYS_PMCFG_DEV_PHY_EN;
@@ -215,7 +223,7 @@ static inline void _ft90x_phy_enable(bool en)
 }
 
 // Safely connect to the USB.
-static void _dcd_ft90x_attach(void)
+static void _dcd_ft9xx_attach(void)
 {
   uint8_t reg;
 
@@ -239,12 +247,12 @@ static void _dcd_ft90x_attach(void)
 
   CRITICAL_SECTION_BEGIN
   // Turn off the device enable bit.
-#if BOARD_DEVICE_RHPORT_SPEED == OPT_MODE_HIGH_SPEED
+#if BOARD_TUD_MAX_SPEED == OPT_MODE_HIGH_SPEED
   USBD_REG(fctrl) = 0;
-#else // BOARD_DEVICE_RHPORT_SPEED == OPT_MODE_FULL_SPEED
+#else // BOARD_TUD_MAX_SPEED == OPT_MODE_FULL_SPEED
   //Set the full speed only bit if required.
   USBD_REG(fctrl) = MASK_USBD_FCTRL_MODE_FS_ONLY;
-#endif // BOARD_DEVICE_RHPORT_SPEED
+#endif // BOARD_TUD_MAX_SPEED
 
   // Clear first reset and suspend interrupts.
   do
@@ -271,7 +279,7 @@ static void _dcd_ft90x_attach(void)
 }
 
 // Gracefully disconnect from the USB.
-static void _dcd_ft90x_detach(void)
+static void _dcd_ft9xx_detach(void)
 {
   // Disable device connect/disconnect/host reset detection.
   SYS->PMCFG_L = SYS->PMCFG_L & (~MASK_SYS_PMCFG_DEV_DETECT_EN);
@@ -291,7 +299,7 @@ static void _dcd_ft90x_detach(void)
   delayms(1);
 
   // Disable USB PHY
-  dcd_disconnect(BOARD_DEVICE_RHPORT_NUM);
+  dcd_disconnect(BOARD_TUD_RHPORT);
   delayms(1);
 
   // Disable Chip USB device clock/PM configuration.
@@ -312,8 +320,8 @@ static void _dcd_ft90x_detach(void)
 
 // Determine the speed of the USB to which we are connected.
 // Set the speed of the PHY accordingly. 
-// High speed can be disabled through CFG_TUSB_RHPORT0_MODE settings.
-static void _ft90x_usb_speed(void)
+// High speed can be disabled through CFG_TUSB_RHPORT0_MODE or CFG_TUD_MAX_SPEED settings.
+static void _ft9xx_usb_speed(void)
 {
 	uint8_t  fctrl_val;
 
@@ -323,7 +331,7 @@ static void _ft90x_usb_speed(void)
 		delayus(200);
 	}
 
-#if BOARD_DEVICE_RHPORT_SPEED == OPT_MODE_HIGH_SPEED
+#if BOARD_TUD_MAX_SPEED == OPT_MODE_HIGH_SPEED
 
 	/* Detect high or full speed */
 	fctrl_val = MASK_USBD_FCTRL_USB_DEV_EN;
@@ -347,11 +355,11 @@ static void _ft90x_usb_speed(void)
 	delayus(125 + 5);
 	_speed = (USBD_REG(cmif) & MASK_USBD_CMIF_SOFIRQ) ?
 		TUSB_SPEED_HIGH : TUSB_SPEED_FULL;
-  dcd_event_bus_reset(BOARD_DEVICE_RHPORT_NUM, _speed, true);
+  dcd_event_bus_reset(BOARD_TUD_RHPORT, _speed, true);
 
 #endif /* !__FT930__ */
 
-#else // BOARD_DEVICE_RHPORT_SPEED == OPT_MODE_FULL_SPEED
+#else // BOARD_TUD_MAX_SPEED == OPT_MODE_FULL_SPEED
 
 	/* User force set to full speed */
   _speed = TUSB_SPEED_FULL;
@@ -364,10 +372,10 @@ static void _ft90x_usb_speed(void)
 	}
 #endif
 	USBD_REG(fctrl) = fctrl_val;
-  dcd_event_bus_reset(BOARD_DEVICE_RHPORT_NUM, _speed, true);
+  dcd_event_bus_reset(BOARD_TUD_RHPORT, _speed, true);
 	return;
 
-#endif // BOARD_DEVICE_RHPORT_SPEED
+#endif // BOARD_TUD_MAX_SPEED
 }
 
 // Send a buffer to the USB IN FIFO.
@@ -376,7 +384,7 @@ static void _ft90x_usb_speed(void)
 // If streaming is disabled then it will send each byte of the buffer in turn 
 // to the FIFO. The is no reason to not stream.
 // The total number of bytes sent to the FIFO is returned.
-static uint16_t _ft90x_dusb_in(uint8_t ep_number, const uint8_t *buffer, uint16_t length)
+static uint16_t _ft9xx_dusb_in(uint8_t ep_number, const uint8_t *buffer, uint16_t length)
 {
   uint16_t bytes_read = 0;
   uint16_t buff_size = length;
@@ -433,7 +441,7 @@ static uint16_t _ft90x_dusb_in(uint8_t ep_number, const uint8_t *buffer, uint16_
 // If streaming is disabled then it will receive each byte from the FIFO in turn 
 // to the buffer. The is no reason to not stream.
 // The total number of bytes received from the FIFO is returned.
-static uint16_t _ft90x_dusb_out(uint8_t ep_number, uint8_t *buffer, uint16_t length)
+static uint16_t _ft9xx_dusb_out(uint8_t ep_number, uint8_t *buffer, uint16_t length)
 {
 #ifdef USBD_USE_STREAMS
   volatile uint8_t *data_reg;
@@ -511,11 +519,11 @@ static uint16_t _ft90x_dusb_out(uint8_t ep_number, uint8_t *buffer, uint16_t len
 // Initialize controller to device mode
 void dcd_init(uint8_t rhport)
 {
-  TU_LOG2("FT90x initialisation\r\n");
+  TU_LOG2("FT9xx initialisation\r\n");
 
-  _dcd_ft90x_attach();
+  _dcd_ft9xx_attach();
 
-  interrupt_attach(interrupt_usb_device, (int8_t)interrupt_usb_device, _ft90x_usbd_ISR);
+  interrupt_attach(interrupt_usb_device, (int8_t)interrupt_usb_device, _ft9xx_usbd_ISR);
 
   dcd_connect(rhport);
 }
@@ -524,7 +532,7 @@ void dcd_init(uint8_t rhport)
 void dcd_int_enable(uint8_t rhport)
 {
   (void)rhport;
-  TU_LOG3("FT90x int enable\r\n");
+  TU_LOG3("FT9xx int enable\r\n");
 
   // Peripheral devices interrupt enable.
   interrupt_enable_globally();
@@ -534,7 +542,7 @@ void dcd_int_enable(uint8_t rhport)
 void dcd_int_disable(uint8_t rhport)
 {
   (void)rhport;
-  TU_LOG3("FT90x int disable\r\n");
+  TU_LOG3("FT9xx int disable\r\n");
 
   // Peripheral devices interrupt disable.
   interrupt_disable_globally();
@@ -587,7 +595,7 @@ void dcd_remote_wakeup(uint8_t rhport)
 
   SYS->MSC0CFG = SYS->MSC0CFG | MASK_SYS_MSC0CFG_DEV_RMWAKEUP;
 
-  // Atleast 2 ms of delay needed for RESUME Data K state.
+  // At least 2 ms of delay needed for RESUME Data K state.
   delayms(2); 
 
   SYS->MSC0CFG &= ~MASK_SYS_MSC0CFG_DEV_RMWAKEUP;
@@ -600,18 +608,18 @@ void dcd_remote_wakeup(uint8_t rhport)
 void dcd_connect(uint8_t rhport)
 {
   (void)rhport;
-  TU_LOG2("FT90x connect\r\n");
+  TU_LOG2("FT9xx connect\r\n");
 
   CRITICAL_SECTION_BEGIN
   // Is device connected?
-  if (board_ft90x_vbus())
+  if (board_ft9xx_vbus())
   {
     // Clear/disable address register.
     USBD_REG(faddr) = 0;
-    _ft90x_phy_enable(true);
+    _ft9xx_phy_enable(true);
 
     // Determine bus speed and signal speed to tusb.
-    _ft90x_usb_speed();
+    _ft9xx_usb_speed();
   }
   
   // Setup the control endpoint only.
@@ -636,17 +644,17 @@ void dcd_connect(uint8_t rhport)
   USBD_REG(epie) = (MASK_USBD_EPIE_EP0IE);
 
   // Restore default endpoint state.
-  _ft90x_reset_edpts();
+  _ft9xx_reset_edpts();
 }
 
 // Disconnect by disabling internal pull-up resistor on D+/D-
 void dcd_disconnect(uint8_t rhport)
 {
   (void)rhport;
-  TU_LOG2("FT90x disconnect\r\n");
+  TU_LOG2("FT9xx disconnect\r\n");
 
   // Disable the USB PHY.
-  _ft90x_phy_enable(false);
+  _ft9xx_phy_enable(false);
 }
 
 void dcd_sof_enable(uint8_t rhport, bool en)
@@ -674,12 +682,12 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *ep_desc)
   uint8_t ep_reg_data = 0;
   int16_t total_ram;
 
-  TU_LOG2("FT90x endpoint open %d %c\r\n", ep_number, ep_dir?'I':'O');
+  TU_LOG2("FT9xx endpoint open %d %c\r\n", ep_number, ep_dir?'I':'O');
 
   // Check that the requested endpoint number is allowable.
   if (ep_number >= USBD_MAX_ENDPOINT_COUNT)
   {
-    TU_LOG1("FT90x endpoint not valid: requested %d max %d\r\n", ep_number, USBD_MAX_ENDPOINT_COUNT);
+    TU_LOG1("FT9xx endpoint not valid: requested %d max %d\r\n", ep_number, USBD_MAX_ENDPOINT_COUNT);
     return false;
   }
 
@@ -691,7 +699,7 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *ep_desc)
   }
   if (ep_reg_size > USBD_EP_MAX_SIZE_1024)
   {
-    TU_LOG1("FT90x endpoint size not valid: requested %d max 1024\r\n", ep_size);
+    TU_LOG1("FT9xx endpoint size not valid: requested %d max 1024\r\n", ep_size);
     return false;
   }
   // Calculate actual amount of buffer RAM used by this endpoint. This may be more than the 
@@ -706,9 +714,9 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *ep_desc)
     if (ep_xfer[ep_number].type != USBD_EP_TYPE_DISABLED)
     {
       // This could be because an endpoint has been assigned with the same number.
-      // On FT90x, IN and OUT endpoints may not have the same number. e.g. There 
+      // On FT9xx, IN and OUT endpoints may not have the same number. e.g. There 
       // cannot been an 0x81 and 0x01 endpoint.
-      TU_LOG1("FT90x endpoint %d already assigned\r\n", ep_number);
+      TU_LOG1("FT9xx endpoint %d already assigned\r\n", ep_number);
       return false;
     }
 
@@ -720,7 +728,7 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *ep_desc)
     else
       total_ram = USBD_RAMTOTAL_OUT;
     // Work out how much has been allocated to existing endpoints.
-    // The total RAM allocated shoudl alsyes be a positive number as this
+    // The total RAM allocated should always be a positive number as this
     // algorithm should not let it go below zero.
     for (int i = 1; i < USBD_MAX_ENDPOINT_COUNT; i++)
     {
@@ -732,23 +740,28 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *ep_desc)
         }
       }
     }
-    // The control endpoint is taken into account as well.
-    total_ram -= ep_xfer[0].buff_size;
+
+    if (sys_check_ft900_revB())
+    {
+      // The control endpoint is taken into account as well on RevB silicon.
+      total_ram -= ep_xfer[0].buff_size;
+    }
+
     // Make sure we have enough space. The corner case is having zero bytes
     // free which means that total_ram must be signed as zero bytes free is
     // allowable.
     if (total_ram < ep_buff_size)
     {
-      TU_LOG1("FT90x insufficient buffer RAM for endpoint %d\r\n", ep_number);
+      TU_LOG1("FT9xx insufficient buffer RAM for endpoint %d\r\n", ep_number);
       return false;    
     }
 
     // Set the type of this endpoint in the control register.
-    if (ep_type == USBD_EP_BULK)
+    if (ep_type == TUSB_XFER_BULK)
       ep_reg_data |= (USBD_EP_DIS_BULK << BIT_USBD_EP_CONTROL_DIS);
-    else if (ep_type == USBD_EP_INT)
+    else if (ep_type == TUSB_XFER_INTERRUPT)
       ep_reg_data |= (USBD_EP_DIS_INT << BIT_USBD_EP_CONTROL_DIS);
-    else if (ep_type == USBD_EP_ISOC)
+    else if (ep_type == TUSB_XFER_ISOCHRONOUS)
       ep_reg_data |= (USBD_EP_DIS_ISO << BIT_USBD_EP_CONTROL_DIS);
     // Set the direction of this endpoint in the control register.
     if (ep_dir == USBD_DIR_IN)
@@ -756,9 +769,9 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *ep_desc)
     // Do not perform double buffering.
     //if (<double buffering flag> != USBD_DB_OFF)
     //ep_reg_data |= MASK_USBD_EPxCR_DB;
-    // Set the control endpoint for this endpoint.
+    // Set the control register for this endpoint.
     USBD_EP_CR_REG(ep_number) = ep_reg_data;
-    TU_LOG2("FT90x endpoint setting %x\r\n", ep_reg_data);
+    TU_LOG2("FT9xx endpoint setting %x\r\n", ep_reg_data);
   }
   else
   {
@@ -766,14 +779,15 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *ep_desc)
     USBD_EP_CR_REG(USBD_EP_0) = (ep_reg_size << BIT_USBD_EP0_MAX_SIZE);
   }
 
+  CRITICAL_SECTION_BEGIN
   // Store the endpoint characteristics for later reference.
   ep_xfer[ep_number].dir = ep_dir;
   ep_xfer[ep_number].type = ep_type;
   ep_xfer[ep_number].size = ep_size;
   ep_xfer[ep_number].buff_size = ep_buff_size;
 
-  CRITICAL_SECTION_BEGIN
   // Clear register transaction continuation and signalling state.
+  ep_xfer[ep_number].ready = 0;
   ep_xfer[ep_number].valid = 0;
   ep_xfer[ep_number].buff_ptr = NULL;
   ep_xfer[ep_number].total_size = 0;
@@ -788,7 +802,7 @@ void dcd_edpt_close_all(uint8_t rhport)
 {
   (void)rhport;
   // Reset the endpoint configurations.
-  _ft90x_reset_edpts();
+  _ft9xx_reset_edpts();
 }
 
 // Submit a transfer, When complete dcd_event_xfer_complete() is invoked to notify the stack
@@ -796,7 +810,7 @@ bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t *buffer, uint16_t to
 {
   (void)rhport;
   uint8_t ep_number = tu_edpt_number(ep_addr);
-  uint8_t dir = tu_edpt_dir(ep_addr);
+  uint8_t ep_dir = tu_edpt_dir(ep_addr);
   uint16_t xfer_bytes;
   bool status = false;
 
@@ -806,19 +820,17 @@ bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t *buffer, uint16_t to
   // ep_xfer is used to tell the interrupt handler what to do.
   // ep_xfer can be used at interrupt level to continue transfers.
   CRITICAL_SECTION_BEGIN
+
   // Transfer currently in progress.
   if (ep_xfer[ep_number].valid == 0)
   {
-    status = true;
-
     ep_xfer[ep_number].total_size = total_bytes;
     ep_xfer[ep_number].remain_size = total_bytes;
     ep_xfer[ep_number].buff_ptr = buffer;
-    ep_xfer[ep_number].valid = 1;
     
     if (ep_number == USBD_EP_0)
     {
-      ep_xfer[USBD_EP_0].dir = dir;
+      ep_xfer[USBD_EP_0].dir = ep_dir;
     }
     else
     {
@@ -827,18 +839,53 @@ bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t *buffer, uint16_t to
       USBD_REG(epie) = USBD_REG(epie) | (1 << ep_number);
     }
 
-    if (dir == TUSB_DIR_IN)
+    if (ep_dir == TUSB_DIR_IN)
     {
       // For IN transfers send the first packet as a starter. Interrupt handler to complete
       // this if it is larger than one packet.
-      xfer_bytes = _ft90x_edpt_xfer_in(ep_number, buffer, total_bytes);
+      xfer_bytes = _ft9xx_edpt_xfer_in(ep_number, buffer, total_bytes);
 
       ep_xfer[ep_number].buff_ptr += xfer_bytes;
       ep_xfer[ep_number].remain_size -= xfer_bytes;
+
+      // Tell the interrupt handler to signal dcd_event_xfer_complete on completion.
+      ep_xfer[ep_number].valid = 1;
     }
+    else // (dir == TUSB_DIR_OUT)
+    {
+      // For OUT transfers on the control endpoint.
+      // The host may already have performed the first data transfer after the SETUP packet
+      // before the transfer is setup for it.
+      if (ep_xfer[ep_number].ready)
+      {
+        // We have received a data packet on the endpoint without a transfer
+        // being initialised. This can be because the host has sent this packet before
+        // a new transfer has been initiated on the endpoint.
+        // We will now stream the data from the FIFO.
+        ep_xfer[ep_number].ready = 0;
+
+        // Transfer incoming data from an OUT packet to the buffer.
+        xfer_bytes = _ft9xx_edpt_xfer_out(ep_number, buffer, total_bytes);
+
+        // Report completion of the transfer.
+        dcd_event_xfer_complete(BOARD_TUD_RHPORT, ep_number /*| TUSB_DIR_OUT_MASK */, xfer_bytes, XFER_RESULT_SUCCESS, false);
+      }
+      else
+      {
+        // Tell the interrupt handler to wait for the packet to be received and
+        // then report the transfer complete with dcd_event_xfer_complete.
+        ep_xfer[ep_number].valid = 1;
+      }
+    } 
+    status = true;
+  }
+  else
+  {
+    // Note: should not arrive here.
   }
+
   CRITICAL_SECTION_END
-  
+
   return status;
 }
 
@@ -889,6 +936,7 @@ void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
     USBD_EP_SR_REG(ep_number) = MASK_USBD_EPxSR_CLR_TOGGLE;
 
     // Allow transfers to restart.
+    ep_xfer[ep_number].ready = 0;
     ep_xfer[ep_number].valid = 0;
     ep_xfer[ep_number].remain_size = 0;
     CRITICAL_SECTION_END
@@ -897,9 +945,9 @@ void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
 
 // Interrupt handling.
 
-void _ft90x_usbd_ISR(void)
+void _ft9xx_usbd_ISR(void)
 {
-  tud_int_handler(BOARD_DEVICE_RHPORT_NUM); // Resolves to dcd_int_handler().
+  dcd_int_handler(BOARD_TUD_RHPORT);
 }
 
 void dcd_int_handler(uint8_t rhport)
@@ -936,20 +984,20 @@ void dcd_int_handler(uint8_t rhport)
     if (cmif & MASK_USBD_CMIF_RSTIRQ) //Handle Reset interrupt
     {
       // Reset endpoints to default state.
-      _ft90x_reset_edpts();
-      dcd_event_bus_reset(BOARD_DEVICE_RHPORT_NUM, _speed, true);
+      _ft9xx_reset_edpts();
+      dcd_event_bus_reset(BOARD_TUD_RHPORT, _speed, true);
     }
     if (cmif & MASK_USBD_CMIF_SUSIRQ) //Handle Suspend interrupt
     {
-      dcd_event_bus_signal(BOARD_DEVICE_RHPORT_NUM, DCD_EVENT_SUSPEND, true);
+      dcd_event_bus_signal(BOARD_TUD_RHPORT, DCD_EVENT_SUSPEND, true);
     }
     if (cmif & MASK_USBD_CMIF_RESIRQ) //Handle Resume interrupt
     {
-      dcd_event_bus_signal(BOARD_DEVICE_RHPORT_NUM, DCD_EVENT_RESUME, true);
+      dcd_event_bus_signal(BOARD_TUD_RHPORT, DCD_EVENT_RESUME, true);
     }
     if (cmif & MASK_USBD_CMIF_SOFIRQ) //Handle SOF interrupt
     {
-      dcd_event_bus_signal(BOARD_DEVICE_RHPORT_NUM, DCD_EVENT_SOF, true);
+      dcd_event_bus_signal(BOARD_TUD_RHPORT, DCD_EVENT_SOF, true);
     }
   }
   // Handle endpoint interrupts.
@@ -962,7 +1010,6 @@ void dcd_int_handler(uint8_t rhport)
     {
       // Clear interrupt register.
       USBD_REG(epif) = MASK_USBD_EPIF_EP0IRQ;
-
       // Test for an incoming SETUP request on the control endpoint.
       if (USBD_EP_SR_REG(USBD_EP_0) & MASK_USBD_EP0SR_SETUP)
       {
@@ -976,16 +1023,19 @@ void dcd_int_handler(uint8_t rhport)
           USBD_EP_SR_REG(USBD_EP_0) = MASK_USBD_EP0SR_STALL;
         }
 
-        // Host has sent a SETUP packet. Recieve this into the setup packet store.
-        _ft90x_dusb_out(USBD_EP_0, (uint8_t *)_ft90x_setup_packet, sizeof(USB_device_request));
+        // Host has sent a SETUP packet. Receive this into the SETUP packet store.
+        _ft9xx_dusb_out(USBD_EP_0, (uint8_t *)_ft9xx_setup_packet, sizeof(USB_device_request));
         
         // Send the packet to tinyusb.
-        dcd_event_setup_received(BOARD_DEVICE_RHPORT_NUM, _ft90x_setup_packet, true);
+        dcd_event_setup_received(BOARD_TUD_RHPORT, _ft9xx_setup_packet, true);
 
         // Clear the interrupt that signals a SETUP packet is received.
         USBD_EP_SR_REG(USBD_EP_0) = (MASK_USBD_EP0SR_SETUP);
 
-        // Allow new transfers on the control endpoint.
+        // Any SETUP packet will clear the incoming FIFO.
+        ep_xfer[USBD_EP_0].ready = 0;
+
+        // Allow new DATA and ACK transfers on the control endpoint.
         ep_xfer[USBD_EP_0].valid = 0;
         return;
       }
@@ -998,15 +1048,29 @@ void dcd_int_handler(uint8_t rhport)
 
           // Transfer incoming data from an OUT packet to the buffer supplied.        
           if (ep_xfer[USBD_EP_0].dir == TUSB_DIR_OUT)
-          { 
-            xfer_bytes = _ft90x_edpt_xfer_out(USBD_EP_0, (uint8_t *)ep_xfer[USBD_EP_0].buff_ptr, xfer_bytes);
+          {
+            xfer_bytes = _ft9xx_edpt_xfer_out(USBD_EP_0, ep_xfer[USBD_EP_0].buff_ptr, xfer_bytes);
           }
           // Now signal completion of data packet.
-          dcd_event_xfer_complete(BOARD_DEVICE_RHPORT_NUM, (ep_xfer[USBD_EP_0].dir ? TUSB_DIR_IN_MASK : 0), xfer_bytes, XFER_RESULT_SUCCESS, true);
+          dcd_event_xfer_complete(BOARD_TUD_RHPORT, USBD_EP_0 | (ep_xfer[USBD_EP_0].dir ? TUSB_DIR_IN_MASK : 0), 
+            xfer_bytes, XFER_RESULT_SUCCESS, true);
+
+          // Incoming FIFO has been cleared.
+          ep_xfer[USBD_EP_0].ready = 0;
 
           // Allow new transfers on the control endpoint.
           ep_xfer[USBD_EP_0].valid = 0;
         }
+        // No transfer is in flight for EP0.
+        else
+        {
+          // We have received a data packet on the control endpoint without a transfer
+          // being initialised. This can be because the host has sent this packet before
+          // a new transfer has been initiated on the control endpoint.
+          // We will record that there is data in the FIFO for dcd_edpt_xfer to obtain
+          // once the transfer is initiated.
+          ep_xfer[USBD_EP_0].ready = 1;
+        }
       }
     }
     else // !(epif & MASK_USBD_EPIF_EP0IRQ)
@@ -1026,7 +1090,6 @@ void dcd_int_handler(uint8_t rhport)
         if (ep_xfer[ep_number].valid)
         {
           xfer_bytes = 0;
-          uint8_t ep_dirmask = (ep_xfer[ep_number].dir ? TUSB_DIR_IN_MASK : 0);
 
           // Clear interrupt register for this endpoint.
           USBD_REG(epif) = MASK_USBD_EPIF_IRQ(ep_number);
@@ -1034,10 +1097,15 @@ void dcd_int_handler(uint8_t rhport)
           // Start or continue an OUT transfer.
           if (ep_xfer[ep_number].dir == TUSB_DIR_OUT)
           {
-            xfer_bytes = _ft90x_edpt_xfer_out(ep_number, 
-                            (uint8_t *)ep_xfer[ep_number].buff_ptr, 
+            xfer_bytes = _ft9xx_edpt_xfer_out(ep_number, 
+                            ep_xfer[ep_number].buff_ptr, 
                             (uint16_t)ep_xfer[ep_number].remain_size);
 
+            // Report each OUT packet received to the stack.
+            dcd_event_xfer_complete(BOARD_TUD_RHPORT, 
+                                      ep_number /* | TUSB_DIR_OUT_MASK */, 
+                                      xfer_bytes, XFER_RESULT_SUCCESS, true);
+
             ep_xfer[ep_number].buff_ptr += xfer_bytes;
             ep_xfer[ep_number].remain_size -= xfer_bytes;
           }
@@ -1046,27 +1114,45 @@ void dcd_int_handler(uint8_t rhport)
           {
             if (ep_xfer[ep_number].remain_size > 0)
             {
-              xfer_bytes = _ft90x_edpt_xfer_in(ep_number, 
-                            (uint8_t *)ep_xfer[ep_number].buff_ptr, 
+              xfer_bytes = _ft9xx_edpt_xfer_in(ep_number, 
+                            ep_xfer[ep_number].buff_ptr, 
                             (uint16_t)ep_xfer[ep_number].remain_size);
 
               ep_xfer[ep_number].buff_ptr += xfer_bytes;
               ep_xfer[ep_number].remain_size -= xfer_bytes;
             }
+
+            if (ep_xfer[ep_number].remain_size == 0)
+            {
+              dcd_event_xfer_complete(BOARD_TUD_RHPORT, 
+                                      ep_number | TUSB_DIR_IN_MASK, 
+                                      ep_xfer[ep_number].total_size, XFER_RESULT_SUCCESS, true);
+            }
           }
 
           // When the transfer is complete...
           if (ep_xfer[ep_number].remain_size == 0)
           {
-            // Signal tinyUSB.
-            dcd_event_xfer_complete(BOARD_DEVICE_RHPORT_NUM, ep_number | ep_dirmask, ep_xfer[ep_number].total_size, XFER_RESULT_SUCCESS, true);
-
-            // Allow new transfers on this endpoint.
+            // Finish this transfer and allow new transfers on this endpoint.
             ep_xfer[ep_number].valid = 0;
 
             // Disable the interrupt for this endpoint now it is complete.
             USBD_REG(epie) = USBD_REG(epie) & (~(1 << ep_number));
           }
+
+          ep_xfer[ep_number].ready = 0;
+        }
+        // No OUT transfer is in flight for this endpoint.
+        else
+        {
+          if (ep_xfer[ep_number].dir == TUSB_DIR_OUT)
+          {
+            // We will record that there is data in the FIFO for dcd_edpt_xfer to obtain
+            // once the transfer is initiated.
+            // Strictly this should not happen for a non-control endpoint. Interrupts
+            // are disabled when there are no transfers setup for an endpoint.
+            ep_xfer[ep_number].ready = 1;          
+          }
         }
       }
     }
@@ -1075,7 +1161,7 @@ void dcd_int_handler(uint8_t rhport)
 
 // Power management interrupt handler. 
 // This handles USB device related power management interrupts only.
-void ft90x_usbd_pm_ISR(void)
+void ft9xx_usbd_pm_ISR(void)
 {
     uint16_t pmcfg = SYS->PMCFG_H;
 
@@ -1084,21 +1170,21 @@ void ft90x_usbd_pm_ISR(void)
   {
       // Signal connection interrupt
       SYS->PMCFG_H = MASK_SYS_PMCFG_PM_GPIO_IRQ_PEND;
-      dcd_event_bus_signal(BOARD_DEVICE_RHPORT_NUM, DCD_EVENT_RESUME, true);
+      dcd_event_bus_signal(BOARD_TUD_RHPORT, DCD_EVENT_RESUME, true);
   }
 
   if (pmcfg & MASK_SYS_PMCFG_DEV_DIS_DEV)
   {
       // Signal disconnection interrupt
       SYS->PMCFG_H = MASK_SYS_PMCFG_PM_GPIO_IRQ_PEND;
-      dcd_event_bus_signal(BOARD_DEVICE_RHPORT_NUM, DCD_EVENT_UNPLUGGED, true);
+      dcd_event_bus_signal(BOARD_TUD_RHPORT, DCD_EVENT_UNPLUGGED, true);
   }
 
   if (pmcfg & MASK_SYS_PMCFG_HOST_RST_DEV)
   {
       // Signal Host Reset interrupt
       SYS->PMCFG_H = MASK_SYS_PMCFG_PM_GPIO_IRQ_PEND;
-      dcd_event_bus_signal(BOARD_DEVICE_RHPORT_NUM, DCD_EVENT_BUS_RESET, true);
+      dcd_event_bus_signal(BOARD_TUD_RHPORT, DCD_EVENT_BUS_RESET, true);
   }
 
   if (pmcfg & MASK_SYS_PMCFG_HOST_RESUME_DEV)
@@ -1109,7 +1195,7 @@ void ft90x_usbd_pm_ISR(void)
       {
           // If we are driving K-state on Device USB port;
           // We must maintain the 1ms requirement before resuming the phy
-          dcd_event_bus_signal(BOARD_DEVICE_RHPORT_NUM, DCD_EVENT_RESUME, true);
+          dcd_event_bus_signal(BOARD_TUD_RHPORT, DCD_EVENT_RESUME, true);
       }
   }
 }
diff --git a/src/portable/chipidea/ci_hs/ci_hs_imxrt.h b/src/portable/chipidea/ci_hs/ci_hs_imxrt.h
index 78ca5a5a2..2de0d9cb4 100644
--- a/src/portable/chipidea/ci_hs/ci_hs_imxrt.h
+++ b/src/portable/chipidea/ci_hs/ci_hs_imxrt.h
@@ -29,6 +29,14 @@
 
 #include "fsl_device_registers.h"
 
+#if !defined(USB1_BASE) && defined(USB_OTG1_BASE)
+#define USB1_BASE USB_OTG1_BASE
+#endif
+
+#if !defined(USB2_BASE) && defined(USB_OTG2_BASE)
+#define USB2_BASE USB_OTG2_BASE
+#endif
+
 static const ci_hs_controller_t _ci_controller[] =
 {
   // RT1010 and RT1020 only has 1 USB controller
diff --git a/src/portable/chipidea/ci_hs/dcd_ci_hs.c b/src/portable/chipidea/ci_hs/dcd_ci_hs.c
index 41d2f1870..c7cc3e0e8 100644
--- a/src/portable/chipidea/ci_hs/dcd_ci_hs.c
+++ b/src/portable/chipidea/ci_hs/dcd_ci_hs.c
@@ -34,7 +34,7 @@
 #include "device/dcd.h"
 #include "ci_hs_type.h"
 
-#if CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX
+#if CFG_TUSB_MCU == OPT_MCU_MIMXRT
   #include "ci_hs_imxrt.h"
 #elif TU_CHECK_MCU(OPT_MCU_LPC18XX, OPT_MCU_LPC43XX)
   #include "ci_hs_lpc18_43.h"
diff --git a/src/portable/chipidea/ci_hs/hcd_ci_hs.c b/src/portable/chipidea/ci_hs/hcd_ci_hs.c
index 221721b0f..d0396daea 100644
--- a/src/portable/chipidea/ci_hs/hcd_ci_hs.c
+++ b/src/portable/chipidea/ci_hs/hcd_ci_hs.c
@@ -29,7 +29,7 @@
 // Chipidea Highspeed USB IP implement EHCI for host functionality
 
 #if CFG_TUH_ENABLED && \
-    (CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_LPC43XX || CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX)
+    (CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_LPC43XX || CFG_TUSB_MCU == OPT_MCU_MIMXRT)
 
 //--------------------------------------------------------------------+
 // INCLUDE
@@ -39,7 +39,7 @@
 #include "portable/ehci/ehci_api.h"
 #include "ci_hs_type.h"
 
-#if CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX
+#if CFG_TUSB_MCU == OPT_MCU_MIMXRT
   #include "ci_hs_imxrt.h"
 #elif TU_CHECK_MCU(OPT_MCU_LPC18XX, OPT_MCU_LPC43XX)
   #include "ci_hs_lpc18_43.h"
diff --git a/src/portable/dialog/da146xx/dcd_da146xx.c b/src/portable/dialog/da146xx/dcd_da146xx.c
index 0bb8b6e41..961da81d6 100644
--- a/src/portable/dialog/da146xx/dcd_da146xx.c
+++ b/src/portable/dialog/da146xx/dcd_da146xx.c
@@ -243,7 +243,7 @@ static struct
 
 // Converts xfer pointer to epnum (0,1,2,3) regardless of xfer direction
 #define XFER_EPNUM(xfer)      ((xfer - &_dcd.xfer_status[0][0]) >> 1)
-// Converts xfer pinter to EPx_REGS pointer (returns same pointer for IN and OUT with same endpoint number)
+// Converts xfer pointer to EPx_REGS pointer (returns same pointer for IN and OUT with same endpoint number)
 #define XFER_REGS(xfer)       ep_regs[XFER_EPNUM(xfer)]
 // Converts epnum (0,1,2,3) to EPx_REGS pointer
 #define EPNUM_REGS(epnum)     ep_regs[epnum]
diff --git a/src/portable/ehci/ehci.c b/src/portable/ehci/ehci.c
index 76ba2a921..7140897a1 100644
--- a/src/portable/ehci/ehci.c
+++ b/src/portable/ehci/ehci.c
@@ -188,7 +188,7 @@ tusb_speed_t hcd_port_speed_get(uint8_t rhport)
 static void list_remove_qhd_by_addr(ehci_link_t* list_head, uint8_t dev_addr)
 {
   for(ehci_link_t* prev = list_head;
-      !prev->terminate && (tu_align32(prev->address) != (uint32_t) list_head);
+      !prev->terminate && (tu_align32(prev->address) != (uint32_t) list_head) && prev != NULL;
       prev = list_next(prev) )
   {
     // TODO check type for ISO iTD and siTD
@@ -199,7 +199,7 @@ static void list_remove_qhd_by_addr(ehci_link_t* list_head, uint8_t dev_addr)
     #pragma GCC diagnostic pop
     if ( qhd->dev_addr == dev_addr )
     {
-      // TODO deactive all TD, wait for QHD to inactive before removal
+      // TODO deactivate all TD, wait for QHD to inactive before removal
       prev->address = qhd->next.address;
 
       // EHCI 4.8.2 link the removed qhd to async head (which always reachable by Host Controller)
@@ -839,7 +839,7 @@ static void qhd_init(ehci_qhd_t *p_qhd, uint8_t dev_addr, tusb_desc_endpoint_t c
     if (TUSB_SPEED_HIGH == p_qhd->ep_speed)
     {
       TU_ASSERT( interval <= 16, );
-      if ( interval < 4) // sub milisecond interval
+      if ( interval < 4) // sub millisecond interval
       {
         p_qhd->interval_ms = 0;
         p_qhd->int_smask   = (interval == 1) ? TU_BIN8(11111111) :
diff --git a/src/portable/ehci/ehci.h b/src/portable/ehci/ehci.h
index ff9ae12e7..36f8649be 100644
--- a/src/portable/ehci/ehci.h
+++ b/src/portable/ehci/ehci.h
@@ -114,7 +114,7 @@ typedef struct
 	volatile uint32_t current_page         : 3  ; ///< Index into the qTD buffer pointer list
 	uint32_t int_on_complete               : 1  ; ///< Interrupt on complete
 	volatile uint32_t total_bytes          : 15 ; ///< Transfer bytes, decreased during transaction
-	volatile uint32_t data_toggle          : 1  ; ///< Data Toogle bit
+	volatile uint32_t data_toggle          : 1  ; ///< Data Toggle bit
 
 
 	/// Buffer Page Pointer List, Each element in the list is a 4K page aligned, physical memory address. The lower 12 bits in each pointer are reserved (except for the first one) as each memory pointer must reference the start of a 4K page
@@ -160,7 +160,7 @@ typedef struct TU_ATTR_ALIGNED(32)
 	uint8_t used;
 	uint8_t removing; // removed from asyn list, waiting for async advance
 	uint8_t pid;
-	uint8_t interval_ms; // polling interval in frames (or milisecond)
+	uint8_t interval_ms; // polling interval in frames (or millisecond)
 
 	uint16_t total_xferred_bytes; // number of bytes xferred until a qtd with ioc bit set
 	uint8_t reserved2[2];
@@ -225,7 +225,7 @@ typedef struct TU_ATTR_ALIGNED(32)
 	uint16_t reserved ; ///< reserved
 
 	// Word 3: siTD Transfer Status and Control
-	// Status [7:0] TODO indentical to qTD Token'status --> refractor later
+	// Status [7:0] TODO identical to qTD Token'status --> refactor later
 	volatile uint32_t                 : 1  ; // reserved
 	volatile uint32_t split_state     : 1  ;
 	volatile uint32_t missed_uframe   : 1  ;
@@ -350,8 +350,8 @@ typedef volatile struct
       uint32_t periodic_status       : 1  ; ///< Periodic schedule status
       uint32_t async_status          : 1  ; ///< Async schedule status
       uint32_t                       : 2  ;
-      uint32_t nxp_int_async         : 1  ; ///< NXP customized: This bit is set by the Host Controller when the cause of an interrupt is a completion of a USB transaction where the Transfer Descriptor (TD) has an interrupt on complete (IOC) bit set andthe TD was from the asynchronous schedule. This bit is also set by the Host when a short packet is detected andthe packet is on the asynchronous schedule.
-      uint32_t nxp_int_period        : 1  ; ///< NXP customized: This bit is set by the Host Controller when the cause of an interrupt is a completion of a USB transaction where the Transfer Descriptor (TD) has an interrupt on complete (IOC) bit set andthe TD was from the periodic schedule.
+      uint32_t nxp_int_async         : 1  ; ///< NXP customized: This bit is set by the Host Controller when the cause of an interrupt is a completion of a USB transaction where the Transfer Descriptor (TD) has an interrupt on complete (IOC) bit set and the TD was from the asynchronous schedule. This bit is also set by the Host when a short packet is detected and the packet is on the asynchronous schedule.
+      uint32_t nxp_int_period        : 1  ; ///< NXP customized: This bit is set by the Host Controller when the cause of an interrupt is a completion of a USB transaction where the Transfer Descriptor (TD) has an interrupt on complete (IOC) bit set and the TD was from the periodic schedule.
       uint32_t                       : 12 ;
     }status_bm;
   };
diff --git a/src/portable/espressif/esp32sx/dcd_esp32sx.c b/src/portable/espressif/esp32sx/dcd_esp32sx.c
index a30fca0d8..41240f737 100644
--- a/src/portable/espressif/esp32sx/dcd_esp32sx.c
+++ b/src/portable/espressif/esp32sx/dcd_esp32sx.c
@@ -59,6 +59,7 @@ typedef struct {
     uint16_t queued_len;
     uint16_t max_size;
     bool short_packet;
+    uint8_t interval;
 } xfer_ctl_t;
 
 static const char *TAG = "TUSB:DCD";
@@ -267,6 +268,7 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *desc_edpt)
 
   xfer_ctl_t *xfer = XFER_CTL_BASE(epnum, dir);
   xfer->max_size = tu_edpt_packet_size(desc_edpt);
+  xfer->interval = desc_edpt->bInterval;
 
   if (dir == TUSB_DIR_OUT) {
     out_ep[epnum].doepctl |= USB_USBACTEP1_M |
@@ -379,6 +381,13 @@ bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t *buffer, uint16_t to
     USB0.in_ep_reg[epnum].dieptsiz = (num_packets << USB_D_PKTCNT0_S) | total_bytes;
     USB0.in_ep_reg[epnum].diepctl |= USB_D_EPENA1_M | USB_D_CNAK1_M; // Enable | CNAK
 
+    // For ISO endpoint with interval=1 set correct DATA0/DATA1 bit for next frame
+    if ((USB0.in_ep_reg[epnum].diepctl & USB_D_EPTYPE0_M) == (1 << USB_D_EPTYPE1_S) && xfer->interval == 1) {
+      // Take odd/even bit from frame counter.
+      uint32_t const odd_frame_now = (USB0.dsts & (1u << USB_SOFFN_S));
+      USB0.in_ep_reg[epnum].diepctl |= (odd_frame_now ? USB_DI_SETD0PID1 : USB_DI_SETD1PID1);
+    }
+
     // Enable fifo empty interrupt only if there are something to put in the fifo.
     if(total_bytes != 0) {
       USB0.dtknqr4_fifoemptymsk |= (1 << epnum);
@@ -387,6 +396,13 @@ bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t *buffer, uint16_t to
     // Each complete packet for OUT xfers triggers XFRC.
     USB0.out_ep_reg[epnum].doeptsiz |= USB_PKTCNT0_M | ((xfer->max_size & USB_XFERSIZE0_V) << USB_XFERSIZE0_S);
     USB0.out_ep_reg[epnum].doepctl  |= USB_EPENA0_M | USB_CNAK0_M;
+
+    // For ISO endpoint with interval=1 set correct DATA0/DATA1 bit for next frame
+    if ((USB0.out_ep_reg[epnum].doepctl & USB_D_EPTYPE0_M) == (1 << USB_D_EPTYPE1_S) && xfer->interval == 1) {
+      // Take odd/even bit from frame counter.
+      uint32_t const odd_frame_now = (USB0.dsts & (1u << USB_SOFFN_S));
+      USB0.out_ep_reg[epnum].doepctl |= (odd_frame_now ? USB_DO_SETD0PID1 : USB_DO_SETD1PID1);
+    }
   }
   return true;
 }
@@ -725,7 +741,7 @@ static void handle_epin_ints(void)
 
       // XFER Timeout
       if (USB0.in_ep_reg[n].diepint & USB_D_TIMEOUT0_M) {
-        // Clear interrupt or enpoint will hang.
+        // Clear interrupt or endpoint will hang.
         USB0.in_ep_reg[n].diepint = USB_D_TIMEOUT0_M;
         // Maybe retry?
       }
diff --git a/src/portable/microchip/pic/dcd_pic.c b/src/portable/microchip/pic/dcd_pic.c
new file mode 100644
index 000000000..6cf0e6285
--- /dev/null
+++ b/src/portable/microchip/pic/dcd_pic.c
@@ -0,0 +1,786 @@
+/* 
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2020 Koji Kitayama
+ * Copyright (c) 2022 Reimu NotMoe <[email protected]>
+ *
+ * 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.
+ */
+
+#include "tusb_option.h"
+
+#if CFG_TUD_ENABLED && \
+    (CFG_TUSB_MCU == OPT_MCU_PIC32MX || CFG_TUSB_MCU == OPT_MCU_PIC32MM || \
+     CFG_TUSB_MCU == OPT_MCU_PIC32MK || CFG_TUSB_MCU == OPT_MCU_PIC24 || \
+     CFG_TUSB_MCU == OPT_MCU_DSPIC33)
+
+#include <xc.h>
+
+#include "device/dcd.h"
+
+
+#if (CFG_TUSB_MCU == OPT_MCU_PIC32MX || CFG_TUSB_MCU == OPT_MCU_PIC32MM || CFG_TUSB_MCU == OPT_MCU_PIC32MK)
+
+#define TU_PIC_INT_SIZE         4
+
+#elif (CFG_TUSB_MCU == OPT_MCU_PIC24 || CFG_TUSB_MCU == OPT_MCU_DSPIC33)
+
+#define TU_PIC_INT_SIZE         2
+
+#else
+
+#error Unsupportd PIC MCU
+
+#endif
+
+
+#if TU_PIC_INT_SIZE == 4
+
+#ifndef KVA_TO_PA
+#define KVA_TO_PA(kva)    ((uint32_t)(kva) & 0x1fffffff)
+#endif
+
+#ifndef PA_TO_KVA1
+#define PA_TO_KVA1(pa)    ((uint32_t)(pa) | 0xA0000000)
+#endif
+
+#else
+
+#ifndef KVA_TO_PA
+#define KVA_TO_PA(kva)    (kva)
+#endif
+
+#ifndef PA_TO_KVA1
+#define PA_TO_KVA1(pa)    (pa)
+#endif
+
+#endif
+
+
+//--------------------------------------------------------------------+
+// MACRO TYPEDEF CONSTANT ENUM DECLARATION
+//--------------------------------------------------------------------+
+
+enum {
+  TOK_PID_OUT   = 0x1u,
+  TOK_PID_IN    = 0x9u,
+  TOK_PID_SETUP = 0xDu,
+};
+
+// The BDT is 8 bytes on 32bit PICs and 4 bytes on 8/16bit PICs
+#if TU_PIC_INT_SIZE == 4
+typedef struct TU_ATTR_PACKED
+{
+  union {
+    uint32_t head;
+    struct {
+      union {
+        struct {
+          uint16_t           :  2;
+          uint16_t tok_pid   :  4;
+          uint16_t data      :  1;
+          uint16_t own       :  1;
+          uint16_t           :  8;
+        };
+        struct {
+          uint16_t           :  2;
+          uint16_t bdt_stall :  1;
+          uint16_t dts       :  1;
+          uint16_t ninc      :  1;
+          uint16_t keep      :  1;
+          uint16_t           : 10;
+        };
+      };
+      uint16_t bc : 10;
+      uint16_t    :  6;
+    };
+  };
+  uint8_t *addr;
+} buffer_descriptor_t;
+
+TU_VERIFY_STATIC( sizeof(buffer_descriptor_t) == 8, "size is not correct" );
+#else
+typedef struct TU_ATTR_PACKED
+{
+  union {
+    uint16_t head;
+
+    struct {
+      uint16_t           :  10;
+      uint16_t tok_pid   :  4;
+      uint16_t data      :  1;
+      uint16_t own       :  1;
+    };
+    struct {
+      uint16_t           :  10;
+      uint16_t bdt_stall :  1;
+      uint16_t dts       :  1;
+      uint16_t ninc      :  1;
+      uint16_t keep      :  1;
+    };
+
+    struct {
+      uint16_t bc : 10;
+      uint16_t    :  6;
+    };
+  };
+  uint8_t *addr;
+} buffer_descriptor_t;
+
+TU_VERIFY_STATIC( sizeof(buffer_descriptor_t) == 4, "size is not correct" );
+#endif
+
+
+typedef struct TU_ATTR_PACKED
+{
+  union {
+    uint32_t state;
+    struct {
+      uint32_t max_packet_size :11;
+      uint32_t                 : 5;
+      uint32_t odd             : 1;
+      uint32_t                 :15;
+    };
+  };
+  uint16_t length;
+  uint16_t remaining;
+} endpoint_state_t;
+
+TU_VERIFY_STATIC( sizeof(endpoint_state_t) == 8, "size is not correct" );
+
+typedef struct
+{
+  union {
+    /* [#EP][OUT,IN][EVEN,ODD] */
+    buffer_descriptor_t bdt[16][2][2];
+#if TU_PIC_INT_SIZE == 4
+    uint16_t            bda[256];
+#else
+    uint8_t            bda[256];
+#endif
+  };
+  TU_ATTR_ALIGNED(4) union {
+    endpoint_state_t endpoint[16][2];
+    endpoint_state_t endpoint_unified[16 * 2];
+  };
+  uint8_t setup_packet[8];
+  uint8_t addr;
+} dcd_data_t;
+
+//--------------------------------------------------------------------+
+// INTERNAL OBJECT & FUNCTION DECLARATION
+//--------------------------------------------------------------------+
+// BDT(Buffer Descriptor Table) must be 256-byte aligned
+CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(512) volatile static dcd_data_t _dcd;
+
+#if TU_PIC_INT_SIZE == 4
+TU_VERIFY_STATIC( sizeof(_dcd.bdt) == 512, "size is not correct" );
+#else
+TU_VERIFY_STATIC( sizeof(_dcd.bdt) == 256, "size is not correct" );
+#endif
+
+#if TU_PIC_INT_SIZE == 4
+typedef uint32_t ep_reg_t;
+#elif TU_PIC_INT_SIZE == 2
+typedef uint16_t ep_reg_t;
+#endif
+
+static inline volatile void *ep_addr(uint8_t rhport, uint8_t ep_num) {
+#if CFG_TUSB_MCU == OPT_MCU_PIC32MK
+  volatile void *ep_reg_base = rhport ? (&U2EP0) : (&U1EP0);
+#else
+  volatile void *ep_reg_base = &U1EP0;
+#endif
+#if TU_PIC_INT_SIZE == 4
+  const size_t offset = 0x10;
+#else
+  const size_t offset = 0x2;
+#endif
+  return ep_reg_base + offset * ep_num;
+}
+
+static inline ep_reg_t ep_read(uint8_t rhport, uint8_t ep_num) {
+  volatile ep_reg_t *ep = ep_addr(rhport, ep_num);
+  return *ep;
+}
+
+static inline void ep_write(uint8_t rhport, uint8_t ep_num, ep_reg_t val) {
+  volatile ep_reg_t *ep = ep_addr(rhport, ep_num);
+  *ep = val;
+}
+
+static inline void ep_clear(uint8_t rhport, uint8_t ep_num, ep_reg_t val) {
+#if TU_PIC_INT_SIZE == 4
+  volatile ep_reg_t *ep_clr = (ep_addr(rhport, ep_num) + 0x4);
+  *ep_clr = val;
+#else
+  ep_reg_t v = ep_read(rhport, ep_num);
+  v &= ~val;
+  ep_write(rhport, ep_num, v);
+#endif
+}
+
+static inline void ep_set(uint8_t rhport, uint8_t ep_num, ep_reg_t val) {
+#if TU_PIC_INT_SIZE == 4
+  volatile ep_reg_t *ep_s = (ep_addr(rhport, ep_num) + 0x8);
+  *ep_s = val;
+#else
+  ep_reg_t v = ep_read(rhport, ep_num);
+  v |= val;
+  ep_write(rhport, ep_num, v);
+#endif
+}
+
+static inline void intr_enable(uint8_t rhport) {
+#if CFG_TUSB_MCU == OPT_MCU_PIC32MM
+  IEC0SET = _IEC0_USBIE_MASK;
+#elif CFG_TUSB_MCU == OPT_MCU_PIC32MX
+  IEC1SET = _IEC1_USBIE_MASK;
+#elif CFG_TUSB_MCU == OPT_MCU_PIC32MK
+  if (rhport == 0)
+    IEC1SET = _IEC1_USB1IE_MASK;
+  else
+    IEC7SET = _IEC7_USB2IE_MASK;
+#elif (CFG_TUSB_MCU == OPT_MCU_PIC24) || (CFG_TUSB_MCU == OPT_MCU_DSPIC33)
+  IEC5bits.USB1IE = 1;
+#endif
+}
+
+static inline void intr_disable(uint8_t rhport) {
+#if CFG_TUSB_MCU == OPT_MCU_PIC32MM
+  IEC0CLR = _IEC0_USBIE_MASK;
+#elif CFG_TUSB_MCU == OPT_MCU_PIC32MX
+  IEC1CLR = _IEC1_USBIE_MASK;
+#elif CFG_TUSB_MCU == OPT_MCU_PIC32MK
+  if (rhport == 0)
+    IEC1CLR = _IEC1_USB1IE_MASK;
+  else
+    IEC7CLR = _IEC7_USB2IE_MASK;
+#elif (CFG_TUSB_MCU == OPT_MCU_PIC24) || (CFG_TUSB_MCU == OPT_MCU_DSPIC33)
+  IEC5bits.USB1IE = 0;
+#endif
+}
+
+static inline int intr_is_enabled(uint8_t rhport) {
+#if CFG_TUSB_MCU == OPT_MCU_PIC32MM
+  return IEC0bits.USBIE;
+#elif CFG_TUSB_MCU == OPT_MCU_PIC32MX
+  return IEC1bits.USBIE;
+#elif CFG_TUSB_MCU == OPT_MCU_PIC32MK
+  if (rhport == 0)
+    return IEC1bits.USB1IE;
+  else
+    return IEC7bits.USB2IE;
+#elif (CFG_TUSB_MCU == OPT_MCU_PIC24) || (CFG_TUSB_MCU == OPT_MCU_DSPIC33)
+  return IEC5bits.USB1IE;
+#endif
+}
+
+static inline void intr_clear(uint8_t rhport) {
+#if CFG_TUSB_MCU == OPT_MCU_PIC32MM
+  IFS0CLR = _IFS0_USBIF_MASK;
+#elif CFG_TUSB_MCU == OPT_MCU_PIC32MX
+  IFS1CLR = _IFS1_USBIF_MASK;
+#elif CFG_TUSB_MCU == OPT_MCU_PIC32MK
+  if (rhport == 0)
+    IFS1CLR = _IFS1_USB1IF_MASK;
+  else
+    IFS7CLR = _IFS7_USB2IF_MASK;
+#elif (CFG_TUSB_MCU == OPT_MCU_PIC24) || (CFG_TUSB_MCU == OPT_MCU_DSPIC33)
+  IFS5bits.USB1IF = 0;
+#endif
+}
+
+static void prepare_next_setup_packet(uint8_t rhport)
+{
+  const unsigned out_odd = _dcd.endpoint[0][0].odd;
+  const unsigned in_odd  = _dcd.endpoint[0][1].odd;
+  TU_ASSERT(0 == _dcd.bdt[0][0][out_odd].own, );
+
+  _dcd.bdt[0][0][out_odd].data     = 0;
+  _dcd.bdt[0][0][out_odd ^ 1].data = 1;
+  _dcd.bdt[0][1][in_odd].data      = 1;
+  _dcd.bdt[0][1][in_odd ^ 1].data  = 0;
+  dcd_edpt_xfer(rhport, tu_edpt_addr(0, TUSB_DIR_OUT),
+          _dcd.setup_packet, sizeof(_dcd.setup_packet));
+}
+
+static void process_stall(uint8_t rhport)
+{
+  for (int i = 0; i < 16; ++i) {
+    unsigned const endpt = ep_read(rhport, i);
+
+    if (endpt & _U1EP0_EPSTALL_MASK) {
+      // prepare next setup if endpoint0
+      if ( i == 0 ) prepare_next_setup_packet(rhport);
+
+      // clear stall bit
+      ep_clear(rhport, i, _U1EP0_EPSTALL_MASK);
+    }
+  }
+}
+
+static void process_tokdne(uint8_t rhport)
+{
+  ep_reg_t s = U1STAT;
+
+  U1IR = _U1IR_TRNIF_MASK;
+
+  uint8_t epnum = (s >> _U1STAT_ENDPT0_POSITION);
+  uint8_t dir   = (s & _U1STAT_DIR_MASK) >> _U1STAT_DIR_POSITION;
+  unsigned odd  = (s & _U1STAT_PPBI_MASK) ? 1 : 0;
+
+  buffer_descriptor_t *bd = (buffer_descriptor_t *)&_dcd.bda[s];
+  endpoint_state_t    *ep = &_dcd.endpoint_unified[s >> 3];
+
+  /* fetch pid before discarded by the next steps */
+  const unsigned pid = bd->tok_pid;
+
+  /* reset values for a next transfer */
+  bd->bdt_stall = 0;
+  bd->dts       = 1;
+  bd->ninc      = 0;
+  bd->keep      = 0;
+  /* update the odd variable to prepare for the next transfer */
+  ep->odd       = odd ^ 1;
+  if (pid == TOK_PID_SETUP) {
+    dcd_event_setup_received(rhport, (uint8_t *)PA_TO_KVA1(bd->addr), true);
+#if TU_PIC_INT_SIZE == 4
+    U1CONCLR = _U1CON_PKTDIS_TOKBUSY_MASK;
+#else
+    U1CONbits.PKTDIS = 0;
+#endif
+    return;
+  }
+
+  const unsigned bc = bd->bc;
+  const unsigned remaining = ep->remaining - bc;
+  if (remaining && bc == ep->max_packet_size) {
+    /* continue the transferring consecutive data */
+    ep->remaining = remaining;
+    const int next_remaining = remaining - ep->max_packet_size;
+    if (next_remaining > 0) {
+      /* prepare to the after next transfer */
+      bd->addr += ep->max_packet_size * 2;
+      bd->bc    = next_remaining > ep->max_packet_size ? ep->max_packet_size: next_remaining;
+      bd->own   = 1; /* the own bit must set after addr */
+    }
+    return;
+  }
+  const unsigned length = ep->length;
+  dcd_event_xfer_complete(rhport,
+        tu_edpt_addr(epnum, dir),
+        length - remaining, XFER_RESULT_SUCCESS, true);
+  if (0 == epnum && 0 == length) {
+    /* After completion a ZLP of control transfer,
+     * it prepares for the next steup transfer. */
+    if (_dcd.addr) {
+      /* When the transfer was the SetAddress,
+       * the device address should be updated here. */
+      U1ADDR = _dcd.addr;
+      _dcd.addr  = 0;
+    }
+    prepare_next_setup_packet(rhport);
+  }
+}
+
+static void process_bus_reset(uint8_t rhport)
+{
+#if TU_PIC_INT_SIZE == 4
+  U1PWRCCLR = _U1PWRC_USUSPEND_MASK;
+  U1CONSET = _U1CON_PPBRST_MASK;
+#else
+  U1PWRCbits.USUSPND = 0;
+  U1CONbits.PPBRST = 1;
+#endif
+  U1ADDR = 0;
+
+  U1IE = _U1IE_URSTIE_MASK | _U1IE_TRNIE_MASK | _U1IE_IDLEIE_MASK |
+         _U1IE_UERRIE_MASK | _U1IE_STALLIE_MASK;
+
+  U1EP0 = _U1EP0_EPHSHK_MASK | _U1EP0_EPRXEN_MASK | _U1EP0_EPTXEN_MASK;
+
+  for (unsigned i = 1; i < 16; ++i) {
+    ep_write(rhport, i, 0);
+  }
+
+  buffer_descriptor_t *bd = _dcd.bdt[0][0];
+  for (unsigned i = 0; i < sizeof(_dcd.bdt)/sizeof(*bd); ++i, ++bd) {
+    bd->head = 0;
+  }
+  const endpoint_state_t ep0 = {
+    .max_packet_size = CFG_TUD_ENDPOINT0_SIZE,
+    .odd             = 0,
+    .length          = 0,
+    .remaining       = 0,
+  };
+  _dcd.endpoint[0][0] = ep0;
+  _dcd.endpoint[0][1] = ep0;
+  tu_memclr(_dcd.endpoint[1], sizeof(_dcd.endpoint) - sizeof(_dcd.endpoint[0]));
+  _dcd.addr = 0;
+  prepare_next_setup_packet(rhport);
+#if TU_PIC_INT_SIZE == 4
+  U1CONCLR = _U1CON_PPBRST_MASK;
+#else
+  U1CONbits.PPBRST = 0;
+#endif
+  dcd_event_bus_reset(rhport, TUSB_SPEED_FULL, true);
+}
+
+static void process_bus_sleep(uint8_t rhport)
+{
+  // Enable resume & disable suspend interrupt
+  dcd_event_bus_signal(rhport, DCD_EVENT_SUSPEND, true);
+}
+
+static void process_bus_resume(uint8_t rhport)
+{
+  // Enable suspend & disable resume interrupt
+#if TU_PIC_INT_SIZE == 4
+  U1PWRCCLR = _U1PWRC_USUSPEND_MASK;
+  U1IECLR = _U1IE_RESUMEIE_MASK;
+  U1IESET = _U1IE_IDLEIE_MASK;
+#else
+  U1PWRCbits.USUSPND = 0;
+  U1IEbits.RESUMEIE = 0;
+  U1IEbits.IDLEIE = 1;
+#endif
+
+  dcd_event_bus_signal(rhport, DCD_EVENT_RESUME, true);
+}
+
+/*------------------------------------------------------------------*/
+/* Device API
+ *------------------------------------------------------------------*/
+void dcd_init(uint8_t rhport)
+{
+  intr_disable(rhport);
+  intr_clear(rhport);
+
+#if CFG_TUSB_MCU == OPT_MCU_PIC32MM
+  TRISBbits.TRISB6 = 1;
+#endif
+
+  tu_memclr(&_dcd, sizeof(_dcd));
+
+#if TU_PIC_INT_SIZE == 4
+  U1PWRCSET = _U1PWRC_USBPWR_MASK;
+#else
+  U1PWRCbits.USBPWR = 1;
+#endif
+  
+#if TU_PIC_INT_SIZE == 4
+  uint32_t bdt_phys = KVA_TO_PA((uintptr_t)_dcd.bdt);
+
+  U1BDTP1 = (uint8_t)(bdt_phys >>  8);
+  U1BDTP2 = (uint8_t)(bdt_phys >> 16);
+  U1BDTP3 = (uint8_t)(bdt_phys >> 24);
+#else
+  U1BDTP1 = (uint8_t)((uint16_t)(void *)_dcd.bdt >> 8);
+
+  U1CNFG1bits.PPB = 2;
+#endif
+
+  U1IE = _U1IE_URSTIE_MASK;
+
+  dcd_connect(rhport);
+}
+
+void dcd_int_enable(uint8_t rhport)
+{
+  intr_enable(rhport);
+}
+
+void dcd_int_disable(uint8_t rhport)
+{
+  intr_disable(rhport);
+}
+
+void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
+{
+  _dcd.addr = dev_addr & 0x7F;
+  /* Response with status first before changing device address */
+  dcd_edpt_xfer(rhport, tu_edpt_addr(0, TUSB_DIR_IN), NULL, 0);
+}
+
+void dcd_remote_wakeup(uint8_t rhport)
+{
+#if TU_PIC_INT_SIZE == 4
+  U1CONSET = _U1CON_RESUME_MASK;
+#else
+  U1CONbits.RESUME = 1;
+#endif
+  unsigned cnt = 25000000 / 1000;
+  while (cnt--) asm volatile("nop");
+
+#if TU_PIC_INT_SIZE == 4
+  U1CONCLR = _U1CON_RESUME_MASK;
+#else
+  U1CONbits.RESUME = 0;
+#endif
+}
+
+void dcd_connect(uint8_t rhport)
+{
+  while (!U1CONbits.USBEN) {
+#if TU_PIC_INT_SIZE == 4
+    U1CONSET = _U1CON_USBEN_SOFEN_MASK;
+#else
+    U1CONbits.USBEN = 1;
+#endif
+  }
+}
+
+void dcd_disconnect(uint8_t rhport)
+{
+  U1CON = 0;
+}
+
+void dcd_sof_enable(uint8_t rhport, bool en)
+{
+  (void) rhport;
+  (void) en;
+}
+
+//--------------------------------------------------------------------+
+// Endpoint API
+//--------------------------------------------------------------------+
+bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * ep_desc)
+{
+  const unsigned ep_addr  = ep_desc->bEndpointAddress;
+  const unsigned epn      = tu_edpt_number(ep_addr);
+  const unsigned dir      = tu_edpt_dir(ep_addr);
+  const unsigned xfer     = ep_desc->bmAttributes.xfer;
+  endpoint_state_t *ep    = &_dcd.endpoint[epn][dir];
+  const unsigned odd      = ep->odd;
+  buffer_descriptor_t *bd = _dcd.bdt[epn][dir];
+
+  /* No support for control transfer */
+  TU_ASSERT(epn && (xfer != TUSB_XFER_CONTROL));
+
+  ep->max_packet_size = tu_edpt_packet_size(ep_desc);
+
+
+  unsigned val = _U1EP0_EPCONDIS_MASK;
+  val |= (xfer != TUSB_XFER_ISOCHRONOUS) ? _U1EP0_EPHSHK_MASK : 0;
+  val |= dir ? _U1EP0_EPTXEN_MASK : _U1EP0_EPRXEN_MASK;
+
+  ep_reg_t tmp = ep_read(rhport, epn);
+  tmp |= val;
+  ep_write(rhport, epn, tmp);
+
+  if (xfer != TUSB_XFER_ISOCHRONOUS) {
+    bd[odd].dts      = 1;
+    bd[odd].data     = 0;
+    bd[odd ^ 1].dts  = 1;
+    bd[odd ^ 1].data = 1;
+  }
+
+  return true;
+}
+
+void dcd_edpt_close_all(uint8_t rhport)
+{
+  const unsigned ie = intr_is_enabled(rhport);
+  intr_disable(rhport);
+
+  for (unsigned i = 1; i < 16; ++i) {
+    ep_write(rhport, i, 0);
+  }
+
+  if (ie) intr_enable(rhport);
+
+  buffer_descriptor_t *bd = _dcd.bdt[1][0];
+  for (unsigned i = 2; i < sizeof(_dcd.bdt)/sizeof(*bd); ++i, ++bd) {
+    bd->head = 0;
+  }
+  endpoint_state_t *ep = &_dcd.endpoint[1][0];
+  for (unsigned i = 2; i < sizeof(_dcd.endpoint)/sizeof(*ep); ++i, ++ep) {
+    /* Clear except the odd */
+    ep->max_packet_size = 0;
+    ep->length          = 0;
+    ep->remaining       = 0;
+  }
+}
+
+void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr)
+{
+  const unsigned epn      = tu_edpt_number(ep_addr);
+  const unsigned dir      = tu_edpt_dir(ep_addr);
+  endpoint_state_t *ep    = &_dcd.endpoint[epn][dir];
+  buffer_descriptor_t *bd = _dcd.bdt[epn][dir];
+  const unsigned msk      = dir ? _U1EP0_EPTXEN_MASK : _U1EP0_EPRXEN_MASK;
+  const unsigned ie       = intr_is_enabled(rhport);
+
+  intr_disable(rhport);
+
+  ep_clear(rhport, epn, msk);
+
+  ep->max_packet_size = 0;
+  ep->length          = 0;
+  ep->remaining       = 0;
+  bd[0].head          = 0;
+  bd[1].head          = 0;
+
+  if (ie) intr_enable(rhport);
+}
+
+bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes)
+{
+
+  const unsigned epn      = tu_edpt_number(ep_addr);
+  const unsigned dir      = tu_edpt_dir(ep_addr);
+  endpoint_state_t    *ep = &_dcd.endpoint[epn][dir];
+  buffer_descriptor_t *bd = &_dcd.bdt[epn][dir][ep->odd];
+  TU_ASSERT(0 == bd->own);
+
+  const unsigned ie       = intr_is_enabled(rhport);
+
+  intr_disable(rhport);
+
+  ep->length    = total_bytes;
+  ep->remaining = total_bytes;
+
+  const unsigned mps = ep->max_packet_size;
+  if (total_bytes > mps) {
+    buffer_descriptor_t *next = ep->odd ? bd - 1: bd + 1;
+    /* When total_bytes is greater than the max packet size,
+     * it prepares to the next transfer to avoid NAK in advance. */
+    next->bc   = total_bytes >= 2 * mps ? mps: total_bytes - mps;
+    next->addr = (uint8_t *)KVA_TO_PA(buffer + mps);
+    next->own  = 1;
+  }
+  bd->bc   = total_bytes >= mps ? mps: total_bytes;
+  bd->addr = (uint8_t *)KVA_TO_PA(buffer);
+  bd->own  = 1; /* This bit must be set last */
+
+  if (ie) intr_enable(rhport);
+
+  return true;
+}
+
+void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+  const unsigned epn = tu_edpt_number(ep_addr);
+
+  if (0 == epn) {
+    ep_set(rhport, epn, _U1EP0_EPSTALL_MASK);
+  } else {
+    const unsigned dir      = tu_edpt_dir(ep_addr);
+    const unsigned odd      = _dcd.endpoint[epn][dir].odd;
+    buffer_descriptor_t *bd = &_dcd.bdt[epn][dir][odd];
+    TU_ASSERT(0 == bd->own,);
+
+    const unsigned ie       = intr_is_enabled(rhport);
+
+    intr_disable(rhport);
+
+    bd->bdt_stall = 1;
+    bd->own       = 1; /* This bit must be set last */
+
+    if (ie) intr_enable(rhport);
+  }
+}
+
+void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
+{
+  const unsigned epn      = tu_edpt_number(ep_addr);
+  TU_VERIFY(epn,);
+  const unsigned dir      = tu_edpt_dir(ep_addr);
+  const unsigned odd      = _dcd.endpoint[epn][dir].odd;
+  buffer_descriptor_t *bd = _dcd.bdt[epn][dir];
+  TU_VERIFY(bd[odd].own,);
+
+  const unsigned ie       = intr_is_enabled(rhport);
+
+  intr_disable(rhport);
+
+  bd[odd].own = 0;
+
+  // clear stall
+  bd[odd].bdt_stall  = 0;
+
+  // Reset data toggle
+  bd[odd    ].data = 0;
+  bd[odd ^ 1].data = 1;
+
+  // We already cleared this in ISR, but just clear it here to be safe
+  const unsigned endpt = ep_read(rhport, epn);
+  if (endpt & _U1EP0_EPSTALL_MASK) {
+    ep_clear(rhport, endpt, _U1EP0_EPSTALL_MASK);
+  }
+
+  if (ie) intr_enable(rhport);
+}
+
+//--------------------------------------------------------------------+
+// ISR
+//--------------------------------------------------------------------+
+void dcd_int_handler(uint8_t rhport)
+{
+  uint32_t is  = U1IR;
+  uint32_t msk = U1IE;
+
+  U1IR = is & ~msk;
+  is &= msk;
+
+  if (is & _U1IR_UERRIF_MASK) {
+    uint32_t es = U1EIR;
+    U1EIR = es;
+    U1IR   = is; /* discard any pending events */
+  }
+
+  if (is & _U1IR_URSTIF_MASK) {
+    U1IR = is; /* discard any pending events */
+    process_bus_reset(rhport);
+  }
+
+  if (is & _U1IR_IDLEIF_MASK) {
+    // 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
+    U1IR = _U1IR_IDLEIF_MASK;
+    process_bus_sleep(rhport);
+  }
+
+  if (is & _U1IR_RESUMEIF_MASK) {
+    U1IR = _U1IR_RESUMEIF_MASK;
+    process_bus_resume(rhport);
+  }
+
+  if (is & _U1IR_SOFIF_MASK) {
+    U1IR = _U1IR_SOFIF_MASK;
+    dcd_event_bus_signal(rhport, DCD_EVENT_SOF, true);
+  }
+
+  if (is & _U1IR_STALLIF_MASK) {
+    U1IR = _U1IR_STALLIF_MASK;
+    process_stall(rhport);
+  }
+
+  if (is & _U1IR_TRNIF_MASK) {
+    process_tokdne(rhport);
+  }
+
+  intr_clear(rhport);
+}
+
+#endif
diff --git a/src/portable/microchip/pic32mz/usbhs_registers.h b/src/portable/microchip/pic32mz/usbhs_registers.h
index 757e3f083..93b552322 100644
--- a/src/portable/microchip/pic32mz/usbhs_registers.h
+++ b/src/portable/microchip/pic32mz/usbhs_registers.h
@@ -21,16 +21,16 @@
 * THAT YOU HAVE PAID DIRECTLY TO MICROCHIP FOR THIS SOFTWARE.
 *******************************************************************************/
 /*******************************************************************************
-  USBHS Peripheral Library Register Defintions 
+  USBHS Peripheral Library Register Definitions 
 
   File Name:
     usbhs_registers.h
 
   Summary:
-    USBHS PLIB Register Defintions
+    USBHS PLIB Register Definitions
 
   Description:
-    This file contains the constants and defintions which are required by the
+    This file contains the constants and definitions which are required by the
     the USBHS library.
 *******************************************************************************/
 
diff --git a/src/portable/microchip/samd/dcd_samd.c b/src/portable/microchip/samd/dcd_samd.c
index 5ae5a554b..976d3dfd0 100644
--- a/src/portable/microchip/samd/dcd_samd.c
+++ b/src/portable/microchip/samd/dcd_samd.c
@@ -222,14 +222,14 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
   UsbDeviceDescBank* bank = &sram_registers[epnum][dir];
   uint32_t size_value = 0;
   while (size_value < 7) {
-    if (1 << (size_value + 3) == tu_edpt_packet_size(desc_edpt)) {
+    if (1 << (size_value + 3) >= tu_edpt_packet_size(desc_edpt)) {
       break;
     }
     size_value++;
   }
 
   // unsupported endpoint size
-  if ( size_value == 7 && tu_edpt_packet_size(desc_edpt) != 1023 ) return false;
+  if ( size_value == 7 && tu_edpt_packet_size(desc_edpt) > 1023 ) return false;
 
   bank->PCKSIZE.bit.SIZE = size_value;
 
@@ -250,6 +250,13 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
   return true;
 }
 
+void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr) {
+  (void) rhport;
+  (void) ep_addr;
+
+  // TODO: implement if necessary?
+}
+
 void dcd_edpt_close_all (uint8_t rhport)
 {
   (void) rhport;
@@ -279,14 +286,14 @@ bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
   {
     bank->PCKSIZE.bit.MULTI_PACKET_SIZE = total_bytes;
     bank->PCKSIZE.bit.BYTE_COUNT = 0;
-    ep->EPSTATUSCLR.reg |= USB_DEVICE_EPSTATUSCLR_BK0RDY;
-    ep->EPINTFLAG.reg |= USB_DEVICE_EPINTFLAG_TRFAIL0;
+    ep->EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_BK0RDY;
+    ep->EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_TRFAIL0;
   } else
   {
     bank->PCKSIZE.bit.MULTI_PACKET_SIZE = 0;
     bank->PCKSIZE.bit.BYTE_COUNT = total_bytes;
-    ep->EPSTATUSSET.reg |= USB_DEVICE_EPSTATUSSET_BK1RDY;
-    ep->EPINTFLAG.reg |= USB_DEVICE_EPINTFLAG_TRFAIL1;
+    ep->EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_BK1RDY;
+    ep->EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_TRFAIL1;
   }
 
   return true;
diff --git a/src/portable/microchip/samx7x/dcd_samx7x.c b/src/portable/microchip/samx7x/dcd_samx7x.c
index 7507c0f69..24657872b 100644
--- a/src/portable/microchip/samx7x/dcd_samx7x.c
+++ b/src/portable/microchip/samx7x/dcd_samx7x.c
@@ -42,7 +42,7 @@
 #  define USE_SOF         0
 #endif
 
-// Dual bank can imporve performance, but need 2 times bigger packet buffer
+// Dual bank can improve performance, but need 2 times bigger packet buffer
 // As SAM7x has only 4KB packet buffer, use with caution !
 // Enable in FS mode as packets are smaller
 #ifndef USE_DUAL_BANK
@@ -644,7 +644,7 @@ bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
     }
     __set_PRIMASK(irq_state);
 
-    // Here a ZLP has been recieved
+    // Here a ZLP has been received
     // and the DMA transfer must be not started.
     // It is the end of transfer
     return false;
@@ -734,7 +734,7 @@ bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16
     }
     __set_PRIMASK(irq_state);
 
-    // Here a ZLP has been recieved
+    // Here a ZLP has been received
     // and the DMA transfer must be not started.
     // It is the end of transfer
     return false;
diff --git a/src/portable/nordic/nrf5x/dcd_nrf5x.c b/src/portable/nordic/nrf5x/dcd_nrf5x.c
index 430d8eded..afc14b010 100644
--- a/src/portable/nordic/nrf5x/dcd_nrf5x.c
+++ b/src/portable/nordic/nrf5x/dcd_nrf5x.c
@@ -28,6 +28,7 @@
 
 #if CFG_TUD_ENABLED && CFG_TUSB_MCU == OPT_MCU_NRF5X
 
+#include <stdatomic.h>
 #include "nrf.h"
 #include "nrf_clock.h"
 #include "nrf_power.h"
@@ -72,6 +73,7 @@ typedef struct
   // nRF will auto accept OUT packet after DMA is done
   // indicate packet is already ACK
   volatile bool data_received;
+  volatile bool started;
 
   // Set to true when data was transferred from RAM to ISO IN output buffer.
   // New data can be put in ISO IN output buffer after SOF.
@@ -79,9 +81,6 @@ typedef struct
 
 } xfer_td_t;
 
-static osal_mutex_def_t dcd_mutex_def;
-static osal_mutex_t dcd_mutex;
-
 // Data for managing dcd
 static struct
 {
@@ -90,7 +89,7 @@ static struct
   xfer_td_t xfer[EP_CBI_COUNT + 1][2];
 
   // nRF can only carry one DMA at a time, this is used to guard the access to EasyDMA
-  volatile bool dma_running;
+  atomic_bool dma_running;
 }_dcd;
 
 /*------------------------------------------------------------------*/
@@ -121,8 +120,6 @@ TU_ATTR_ALWAYS_INLINE static inline bool is_in_isr(void)
 // helper to start DMA
 static void start_dma(volatile uint32_t* reg_startep)
 {
-  _dcd.dma_running = true;
-
   (*reg_startep) = 1;
   __ISB(); __DSB();
 
@@ -131,50 +128,18 @@ static void start_dma(volatile uint32_t* reg_startep)
   // Therefore dma_pending is corrected right away
   if ( (reg_startep == &NRF_USBD->TASKS_EP0STATUS) || (reg_startep == &NRF_USBD->TASKS_EP0RCVOUT) )
   {
-    _dcd.dma_running = false;
+    atomic_flag_clear(&_dcd.dma_running);
   }
 }
 
-// only 1 EasyDMA can be active at any time
-// TODO use Cortex M4 LDREX and STREX command (atomic) to have better mutex access to EasyDMA
-// since current implementation does not 100% guarded against race condition
 static void edpt_dma_start(volatile uint32_t* reg_startep)
 {
-  // Called in critical section i.e within USB ISR, or USB/Global interrupt disabled
-  if ( is_in_isr() || __get_PRIMASK() || !NVIC_GetEnableIRQ(USBD_IRQn) )
+  if ( atomic_flag_test_and_set(&_dcd.dma_running) )
   {
-    if (_dcd.dma_running)
-    {
-      //use usbd task to defer later
-      usbd_defer_func((osal_task_func_t) edpt_dma_start, (void*) (uintptr_t) reg_startep, true);
-    }else
-    {
-      start_dma(reg_startep);
-    }
+    usbd_defer_func((osal_task_func_t) edpt_dma_start, (void*) (uintptr_t) reg_startep, true);
   }else
   {
-    // Called in non-critical thread-mode, should be 99% of the time.
-    // Should be safe to blocking wait until previous DMA transfer complete
-    uint8_t const rhport = 0;
-    bool started = false;
-    osal_mutex_lock(dcd_mutex, OSAL_TIMEOUT_WAIT_FOREVER);
-    while(!started)
-    {
-      // LDREX/STREX may be needed in form of std atomic (required C11) or
-      // use osal mutex to guard against multiple core MCUs such as nRF53
-      dcd_int_disable(rhport);
-
-      if ( !_dcd.dma_running )
-      {
-        start_dma(reg_startep);
-        started = true;
-      }
-
-      dcd_int_enable(rhport);
-
-      // osal_yield();
-    }
-    osal_mutex_unlock(dcd_mutex);
+    start_dma(reg_startep);
   }
 }
 
@@ -182,7 +147,7 @@ static void edpt_dma_start(volatile uint32_t* reg_startep)
 static void edpt_dma_end(void)
 {
   TU_ASSERT(_dcd.dma_running, );
-  _dcd.dma_running = false;
+  atomic_flag_clear(&_dcd.dma_running);
 }
 
 // helper getting td
@@ -191,12 +156,26 @@ static inline xfer_td_t* get_td(uint8_t epnum, uint8_t dir)
   return &_dcd.xfer[epnum][dir];
 }
 
+static void xact_out_dma(uint8_t epnum);
+// Function wraps xact_out_dma which wants uint8_t while usbd_defer_func wants void (*)(void *)
+static void xact_out_dma_wrapper(void *epnum)
+{
+  xact_out_dma((uint8_t)((uintptr_t)epnum));
+}
+
 // Start DMA to move data from Endpoint -> RAM
 static void xact_out_dma(uint8_t epnum)
 {
   xfer_td_t* xfer = get_td(epnum, TUSB_DIR_OUT);
   uint32_t xact_len;
 
+  // DMA can't be active during read of SIZE.EPOUT or SIZE.ISOOUT, so try to lock,
+  // If already running defer call regardless if it was called from ISR or task,
+  if ( atomic_flag_test_and_set(&_dcd.dma_running) )
+  {
+    usbd_defer_func((osal_task_func_t)xact_out_dma_wrapper, (void *)(uint32_t)epnum, is_in_isr());
+    return;
+  }
   if (epnum == EP_ISO_NUM)
   {
     xact_len = NRF_USBD->SIZE.ISOOUT;
@@ -204,6 +183,7 @@ static void xact_out_dma(uint8_t epnum)
     if (xact_len & USBD_SIZE_ISOOUT_ZERO_Msk)
     {
       xact_len = 0;
+      atomic_flag_clear(&_dcd.dma_running);
     }
     else
     {
@@ -211,7 +191,7 @@ static void xact_out_dma(uint8_t epnum)
       NRF_USBD->ISOOUT.PTR = (uint32_t) xfer->buffer;
       NRF_USBD->ISOOUT.MAXCNT = xact_len;
 
-      edpt_dma_start(&NRF_USBD->TASKS_STARTISOOUT);
+      start_dma(&NRF_USBD->TASKS_STARTISOOUT);
     }
   }
   else
@@ -223,7 +203,7 @@ static void xact_out_dma(uint8_t epnum)
     NRF_USBD->EPOUT[epnum].PTR = (uint32_t) xfer->buffer;
     NRF_USBD->EPOUT[epnum].MAXCNT = xact_len;
 
-    edpt_dma_start(&NRF_USBD->TASKS_STARTEPOUT[epnum]);
+    start_dma(&NRF_USBD->TASKS_STARTEPOUT[epnum]);
   }
 }
 
@@ -248,7 +228,6 @@ static void xact_in_dma(uint8_t epnum)
 void dcd_init (uint8_t rhport)
 {
   TU_LOG1("dcd init\r\n");
-  dcd_mutex = osal_mutex_create(&dcd_mutex_def);
   (void) rhport;
 }
 
@@ -459,6 +438,7 @@ void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr)
     // When both ISO endpoint are close there is no need for SOF any more.
     if (_dcd.xfer[EP_ISO_NUM][TUSB_DIR_IN].mps + _dcd.xfer[EP_ISO_NUM][TUSB_DIR_OUT].mps == 0) NRF_USBD->INTENCLR = USBD_INTENCLR_SOF_Msk;
   }
+  _dcd.xfer[epnum][dir].started = false;
   __ISB(); __DSB();
 }
 
@@ -471,17 +451,10 @@ bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
 
   xfer_td_t* xfer = get_td(epnum, dir);
 
-  if (!is_in_isr()) {
-    osal_mutex_lock(dcd_mutex, OSAL_TIMEOUT_WAIT_FOREVER);
-    dcd_int_disable(rhport);
-  }
+  TU_ASSERT(!xfer->started);
   xfer->buffer     = buffer;
   xfer->total_len  = total_bytes;
   xfer->actual_len = 0;
-  if (!is_in_isr()) {
-    dcd_int_enable(rhport);
-    osal_mutex_unlock(dcd_mutex);
-  }
 
   // Control endpoint with zero-length packet and opposite direction to 1st request byte --> status stage
   bool const control_status = (epnum == 0 && total_bytes == 0 && dir != tu_edpt_dir(NRF_USBD->BMREQUESTTYPE));
@@ -496,13 +469,20 @@ bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
   }
   else if ( dir == TUSB_DIR_OUT )
   {
+    xfer->started = true;
     if ( epnum == 0 )
     {
       // Accept next Control Out packet. TASKS_EP0RCVOUT also require EasyDMA
       edpt_dma_start(&NRF_USBD->TASKS_EP0RCVOUT);
     }else
     {
-      if ( xfer->data_received && xfer->total_len > xfer->actual_len)
+      // started just set, it could start DMA transfer if interrupt was trigger after this line
+      // code only needs to start transfer (from Endpoint to RAM) when data_received was set
+      // before started was set. If started is NOT set but data_received is, it means that
+      // current transfer was already finished and next data is already present in endpoint and
+      // can be consumed by future transfer
+      __ISB(); __DSB();
+      if ( xfer->data_received && xfer->started )
       {
         // Data is already received previously
         // start DMA to copy to SRAM
@@ -785,7 +765,7 @@ void dcd_int_handler(uint8_t rhport)
     if ( tu_bit_test(int_status, USBD_INTEN_ENDEPOUT0_Pos+epnum))
     {
       xfer_td_t* xfer = get_td(epnum, TUSB_DIR_OUT);
-      uint8_t const xact_len = NRF_USBD->EPOUT[epnum].AMOUNT;
+      uint16_t const xact_len = NRF_USBD->EPOUT[epnum].AMOUNT;
 
       xfer->buffer     += xact_len;
       xfer->actual_len += xact_len;
@@ -804,7 +784,9 @@ void dcd_int_handler(uint8_t rhport)
         }
       }else
       {
+        TU_ASSERT(xfer->started,);
         xfer->total_len = xfer->actual_len;
+        xfer->started = false;
 
         // CBI OUT complete
         dcd_event_xfer_complete(0, epnum, xfer->actual_len, XFER_RESULT_SUCCESS, true);
@@ -857,7 +839,7 @@ void dcd_int_handler(uint8_t rhport)
       {
         xfer_td_t* xfer = get_td(epnum, TUSB_DIR_OUT);
 
-        if (xfer->actual_len < xfer->total_len)
+        if ( xfer->started && xfer->actual_len < xfer->total_len )
         {
           xact_out_dma(epnum);
         }else
diff --git a/src/portable/nuvoton/nuc505/dcd_nuc505.c b/src/portable/nuvoton/nuc505/dcd_nuc505.c
index 886720e33..3fa7c1ec1 100644
--- a/src/portable/nuvoton/nuc505/dcd_nuc505.c
+++ b/src/portable/nuvoton/nuc505/dcd_nuc505.c
@@ -181,7 +181,7 @@ static void dcd_userEP_in_xfer(struct xfer_ctl_t *xfer, USBD_EP_T *ep)
     ep->EPINTEN = USBD_EPINTEN_TXPKIEN_Msk;
   }
 
-  /* provided buffers are thankfully 32-bit aligned, allowing most data to be transfered as 32-bit */
+  /* provided buffers are thankfully 32-bit aligned, allowing most data to be transferred as 32-bit */
 #if 0 // TODO support dcd_edpt_xfer_fifo API
   if (xfer->ff)
   {
diff --git a/src/portable/nxp/lpc_ip3511/dcd_lpc_ip3511.c b/src/portable/nxp/lpc_ip3511/dcd_lpc_ip3511.c
index 09ebf3b0d..124656307 100644
--- a/src/portable/nxp/lpc_ip3511/dcd_lpc_ip3511.c
+++ b/src/portable/nxp/lpc_ip3511/dcd_lpc_ip3511.c
@@ -78,8 +78,10 @@ typedef struct {
 
 // Max nbytes for each control/bulk/interrupt transfer
 enum {
-  NBYTES_CBI_FULLSPEED_MAX = 64,
-  NBYTES_CBI_HIGHSPEED_MAX = 32767 // can be up to all 15-bit, but only tested with 4096
+  NBYTES_ISO_FS_MAX = 1023, // FS ISO
+  NBYTES_ISO_HS_MAX = 1024, // HS ISO
+  NBYTES_CBI_FS_MAX = 64, // FS control/bulk/interrupt
+  NBYTES_CBI_HS_MAX = 32767 // can be up to all 15-bit, but only tested with 4096
 };
 
 enum {
@@ -112,6 +114,8 @@ enum {
 typedef union TU_ATTR_PACKED
 {
   // Full and High speed has different bit layout for buffer_offset and nbytes
+  // TODO FS/HS layout depends on the max speed of controller e.g
+  // lpc55s69 PORT0 is only FS but actually has the same layout as HS on port1
 
   // Buffer (aligned 64) = DATABUFSTART [31:22]  | buffer_offset [21:6]
   volatile struct {
@@ -135,7 +139,7 @@ typedef union TU_ATTR_PACKED
     uint32_t stall        : 1 ;
     uint32_t disable      : 1 ;
     uint32_t active       : 1 ;
-  };
+  } cmd_sts;
 }ep_cmd_sts_t;
 
 TU_VERIFY_STATIC( sizeof(ep_cmd_sts_t) == 4, "size is not correct" );
@@ -175,6 +179,9 @@ typedef struct
 //    Use CFG_TUSB_MEM_SECTION to place it accordingly.
 CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(256) static dcd_data_t _dcd;
 
+// Dummy buffer to fix ZLPs overwriting the buffer (probably an USB/DMA controller bug)
+CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(64) static uint8_t dummy[8];
+
 //--------------------------------------------------------------------+
 // Multiple Controllers
 //--------------------------------------------------------------------+
@@ -225,8 +232,36 @@ static inline uint8_t ep_addr2id(uint8_t ep_addr)
 //--------------------------------------------------------------------+
 // CONTROLLER API
 //--------------------------------------------------------------------+
+
+static void prepare_setup_packet(uint8_t rhport)
+{
+  if (_dcd_controller[rhport].max_speed == TUSB_SPEED_FULL )
+  {
+    _dcd.ep[0][1].buffer_fs.offset = get_buf_offset(_dcd.setup_packet);
+  }else
+  {
+    _dcd.ep[0][1].buffer_hs.offset = get_buf_offset(_dcd.setup_packet);
+  }
+}
+
+static void edpt_reset(uint8_t rhport, uint8_t ep_id)
+{
+  (void) rhport;
+  tu_memclr(&_dcd.ep[ep_id], sizeof(_dcd.ep[ep_id]));
+}
+
+static void edpt_reset_all(uint8_t rhport)
+{
+  for (uint8_t ep_id = 0; ep_id < 2*_dcd_controller[rhport].ep_pairs; ++ep_id)
+  {
+    edpt_reset(rhport, ep_id);
+  }
+  prepare_setup_packet(rhport);
+}
 void dcd_init(uint8_t rhport)
 {
+  edpt_reset_all(rhport);
+
   dcd_registers_t* dcd_reg = _dcd_controller[rhport].regs;
 
   dcd_reg->EPLISTSTART  = (uint32_t) _dcd.ep;
@@ -294,7 +329,7 @@ void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
 
   // TODO cannot able to STALL Control OUT endpoint !!!!! FIXME try some walk-around
   uint8_t const ep_id = ep_addr2id(ep_addr);
-  _dcd.ep[ep_id][0].stall = 1;
+  _dcd.ep[ep_id][0].cmd_sts.stall = 1;
 }
 
 void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
@@ -303,26 +338,21 @@ void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
 
   uint8_t const ep_id = ep_addr2id(ep_addr);
 
-  _dcd.ep[ep_id][0].stall        = 0;
-  _dcd.ep[ep_id][0].toggle_reset = 1;
-  _dcd.ep[ep_id][0].toggle_mode  = 0;
+  _dcd.ep[ep_id][0].cmd_sts.stall        = 0;
+  _dcd.ep[ep_id][0].cmd_sts.toggle_reset = 1;
+  _dcd.ep[ep_id][0].cmd_sts.toggle_mode  = 0;
 }
 
 bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc)
 {
-  (void) rhport;
-
-  // TODO not support ISO yet
-  TU_VERIFY(p_endpoint_desc->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS);
-
   //------------- Prepare Queue Head -------------//
   uint8_t ep_id = ep_addr2id(p_endpoint_desc->bEndpointAddress);
 
   // Check if endpoint is available
-  TU_ASSERT( _dcd.ep[ep_id][0].disable && _dcd.ep[ep_id][1].disable );
+  TU_ASSERT( _dcd.ep[ep_id][0].cmd_sts.disable && _dcd.ep[ep_id][1].cmd_sts.disable );
 
-  tu_memclr(_dcd.ep[ep_id], 2*sizeof(ep_cmd_sts_t));
-  _dcd.ep[ep_id][0].is_iso = (p_endpoint_desc->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS);
+  edpt_reset(rhport, ep_id);
+  _dcd.ep[ep_id][0].cmd_sts.is_iso = (p_endpoint_desc->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS);
 
   // Enable EP interrupt
   dcd_registers_t* dcd_reg = _dcd_controller[rhport].regs;
@@ -333,19 +363,20 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc)
 
 void dcd_edpt_close_all (uint8_t rhport)
 {
-  (void) rhport;
-  // TODO implement dcd_edpt_close_all()
+  for (uint8_t ep_id = 0; ep_id < 2*_dcd_controller[rhport].ep_pairs; ++ep_id)
+  {
+    _dcd.ep[ep_id][0].cmd_sts.active = _dcd.ep[ep_id][0].cmd_sts.active = 0; // TODO proper way is to EPSKIP then wait ep[][].active then write ep[][].disable (see table 778 in LPC55S69 Use Manual)
+    _dcd.ep[ep_id][0].cmd_sts.disable = _dcd.ep[ep_id][1].cmd_sts.disable = 1;
+  }
 }
 
-static void prepare_setup_packet(uint8_t rhport)
+void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr)
 {
-  if (_dcd_controller[rhport].max_speed == TUSB_SPEED_FULL )
-  {
-    _dcd.ep[0][1].buffer_fs.offset = get_buf_offset(_dcd.setup_packet);;
-  }else
-  {
-    _dcd.ep[0][1].buffer_hs.offset = get_buf_offset(_dcd.setup_packet);;
-  }
+  (void) rhport;
+  
+  uint8_t ep_id = ep_addr2id(ep_addr);
+  _dcd.ep[ep_id][0].cmd_sts.active = _dcd.ep[ep_id][0].cmd_sts.active = 0; // TODO proper way is to EPSKIP then wait ep[][].active then write ep[][].disable (see table 778 in LPC55S69 Use Manual)
+  _dcd.ep[ep_id][0].cmd_sts.disable = _dcd.ep[ep_id][1].cmd_sts.disable = 1;
 }
 
 static void prepare_ep_xfer(uint8_t rhport, uint8_t ep_id, uint16_t buf_offset, uint16_t total_bytes)
@@ -354,31 +385,36 @@ static void prepare_ep_xfer(uint8_t rhport, uint8_t ep_id, uint16_t buf_offset,
 
   if (_dcd_controller[rhport].max_speed == TUSB_SPEED_FULL )
   {
-    // TODO ISO FullSpeed can have up to 1023 bytes
-    nbytes = tu_min16(total_bytes, NBYTES_CBI_FULLSPEED_MAX);
+    nbytes = tu_min16(total_bytes, _dcd.ep[ep_id][0].cmd_sts.is_iso ? NBYTES_ISO_FS_MAX : NBYTES_CBI_FS_MAX);
     _dcd.ep[ep_id][0].buffer_fs.offset = buf_offset;
     _dcd.ep[ep_id][0].buffer_fs.nbytes = nbytes;
   }else
   {
-    nbytes = tu_min16(total_bytes, NBYTES_CBI_HIGHSPEED_MAX);
+    nbytes = tu_min16(total_bytes, NBYTES_CBI_HS_MAX);
     _dcd.ep[ep_id][0].buffer_hs.offset = buf_offset;
     _dcd.ep[ep_id][0].buffer_hs.nbytes = nbytes;
   }
 
   _dcd.dma[ep_id].nbytes = nbytes;
 
-  _dcd.ep[ep_id][0].active = 1;
+  _dcd.ep[ep_id][0].cmd_sts.active = 1;
 }
 
 bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes)
 {
-  (void) rhport;
-
   uint8_t const ep_id = ep_addr2id(ep_addr);
 
   tu_memclr(&_dcd.dma[ep_id], sizeof(xfer_dma_t));
   _dcd.dma[ep_id].total_bytes = total_bytes;
 
+  if (!buffer)
+  {
+    // Although having no data, ZLPs can cause buffer overwritten to zeroes.
+    // Probably due to USB/DMA controller side effect/bug.
+    // Assigned buffer offset to (valid) dummy to prevent overwriting to DATABUFSTART
+    buffer = (uint8_t*)(uint32_t)dummy;
+  }
+
   prepare_ep_xfer(rhport, ep_id, get_buf_offset(buffer), total_bytes);
 
   return true;
@@ -390,15 +426,14 @@ bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t to
 static void bus_reset(uint8_t rhport)
 {
   tu_memclr(&_dcd, sizeof(dcd_data_t));
+  edpt_reset_all(rhport); 
 
-  // disable all non-control endpoints on bus reset
-  for(uint8_t ep_id = 2; ep_id < 2*MAX_EP_PAIRS; ep_id++)
+  // disable all endpoints as specified by LPC55S69 UM Table 778
+  for(uint8_t ep_id = 0; ep_id < 2*MAX_EP_PAIRS; ep_id++)
   {
-    _dcd.ep[ep_id][0].disable = _dcd.ep[ep_id][1].disable = 1;
+    _dcd.ep[ep_id][0].cmd_sts.disable = _dcd.ep[ep_id][1].cmd_sts.disable = 1;
   }
 
-  prepare_setup_packet(rhport);
-
   dcd_registers_t* dcd_reg = _dcd_controller[rhport].regs;
 
   dcd_reg->EPINUSE      = 0;
@@ -424,7 +459,7 @@ static void process_xfer_isr(uint8_t rhport, uint32_t int_status)
       if ( ep_id == 0 || ep_id == 1)
       {
         // For control endpoint, we need to manually clear Active bit
-        ep_cs->active = 0;
+        ep_cs->cmd_sts.active = 0;
       }
 
       uint16_t buf_offset;
@@ -506,31 +541,23 @@ void dcd_int_handler(uint8_t rhport)
       }
     }
 
-    // TODO support suspend & resume
     if (cmd_stat & CMDSTAT_SUSPEND_CHANGE_MASK)
     {
-      if (cmd_stat & CMDSTAT_DEVICE_SUSPEND_MASK)
-      { // suspend signal, bus idle for more than 3ms
-        // Note: Host may delay more than 3 ms before and/or after bus reset before doing enumeration.
-        if (cmd_stat & CMDSTAT_DEVICE_ADDR_MASK)
-        {
-          dcd_event_bus_signal(rhport, DCD_EVENT_SUSPEND, true);
-        }
+      // suspend signal, bus idle for more than 3ms
+      // Note: Host may delay more than 3 ms before and/or after bus reset before doing enumeration.
+      if (cmd_stat & CMDSTAT_DEVICE_ADDR_MASK)
+      {
+        dcd_event_bus_signal(rhport, (cmd_stat & CMDSTAT_DEVICE_SUSPEND_MASK) ? DCD_EVENT_SUSPEND : DCD_EVENT_RESUME, true);
       }
     }
-//        else
-//      { // resume signal
-//    dcd_event_bus_signal(rhport, DCD_EVENT_RESUME, true);
-//      }
-//    }
   }
 
   // Setup Receive
   if ( tu_bit_test(int_status, 0) && (cmd_stat & CMDSTAT_SETUP_RECEIVED_MASK) )
   {
     // Follow UM flowchart to clear Active & Stall on both Control IN/OUT endpoints
-    _dcd.ep[0][0].active = _dcd.ep[1][0].active = 0;
-    _dcd.ep[0][0].stall = _dcd.ep[1][0].stall = 0;
+    _dcd.ep[0][0].cmd_sts.active = _dcd.ep[1][0].cmd_sts.active = 0;
+    _dcd.ep[0][0].cmd_sts.stall = _dcd.ep[1][0].cmd_sts.stall = 0;
 
     dcd_reg->DEVCMDSTAT |= CMDSTAT_SETUP_RECEIVED_MASK;
 
diff --git a/src/portable/nxp/transdimension/dcd_transdimension.c b/src/portable/nxp/transdimension/dcd_transdimension.c
index 2347e1dae..1f27a6872 100644
--- a/src/portable/nxp/transdimension/dcd_transdimension.c
+++ b/src/portable/nxp/transdimension/dcd_transdimension.c
@@ -27,14 +27,14 @@
 #include "tusb_option.h"
 
 #if CFG_TUD_ENABLED && \
-    (CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_LPC43XX || CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX)
+    (CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_LPC43XX || CFG_TUSB_MCU == OPT_MCU_MIMXRT)
 
 #warning "transdimenion is renamed to chipidea (portable/chipidea/ci_hs) to match other opensource naming convention such as linux. This file will be removed in the future, please update your makefile accordingly"
 
 //--------------------------------------------------------------------+
 // INCLUDE
 //--------------------------------------------------------------------+
-#if CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX
+#if CFG_TUSB_MCU == OPT_MCU_MIMXRT
   #include "fsl_device_registers.h"
   #define INCLUDE_FSL_DEVICE_REGISTERS
 #else
@@ -153,7 +153,7 @@ typedef struct
   const uint8_t ep_count; // Max bi-directional Endpoints
 }dcd_controller_t;
 
-#if CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX
+#if CFG_TUSB_MCU == OPT_MCU_MIMXRT
   static const dcd_controller_t _dcd_controller[] =
   {
     // RT1010 and RT1020 only has 1 USB controller
diff --git a/src/portable/nxp/transdimension/hcd_transdimension.c b/src/portable/nxp/transdimension/hcd_transdimension.c
index 2d0830981..392764ff6 100644
--- a/src/portable/nxp/transdimension/hcd_transdimension.c
+++ b/src/portable/nxp/transdimension/hcd_transdimension.c
@@ -29,14 +29,14 @@
 // NXP Trans-Dimension USB IP implement EHCI for host functionality
 
 #if CFG_TUH_ENABLED && \
-    (CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_LPC43XX || CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX)
+    (CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_LPC43XX || CFG_TUSB_MCU == OPT_MCU_MIMXRT)
 
 #warning "transdimenion is renamed to chipidea (portable/chipidea/ci_hs) to match other opensource naming convention such as linux. This file will be removed in the future, please update your makefile accordingly"
 
 //--------------------------------------------------------------------+
 // INCLUDE
 //--------------------------------------------------------------------+
-#if CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX
+#if CFG_TUSB_MCU == OPT_MCU_MIMXRT
   #include "fsl_device_registers.h"
 #else
   // LPCOpen for 18xx & 43xx
@@ -58,7 +58,7 @@ typedef struct
   const IRQn_Type irqnum; // IRQ number
 }hcd_controller_t;
 
-#if CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX
+#if CFG_TUSB_MCU == OPT_MCU_MIMXRT
   static const hcd_controller_t _hcd_controller[] =
   {
     // RT1010 and RT1020 only has 1 USB controller
diff --git a/src/portable/ohci/ohci.c b/src/portable/ohci/ohci.c
index 3e523ebc2..b615743b8 100644
--- a/src/portable/ohci/ohci.c
+++ b/src/portable/ohci/ohci.c
@@ -28,6 +28,10 @@
 
 #if CFG_TUH_ENABLED && defined(TUP_USBIP_OHCI)
 
+#ifndef TUP_OHCI_RHPORTS
+#error  OHCI is enabled, but TUP_OHCI_RHPORTS is not defined.
+#endif
+
 //--------------------------------------------------------------------+
 // INCLUDE
 //--------------------------------------------------------------------+
@@ -157,6 +161,21 @@ static void ed_list_remove_by_addr(ohci_ed_t * p_head, uint8_t dev_addr);
 //--------------------------------------------------------------------+
 // USBH-HCD API
 //--------------------------------------------------------------------+
+
+// If your system requires separation of virtual and physical memory, implement
+// tusb_app_virt_to_phys and tusb_app_virt_to_phys in your application.
+TU_ATTR_ALWAYS_INLINE static inline void *_phys_addr(void *virtual_address)
+{
+  if (tusb_app_virt_to_phys) return tusb_app_virt_to_phys(virtual_address);
+  return virtual_address;
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void *_virt_addr(void *physical_address)
+{
+  if (tusb_app_phys_to_virt) return tusb_app_phys_to_virt(physical_address);
+  return physical_address;
+}
+
 // Initialization according to 5.1.1.4
 bool hcd_init(uint8_t rhport)
 {
@@ -165,22 +184,44 @@ bool hcd_init(uint8_t rhport)
   //------------- Data Structure init -------------//
   tu_memclr(&ohci_data, sizeof(ohci_data_t));
   for(uint8_t i=0; i<32; i++)
-  { // assign all interrupt pointes to period head ed
-    ohci_data.hcca.interrupt_table[i] = (uint32_t) &ohci_data.period_head_ed;
+  { // assign all interrupt pointers to period head ed
+    ohci_data.hcca.interrupt_table[i] = (uint32_t) _phys_addr(&ohci_data.period_head_ed);
   }
 
   ohci_data.control[0].ed.skip  = 1;
   ohci_data.bulk_head_ed.skip   = 1;
   ohci_data.period_head_ed.skip = 1;
 
+  //If OHCI hardware is in SMM mode, gain ownership (Ref OHCI spec 5.1.1.3.3)
+  if (OHCI_REG->control_bit.interrupt_routing == 1)
+  {
+    OHCI_REG->command_status_bit.ownership_change_request = 1;
+    while (OHCI_REG->control_bit.interrupt_routing == 1) {}
+  }
+
+  //If OHCI hardware has come from warm-boot, signal resume (Ref OHCI spec 5.1.1.3.4)
+  else if (OHCI_REG->control_bit.hc_functional_state != OHCI_CONTROL_FUNCSTATE_RESET &&
+           OHCI_REG->control_bit.hc_functional_state != OHCI_CONTROL_FUNCSTATE_OPERATIONAL)
+  {
+    //Wait 20 ms. (Ref Usb spec 7.1.7.7)
+    OHCI_REG->control_bit.hc_functional_state = OHCI_CONTROL_FUNCSTATE_RESUME;
+
+#if CFG_TUSB_OS != OPT_OS_NONE
+    // os_none implement task delay using usb frame counter which is not started yet
+    // therefore cause infinite delay.
+    // TODO find a way to delay in case of os none e.g __nop
+    osal_task_delay(20);
+#endif
+  }
+
   // reset controller
   OHCI_REG->command_status_bit.controller_reset = 1;
   while( OHCI_REG->command_status_bit.controller_reset ) {} // should not take longer than 10 us
 
   //------------- init ohci registers -------------//
-  OHCI_REG->control_head_ed = (uint32_t) &ohci_data.control[0].ed;
-  OHCI_REG->bulk_head_ed    = (uint32_t) &ohci_data.bulk_head_ed;
-  OHCI_REG->hcca            = (uint32_t) &ohci_data.hcca;
+  OHCI_REG->control_head_ed = (uint32_t) _phys_addr(&ohci_data.control[0].ed);
+  OHCI_REG->bulk_head_ed    = (uint32_t) _phys_addr(&ohci_data.bulk_head_ed);
+  OHCI_REG->hcca            = (uint32_t) _phys_addr(&ohci_data.hcca);
 
   OHCI_REG->interrupt_disable = OHCI_REG->interrupt_enable; // disable all interrupts
   OHCI_REG->interrupt_status  = OHCI_REG->interrupt_status; // clear current set bits
@@ -188,15 +229,21 @@ bool hcd_init(uint8_t rhport)
       OHCI_INT_UNRECOVERABLE_ERROR_MASK | OHCI_INT_FRAME_OVERFLOW_MASK | OHCI_INT_RHPORT_STATUS_CHANGE_MASK |
       OHCI_INT_MASTER_ENABLE_MASK;
 
-  OHCI_REG->control |= OHCI_CONTROL_CONTROL_BULK_RATIO | OHCI_CONTROL_LIST_CONTROL_ENABLE_MASK |
+  OHCI_REG->control = OHCI_CONTROL_CONTROL_BULK_RATIO | OHCI_CONTROL_LIST_CONTROL_ENABLE_MASK |
        OHCI_CONTROL_LIST_BULK_ENABLE_MASK | OHCI_CONTROL_LIST_PERIODIC_ENABLE_MASK; // TODO Isochronous
 
   OHCI_REG->frame_interval = (OHCI_FMINTERVAL_FSMPS << 16) | OHCI_FMINTERVAL_FI;
+  OHCI_REG->frame_interval ^= (1 << 31); //Must toggle when frame_interval is updated.
   OHCI_REG->periodic_start = (OHCI_FMINTERVAL_FI * 9) / 10; // Periodic start is 90% of frame interval
 
   OHCI_REG->control_bit.hc_functional_state = OHCI_CONTROL_FUNCSTATE_OPERATIONAL; // make HC's state to operational state TODO use this to suspend (save power)
   OHCI_REG->rh_status_bit.local_power_status_change = 1; // set global power for ports
 
+#if CFG_TUSB_OS != OPT_OS_NONE
+  // TODO as above delay
+  osal_task_delay(OHCI_REG->rh_descriptorA_bit.power_on_to_good_time * 2); // Wait POTG after power up
+#endif
+
   return true;
 }
 
@@ -212,8 +259,7 @@ uint32_t hcd_frame_number(uint8_t rhport)
 //--------------------------------------------------------------------+
 void hcd_port_reset(uint8_t hostid)
 {
-  (void) hostid;
-  OHCI_REG->rhport_status[0] = RHPORT_PORT_RESET_STATUS_MASK;
+  OHCI_REG->rhport_status[hostid] = RHPORT_PORT_RESET_STATUS_MASK;
 }
 
 void hcd_port_reset_end(uint8_t rhport)
@@ -223,14 +269,12 @@ void hcd_port_reset_end(uint8_t rhport)
 
 bool hcd_port_connect_status(uint8_t hostid)
 {
-  (void) hostid;
-  return OHCI_REG->rhport_status_bit[0].current_connect_status;
+  return OHCI_REG->rhport_status_bit[hostid].current_connect_status;
 }
 
 tusb_speed_t hcd_port_speed_get(uint8_t hostid)
 {
-  (void) hostid;
-  return OHCI_REG->rhport_status_bit[0].low_speed_device_attached ? TUSB_SPEED_LOW : TUSB_SPEED_FULL;
+  return OHCI_REG->rhport_status_bit[hostid].low_speed_device_attached ? TUSB_SPEED_LOW : TUSB_SPEED_FULL;
 }
 
 // endpoints are tied to an address, which only reclaim after a long delay when enumerating
@@ -308,8 +352,8 @@ static void gtd_init(ohci_gtd_t* p_td, uint8_t* data_ptr, uint16_t total_bytes)
   p_td->delay_interrupt        = OHCI_INT_ON_COMPLETE_NO;
   p_td->condition_code         = OHCI_CCODE_NOT_ACCESSED;
 
-  p_td->current_buffer_pointer = data_ptr;
-  p_td->buffer_end             = total_bytes ? (data_ptr + total_bytes-1) : data_ptr;
+  p_td->current_buffer_pointer = _phys_addr(data_ptr);
+  p_td->buffer_end             = total_bytes ? (_phys_addr(data_ptr + total_bytes - 1)) : (uint8_t *)p_td->current_buffer_pointer;
 }
 
 static ohci_ed_t * ed_from_addr(uint8_t dev_addr, uint8_t ep_addr)
@@ -345,7 +389,7 @@ static ohci_ed_t * ed_find_free(void)
 static void ed_list_insert(ohci_ed_t * p_pre, ohci_ed_t * p_ed)
 {
   p_ed->next = p_pre->next;
-  p_pre->next = (uint32_t) p_ed;
+  p_pre->next = (uint32_t) _phys_addr(p_ed);
 }
 
 static void ed_list_remove_by_addr(ohci_ed_t * p_head, uint8_t dev_addr)
@@ -354,20 +398,24 @@ static void ed_list_remove_by_addr(ohci_ed_t * p_head, uint8_t dev_addr)
 
   while( p_prev->next )
   {
-    ohci_ed_t* ed = (ohci_ed_t*) p_prev->next;
+    ohci_ed_t* ed = (ohci_ed_t*) _virt_addr((void *)p_prev->next);
 
     if (ed->dev_addr == dev_addr)
     {
-      // unlink ed
+      // Prevent Host Controller from processing this ED while we remove it
+      ed->skip = 1;
+
+      // unlink ed, will also move up p_prev
       p_prev->next = ed->next;
 
       // point the removed ED's next pointer to list head to make sure HC can always safely move away from this ED
-      ed->next = (uint32_t) p_head;
+      ed->next = (uint32_t) _phys_addr(p_head);
       ed->used = 0;
+      ed->skip = 0;
+    }else
+    {
+      p_prev = (ohci_ed_t*) _virt_addr((void *)p_prev->next);
     }
-
-    // check next valid since we could remove it
-    if (p_prev->next) p_prev = (ohci_ed_t*) p_prev->next;
   }
 }
 
@@ -386,11 +434,11 @@ static void td_insert_to_ed(ohci_ed_t* p_ed, ohci_gtd_t * p_gtd)
   // tail is always NULL
   if ( tu_align16(p_ed->td_head.address) == 0 )
   { // TD queue is empty --> head = TD
-    p_ed->td_head.address |= (uint32_t) p_gtd;
+    p_ed->td_head.address |= (uint32_t) _phys_addr(p_gtd);
   }
   else
   { // TODO currently only support queue up to 2 TD each endpoint at a time
-    ((ohci_gtd_t*) tu_align16(p_ed->td_head.address))->next = (uint32_t) p_gtd;
+    ((ohci_gtd_t*) tu_align16((uint32_t)_virt_addr((void *)p_ed->td_head.address)))->next = (uint32_t) _phys_addr(p_gtd);
   }
 }
 
@@ -443,10 +491,10 @@ bool hcd_setup_send(uint8_t rhport, uint8_t dev_addr, uint8_t const setup_packet
   qtd->index           = dev_addr;
   qtd->pid             = PID_SETUP;
   qtd->data_toggle     = GTD_DT_DATA0;
-  qtd->delay_interrupt = 0;
+  qtd->delay_interrupt = OHCI_INT_ON_COMPLETE_YES;
 
   //------------- Attach TDs list to Control Endpoint -------------//
-  ed->td_head.address = (uint32_t) qtd;
+  ed->td_head.address = (uint32_t) _phys_addr(qtd);
 
   OHCI_REG->command_status_bit.control_list_filled = 1;
 
@@ -470,9 +518,9 @@ bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t *
     gtd->index           = dev_addr;
     gtd->pid             = dir ? PID_IN : PID_OUT;
     gtd->data_toggle     = GTD_DT_DATA1; // Both Data and Ack stage start with DATA1
-    gtd->delay_interrupt = 0;
+    gtd->delay_interrupt = OHCI_INT_ON_COMPLETE_YES;
 
-    ed->td_head.address = (uint32_t) gtd;
+    ed->td_head.address = (uint32_t) _phys_addr(gtd);
 
     OHCI_REG->command_status_bit.control_list_filled = 1;
   }else
@@ -484,7 +532,7 @@ bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t *
 
     gtd_init(gtd, buffer, buflen);
     gtd->index = ed-ohci_data.ed_pool;
-    gtd->delay_interrupt = 0;
+    gtd->delay_interrupt = OHCI_INT_ON_COMPLETE_YES;
 
     td_insert_to_ed(ed, gtd);
 
@@ -520,16 +568,17 @@ static ohci_td_item_t* list_reverse(ohci_td_item_t* td_head)
 
   while(td_head != NULL)
   {
+    td_head = _virt_addr(td_head);
     uint32_t next = td_head->next;
 
     // make current's item become reverse's first item
     td_head->next = (uint32_t) td_reverse_head;
-    td_reverse_head  = td_head;
+    td_reverse_head  = _phys_addr(td_head);
 
     td_head = (ohci_td_item_t*) next; // advance to next item
   }
 
-  return td_reverse_head;
+  return _virt_addr(td_reverse_head);
 }
 
 static inline bool gtd_is_control(ohci_gtd_t const * const p_qtd)
@@ -565,6 +614,7 @@ static void done_queue_isr(uint8_t hostid)
 
   // done head is written in reversed order of completion --> need to reverse the done queue first
   ohci_td_item_t* td_head = list_reverse ( (ohci_td_item_t*) tu_align16(ohci_data.hcca.done_head) );
+  ohci_data.hcca.done_head = 0;
 
   while( td_head != NULL )
   {
@@ -600,7 +650,7 @@ static void done_queue_isr(uint8_t hostid)
       hcd_event_xfer_complete(ed->dev_addr, tu_edpt_addr(ed->ep_number, dir), xferred_bytes, event, true);
     }
 
-    td_head = (ohci_td_item_t*) td_head->next;
+    td_head = (ohci_td_item_t*) _virt_addr((void *)td_head->next);
   }
 }
 
@@ -611,6 +661,9 @@ void hcd_int_handler(uint8_t hostid)
 
   if (int_status == 0) return;
 
+  // Disable MIE as per OHCI spec 5.3
+  OHCI_REG->interrupt_disable = OHCI_INT_MASTER_ENABLE_MASK;
+
   // Frame number overflow
   if ( int_status & OHCI_INT_FRAME_OVERFLOW_MASK )
   {
@@ -620,29 +673,30 @@ void hcd_int_handler(uint8_t hostid)
   //------------- RootHub status -------------//
   if ( int_status & OHCI_INT_RHPORT_STATUS_CHANGE_MASK )
   {
-    uint32_t const rhport_status = OHCI_REG->rhport_status[0] & RHPORT_ALL_CHANGE_MASK;
-
-    // TODO dual port is not yet supported
-    if ( rhport_status & RHPORT_CONNECT_STATUS_CHANGE_MASK )
+    for (int i = 0; i < TUP_OHCI_RHPORTS; i++)
     {
-      // TODO check if remote wake-up
-      if ( OHCI_REG->rhport_status_bit[0].current_connect_status )
+      uint32_t const rhport_status = OHCI_REG->rhport_status[i] & RHPORT_ALL_CHANGE_MASK;
+      if ( rhport_status & RHPORT_CONNECT_STATUS_CHANGE_MASK )
       {
-        // TODO reset port immediately, without this controller will got 2-3 (debouncing connection status change)
-        OHCI_REG->rhport_status[0] = RHPORT_PORT_RESET_STATUS_MASK;
-        hcd_event_device_attach(hostid, true);
-      }else
-      {
-        hcd_event_device_remove(hostid, true);
+        // TODO check if remote wake-up
+        if ( OHCI_REG->rhport_status_bit[i].current_connect_status )
+        {
+          // TODO reset port immediately, without this controller will got 2-3 (debouncing connection status change)
+          OHCI_REG->rhport_status[i] = RHPORT_PORT_RESET_STATUS_MASK;
+          hcd_event_device_attach(i, true);
+        }else
+        {
+          hcd_event_device_remove(i, true);
+        }
       }
-    }
 
-    if ( rhport_status & RHPORT_PORT_SUSPEND_CHANGE_MASK)
-    {
+      if ( rhport_status & RHPORT_PORT_SUSPEND_CHANGE_MASK)
+      {
 
-    }
+      }
 
-    OHCI_REG->rhport_status[0] = rhport_status; // acknowledge all interrupt
+      OHCI_REG->rhport_status[i] = rhport_status; // acknowledge all interrupt
+    }
   }
 
   //------------- Transfer Complete -------------//
@@ -652,6 +706,8 @@ void hcd_int_handler(uint8_t hostid)
   }
 
   OHCI_REG->interrupt_status = int_status; // Acknowledge handled interrupt
+
+  OHCI_REG->interrupt_enable = OHCI_INT_MASTER_ENABLE_MASK; // Enable MIE
 }
 //--------------------------------------------------------------------+
 // HELPER
diff --git a/src/portable/ohci/ohci.h b/src/portable/ohci/ohci.h
index 9fc954c8f..2081ffabb 100644
--- a/src/portable/ohci/ohci.h
+++ b/src/portable/ohci/ohci.h
@@ -34,7 +34,7 @@
 //--------------------------------------------------------------------+
 // OHCI CONFIGURATION & CONSTANTS
 //--------------------------------------------------------------------+
-#define HOST_HCD_XFER_INTERRUPT // TODO interrupt is used widely, should always be enalbed
+#define HOST_HCD_XFER_INTERRUPT // TODO interrupt is used widely, should always be enabled
 #define OHCI_PERIODIC_LIST (defined HOST_HCD_XFER_INTERRUPT || defined HOST_HCD_XFER_ISOCHRONOUS)
 
 // TODO merge OHCI with EHCI
@@ -58,7 +58,9 @@ typedef struct {
 
 TU_VERIFY_STATIC( sizeof(ohci_hcca_t) == 256, "size is not correct" );
 
-typedef struct {
+// common link item for gtd and itd for list travel
+// use as pointer only
+typedef struct TU_ATTR_ALIGNED(16) {
   uint32_t reserved[2];
   volatile uint32_t next;
   uint32_t reserved2;
@@ -230,8 +232,27 @@ typedef volatile struct
   uint32_t periodic_start;
   uint32_t lowspeed_threshold;
 
-  uint32_t rh_descriptorA;
-  uint32_t rh_descriptorB;
+  union {
+    uint32_t rh_descriptorA;
+    struct {
+      uint32_t number_downstream_ports     : 8;
+      uint32_t power_switching_mode        : 1;
+      uint32_t no_power_switching          : 1;
+      uint32_t device_type                 : 1;
+      uint32_t overcurrent_protection_mode : 1;
+      uint32_t no_over_current_protection  : 1;
+      uint32_t reserved                    : 11;
+      uint32_t power_on_to_good_time       : 8;
+    } rh_descriptorA_bit;
+  };
+
+  union {
+    uint32_t rh_descriptorB;
+    struct {
+      uint32_t device_removable        : 16;
+      uint32_t port_power_control_mask : 16;
+    } rh_descriptorB_bit;
+  };
 
   union {
     uint32_t rh_status;
@@ -248,7 +269,7 @@ typedef volatile struct
   };
 
   union {
-    uint32_t rhport_status[2]; // TODO NXP OHCI controller only has 2 ports
+    uint32_t rhport_status[TUP_OHCI_RHPORTS];
     struct {
       uint32_t current_connect_status             : 1;
       uint32_t port_enable_status                 : 1;
@@ -265,11 +286,11 @@ typedef volatile struct
       uint32_t port_over_current_indicator_change : 1;
       uint32_t port_reset_status_change           : 1;
       uint32_t TU_RESERVED                        : 11;
-    }rhport_status_bit[2];
+    }rhport_status_bit[TUP_OHCI_RHPORTS];
   };
 }ohci_registers_t;
 
-TU_VERIFY_STATIC( sizeof(ohci_registers_t) == 0x5c, "size is not correct");
+TU_VERIFY_STATIC( sizeof(ohci_registers_t) == (0x54 + (4 * TUP_OHCI_RHPORTS)), "size is not correct");
 
 #ifdef __cplusplus
  }
diff --git a/src/portable/raspberrypi/pio_usb/hcd_pio_usb.c b/src/portable/raspberrypi/pio_usb/hcd_pio_usb.c
index 1b2d29331..58b153ac3 100644
--- a/src/portable/raspberrypi/pio_usb/hcd_pio_usb.c
+++ b/src/portable/raspberrypi/pio_usb/hcd_pio_usb.c
@@ -43,17 +43,25 @@
 #define RHPORT_OFFSET     1
 #define RHPORT_PIO(_x)    ((_x)-RHPORT_OFFSET)
 
-static pio_usb_configuration_t pio_host_config = PIO_USB_DEFAULT_CONFIG;
+static pio_usb_configuration_t pio_host_cfg = PIO_USB_DEFAULT_CONFIG;
 
 //--------------------------------------------------------------------+
 // HCD API
 //--------------------------------------------------------------------+
+bool hcd_configure(uint8_t rhport, uint32_t cfg_id, const void* cfg_param)
+{
+  (void) rhport;
+  TU_VERIFY(cfg_id == TUH_CFGID_RPI_PIO_USB_CONFIGURATION);
+  memcpy(&pio_host_cfg, cfg_param, sizeof(pio_usb_configuration_t));
+  return true;
+}
+
 bool hcd_init(uint8_t rhport)
 {
   (void) rhport;
 
   // To run USB SOF interrupt in core1, call this init in core1
-  pio_usb_host_init(&pio_host_config);
+  pio_usb_host_init(&pio_host_cfg);
 
   return true;
 }
diff --git a/src/portable/raspberrypi/rp2040/dcd_rp2040.c b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
index 48793b0d0..8df7bef2b 100644
--- a/src/portable/raspberrypi/rp2040/dcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
@@ -46,16 +46,16 @@
 /* Low level controller
  *------------------------------------------------------------------*/
 
-#define usb_hw_set hw_set_alias(usb_hw)
-#define usb_hw_clear hw_clear_alias(usb_hw)
-
 // Init these in dcd_init
 static uint8_t *next_buffer_ptr;
 
 // USB_MAX_ENDPOINTS Endpoints, direction TUSB_DIR_OUT for out and TUSB_DIR_IN for in.
 static struct hw_endpoint hw_endpoints[USB_MAX_ENDPOINTS][2];
 
-static inline struct hw_endpoint *hw_endpoint_get_by_num(uint8_t num, tusb_dir_t dir)
+// SOF may be used by remote wakeup as RESUME, this indicate whether SOF is actually used by usbd
+static bool _sof_enable = false;
+
+TU_ATTR_ALWAYS_INLINE static inline struct hw_endpoint *hw_endpoint_get_by_num(uint8_t num, tusb_dir_t dir)
 {
   return &hw_endpoints[num][dir];
 }
@@ -85,7 +85,7 @@ static void _hw_endpoint_alloc(struct hw_endpoint *ep, uint8_t transfer_type)
   uint dpram_offset = hw_data_offset(ep->hw_data_buf);
   hard_assert(hw_data_offset(next_buffer_ptr) <= USB_DPRAM_MAX);
 
-  pico_info("  Alloced %d bytes at offset 0x%x (0x%p)\r\n", size, dpram_offset, ep->hw_data_buf);
+  pico_info("  Allocated %d bytes at offset 0x%x (0x%p)\r\n", size, dpram_offset, ep->hw_data_buf);
 
   // Fill in endpoint control register with buffer offset
   uint32_t const reg = EP_CTRL_ENABLE_BITS | ((uint)transfer_type << EP_CTRL_BUFFER_TYPE_LSB) | dpram_offset;
@@ -185,7 +185,7 @@ static void hw_endpoint_xfer(uint8_t ep_addr, uint8_t *buffer, uint16_t total_by
     hw_endpoint_xfer_start(ep, buffer, total_bytes);
 }
 
-static void hw_handle_buff_status(void)
+static void __tusb_irq_path_func(hw_handle_buff_status)(void)
 {
     uint32_t remaining_buffers = usb_hw->buf_status;
     pico_trace("buf_status = 0x%08x\n", remaining_buffers);
@@ -214,7 +214,7 @@ static void hw_handle_buff_status(void)
     }
 }
 
-static void reset_ep0_pid(void)
+TU_ATTR_ALWAYS_INLINE static inline void reset_ep0_pid(void)
 {
     // If we have finished this transfer on EP0 set pid back to 1 for next
     // setup transfer. Also clear a stall in case
@@ -226,7 +226,7 @@ static void reset_ep0_pid(void)
     }
 }
 
-static void reset_non_control_endpoints(void)
+static void __tusb_irq_path_func(reset_non_control_endpoints)(void)
 {
   // Disable all non-control
   for ( uint8_t i = 0; i < USB_MAX_ENDPOINTS-1; i++ )
@@ -242,102 +242,135 @@ static void reset_non_control_endpoints(void)
   next_buffer_ptr = &usb_dpram->epx_data[0];
 }
 
-static void dcd_rp2040_irq(void)
+static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
 {
-    uint32_t const status = usb_hw->ints;
-    uint32_t handled = 0;
+  uint32_t const status = usb_hw->ints;
+  uint32_t handled = 0;
 
-    if (status & USB_INTF_DEV_SOF_BITS)
-    {
-      handled |= USB_INTF_DEV_SOF_BITS;
-      dcd_event_sof(0, usb_hw->sof_rd & USB_SOF_RD_BITS, true);
-    }
+  if ( status & USB_INTF_DEV_SOF_BITS )
+  {
+    bool keep_sof_alive = false;
 
-    // xfer events are handled before setup req. So if a transfer completes immediately
-    // before closing the EP, the events will be delivered in same order.
-    if (status & USB_INTS_BUFF_STATUS_BITS)
-    {
-        handled |= USB_INTS_BUFF_STATUS_BITS;
-        hw_handle_buff_status();
-    }
+    handled |= USB_INTF_DEV_SOF_BITS;
 
-    if (status & USB_INTS_SETUP_REQ_BITS)
+#if TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
+    // Errata 15 workaround for Device Bulk-In endpoint
+    e15_last_sof = time_us_32();
+
+    for ( uint8_t i = 0; i < USB_MAX_ENDPOINTS; i++ )
     {
-        handled |= USB_INTS_SETUP_REQ_BITS;
-        uint8_t const *setup = (uint8_t const *)&usb_dpram->setup_packet;
+      struct hw_endpoint * ep = hw_endpoint_get_by_num(i, TUSB_DIR_IN);
 
-        // reset pid to both 1 (data and ack)
-        reset_ep0_pid();
+      // Active Bulk IN endpoint requires SOF
+      if ( (ep->transfer_type == TUSB_XFER_BULK) && ep->active )
+      {
+        keep_sof_alive = true;
 
-        // Pass setup packet to tiny usb
-        dcd_event_setup_received(0, setup, true);
-        usb_hw_clear->sie_status = USB_SIE_STATUS_SETUP_REC_BITS;
-    }
+        hw_endpoint_lock_update(ep, 1);
 
-#if FORCE_VBUS_DETECT == 0
-    // Since we force VBUS detect On, device will always think it is connected and
-    // couldn't distinguish between disconnect and suspend
-    if (status & USB_INTS_DEV_CONN_DIS_BITS)
-    {
-        handled |= USB_INTS_DEV_CONN_DIS_BITS;
-
-        if ( usb_hw->sie_status & USB_SIE_STATUS_CONNECTED_BITS )
-        {
-          // Connected: nothing to do
-        }else
+        // Deferred enable?
+        if ( ep->pending )
         {
-          // Disconnected
-          dcd_event_bus_signal(0, DCD_EVENT_UNPLUGGED, true);
+          ep->pending = 0;
+          hw_endpoint_start_next_buffer(ep);
         }
 
-        usb_hw_clear->sie_status = USB_SIE_STATUS_CONNECTED_BITS;
+        hw_endpoint_lock_update(ep, -1);
+      }
     }
 #endif
 
-    // SE0 for 2.5 us or more (will last at least 10ms)
-    if (status & USB_INTS_BUS_RESET_BITS)
+    // disable SOF interrupt if it is used for RESUME in remote wakeup
+    if ( !keep_sof_alive && !_sof_enable ) usb_hw_clear->inte = USB_INTS_DEV_SOF_BITS;
+
+    dcd_event_sof(0, usb_hw->sof_rd & USB_SOF_RD_BITS, true);
+  }
+
+  // xfer events are handled before setup req. So if a transfer completes immediately
+  // before closing the EP, the events will be delivered in same order.
+  if ( status & USB_INTS_BUFF_STATUS_BITS )
+  {
+    handled |= USB_INTS_BUFF_STATUS_BITS;
+    hw_handle_buff_status();
+  }
+
+  if ( status & USB_INTS_SETUP_REQ_BITS )
+  {
+    handled |= USB_INTS_SETUP_REQ_BITS;
+    uint8_t const * setup = remove_volatile_cast(uint8_t const*, &usb_dpram->setup_packet);
+
+    // reset pid to both 1 (data and ack)
+    reset_ep0_pid();
+
+    // Pass setup packet to tiny usb
+    dcd_event_setup_received(0, setup, true);
+    usb_hw_clear->sie_status = USB_SIE_STATUS_SETUP_REC_BITS;
+  }
+
+#if FORCE_VBUS_DETECT == 0
+  // Since we force VBUS detect On, device will always think it is connected and
+  // couldn't distinguish between disconnect and suspend
+  if (status & USB_INTS_DEV_CONN_DIS_BITS)
+  {
+    handled |= USB_INTS_DEV_CONN_DIS_BITS;
+
+    if ( usb_hw->sie_status & USB_SIE_STATUS_CONNECTED_BITS )
     {
-        pico_trace("BUS RESET\n");
+      // Connected: nothing to do
+    }else
+    {
+      // Disconnected
+      dcd_event_bus_signal(0, DCD_EVENT_UNPLUGGED, true);
+    }
+
+    usb_hw_clear->sie_status = USB_SIE_STATUS_CONNECTED_BITS;
+  }
+#endif
+
+  // SE0 for 2.5 us or more (will last at least 10ms)
+  if ( status & USB_INTS_BUS_RESET_BITS )
+  {
+    pico_trace("BUS RESET\n");
 
-        handled |= USB_INTS_BUS_RESET_BITS;
+    handled |= USB_INTS_BUS_RESET_BITS;
 
-        usb_hw->dev_addr_ctrl = 0;
-        reset_non_control_endpoints();
-        dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
-        usb_hw_clear->sie_status = USB_SIE_STATUS_BUS_RESET_BITS;
+    usb_hw->dev_addr_ctrl = 0;
+    reset_non_control_endpoints();
+    dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
+    usb_hw_clear->sie_status = USB_SIE_STATUS_BUS_RESET_BITS;
 
 #if TUD_OPT_RP2040_USB_DEVICE_ENUMERATION_FIX
-        // Only run enumeration walk-around if pull up is enabled
-        if ( usb_hw->sie_ctrl & USB_SIE_CTRL_PULLUP_EN_BITS ) rp2040_usb_device_enumeration_fix();
+    // Only run enumeration workaround if pull up is enabled
+    if ( usb_hw->sie_ctrl & USB_SIE_CTRL_PULLUP_EN_BITS ) rp2040_usb_device_enumeration_fix();
 #endif
-    }
+  }
 
-    /* Note from pico datasheet 4.1.2.6.4 (v1.2)
-     * If you enable the suspend interrupt, it is likely you will see a suspend interrupt when
-     * the device is first connected but the bus is idle. The bus can be idle for a few ms before
-     * the host begins sending start of frame packets. You will also see a suspend interrupt
-     * when the device is disconnected if you do not have a VBUS detect circuit connected. This is
-     * because without VBUS detection, it is impossible to tell the difference between
-     * being disconnected and suspended.
-     */
-    if (status & USB_INTS_DEV_SUSPEND_BITS)
-    {
-        handled |= USB_INTS_DEV_SUSPEND_BITS;
-        dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
-        usb_hw_clear->sie_status = USB_SIE_STATUS_SUSPENDED_BITS;
-    }
+  /* Note from pico datasheet 4.1.2.6.4 (v1.2)
+   * If you enable the suspend interrupt, it is likely you will see a suspend interrupt when
+   * the device is first connected but the bus is idle. The bus can be idle for a few ms before
+   * the host begins sending start of frame packets. You will also see a suspend interrupt
+   * when the device is disconnected if you do not have a VBUS detect circuit connected. This is
+   * because without VBUS detection, it is impossible to tell the difference between
+   * being disconnected and suspended.
+   */
+  if ( status & USB_INTS_DEV_SUSPEND_BITS )
+  {
+    handled |= USB_INTS_DEV_SUSPEND_BITS;
+    dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
+    usb_hw_clear->sie_status = USB_SIE_STATUS_SUSPENDED_BITS;
+  }
 
-    if (status & USB_INTS_DEV_RESUME_FROM_HOST_BITS)
-    {
-        handled |= USB_INTS_DEV_RESUME_FROM_HOST_BITS;
-        dcd_event_bus_signal(0, DCD_EVENT_RESUME, true);
-        usb_hw_clear->sie_status = USB_SIE_STATUS_RESUME_BITS;
-    }
+  if ( status & USB_INTS_DEV_RESUME_FROM_HOST_BITS )
+  {
+    handled |= USB_INTS_DEV_RESUME_FROM_HOST_BITS;
+    dcd_event_bus_signal(0, DCD_EVENT_RESUME, true);
+    usb_hw_clear->sie_status = USB_SIE_STATUS_RESUME_BITS;
+  }
 
-    if (status ^ handled)
-    {
-        panic("Unhandled IRQ 0x%x\n", (uint) (status ^ handled));
-    }
+  if ( status ^ handled )
+  {
+    panic("Unhandled IRQ 0x%x\n", (uint) (status ^ handled));
+  }
 }
 
 #define USB_INTS_ERROR_BITS ( \
@@ -363,7 +396,7 @@ void dcd_init (uint8_t rhport)
   usb_hw->pwr = USB_USB_PWR_VBUS_DETECT_BITS | USB_USB_PWR_VBUS_DETECT_OVERRIDE_EN_BITS;
 #endif
 
-  irq_set_exclusive_handler(USBCTRL_IRQ, dcd_rp2040_irq);
+  irq_add_shared_handler(USBCTRL_IRQ, dcd_rp2040_irq, PICO_SHARED_IRQ_HANDLER_HIGHEST_ORDER_PRIORITY);
 
   // Init control endpoints
   tu_memclr(hw_endpoints[0], 2*sizeof(hw_endpoint_t));
@@ -411,9 +444,13 @@ void dcd_set_address (__unused uint8_t rhport, __unused uint8_t dev_addr)
 
 void dcd_remote_wakeup(__unused uint8_t rhport)
 {
-    pico_info("dcd_remote_wakeup %d\n", rhport);
-    assert(rhport == 0);
-    usb_hw_set->sie_ctrl = USB_SIE_CTRL_RESUME_BITS;
+  pico_info("dcd_remote_wakeup %d\n", rhport);
+  assert(rhport == 0);
+
+  // since RESUME interrupt is not triggered if we are the one initiate
+  // briefly enable SOF to notify usbd when bus is ready
+  usb_hw_set->inte = USB_INTS_DEV_SOF_BITS;
+  usb_hw_set->sie_ctrl = USB_SIE_CTRL_RESUME_BITS;
 }
 
 // disconnect by disabling internal pull-up resistor on D+/D-
@@ -434,17 +471,19 @@ void dcd_sof_enable(uint8_t rhport, bool en)
 {
   (void) rhport;
 
-  uint32_t inte = usb_hw->inte;
+  _sof_enable = en;
 
   if (en)
   {
-    inte |= USB_INTS_DEV_SOF_BITS;
+    usb_hw_set->inte = USB_INTS_DEV_SOF_BITS;
   }else
   {
-    inte &= ~USB_INTS_DEV_SOF_BITS;
+    // Don't clear immediately if the SOF workaround is in use.
+    // The SOF handler will conditionally disable the interrupt.
+#if !TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
+    usb_hw_clear->inte = USB_INTS_DEV_SOF_BITS;
+#endif
   }
-
-  usb_hw->inte = inte;
 }
 
 /*------------------------------------------------------------------*/
@@ -524,7 +563,7 @@ void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr)
     hw_endpoint_close(ep_addr);
 }
 
-void dcd_int_handler(uint8_t rhport)
+void __tusb_irq_path_func(dcd_int_handler)(uint8_t rhport)
 {
   (void) rhport;
   dcd_rp2040_irq();
diff --git a/src/portable/raspberrypi/rp2040/hcd_rp2040.c b/src/portable/raspberrypi/rp2040/hcd_rp2040.c
index 54c27ffe3..28abd7939 100644
--- a/src/portable/raspberrypi/rp2040/hcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/hcd_rp2040.c
@@ -56,9 +56,6 @@ static_assert(PICO_USB_HOST_INTERRUPT_ENDPOINTS <= USB_MAX_ENDPOINTS, "");
 static struct hw_endpoint ep_pool[1 + PICO_USB_HOST_INTERRUPT_ENDPOINTS];
 #define epx (ep_pool[0])
 
-#define usb_hw_set   hw_set_alias(usb_hw)
-#define usb_hw_clear hw_clear_alias(usb_hw)
-
 // Flags we set by default in sie_ctrl (we add other bits on top)
 enum {
   SIE_CTRL_BASE = USB_SIE_CTRL_SOF_EN_BITS      | USB_SIE_CTRL_KEEP_ALIVE_EN_BITS |
@@ -79,93 +76,103 @@ static struct hw_endpoint *get_dev_ep(uint8_t dev_addr, uint8_t ep_addr)
   return NULL;
 }
 
-static inline uint8_t dev_speed(void)
+TU_ATTR_ALWAYS_INLINE static inline uint8_t dev_speed(void)
 {
-    return (usb_hw->sie_status & USB_SIE_STATUS_SPEED_BITS) >> USB_SIE_STATUS_SPEED_LSB;
+  return (usb_hw->sie_status & USB_SIE_STATUS_SPEED_BITS) >> USB_SIE_STATUS_SPEED_LSB;
 }
 
-static bool need_pre(uint8_t dev_addr)
+TU_ATTR_ALWAYS_INLINE static inline bool need_pre(uint8_t dev_addr)
 {
-    // If this device is different to the speed of the root device
-    // (i.e. is a low speed device on a full speed hub) then need pre
-    return hcd_port_speed_get(0) != tuh_speed_get(dev_addr);
+  // If this device is different to the speed of the root device
+  // (i.e. is a low speed device on a full speed hub) then need pre
+  return hcd_port_speed_get(0) != tuh_speed_get(dev_addr);
 }
 
-static void hw_xfer_complete(struct hw_endpoint *ep, xfer_result_t xfer_result)
+static void __tusb_irq_path_func(hw_xfer_complete)(struct hw_endpoint *ep, xfer_result_t xfer_result)
 {
-    // Mark transfer as done before we tell the tinyusb stack
-    uint8_t dev_addr = ep->dev_addr;
-    uint8_t ep_addr = ep->ep_addr;
-    uint xferred_len = ep->xferred_len;
-    hw_endpoint_reset_transfer(ep);
-    hcd_event_xfer_complete(dev_addr, ep_addr, xferred_len, xfer_result, true);
+  // Mark transfer as done before we tell the tinyusb stack
+  uint8_t dev_addr = ep->dev_addr;
+  uint8_t ep_addr = ep->ep_addr;
+  uint xferred_len = ep->xferred_len;
+  hw_endpoint_reset_transfer(ep);
+  hcd_event_xfer_complete(dev_addr, ep_addr, xferred_len, xfer_result, true);
 }
 
-static void _handle_buff_status_bit(uint bit, struct hw_endpoint *ep)
+static void __tusb_irq_path_func(_handle_buff_status_bit)(uint bit, struct hw_endpoint *ep)
 {
-    usb_hw_clear->buf_status = bit;
-    bool done = hw_endpoint_xfer_continue(ep);
-    if (done)
-    {
-        hw_xfer_complete(ep, XFER_RESULT_SUCCESS);
-    }
+  usb_hw_clear->buf_status = bit;
+  // EP may have been stalled?
+  assert(ep->active);
+  bool done = hw_endpoint_xfer_continue(ep);
+  if ( done )
+  {
+    hw_xfer_complete(ep, XFER_RESULT_SUCCESS);
+  }
 }
 
-static void hw_handle_buff_status(void)
+static void __tusb_irq_path_func(hw_handle_buff_status)(void)
 {
-    uint32_t remaining_buffers = usb_hw->buf_status;
-    pico_trace("buf_status 0x%08x\n", remaining_buffers);
+  uint32_t remaining_buffers = usb_hw->buf_status;
+  pico_trace("buf_status 0x%08x\n", remaining_buffers);
 
-    // Check EPX first
-    uint bit = 0b1;
-    if (remaining_buffers & bit)
-    {
-        remaining_buffers &= ~bit;
-        struct hw_endpoint *ep = &epx;
-
-        uint32_t ep_ctrl = *ep->endpoint_control;
-        if (ep_ctrl & EP_CTRL_DOUBLE_BUFFERED_BITS)
-        {
-          TU_LOG(3, "Double Buffered: ");
-        }else
-        {
-          TU_LOG(3, "Single Buffered: ");
-        }
-        TU_LOG_HEX(3, ep_ctrl);
+  // Check EPX first
+  uint bit = 0b1;
+  if ( remaining_buffers & bit )
+  {
+    remaining_buffers &= ~bit;
+    struct hw_endpoint * ep = &epx;
 
-        _handle_buff_status_bit(bit, ep);
+    uint32_t ep_ctrl = *ep->endpoint_control;
+    if ( ep_ctrl & EP_CTRL_DOUBLE_BUFFERED_BITS )
+    {
+      TU_LOG(3, "Double Buffered: ");
     }
-
-    // Check interrupt endpoints
-    for (uint i = 1; i <= USB_HOST_INTERRUPT_ENDPOINTS && remaining_buffers; i++)
+    else
     {
-        // EPX is bit 0
-        // IEP1 is bit 2
-        // IEP2 is bit 4
-        // IEP3 is bit 6
-        // etc
-        bit = 1 << (i*2);
-
-        if (remaining_buffers & bit)
-        {
-            remaining_buffers &= ~bit;
-            _handle_buff_status_bit(bit, &ep_pool[i]);
-        }
+      TU_LOG(3, "Single Buffered: ");
     }
+    TU_LOG_HEX(3, ep_ctrl);
 
-    if (remaining_buffers)
+    _handle_buff_status_bit(bit, ep);
+  }
+
+  // Check "interrupt" (asynchronous) endpoints for both IN and OUT
+  for ( uint i = 1; i <= USB_HOST_INTERRUPT_ENDPOINTS && remaining_buffers; i++ )
+  {
+    // EPX is bit 0 & 1
+    // IEP1 IN  is bit 2
+    // IEP1 OUT is bit 3
+    // IEP2 IN  is bit 4
+    // IEP2 OUT is bit 5
+    // IEP3 IN  is bit 6
+    // IEP3 OUT is bit 7
+    // etc
+    for ( uint j = 0; j < 2; j++ )
     {
-        panic("Unhandled buffer %d\n", remaining_buffers);
+      bit = 1 << (i * 2 + j);
+      if ( remaining_buffers & bit )
+      {
+        remaining_buffers &= ~bit;
+        _handle_buff_status_bit(bit, &ep_pool[i]);
+      }
     }
+  }
+
+  if ( remaining_buffers )
+  {
+    panic("Unhandled buffer %d\n", remaining_buffers);
+  }
 }
 
-static void hw_trans_complete(void)
+static void __tusb_irq_path_func(hw_trans_complete)(void)
 {
   if (usb_hw->sie_ctrl & USB_SIE_CTRL_SEND_SETUP_BITS)
   {
     pico_trace("Sent setup packet\n");
     struct hw_endpoint *ep = &epx;
     assert(ep->active);
+    // Set transferred length to 8 for a setup packet
+    ep->xferred_len = 8;
     hw_xfer_complete(ep, XFER_RESULT_SUCCESS);
   }
   else
@@ -175,178 +182,196 @@ static void hw_trans_complete(void)
   }
 }
 
-static void hcd_rp2040_irq(void)
+static void __tusb_irq_path_func(hcd_rp2040_irq)(void)
 {
-    uint32_t status = usb_hw->ints;
-    uint32_t handled = 0;
+  uint32_t status = usb_hw->ints;
+  uint32_t handled = 0;
 
-    if (status & USB_INTS_HOST_CONN_DIS_BITS)
-    {
-        handled |= USB_INTS_HOST_CONN_DIS_BITS;
-        
-        if (dev_speed())
-        {
-            hcd_event_device_attach(RHPORT_NATIVE, true);
-        }
-        else
-        {
-            hcd_event_device_remove(RHPORT_NATIVE, true);
-        }
-
-        // Clear speed change interrupt
-        usb_hw_clear->sie_status = USB_SIE_STATUS_SPEED_BITS;
-    }
+  if ( status & USB_INTS_HOST_CONN_DIS_BITS )
+  {
+    handled |= USB_INTS_HOST_CONN_DIS_BITS;
 
-    if (status & USB_INTS_BUFF_STATUS_BITS)
+    if ( dev_speed() )
     {
-        handled |= USB_INTS_BUFF_STATUS_BITS;
-        TU_LOG(2, "Buffer complete\n");
-        hw_handle_buff_status();
+      hcd_event_device_attach(RHPORT_NATIVE, true);
     }
-
-    if (status & USB_INTS_TRANS_COMPLETE_BITS)
+    else
     {
-        handled |= USB_INTS_TRANS_COMPLETE_BITS;
-        usb_hw_clear->sie_status = USB_SIE_STATUS_TRANS_COMPLETE_BITS;
-        TU_LOG(2, "Transfer complete\n");
-        hw_trans_complete();
+      hcd_event_device_remove(RHPORT_NATIVE, true);
     }
 
-    if (status & USB_INTS_STALL_BITS)
-    {
-        // We have rx'd a stall from the device
-        pico_trace("Stall REC\n");
-        handled |= USB_INTS_STALL_BITS;
-        usb_hw_clear->sie_status = USB_SIE_STATUS_STALL_REC_BITS;
-        hw_xfer_complete(&epx, XFER_RESULT_STALLED);
-    }
+    // Clear speed change interrupt
+    usb_hw_clear->sie_status = USB_SIE_STATUS_SPEED_BITS;
+  }
 
-    if (status & USB_INTS_ERROR_RX_TIMEOUT_BITS)
-    {
-        handled |= USB_INTS_ERROR_RX_TIMEOUT_BITS;
-        usb_hw_clear->sie_status = USB_SIE_STATUS_RX_TIMEOUT_BITS;
-    }
+  if ( status & USB_INTS_STALL_BITS )
+  {
+    // We have rx'd a stall from the device
+    // NOTE THIS SHOULD HAVE PRIORITY OVER BUFF_STATUS
+    // AND TRANS_COMPLETE as the stall is an alternative response
+    // to one of those events
+    pico_trace("Stall REC\n");
+    handled |= USB_INTS_STALL_BITS;
+    usb_hw_clear->sie_status = USB_SIE_STATUS_STALL_REC_BITS;
+    hw_xfer_complete(&epx, XFER_RESULT_STALLED);
+  }
 
-    if (status & USB_INTS_ERROR_DATA_SEQ_BITS)
-    {
-        usb_hw_clear->sie_status = USB_SIE_STATUS_DATA_SEQ_ERROR_BITS;
-        TU_LOG(3, "  Seq Error: [0] = 0x%04u  [1] = 0x%04x\r\n", tu_u32_low16(*epx.buffer_control), tu_u32_high16(*epx.buffer_control));
-        panic("Data Seq Error \n");
-    }
+  if ( status & USB_INTS_BUFF_STATUS_BITS )
+  {
+    handled |= USB_INTS_BUFF_STATUS_BITS;
+    TU_LOG(2, "Buffer complete\n");
+    hw_handle_buff_status();
+  }
 
-    if (status ^ handled)
-    {
-        panic("Unhandled IRQ 0x%x\n", (uint) (status ^ handled));
-    }
+  if ( status & USB_INTS_TRANS_COMPLETE_BITS )
+  {
+    handled |= USB_INTS_TRANS_COMPLETE_BITS;
+    usb_hw_clear->sie_status = USB_SIE_STATUS_TRANS_COMPLETE_BITS;
+    TU_LOG(2, "Transfer complete\n");
+    hw_trans_complete();
+  }
+
+  if ( status & USB_INTS_ERROR_RX_TIMEOUT_BITS )
+  {
+    handled |= USB_INTS_ERROR_RX_TIMEOUT_BITS;
+    usb_hw_clear->sie_status = USB_SIE_STATUS_RX_TIMEOUT_BITS;
+  }
+
+  if ( status & USB_INTS_ERROR_DATA_SEQ_BITS )
+  {
+    usb_hw_clear->sie_status = USB_SIE_STATUS_DATA_SEQ_ERROR_BITS;
+    TU_LOG(3, "  Seq Error: [0] = 0x%04u  [1] = 0x%04x\r\n",
+           tu_u32_low16(*epx.buffer_control),
+           tu_u32_high16(*epx.buffer_control));
+    panic("Data Seq Error \n");
+  }
+
+  if ( status ^ handled )
+  {
+    panic("Unhandled IRQ 0x%x\n", (uint) (status ^ handled));
+  }
+}
+
+void __tusb_irq_path_func(hcd_int_handler)(uint8_t rhport)
+{
+  (void) rhport;
+  hcd_rp2040_irq();
 }
 
 static struct hw_endpoint *_next_free_interrupt_ep(void)
 {
-    struct hw_endpoint *ep = NULL;
-    for (uint i = 1; i < TU_ARRAY_SIZE(ep_pool); i++)
+  struct hw_endpoint * ep = NULL;
+  for ( uint i = 1; i < TU_ARRAY_SIZE(ep_pool); i++ )
+  {
+    ep = &ep_pool[i];
+    if ( !ep->configured )
     {
-        ep = &ep_pool[i];
-        if (!ep->configured)
-        {
-            // Will be configured by _hw_endpoint_init / _hw_endpoint_allocate
-            ep->interrupt_num = i - 1;
-            return ep;
-        }
+      // Will be configured by _hw_endpoint_init / _hw_endpoint_allocate
+      ep->interrupt_num = (uint8_t) (i - 1);
+      return ep;
     }
-    return ep;
+  }
+  return ep;
 }
 
 static struct hw_endpoint *_hw_endpoint_allocate(uint8_t transfer_type)
 {
-    struct hw_endpoint *ep = NULL;
+  struct hw_endpoint * ep = NULL;
 
-    if (transfer_type == TUSB_XFER_INTERRUPT)
-    {
-        ep = _next_free_interrupt_ep();
-        pico_info("Allocate interrupt ep %d\n", ep->interrupt_num);
-        assert(ep);
-        ep->buffer_control = &usbh_dpram->int_ep_buffer_ctrl[ep->interrupt_num].ctrl;
-        ep->endpoint_control = &usbh_dpram->int_ep_ctrl[ep->interrupt_num].ctrl;
-        // 0 for epx (double buffered): TODO increase to 1024 for ISO
-        // 2x64 for intep0
-        // 3x64 for intep1
-        // etc
-        ep->hw_data_buf = &usbh_dpram->epx_data[64 * (ep->interrupt_num + 2)];
-    }
-    else
-    {
-        ep = &epx;
-        ep->buffer_control = &usbh_dpram->epx_buf_ctrl;
-        ep->endpoint_control = &usbh_dpram->epx_ctrl;
-        ep->hw_data_buf = &usbh_dpram->epx_data[0];
-    }
+  if ( transfer_type != TUSB_XFER_CONTROL )
+  {
+    // Note: even though datasheet name these "Interrupt" endpoints. These are actually
+    // "Asynchronous" endpoints and can be used for other type such as: Bulk  (ISO need confirmation)
+    ep = _next_free_interrupt_ep();
+    pico_info("Allocate %s ep %d\n", tu_edpt_type_str(transfer_type), ep->interrupt_num);
+    assert(ep);
+    ep->buffer_control = &usbh_dpram->int_ep_buffer_ctrl[ep->interrupt_num].ctrl;
+    ep->endpoint_control = &usbh_dpram->int_ep_ctrl[ep->interrupt_num].ctrl;
+    // 0 for epx (double buffered): TODO increase to 1024 for ISO
+    // 2x64 for intep0
+    // 3x64 for intep1
+    // etc
+    ep->hw_data_buf = &usbh_dpram->epx_data[64 * (ep->interrupt_num + 2)];
+  }
+  else
+  {
+    ep = &epx;
+    ep->buffer_control = &usbh_dpram->epx_buf_ctrl;
+    ep->endpoint_control = &usbh_dpram->epx_ctrl;
+    ep->hw_data_buf = &usbh_dpram->epx_data[0];
+  }
 
-    return ep;
+  return ep;
 }
 
-static void _hw_endpoint_init(struct hw_endpoint *ep, uint8_t dev_addr, uint8_t ep_addr, uint wMaxPacketSize, uint8_t transfer_type, uint8_t bmInterval)
+static void _hw_endpoint_init(struct hw_endpoint *ep, uint8_t dev_addr, uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t transfer_type, uint8_t bmInterval)
 {
-    // Already has data buffer, endpoint control, and buffer control allocated at this point
-    assert(ep->endpoint_control);
-    assert(ep->buffer_control);
-    assert(ep->hw_data_buf);
+  // Already has data buffer, endpoint control, and buffer control allocated at this point
+  assert(ep->endpoint_control);
+  assert(ep->buffer_control);
+  assert(ep->hw_data_buf);
 
-    uint8_t const num = tu_edpt_number(ep_addr);
-    tusb_dir_t const dir = tu_edpt_dir(ep_addr);
+  uint8_t const num = tu_edpt_number(ep_addr);
+  tusb_dir_t const dir = tu_edpt_dir(ep_addr);
 
-    ep->ep_addr = ep_addr;
-    ep->dev_addr = dev_addr;
+  ep->ep_addr = ep_addr;
+  ep->dev_addr = dev_addr;
 
-    // For host, IN to host == RX, anything else rx == false
-    ep->rx = (dir == TUSB_DIR_IN);
+  // For host, IN to host == RX, anything else rx == false
+  ep->rx = (dir == TUSB_DIR_IN);
 
-    // Response to a setup packet on EP0 starts with pid of 1
-    ep->next_pid = (num == 0 ? 1u : 0u);
-    ep->wMaxPacketSize = wMaxPacketSize;
-    ep->transfer_type = transfer_type;
+  // Response to a setup packet on EP0 starts with pid of 1
+  ep->next_pid = (num == 0 ? 1u : 0u);
+  ep->wMaxPacketSize = wMaxPacketSize;
+  ep->transfer_type = transfer_type;
 
-    pico_trace("hw_endpoint_init dev %d ep %d %s xfer %d\n", ep->dev_addr, tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)], ep->transfer_type);
-    pico_trace("dev %d ep %d %s setup buffer @ 0x%p\n", ep->dev_addr, tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)], ep->hw_data_buf);
-    uint dpram_offset = hw_data_offset(ep->hw_data_buf);
-    // Bits 0-5 should be 0
-    assert(!(dpram_offset & 0b111111));
+  pico_trace("hw_endpoint_init dev %d ep %d %s xfer %d\n", ep->dev_addr, tu_edpt_number(ep->ep_addr),
+             ep_dir_string[tu_edpt_dir(ep->ep_addr)], ep->transfer_type);
+  pico_trace("dev %d ep %d %s setup buffer @ 0x%p\n", ep->dev_addr, tu_edpt_number(ep->ep_addr),
+             ep_dir_string[tu_edpt_dir(ep->ep_addr)], ep->hw_data_buf);
+  uint dpram_offset = hw_data_offset(ep->hw_data_buf);
+  // Bits 0-5 should be 0
+  assert(!(dpram_offset & 0b111111));
 
-    // Fill in endpoint control register with buffer offset
-    uint32_t ep_reg =  EP_CTRL_ENABLE_BITS
-                  | EP_CTRL_INTERRUPT_PER_BUFFER
-                  | (ep->transfer_type << EP_CTRL_BUFFER_TYPE_LSB)
-                  | dpram_offset;
-    ep_reg |= bmInterval ? (bmInterval - 1) << EP_CTRL_HOST_INTERRUPT_INTERVAL_LSB : 0;
-    *ep->endpoint_control = ep_reg;
-    pico_trace("endpoint control (0x%p) <- 0x%x\n", ep->endpoint_control, ep_reg);
-    ep->configured = true;
+  // Fill in endpoint control register with buffer offset
+  uint32_t ep_reg = EP_CTRL_ENABLE_BITS
+                    | EP_CTRL_INTERRUPT_PER_BUFFER
+                    | (ep->transfer_type << EP_CTRL_BUFFER_TYPE_LSB)
+                    | dpram_offset;
+  if ( bmInterval )
+  {
+    ep_reg |= (uint32_t) ((bmInterval - 1) << EP_CTRL_HOST_INTERRUPT_INTERVAL_LSB);
+  }
+  *ep->endpoint_control = ep_reg;
+  pico_trace("endpoint control (0x%p) <- 0x%x\n", ep->endpoint_control, ep_reg);
+  ep->configured = true;
 
-    if (bmInterval)
-    {
-        // This is an interrupt endpoint
-        // so need to set up interrupt endpoint address control register with:
-        // device address
-        // endpoint number / direction
-        // preamble
-        uint32_t reg = dev_addr | (num << USB_ADDR_ENDP1_ENDPOINT_LSB);
+  if ( ep != &epx )
+  {
+    // Endpoint has its own addr_endp and interrupt bits to be setup!
+    // This is an interrupt/async endpoint. so need to set up ADDR_ENDP register with:
+    // - device address
+    // - endpoint number / direction
+    // - preamble
+    uint32_t reg = (uint32_t) (dev_addr | (num << USB_ADDR_ENDP1_ENDPOINT_LSB));
 
-        if (dir == TUSB_DIR_OUT)
-        {
-            reg |= USB_ADDR_ENDP1_INTEP_DIR_BITS;
-        }
+    if ( dir == TUSB_DIR_OUT )
+    {
+      reg |= USB_ADDR_ENDP1_INTEP_DIR_BITS;
+    }
 
-        if (need_pre(dev_addr))
-        {
-            reg |= USB_ADDR_ENDP1_INTEP_PREAMBLE_BITS;
-        }
-        usb_hw->int_ep_addr_ctrl[ep->interrupt_num] = reg;
+    if ( need_pre(dev_addr) )
+    {
+      reg |= USB_ADDR_ENDP1_INTEP_PREAMBLE_BITS;
+    }
+    usb_hw->int_ep_addr_ctrl[ep->interrupt_num] = reg;
 
-        // Finally, enable interrupt that endpoint
-        usb_hw_set->int_ep_ctrl = 1 << (ep->interrupt_num + 1);
+    // Finally, enable interrupt that endpoint
+    usb_hw_set->int_ep_ctrl = 1 << (ep->interrupt_num + 1);
 
-        // If it's an interrupt endpoint we need to set up the buffer control
-        // register
-    }
+    // If it's an interrupt endpoint we need to set up the buffer control
+    // register
+  }
 }
 
 //--------------------------------------------------------------------+
@@ -354,39 +379,44 @@ static void _hw_endpoint_init(struct hw_endpoint *ep, uint8_t dev_addr, uint8_t
 //--------------------------------------------------------------------+
 bool hcd_init(uint8_t rhport)
 {
-    pico_trace("hcd_init %d\n", rhport);
-    assert(rhport == 0);
+  (void) rhport;
+  pico_trace("hcd_init %d\n", rhport);
+  assert(rhport == 0);
+
+  // Reset any previous state
+  rp2040_usb_init();
 
-    // Reset any previous state
-    rp2040_usb_init();
+  // Force VBUS detect to always present, for now we assume vbus is always provided (without using VBUS En)
+  usb_hw->pwr = USB_USB_PWR_VBUS_DETECT_BITS | USB_USB_PWR_VBUS_DETECT_OVERRIDE_EN_BITS;
 
-    // Force VBUS detect to always present, for now we assume vbus is always provided (without using VBUS En)
-    usb_hw->pwr = USB_USB_PWR_VBUS_DETECT_BITS | USB_USB_PWR_VBUS_DETECT_OVERRIDE_EN_BITS;
+  // Remove shared irq if it was previously added so as not to fill up shared irq slots
+  irq_remove_handler(USBCTRL_IRQ, hcd_rp2040_irq);
 
-    irq_set_exclusive_handler(USBCTRL_IRQ, hcd_rp2040_irq);
+  irq_add_shared_handler(USBCTRL_IRQ, hcd_rp2040_irq, PICO_SHARED_IRQ_HANDLER_HIGHEST_ORDER_PRIORITY);
 
-    // clear epx and interrupt eps
-    memset(&ep_pool, 0, sizeof(ep_pool));
+  // clear epx and interrupt eps
+  memset(&ep_pool, 0, sizeof(ep_pool));
 
-    // Enable in host mode with SOF / Keep alive on
-    usb_hw->main_ctrl = USB_MAIN_CTRL_CONTROLLER_EN_BITS | USB_MAIN_CTRL_HOST_NDEVICE_BITS;
-    usb_hw->sie_ctrl = SIE_CTRL_BASE;
-    usb_hw->inte = USB_INTE_BUFF_STATUS_BITS      | 
-                   USB_INTE_HOST_CONN_DIS_BITS    | 
-                   USB_INTE_HOST_RESUME_BITS      | 
-                   USB_INTE_STALL_BITS            | 
-                   USB_INTE_TRANS_COMPLETE_BITS   |
-                   USB_INTE_ERROR_RX_TIMEOUT_BITS |
-                   USB_INTE_ERROR_DATA_SEQ_BITS   ;
+  // Enable in host mode with SOF / Keep alive on
+  usb_hw->main_ctrl = USB_MAIN_CTRL_CONTROLLER_EN_BITS | USB_MAIN_CTRL_HOST_NDEVICE_BITS;
+  usb_hw->sie_ctrl = SIE_CTRL_BASE;
+  usb_hw->inte = USB_INTE_BUFF_STATUS_BITS      |
+                 USB_INTE_HOST_CONN_DIS_BITS    |
+                 USB_INTE_HOST_RESUME_BITS      |
+                 USB_INTE_STALL_BITS            |
+                 USB_INTE_TRANS_COMPLETE_BITS   |
+                 USB_INTE_ERROR_RX_TIMEOUT_BITS |
+                 USB_INTE_ERROR_DATA_SEQ_BITS   ;
 
-    return true;
+  return true;
 }
 
 void hcd_port_reset(uint8_t rhport)
 {
-    pico_trace("hcd_port_reset\n");
-    assert(rhport == 0);
-    // TODO: Nothing to do here yet. Perhaps need to reset some state?
+  (void) rhport;
+  pico_trace("hcd_port_reset\n");
+  assert(rhport == 0);
+  // TODO: Nothing to do here yet. Perhaps need to reset some state?
 }
 
 void hcd_port_reset_end(uint8_t rhport)
@@ -396,25 +426,28 @@ void hcd_port_reset_end(uint8_t rhport)
 
 bool hcd_port_connect_status(uint8_t rhport)
 {
-    pico_trace("hcd_port_connect_status\n");
-    assert(rhport == 0);
-    return usb_hw->sie_status & USB_SIE_STATUS_SPEED_BITS;
+  (void) rhport;
+  pico_trace("hcd_port_connect_status\n");
+  assert(rhport == 0);
+  return usb_hw->sie_status & USB_SIE_STATUS_SPEED_BITS;
 }
 
 tusb_speed_t hcd_port_speed_get(uint8_t rhport)
 {
-    assert(rhport == 0);
-    // TODO: Should enumval this register
-    switch (dev_speed())
-    {
-        case 1:
-            return TUSB_SPEED_LOW;
-        case 2:
-            return TUSB_SPEED_FULL;
-        default:
-            panic("Invalid speed\n");
-            return TUSB_SPEED_INVALID;
-    }
+  (void) rhport;
+  assert(rhport == 0);
+
+  // TODO: Should enumval this register
+  switch ( dev_speed() )
+  {
+    case 1:
+      return TUSB_SPEED_LOW;
+    case 2:
+      return TUSB_SPEED_FULL;
+    default:
+      panic("Invalid speed\n");
+      return TUSB_SPEED_INVALID;
+  }
 }
 
 // Close all opened endpoint belong to this device
@@ -446,21 +479,23 @@ void hcd_device_close(uint8_t rhport, uint8_t dev_addr)
 
 uint32_t hcd_frame_number(uint8_t rhport)
 {
-    (void) rhport;
-    return usb_hw->sof_rd;
+  (void) rhport;
+  return usb_hw->sof_rd;
 }
 
 void hcd_int_enable(uint8_t rhport)
 {
-    assert(rhport == 0);
-    irq_set_enabled(USBCTRL_IRQ, true);
+  (void) rhport;
+  assert(rhport == 0);
+  irq_set_enabled(USBCTRL_IRQ, true);
 }
 
 void hcd_int_disable(uint8_t rhport)
 {
-    // todo we should check this is disabling from the correct core; note currently this is never called
-    assert(rhport == 0);
-    irq_set_enabled(USBCTRL_IRQ, false);
+  (void) rhport;
+  // todo we should check this is disabling from the correct core; note currently this is never called
+  assert(rhport == 0);
+  irq_set_enabled(USBCTRL_IRQ, false);
 }
 
 //--------------------------------------------------------------------+
@@ -469,124 +504,116 @@ void hcd_int_disable(uint8_t rhport)
 
 bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const * ep_desc)
 {
-    (void) rhport;
+  (void) rhport;
 
-    pico_trace("hcd_edpt_open dev_addr %d, ep_addr %d\n", dev_addr, ep_desc->bEndpointAddress);
+  pico_trace("hcd_edpt_open dev_addr %d, ep_addr %d\n", dev_addr, ep_desc->bEndpointAddress);
 
-    // Allocated differently based on if it's an interrupt endpoint or not
-    struct hw_endpoint *ep = _hw_endpoint_allocate(ep_desc->bmAttributes.xfer);
+  // Allocated differently based on if it's an interrupt endpoint or not
+  struct hw_endpoint *ep = _hw_endpoint_allocate(ep_desc->bmAttributes.xfer);
+  TU_ASSERT(ep);
 
-    _hw_endpoint_init(ep,
-        dev_addr,
-        ep_desc->bEndpointAddress,
-        tu_edpt_packet_size(ep_desc),
-        ep_desc->bmAttributes.xfer,
-        ep_desc->bInterval);
+  _hw_endpoint_init(ep,
+                    dev_addr,
+                    ep_desc->bEndpointAddress,
+                    tu_edpt_packet_size(ep_desc),
+                    ep_desc->bmAttributes.xfer,
+                    ep_desc->bInterval);
 
-    return true;
+  return true;
 }
 
 bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t * buffer, uint16_t buflen)
 {
-    (void) rhport;
+  (void) rhport;
 
-    pico_trace("hcd_edpt_xfer dev_addr %d, ep_addr 0x%x, len %d\n", dev_addr, ep_addr, buflen);
-    
-    uint8_t const ep_num = tu_edpt_number(ep_addr);
-    tusb_dir_t const ep_dir = tu_edpt_dir(ep_addr);
+  pico_trace("hcd_edpt_xfer dev_addr %d, ep_addr 0x%x, len %d\n", dev_addr, ep_addr, buflen);
 
-    // Get appropriate ep. Either EPX or interrupt endpoint
-    struct hw_endpoint *ep = get_dev_ep(dev_addr, ep_addr);
-    assert(ep);
+  uint8_t const ep_num = tu_edpt_number(ep_addr);
+  tusb_dir_t const ep_dir = tu_edpt_dir(ep_addr);
 
-    // Control endpoint can change direction 0x00 <-> 0x80
-    if ( ep_addr != ep->ep_addr )
-    {
-      assert(ep_num == 0);
+  // Get appropriate ep. Either EPX or interrupt endpoint
+  struct hw_endpoint *ep = get_dev_ep(dev_addr, ep_addr);
 
-      // Direction has flipped on endpoint control so re init it but with same properties
-      _hw_endpoint_init(ep, dev_addr, ep_addr, ep->wMaxPacketSize, ep->transfer_type, 0);
-    }
+  TU_ASSERT(ep);
 
-    // If a normal transfer (non-interrupt) then initiate using
-    // sie ctrl registers. Otherwise interrupt ep registers should
-    // already be configured
-    if (ep == &epx) {
-        hw_endpoint_xfer_start(ep, buffer, buflen);
+  // EP should be inactive
+  assert(!ep->active);
 
-        // That has set up buffer control, endpoint control etc
-        // for host we have to initiate the transfer
-        usb_hw->dev_addr_ctrl = dev_addr | (ep_num << USB_ADDR_ENDP_ENDPOINT_LSB);
+  // Control endpoint can change direction 0x00 <-> 0x80
+  if ( ep_addr != ep->ep_addr )
+  {
+    assert(ep_num == 0);
 
-        uint32_t flags = USB_SIE_CTRL_START_TRANS_BITS | SIE_CTRL_BASE |
-                         (ep_dir ? USB_SIE_CTRL_RECEIVE_DATA_BITS : USB_SIE_CTRL_SEND_DATA_BITS);
-        // Set pre if we are a low speed device on full speed hub
-        flags |= need_pre(dev_addr) ? USB_SIE_CTRL_PREAMBLE_EN_BITS : 0;
+    // Direction has flipped on endpoint control so re init it but with same properties
+    _hw_endpoint_init(ep, dev_addr, ep_addr, ep->wMaxPacketSize, ep->transfer_type, 0);
+  }
 
-        usb_hw->sie_ctrl = flags;
-    }else
-    {
-      hw_endpoint_xfer_start(ep, buffer, buflen);
-    }
+  // If a normal transfer (non-interrupt) then initiate using
+  // sie ctrl registers. Otherwise interrupt ep registers should
+  // already be configured
+  if ( ep == &epx )
+  {
+    hw_endpoint_xfer_start(ep, buffer, buflen);
+
+    // That has set up buffer control, endpoint control etc
+    // for host we have to initiate the transfer
+    usb_hw->dev_addr_ctrl = (uint32_t) (dev_addr | (ep_num << USB_ADDR_ENDP_ENDPOINT_LSB));
+
+    uint32_t flags = USB_SIE_CTRL_START_TRANS_BITS | SIE_CTRL_BASE |
+                     (ep_dir ? USB_SIE_CTRL_RECEIVE_DATA_BITS : USB_SIE_CTRL_SEND_DATA_BITS) |
+                     (need_pre(dev_addr) ? USB_SIE_CTRL_PREAMBLE_EN_BITS : 0);
+    usb_hw->sie_ctrl = flags;
+  }else
+  {
+    hw_endpoint_xfer_start(ep, buffer, buflen);
+  }
 
-    return true;
+  return true;
 }
 
 bool hcd_setup_send(uint8_t rhport, uint8_t dev_addr, uint8_t const setup_packet[8])
 {
-    (void) rhport;
+  (void) rhport;
 
-    // Copy data into setup packet buffer
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Warray-bounds"
-#pragma GCC diagnostic ignored "-Wstringop-overflow"
-    memcpy((void*)&usbh_dpram->setup_packet[0], setup_packet, 8);
-#pragma GCC diagnostic pop
+  // Copy data into setup packet buffer
+  for ( uint8_t i = 0; i < 8; i++ )
+  {
+    usbh_dpram->setup_packet[i] = setup_packet[i];
+  }
 
-    // Configure EP0 struct with setup info for the trans complete
-    struct hw_endpoint *ep = _hw_endpoint_allocate(0);
+  // Configure EP0 struct with setup info for the trans complete
+  struct hw_endpoint * ep = _hw_endpoint_allocate(0);
+  TU_ASSERT(ep);
 
-    // EP0 out
-    _hw_endpoint_init(ep, dev_addr, 0x00, ep->wMaxPacketSize, 0, 0);
-    assert(ep->configured);
+  // EPX should be inactive
+  assert(!ep->active);
 
-    ep->remaining_len = 8;
-    ep->active        = true;
+  // EP0 out
+  _hw_endpoint_init(ep, dev_addr, 0x00, ep->wMaxPacketSize, 0, 0);
+  assert(ep->configured);
 
-    // Set device address
-    usb_hw->dev_addr_ctrl = dev_addr;
+  ep->remaining_len = 8;
+  ep->active = true;
 
-    // Set pre if we are a low speed device on full speed hub
-    uint32_t const flags = SIE_CTRL_BASE | USB_SIE_CTRL_SEND_SETUP_BITS | USB_SIE_CTRL_START_TRANS_BITS |
-                           (need_pre(dev_addr) ? USB_SIE_CTRL_PREAMBLE_EN_BITS : 0);
+  // Set device address
+  usb_hw->dev_addr_ctrl = dev_addr;
 
-    usb_hw->sie_ctrl = flags;
-
-    return true;
-}
+  // Set pre if we are a low speed device on full speed hub
+  uint32_t const flags = SIE_CTRL_BASE | USB_SIE_CTRL_SEND_SETUP_BITS | USB_SIE_CTRL_START_TRANS_BITS |
+                         (need_pre(dev_addr) ? USB_SIE_CTRL_PREAMBLE_EN_BITS : 0);
 
+  usb_hw->sie_ctrl = flags;
 
-//bool hcd_edpt_busy(uint8_t dev_addr, uint8_t ep_addr)
-//{
-//    // EPX is shared, so multiple device addresses and endpoint addresses share that
-//    // so if any transfer is active on epx, we are busy. Interrupt endpoints have their own
-//    // EPX so ep->active will only be busy if there is a pending transfer on that interrupt endpoint
-//    // on that device
-//    pico_trace("hcd_edpt_busy dev addr %d ep_addr 0x%x\n", dev_addr, ep_addr);
-//    struct hw_endpoint *ep = get_dev_ep(dev_addr, ep_addr);
-//    assert(ep);
-//    bool busy = ep->active;
-//    pico_trace("busy == %d\n", busy);
-//    return busy;
-//}
+  return true;
+}
 
 bool hcd_edpt_clear_stall(uint8_t dev_addr, uint8_t ep_addr)
 {
-    (void) dev_addr;
-    (void) ep_addr;
+  (void) dev_addr;
+  (void) ep_addr;
 
-    panic("hcd_clear_stall");
-    return true;
+  panic("hcd_clear_stall");
+  return true;
 }
 
 #endif
diff --git a/src/portable/raspberrypi/rp2040/rp2040_usb.c b/src/portable/raspberrypi/rp2040/rp2040_usb.c
index c93199fd0..df05697fe 100644
--- a/src/portable/raspberrypi/rp2040/rp2040_usb.c
+++ b/src/portable/raspberrypi/rp2040/rp2040_usb.c
@@ -32,23 +32,34 @@
 #include <stdlib.h>
 #include "rp2040_usb.h"
 
+//--------------------------------------------------------------------+
+// MACRO CONSTANT TYPEDEF PROTOTYPE
+//--------------------------------------------------------------------+
+
 // Direction strings for debug
 const char *ep_dir_string[] = {
         "out",
         "in",
 };
 
-static inline void _hw_endpoint_lock_update(__unused struct hw_endpoint * ep, __unused int delta) {
-    // todo add critsec as necessary to prevent issues between worker and IRQ...
-    //  note that this is perhaps as simple as disabling IRQs because it would make
-    //  sense to have worker and IRQ on same core, however I think using critsec is about equivalent.
-}
-
 static void _hw_endpoint_xfer_sync(struct hw_endpoint *ep);
-static void _hw_endpoint_start_next_buffer(struct hw_endpoint *ep);
+
+#if TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
+  static bool e15_is_bulkin_ep(struct hw_endpoint *ep);
+  static bool e15_is_critical_frame_period(struct hw_endpoint *ep);
+#else
+  #define e15_is_bulkin_ep(x)             (false)
+  #define e15_is_critical_frame_period(x) (false)
+#endif
+
+// if usb hardware is in host mode
+TU_ATTR_ALWAYS_INLINE static inline bool is_host_mode(void)
+{
+  return (usb_hw->main_ctrl & USB_MAIN_CTRL_HOST_NDEVICE_BITS) ? true : false;
+}
 
 //--------------------------------------------------------------------+
-//
+// Implementation
 //--------------------------------------------------------------------+
 
 void rp2040_usb_init(void)
@@ -60,16 +71,20 @@ void rp2040_usb_init(void)
   // Clear any previous state just in case
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Warray-bounds"
+#if __GNUC__ > 6
 #pragma GCC diagnostic ignored "-Wstringop-overflow"
+#endif
   memset(usb_hw, 0, sizeof(*usb_hw));
   memset(usb_dpram, 0, sizeof(*usb_dpram));
 #pragma GCC diagnostic pop
 
   // Mux the controller to the onboard usb phy
   usb_hw->muxing = USB_USB_MUXING_TO_PHY_BITS | USB_USB_MUXING_SOFTCON_BITS;
+
+  TU_LOG2_INT(sizeof(hw_endpoint_t));
 }
 
-void hw_endpoint_reset_transfer(struct hw_endpoint *ep)
+void __tusb_irq_path_func(hw_endpoint_reset_transfer)(struct hw_endpoint *ep)
 {
   ep->active = false;
   ep->remaining_len = 0;
@@ -77,20 +92,27 @@ void hw_endpoint_reset_transfer(struct hw_endpoint *ep)
   ep->user_buf = 0;
 }
 
-void _hw_endpoint_buffer_control_update32(struct hw_endpoint *ep, uint32_t and_mask, uint32_t or_mask) {
-    uint32_t value = 0;
-    if (and_mask) {
-        value = *ep->buffer_control & and_mask;
-    }
-    if (or_mask) {
-        value |= or_mask;
-        if (or_mask & USB_BUF_CTRL_AVAIL) {
-            if (*ep->buffer_control & USB_BUF_CTRL_AVAIL) {
-                panic("ep %d %s was already available", tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
-            }
-            *ep->buffer_control = value & ~USB_BUF_CTRL_AVAIL;
-            // 12 cycle delay.. (should be good for 48*12Mhz = 576Mhz)
-            // Don't need delay in host mode as host is in charge
+void __tusb_irq_path_func(_hw_endpoint_buffer_control_update32)(struct hw_endpoint *ep, uint32_t and_mask, uint32_t or_mask)
+{
+  uint32_t value = 0;
+
+  if ( and_mask )
+  {
+    value = *ep->buffer_control & and_mask;
+  }
+
+  if ( or_mask )
+  {
+    value |= or_mask;
+    if ( or_mask & USB_BUF_CTRL_AVAIL )
+    {
+      if ( *ep->buffer_control & USB_BUF_CTRL_AVAIL )
+      {
+        panic("ep %d %s was already available", tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
+      }
+      *ep->buffer_control = value & ~USB_BUF_CTRL_AVAIL;
+      // 12 cycle delay.. (should be good for 48*12Mhz = 576Mhz)
+      // Don't need delay in host mode as host is in charge
 #if !CFG_TUH_ENABLED
             __asm volatile (
                     "b 1f\n"
@@ -102,13 +124,14 @@ void _hw_endpoint_buffer_control_update32(struct hw_endpoint *ep, uint32_t and_m
                     "1:\n"
                     : : : "memory");
 #endif
-        }
     }
-    *ep->buffer_control = value;
+  }
+
+  *ep->buffer_control = value;
 }
 
 // prepare buffer, return buffer control
-static uint32_t prepare_ep_buffer(struct hw_endpoint *ep, uint8_t buf_id)
+static uint32_t __tusb_irq_path_func(prepare_ep_buffer)(struct hw_endpoint *ep, uint8_t buf_id)
 {
   uint16_t const buflen = tu_min16(ep->remaining_len, ep->wMaxPacketSize);
   ep->remaining_len = (uint16_t)(ep->remaining_len - buflen);
@@ -143,17 +166,21 @@ static uint32_t prepare_ep_buffer(struct hw_endpoint *ep, uint8_t buf_id)
 }
 
 // Prepare buffer control register value
-static void _hw_endpoint_start_next_buffer(struct hw_endpoint *ep)
+void __tusb_irq_path_func(hw_endpoint_start_next_buffer)(struct hw_endpoint *ep)
 {
   uint32_t ep_ctrl = *ep->endpoint_control;
 
   // always compute and start with buffer 0
   uint32_t buf_ctrl = prepare_ep_buffer(ep, 0) | USB_BUF_CTRL_SEL;
 
-  // For now: skip double buffered for Device mode, OUT endpoint since
+  // For now: skip double buffered for OUT endpoint in Device mode, since
   // host could send < 64 bytes and cause short packet on buffer0
-  // NOTE this could happen to Host mode IN endpoint
-  bool const force_single = !(usb_hw->main_ctrl & USB_MAIN_CTRL_HOST_NDEVICE_BITS) && !tu_edpt_dir(ep->ep_addr);
+  // NOTE: this could happen to Host mode IN endpoint
+  // Also, Host mode "interrupt" endpoint hardware is only single buffered,
+  // NOTE2: Currently Host bulk is implemented using "interrupt" endpoint
+  bool const is_host = is_host_mode();
+  bool const force_single = (!is_host && !tu_edpt_dir(ep->ep_addr)) ||
+                            (is_host && tu_edpt_number(ep->ep_addr) != 0);
 
   if(ep->remaining_len && !force_single)
   {
@@ -174,7 +201,7 @@ static void _hw_endpoint_start_next_buffer(struct hw_endpoint *ep)
 
   *ep->endpoint_control = ep_ctrl;
 
-  TU_LOG(3, "  Prepare BufCtrl: [0] = 0x%04u  [1] = 0x%04x\r\n", tu_u32_low16(buf_ctrl), tu_u32_high16(buf_ctrl));
+  TU_LOG(3, "  Prepare BufCtrl: [0] = 0x%04x  [1] = 0x%04x\r\n", tu_u32_low16(buf_ctrl), tu_u32_high16(buf_ctrl));
 
   // Finally, write to buffer_control which will trigger the transfer
   // the next time the controller polls this dpram address
@@ -183,7 +210,7 @@ static void _hw_endpoint_start_next_buffer(struct hw_endpoint *ep)
 
 void hw_endpoint_xfer_start(struct hw_endpoint *ep, uint8_t *buffer, uint16_t total_len)
 {
-  _hw_endpoint_lock_update(ep, 1);
+  hw_endpoint_lock_update(ep, 1);
 
   if ( ep->active )
   {
@@ -200,12 +227,24 @@ void hw_endpoint_xfer_start(struct hw_endpoint *ep, uint8_t *buffer, uint16_t to
   ep->active        = true;
   ep->user_buf      = buffer;
 
-  _hw_endpoint_start_next_buffer(ep);
-  _hw_endpoint_lock_update(ep, -1);
+  if ( e15_is_bulkin_ep(ep) )
+  {
+    usb_hw_set->inte = USB_INTS_DEV_SOF_BITS;
+  }
+
+  if ( e15_is_critical_frame_period(ep) )
+  {
+    ep->pending = 1;
+  } else
+  {
+    hw_endpoint_start_next_buffer(ep);
+  }
+
+  hw_endpoint_lock_update(ep, -1);
 }
 
 // sync endpoint buffer and return transferred bytes
-static uint16_t sync_ep_buffer(struct hw_endpoint *ep, uint8_t buf_id)
+static uint16_t __tusb_irq_path_func(sync_ep_buffer)(struct hw_endpoint *ep, uint8_t buf_id)
 {
   uint32_t buf_ctrl = _hw_endpoint_buffer_control_get_value32(ep);
   if (buf_id)  buf_ctrl = buf_ctrl >> 16;
@@ -241,13 +280,13 @@ static uint16_t sync_ep_buffer(struct hw_endpoint *ep, uint8_t buf_id)
   return xferred_bytes;
 }
 
-static void _hw_endpoint_xfer_sync (struct hw_endpoint *ep)
+static void __tusb_irq_path_func(_hw_endpoint_xfer_sync) (struct hw_endpoint *ep)
 {
   // Update hw endpoint struct with info from hardware
   // after a buff status interrupt
 
   uint32_t __unused buf_ctrl = _hw_endpoint_buffer_control_get_value32(ep);
-  TU_LOG(3, "  Sync BufCtrl: [0] = 0x%04u  [1] = 0x%04x\r\n", tu_u32_low16(buf_ctrl), tu_u32_high16(buf_ctrl));
+  TU_LOG(3, "  Sync BufCtrl: [0] = 0x%04x  [1] = 0x%04x\r\n", tu_u32_low16(buf_ctrl), tu_u32_high16(buf_ctrl));
 
   // always sync buffer 0
   uint16_t buf0_bytes = sync_ep_buffer(ep, 0);
@@ -285,16 +324,17 @@ static void _hw_endpoint_xfer_sync (struct hw_endpoint *ep)
       usb_hw->abort &= ~TU_BIT(ep_id);
 
       TU_LOG(3, "----SHORT PACKET buffer0 on EP %02X:\r\n", ep->ep_addr);
-      TU_LOG(3, "  BufCtrl: [0] = 0x%04u  [1] = 0x%04x\r\n", tu_u32_low16(buf_ctrl), tu_u32_high16(buf_ctrl));
+      TU_LOG(3, "  BufCtrl: [0] = 0x%04x  [1] = 0x%04x\r\n", tu_u32_low16(buf_ctrl), tu_u32_high16(buf_ctrl));
 #endif
     }
   }
 }
 
 // Returns true if transfer is complete
-bool hw_endpoint_xfer_continue(struct hw_endpoint *ep)
+bool __tusb_irq_path_func(hw_endpoint_xfer_continue)(struct hw_endpoint *ep)
 {
-  _hw_endpoint_lock_update(ep, 1);
+  hw_endpoint_lock_update(ep, 1);
+
   // Part way through a transfer
   if (!ep->active)
   {
@@ -311,17 +351,75 @@ bool hw_endpoint_xfer_continue(struct hw_endpoint *ep)
     pico_trace("Completed transfer of %d bytes on ep %d %s\n",
                ep->xferred_len, tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
     // Notify caller we are done so it can notify the tinyusb stack
-    _hw_endpoint_lock_update(ep, -1);
+    hw_endpoint_lock_update(ep, -1);
     return true;
   }
   else
   {
-    _hw_endpoint_start_next_buffer(ep);
+    if ( e15_is_critical_frame_period(ep) )
+    {
+      ep->pending = 1;
+    } else
+    {
+      hw_endpoint_start_next_buffer(ep);
+    }
   }
 
-  _hw_endpoint_lock_update(ep, -1);
+  hw_endpoint_lock_update(ep, -1);
   // More work to do
   return false;
 }
 
+//--------------------------------------------------------------------+
+// Errata 15
+//--------------------------------------------------------------------+
+
+#if TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
+
+/* Don't mark IN buffers as available during the last 200us of a full-speed
+   frame. This avoids a situation seen with the USB2.0 hub on a Raspberry
+   Pi 4 where a late IN token before the next full-speed SOF can cause port
+   babble and a corrupt ACK packet. The nature of the data corruption has a
+   chance to cause device lockup.
+
+   Use the next SOF to mark delayed buffers as available. This reduces
+   available Bulk IN bandwidth by approximately 20%, and requires that the
+   SOF interrupt is enabled while these transfers are ongoing.
+
+   Inherit the top-level enable from the corresponding Pico-SDK flag.
+   Applications that will not use the device in a situation where it could
+   be plugged into a Pi 4 or Pi 400 (for example, when directly connected
+   to a commodity hub or other host) can turn off the flag in the SDK.
+*/
+
+volatile uint32_t e15_last_sof = 0;
+
+// check if Errata 15 is needed for this endpoint i.e device bulk-in
+static bool __tusb_irq_path_func(e15_is_bulkin_ep) (struct hw_endpoint *ep)
+{
+  return (!is_host_mode() && tu_edpt_dir(ep->ep_addr) == TUSB_DIR_IN &&
+          ep->transfer_type == TUSB_XFER_BULK);
+}
+
+// check if we need to apply Errata 15 workaround : i.e
+// Endpoint is BULK IN and is currently in critical frame period i.e 20% of last usb frame
+static bool __tusb_irq_path_func(e15_is_critical_frame_period) (struct hw_endpoint *ep)
+{
+  TU_VERIFY(e15_is_bulkin_ep(ep));
+
+  /* Avoid the last 200us (uframe 6.5-7) of a frame, up to the EOF2 point.
+   * The device state machine cannot recover from receiving an incorrect PID
+   * when it is expecting an ACK.
+   */
+  uint32_t delta = time_us_32() - e15_last_sof;
+  if (delta < 800 || delta > 998) {
+    return false;
+  }
+  TU_LOG(3, "Avoiding sof %u now %lu last %lu\n", (usb_hw->sof_rd + 1) & USB_SOF_RD_BITS, time_us_32(), e15_last_sof);
+  return true;
+}
+
+#endif
+
+
 #endif
diff --git a/src/portable/raspberrypi/rp2040/rp2040_usb.h b/src/portable/raspberrypi/rp2040/rp2040_usb.h
index da1933ddd..a06407f23 100644
--- a/src/portable/raspberrypi/rp2040/rp2040_usb.h
+++ b/src/portable/raspberrypi/rp2040/rp2040_usb.h
@@ -11,11 +11,33 @@
 #include "hardware/structs/usb.h"
 #include "hardware/irq.h"
 #include "hardware/resets.h"
+#include "hardware/timer.h"
 
 #if defined(PICO_RP2040_USB_DEVICE_ENUMERATION_FIX) && !defined(TUD_OPT_RP2040_USB_DEVICE_ENUMERATION_FIX)
 #define TUD_OPT_RP2040_USB_DEVICE_ENUMERATION_FIX PICO_RP2040_USB_DEVICE_ENUMERATION_FIX
 #endif
 
+#if defined(PICO_RP2040_USB_DEVICE_UFRAME_FIX) && !defined(TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX)
+#define TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX PICO_RP2040_USB_DEVICE_UFRAME_FIX
+#endif
+
+#if TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
+#undef PICO_RP2040_USB_FAST_IRQ
+#define PICO_RP2040_USB_FAST_IRQ 1
+#endif
+
+#ifndef PICO_RP2040_USB_FAST_IRQ
+#define PICO_RP2040_USB_FAST_IRQ 0
+#endif
+
+#if PICO_RP2040_USB_FAST_IRQ
+#define __tusb_irq_path_func(x) __no_inline_not_in_flash_func(x)
+#else
+#define __tusb_irq_path_func(x) x
+#endif
+
+#define usb_hw_set hw_set_alias(usb_hw)
+#define usb_hw_clear hw_clear_alias(usb_hw)
 
 #define pico_info(...)  TU_LOG(2, __VA_ARGS__)
 #define pico_trace(...) TU_LOG(3, __VA_ARGS__)
@@ -29,7 +51,7 @@ typedef struct hw_endpoint
     // Transfer direction (i.e. IN is rx for host but tx for device)
     // allows us to common up transfer functions
     bool rx;
-    
+
     uint8_t ep_addr;
     uint8_t next_pid;
 
@@ -42,20 +64,25 @@ typedef struct hw_endpoint
     // Buffer pointer in usb dpram
     uint8_t *hw_data_buf;
 
+    // User buffer in main memory
+    uint8_t *user_buf;
+
     // Current transfer information
-    bool active;
     uint16_t remaining_len;
     uint16_t xferred_len;
 
-    // User buffer in main memory
-    uint8_t *user_buf;
-
     // Data needed from EP descriptor
     uint16_t wMaxPacketSize;
 
+    // Endpoint is in use
+    bool active;
+
     // Interrupt, bulk, etc
     uint8_t transfer_type;
-    
+
+    // Transfer scheduled but not active
+    uint8_t pending;
+
 #if CFG_TUH_ENABLED
     // Only needed for host
     uint8_t dev_addr;
@@ -63,32 +90,52 @@ typedef struct hw_endpoint
     // If interrupt endpoint
     uint8_t interrupt_num;
 #endif
+
 } hw_endpoint_t;
 
+#if TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
+extern volatile uint32_t e15_last_sof;
+#endif
+
 void rp2040_usb_init(void);
 
 void hw_endpoint_xfer_start(struct hw_endpoint *ep, uint8_t *buffer, uint16_t total_len);
 bool hw_endpoint_xfer_continue(struct hw_endpoint *ep);
 void hw_endpoint_reset_transfer(struct hw_endpoint *ep);
+void hw_endpoint_start_next_buffer(struct hw_endpoint *ep);
+
+TU_ATTR_ALWAYS_INLINE static inline void hw_endpoint_lock_update(__unused struct hw_endpoint * ep, __unused int delta) {
+  // todo add critsec as necessary to prevent issues between worker and IRQ...
+  //  note that this is perhaps as simple as disabling IRQs because it would make
+  //  sense to have worker and IRQ on same core, however I think using critsec is about equivalent.
+}
 
 void _hw_endpoint_buffer_control_update32(struct hw_endpoint *ep, uint32_t and_mask, uint32_t or_mask);
-static inline uint32_t _hw_endpoint_buffer_control_get_value32(struct hw_endpoint *ep) {
-    return *ep->buffer_control;
+
+TU_ATTR_ALWAYS_INLINE static inline uint32_t _hw_endpoint_buffer_control_get_value32 (struct hw_endpoint *ep)
+{
+  return *ep->buffer_control;
 }
-static inline void _hw_endpoint_buffer_control_set_value32(struct hw_endpoint *ep, uint32_t value) {
-    return _hw_endpoint_buffer_control_update32(ep, 0, value);
+
+TU_ATTR_ALWAYS_INLINE static inline void _hw_endpoint_buffer_control_set_value32 (struct hw_endpoint *ep, uint32_t value)
+{
+  return _hw_endpoint_buffer_control_update32(ep, 0, value);
 }
-static inline void _hw_endpoint_buffer_control_set_mask32(struct hw_endpoint *ep, uint32_t value) {
-    return _hw_endpoint_buffer_control_update32(ep, ~value, value);
+
+TU_ATTR_ALWAYS_INLINE static inline void _hw_endpoint_buffer_control_set_mask32 (struct hw_endpoint *ep, uint32_t value)
+{
+  return _hw_endpoint_buffer_control_update32(ep, ~value, value);
 }
-static inline void _hw_endpoint_buffer_control_clear_mask32(struct hw_endpoint *ep, uint32_t value) {
-    return _hw_endpoint_buffer_control_update32(ep, ~value, 0);
+
+TU_ATTR_ALWAYS_INLINE static inline void _hw_endpoint_buffer_control_clear_mask32 (struct hw_endpoint *ep, uint32_t value)
+{
+  return _hw_endpoint_buffer_control_update32(ep, ~value, 0);
 }
 
-static inline uintptr_t hw_data_offset(uint8_t *buf)
+static inline uintptr_t hw_data_offset (uint8_t *buf)
 {
-    // Remove usb base from buffer pointer
-    return (uintptr_t)buf ^ (uintptr_t)usb_dpram;
+  // Remove usb base from buffer pointer
+  return (uintptr_t) buf ^ (uintptr_t) usb_dpram;
 }
 
 extern const char *ep_dir_string[];
diff --git a/src/portable/renesas/link/hcd_link.c b/src/portable/renesas/link/hcd_link.c
index f1ea2b821..a10fe54ea 100644
--- a/src/portable/renesas/link/hcd_link.c
+++ b/src/portable/renesas/link/hcd_link.c
@@ -1,4 +1,4 @@
-/*
+/* 
  * The MIT License (MIT)
  *
  * Copyright (c) 2021 Koji Kitayama
@@ -27,37 +27,86 @@
 
 #include "tusb_option.h"
 
-#if CFG_TUH_ENABLED && (CFG_TUSB_MCU == OPT_MCU_RX63X || \
-                        CFG_TUSB_MCU == OPT_MCU_RX65X || \
-                        CFG_TUSB_MCU == OPT_MCU_RX72N || \
-                        CFG_TUSB_MCU == OPT_MCU_RAXXX)
-
+#if CFG_TUH_ENABLED && ( CFG_TUSB_MCU == OPT_MCU_RX63X || \
+                               CFG_TUSB_MCU == OPT_MCU_RX65X || \
+                               CFG_TUSB_MCU == OPT_MCU_RX72N )
 #include "host/hcd.h"
-#include "link_type.h"
-
-#if TU_CHECK_MCU(OPT_MCU_RX63X, OPT_MCU_RX65X, OPT_MCU_RX72N)
-#include "link_rx.h"
-#elif TU_CHECK_MCU(OPT_MCU_RAXXX)
-#include "link_ra.h"
-#else
-#error "Unsupported MCU"
-#endif
+#include "iodefine.h"
 
 //--------------------------------------------------------------------+
 // MACRO TYPEDEF CONSTANT ENUM DECLARATION
 //--------------------------------------------------------------------+
+#define SYSTEM_PRCR_PRC1     (1<<1)
+#define SYSTEM_PRCR_PRKEY    (0xA5u<<8)
 
-/* LINK core registers */
-#if defined(__CCRX__)
-#define LINK_REG ((LINK_REG_t __evenaccess*)LINK_REG_BASE)
-#else
-#define LINK_REG ((LINK_REG_t*)LINK_REG_BASE)
-#endif
+#define USB_DVSTCTR0_LOW     (1u)
+#define USB_DVSTCTR0_FULL    (2u)
+
+#define USB_FIFOSEL_TX       ((uint16_t)(1u<<5))
+#define USB_FIFOSEL_BIGEND   ((uint16_t)(1u<<8))
+#define USB_FIFOSEL_MBW_8    ((uint16_t)(0u<<10))
+#define USB_FIFOSEL_MBW_16   ((uint16_t)(1u<<10))
+#define USB_IS0_CTSQ         ((uint16_t)(7u))
+#define USB_IS0_DVSQ         ((uint16_t)(7u<<4))
+#define USB_IS0_VALID        ((uint16_t)(1u<<3))
+#define USB_IS0_BRDY         ((uint16_t)(1u<<8))
+#define USB_IS0_NRDY         ((uint16_t)(1u<<9))
+#define USB_IS0_BEMP         ((uint16_t)(1u<<10))
+#define USB_IS0_CTRT         ((uint16_t)(1u<<11))
+#define USB_IS0_DVST         ((uint16_t)(1u<<12))
+#define USB_IS0_SOFR         ((uint16_t)(1u<<13))
+#define USB_IS0_RESM         ((uint16_t)(1u<<14))
+#define USB_IS0_VBINT        ((uint16_t)(1u<<15))
+#define USB_IS1_SACK         ((uint16_t)(1u<<4))
+#define USB_IS1_SIGN         ((uint16_t)(1u<<5))
+#define USB_IS1_EOFERR       ((uint16_t)(1u<<6))
+#define USB_IS1_ATTCH        ((uint16_t)(1u<<11))
+#define USB_IS1_DTCH         ((uint16_t)(1u<<12))
+#define USB_IS1_BCHG         ((uint16_t)(1u<<14))
+#define USB_IS1_OVRCR        ((uint16_t)(1u<<15))
+
+#define USB_IS0_CTSQ_MSK     (7u)
+#define USB_IS0_CTSQ_SETUP   (1u)
+#define USB_IS0_DVSQ_DEF     (1u<<4)
+#define USB_IS0_DVSQ_ADDR    (2u<<4)
+#define USB_IS0_DVSQ_SUSP0   (4u<<4)
+#define USB_IS0_DVSQ_SUSP1   (5u<<4)
+#define USB_IS0_DVSQ_SUSP2   (6u<<4)
+#define USB_IS0_DVSQ_SUSP3   (7u<<4)
+
+#define USB_PIPECTR_PID_MSK   (3u)
+#define USB_PIPECTR_PID_NAK   (0u)
+#define USB_PIPECTR_PID_BUF   (1u)
+#define USB_PIPECTR_PID_STALL (2u)
+#define USB_PIPECTR_CCPL      (1u<<2)
+#define USB_PIPECTR_SQMON     (1u<<6)
+#define USB_PIPECTR_SQCLR     (1u<<8)
+#define USB_PIPECTR_ACLRM     (1u<<9)
+#define USB_PIPECTR_INBUFM    (1u<<14)
+#define USB_PIPECTR_BSTS      (1u<<15)
+
+#define USB_FIFOCTR_DTLN     (0x1FF)
+#define USB_FIFOCTR_FRDY     (1u<<13)
+#define USB_FIFOCTR_BCLR     (1u<<14)
+#define USB_FIFOCTR_BVAL     (1u<<15)
+
+#define USB_PIPECFG_SHTNAK   (1u<<7)
+#define USB_PIPECFG_DBLB     (1u<<9)
+#define USB_PIPECFG_BULK     (1u<<14)
+#define USB_PIPECFG_ISO      (3u<<14)
+#define USB_PIPECFG_INT      (2u<<14)
 
+#define USB_DEVADD_LOW       (1u<<6)
+#define USB_DEVADD_FULL      (2u<<6)
+
+#define FIFO_REQ_CLR         (1u)
+#define FIFO_COMPLETE        (1u<<1)
+
+// Start of definition of packed structs (used by the CCRX toolchain)
 TU_ATTR_PACKED_BEGIN
 TU_ATTR_BIT_FIELD_ORDER_BEGIN
 
-typedef struct TU_ATTR_PACKED {
+typedef struct {
   union {
     struct {
       uint16_t      : 8;
@@ -70,7 +119,7 @@ typedef struct TU_ATTR_PACKED {
   uint16_t TRN;
 } reg_pipetre_t;
 
-typedef union TU_ATTR_PACKED {
+typedef union {
   struct {
     volatile uint16_t u8: 8;
     volatile uint16_t   : 0;
@@ -78,7 +127,8 @@ typedef union TU_ATTR_PACKED {
   volatile uint16_t u16;
 } hw_fifo_t;
 
-typedef struct TU_ATTR_PACKED {
+typedef struct TU_ATTR_PACKED
+{
   void      *buf;      /* the start address of a transfer data buffer */
   uint16_t  length;    /* the number of bytes in the buffer */
   uint16_t  remaining; /* the number of bytes remaining in the buffer */
@@ -106,6 +156,28 @@ typedef struct
 //--------------------------------------------------------------------+
 static hcd_data_t _hcd;
 
+static uint32_t disable_interrupt(void)
+{
+  uint32_t pswi;
+#if defined(__CCRX__)
+  pswi = get_psw() & 0x010000;
+  clrpsw_i();
+#else
+  pswi = __builtin_rx_mvfc(0) & 0x010000;
+  __builtin_rx_clrpsw('I');
+#endif
+  return pswi;
+}
+
+static void enable_interrupt(uint32_t pswi)
+{
+#if defined(__CCRX__)
+  set_psw(get_psw() | pswi);
+#else
+  __builtin_rx_mvtc(0, __builtin_rx_mvfc(0) | pswi);
+#endif
+}
+
 static unsigned find_pipe(unsigned xfer)
 {
   switch (xfer) {
@@ -136,49 +208,58 @@ static unsigned find_pipe(unsigned xfer)
 
 static volatile uint16_t* get_pipectr(unsigned num)
 {
+  volatile uint16_t *ctr = NULL;
   if (num) {
-    return (volatile uint16_t*)&(LINK_REG->PIPE_CTR[num - 1]);
+    ctr = (volatile uint16_t*)&USB0.PIPE1CTR.WORD;
+    ctr += num - 1;
   } else {
-    return (volatile uint16_t*)&(LINK_REG->DCPCTR);
+    ctr = (volatile uint16_t*)&USB0.DCPCTR.WORD;
   }
+  return ctr;
 }
 
 static volatile reg_pipetre_t* get_pipetre(unsigned num)
 {
   volatile reg_pipetre_t* tre = NULL;
   if ((1 <= num) && (num <= 5)) {
-    tre = (volatile reg_pipetre_t*)&(LINK_REG->PIPE_TR[num - 1].E);
+    tre = (volatile reg_pipetre_t*)&USB0.PIPE1TRE.WORD;
+    tre += num - 1;
   }
   return tre;
 }
 
 static volatile uint16_t* addr_to_pipectr(uint8_t dev_addr, unsigned ep_addr)
 {
-  const unsigned epn = tu_edpt_number(ep_addr);
+  volatile uint16_t *ctr = NULL;
+  const unsigned epn   = tu_edpt_number(ep_addr);
   if (epn) {
     const unsigned dir_in = tu_edpt_dir(ep_addr);
-    const unsigned num = _hcd.ep[dev_addr][dir_in][epn - 1];
-    return get_pipectr(num);
+    const unsigned num    = _hcd.ep[dev_addr][dir_in][epn - 1];
+    if (num) {
+      ctr = (volatile uint16_t*)&USB0.PIPE1CTR.WORD;
+      ctr += num - 1;
+    }
   } else {
-    return get_pipectr(0);
+    ctr = (volatile uint16_t*)&USB0.DCPCTR.WORD;
   }
+  return ctr;
 }
 
 static unsigned edpt0_max_packet_size(void)
 {
-  return LINK_REG->DCPMAXP_b.MXPS;
+  return USB0.DCPMAXP.BIT.MXPS;
 }
 
 static unsigned edpt_max_packet_size(unsigned num)
 {
-  LINK_REG->PIPESEL = num;
-  return LINK_REG->PIPEMAXP_b.MXPS;
+  USB0.PIPESEL.WORD = num;
+  return USB0.PIPEMAXP.BIT.MXPS;
 }
 
 static inline void pipe_wait_for_ready(unsigned num)
 {
-  while (LINK_REG->D0FIFOSEL_b.CURPIPE != num) ;
-  while (!LINK_REG->D0FIFOCTR_b.FRDY) ;
+  while (USB0.D0FIFOSEL.BIT.CURPIPE != num) ;
+  while (!USB0.D0FIFOCTR.BIT.FRDY) ;
 }
 
 static void pipe_write_packet(void *buf, volatile void *fifo, unsigned len)
@@ -209,22 +290,21 @@ static bool pipe0_xfer_in(void)
   const unsigned rem = pipe->remaining;
 
   const unsigned mps = edpt0_max_packet_size();
-  const unsigned vld = LINK_REG->CFIFOCTR_b.DTLN;
+  const unsigned vld = USB0.CFIFOCTR.BIT.DTLN;
   const unsigned len = TU_MIN(TU_MIN(rem, mps), vld);
   void          *buf = pipe->buf;
   if (len) {
-    LINK_REG->DCPCTR = LINK_REG_PIPE_CTR_PID_NAK;
-    pipe_read_packet(buf, (volatile void*)&LINK_REG->CFIFO, len);
+    USB0.DCPCTR.WORD = USB_PIPECTR_PID_NAK;
+    pipe_read_packet(buf, (volatile void*)&USB0.CFIFO.WORD, len);
     pipe->buf = (uint8_t*)buf + len;
   }
-  if (len < mps)
-    LINK_REG->CFIFOCTR = LINK_REG_CFIFOCTR_BCLR_Msk;
+  if (len < mps) USB0.CFIFOCTR.WORD = USB_FIFOCTR_BCLR;
   pipe->remaining = rem - len;
   if ((len < mps) || (rem == len)) {
     pipe->buf = NULL;
     return true;
   }
-  LINK_REG->DCPCTR = LINK_REG_PIPE_CTR_PID_BUF;
+  USB0.DCPCTR.WORD = USB_PIPECTR_PID_BUF;
   return false;
 }
 
@@ -240,11 +320,10 @@ static bool pipe0_xfer_out(void)
   const unsigned len = TU_MIN(mps, rem);
   void          *buf = pipe->buf;
   if (len) {
-    pipe_write_packet(buf, (volatile void*)&LINK_REG->CFIFO, len);
+    pipe_write_packet(buf, (volatile void*)&USB0.CFIFO.WORD, len);
     pipe->buf = (uint8_t*)buf + len;
   }
-  if (len < mps)
-    LINK_REG->CFIFOCTR = LINK_REG_CFIFOCTR_BVAL_Msk;
+  if (len < mps) USB0.CFIFOCTR.WORD = USB_FIFOCTR_BVAL;
   pipe->remaining = rem - len;
   return false;
 }
@@ -254,21 +333,20 @@ static bool pipe_xfer_in(unsigned num)
   pipe_state_t  *pipe = &_hcd.pipe[num];
   const unsigned rem  = pipe->remaining;
 
-  LINK_REG->D0FIFOSEL = num | LINK_REG_FIFOSEL_MBW_8BIT;
+  USB0.D0FIFOSEL.WORD = num | USB_FIFOSEL_MBW_8;
   const unsigned mps  = edpt_max_packet_size(num);
   pipe_wait_for_ready(num);
-  const unsigned vld  = LINK_REG->D0FIFOCTR_b.DTLN;
+  const unsigned vld  = USB0.D0FIFOCTR.BIT.DTLN;
   const unsigned len  = TU_MIN(TU_MIN(rem, mps), vld);
   void          *buf  = pipe->buf;
   if (len) {
-    pipe_read_packet(buf, (volatile void*)&LINK_REG->D0FIFO, len);
+    pipe_read_packet(buf, (volatile void*)&USB0.D0FIFO.WORD, len);
     pipe->buf = (uint8_t*)buf + len;
   }
-  if (len < mps)
-    LINK_REG->D0FIFOCTR = LINK_REG_CFIFOCTR_BCLR_Msk;
-  LINK_REG->D0FIFOSEL = 0;
-  while (LINK_REG->D0FIFOSEL_b.CURPIPE) ; /* if CURPIPE bits changes, check written value */
-  pipe->remaining = rem - len;
+  if (len < mps) USB0.D0FIFOCTR.WORD = USB_FIFOCTR_BCLR;
+  USB0.D0FIFOSEL.WORD = 0;
+  while (USB0.D0FIFOSEL.BIT.CURPIPE) ; /* if CURPIPE bits changes, check written value */
+  pipe->remaining     = rem - len;
   if ((len < mps) || (rem == len)) {
     pipe->buf = NULL;
     return NULL != buf;
@@ -286,19 +364,18 @@ static bool pipe_xfer_out(unsigned num)
     return true;
   }
 
-  LINK_REG->D0FIFOSEL = num | LINK_REG_FIFOSEL_MBW_16BIT | (TU_BYTE_ORDER == TU_BIG_ENDIAN ? LINK_REG_FIFOSEL_BIGEND : 0);
+  USB0.D0FIFOSEL.WORD = num | USB_FIFOSEL_MBW_16 | (TU_BYTE_ORDER == TU_BIG_ENDIAN ? USB_FIFOSEL_BIGEND : 0);
   const unsigned mps  = edpt_max_packet_size(num);
   pipe_wait_for_ready(num);
   const unsigned len  = TU_MIN(rem, mps);
   void          *buf  = pipe->buf;
   if (len) {
-    pipe_write_packet(buf, (volatile void*)&LINK_REG->D0FIFO, len);
+    pipe_write_packet(buf, (volatile void*)&USB0.D0FIFO.WORD, len);
     pipe->buf = (uint8_t*)buf + len;
   }
-  if (len < mps)
-    LINK_REG->D0FIFOCTR = LINK_REG_CFIFOCTR_BVAL_Msk;
-  LINK_REG->D0FIFOSEL = 0;
-  while (LINK_REG->D0FIFOSEL_b.CURPIPE) ; /* if CURPIPE bits changes, check written value */
+  if (len < mps) USB0.D0FIFOCTR.WORD = USB_FIFOCTR_BVAL;
+  USB0.D0FIFOSEL.WORD = 0;
+  while (USB0.D0FIFOSEL.BIT.CURPIPE) ; /* if CURPIPE bits changes, check written value */
   pipe->remaining = rem - len;
   return false;
 }
@@ -310,12 +387,11 @@ static bool process_pipe0_xfer(uint8_t dev_addr, uint8_t ep_addr, void* buffer,
 
   /* configure fifo direction and access unit settings */
   if (dir_in) { /* IN, a byte */
-    LINK_REG->CFIFOSEL = LINK_REG_FIFOSEL_MBW_8BIT;
-    while (LINK_REG->CFIFOSEL & LINK_REG_CFIFOSEL_ISEL_WRITE) ;
-  } else { /* OUT, 2 bytes */
-    LINK_REG->CFIFOSEL = LINK_REG_CFIFOSEL_ISEL_WRITE | LINK_REG_FIFOSEL_MBW_16BIT |
-             (TU_BYTE_ORDER == TU_BIG_ENDIAN ? LINK_REG_FIFOSEL_BIGEND : 0);
-    while (!(LINK_REG->CFIFOSEL & LINK_REG_CFIFOSEL_ISEL_WRITE)) ;
+    USB0.CFIFOSEL.WORD  = USB_FIFOSEL_MBW_8;
+    while (USB0.CFIFOSEL.WORD & USB_FIFOSEL_TX) ;
+  } else {       /* OUT, 2 bytes */
+    USB0.CFIFOSEL.WORD  = USB_FIFOSEL_TX | USB_FIFOSEL_MBW_16 | (TU_BYTE_ORDER == TU_BIG_ENDIAN ? USB_FIFOSEL_BIGEND : 0);
+    while (!(USB0.CFIFOSEL.WORD & USB_FIFOSEL_TX)) ;
   }
 
   pipe_state_t *pipe = &_hcd.pipe[0];
@@ -325,25 +401,25 @@ static bool process_pipe0_xfer(uint8_t dev_addr, uint8_t ep_addr, void* buffer,
   if (buflen) {
     pipe->buf     = buffer;
     if (!dir_in) { /* OUT */
-      TU_ASSERT(LINK_REG->DCPCTR_b.BSTS && (LINK_REG->USBREQ & 0x80));
+      TU_ASSERT(USB0.DCPCTR.BIT.BSTS && (USB0.USBREQ.WORD & 0x80));
       pipe0_xfer_out();
     }
   } else { /* ZLP */
     pipe->buf        = NULL;
     if (!dir_in) { /* OUT */
-      LINK_REG->CFIFOCTR = LINK_REG_CFIFOCTR_BVAL_Msk;
+      USB0.CFIFOCTR.WORD = USB_FIFOCTR_BVAL;
     }
-    if (dir_in == LINK_REG->DCPCFG_b.DIR) {
-      TU_ASSERT(LINK_REG_PIPE_CTR_PID_NAK == LINK_REG->DCPCTR_b.PID);
-      LINK_REG->DCPCTR_b.SQSET = 1;
-      LINK_REG->DCPCFG_b.DIR = dir_in ^ 1;
+    if (dir_in == USB0.DCPCFG.BIT.DIR) {
+      TU_ASSERT(USB_PIPECTR_PID_NAK == USB0.DCPCTR.BIT.PID);
+      USB0.DCPCTR.BIT.SQSET = 1;
+      USB0.DCPCFG.BIT.DIR = dir_in ^ 1;
     }
   }
-  LINK_REG->DCPCTR = LINK_REG_PIPE_CTR_PID_BUF;
+  USB0.DCPCTR.WORD = USB_PIPECTR_PID_BUF;
   return true;
 }
 
-static bool process_pipe_xfer(uint8_t dev_addr, uint8_t ep_addr, void *buffer, uint16_t buflen)
+static bool process_pipe_xfer(uint8_t dev_addr, uint8_t ep_addr, void* buffer, uint16_t buflen)
 {
   const unsigned epn    = tu_edpt_number(ep_addr);
   const unsigned dir_in = tu_edpt_dir(ep_addr);
@@ -359,23 +435,23 @@ static bool process_pipe_xfer(uint8_t dev_addr, uint8_t ep_addr, void *buffer, u
     if (buflen) {
       pipe_xfer_out(num);
     } else { /* ZLP */
-      LINK_REG->D0FIFOSEL = num;
+      USB0.D0FIFOSEL.WORD = num;
       pipe_wait_for_ready(num);
-      LINK_REG->D0FIFOCTR = LINK_REG_CFIFOCTR_BVAL_Msk;
-      LINK_REG->D0FIFOSEL = 0;
-      while (LINK_REG->D0FIFOSEL_b.CURPIPE) continue; /* if CURPIPE bits changes, check written value */
+      USB0.D0FIFOCTR.WORD = USB_FIFOCTR_BVAL;
+      USB0.D0FIFOSEL.WORD = 0;
+      while (USB0.D0FIFOSEL.BIT.CURPIPE) ; /* if CURPIPE bits changes, check written value */
     }
   } else {
     volatile uint16_t     *ctr = get_pipectr(num);
     volatile reg_pipetre_t *pt = get_pipetre(num);
     if (pt) {
       const unsigned     mps = edpt_max_packet_size(num);
-      if (*ctr & 0x3) *ctr = LINK_REG_PIPE_CTR_PID_NAK;
+      if (*ctr & 0x3) *ctr = USB_PIPECTR_PID_NAK;
       pt->TRE   = TU_BIT(8);
       pt->TRN   = (buflen + mps - 1) / mps;
       pt->TRENB = 1;
     }
-    *ctr = LINK_REG_PIPE_CTR_PID_BUF;
+    *ctr = USB_PIPECTR_PID_BUF;
   }
   return true;
 }
@@ -409,10 +485,10 @@ static void process_pipe_nrdy(uint8_t rhport, unsigned num)
   unsigned result;
   uint16_t volatile *ctr = get_pipectr(num);
   // TU_LOG1("NRDY %d %x\n", num, *ctr);
-  switch (*ctr & LINK_REG_PIPE_CTR_PID_Msk) {
+  switch (*ctr & USB_PIPECTR_PID_MSK) {
     default: return;
-    case LINK_REG_PIPE_CTR_PID_STALL: result = XFER_RESULT_STALLED; break;
-    case LINK_REG_PIPE_CTR_PID_NAK:   result = XFER_RESULT_FAILED;  break;
+    case USB_PIPECTR_PID_STALL: result = XFER_RESULT_STALLED; break;
+    case USB_PIPECTR_PID_NAK:   result = XFER_RESULT_FAILED;  break;
   }
   pipe_state_t *pipe = &_hcd.pipe[num];
   hcd_event_xfer_complete(pipe->dev, pipe->ep,
@@ -444,60 +520,78 @@ static void process_pipe_brdy(uint8_t rhport, unsigned num)
   }
 }
 
+
 /*------------------------------------------------------------------*/
 /* Host API
  *------------------------------------------------------------------*/
 bool hcd_init(uint8_t rhport)
 {
   (void)rhport;
+  /* Enable USB0 */
+  uint32_t pswi = disable_interrupt();
+  SYSTEM.PRCR.WORD = SYSTEM_PRCR_PRKEY | SYSTEM_PRCR_PRC1;
+  MSTP(USB0) = 0;
+  SYSTEM.PRCR.WORD = SYSTEM_PRCR_PRKEY;
+  enable_interrupt(pswi);
+  USB0.SYSCFG.BIT.SCKE = 1;
+  while (!USB0.SYSCFG.BIT.SCKE) ;
+  USB0.SYSCFG.BIT.DPRPU = 0;
+  USB0.SYSCFG.BIT.DRPD = 0;
+  USB0.SYSCFG.BIT.DCFM = 1;
 
-  LINK_REG->SYSCFG_b.SCKE = 1;
-  while (!LINK_REG->SYSCFG_b.SCKE) ;
-  LINK_REG->SYSCFG_b.DPRPU = 0;
-  LINK_REG->SYSCFG_b.DRPD = 0;
-  LINK_REG->SYSCFG_b.DCFM = 1;
+  USB0.DVSTCTR0.BIT.VBUSEN = 1;
 
-  LINK_REG->DVSTCTR0_b.VBUSEN = 1;
-
-  LINK_REG->SYSCFG_b.DRPD = 1;
+  USB0.SYSCFG.BIT.DRPD = 1;
   for (volatile int i = 0; i < 30000; ++i) ;
-  LINK_REG->SYSCFG_b.USBE = 1;
-
-  // MCU specific PHY init
-  link_phy_init();
+  USB0.SYSCFG.BIT.USBE = 1;
 
-  LINK_REG->PHYSLEW = 0x5;
-  LINK_REG->DPUSR0R_FS_b.FIXPHY0 = 0u; /* Transceiver Output fixed */
+  USB.DPUSR0R.BIT.FIXPHY0 = 0u;    /* USB0 Transceiver Output fixed */
+#if ( CFG_TUSB_MCU == OPT_MCU_RX72N )
+  USB0.PHYSLEW.LONG = 0x5;
+  IR(PERIB, INTB185) = 0;
+#else
+  IR(USB0, USBI0)   = 0;
+#endif
 
   /* Setup default control pipe */
-  LINK_REG->DCPCFG = LINK_REG_PIPECFG_SHTNAK_Msk;
-  LINK_REG->DCPMAXP = 64;
-  LINK_REG->INTENB0 = LINK_REG_INTSTS0_BRDY_Msk | LINK_REG_INTSTS0_NRDY_Msk | LINK_REG_INTSTS0_BEMP_Msk;
-  LINK_REG->INTENB1 = LINK_REG_INTSTS1_SACK_Msk | LINK_REG_INTSTS1_SIGN_Msk | LINK_REG_INTSTS1_ATTCH_Msk | LINK_REG_INTSTS1_DTCH_Msk;
-  LINK_REG->BEMPENB = 1;
-  LINK_REG->NRDYENB = 1;
-  LINK_REG->BRDYENB = 1;
-
+  USB0.DCPCFG.WORD  = USB_PIPECFG_SHTNAK;
+  USB0.DCPMAXP.WORD = 64;
+  USB0.INTENB0.WORD = USB_IS0_BRDY | USB_IS0_NRDY | USB_IS0_BEMP;
+  USB0.INTENB1.WORD = USB_IS1_SACK | USB_IS1_SIGN |
+    USB_IS1_ATTCH | USB_IS1_DTCH;
+  USB0.BEMPENB.WORD = 1;
+  USB0.NRDYENB.WORD = 1;
+  USB0.BRDYENB.WORD = 1;
   return true;
 }
 
 void hcd_int_enable(uint8_t rhport)
 {
-  link_int_enable(rhport);
+  (void)rhport;
+#if ( CFG_TUSB_MCU == OPT_MCU_RX72N )
+  IEN(PERIB, INTB185) = 1;
+#else
+  IEN(USB0, USBI0) = 1;
+#endif
 }
 
 void hcd_int_disable(uint8_t rhport)
 {
-  link_int_disable(rhport);
+  (void)rhport;
+#if ( CFG_TUSB_MCU == OPT_MCU_RX72N )
+  IEN(PERIB, INTB185) = 0;
+#else
+  IEN(USB0, USBI0) = 0;
+#endif
 }
 
 uint32_t hcd_frame_number(uint8_t rhport)
 {
   (void)rhport;
-  /* The device must be reset at least once after connection
+  /* The device must be reset at least once after connection 
    * in order to start the frame counter. */
   if (_hcd.need_reset) hcd_port_reset(rhport);
-  return LINK_REG->FRMNUM_b.FRNM;
+  return USB0.FRMNUM.BIT.FRNM;
 }
 
 /*--------------------------------------------------------------------+
@@ -511,18 +605,18 @@ bool hcd_port_connect_status(uint8_t rhport)
 
 void hcd_port_reset(uint8_t rhport)
 {
-  LINK_REG->DCPCTR = LINK_REG_PIPE_CTR_PID_NAK;
-  while (LINK_REG->DCPCTR_b.PBUSY) ;
+  USB0.DCPCTR.WORD = USB_PIPECTR_PID_NAK;
+  while (USB0.DCPCTR.BIT.PBUSY) ;
   hcd_int_disable(rhport);
-  LINK_REG->DVSTCTR0_b.UACT = 0;
-  if (LINK_REG->DCPCTR_b.SUREQ)
-    LINK_REG->DCPCTR_b.SUREQCLR = 1;
+  USB0.DVSTCTR0.BIT.UACT   = 0;
+  if (USB0.DCPCTR.BIT.SUREQ)
+    USB0.DCPCTR.BIT.SUREQCLR = 1;
   hcd_int_enable(rhport);
   /* Reset should be asserted 10-20ms. */
-  LINK_REG->DVSTCTR0_b.USBRST = 1;
+  USB0.DVSTCTR0.BIT.USBRST = 1;
   for (volatile int i = 0; i < 2400000; ++i) ;
-  LINK_REG->DVSTCTR0_b.USBRST = 0;
-  LINK_REG->DVSTCTR0_b.UACT = 1;
+  USB0.DVSTCTR0.BIT.USBRST = 0;
+  USB0.DVSTCTR0.BIT.UACT   = 1;
   _hcd.need_reset = false;
 }
 
@@ -534,10 +628,10 @@ void hcd_port_reset_end(uint8_t rhport)
 tusb_speed_t hcd_port_speed_get(uint8_t rhport)
 {
   (void)rhport;
-  switch (LINK_REG->DVSTCTR0_b.RHST) {
+  switch (USB0.DVSTCTR0.BIT.RHST) {
     default: return TUSB_SPEED_INVALID;
-    case LINK_REG_DVSTCTR0_RHST_FS: return TUSB_SPEED_FULL;
-    case LINK_REG_DVSTCTR0_RHST_LS:  return TUSB_SPEED_LOW;
+    case USB_DVSTCTR0_FULL: return TUSB_SPEED_FULL;
+    case USB_DVSTCTR0_LOW:  return TUSB_SPEED_LOW;
   }
 }
 
@@ -553,13 +647,13 @@ void hcd_device_close(uint8_t rhport, uint8_t dev_addr)
     unsigned num = *ep;
     if (!num || dev_addr != _hcd.pipe[num].dev) continue;
 
-    ctr = (uint16_t volatile*)&LINK_REG->PIPE_CTR[num - 1];
+    ctr  = (uint16_t volatile*)&USB0.PIPE1CTR.WORD + num - 1;
     *ctr = 0;
-    LINK_REG->NRDYENB &= ~TU_BIT(num);
-    LINK_REG->BRDYENB &= ~TU_BIT(num);
-    LINK_REG->PIPESEL = num;
-    LINK_REG->PIPECFG = 0;
-    LINK_REG->PIPEMAXP = 0;
+    USB0.NRDYENB.WORD &= ~TU_BIT(num);
+    USB0.BRDYENB.WORD &= ~TU_BIT(num);
+    USB0.PIPESEL.WORD  = num;
+    USB0.PIPECFG.WORD  = 0;
+    USB0.PIPEMAXP.WORD = 0;
 
     _hcd.pipe[num].ep  = 0;
     _hcd.pipe[num].dev = 0;
@@ -573,36 +667,36 @@ void hcd_device_close(uint8_t rhport, uint8_t dev_addr)
 bool hcd_setup_send(uint8_t rhport, uint8_t dev_addr, uint8_t const setup_packet[8])
 {
   (void)rhport;
-  //  TU_LOG1("S %d %x\n", dev_addr, LINK_REG->DCPCTR);
+  //  TU_LOG1("S %d %x\n", dev_addr, USB0.DCPCTR.WORD);
 
   TU_ASSERT(dev_addr < 6); /* USBa can only handle addresses from 0 to 5. */
-  TU_ASSERT(0 == LINK_REG->DCPCTR_b.SUREQ);
+  TU_ASSERT(0 == USB0.DCPCTR.BIT.SUREQ);
 
-  LINK_REG->DCPCTR = LINK_REG_PIPE_CTR_PID_NAK;
+  USB0.DCPCTR.WORD = USB_PIPECTR_PID_NAK;
 
-  _hcd.pipe[0].buf = NULL;
-  _hcd.pipe[0].length = 8;
+  _hcd.pipe[0].buf       = NULL;
+  _hcd.pipe[0].length    = 8;
   _hcd.pipe[0].remaining = 0;
-  _hcd.pipe[0].dev = dev_addr;
+  _hcd.pipe[0].dev       = dev_addr;
 
-  while (LINK_REG->DCPCTR_b.PBUSY) ;
-  LINK_REG->DCPMAXP = (dev_addr << 12) | _hcd.ctl_mps[dev_addr];
+  while (USB0.DCPCTR.BIT.PBUSY) ;
+  USB0.DCPMAXP.WORD      = (dev_addr << 12) | _hcd.ctl_mps[dev_addr];
 
   /* Set direction in advance for DATA stage */
   uint8_t const bmRequesttype = setup_packet[0];
-  LINK_REG->DCPCFG_b.DIR = tu_edpt_dir(bmRequesttype) ? 0: 1;
+  USB0.DCPCFG.BIT.DIR = tu_edpt_dir(bmRequesttype) ? 0: 1;
 
   uint16_t const* p = (uint16_t const*)(uintptr_t)&setup_packet[0];
-  LINK_REG->USBREQ = tu_htole16(p[0]);
-  LINK_REG->USBVAL = p[1];
-  LINK_REG->USBINDX = p[2];
-  LINK_REG->USBLENG = p[3];
+  USB0.USBREQ.WORD = tu_htole16(p[0]);
+  USB0.USBVAL      = p[1];
+  USB0.USBINDX     = p[2];
+  USB0.USBLENG     = p[3];
 
-  LINK_REG->DCPCTR_b.SUREQ = 1;
+  USB0.DCPCTR.BIT.SUREQ = 1;
   return true;
 }
 
-bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const *ep_desc)
+bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const * ep_desc)
 {
   (void)rhport;
   TU_ASSERT(dev_addr < 6); /* USBa can only handle addresses from 0 to 5. */
@@ -611,14 +705,14 @@ bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const
   const unsigned epn     = tu_edpt_number(ep_addr);
   const unsigned mps     = tu_edpt_packet_size(ep_desc);
   if (0 == epn) {
-    LINK_REG->DCPCTR = LINK_REG_PIPE_CTR_PID_NAK;
+    USB0.DCPCTR.WORD = USB_PIPECTR_PID_NAK;
     hcd_devtree_info_t devtree;
     hcd_devtree_get_info(dev_addr, &devtree);
-    uint16_t volatile *devadd = (uint16_t volatile *)(uintptr_t) &LINK_REG->DEVADD[0];
+    uint16_t volatile *devadd = (uint16_t volatile *)(uintptr_t)&USB0.DEVADD0.WORD;
     devadd += dev_addr;
-    while (LINK_REG->DCPCTR_b.PBUSY) ;
-    LINK_REG->DCPMAXP = (dev_addr << 12) | mps;
-    *devadd = (TUSB_SPEED_FULL == devtree.speed) ? LINK_REG_DEVADD_USBSPD_FS : LINK_REG_DEVADD_USBSPD_LS;
+    while (USB0.DCPCTR.BIT.PBUSY) ;
+    USB0.DCPMAXP.WORD = (dev_addr << 12) | mps;
+    *devadd = (TUSB_SPEED_FULL == devtree.speed) ? USB_DEVADD_FULL : USB_DEVADD_LOW;
     _hcd.ctl_mps[dev_addr] = mps;
     return true;
   }
@@ -637,25 +731,25 @@ bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const
 
   /* setup pipe */
   hcd_int_disable(rhport);
-  LINK_REG->PIPESEL = num;
-  LINK_REG->PIPEMAXP = (dev_addr << 12) | mps;
+  USB0.PIPESEL.WORD  = num;
+  USB0.PIPEMAXP.WORD = (dev_addr << 12) | mps;
   volatile uint16_t *ctr = get_pipectr(num);
-  *ctr = LINK_REG_PIPE_CTR_ACLRM_Msk | LINK_REG_PIPE_CTR_SQCLR_Msk;
+  *ctr = USB_PIPECTR_ACLRM | USB_PIPECTR_SQCLR;
   *ctr = 0;
   unsigned cfg = ((1 ^ dir_in) << 4) | epn;
   if (xfer == TUSB_XFER_BULK) {
-    cfg |= LINK_REG_PIPECFG_TYPE_BULK | LINK_REG_PIPECFG_SHTNAK_Msk | LINK_REG_PIPECFG_DBLB_Msk;
+    cfg |= USB_PIPECFG_BULK | USB_PIPECFG_SHTNAK | USB_PIPECFG_DBLB;
   } else if (xfer == TUSB_XFER_INTERRUPT) {
-    cfg |= LINK_REG_PIPECFG_TYPE_INT;
+    cfg |= USB_PIPECFG_INT;
   } else {
-    cfg |= LINK_REG_PIPECFG_TYPE_ISO | LINK_REG_PIPECFG_DBLB_Msk;
+    cfg |= USB_PIPECFG_ISO | USB_PIPECFG_DBLB;
   }
-  LINK_REG->PIPECFG = cfg;
-  LINK_REG->BRDYSTS = 0x1FFu ^ TU_BIT(num);
-  LINK_REG->NRDYENB |= TU_BIT(num);
-  LINK_REG->BRDYENB |= TU_BIT(num);
+  USB0.PIPECFG.WORD  = cfg;
+  USB0.BRDYSTS.WORD  = 0x1FFu ^ TU_BIT(num);
+  USB0.NRDYENB.WORD |= TU_BIT(num);
+  USB0.BRDYENB.WORD |= TU_BIT(num);
   if (!dir_in) {
-    *ctr = LINK_REG_PIPE_CTR_PID_BUF;
+    *ctr = USB_PIPECTR_PID_BUF;
   }
   hcd_int_enable(rhport);
 
@@ -682,12 +776,12 @@ bool hcd_edpt_clear_stall(uint8_t dev_addr, uint8_t ep_addr)
     *ctr = pid & 2;
     *ctr = 0;
   }
-  *ctr = LINK_REG_PIPE_CTR_SQCLR_Msk;
+  *ctr = USB_PIPECTR_SQCLR;
   unsigned const epn = tu_edpt_number(ep_addr);
   if (!epn) return true;
 
   if (!tu_edpt_dir(ep_addr)) { /* OUT */
-    *ctr = LINK_REG_PIPE_CTR_PID_BUF;
+    *ctr = USB_PIPECTR_PID_BUF;
   }
   return true;
 }
@@ -705,50 +799,52 @@ void hcd_int_handler(uint8_t rhport)
     20, 8, 19, 18};
 #endif
 
-  unsigned is1 = LINK_REG->INTSTS1;
-  unsigned is0 = LINK_REG->INTSTS0;
+  unsigned is1 = USB0.INTSTS1.WORD;
+  unsigned is0 = USB0.INTSTS0.WORD;
   /* clear active bits except VALID (don't write 0 to already cleared bits according to the HW manual) */
-  LINK_REG->INTSTS1 = ~((LINK_REG_INTSTS1_SACK_Msk | LINK_REG_INTSTS1_SIGN_Msk | LINK_REG_INTSTS1_ATTCH_Msk | LINK_REG_INTSTS1_DTCH_Msk) & is1);
-  LINK_REG->INTSTS0 = ~((LINK_REG_INTSTS0_BRDY_Msk | LINK_REG_INTSTS0_NRDY_Msk | LINK_REG_INTSTS0_BEMP_Msk) & is0);
+  USB0.INTSTS1.WORD = ~((USB_IS1_SACK | USB_IS1_SIGN | USB_IS1_ATTCH | USB_IS1_DTCH) & is1);
+  USB0.INTSTS0.WORD = ~((USB_IS0_BRDY | USB_IS0_NRDY | USB_IS0_BEMP) & is0);
   // TU_LOG1("IS %04x %04x\n", is0, is1);
-  is1 &= LINK_REG->INTENB1;
-  is0 &= LINK_REG->INTENB0;
+  is1 &= USB0.INTENB1.WORD;
+  is0 &= USB0.INTENB0.WORD;
 
-  if (is1 & LINK_REG_INTSTS1_SACK_Msk) {
+  if (is1 & USB_IS1_SACK) {
     /* Set DATA1 in advance for the next transfer. */
-    LINK_REG->DCPCTR_b.SQSET = 1;
-    hcd_event_xfer_complete(
-      LINK_REG->DCPMAXP_b.DEVSEL, tu_edpt_addr(0, TUSB_DIR_OUT), 8, XFER_RESULT_SUCCESS, true);
+    USB0.DCPCTR.BIT.SQSET = 1;
+    hcd_event_xfer_complete(USB0.DCPMAXP.BIT.DEVSEL,
+                            tu_edpt_addr(0, TUSB_DIR_OUT),
+                            8, XFER_RESULT_SUCCESS, true);
   }
-  if (is1 & LINK_REG_INTSTS1_SIGN_Msk) {
-    hcd_event_xfer_complete(
-      LINK_REG->DCPMAXP_b.DEVSEL, tu_edpt_addr(0, TUSB_DIR_OUT), 8, XFER_RESULT_FAILED, true);
+  if (is1 & USB_IS1_SIGN) {
+    hcd_event_xfer_complete(USB0.DCPMAXP.BIT.DEVSEL,
+                            tu_edpt_addr(0, TUSB_DIR_OUT),
+                            8, XFER_RESULT_FAILED, true);
   }
-  if (is1 & LINK_REG_INTSTS1_ATTCH_Msk) {
-    LINK_REG->DVSTCTR0_b.UACT = 1;
+  if (is1 & USB_IS1_ATTCH) {
+    USB0.DVSTCTR0.BIT.UACT = 1;
     _hcd.need_reset = true;
-    LINK_REG->INTENB1 = (LINK_REG->INTENB1 & ~LINK_REG_INTSTS1_ATTCH_Msk) | LINK_REG_INTSTS1_DTCH_Msk;
+    USB0.INTENB1.WORD = (USB0.INTENB1.WORD & ~USB_IS1_ATTCH) | USB_IS1_DTCH;
     hcd_event_device_attach(rhport, true);
   }
-  if (is1 & LINK_REG_INTSTS1_DTCH_Msk) {
-    LINK_REG->DVSTCTR0_b.UACT = 0;
-    if (LINK_REG->DCPCTR_b.SUREQ)
-      LINK_REG->DCPCTR_b.SUREQCLR = 1;
-    LINK_REG->INTENB1 = (LINK_REG->INTENB1 & ~LINK_REG_INTSTS1_DTCH_Msk) | LINK_REG_INTSTS1_ATTCH_Msk;
+  if (is1 & USB_IS1_DTCH) {
+    USB0.DVSTCTR0.BIT.UACT = 0;
+    if (USB0.DCPCTR.BIT.SUREQ)
+      USB0.DCPCTR.BIT.SUREQCLR = 1;
+    USB0.INTENB1.WORD = (USB0.INTENB1.WORD & ~USB_IS1_DTCH) | USB_IS1_ATTCH;
     hcd_event_device_remove(rhport, true);
   }
 
-  if (is0 & LINK_REG_INTSTS0_BEMP_Msk) {
-    const unsigned s = LINK_REG->BEMPSTS;
-    LINK_REG->BEMPSTS = 0;
+  if (is0 & USB_IS0_BEMP) {
+    const unsigned s = USB0.BEMPSTS.WORD;
+    USB0.BEMPSTS.WORD = 0;
     if (s & 1) {
       process_pipe0_bemp(rhport);
     }
   }
-  if (is0 & LINK_REG_INTSTS0_NRDY_Msk) {
-    const unsigned m = LINK_REG->NRDYENB;
-    unsigned s = LINK_REG->NRDYSTS & m;
-    LINK_REG->NRDYSTS = ~s;
+  if (is0 & USB_IS0_NRDY) {
+    const unsigned m = USB0.NRDYENB.WORD;
+    unsigned s       = USB0.NRDYSTS.WORD & m;
+    USB0.NRDYSTS.WORD = ~s;
     while (s) {
 #if defined(__CCRX__)
       const unsigned num = Mod37BitPosition[(-s & s) % 37];
@@ -759,11 +855,11 @@ void hcd_int_handler(uint8_t rhport)
       s &= ~TU_BIT(num);
     }
   }
-  if (is0 & LINK_REG_INTSTS0_BRDY_Msk) {
-    const unsigned m = LINK_REG->BRDYENB;
-    unsigned s = LINK_REG->BRDYSTS & m;
+  if (is0 & USB_IS0_BRDY) {
+    const unsigned m = USB0.BRDYENB.WORD;
+    unsigned s       = USB0.BRDYSTS.WORD & m;
     /* clear active bits (don't write 0 to already cleared bits according to the HW manual) */
-    LINK_REG->BRDYSTS = ~s;
+    USB0.BRDYSTS.WORD = ~s;
     while (s) {
 #if defined(__CCRX__)
       const unsigned num = Mod37BitPosition[(-s & s) % 37];
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index 0e78c796e..e49c0b52e 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -6,6 +6,8 @@
  * Portions:
  * Copyright (c) 2016 STMicroelectronics
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
+ * Copyright (c) 2022 Simon Küppers (skuep)
+ * Copyright (c) 2022 HiFiPhile
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -64,10 +66,6 @@
  * - STALL handled, but not tested.
  *   - Does it work? No clue.
  * - All EP BTABLE buffers are created based on max packet size of first EP opened with that address.
- * - No isochronous endpoints
- * - Endpoint index is the ID of the endpoint
- *   - This means that priority is given to endpoints with lower ID numbers
- *   - Code is mixing up EP IX with EP ID. Everywhere.
  * - Packet buffer memory is copied in the interrupt.
  *   - This is better for performance, but means interrupts are disabled for longer
  *   - DMA may be the best choice, but it could also be pushed to the USBD task.
@@ -103,24 +101,15 @@
 
 #include "tusb_option.h"
 
-#if defined(STM32F102x6) || defined(STM32F102xB) || \
-    defined(STM32F103x6) || defined(STM32F103xB) || \
-    defined(STM32F103xE) || defined(STM32F103xG)
-#define STM32F1_FSDEV
-#endif
-
-#if CFG_TUD_ENABLED && \
-      ( TU_CHECK_MCU(OPT_MCU_STM32F0, OPT_MCU_STM32F3, OPT_MCU_STM32L0, OPT_MCU_STM32L1, OPT_MCU_STM32G4, OPT_MCU_STM32WB) || \
-        (TU_CHECK_MCU(OPT_MCU_STM32F1) && defined(STM32F1_FSDEV)) \
-      )
-
-// In order to reduce the dependance on HAL, we undefine this.
-// Some definitions are copied to our private include file.
-#undef USE_HAL_DRIVER
+#if CFG_TUD_ENABLED && defined(TUP_USBIP_FSDEV)
 
 #include "device/dcd.h"
-#include "portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h"
 
+#ifdef TUP_USBIP_FSDEV_STM32
+  // Undefine to reduce the dependence on HAL
+  #undef USE_HAL_DRIVER
+  #include "portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h"
+#endif
 
 /*****************************************************
  * Configuration
@@ -142,46 +131,51 @@
 #  define DCD_STM32_BTABLE_LENGTH (PMA_LENGTH - DCD_STM32_BTABLE_BASE)
 #endif
 
-// Since TinyUSB doesn't use SOF for now, and this interrupt too often (1ms interval)
-// We disable SOF for now until needed later on
-#ifndef USE_SOF
-#  define USE_SOF     0
-#endif
-
 /***************************************************
  * Checks, structs, defines, function definitions, etc.
  */
 
 TU_VERIFY_STATIC((MAX_EP_COUNT) <= STFSDEV_EP_COUNT, "Only 8 endpoints supported on the hardware");
-
-TU_VERIFY_STATIC(((DCD_STM32_BTABLE_BASE) + (DCD_STM32_BTABLE_LENGTH))<=(PMA_LENGTH),
-    "BTABLE does not fit in PMA RAM");
-
+TU_VERIFY_STATIC(((DCD_STM32_BTABLE_BASE) + (DCD_STM32_BTABLE_LENGTH))<=(PMA_LENGTH), "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
+//--------------------------------------------------------------------+
+
 // One of these for every EP IN & OUT, uses a bit of RAM....
 typedef struct
 {
   uint8_t * buffer;
-  // tu_fifo_t * ff;  // TODO support dcd_edpt_xfer_fifo API
+  tu_fifo_t * ff;
   uint16_t total_len;
   uint16_t queued_len;
   uint16_t pma_ptr;
-  uint8_t max_packet_size;
-  uint8_t pma_alloc_size;
+  uint16_t max_packet_size;
+  uint16_t pma_alloc_size;
+  uint8_t ep_idx; // index for USB_EPnR register
 } xfer_ctl_t;
 
+// EP allocator
+typedef struct
+{
+  uint8_t ep_num;
+  uint8_t ep_type;
+  bool allocated[2];
+} ep_alloc_t;
+
 static xfer_ctl_t xfer_status[MAX_EP_COUNT][2];
 
-static inline xfer_ctl_t* xfer_ctl_ptr(uint32_t epnum, uint32_t dir)
-{
-  return &xfer_status[epnum][dir];
-}
+static ep_alloc_t ep_alloc_status[STFSDEV_EP_COUNT];
 
 static TU_ATTR_ALIGNED(4) uint32_t _setup_packet[6];
 
 static uint8_t remoteWakeCountdown; // When wake is requested
 
+//--------------------------------------------------------------------+
+// Prototypes
+//--------------------------------------------------------------------+
+
 // into the stack.
 static void dcd_handle_bus_reset(void);
 static void dcd_transmit_packet(xfer_ctl_t * xfer, uint16_t ep_ix);
@@ -193,23 +187,43 @@ static uint16_t ep_buf_ptr; ///< Points to first free memory location
 static void dcd_pma_alloc_reset(void);
 static uint16_t dcd_pma_alloc(uint8_t ep_addr, size_t length);
 static void dcd_pma_free(uint8_t ep_addr);
+static void dcd_ep_free(uint8_t ep_addr);
+static uint8_t dcd_ep_alloc(uint8_t ep_addr, uint8_t ep_type);
 static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, size_t wNBytes);
 static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, size_t wNBytes);
 
-//static bool dcd_write_packet_memory_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 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);
+
+//--------------------------------------------------------------------+
+// 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 < MAX_EP_COUNT, &xfer_status[0][0]);
+
+  return &xfer_status[epnum][dir];
+}
 
 // Using a function due to better type checks
 // This seems better than having to do type casts everywhere else
-static inline void reg16_clear_bits(__IO uint16_t *reg, uint16_t mask) {
+TU_ATTR_ALWAYS_INLINE static inline void reg16_clear_bits(__IO uint16_t *reg, uint16_t mask) {
   *reg = (uint16_t)(*reg & ~mask);
 }
 
 // Bits in ISTR are cleared upon writing 0
-static inline void clear_istr_bits(uint16_t mask) {
+TU_ATTR_ALWAYS_INLINE static inline void clear_istr_bits(uint16_t mask) {
   USB->ISTR = ~mask;
 }
 
+//--------------------------------------------------------------------+
+// Controller API
+//--------------------------------------------------------------------+
+
 void dcd_init (uint8_t rhport)
 {
   /* Clocks should already be enabled */
@@ -222,7 +236,7 @@ void dcd_init (uint8_t rhport)
   {
     asm("NOP");
   }
-	// Perform USB peripheral reset
+  // 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
   {
@@ -247,7 +261,7 @@ void dcd_init (uint8_t rhport)
     pcd_set_endpoint(USB,i,0u);
   }
 
-  USB->CNTR |= USB_CNTR_RESETM | (USE_SOF ? USB_CNTR_SOFM : 0) | USB_CNTR_ESOFM | USB_CNTR_CTRM | USB_CNTR_SUSPM | USB_CNTR_WKUPM;
+  USB->CNTR |= USB_CNTR_RESETM | USB_CNTR_ESOFM | USB_CNTR_CTRM | USB_CNTR_SUSPM | USB_CNTR_WKUPM;
   dcd_handle_bus_reset();
   
   // Enable pull-up if supported
@@ -292,7 +306,14 @@ void dcd_sof_enable(uint8_t rhport, bool en)
   (void) rhport;
   (void) en;
 
-  // TODO implement later
+  if (en)
+  {
+    USB->CNTR |= USB_CNTR_SOFM;
+  }
+  else
+  {
+    USB->CNTR &= (uint16_t) ~USB_CNTR_SOFM;
+  }
 }
 
 // Enable device interrupt
@@ -302,7 +323,8 @@ void dcd_int_enable (uint8_t rhport)
   // Member here forces write to RAM before allowing ISR to execute
   __DSB();
   __ISB();
-#if CFG_TUSB_MCU == OPT_MCU_STM32F0 || CFG_TUSB_MCU == OPT_MCU_STM32L0
+#if CFG_TUSB_MCU == OPT_MCU_STM32F0 || CFG_TUSB_MCU == OPT_MCU_STM32L0 || \
+    CFG_TUSB_MCU == OPT_MCU_STM32L4
   NVIC_EnableIRQ(USB_IRQn);
 
 #elif CFG_TUSB_MCU == OPT_MCU_STM32L1
@@ -350,7 +372,8 @@ void dcd_int_disable(uint8_t rhport)
 {
   (void)rhport;
 
-#if CFG_TUSB_MCU == OPT_MCU_STM32F0 || CFG_TUSB_MCU == OPT_MCU_STM32L0
+#if CFG_TUSB_MCU == OPT_MCU_STM32F0 || CFG_TUSB_MCU == OPT_MCU_STM32L0 || \
+    CFG_TUSB_MCU == OPT_MCU_STM32L4
   NVIC_DisableIRQ(USB_IRQn);
 #elif CFG_TUSB_MCU == OPT_MCU_STM32L1
   NVIC_DisableIRQ(USB_LP_IRQn);
@@ -400,7 +423,7 @@ void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
   (void) dev_addr;
 
   // Respond with status
-  dcd_edpt_xfer(rhport, tu_edpt_addr(0, TUSB_DIR_IN), NULL, 0);
+  dcd_edpt_xfer(rhport, TUSB_DIR_IN_MASK | 0x00, NULL, 0);
 
   // DCD can only set address after status for this request is complete.
   // do it at dcd_edpt0_status_complete()
@@ -441,10 +464,17 @@ static void dcd_handle_bus_reset(void)
   //__IO uint16_t * const epreg = &(EPREG(0));
   USB->DADDR = 0u; // disable USB peripheral by clearing the EF flag
 
-  // Clear all EPREG (or maybe this is automatic? I'm not sure)
+  
   for(uint32_t i=0; i<STFSDEV_EP_COUNT; i++)
   {
+    // Clear all EPREG (or maybe this is automatic? I'm not sure)
     pcd_set_endpoint(USB,i,0u);
+    
+    // Clear EP allocation status
+    ep_alloc_status[i].ep_num = 0xFF;
+    ep_alloc_status[i].ep_type = 0xFF;
+    ep_alloc_status[i].allocated[0] = false;
+    ep_alloc_status[i].allocated[1] = false;
   }
 
   dcd_pma_alloc_reset();
@@ -461,6 +491,7 @@ 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?
@@ -472,14 +503,14 @@ static void dcd_ep_ctr_tx_handler(uint32_t wIstr)
   /* clear int flag */
   pcd_clear_tx_ep_ctr(USB, EPindex);
 
-  xfer_ctl_t * xfer = xfer_ctl_ptr(EPindex,TUSB_DIR_IN);
+  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
   if((xfer->total_len != xfer->queued_len)) /* TX not complete */
   {
       dcd_transmit_packet(xfer, EPindex);
   }
   else /* TX Complete */
   {
-    dcd_event_xfer_complete(0, (uint8_t)(0x80 + EPindex), xfer->total_len, XFER_RESULT_SUCCESS, true);
+    dcd_event_xfer_complete(0, ep_addr, xfer->total_len, XFER_RESULT_SUCCESS, true);
   }
 }
 
@@ -490,9 +521,9 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
 {
   uint32_t EPindex = wIstr & USB_ISTR_EP_ID;
   uint32_t wEPRegVal = pcd_get_endpoint(USB, EPindex);
-  uint32_t count = pcd_get_ep_rx_cnt(USB,EPindex);
+  uint8_t ep_addr = wEPRegVal & USB_EPADDR_FIELD;
 
-  xfer_ctl_t *xfer = xfer_ctl_ptr(EPindex,TUSB_DIR_OUT);
+  xfer_ctl_t *xfer = xfer_ctl_ptr(ep_addr);
 
   // Verify the CTR_RX bit is set. This was in the ST Micro code,
   // but I'm not sure it's actually necessary?
@@ -501,11 +532,12 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
     return;
   }
   
-  if((EPindex == 0U) && ((wEPRegVal & USB_EP_SETUP) != 0U)) /* Setup packet */
+  if((ep_addr == 0U) && ((wEPRegVal & USB_EP_SETUP) != 0U)) /* Setup packet */
   {
     // The setup_received function uses memcpy, so this must first copy the setup data into
     // user memory, to allow for the 32-bit access that memcpy performs.
     uint8_t userMemBuf[8];
+    uint32_t count = pcd_get_ep_rx_cnt(USB, EPindex);
     /* Get SETUP Packet*/
     if(count == 8) // Setup packet should always be 8 bytes. If not, ignore it, and try again.
     {
@@ -518,23 +550,33 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
   }
   else
   {
+    uint32_t count;
+    /* Read from correct register when ISOCHRONOUS (double buffered) */
+    if ( (wEPRegVal & USB_EP_DTOG_RX) && ( (wEPRegVal & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS) ) {
+      count = pcd_get_ep_tx_cnt(USB, EPindex);
+    } else {
+      count = pcd_get_ep_rx_cnt(USB, EPindex);
+    }
+
+    TU_ASSERT(count <= xfer->max_packet_size, /**/);
+
     // Clear RX CTR interrupt flag
-    if(EPindex != 0u)
+    if(ep_addr != 0u)
     {
       pcd_clear_rx_ep_ctr(USB, EPindex);
     }
 
     if (count != 0U)
     {
-#if 0 // TODO support dcd_edpt_xfer_fifo API
+      uint16_t addr = *pcd_ep_rx_address_ptr(USB, EPindex);
+
       if (xfer->ff)
       {
-        dcd_read_packet_memory_ff(xfer->ff, *pcd_ep_rx_address_ptr(USB,EPindex), count);
+        dcd_read_packet_memory_ff(xfer->ff, addr, count);
       }
       else
-#endif
       {
-        dcd_read_packet_memory(&(xfer->buffer[xfer->queued_len]), *pcd_ep_rx_address_ptr(USB,EPindex), count);
+        dcd_read_packet_memory(&(xfer->buffer[xfer->queued_len]), addr, count);
       }
 
       xfer->queued_len = (uint16_t)(xfer->queued_len + count);
@@ -543,7 +585,7 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
     if ((count < xfer->max_packet_size) || (xfer->queued_len == xfer->total_len))
     {
       /* RX COMPLETE */
-      dcd_event_xfer_complete(0, EPindex, xfer->queued_len, XFER_RESULT_SUCCESS, true);
+      dcd_event_xfer_complete(0, ep_addr, xfer->queued_len, XFER_RESULT_SUCCESS, true);
       // Though the host could still send, we don't know.
       // Does the bulk pipe need to be reset to valid to allow for a ZLP?
     }
@@ -551,21 +593,26 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
     {
       uint32_t remaining = (uint32_t)xfer->total_len - (uint32_t)xfer->queued_len;
       if(remaining >= xfer->max_packet_size) {
-        pcd_set_ep_rx_cnt(USB, EPindex,xfer->max_packet_size);
+        pcd_set_ep_rx_bufsize(USB, EPindex,xfer->max_packet_size);
       } else {
-        pcd_set_ep_rx_cnt(USB, EPindex,remaining);
+        pcd_set_ep_rx_bufsize(USB, EPindex,remaining);
+      }
+
+      if (!((wEPRegVal & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS)) {
+        /* Set endpoint active again for receiving more data.
+         * Note that isochronous endpoints stay active always */
+        pcd_set_ep_rx_status(USB, EPindex, USB_EP_RX_VALID);
       }
-      pcd_set_ep_rx_status(USB, EPindex, USB_EP_RX_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(EPindex == 0u)
+  if(ep_addr == 0u)
   {
       // Always be prepared for a status packet...
-    pcd_set_ep_rx_cnt(USB, EPindex, CFG_TUD_ENDPOINT0_SIZE);
+    pcd_set_ep_rx_bufsize(USB, EPindex, CFG_TUD_ENDPOINT0_SIZE);
     pcd_clear_rx_ep_ctr(USB, EPindex);
   }
 }
@@ -602,6 +649,12 @@ void dcd_int_handler(uint8_t rhport) {
   // dcd_ep_ctr_handler(), so less need to loop here. The other interrupts shouldn't
   // be triggered repeatedly.
 
+  /* Put SOF flag at the beginning of ISR in case to get least amount of jitter if it is used for timing purposes */
+  if(int_status & USB_ISTR_SOF) {
+    clear_istr_bits(USB_ISTR_SOF);
+    dcd_event_sof(0, USB->FNR & USB_FNR_FN, true);
+  }
+
   if(int_status & USB_ISTR_RESET) {
     // USBRST is start of reset.
     clear_istr_bits(USB_ISTR_RESET);
@@ -639,13 +692,6 @@ void dcd_int_handler(uint8_t rhport) {
     dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
   }
 
-#if USE_SOF
-  if(int_status & USB_ISTR_SOF) {
-    clear_istr_bits(USB_ISTR_SOF);
-    dcd_event_bus_signal(0, DCD_EVENT_SOF, true);
-  }
-#endif 
-
   if(int_status & USB_ISTR_ESOF) {
     if(remoteWakeCountdown == 1u)
     {
@@ -683,14 +729,15 @@ void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const * re
 
 static void dcd_pma_alloc_reset(void)
 {
+  open_ep_count = 0;
   ep_buf_ptr = DCD_STM32_BTABLE_BASE + 8*MAX_EP_COUNT; // 8 bytes per endpoint (two TX and two RX words, each)
   //TU_LOG2("dcd_pma_alloc_reset()\r\n");
   for(uint32_t i=0; i<MAX_EP_COUNT; i++)
   {
-    xfer_ctl_ptr(i,TUSB_DIR_OUT)->pma_alloc_size = 0U;
-    xfer_ctl_ptr(i,TUSB_DIR_IN)->pma_alloc_size = 0U;
-    xfer_ctl_ptr(i,TUSB_DIR_OUT)->pma_ptr = 0U;
-    xfer_ctl_ptr(i,TUSB_DIR_IN)->pma_ptr = 0U;
+    xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_OUT))->pma_alloc_size = 0U;
+    xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_IN))->pma_alloc_size = 0U;
+    xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_OUT))->pma_ptr = 0U;
+    xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_IN))->pma_ptr = 0U;
   }
 }
 
@@ -705,9 +752,7 @@ static void dcd_pma_alloc_reset(void)
  */
 static uint16_t dcd_pma_alloc(uint8_t ep_addr, size_t length)
 {
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
-  xfer_ctl_t* epXferCtl = xfer_ctl_ptr(epnum,dir);
+  xfer_ctl_t* epXferCtl = xfer_ctl_ptr(ep_addr);
 
   if(epXferCtl->pma_alloc_size != 0U)
   {
@@ -716,13 +761,15 @@ static uint16_t dcd_pma_alloc(uint8_t ep_addr, size_t length)
     TU_ASSERT(length <= epXferCtl->pma_alloc_size, 0xFFFF);  // Verify no larger than previous alloc
     return epXferCtl->pma_ptr;
   }
-  
-  uint16_t addr = ep_buf_ptr; 
+
+  open_ep_count++;
+
+  uint16_t addr = ep_buf_ptr;
   ep_buf_ptr = (uint16_t)(ep_buf_ptr + length); // increment buffer pointer
-  
+
   // Verify no overflow
   TU_ASSERT(ep_buf_ptr <= PMA_LENGTH, 0xFFFF);
-  
+
   epXferCtl->pma_ptr = addr;
   epXferCtl->pma_alloc_size = length;
   //TU_LOG2("dcd_pma_alloc(%x,%x)=%x\r\n",ep_addr,length,addr);
@@ -735,12 +782,9 @@ static uint16_t dcd_pma_alloc(uint8_t ep_addr, size_t length)
  */
 static void dcd_pma_free(uint8_t ep_addr)
 {
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
-
   // Presently, this should never be called for EP0 IN/OUT
   TU_ASSERT(open_ep_count > 2, /**/);
-  TU_ASSERT(xfer_ctl_ptr(epnum,dir)->max_packet_size != 0, /**/);
+  TU_ASSERT(xfer_ctl_ptr(ep_addr)->max_packet_size != 0, /**/);
   open_ep_count--;
 
   // If count is 2, only EP0 should be open, so allocations can be mostly reset.
@@ -752,10 +796,83 @@ static void dcd_pma_free(uint8_t ep_addr)
     // Skip EP0
     for(uint32_t i=1; i<MAX_EP_COUNT; i++)
     {
-      xfer_ctl_ptr(i,TUSB_DIR_OUT)->pma_alloc_size = 0U;
-      xfer_ctl_ptr(i,TUSB_DIR_IN)->pma_alloc_size = 0U;
-      xfer_ctl_ptr(i,TUSB_DIR_OUT)->pma_ptr = 0U;
-      xfer_ctl_ptr(i,TUSB_DIR_IN)->pma_ptr = 0U;
+      xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_OUT))->pma_alloc_size = 0U;
+      xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_IN))->pma_alloc_size = 0U;
+      xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_OUT))->pma_ptr = 0U;
+      xfer_ctl_ptr(tu_edpt_addr(i,TUSB_DIR_IN))->pma_ptr = 0U;
+    }
+  }
+}
+
+/***
+ * Allocate hardware endpoint
+ */
+static uint8_t dcd_ep_alloc(uint8_t ep_addr, uint8_t ep_type)
+{
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir   = tu_edpt_dir(ep_addr);
+
+  for(uint8_t i = 0; i < STFSDEV_EP_COUNT; i++)
+  {
+    // Check if already allocated
+    if(ep_alloc_status[i].allocated[dir] &&
+       ep_alloc_status[i].ep_type == ep_type &&
+       ep_alloc_status[i].ep_num == epnum)
+    {
+      return i;
+    }
+
+    // If EP of current direction is not allocated
+    // Except for ISO endpoint, both direction should be free
+    if(!ep_alloc_status[i].allocated[dir] &&
+       (ep_type != TUSB_XFER_ISOCHRONOUS || !ep_alloc_status[i].allocated[dir ^ 1]))
+    {
+      // Check if EP number is the same
+      if(ep_alloc_status[i].ep_num == 0xFF ||
+         ep_alloc_status[i].ep_num == epnum)
+      {
+        // One EP pair has to be the same type
+        if(ep_alloc_status[i].ep_type == 0xFF ||
+           ep_alloc_status[i].ep_type == ep_type)
+        {
+          ep_alloc_status[i].ep_num = epnum;
+          ep_alloc_status[i].ep_type = ep_type;
+          ep_alloc_status[i].allocated[dir] = true;
+          
+          return i;
+        }
+      }
+    }
+  }
+
+  // Allocation failed
+  TU_ASSERT(0);
+}
+
+/***
+ * Free hardware endpoint
+ */
+static void dcd_ep_free(uint8_t ep_addr)
+{
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir   = tu_edpt_dir(ep_addr);
+
+  for(uint8_t i = 0; i < STFSDEV_EP_COUNT; i++)
+  {
+    // Check if EP number & dir are the same
+    if(ep_alloc_status[i].ep_num == epnum && 
+       ep_alloc_status[i].allocated[dir] == dir)
+    {
+      ep_alloc_status[i].allocated[dir] = false;
+      // Reset entry if ISO endpoint or both direction are free
+      if(ep_alloc_status[i].ep_type == TUSB_XFER_ISOCHRONOUS ||
+         !ep_alloc_status[i].allocated[dir ^ 1])
+      {
+        ep_alloc_status[i].ep_num = 0xFF;
+        ep_alloc_status[i].ep_type = 0xFF;
+
+        return;
+      }
     }
   }
 }
@@ -766,27 +883,24 @@ static void dcd_pma_free(uint8_t ep_addr)
 bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc)
 {
   (void)rhport;
-  uint8_t const epnum = tu_edpt_number(p_endpoint_desc->bEndpointAddress);
+  uint8_t const ep_idx = dcd_ep_alloc(p_endpoint_desc->bEndpointAddress, p_endpoint_desc->bmAttributes.xfer);
   uint8_t const dir   = tu_edpt_dir(p_endpoint_desc->bEndpointAddress);
-  const uint16_t epMaxPktSize = tu_edpt_packet_size(p_endpoint_desc);
+  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;
-  
-  // Isochronous not supported (yet), and some other driver assumptions.
-  TU_ASSERT(p_endpoint_desc->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS);
-  TU_ASSERT(epnum < MAX_EP_COUNT);
+
+  TU_ASSERT(ep_idx < STFSDEV_EP_COUNT);
+  TU_ASSERT(buffer_size <= 1024);
 
   // Set type
   switch(p_endpoint_desc->bmAttributes.xfer) {
   case TUSB_XFER_CONTROL:
     wType = USB_EP_CONTROL;
     break;
-#if (0)
-  case TUSB_XFER_ISOCHRONOUS: // FIXME: Not yet supported
+  case TUSB_XFER_ISOCHRONOUS:
     wType = USB_EP_ISOCHRONOUS;
     break;
-#endif
-
   case TUSB_XFER_BULK:
     wType = USB_EP_CONTROL;
     break;
@@ -799,30 +913,49 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc
     TU_ASSERT(false);
   }
 
-  pcd_set_eptype(USB, epnum, wType);
-  pcd_set_ep_address(USB, epnum, epnum);
+  pcd_set_eptype(USB, ep_idx, wType);
+  pcd_set_ep_address(USB, ep_idx, tu_edpt_number(p_endpoint_desc->bEndpointAddress));
   // Be normal, for now, instead of only accepting zero-byte packets (on control endpoint)
   // or being double-buffered (bulk endpoints)
   pcd_clear_ep_kind(USB,0);
 
-  pma_addr = dcd_pma_alloc(p_endpoint_desc->bEndpointAddress, epMaxPktSize);
+  /* Create a packet memory buffer area. For isochronous endpoints,
+   * use the same buffer as the double buffer, essentially disabling double buffering */
+  pma_addr = dcd_pma_alloc(p_endpoint_desc->bEndpointAddress, buffer_size);
 
-  if(dir == TUSB_DIR_IN)
+  if( (dir == TUSB_DIR_IN) || (wType == USB_EP_ISOCHRONOUS) )
   {
-    *pcd_ep_tx_address_ptr(USB, epnum) = pma_addr;
-    pcd_set_ep_tx_cnt(USB, epnum, epMaxPktSize);
-    pcd_clear_tx_dtog(USB, epnum);
-    pcd_set_ep_tx_status(USB,epnum,USB_EP_TX_NAK);
+    *pcd_ep_tx_address_ptr(USB, ep_idx) = pma_addr;
+    pcd_set_ep_tx_bufsize(USB, ep_idx, buffer_size);
+    pcd_clear_tx_dtog(USB, ep_idx);
   }
-  else
+
+  if( (dir == TUSB_DIR_OUT) || (wType == USB_EP_ISOCHRONOUS) )
   {
-    *pcd_ep_rx_address_ptr(USB, epnum) = pma_addr;
-    pcd_set_ep_rx_cnt(USB, epnum, epMaxPktSize);
-    pcd_clear_rx_dtog(USB, epnum);
-    pcd_set_ep_rx_status(USB, epnum, USB_EP_RX_NAK);
+    *pcd_ep_rx_address_ptr(USB, ep_idx) = pma_addr;
+    pcd_set_ep_rx_bufsize(USB, ep_idx, buffer_size);
+    pcd_clear_rx_dtog(USB, ep_idx);
   }
 
-  xfer_ctl_ptr(epnum, dir)->max_packet_size = epMaxPktSize;
+  /* Enable endpoint */
+  if (dir == TUSB_DIR_IN)
+  {
+    if(wType == USB_EP_ISOCHRONOUS) {
+      pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_DIS);
+    } else {
+      pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_NAK);
+    }
+  } else
+  {
+    if(wType == USB_EP_ISOCHRONOUS) {
+      pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_DIS);
+    } else {
+      pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_NAK);
+    }
+  }
+
+  xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->max_packet_size = packet_size;
+  xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->ep_idx = ep_idx;
 
   return true;
 }
@@ -843,21 +976,81 @@ void dcd_edpt_close_all (uint8_t rhport)
 void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr)
 {
   (void)rhport;
-  uint32_t const epnum = tu_edpt_number(ep_addr);
-  uint32_t const dir   = tu_edpt_dir(ep_addr);
-  
+
+  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  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,epnum,USB_EP_TX_DIS);
+    pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_DIS);
   }
   else
   {
-    pcd_set_ep_rx_status(USB, epnum, USB_EP_RX_DIS);
+    pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_DIS);
   }
 
+  dcd_ep_free(ep_addr);
+
   dcd_pma_free(ep_addr);
 }
 
+bool dcd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size)
+{
+  (void)rhport;
+  
+  TU_ASSERT(largest_packet_size <= 1024);
+  
+  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. For isochronous endpoints,
+   * use the same buffer as the double buffer, essentially disabling double buffering */
+  uint16_t pma_addr = dcd_pma_alloc(ep_addr, buffer_size);
+
+  xfer_ctl_ptr(ep_addr)->ep_idx = ep_idx;
+
+  pcd_set_eptype(USB, ep_idx, USB_EP_ISOCHRONOUS);
+  
+  *pcd_ep_tx_address_ptr(USB, ep_idx) = pma_addr;
+  *pcd_ep_rx_address_ptr(USB, ep_idx) = pma_addr;
+  
+  return true;
+}
+
+bool dcd_edpt_iso_activate(uint8_t rhport,  tusb_desc_endpoint_t const * p_endpoint_desc)
+{
+  (void)rhport;
+  uint8_t const ep_idx = xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->ep_idx;
+  uint8_t const dir    = tu_edpt_dir(p_endpoint_desc->bEndpointAddress);
+  const uint16_t packet_size = tu_edpt_packet_size(p_endpoint_desc);
+  const uint16_t buffer_size = pcd_aligned_buffer_size(packet_size);
+
+  /* Disable endpoint */
+  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);
+  }
+  
+  pcd_set_ep_address(USB, ep_idx, tu_edpt_number(p_endpoint_desc->bEndpointAddress));
+  // Be normal, for now, instead of only accepting zero-byte packets (on control endpoint)
+  // or being double-buffered (bulk endpoints)
+  pcd_clear_ep_kind(USB,0);
+
+  pcd_set_ep_tx_bufsize(USB, ep_idx, buffer_size);
+  pcd_set_ep_rx_bufsize(USB, ep_idx, buffer_size);
+  pcd_clear_tx_dtog(USB, ep_idx);
+  pcd_clear_rx_dtog(USB, ep_idx);
+
+  xfer_ctl_ptr(p_endpoint_desc->bEndpointAddress)->max_packet_size = packet_size;
+
+  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)
@@ -868,21 +1061,27 @@ static void dcd_transmit_packet(xfer_ctl_t * xfer, uint16_t ep_ix)
   {
     len = xfer->max_packet_size;
   }
-  uint16_t oldAddr = *pcd_ep_tx_address_ptr(USB,ep_ix);
 
-#if 0 // TODO support dcd_edpt_xfer_fifo API
+  uint16_t ep_reg = pcd_get_endpoint(USB, ep_ix);
+  uint16_t addr_ptr = *pcd_ep_tx_address_ptr(USB,ep_ix);
+
   if (xfer->ff)
   {
-    dcd_write_packet_memory_ff(xfer->ff, oldAddr, len);
+    dcd_write_packet_memory_ff(xfer->ff, addr_ptr, len);
   }
   else
-#endif
   {
-    dcd_write_packet_memory(oldAddr, &(xfer->buffer[xfer->queued_len]), len);
+    dcd_write_packet_memory(addr_ptr, &(xfer->buffer[xfer->queued_len]), len);
   }
   xfer->queued_len = (uint16_t)(xfer->queued_len + len);
 
-  pcd_set_ep_tx_cnt(USB,ep_ix,len);
+  /* Write into correct register when ISOCHRONOUS (double buffered) */
+  if ( (ep_reg & USB_EP_DTOG_TX) && ( (ep_reg & USB_EP_TYPE_MASK) == USB_EP_ISOCHRONOUS) ) {
+    pcd_set_ep_rx_cnt(USB, ep_ix, len);
+  } else {
+    pcd_set_ep_tx_cnt(USB, ep_ix, len);
+  }
+
   pcd_set_ep_tx_status(USB, ep_ix, USB_EP_TX_VALID);
 }
 
@@ -890,13 +1089,12 @@ bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
 {
   (void) rhport;
 
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
-
-  xfer_ctl_t * xfer = xfer_ctl_ptr(epnum,dir);
+  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  uint8_t const ep_idx = xfer->ep_idx;
+  uint8_t const dir    = tu_edpt_dir(ep_addr);
 
   xfer->buffer = buffer;
-  // xfer->ff     = NULL; // TODO support dcd_edpt_xfer_fifo API
+  xfer->ff     = NULL;
   xfer->total_len = total_bytes;
   xfer->queued_len = 0;
 
@@ -904,37 +1102,36 @@ bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
   {
     // A setup token can occur immediately after an OUT STATUS packet so make sure we have a valid
     // buffer for the control endpoint.
-    if (epnum == 0 && buffer == NULL)
+    if (ep_idx == 0 && buffer == NULL)
     {
         xfer->buffer = (uint8_t*)_setup_packet;
     }
+
     if(total_bytes > xfer->max_packet_size)
     {
-      pcd_set_ep_rx_cnt(USB,epnum,xfer->max_packet_size);
+      pcd_set_ep_rx_bufsize(USB,ep_idx,xfer->max_packet_size);
     } else {
-      pcd_set_ep_rx_cnt(USB,epnum,total_bytes);
+      pcd_set_ep_rx_bufsize(USB,ep_idx,total_bytes);
     }
-    pcd_set_ep_rx_status(USB, epnum, USB_EP_RX_VALID);
+    pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_VALID);
   }
   else // IN
   {
-    dcd_transmit_packet(xfer,epnum);
+    dcd_transmit_packet(xfer,ep_idx);
   }
   return true;
 }
 
-#if 0 // TODO support dcd_edpt_xfer_fifo API
 bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16_t total_bytes)
 {
   (void) rhport;
 
-  uint8_t const epnum = tu_edpt_number(ep_addr);
+  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  uint8_t const epnum = xfer->ep_idx;
   uint8_t const dir   = tu_edpt_dir(ep_addr);
 
-  xfer_ctl_t * xfer = xfer_ctl_ptr(epnum,dir);
-
   xfer->buffer = NULL;
-  // xfer->ff     = ff; // TODO support dcd_edpt_xfer_fifo API
+  xfer->ff     = ff;
   xfer->total_len = total_bytes;
   xfer->queued_len = 0;
 
@@ -942,9 +1139,9 @@ bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16
   {
     if(total_bytes > xfer->max_packet_size)
     {
-      pcd_set_ep_rx_cnt(USB,epnum,xfer->max_packet_size);
+      pcd_set_ep_rx_bufsize(USB,epnum,xfer->max_packet_size);
     } else {
-      pcd_set_ep_rx_cnt(USB,epnum,total_bytes);
+      pcd_set_ep_rx_bufsize(USB,epnum,total_bytes);
     }
     pcd_set_ep_rx_status(USB, epnum, USB_EP_RX_VALID);
   }
@@ -954,19 +1151,22 @@ bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16
   }
   return true;
 }
-#endif
 
 void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
 {
   (void)rhport;
 
-  if (ep_addr & 0x80)
+  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  uint8_t const ep_idx = xfer->ep_idx;
+  uint8_t const dir    = tu_edpt_dir(ep_addr);
+
+  if (dir == TUSB_DIR_IN)
   { // IN
-    pcd_set_ep_tx_status(USB, ep_addr & 0x7F, USB_EP_TX_STALL);
+    pcd_set_ep_tx_status(USB, ep_idx, USB_EP_TX_STALL);
   }
   else
   { // OUT
-    pcd_set_ep_rx_status(USB, ep_addr, USB_EP_RX_STALL);
+    pcd_set_ep_rx_status(USB, ep_idx, USB_EP_RX_STALL);
   }
 }
 
@@ -974,21 +1174,26 @@ void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
 {
   (void)rhport;
 
-  if (ep_addr & 0x80)
-  { // IN
-    ep_addr &= 0x7F;
+  xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
+  uint8_t const ep_idx = xfer->ep_idx;
+  uint8_t const dir    = tu_edpt_dir(ep_addr);
 
-    pcd_set_ep_tx_status(USB,ep_addr, USB_EP_TX_NAK);
+  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);
+    }
 
     /* Reset to DATA0 if clearing stall condition. */
-    pcd_clear_tx_dtog(USB,ep_addr);
+    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_addr);
-
-    pcd_set_ep_rx_status(USB,ep_addr, USB_EP_RX_NAK);
+    pcd_clear_rx_dtog(USB, ep_idx);
   }
 }
 
@@ -1005,8 +1210,7 @@ void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
   */
 static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, size_t wNBytes)
 {
-  uint32_t n =  ((uint32_t)wNBytes + 1U) >> 1U;
-  uint32_t i;
+  uint32_t n = (uint32_t)wNBytes >> 1U;
   uint16_t temp1, temp2;
   const uint8_t * srcVal;
 
@@ -1017,64 +1221,74 @@ static bool dcd_write_packet_memory(uint16_t dst, const void *__restrict src, si
   srcVal = src;
   pdwVal = &pma[PMA_STRIDE*(dst>>1)];
 
-  for (i = n; i != 0; i--)
+  while (n--)
   {
-    temp1 = (uint16_t) *srcVal;
+    temp1 = (uint16_t)*srcVal;
     srcVal++;
-    temp2 = temp1 | ((uint16_t)((uint16_t) ((*srcVal) << 8U))) ;
+    temp2 = temp1 | ((uint16_t)(((uint16_t)(*srcVal)) << 8U)) ;
     *pdwVal = temp2;
     pdwVal += PMA_STRIDE;
     srcVal++;
   }
+
+  if (wNBytes & 0x01)
+  {
+    temp1 = *srcVal;
+    *pdwVal = temp1;
+  }
+
   return true;
 }
 
-#if 0 // TODO support dcd_edpt_xfer_fifo API
 /**
   * @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
   */
-
-// THIS FUNCTION IS UNTESTED
-
 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
-  // Check for first linear part
-  void * src;
-  uint16_t len = tu_fifo_get_linear_read_info(ff, 0, &src, wNBytes);  // We want to read from the FIFO        - THIS FUNCTION CHANGED!!!
-  TU_VERIFY(len && dcd_write_packet_memory(dst, src, len));           // and write it into the PMA
-  tu_fifo_advance_read_pointer(ff, len);
-
-  // Check for wrapped part
-  if (len < wNBytes)
+  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);
+  
+  // We want to read from the FIFO and write it into the PMA, if LIN part is ODD and has WRAPPED part,
+  // last lin byte will be combined with wrapped part
+  // To ensure PMA is always access 16bit aligned (dst aligned to 16 bit)
+  if((cnt_lin & 0x01) && cnt_wrap)
   {
-    // Get remaining wrapped length
-    uint16_t len2 = tu_fifo_get_linear_read_info(ff, 0, &src, wNBytes - len);
-    TU_VERIFY(len2);
+    // Copy first linear part
+    dcd_write_packet_memory(dst, info.ptr_lin, cnt_lin &~0x01);
+    dst += cnt_lin &~0x01;
 
-    // Update destination pointer
-    dst += len;
+    // 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;
 
-    // Since PMA is accessed 16-bit wise we need to handle the case when a 16 bit value was split
-    if (len % 2)    // If len is uneven there is a byte left to copy
+    // Copy rest of wrapped byte
+    dcd_write_packet_memory(dst, ((uint8_t*)info.ptr_wrap) + 1, cnt_wrap - 1);
+  }
+  else
+  {
+    // Copy linear part
+    dcd_write_packet_memory(dst, info.ptr_lin, cnt_lin);
+    dst += info.len_lin;
+
+    if(info.len_wrap)
     {
-      // Since PMA can accessed only 16 bit-wise we copy the last byte again
-      tu_fifo_backward_read_pointer(ff, 1);                 // Move one byte back and copy two bytes for the PMA
-      tu_fifo_read_n(ff, (void *) &pma[PMA_STRIDE*(dst>>1)], 2);     // Since EP FIFOs must be of item size 1 this is safe to do
-      dst++;
-      len2--;
+      // Copy wrapped byte
+      dcd_write_packet_memory(dst, info.ptr_wrap, cnt_wrap);
     }
-
-    TU_VERIFY(dcd_write_packet_memory(dst, src, len2));
-    tu_fifo_advance_write_pointer(ff, len2);
   }
 
+  tu_fifo_advance_read_pointer(ff, cnt_lin + cnt_wrap);
+
   return true;
 }
-#endif
 
 /**
   * @brief Copy a buffer from packet memory area (PMA) to user memory area.
@@ -1085,7 +1299,6 @@ static bool dcd_write_packet_memory_ff(tu_fifo_t * ff, uint16_t dst, uint16_t wN
 static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, size_t wNBytes)
 {
   uint32_t n = (uint32_t)wNBytes >> 1U;
-  uint32_t i;
   // 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;
@@ -1094,7 +1307,7 @@ static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, size_t wN
   pdwVal = &pma[PMA_STRIDE*(src>>1)];
   uint8_t *dstVal = (uint8_t*)dst;
 
-  for (i = n; i != 0U; i--)
+  while (n--)
   {
     temp = *pdwVal;
     pdwVal += PMA_STRIDE;
@@ -1102,7 +1315,7 @@ static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, size_t wN
     *dstVal++ = ((temp >> 8) & 0xFF);
   }
 
-  if (wNBytes % 2)
+  if (wNBytes & 0x01)
   {
     temp = *pdwVal;
     pdwVal += PMA_STRIDE;
@@ -1111,52 +1324,59 @@ static bool dcd_read_packet_memory(void *__restrict dst, uint16_t src, size_t wN
   return true;
 }
 
-#if 0 // TODO support dcd_edpt_xfer_fifo API
 /**
   * @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
   */
-
-// THIS FUNCTION IS UNTESTED
-
 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
-  void * dst;
-  uint16_t len = tu_fifo_get_linear_write_info(ff, 0, &dst, wNBytes);           // THIS FUNCTION CHANGED!!!!
-  TU_VERIFY(len && dcd_read_packet_memory(dst, src, len));
-  tu_fifo_advance_write_pointer(ff, len);
+  tu_fifo_buffer_info_t info;
+  tu_fifo_get_write_info(ff, &info);  // We want to read from the FIFO
 
-  // Check for wrapped part
-  if (len < wNBytes)
+  uint16_t cnt_lin =  TU_MIN(wNBytes, info.len_lin);
+  uint16_t cnt_wrap = TU_MIN(wNBytes - cnt_lin, info.len_wrap);
+
+  // We want to read from PMA and write it into the FIFO, if LIN part is ODD and has WRAPPED part,
+  // last lin byte will be combined with wrapped part
+  // To ensure PMA is always access 16bit aligned (src aligned to 16 bit)
+  if((cnt_lin & 0x01) && cnt_wrap)
   {
-    // Get remaining wrapped length
-    uint16_t len2 = tu_fifo_get_linear_write_info(ff, 0, &dst, wNBytes - len);
-    TU_VERIFY(len2);
+    // Copy first linear part
+    dcd_read_packet_memory(info.ptr_lin, src, cnt_lin &~0x01);
+    src += cnt_lin &~0x01;
+
+    // Copy last linear byte & first wrapped byte
+    uint16_t tmp;
+    dcd_read_packet_memory(&tmp, src, 2);
+    
+    ((uint8_t*)info.ptr_lin)[cnt_lin - 1] = (uint8_t)tmp;
+    ((uint8_t*)info.ptr_wrap)[0] = (uint8_t)(tmp >> 8U);
+    src += 2;
 
-    // Update source pointer
-    src += len;
+    // Copy rest of wrapped byte
+    dcd_read_packet_memory(((uint8_t*)info.ptr_wrap) + 1, src, cnt_wrap - 1);
+  }
+  else
+  {
+    // Copy linear part
+    dcd_read_packet_memory(info.ptr_lin, src, cnt_lin);
+    src += cnt_lin;
 
-    // Since PMA is accessed 16-bit wise we need to handle the case when a 16 bit value was split
-    if (len % 2)    // If len is uneven there is a byte left to copy
+    if(info.len_wrap)
     {
-      uint32_t temp = pma[PMA_STRIDE*(src>>1)];
-      *((uint8_t *)dst++) = ((temp >> 8) & 0xFF);
-      src++;
-      len2--;
+      // Copy wrapped byte
+      dcd_read_packet_memory(info.ptr_wrap, src, cnt_wrap);
     }
-
-    TU_VERIFY(dcd_read_packet_memory(dst, src, len2));
-    tu_fifo_advance_write_pointer(ff, len2);
   }
 
+  tu_fifo_advance_write_pointer(ff, cnt_lin + cnt_wrap);
+
   return true;
 }
 
 #endif
 
-#endif
-
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
index bffae4500..015b177cf 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
@@ -1,12 +1,6 @@
 /**
-  ******************************************************************************
-  * @file    dcd_stm32f0_pvt_st.h
-  * @brief   DCD utilities from ST code
-  ******************************************************************************
-  * @attention
-  *
-  * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
-  * <h2><center>&copy; parts COPYRIGHT(c) N Conrad</center></h2>
+  * Copyright(c) 2016 STMicroelectronics
+  * Copyright(c) N Conrad
   *
   * Redistribution and use in source and binary forms, with or without modification,
   * are permitted provided that the following conditions are met:
@@ -30,7 +24,7 @@
   * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
   * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
   *
-  **********/
+  */
 
 // This file contains source copied from ST's HAL, and thus should have their copyright statement.
 
@@ -41,10 +35,7 @@
 #ifndef PORTABLE_ST_STM32F0_DCD_STM32F0_FSDEV_PVT_ST_H_
 #define PORTABLE_ST_STM32F0_DCD_STM32F0_FSDEV_PVT_ST_H_
 
-#if defined(STM32F042x6) || \
-    defined(STM32F070x6) || defined(STM32F070xB) || \
-    defined(STM32F072xB) || \
-    defined(STM32F078xx)
+#if CFG_TUSB_MCU == OPT_MCU_STM32F0
   #include "stm32f0xx.h"
   #define PMA_LENGTH (1024u)
   // F0x2 models are crystal-less
@@ -52,7 +43,7 @@
   // 070RB:    2 x 16 bits/word memory     LPM Support, BCD Support
   // PMA dedicated to USB (no sharing with CAN)
 
-#elif defined(STM32F1_FSDEV)
+#elif CFG_TUSB_MCU == OPT_MCU_STM32F1
   #include "stm32f1xx.h"
   #define PMA_LENGTH (512u)
   // NO internal Pull-ups
@@ -98,6 +89,10 @@
   #undef USB_PMAADDR
   #define USB_PMAADDR USB1_PMAADDR
 
+#elif CFG_TUSB_MCU == OPT_MCU_STM32L4
+  #include "stm32l4xx.h"
+  #define PMA_LENGTH (1024u)
+
 #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
@@ -116,76 +111,89 @@
 static __IO uint16_t * const pma = (__IO uint16_t*)USB_PMAADDR;
 
 // prototypes
-static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpNum);
-static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpNum);
-static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpNum, uint32_t wRegValue);
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx);
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx);
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wRegValue);
+
+/* 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;
+}
 
 /* SetENDPOINT */
-static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpNum, uint32_t wRegValue)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wRegValue)
 {
-  __O uint16_t *reg = (__O uint16_t *)((&USBx->EP0R) + bEpNum*2u);
+  __O uint16_t *reg = (__O uint16_t *)((&USBx->EP0R) + bEpIdx*2u);
   *reg = (uint16_t)wRegValue;
 }
 
 /* GetENDPOINT */
-static inline uint16_t pcd_get_endpoint(USB_TypeDef * USBx, uint32_t bEpNum) {
-  __I uint16_t *reg = (__I uint16_t *)((&USBx->EP0R) + bEpNum*2u);
+TU_ATTR_ALWAYS_INLINE static inline uint16_t pcd_get_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx) {
+  __I uint16_t *reg = (__I uint16_t *)((&USBx->EP0R) + bEpIdx*2u);
   return *reg;
 }
 
-static inline void pcd_set_eptype(USB_TypeDef * USBx, uint32_t bEpNum, uint32_t wType)
+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, bEpNum);
+  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, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
-static inline uint32_t pcd_get_eptype(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_eptype(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  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  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @retval None
   */
-static inline void pcd_clear_rx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpNum)
+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, bEpNum);
+  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, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
-static inline void pcd_clear_tx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpNum)
+
+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, bEpNum);
+  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, bEpNum,regVal);
+  pcd_set_endpoint(USBx, bEpIdx,regVal);
 }
 /**
   * @brief  gets counter of the tx buffer.
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @retval Counter value
   */
-static inline uint32_t pcd_get_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  __I uint16_t *regPtr = pcd_ep_tx_cnt_ptr(USBx, bEpNum);
+  __I uint16_t *regPtr = pcd_ep_tx_cnt_ptr(USBx, bEpIdx);
   return *regPtr & 0x3ffU;
 }
 
-static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  __I uint16_t *regPtr = pcd_ep_rx_cnt_ptr(USBx, bEpNum);
+  __I uint16_t *regPtr = pcd_ep_rx_cnt_ptr(USBx, bEpIdx);
   return *regPtr & 0x3ffU;
 }
 
@@ -196,49 +204,36 @@ static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpNum)
   * @param  wNBlocks no. of Blocks.
   * @retval None
   */
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_cnt_reg(__O uint16_t * pdwReg, size_t wCount)
+{
+  /* We assume that the buffer size is already aligned to hardware requirements. */
+  uint16_t blocksize = (wCount > 62) ? 1 : 0;
+  uint16_t numblocks = wCount / (blocksize ? 32 : 2);
 
-static inline void pcd_set_ep_cnt_rx_reg(__O uint16_t * pdwReg, size_t wCount)  {
-  uint32_t wNBlocks;
-  if(wCount > 62u)
-  {
-    wNBlocks = wCount >> 5u;
-    if((wCount & 0x1fU) == 0u)
-    {
-      wNBlocks--;
-    }
-    wNBlocks = wNBlocks << 10u;
-    wNBlocks |= 0x8000u; // Mark block size as 32byte
-    *pdwReg = (uint16_t)wNBlocks;
-  }
-  else
-  {
-    wNBlocks = wCount >> 1u;
-    if((wCount & 0x1U) != 0u)
-    {
-      wNBlocks++;
-    }
-    *pdwReg = (uint16_t)((wNBlocks) << 10u);
-  }
-}
+  /* There should be no remainder in the above calculation */
+  TU_ASSERT((wCount - (numblocks * (blocksize ? 32 : 2))) == 0, /**/);
 
+  /* Encode into register. When BLSIZE==1, we need to subtract 1 block count */
+  *pdwReg = (blocksize << 15) | ((numblocks - blocksize) << 10);
+}
 
 /**
   * @brief  Sets address in an endpoint register.
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @param  bAddr Address.
   * @retval None
   */
-static inline void pcd_set_ep_address(USB_TypeDef * USBx,  uint32_t bEpNum, uint32_t bAddr)
+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, bEpNum);
+  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, bEpNum,regVal);
+  pcd_set_endpoint(USBx, bEpIdx,regVal);
 }
 
-static inline __IO uint16_t * pcd_btable_word_ptr(USB_TypeDef * USBx, size_t x)
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t * pcd_btable_word_ptr(USB_TypeDef * USBx, size_t x)
 {
   size_t total_word_offset = (((USBx)->BTABLE)>>1) + x;
   total_word_offset *= PMA_STRIDE;
@@ -246,46 +241,61 @@ static inline __IO uint16_t * pcd_btable_word_ptr(USB_TypeDef * USBx, size_t x)
 }
 
 // Pointers to the PMA table entries (using the ARM address space)
-static inline __IO uint16_t* pcd_ep_tx_address_ptr(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_address_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
+{
+  return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 0u);
+}
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  return pcd_btable_word_ptr(USBx,(bEpNum)*4u + 0u);
+  return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 1u);
 }
-static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpNum)
+
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_address_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
+{
+  return  pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 2u);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  return pcd_btable_word_ptr(USBx,(bEpNum)*4u + 1u);
+  return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 3u);
 }
 
-static inline __IO uint16_t* pcd_ep_rx_address_ptr(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_cnt(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wCount)
 {
-  return  pcd_btable_word_ptr(USBx,(bEpNum)*4u + 2u);
+  __IO uint16_t * reg = pcd_ep_tx_cnt_ptr(USBx, bEpIdx);
+  *reg = (uint16_t) (*reg & (uint16_t) ~0x3FFU) | (wCount & 0x3FFU);
 }
 
-static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_cnt(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wCount)
 {
-  return pcd_btable_word_ptr(USBx,(bEpNum)*4u + 3u);
+  __IO uint16_t * reg = pcd_ep_rx_cnt_ptr(USBx, bEpIdx);
+  *reg = (uint16_t) (*reg & (uint16_t) ~0x3FFU) | (wCount & 0x3FFU);
 }
 
-static inline void pcd_set_ep_tx_cnt(USB_TypeDef * USBx,  uint32_t bEpNum, uint32_t wCount)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_bufsize(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wCount)
 {
-  *pcd_ep_tx_cnt_ptr(USBx, bEpNum) = (uint16_t)wCount;
+  __IO uint16_t *pdwReg = pcd_ep_tx_cnt_ptr((USBx),(bEpIdx));
+  wCount = pcd_aligned_buffer_size(wCount);
+  pcd_set_ep_cnt_reg(pdwReg, wCount);
 }
 
-static inline void pcd_set_ep_rx_cnt(USB_TypeDef * USBx,  uint32_t bEpNum, uint32_t wCount)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_bufsize(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wCount)
 {
-  __IO uint16_t *pdwReg = pcd_ep_rx_cnt_ptr((USBx),(bEpNum));
-  pcd_set_ep_cnt_rx_reg(pdwReg, wCount);
+  __IO uint16_t *pdwReg = pcd_ep_rx_cnt_ptr((USBx),(bEpIdx));
+  wCount = pcd_aligned_buffer_size(wCount);
+  pcd_set_ep_cnt_reg(pdwReg, wCount);
 }
 
 /**
   * @brief  sets the status for tx transfer (bits STAT_TX[1:0]).
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @param  wState new state
   * @retval None
   */
-static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx,  uint32_t bEpNum, uint32_t wState)
+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, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPTX_DTOGMASK;
 
   /* toggle first bit ? */
@@ -298,21 +308,22 @@ static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx,  uint32_t bEpNum, ui
   {
     regVal ^= USB_EPTX_DTOG2;
   }
+
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
-  pcd_set_endpoint(USBx, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 } /* pcd_set_ep_tx_status */
 
 /**
   * @brief  sets the status for rx transfer (bits STAT_TX[1:0])
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @param  wState new state
   * @retval None
   */
 
-static inline void pcd_set_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpNum, uint32_t wState)
+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, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPRX_DTOGMASK;
 
   /* toggle first bit ? */
@@ -325,13 +336,14 @@ static inline void pcd_set_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpNum, ui
   {
     regVal ^= USB_EPRX_DTOG2;
   }
+
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
-  pcd_set_endpoint(USBx, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 } /* pcd_set_ep_rx_status */
 
-static inline uint32_t pcd_get_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpNum)
+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, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   return (regVal & USB_EPRX_STAT) >> (12u);
 } /* pcd_get_ep_rx_status */
 
@@ -339,71 +351,71 @@ static inline uint32_t pcd_get_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpNum
 /**
   * @brief  Toggles DTOG_RX / DTOG_TX bit in the endpoint register.
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @retval None
   */
-static inline void pcd_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  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, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
-static inline void pcd_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  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, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
 /**
   * @brief  Clears DTOG_RX / DTOG_TX bit in the endpoint register.
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @retval None
   */
 
-static inline void pcd_clear_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   if((regVal & USB_EP_DTOG_RX) != 0)
   {
-    pcd_rx_dtog(USBx,bEpNum);
+    pcd_rx_dtog(USBx,bEpIdx);
   }
 }
 
-static inline void pcd_clear_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   if((regVal & USB_EP_DTOG_TX) != 0)
   {
-    pcd_tx_dtog(USBx,bEpNum);
+    pcd_tx_dtog(USBx,bEpIdx);
   }
 }
 
 /**
   * @brief  set & clear EP_KIND bit.
   * @param  USBx USB peripheral instance register address.
-  * @param  bEpNum Endpoint Number.
+  * @param  bEpIdx Endpoint Number.
   * @retval None
   */
 
-static inline void pcd_set_ep_kind(USB_TypeDef * USBx,  uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_kind(USB_TypeDef * USBx,  uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  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, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
-static inline void pcd_clear_ep_kind(USB_TypeDef * USBx, uint32_t bEpNum)
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_ep_kind(USB_TypeDef * USBx, uint32_t bEpIdx)
 {
-  uint32_t regVal = pcd_get_endpoint(USBx, bEpNum);
+  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, bEpNum, regVal);
+  pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
 // This checks if the device has "LPM"
@@ -417,6 +429,7 @@ static inline void pcd_clear_ep_kind(USB_TypeDef * USBx, uint32_t bEpNum)
      USB_ISTR_RESET | USB_ISTR_SOF | USB_ISTR_ESOF | USB_ISTR_L1REQ_FORCED )
 
 // Number of endpoints in hardware
+// TODO should use TUP_DCD_ENDPOINT_MAX
 #define STFSDEV_EP_COUNT (8u)
 
 #endif /* PORTABLE_ST_STM32F0_DCD_STM32F0_FSDEV_PVT_ST_H_ */
diff --git a/src/portable/sunxi/dcd_sunxi_musb.c b/src/portable/sunxi/dcd_sunxi_musb.c
index a0be846a5..2d0e379ad 100644
--- a/src/portable/sunxi/dcd_sunxi_musb.c
+++ b/src/portable/sunxi/dcd_sunxi_musb.c
@@ -243,7 +243,7 @@ static void USBC_Dev_SetAddress(u8 address)
 
 static void __USBC_Dev_Tx_SendStall(void)
 {
-	//send stall, and fifo is flushed automaticly
+	//send stall, and fifo is flushed automatically
 	USBC_REG_set_bit_w(USBC_BP_TXCSR_D_SEND_STALL, USBC_REG_TXCSR(USBC0_BASE));
 }
 static u32 __USBC_Dev_Tx_IsEpStall(void)
diff --git a/src/portable/sunxi/musb_def.h b/src/portable/sunxi/musb_def.h
index 602b4f113..53da5ded2 100644
--- a/src/portable/sunxi/musb_def.h
+++ b/src/portable/sunxi/musb_def.h
@@ -93,7 +93,7 @@
 #define  USBC1_BASE                 0x01c14000
 #define  USBC2_BASE                 0x01c1E000
 
-//Some reg whithin musb
+//Some reg within musb
 #define USBPHY_CLK_REG 		0x01c200CC
 #define USBPHY_CLK_RST_BIT 0
 #define USBPHY_CLK_GAT_BIT 1
diff --git a/src/portable/synopsys/dwc2/dcd_dwc2.c b/src/portable/synopsys/dwc2/dcd_dwc2.c
index e489f6445..c6132a1f5 100644
--- a/src/portable/synopsys/dwc2/dcd_dwc2.c
+++ b/src/portable/synopsys/dwc2/dcd_dwc2.c
@@ -34,6 +34,13 @@
 #include "device/dcd.h"
 #include "dwc2_type.h"
 
+// Following symbols must be defined by port header
+// - _dwc2_controller[]: array of controllers
+// - DWC2_EP_MAX: largest EP counts of all controllers
+// - dwc2_phy_init/dwc2_phy_update: phy init called before and after core reset
+// - dwc2_dcd_int_enable/dwc2_dcd_int_disable
+// - dwc2_remote_wakeup_delay
+
 #if defined(TUP_USBIP_DWC2_STM32)
   #include "dwc2_stm32.h"
 #elif TU_CHECK_MCU(OPT_MCU_ESP32S2, OPT_MCU_ESP32S3)
@@ -55,7 +62,7 @@
 //--------------------------------------------------------------------+
 
 // DWC2 registers
-#define DWC2_REG(_port)       ((dwc2_regs_t*) DWC2_REG_BASE)
+#define DWC2_REG(_port)       ((dwc2_regs_t*) _dwc2_controller[_port].reg_base)
 
 // Debug level for DWC2
 #define DWC2_DEBUG    2
@@ -72,7 +79,6 @@
 #define dcache_clean_invalidate(_addr, _size)
 #endif
 
-
 static TU_ATTR_ALIGNED(4) uint32_t _setup_packet[2];
 
 typedef struct {
@@ -97,34 +103,32 @@ static bool     _out_ep_closed;                   // Flag to check if RX FIFO si
 static bool _sof_en;
 
 // Calculate the RX FIFO size according to recommendations from reference manual
-static inline uint16_t calc_rx_ff_size(uint16_t ep_size)
+static inline uint16_t calc_grxfsiz(uint16_t max_ep_size, uint8_t ep_count)
 {
-  return 15 + 2*(ep_size/4) + 2*DWC2_EP_MAX;
+  return 15 + 2*(max_ep_size/4) + 2*ep_count;
 }
 
 static void update_grxfsiz(uint8_t rhport)
 {
-  (void) rhport;
-
-  dwc2_regs_t * dwc2 = DWC2_REG(rhport);
+  dwc2_regs_t * dwc2     = DWC2_REG(rhport);
+  uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
 
   // Determine largest EP size for RX FIFO
   uint16_t max_epsize = 0;
-  for (uint8_t epnum = 0; epnum < DWC2_EP_MAX; epnum++)
+  for (uint8_t epnum = 0; epnum < ep_count; epnum++)
   {
     max_epsize = tu_max16(max_epsize, xfer_status[epnum][TUSB_DIR_OUT].max_size);
   }
 
   // Update size of RX FIFO
-  dwc2->grxfsiz = calc_rx_ff_size(max_epsize);
+  dwc2->grxfsiz = calc_grxfsiz(max_epsize, ep_count);
 }
 
-// Setup the control endpoint 0.
+// Start of Bus Reset
 static void bus_reset(uint8_t rhport)
 {
-  (void) rhport;
-
-  dwc2_regs_t * dwc2 = DWC2_REG(rhport);
+  dwc2_regs_t * dwc2     = DWC2_REG(rhport);
+  uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
 
   tu_memclr(xfer_status, sizeof(xfer_status));
   _out_ep_closed = false;
@@ -135,7 +139,7 @@ static void bus_reset(uint8_t rhport)
   dwc2->dcfg &= ~DCFG_DAD_Msk;
 
   // 1. NAK for all OUT endpoints
-  for ( uint8_t n = 0; n < DWC2_EP_MAX; n++ )
+  for ( uint8_t n = 0; n < ep_count; n++ )
   {
     dwc2->epout[n].doepctl |= DOEPCTL_SNAK;
   }
@@ -185,22 +189,24 @@ static void bus_reset(uint8_t rhport)
   //   - 2 for each used OUT endpoint
   //
   //   Therefore GRXFSIZ = 13 + 1 + 1 + 2 x (Largest-EPsize/4) + 2 x EPOUTnum
-  //   - FullSpeed (64 Bytes ): GRXFSIZ = 15 + 2 x  16 + 2 x DWC2_EP_MAX = 47  + 2 x DWC2_EP_MAX
-  //   - Highspeed (512 bytes): GRXFSIZ = 15 + 2 x 128 + 2 x DWC2_EP_MAX = 271 + 2 x DWC2_EP_MAX
+  //   - FullSpeed (64 Bytes ): GRXFSIZ = 15 + 2 x  16 + 2 x ep_count = 47  + 2 x ep_count
+  //   - Highspeed (512 bytes): GRXFSIZ = 15 + 2 x 128 + 2 x ep_count = 271 + 2 x ep_count
   //
   //   NOTE: Largest-EPsize & EPOUTnum is actual used endpoints in configuration. Since DCD has no knowledge
-  //   of the overall picture yet. We will use the worst scenario: largest possible + DWC2_EP_MAX
+  //   of the overall picture yet. We will use the worst scenario: largest possible + ep_count
   //
   //   For Isochronous, largest EP size can be 1023/1024 for FS/HS respectively. In addition if multiple ISO
   //   are enabled at least "2 x (Largest-EPsize/4) + 1" are recommended.  Maybe provide a macro for application to
   //   overwrite this.
 
-  dwc2->grxfsiz = calc_rx_ff_size(TUD_OPT_HIGH_SPEED ? 512 : 64);
+  // EP0 out max is 64
+  dwc2->grxfsiz = calc_grxfsiz(64, ep_count);
 
+  // Setup the control endpoint 0
   _allocated_fifo_words_tx = 16;
 
   // Control IN uses FIFO 0 with 64 bytes ( 16 32-bit word )
-  dwc2->dieptxf0 = (16 << DIEPTXF0_TX0FD_Pos) | (DWC2_EP_FIFO_SIZE/4 - _allocated_fifo_words_tx);
+  dwc2->dieptxf0 = (16 << DIEPTXF0_TX0FD_Pos) | (_dwc2_controller[rhport].ep_fifo_size/4 - _allocated_fifo_words_tx);
 
   // Fixed control EP0 size to 64 bytes
   dwc2->epin[0].diepctl &= ~(0x03 << DIEPCTL_MPSIZ_Pos);
@@ -363,7 +369,11 @@ static void reset_core(dwc2_regs_t * dwc2)
 static bool phy_hs_supported(dwc2_regs_t * dwc2)
 {
   // note: esp32 incorrect report its hs_phy_type as utmi
+#if TU_CHECK_MCU(OPT_MCU_ESP32S2, OPT_MCU_ESP32S3)
+  return false;
+#else
   return TUD_OPT_HIGH_SPEED && dwc2->ghwcfg2_bm.hs_phy_type != HS_PHY_TYPE_NONE;
+#endif
 }
 
 static void phy_fs_init(dwc2_regs_t * dwc2)
@@ -462,6 +472,7 @@ static bool check_dwc2(dwc2_regs_t * dwc2)
 #endif
 
   // For some reasons: GD32VF103 snpsid and all hwcfg register are always zero (skip it)
+  (void) dwc2;
 #if !TU_CHECK_MCU(OPT_MCU_GD32VF103)
   uint32_t const gsnpsid = dwc2->gsnpsid & GSNPSID_ID_MASK;
   TU_ASSERT(gsnpsid == DWC2_OTG_ID || gsnpsid == DWC2_FS_IOT_ID || gsnpsid == DWC2_HS_IOT_ID);
@@ -518,8 +529,10 @@ void dcd_init (uint8_t rhport)
   dwc2->dcfg |= DCFG_NZLSOHSK;
 
   // Clear all interrupts
-  dwc2->gintsts |= dwc2->gintsts;
-  dwc2->gotgint |= dwc2->gotgint;
+  uint32_t int_mask = dwc2->gintsts;
+  dwc2->gintsts |= int_mask;
+  int_mask = dwc2->gotgint;
+  dwc2->gotgint |= int_mask;
 
   // Required as part of core initialization.
   // TODO: How should mode mismatch be handled? It will cause
@@ -620,12 +633,13 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
 {
   (void) rhport;
 
-  dwc2_regs_t * dwc2 = DWC2_REG(rhport);
+  dwc2_regs_t * dwc2     = DWC2_REG(rhport);
+  uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
 
   uint8_t const epnum = tu_edpt_number(desc_edpt->bEndpointAddress);
   uint8_t const dir   = tu_edpt_dir(desc_edpt->bEndpointAddress);
 
-  TU_ASSERT(epnum < DWC2_EP_MAX);
+  TU_ASSERT(epnum < ep_count);
 
   xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir);
   xfer->max_size = tu_edpt_packet_size(desc_edpt);
@@ -636,12 +650,12 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
   if(dir == TUSB_DIR_OUT)
   {
     // Calculate required size of RX FIFO
-    uint16_t const sz = calc_rx_ff_size(4*fifo_size);
+    uint16_t const sz = calc_grxfsiz(4*fifo_size, ep_count);
 
     // If size_rx needs to be extended check if possible and if so enlarge it
     if (dwc2->grxfsiz < sz)
     {
-      TU_ASSERT(sz + _allocated_fifo_words_tx <= DWC2_EP_FIFO_SIZE/4);
+      TU_ASSERT(sz + _allocated_fifo_words_tx <= _dwc2_controller[rhport].ep_fifo_size/4);
 
       // Enlarge RX FIFO
       dwc2->grxfsiz = sz;
@@ -678,15 +692,15 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
     // - IN EP 1 gets FIFO 1, IN EP "n" gets FIFO "n".
 
     // Check if free space is available
-    TU_ASSERT(_allocated_fifo_words_tx + fifo_size + dwc2->grxfsiz <= DWC2_EP_FIFO_SIZE/4);
+    TU_ASSERT(_allocated_fifo_words_tx + fifo_size + dwc2->grxfsiz <= _dwc2_controller[rhport].ep_fifo_size/4);
 
     _allocated_fifo_words_tx += fifo_size;
 
-    TU_LOG(DWC2_DEBUG, "    Allocated %u bytes at offset %u", fifo_size*4, DWC2_EP_FIFO_SIZE-_allocated_fifo_words_tx*4);
+    TU_LOG(DWC2_DEBUG, "    Allocated %u bytes at offset %lu", fifo_size*4, _dwc2_controller[rhport].ep_fifo_size-_allocated_fifo_words_tx*4);
 
     // DIEPTXF starts at FIFO #1.
     // Both TXFD and TXSA are in unit of 32-bit words.
-    dwc2->dieptxf[epnum - 1] = (fifo_size << DIEPTXF_INEPTXFD_Pos) | (DWC2_EP_FIFO_SIZE/4 - _allocated_fifo_words_tx);
+    dwc2->dieptxf[epnum - 1] = (fifo_size << DIEPTXF_INEPTXFD_Pos) | (_dwc2_controller[rhport].ep_fifo_size/4 - _allocated_fifo_words_tx);
 
     dwc2->epin[epnum].diepctl |= (1 << DIEPCTL_USBAEP_Pos) |
                                  (epnum << DIEPCTL_TXFNUM_Pos) |
@@ -703,14 +717,13 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
 // Close all non-control endpoints, cancel all pending transfers if any.
 void dcd_edpt_close_all (uint8_t rhport)
 {
-  (void) rhport;
-
-  dwc2_regs_t * dwc2 = DWC2_REG(rhport);
+  dwc2_regs_t * dwc2     = DWC2_REG(rhport);
+  uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
 
   // Disable non-control interrupt
   dwc2->daintmsk = (1 << DAINTMSK_OEPM_Pos) | (1 << DAINTMSK_IEPM_Pos);
 
-  for(uint8_t n = 1; n < DWC2_EP_MAX; n++)
+  for(uint8_t n = 1; n < ep_count; n++)
   {
     // disable OUT endpoint
     dwc2->epout[n].doepctl = 0;
@@ -871,8 +884,9 @@ void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr)
   {
     uint16_t const fifo_size = (dwc2->dieptxf[epnum - 1] & DIEPTXF_INEPTXFD_Msk) >> DIEPTXF_INEPTXFD_Pos;
     uint16_t const fifo_start = (dwc2->dieptxf[epnum - 1] & DIEPTXF_INEPTXSA_Msk) >> DIEPTXF_INEPTXSA_Pos;
+
     // For now only the last opened endpoint can be closed without fuss.
-    TU_ASSERT(fifo_start == DWC2_EP_FIFO_SIZE/4 - _allocated_fifo_words_tx,);
+    TU_ASSERT(fifo_start == _dwc2_controller[rhport].ep_fifo_size/4 - _allocated_fifo_words_tx,);
     _allocated_fifo_words_tx -= fifo_size;
   }
   else
@@ -991,7 +1005,7 @@ static void handle_rxflvl_irq(uint8_t rhport)
 
   switch ( pktsts )
   {
-    // Global OUT NAK: do nothign
+    // Global OUT NAK: do nothing
     case GRXSTS_PKTSTS_GLOBALOUTNAK: break;
 
     case GRXSTS_PKTSTS_SETUPRX:
@@ -1075,11 +1089,12 @@ static void handle_rxflvl_irq(uint8_t rhport)
 
 static void handle_epout_irq (uint8_t rhport)
 {
-  dwc2_regs_t *dwc2 = DWC2_REG(rhport);
+  dwc2_regs_t * dwc2     = DWC2_REG(rhport);
+  uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
 
   // DAINT for a given EP clears when DOEPINTx is cleared.
   // OEPINT will be cleared when DAINT's out bits are cleared.
-  for ( uint8_t n = 0; n < DWC2_EP_MAX; n++ )
+  for ( uint8_t n = 0; n < ep_count; n++ )
   {
     if ( dwc2->daint & TU_BIT(DAINT_OEPINT_Pos + n) )
     {
@@ -1126,12 +1141,13 @@ static void handle_epout_irq (uint8_t rhport)
 
 static void handle_epin_irq (uint8_t rhport)
 {
-  dwc2_regs_t *dwc2 = DWC2_REG(rhport);
-  dwc2_epin_t* epin = dwc2->epin;
+  dwc2_regs_t * dwc2     = DWC2_REG(rhport);
+  uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
+  dwc2_epin_t* epin      = dwc2->epin;
 
   // DAINT for a given EP clears when DIEPINTx is cleared.
   // IEPINT will be cleared when DAINT's out bits are cleared.
-  for ( uint8_t n = 0; n < DWC2_EP_MAX; n++ )
+  for ( uint8_t n = 0; n < ep_count; n++ )
   {
     if ( dwc2->daint & TU_BIT(DAINT_IEPINT_Pos + n) )
     {
@@ -1205,7 +1221,8 @@ void dcd_int_handler(uint8_t rhport)
 {
   dwc2_regs_t *dwc2 = DWC2_REG(rhport);
 
-  uint32_t const int_status = dwc2->gintsts & dwc2->gintmsk;
+  uint32_t const int_mask = dwc2->gintmsk;
+  uint32_t const int_status = dwc2->gintsts & int_mask;
 
   if(int_status & GINTSTS_USBRST)
   {
diff --git a/src/portable/synopsys/dwc2/dwc2_bcm.h b/src/portable/synopsys/dwc2/dwc2_bcm.h
index 14194e754..732d96ae0 100644
--- a/src/portable/synopsys/dwc2/dwc2_bcm.h
+++ b/src/portable/synopsys/dwc2/dwc2_bcm.h
@@ -35,9 +35,12 @@
 #include "broadcom/interrupts.h"
 #include "broadcom/caches.h"
 
-#define DWC2_REG_BASE       USB_OTG_GLOBAL_BASE
 #define DWC2_EP_MAX         8
-#define DWC2_EP_FIFO_SIZE   4096
+
+static const dwc2_controller_t _dwc2_controller[] =
+{
+  { .reg_base = USB_OTG_GLOBAL_BASE, .irqnum = USB_IRQn, .ep_count = DWC2_EP_MAX, .ep_fifo_size = 4096 }
+};
 
 #define dcache_clean(_addr, _size)              data_clean(_addr, _size)
 #define dcache_invalidate(_addr, _size)         data_invalidate(_addr, _size)
@@ -46,15 +49,13 @@
 TU_ATTR_ALWAYS_INLINE
 static inline void dwc2_dcd_int_enable(uint8_t rhport)
 {
-  (void) rhport;
-  BP_EnableIRQ(USB_IRQn);
+  BP_EnableIRQ(_dwc2_controller[rhport].irqnum);
 }
 
 TU_ATTR_ALWAYS_INLINE
 static inline void dwc2_dcd_int_disable (uint8_t rhport)
 {
-  (void) rhport;
-  BP_DisableIRQ(USB_IRQn);
+  BP_DisableIRQ(_dwc2_controller[rhport].irqnum);
 }
 
 static inline void dwc2_remote_wakeup_delay(void)
diff --git a/src/portable/synopsys/dwc2/dwc2_efm32.h b/src/portable/synopsys/dwc2/dwc2_efm32.h
index ee4c3c715..0e3570cbb 100644
--- a/src/portable/synopsys/dwc2/dwc2_efm32.h
+++ b/src/portable/synopsys/dwc2/dwc2_efm32.h
@@ -37,20 +37,22 @@
 // EFM32 has custom control register before DWC registers
 #define DWC2_REG_BASE       (USB_BASE + offsetof(USB_TypeDef, GOTGCTL))
 #define DWC2_EP_MAX         7
-#define DWC2_EP_FIFO_SIZE   2048
+
+static const dwc2_controller_t _dwc2_controller[] =
+{
+  { .reg_base = DWC2_REG_BASE, .irqnum = USB_IRQn, .ep_count = DWC2_EP_MAX, .ep_fifo_size = 2048 }
+};
 
 TU_ATTR_ALWAYS_INLINE
 static inline void dwc2_dcd_int_enable(uint8_t rhport)
 {
-  (void) rhport;
-  NVIC_EnableIRQ(USB_IRQn);
+  NVIC_EnableIRQ(_dwc2_controller[rhport].irqnum);
 }
 
 TU_ATTR_ALWAYS_INLINE
 static inline void dwc2_dcd_int_disable (uint8_t rhport)
 {
-  (void) rhport;
-  NVIC_DisableIRQ(USB_IRQn);
+  NVIC_DisableIRQ(_dwc2_controller[rhport].irqnum);
 }
 
 static inline void dwc2_remote_wakeup_delay(void)
diff --git a/src/portable/synopsys/dwc2/dwc2_esp32.h b/src/portable/synopsys/dwc2/dwc2_esp32.h
index 78da277d5..c50dd66b8 100644
--- a/src/portable/synopsys/dwc2/dwc2_esp32.h
+++ b/src/portable/synopsys/dwc2/dwc2_esp32.h
@@ -37,10 +37,12 @@
 //#include "soc/usb_periph.h"
 
 #define DWC2_REG_BASE       0x60080000UL
-#define DWC2_EP_MAX         5             // USB_OUT_EP_NUM
-#define DWC2_EP_FIFO_SIZE   1024
+#define DWC2_EP_MAX         6             // USB_OUT_EP_NUM. TODO ESP32Sx only has 5 tx fifo (5 endpoint IN)
 
-// #define EP_FIFO_NUM 5
+static const dwc2_controller_t _dwc2_controller[] =
+{
+  { .reg_base = DWC2_REG_BASE, .irqnum = 0, .ep_count = DWC2_EP_MAX, .ep_fifo_size = 1024 }
+};
 
 static intr_handle_t usb_ih;
 
diff --git a/src/portable/synopsys/dwc2/dwc2_gd32.h b/src/portable/synopsys/dwc2/dwc2_gd32.h
index f8fa01ee0..0375fffe4 100644
--- a/src/portable/synopsys/dwc2/dwc2_gd32.h
+++ b/src/portable/synopsys/dwc2/dwc2_gd32.h
@@ -34,8 +34,11 @@
 
 #define DWC2_REG_BASE       0x50000000UL
 #define DWC2_EP_MAX         4
-#define DWC2_EP_FIFO_SIZE   1280
-#define RHPORT_IRQn         86
+
+static const dwc2_controller_t _dwc2_controller[] =
+{
+  { .reg_base = DWC2_REG_BASE, .irqnum = 86, .ep_count = DWC2_EP_MAX, .ep_fifo_size = 1280 }
+};
 
 extern uint32_t SystemCoreClock;
 
@@ -57,15 +60,13 @@ static inline void __eclic_disable_interrupt (uint32_t irq){
 TU_ATTR_ALWAYS_INLINE
 static inline void dwc2_dcd_int_enable(uint8_t rhport)
 {
-  (void) rhport;
-  __eclic_enable_interrupt(RHPORT_IRQn);
+  __eclic_enable_interrupt(_dwc2_controller[rhport].irqnum);
 }
 
 TU_ATTR_ALWAYS_INLINE
 static inline void dwc2_dcd_int_disable (uint8_t rhport)
 {
-  (void) rhport;
-  __eclic_disable_interrupt(RHPORT_IRQn);
+  __eclic_disable_interrupt(_dwc2_controller[rhport].irqnum);
 }
 
 static inline void dwc2_remote_wakeup_delay(void)
diff --git a/src/portable/synopsys/dwc2/dwc2_stm32.h b/src/portable/synopsys/dwc2/dwc2_stm32.h
index d54bf1fd7..cb455bd90 100644
--- a/src/portable/synopsys/dwc2/dwc2_stm32.h
+++ b/src/portable/synopsys/dwc2/dwc2_stm32.h
@@ -43,10 +43,14 @@
   #define EP_MAX_FS       USB_OTG_FS_MAX_IN_ENDPOINTS
   #define EP_FIFO_SIZE_FS USB_OTG_FS_TOTAL_FIFO_SIZE
 
+  #define EP_MAX_HS       USB_OTG_HS_MAX_IN_ENDPOINTS
+  #define EP_FIFO_SIZE_HS USB_OTG_HS_TOTAL_FIFO_SIZE
+
 #elif CFG_TUSB_MCU == OPT_MCU_STM32F4
   #include "stm32f4xx.h"
   #define EP_MAX_FS       USB_OTG_FS_MAX_IN_ENDPOINTS
   #define EP_FIFO_SIZE_FS USB_OTG_FS_TOTAL_FIFO_SIZE
+
   #define EP_MAX_HS       USB_OTG_HS_MAX_IN_ENDPOINTS
   #define EP_FIFO_SIZE_HS USB_OTG_HS_TOTAL_FIFO_SIZE
 
@@ -54,11 +58,13 @@
   #include "stm32h7xx.h"
   #define EP_MAX_FS       9
   #define EP_FIFO_SIZE_FS 4096
+
   #define EP_MAX_HS       9
   #define EP_FIFO_SIZE_HS 4096
+
+  // NOTE: H7 with only 1 USB port: H72x / H73x / H7Ax / H7Bx
+  // USB_OTG_FS_PERIPH_BASE and OTG_FS_IRQn not defined
   #if (! defined USB2_OTG_FS)
-    // H7 with only 1 USB port: H72x / H73x / H7Ax / H7Bx
-    // USB_OTG_FS_PERIPH_BASE and OTG_FS_IRQn not defined
     #define USB_OTG_FS_PERIPH_BASE  USB1_OTG_HS_PERIPH_BASE
     #define OTG_FS_IRQn             OTG_HS_IRQn
   #endif
@@ -67,6 +73,7 @@
   #include "stm32f7xx.h"
   #define EP_MAX_FS       6
   #define EP_FIFO_SIZE_FS 1280
+
   #define EP_MAX_HS       9
   #define EP_FIFO_SIZE_HS 4096
 
@@ -75,39 +82,53 @@
   #define EP_MAX_FS       6
   #define EP_FIFO_SIZE_FS 1280
 
+#elif CFG_TUSB_MCU == OPT_MCU_STM32U5
+  #include "stm32u5xx.h"
+  #define USB_OTG_FS_PERIPH_BASE       USB_OTG_FS_BASE
+  #define EP_MAX_FS       6
+  #define EP_FIFO_SIZE_FS 1280
+
 #else
   #error "Unsupported MCUs"
 #endif
 
-// On STM32 we associate Port0 to OTG_FS, and Port1 to OTG_HS
-#if TUD_OPT_RHPORT == 0
-  #define DWC2_REG_BASE       USB_OTG_FS_PERIPH_BASE
-  #define DWC2_EP_MAX         EP_MAX_FS
-  #define DWC2_EP_FIFO_SIZE   EP_FIFO_SIZE_FS
-  #define RHPORT_IRQn         OTG_FS_IRQn
-
+// OTG HS always has higher number of endpoints than FS
+#ifdef USB_OTG_HS_PERIPH_BASE
+  #define DWC2_EP_MAX   EP_MAX_HS
 #else
-  #define DWC2_REG_BASE       USB_OTG_HS_PERIPH_BASE
-  #define DWC2_EP_MAX         EP_MAX_HS
-  #define DWC2_EP_FIFO_SIZE   EP_FIFO_SIZE_HS
-  #define RHPORT_IRQn         OTG_HS_IRQn
+  #define DWC2_EP_MAX   EP_MAX_FS
+#endif
 
+// On STM32 for consistency we associate
+// - Port0 to OTG_FS, and Port1 to OTG_HS
+static const dwc2_controller_t _dwc2_controller[] =
+{
+#ifdef USB_OTG_FS_PERIPH_BASE
+  { .reg_base = USB_OTG_FS_PERIPH_BASE, .irqnum = OTG_FS_IRQn, .ep_count = EP_MAX_FS, .ep_fifo_size = EP_FIFO_SIZE_FS },
+#endif
+
+#ifdef USB_OTG_HS_PERIPH_BASE
+  { .reg_base = USB_OTG_HS_PERIPH_BASE, .irqnum = OTG_HS_IRQn, .ep_count = EP_MAX_HS, .ep_fifo_size = EP_FIFO_SIZE_HS },
 #endif
+};
+
+//--------------------------------------------------------------------+
+//
+//--------------------------------------------------------------------+
 
-extern uint32_t SystemCoreClock;
+// SystemCoreClock is already included by family header
+// extern uint32_t SystemCoreClock;
 
 TU_ATTR_ALWAYS_INLINE
 static inline void dwc2_dcd_int_enable(uint8_t rhport)
 {
-  (void) rhport;
-  NVIC_EnableIRQ(RHPORT_IRQn);
+  NVIC_EnableIRQ((IRQn_Type)_dwc2_controller[rhport].irqnum);
 }
 
 TU_ATTR_ALWAYS_INLINE
 static inline void dwc2_dcd_int_disable (uint8_t rhport)
 {
-  (void) rhport;
-  NVIC_DisableIRQ(RHPORT_IRQn);
+  NVIC_DisableIRQ((IRQn_Type)_dwc2_controller[rhport].irqnum);
 }
 
 TU_ATTR_ALWAYS_INLINE
diff --git a/src/portable/synopsys/dwc2/dwc2_type.h b/src/portable/synopsys/dwc2/dwc2_type.h
index 7fa2028eb..3fc979337 100644
--- a/src/portable/synopsys/dwc2/dwc2_type.h
+++ b/src/portable/synopsys/dwc2/dwc2_type.h
@@ -19,6 +19,19 @@
 
 #include "stdint.h"
 
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+// Controller
+typedef struct
+{
+  uintptr_t reg_base;
+  uint32_t  irqnum;
+  uint8_t   ep_count;
+  uint32_t  ep_fifo_size;
+}dwc2_controller_t;
+
 /* DWC OTG HW Release versions */
 #define DWC2_CORE_REV_2_71a   0x4f54271a
 #define DWC2_CORE_REV_2_72a   0x4f54272a
@@ -40,10 +53,6 @@
 #define DWC2_FS_IOT_ID        0x55310000
 #define DWC2_HS_IOT_ID        0x55320000
 
-#ifdef __cplusplus
- extern "C" {
-#endif
-
 #if 0
 // HS PHY
 typedef struct
diff --git a/src/portable/synopsys/dwc2/dwc2_xmc.h b/src/portable/synopsys/dwc2/dwc2_xmc.h
index 4e6bebb01..63419abf7 100644
--- a/src/portable/synopsys/dwc2/dwc2_xmc.h
+++ b/src/portable/synopsys/dwc2/dwc2_xmc.h
@@ -34,23 +34,24 @@
 
 #include "xmc_device.h"
 
-// XMC has custom control register before DWC registers
-#define DWC2_REG_BASE       USB0_BASE
 #define DWC2_EP_MAX         7
-#define DWC2_EP_FIFO_SIZE   2048
+
+static const dwc2_controller_t _dwc2_controller[] =
+{
+  // Note: XMC has some custom control registers before DWC registers
+  { .reg_base = USB0_BASE, .irqnum = USB0_0_IRQn, .ep_count = DWC2_EP_MAX, .ep_fifo_size = 2048 }
+};
 
 TU_ATTR_ALWAYS_INLINE
 static inline void dwc2_dcd_int_enable(uint8_t rhport)
 {
-  (void) rhport;
-  NVIC_EnableIRQ(USB0_0_IRQn);
+  NVIC_EnableIRQ(_dwc2_controller[rhport].irqnum);
 }
 
 TU_ATTR_ALWAYS_INLINE
 static inline void dwc2_dcd_int_disable (uint8_t rhport)
 {
-  (void) rhport;
-  NVIC_DisableIRQ(USB0_0_IRQn);
+  NVIC_DisableIRQ(_dwc2_controller[rhport].irqnum);
 }
 
 static inline void dwc2_remote_wakeup_delay(void)
diff --git a/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c b/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
index 605c13b4a..b4dfda575 100644
--- a/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
+++ b/src/portable/ti/msp430x5xx/dcd_msp430x5xx.c
@@ -631,7 +631,18 @@ void dcd_int_handler(uint8_t rhport)
     handle_setup_packet();
   }
 
-  uint16_t curr_vector = USBVECINT;
+  // Workaround possible bug in MSP430 GCC 9.3.0 where volatile variable
+  // USBVECINT is read from twice when only once is intended. The second
+  // (garbage) read seems to be triggered by certain switch statement
+  // configurations.
+  uint16_t curr_vector;
+  #if __GNUC__ > 9 || (__GNUC__ == 9 && __GNUC_MINOR__ > 2)
+    asm volatile ("mov %1, %0"
+                  : "=r" (curr_vector)
+                  : "m" (USBVECINT));
+  #else
+    curr_vector = USBVECINT;
+  #endif
 
   switch(curr_vector)
   {
diff --git a/src/portable/valentyusb/eptri/dcd_eptri.c b/src/portable/valentyusb/eptri/dcd_eptri.c
index e368b5f39..a3f228dd9 100644
--- a/src/portable/valentyusb/eptri/dcd_eptri.c
+++ b/src/portable/valentyusb/eptri/dcd_eptri.c
@@ -465,7 +465,7 @@ void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
       enable = 1;
     usb_out_ctrl_write((0 << CSR_USB_OUT_CTRL_STALL_OFFSET) | (enable << CSR_USB_OUT_CTRL_ENABLE_OFFSET) | tu_edpt_number(ep_addr));
   }
-  // IN endpoints will get unstalled when more data is written.
+  // IN endpoints will get un-stalled when more data is written.
 }
 
 bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes)
diff --git a/src/portable/wch/ch32v307/ch32_usbhs_reg.h b/src/portable/wch/ch32v307/ch32_usbhs_reg.h
new file mode 100644
index 000000000..5a2c1fbc9
--- /dev/null
+++ b/src/portable/wch/ch32v307/ch32_usbhs_reg.h
@@ -0,0 +1,345 @@
+#ifndef _USB_CH32_USBHS_REG_H
+#define _USB_CH32_USBHS_REG_H
+
+#include <ch32v30x.h>
+
+/******************* GLOBAL ******************/
+
+// USB CONTROL
+#define USBHS_CONTROL_OFFSET 0x00
+#define USBHS_DMA_EN         (1 << 0)
+#define USBHS_ALL_CLR        (1 << 1)
+#define USBHS_FORCE_RST      (1 << 2)
+#define USBHS_INT_BUSY_EN    (1 << 3)
+#define USBHS_DEV_PU_EN      (1 << 4)
+#define USBHS_SPEED_MASK     (3 << 5)
+#define USBHS_FULL_SPEED     (0 << 5)
+#define USBHS_HIGH_SPEED     (1 << 5)
+#define USBHS_LOW_SPEED      (2 << 5)
+#define USBHS_HOST_MODE      (1 << 7)
+
+// USB_INT_EN
+#define USBHS_INT_EN_OFFSET 0x02
+#define USBHS_BUS_RST_EN    (1 << 0)
+#define USBHS_DETECT_EN     (1 << 0)
+#define USBHS_TRANSFER_EN   (1 << 1)
+#define USBHS_SUSPEND_EN    (1 << 2)
+#define USBHS_SOF_ACT_EN    (1 << 3)
+#define USBHS_FIFO_OV_EN    (1 << 4)
+#define USBHS_SETUP_ACT_EN  (1 << 5)
+#define USBHS_ISO_ACT_EN    (1 << 6)
+#define USBHS_DEV_NAK_EN    (1 << 7)
+
+// USB DEV AD
+#define USBHS_DEV_AD_OFFSET 0x03
+// USB FRAME_NO
+#define USBHS_FRAME_NO_OFFSET 0x04
+// USB SUSPEND
+#define USBHS_SUSPEND_OFFSET    0x06
+#define USBHS_DEV_REMOTE_WAKEUP (1 << 2)
+#define USBHS_LINESTATE_MASK    (2 << 4) /* Read Only */
+
+// RESERVED0
+
+// USB SPEED TYPE
+#define USBHS_SPEED_TYPE_OFFSET 0x08
+#define USBSPEED_MASK           (0x03)
+
+// USB_MIS_ST
+#define USBHS_MIS_ST_OFFSET 0x09
+#define USBHS_SPLIT_CAN     (1 << 0)
+#define USBHS_ATTACH        (1 << 1)
+#define USBHS_SUSPEND       (1 << 2)
+#define USBHS_BUS_RESET     (1 << 3)
+#define USBHS_R_FIFO_RDY    (1 << 4)
+#define USBHS_SIE_FREE      (1 << 5)
+#define USBHS_SOF_ACT       (1 << 6)
+#define USBHS_SOF_PRES      (1 << 7)
+
+// INT_FLAG
+#define USBHS_INT_FLAG_OFFSET 0x0A
+#define USBHS_BUS_RST_FLAG    (1 << 0)
+#define USBHS_DETECT_FLAG     (1 << 0)
+#define USBHS_TRANSFER_FLAG   (1 << 1)
+#define USBHS_SUSPEND_FLAG    (1 << 2)
+#define USBHS_HST_SOF_FLAG    (1 << 3)
+#define USBHS_FIFO_OV_FLAG    (1 << 4)
+#define USBHS_SETUP_FLAG      (1 << 5)
+#define USBHS_ISO_ACT_FLAG    (1 << 6)
+
+// INT_ST
+#define USBHS_INT_ST_OFFSET  0x0B
+#define USBHS_DEV_UIS_IS_NAK (1 << 7)
+#define USBHS_DEV_UIS_TOG_OK (1 << 6)
+#define MASK_UIS_TOKEN       (3 << 4)
+#define MASK_UIS_ENDP        (0x0F)
+#define MASK_UIS_H_RES       (0x0F)
+
+#define USBHS_TOGGLE_OK (0x40)
+#define USBHS_HOST_RES  (0x0f)
+
+//USB_RX_LEN
+#define USBHS_RX_LEN_OFFSET 0x0C
+/******************* DEVICE ******************/
+
+//UEP_CONFIG
+#define USBHS_UEP_CONFIG_OFFSET 0x10
+#define USBHS_EP0_T_EN          (1 << 0)
+#define USBHS_EP0_R_EN          (1 << 16)
+
+#define USBHS_EP1_T_EN (1 << 1)
+#define USBHS_EP1_R_EN (1 << 17)
+
+#define USBHS_EP2_T_EN (1 << 2)
+#define USBHS_EP2_R_EN (1 << 18)
+
+#define USBHS_EP3_T_EN (1 << 3)
+#define USBHS_EP3_R_EN (1 << 19)
+
+#define USBHS_EP4_T_EN (1 << 4)
+#define USBHS_EP4_R_EN (1 << 20)
+
+#define USBHS_EP5_T_EN (1 << 5)
+#define USBHS_EP5_R_EN (1 << 21)
+
+#define USBHS_EP6_T_EN (1 << 6)
+#define USBHS_EP6_R_EN (1 << 22)
+
+#define USBHS_EP7_T_EN (1 << 7)
+#define USBHS_EP7_R_EN (1 << 23)
+
+#define USBHS_EP8_T_EN (1 << 8)
+#define USBHS_EP8_R_EN (1 << 24)
+
+#define USBHS_EP9_T_EN (1 << 9)
+#define USBHS_EP9_R_EN (1 << 25)
+
+#define USBHS_EP10_T_EN (1 << 10)
+#define USBHS_EP10_R_EN (1 << 26)
+
+#define USBHS_EP11_T_EN (1 << 11)
+#define USBHS_EP11_R_EN (1 << 27)
+
+#define USBHS_EP12_T_EN (1 << 12)
+#define USBHS_EP12_R_EN (1 << 28)
+
+#define USBHS_EP13_T_EN (1 << 13)
+#define USBHS_EP13_R_EN (1 << 29)
+
+#define USBHS_EP14_T_EN (1 << 14)
+#define USBHS_EP14_R_EN (1 << 30)
+
+#define USBHS_EP15_T_EN (1 << 15)
+#define USBHS_EP15_R_EN (1 << 31)
+
+//UEP_TYPE
+#define USBHS_UEP_TYPE_OFFSET 0x14
+#define USBHS_EP0_T_TYP       (1 << 0)
+#define USBHS_EP0_R_TYP       (1 << 16)
+
+#define USBHS_EP1_T_TYP (1 << 1)
+#define USBHS_EP1_R_TYP (1 << 17)
+
+#define USBHS_EP2_T_TYP (1 << 2)
+#define USBHS_EP2_R_TYP (1 << 18)
+
+#define USBHS_EP3_T_TYP (1 << 3)
+#define USBHS_EP3_R_TYP (1 << 19)
+
+#define USBHS_EP4_T_TYP (1 << 4)
+#define USBHS_EP4_R_TYP (1 << 20)
+
+#define USBHS_EP5_T_TYP (1 << 5)
+#define USBHS_EP5_R_TYP (1 << 21)
+
+#define USBHS_EP6_T_TYP (1 << 6)
+#define USBHS_EP6_R_TYP (1 << 22)
+
+#define USBHS_EP7_T_TYP (1 << 7)
+#define USBHS_EP7_R_TYP (1 << 23)
+
+#define USBHS_EP8_T_TYP (1 << 8)
+#define USBHS_EP8_R_TYP (1 << 24)
+
+#define USBHS_EP9_T_TYP (1 << 8)
+#define USBHS_EP9_R_TYP (1 << 25)
+
+#define USBHS_EP10_T_TYP (1 << 10)
+#define USBHS_EP10_R_TYP (1 << 26)
+
+#define USBHS_EP11_T_TYP (1 << 11)
+#define USBHS_EP11_R_TYP (1 << 27)
+
+#define USBHS_EP12_T_TYP (1 << 12)
+#define USBHS_EP12_R_TYP (1 << 28)
+
+#define USBHS_EP13_T_TYP (1 << 13)
+#define USBHS_EP13_R_TYP (1 << 29)
+
+#define USBHS_EP14_T_TYP (1 << 14)
+#define USBHS_EP14_R_TYP (1 << 30)
+
+#define USBHS_EP15_T_TYP (1 << 15)
+#define USBHS_EP15_R_TYP (1 << 31)
+
+/* BUF_MOD UEP1~15 */
+#define USBHS_BUF_MOD_OFFSET  0x18
+#define USBHS_EP0_BUF_MOD     (1 << 0)
+#define USBHS_EP0_ISO_BUF_MOD (1 << 16)
+
+#define USBHS_EP1_BUF_MOD     (1 << 1)
+#define USBHS_EP1_ISO_BUF_MOD (1 << 17)
+
+#define USBHS_EP2_BUF_MOD     (1 << 2)
+#define USBHS_EP2_ISO_BUF_MOD (1 << 18)
+
+#define USBHS_EP3_BUF_MOD     (1 << 3)
+#define USBHS_EP3_ISO_BUF_MOD (1 << 19)
+
+#define USBHS_EP4_BUF_MOD     (1 << 4)
+#define USBHS_EP4_ISO_BUF_MOD (1 << 20)
+
+#define USBHS_EP5_BUF_MOD     (1 << 5)
+#define USBHS_EP5_ISO_BUF_MOD (1 << 21)
+
+#define USBHS_EP6_BUF_MOD     (1 << 6)
+#define USBHS_EP6_ISO_BUF_MOD (1 << 22)
+
+#define USBHS_EP7_BUF_MOD     (1 << 7)
+#define USBHS_EP7_ISO_BUF_MOD (1 << 23)
+
+#define USBHS_EP8_BUF_MOD     (1 << 8)
+#define USBHS_EP8_ISO_BUF_MOD (1 << 24)
+
+#define USBHS_EP9_BUF_MOD     (1 << 9)
+#define USBHS_EP9_ISO_BUF_MOD (1 << 25)
+
+#define USBHS_EP10_BUF_MOD     (1 << 10)
+#define USBHS_EP10_ISO_BUF_MOD (1 << 26)
+
+#define USBHS_EP11_BUF_MOD     (1 << 11)
+#define USBHS_EP11_ISO_BUF_MOD (1 << 27)
+
+#define USBHS_EP12_BUF_MOD     (1 << 12)
+#define USBHS_EP12_ISO_BUF_MOD (1 << 28)
+
+#define USBHS_EP13_BUF_MOD     (1 << 13)
+#define USBHS_EP13_ISO_BUF_MOD (1 << 29)
+
+#define USBHS_EP14_BUF_MOD     (1 << 14)
+#define USBHS_EP14_ISO_BUF_MOD (1 << 30)
+
+#define USBHS_EP15_BUF_MOD     (1 << 15)
+#define USBHS_EP15_ISO_BUF_MOD (1 << 31)
+//USBHS_EPn_T_EN  USBHS_EPn_R_EN  USBHS_EPn_BUF_MOD  Description: Arrange from low to high with UEPn_DMA as the starting address
+//      0               0               x            The endpoint is disabled and the UEPn_*_DMA buffers are not used.
+//      1               0               0            The first address of the receive (OUT) buffer is UEPn_RX_DMA
+//      1               0               1            RB_UEPn_RX_TOG[0]=0, use buffer UEPn_RX_DMA RB_UEPn_RX_TOG[0]=1, use buffer UEPn_TX_DMA
+//      0               1               0            The first address of the transmit (IN) buffer is UEPn_TX_DMA.
+//      0               1               1            RB_UEPn_TX_TOG[0]=0, use buffer UEPn_TX_DMA RB_UEPn_TX_TOG[0]=1, use buffer UEPn_RX_DMA
+
+/* USB0_DMA */
+#define USBHS_UEP0_DMA_OFFSET(n) (0x1C) // endpoint 0 DMA buffer address
+
+/* USBX_RX_DMA */
+#define USBHS_UEPx_RX_DMA_OFFSET(n) (0x1C + 4 * (n)) // endpoint x DMA buffer address
+
+#define USBHS_UEPx_TX_DMA_OFFSET(n) (0x58 + 4 * (n)) // endpoint x DMA buffer address
+
+#define USBHS_UEPx_MAX_LEN_OFFSET(n) (0x98 + 4 * (n)) // endpoint x DMA buffer address
+
+#define USBHS_UEPx_T_LEN_OFFSET(n)   (0xD8 + 4 * (n)) // endpoint x DMA buffer address
+#define USBHS_UEPx_TX_CTRL_OFFSET(n) (0xD8 + 4 * (n) + 2) // endpoint x DMA buffer address
+#define USBHS_UEPx_RX_CTRL_OFFSET(n) (0xD8 + 4 * (n) + 3) // endpoint x DMA buffer address
+
+// UEPn_T_LEN
+#define USBHS_EP_T_LEN_MASK (0x7FF)
+
+//UEPn_TX_CTRL
+#define USBHS_EP_T_RES_MASK  (3 << 0)
+#define USBHS_EP_T_RES_ACK   (0 << 0)
+#define USBHS_EP_T_RES_NYET  (1 << 0)
+#define USBHS_EP_T_RES_NAK   (2 << 0)
+#define USBHS_EP_T_RES_STALL (3 << 0)
+
+#define USBHS_EP_T_TOG_MASK (3 << 3)
+#define USBHS_EP_T_TOG_0    (0 << 3)
+#define USBHS_EP_T_TOG_1    (1 << 3)
+#define USBHS_EP_T_TOG_2    (2 << 3)
+#define USBHS_EP_T_TOG_M    (3 << 3)
+
+#define USBHS_EP_T_AUTOTOG (1 << 5)
+
+//UEPn_RX_CTRL
+#define USBHS_EP_R_RES_MASK  (3 << 0)
+#define USBHS_EP_R_RES_ACK   (0 << 0)
+#define USBHS_EP_R_RES_NYET  (1 << 0)
+#define USBHS_EP_R_RES_NAK   (2 << 0)
+#define USBHS_EP_R_RES_STALL (3 << 0)
+
+#define USBHS_EP_R_TOG_MASK (3 << 3)
+#define USBHS_EP_R_TOG_0    (0 << 3)
+#define USBHS_EP_R_TOG_1    (1 << 3)
+#define USBHS_EP_R_TOG_2    (2 << 3)
+#define USBHS_EP_R_TOG_M    (3 << 3)
+
+#define USBHS_EP_R_AUTOTOG (1 << 5)
+
+#define USBHS_TOG_MATCH (1 << 6)
+
+/******************* HOST ******************/
+// USB HOST_CTRL
+#define USBHS_SEND_BUS_RESET   (1 << 0)
+#define USBHS_SEND_BUS_SUSPEND (1 << 1)
+#define USBHS_SEND_BUS_RESUME  (1 << 2)
+#define USBHS_REMOTE_WAKE      (1 << 3)
+#define USBHS_PHY_SUSPENDM     (1 << 4)
+#define USBHS_UH_SOFT_FREE     (1 << 6)
+#define USBHS_SEND_SOF_EN      (1 << 7)
+
+//UH_CONFIG
+#define USBHS_HOST_TX_EN (1 << 3)
+#define USBHS_HOST_RX_EN (1 << 18)
+
+// HOST_EP_TYPE
+#define USBHS_ENDP_TX_ISO (1 << 3)
+#define USBHS_ENDP_RX_ISO (1 << (16 + 2))
+
+// R32_UH_EP_PID
+#define USBHS_HOST_MASK_TOKEN (0x0f)
+#define USBHS_HOST_MASK_ENDP  (0x0f << 4)
+
+//R8_UH_RX_CTRL
+#define USBHS_EP_R_RES_MASK  (3 << 0)
+#define USBHS_EP_R_RES_ACK   (0 << 0)
+#define USBHS_EP_R_RES_NYET  (1 << 0)
+#define USBHS_EP_R_RES_NAK   (2 << 0)
+#define USBHS_EP_R_RES_STALL (3 << 0)
+
+#define USBHS_UH_R_RES_NO   (1 << 2)
+#define USBHS_UH_R_TOG_1    (1 << 3)
+#define USBHS_UH_R_TOG_2    (2 << 3)
+#define USBHS_UH_R_TOG_3    (3 << 3)
+#define USBHS_UH_R_TOG_AUTO (1 << 5)
+#define USBHS_UH_R_DATA_NO  (1 << 6)
+//R8_UH_TX_CTRL
+#define USBHS_UH_T_RES_MASK  (3 << 0)
+#define USBHS_UH_T_RES_ACK   (0 << 0)
+#define USBHS_UH_T_RES_NYET  (1 << 0)
+#define USBHS_UH_T_RES_NAK   (2 << 0)
+#define USBHS_UH_T_RES_STALL (3 << 0)
+
+#define USBHS_UH_T_RES_NO   (1 << 2)
+#define USBHS_UH_T_TOG_1    (1 << 3)
+#define USBHS_UH_T_TOG_2    (2 << 3)
+#define USBHS_UH_T_TOG_3    (3 << 3)
+#define USBHS_UH_T_TOG_AUTO (1 << 5)
+#define USBHS_UH_T_DATA_NO  (1 << 6)
+
+// 00: OUT, 01:SOF, 10:IN, 11:SETUP
+#define PID_OUT   0
+#define PID_SOF   1
+#define PID_IN    2
+#define PID_SETUP 3
+
+#endif
diff --git a/src/portable/wch/ch32v307/dcd_usbhs.c b/src/portable/wch/ch32v307/dcd_usbhs.c
new file mode 100644
index 000000000..b7a79e166
--- /dev/null
+++ b/src/portable/wch/ch32v307/dcd_usbhs.c
@@ -0,0 +1,391 @@
+/* 
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2022 Greg Davill 
+ *
+ * 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.
+ */
+
+#include "tusb_option.h"
+
+#if CFG_TUD_ENABLED && (CFG_TUSB_MCU == OPT_MCU_CH32V307)
+#include "device/dcd.h"
+
+#include "ch32_usbhs_reg.h"
+#include "core_riscv.h"
+
+// Max number of bi-directional endpoints including EP0
+#define EP_MAX 16
+
+typedef struct {
+    uint8_t *buffer;
+    // tu_fifo_t * ff; // TODO support dcd_edpt_xfer_fifo API
+    uint16_t total_len;
+    uint16_t queued_len;
+    uint16_t max_size;
+    bool short_packet;
+} xfer_ctl_t;
+
+#define XFER_CTL_BASE(_ep, _dir) &xfer_status[_ep][_dir]
+static xfer_ctl_t xfer_status[EP_MAX][2];
+
+#define EP_TX_LEN(ep) *(volatile uint16_t *)((volatile uint16_t *)&(USBHSD->UEP0_TX_LEN) + (ep)*2)
+#define EP_TX_CTRL(ep) *(volatile uint8_t *)((volatile uint8_t *)&(USBHSD->UEP0_TX_CTRL) + (ep)*4)
+#define EP_RX_CTRL(ep) *(volatile uint8_t *)((volatile uint8_t *)&(USBHSD->UEP0_RX_CTRL) + (ep)*4)
+#define EP_RX_MAX_LEN(ep) *(volatile uint16_t *)((volatile uint16_t *)&(USBHSD->UEP0_MAX_LEN) + (ep)*2)
+
+#define EP_TX_DMA_ADDR(ep) *(volatile uint32_t *)((volatile uint32_t *)&(USBHSD->UEP1_TX_DMA) + (ep - 1))
+#define EP_RX_DMA_ADDR(ep) *(volatile uint32_t *)((volatile uint32_t *)&(USBHSD->UEP1_RX_DMA) + (ep - 1))
+
+/* Endpoint Buffer */
+TU_ATTR_ALIGNED(4) uint8_t EP0_DatabufHD[64];  // ep0(64)
+
+volatile uint8_t USBHS_Dev_Endp0_Tog = 0x01;
+
+void dcd_init(uint8_t rhport) {
+    (void)rhport;
+
+    memset(&xfer_status, 0, sizeof(xfer_status));
+
+    USBHSD->HOST_CTRL = 0x00;
+    USBHSD->HOST_CTRL = USBHS_PHY_SUSPENDM;
+
+    USBHSD->CONTROL = 0;
+
+#if TUD_OPT_HIGH_SPEED
+    USBHSD->CONTROL = USBHS_DMA_EN | USBHS_INT_BUSY_EN | USBHS_HIGH_SPEED;
+#else
+    #error OPT_MODE_FULL_SPEED not currently supported on CH32V307
+    USBHSD->CONTROL = USBHS_DMA_EN | USBHS_INT_BUSY_EN | USBHS_FULL_SPEED;
+#endif
+
+    USBHSD->INT_EN = 0;
+    USBHSD->INT_EN = USBHS_SETUP_ACT_EN | USBHS_TRANSFER_EN | USBHS_DETECT_EN | USBHS_SUSPEND_EN;
+
+    /* ALL endpoint enable */
+    USBHSD->ENDP_CONFIG = 0xffffffff;
+
+    USBHSD->ENDP_CONFIG = USBHS_EP0_T_EN | USBHS_EP0_R_EN;
+    USBHSD->ENDP_TYPE = 0x00;
+    USBHSD->BUF_MODE = 0x00;
+
+    USBHSD->UEP0_MAX_LEN = 64;
+
+    USBHSD->UEP0_DMA = (uint32_t)EP0_DatabufHD;
+
+    USBHSD->UEP0_TX_LEN = 0;
+    USBHSD->UEP0_TX_CTRL = USBHS_EP_T_RES_NAK;
+    USBHSD->UEP0_RX_CTRL = USBHS_EP_R_RES_ACK;
+
+    for (int ep = 1; ep < EP_MAX; ep++) {
+        EP_TX_LEN(ep) = 0;
+        EP_TX_CTRL(ep) = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
+        EP_RX_CTRL(ep) = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_NAK;
+
+        EP_RX_MAX_LEN(ep) = 512;
+    }
+
+    USBHSD->DEV_AD = 0;
+    USBHSD->CONTROL |= USBHS_DEV_PU_EN;
+}
+
+void dcd_int_enable(uint8_t rhport) {
+    (void)rhport;
+
+    NVIC_EnableIRQ(USBHS_IRQn);
+}
+
+void dcd_int_disable(uint8_t rhport) {
+    (void)rhport;
+
+    NVIC_DisableIRQ(USBHS_IRQn);
+}
+
+void dcd_edpt_close_all(uint8_t rhport) {
+    (void)rhport;
+}
+
+void dcd_set_address(uint8_t rhport, uint8_t dev_addr) {
+    (void)dev_addr;
+
+    // Response with zlp status
+    dcd_edpt_xfer(rhport, 0x80, NULL, 0);
+}
+
+void dcd_remote_wakeup(uint8_t rhport)
+{
+  (void) rhport;
+}
+
+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) {
+        USBHSD->DEV_AD = (uint8_t)request->wValue;
+    }
+
+    EP_TX_CTRL(0) = USBHS_EP_T_RES_NAK;
+    EP_RX_CTRL(0) = USBHS_EP_R_RES_ACK;
+}
+
+bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const *desc_edpt) {
+    (void)rhport;
+
+    uint8_t const epnum = tu_edpt_number(desc_edpt->bEndpointAddress);
+    uint8_t const dir = tu_edpt_dir(desc_edpt->bEndpointAddress);
+
+    TU_ASSERT(epnum < EP_MAX);
+
+    xfer_ctl_t *xfer = XFER_CTL_BASE(epnum, dir);
+    xfer->max_size = tu_edpt_packet_size(desc_edpt);
+
+    if (epnum != 0) {
+        if (tu_edpt_dir(desc_edpt->bEndpointAddress) == TUSB_DIR_OUT) {
+            EP_RX_CTRL(epnum) = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
+        } else {
+            EP_TX_LEN(epnum) = 0;
+            EP_TX_CTRL(epnum) = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK | USBHS_EP_T_TOG_0;
+        }
+    }
+
+    return true;
+}
+
+int usbd_ep_close(const uint8_t ep) {
+    (void)ep;
+
+    return 0;
+}
+void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr) {
+    (void)rhport;
+
+    uint8_t const epnum = tu_edpt_number(ep_addr);
+    uint8_t const dir = tu_edpt_dir(ep_addr);
+
+    if (epnum == 0) {
+        if (dir == TUSB_DIR_OUT) {
+            USBHSD->UEP0_RX_CTRL = USBHS_EP_R_RES_STALL;
+        } else {
+            USBHSD->UEP0_TX_LEN = 0;
+            USBHSD->UEP0_TX_CTRL = USBHS_EP_T_RES_STALL;
+        }
+    } else {
+        if (dir == TUSB_DIR_OUT) {
+            EP_RX_CTRL(epnum) = (EP_RX_CTRL(epnum) & ~USBHS_EP_R_RES_MASK) | USBHS_EP_R_RES_STALL;
+
+        } else {
+            EP_TX_CTRL(epnum) = (EP_TX_CTRL(epnum) & ~USBHS_EP_T_RES_MASK) | USBHS_EP_T_RES_STALL;
+        }
+    }
+}
+
+void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr) {
+    (void)rhport;
+
+    uint8_t const epnum = tu_edpt_number(ep_addr);
+    uint8_t const dir = tu_edpt_dir(ep_addr);
+
+    if (epnum == 0) {
+        if (dir == TUSB_DIR_OUT) {
+            USBHSD->UEP0_RX_CTRL = USBHS_EP_R_RES_ACK;
+        } else {
+        }
+    } else {
+        if (dir == TUSB_DIR_OUT) {
+            EP_RX_CTRL(epnum) = (EP_RX_CTRL(epnum) & ~(USBHS_EP_R_RES_MASK | USBHS_EP_T_TOG_MASK)) | USBHS_EP_T_RES_ACK;
+
+        } else {
+            EP_TX_CTRL(epnum) = (EP_TX_CTRL(epnum) & ~(USBHS_EP_T_RES_MASK | USBHS_EP_T_TOG_MASK)) | USBHS_EP_T_RES_NAK;
+        }
+    }
+}
+
+bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t *buffer, uint16_t total_bytes) {
+    (void)rhport;
+    uint8_t const epnum = tu_edpt_number(ep_addr);
+    uint8_t const dir = tu_edpt_dir(ep_addr);
+
+    xfer_ctl_t *xfer = XFER_CTL_BASE(epnum, dir);
+    xfer->buffer = buffer;
+    // xfer->ff           = NULL; // TODO support dcd_edpt_xfer_fifo API
+    xfer->total_len = total_bytes;
+    xfer->queued_len = 0;
+    xfer->short_packet = false;
+
+    // uint16_t num_packets = (total_bytes / xfer->max_size);
+    uint16_t short_packet_size = total_bytes % (xfer->max_size + 1);
+
+    // Zero-size packet is special case.
+    if (short_packet_size == 0 || (total_bytes == 0)) {
+        xfer->short_packet = true;
+    }
+
+    if (tu_edpt_dir(ep_addr) == TUSB_DIR_IN) {
+        if (!total_bytes) {
+            xfer->short_packet = true;
+            if (epnum == 0) {
+                USBHSD->UEP0_TX_LEN = 0;
+                USBHSD->UEP0_TX_CTRL = USBHS_EP_T_RES_ACK | (USBHS_Dev_Endp0_Tog ? USBHS_EP_T_TOG_1 : USBHS_EP_T_TOG_0);
+                USBHS_Dev_Endp0_Tog ^= 1;
+            } else {
+                EP_TX_LEN(epnum) = 0;
+                EP_TX_CTRL(epnum) = (EP_TX_CTRL(epnum) & ~(USBHS_EP_T_RES_MASK)) | USBHS_EP_T_RES_ACK;
+            }
+        } else {
+            if (epnum == 0) {
+                xfer->queued_len += short_packet_size;
+                memcpy(&EP0_DatabufHD[0], buffer, short_packet_size);
+
+                USBHSD->UEP0_TX_LEN = short_packet_size;
+                USBHSD->UEP0_TX_CTRL = USBHS_EP_T_RES_ACK | (USBHS_Dev_Endp0_Tog ? USBHS_EP_T_TOG_1 : USBHS_EP_T_TOG_0);
+                USBHS_Dev_Endp0_Tog ^= 1;
+            } else {
+                xfer->queued_len += short_packet_size;
+                
+                EP_TX_DMA_ADDR(epnum) = (uint32_t)buffer;
+                USBHSD->ENDP_CONFIG |= (USBHS_EP0_T_EN << epnum);
+                EP_TX_LEN(epnum) = short_packet_size;
+                EP_TX_CTRL(epnum) = (EP_TX_CTRL(epnum) & ~(USBHS_EP_T_RES_MASK)) | USBHS_EP_T_RES_ACK;
+            }
+        }
+    } else { /* TUSB_DIR_OUT */
+        if (epnum == 0) {
+            uint32_t read_count = USBHSD->RX_LEN;
+            read_count = TU_MIN(read_count, total_bytes);
+
+            if ((total_bytes == 8)) {
+                read_count = 8;
+                memcpy(buffer, &EP0_DatabufHD[0], 8);
+            } else {
+                memcpy(buffer, &EP0_DatabufHD[0], read_count);
+            }
+        } else {
+            EP_RX_DMA_ADDR(epnum) = (uint32_t)xfer->buffer;
+            USBHSD->ENDP_CONFIG |= (USBHS_EP0_R_EN << epnum);
+        }
+
+        // usbd_ep_read(ep_addr, buffer, total_bytes, &ret_bytes);
+    }
+    return true;
+}
+
+
+static void receive_packet(xfer_ctl_t *xfer, uint16_t xfer_size) {
+    // xfer->queued_len = xfer->total_len - remaining;
+
+    uint16_t remaining = xfer->total_len - xfer->queued_len;
+    uint16_t to_recv_size;
+
+    if (remaining <= xfer->max_size) {
+        // Avoid buffer overflow.
+        to_recv_size = (xfer_size > remaining) ? remaining : xfer_size;
+    } else {
+        // Room for full packet, choose recv_size based on what the microcontroller
+        // claims.
+        to_recv_size = (xfer_size > xfer->max_size) ? xfer->max_size : xfer_size;
+    }
+
+    if (to_recv_size) {
+    }
+
+    xfer->queued_len += xfer_size;
+
+    // Per USB spec, a short OUT packet (including length 0) is always
+    // indicative of the end of a transfer (at least for ctl, bulk, int).
+    xfer->short_packet = (xfer_size < xfer->max_size);
+}
+
+void dcd_int_handler(uint8_t rhport) {
+    (void)rhport;
+
+    uint32_t end_num, rx_token;
+    uint8_t intflag = 0;
+
+    intflag = USBHSD->INT_FG;
+
+    if (intflag & USBHS_TRANSFER_FLAG) {
+
+        end_num = (USBHSD->INT_ST) & MASK_UIS_ENDP;
+        rx_token = (((USBHSD->INT_ST) & MASK_UIS_TOKEN) >> 4) & 0x03;
+
+        uint8_t endp = end_num | (rx_token == PID_IN ? TUSB_DIR_IN_MASK : 0);
+
+        xfer_ctl_t *xfer = XFER_CTL_BASE(end_num, tu_edpt_dir(endp));
+
+        if (rx_token == PID_OUT) {
+            uint16_t rx_len = USBHSD->RX_LEN;
+
+            receive_packet(xfer, rx_len);
+
+            if (xfer->short_packet || (xfer->queued_len == xfer->total_len)) {
+                xfer->short_packet = false;
+
+                dcd_event_xfer_complete(0, endp, xfer->queued_len, XFER_RESULT_SUCCESS, true);
+            }
+
+            if (end_num == 0) {
+                USBHSD->UEP0_RX_CTRL = USBHS_EP_R_RES_ACK | USBHS_EP_R_TOG_0;
+            }
+
+        } else if (rx_token == PID_IN) {
+            if (xfer->short_packet || (xfer->queued_len == xfer->total_len)) {
+                xfer->short_packet = false;
+                xfer->total_len = 0;
+                dcd_event_xfer_complete(0, endp, xfer->queued_len, XFER_RESULT_SUCCESS, true);
+
+                EP_TX_CTRL(end_num) = (EP_TX_CTRL(end_num) & ~(USBHS_EP_T_RES_MASK)) | USBHS_EP_T_RES_NAK;
+
+                if (end_num == 0) {
+                }
+            } else {
+                dcd_edpt_xfer(0, endp, xfer->buffer + xfer->queued_len, xfer->total_len - xfer->queued_len);
+            }
+        }
+
+        USBHSD->INT_FG = USBHS_TRANSFER_FLAG; /* Clear flag */
+    } else if (intflag & USBHS_SETUP_FLAG) {
+        USBHS_Dev_Endp0_Tog = 1;
+        dcd_event_setup_received(0, EP0_DatabufHD, true);
+        
+        USBHSD->INT_FG = USBHS_SETUP_FLAG; /* Clear flag */
+    } else if (intflag & USBHS_DETECT_FLAG) {
+        USBHS_Dev_Endp0_Tog = 1;
+
+        xfer_status[0][TUSB_DIR_OUT].max_size = 64;
+        xfer_status[0][TUSB_DIR_IN].max_size = 64;
+
+        dcd_event_bus_reset(0, TUSB_SPEED_HIGH, true);
+
+        USBHSD->DEV_AD = 0;
+        USBHSD->UEP0_RX_CTRL = USBHS_EP_R_RES_ACK | USBHS_EP_R_TOG_0;
+
+        USBHSD->INT_FG = USBHS_DETECT_FLAG; /* Clear flag */
+    } else if (intflag & USBHS_SUSPEND_FLAG) {
+        dcd_event_t event = { .rhport = rhport, .event_id = DCD_EVENT_SUSPEND };
+        dcd_event_handler(&event, true);
+
+        USBHSD->INT_FG = USBHS_SUSPEND_FLAG; /* Clear flag */
+    }
+}
+
+#endif