Merge branch 'master' into MCX

12 files changed, 699 insertions, 688 deletions

diff --git a/src/portable/analog/max3421/hcd_max3421.c b/src/portable/analog/max3421/hcd_max3421.c
index cc4799dd6..61a7e2703 100644
--- a/src/portable/analog/max3421/hcd_max3421.c
+++ b/src/portable/analog/max3421/hcd_max3421.c
@@ -30,6 +30,7 @@
 
 #include <stdatomic.h>
 #include "host/hcd.h"
+#include "host/usbh.h"
 
 //--------------------------------------------------------------------+
 //
@@ -166,6 +167,17 @@ enum {
   DEFAULT_HIEN = HIRQ_CONDET_IRQ | HIRQ_FRAME_IRQ | HIRQ_HXFRDN_IRQ | HIRQ_RCVDAV_IRQ
 };
 
+enum {
+  MAX_NAK_DEFAULT = 1 // Number of NAK per endpoint per usb frame
+};
+
+enum {
+  EP_STATE_IDLE        = 0,
+  EP_STATE_COMPLETE    = 1,
+  EP_STATE_ATTEMPT_1   = 2, // pending 1st attempt
+  EP_STATE_ATTEMPT_MAX = 15
+};
+
 //--------------------------------------------------------------------+
 //
 //--------------------------------------------------------------------+
@@ -173,18 +185,21 @@ enum {
 typedef struct {
   uint8_t daddr;
 
-  struct TU_ATTR_PACKED {
-    uint8_t ep_dir        : 1;
-    uint8_t is_iso        : 1;
-    uint8_t is_setup      : 1;
-    uint8_t data_toggle   : 1;
-    uint8_t xfer_pending  : 1;
-    uint8_t xfer_complete : 1;
+  union { ;
+    struct TU_ATTR_PACKED {
+      uint8_t ep_num   : 4;
+      uint8_t is_setup : 1;
+      uint8_t is_out   : 1;
+      uint8_t is_iso   : 1;
+    }hxfr_bm;
+
+    uint8_t hxfr;
   };
 
   struct TU_ATTR_PACKED {
-    uint8_t ep_num : 4;
-    uint16_t packet_size : 12;
+    uint8_t state        : 4;
+    uint8_t data_toggle  : 1;
+    uint16_t packet_size : 11;
   };
 
   uint16_t total_len;
@@ -195,6 +210,8 @@ typedef struct {
 TU_VERIFY_STATIC(sizeof(max3421_ep_t) == 12, "size is not correct");
 
 typedef struct {
+  volatile uint16_t frame_count;
+
   // cached register
   uint8_t sndbc;
   uint8_t hirq;
@@ -204,18 +221,20 @@ typedef struct {
   uint8_t hxfr;
 
   atomic_flag busy; // busy transferring
-  volatile uint16_t frame_count;
 
-  max3421_ep_t ep[CFG_TUH_MAX3421_ENDPOINT_TOTAL]; // [0] is reserved for addr0
-
-  OSAL_MUTEX_DEF(spi_mutexdef);
 #if OSAL_MUTEX_REQUIRED
+  OSAL_MUTEX_DEF(spi_mutexdef);
   osal_mutex_t spi_mutex;
 #endif
+
+  max3421_ep_t ep[CFG_TUH_MAX3421_ENDPOINT_TOTAL]; // [0] is reserved for addr0
 } max3421_data_t;
 
 static max3421_data_t _hcd_data;
 
+// max NAK before giving up in a frame. 0 means infinite NAKs
+static uint8_t _max_nak = MAX_NAK_DEFAULT;
+
 //--------------------------------------------------------------------+
 // API: SPI transfer with MAX3421E
 // - spi_cs_api(), spi_xfer_api(), int_api(): must be implemented by application
@@ -304,7 +323,6 @@ static void fifo_write(uint8_t rhport, uint8_t reg, uint8_t const * buffer, uint
   tuh_max3421_spi_xfer_api(rhport, buffer, NULL, len);
 
   max3421_spi_unlock(rhport, in_isr);
-
 }
 
 static void fifo_read(uint8_t rhport, uint8_t * buffer, uint16_t len, bool in_isr) {
@@ -321,35 +339,35 @@ static void fifo_read(uint8_t rhport, uint8_t * buffer, uint16_t len, bool in_is
 }
 
 //------------- register write helper -------------//
-static inline void hirq_write(uint8_t rhport, uint8_t data, bool in_isr) {
+TU_ATTR_ALWAYS_INLINE static inline void hirq_write(uint8_t rhport, uint8_t data, bool in_isr) {
   reg_write(rhport, HIRQ_ADDR, data, in_isr);
   // HIRQ write 1 is clear
   _hcd_data.hirq &= (uint8_t) ~data;
 }
 
-static inline void hien_write(uint8_t rhport, uint8_t data, bool in_isr) {
+TU_ATTR_ALWAYS_INLINE static inline void hien_write(uint8_t rhport, uint8_t data, bool in_isr) {
   _hcd_data.hien = data;
   reg_write(rhport, HIEN_ADDR, data, in_isr);
 }
 
-static inline void mode_write(uint8_t rhport, uint8_t data, bool in_isr) {
+TU_ATTR_ALWAYS_INLINE static inline void mode_write(uint8_t rhport, uint8_t data, bool in_isr) {
   _hcd_data.mode = data;
   reg_write(rhport, MODE_ADDR, data, in_isr);
 }
 
-static inline void peraddr_write(uint8_t rhport, uint8_t data, bool in_isr) {
+TU_ATTR_ALWAYS_INLINE static inline void peraddr_write(uint8_t rhport, uint8_t data, bool in_isr) {
   if ( _hcd_data.peraddr == data ) return; // no need to change address
 
   _hcd_data.peraddr = data;
   reg_write(rhport, PERADDR_ADDR, data, in_isr);
 }
 
-static inline void hxfr_write(uint8_t rhport, uint8_t data, bool in_isr) {
+TU_ATTR_ALWAYS_INLINE static inline void hxfr_write(uint8_t rhport, uint8_t data, bool in_isr) {
   _hcd_data.hxfr = data;
   reg_write(rhport, HXFR_ADDR, data, in_isr);
 }
 
-static inline void sndbc_write(uint8_t rhport, uint8_t data, bool in_isr) {
+TU_ATTR_ALWAYS_INLINE static inline void sndbc_write(uint8_t rhport, uint8_t data, bool in_isr) {
   _hcd_data.sndbc = data;
   reg_write(rhport, SNDBC_ADDR, data, in_isr);
 }
@@ -359,10 +377,11 @@ static inline void sndbc_write(uint8_t rhport, uint8_t data, bool in_isr) {
 //--------------------------------------------------------------------+
 
 static max3421_ep_t* find_ep_not_addr0(uint8_t daddr, uint8_t ep_num, uint8_t ep_dir) {
+  uint8_t const is_out = 1-ep_dir;
   for(size_t i=1; i<CFG_TUH_MAX3421_ENDPOINT_TOTAL; i++) {
     max3421_ep_t* ep = &_hcd_data.ep[i];
-    // for control endpoint, skip direction check
-    if (daddr == ep->daddr && ep_num == ep->ep_num && (ep_dir == ep->ep_dir || ep_num == 0)) {
+    // control endpoint is bi-direction (skip check)
+    if (daddr == ep->daddr && ep_num == ep->hxfr_bm.ep_num && (ep_num == 0 || is_out == ep->hxfr_bm.is_out)) {
       return ep;
     }
   }
@@ -393,14 +412,23 @@ static void free_ep(uint8_t daddr) {
   }
 }
 
+// Check if endpoint has an queued transfer and not reach max NAK
+TU_ATTR_ALWAYS_INLINE static inline bool is_ep_pending(max3421_ep_t const * ep) {
+  uint8_t const state = ep->state;
+  return ep->packet_size && (state >= EP_STATE_ATTEMPT_1) &&
+         (_max_nak == 0 || state < EP_STATE_ATTEMPT_1 + _max_nak);
+}
+
+// Find the next pending endpoint using round-robin scheduling, starting from next endpoint.
+// return NULL if not found
+// TODO respect interrupt endpoint's interval
 static max3421_ep_t * find_next_pending_ep(max3421_ep_t * cur_ep) {
   size_t const idx = (size_t) (cur_ep - _hcd_data.ep);
 
   // starting from next endpoint
   for (size_t i = idx + 1; i < CFG_TUH_MAX3421_ENDPOINT_TOTAL; i++) {
     max3421_ep_t* ep = &_hcd_data.ep[i];
-    if (ep->xfer_pending && ep->packet_size) {
-//      TU_LOG3("next pending i = %u\r\n", i);
+    if (is_ep_pending(ep)) {
       return ep;
     }
   }
@@ -408,8 +436,7 @@ static max3421_ep_t * find_next_pending_ep(max3421_ep_t * cur_ep) {
   // wrap around including current endpoint
   for (size_t i = 0; i <= idx; i++) {
     max3421_ep_t* ep = &_hcd_data.ep[i];
-    if (ep->xfer_pending && ep->packet_size) {
-//      TU_LOG3("next pending i = %u\r\n", i);
+    if (is_ep_pending(ep)) {
       return ep;
     }
   }
@@ -424,10 +451,11 @@ static max3421_ep_t * find_next_pending_ep(max3421_ep_t * cur_ep) {
 // optional hcd configuration, called by tuh_configure()
 bool hcd_configure(uint8_t rhport, uint32_t cfg_id, const void* cfg_param) {
   (void) rhport;
-  (void) cfg_id;
-  (void) cfg_param;
+  TU_VERIFY(cfg_id == TUH_CFGID_MAX3421);
 
-  return false;
+  tuh_configure_param_t const* cfg = (tuh_configure_param_t const*) cfg_param;
+  _max_nak = cfg->max3421.max_nak;
+  return true;
 }
 
 // Initialize controller to host mode
@@ -438,6 +466,7 @@ bool hcd_init(uint8_t rhport) {
 
   TU_LOG2_INT(sizeof(max3421_ep_t));
   TU_LOG2_INT(sizeof(max3421_data_t));
+  TU_LOG2_INT(offsetof(max3421_data_t, ep));
 
   tu_memclr(&_hcd_data, sizeof(_hcd_data));
   _hcd_data.peraddr = 0xff; // invalid
@@ -449,7 +478,7 @@ bool hcd_init(uint8_t rhport) {
   // full duplex, interrupt negative edge
   reg_write(rhport, PINCTL_ADDR, PINCTL_FDUPSPI, false);
 
-  // V1 is 0x01, V2 is 0x12, V3 is 0x13
+  // v1 is 0x01, v2 is 0x12, v3 is 0x13
   uint8_t const revision = reg_read(rhport, REVISION_ADDR, false);
   TU_ASSERT(revision == 0x01 || revision == 0x12 || revision == 0x13, false);
   TU_LOG2_HEX(revision);
@@ -481,6 +510,24 @@ bool hcd_init(uint8_t rhport) {
   return true;
 }
 
+bool hcd_deinit(uint8_t rhport) {
+  (void) rhport;
+
+  // disable interrupt
+  tuh_max3421_int_api(rhport, false);
+
+  // reset max3421
+  reg_write(rhport, USBCTL_ADDR, USBCTL_CHIPRES, false);
+  reg_write(rhport, USBCTL_ADDR, 0, false);
+
+  #if OSAL_MUTEX_REQUIRED
+  osal_mutex_delete(_hcd_data.spi_mutex);
+  _hcd_data.spi_mutex = NULL;
+  #endif
+
+  return true;
+}
+
 // Enable USB interrupt
 // Not actually enable GPIO interrupt, just set variable to prevent handler to process
 void hcd_int_enable (uint8_t rhport) {
@@ -539,7 +586,6 @@ void hcd_device_close(uint8_t rhport, uint8_t dev_addr) {
 // Open an endpoint
 bool hcd_edpt_open(uint8_t rhport, uint8_t daddr, tusb_desc_endpoint_t const * ep_desc) {
   (void) rhport;
-  (void) daddr;
 
   uint8_t const ep_num = tu_edpt_number(ep_desc->bEndpointAddress);
   tusb_dir_t const ep_dir = tu_edpt_dir(ep_desc->bEndpointAddress);
@@ -551,12 +597,9 @@ bool hcd_edpt_open(uint8_t rhport, uint8_t daddr, tusb_desc_endpoint_t const * e
     ep = allocate_ep();
     TU_ASSERT(ep);
     ep->daddr = daddr;
-    ep->ep_num = (uint8_t) (ep_num & 0x0f);
-    ep->ep_dir = (ep_dir == TUSB_DIR_IN) ? 1 : 0;
-  }
-
-  if ( TUSB_XFER_ISOCHRONOUS == ep_desc->bmAttributes.xfer ) {
-    ep->is_iso = 1;
+    ep->hxfr_bm.ep_num = (uint8_t) (ep_num & 0x0f);
+    ep->hxfr_bm.is_out = (ep_dir == TUSB_DIR_OUT) ? 1 : 0;
+    ep->hxfr_bm.is_iso = (TUSB_XFER_ISOCHRONOUS == ep_desc->bmAttributes.xfer) ? 1 : 0;
   }
 
   ep->packet_size = (uint16_t) (tu_edpt_packet_size(ep_desc) & 0x7ff);
@@ -564,7 +607,7 @@ bool hcd_edpt_open(uint8_t rhport, uint8_t daddr, tusb_desc_endpoint_t const * e
   return true;
 }
 
-void xact_out(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
+static void xact_out(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
   // Page 12: Programming BULK-OUT Transfers
   // TODO double buffered
   if (switch_ep) {
@@ -580,12 +623,10 @@ void xact_out(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
     fifo_write(rhport, SNDFIFO_ADDR, ep->buf, xact_len, in_isr);
   }
   sndbc_write(rhport, xact_len, in_isr);
-
-  uint8_t const hxfr = (uint8_t ) (ep->ep_num | HXFR_OUT_NIN | (ep->is_iso ? HXFR_ISO : 0));
-  hxfr_write(rhport, hxfr, in_isr);
+  hxfr_write(rhport, ep->hxfr, in_isr);
 }
 
-void xact_in(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
+static void xact_in(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
   // Page 13: Programming BULK-IN Transfers
   if (switch_ep) {
     peraddr_write(rhport, ep->daddr, in_isr);
@@ -594,33 +635,36 @@ void xact_in(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
     reg_write(rhport, HCTL_ADDR, hctl, in_isr);
   }
 
-  uint8_t const hxfr = (uint8_t) (ep->ep_num | (ep->is_iso ? HXFR_ISO : 0));
-  hxfr_write(rhport, hxfr, in_isr);
+  hxfr_write(rhport, ep->hxfr, in_isr);
+}
+
+static void xact_setup(uint8_t rhport, max3421_ep_t *ep, bool in_isr) {
+  peraddr_write(rhport, ep->daddr, in_isr);
+  fifo_write(rhport, SUDFIFO_ADDR, ep->buf, 8, in_isr);
+  hxfr_write(rhport, HXFR_SETUP, in_isr);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void xact_inout(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
-  if (ep->ep_num == 0 ) {
+static void xact_generic(uint8_t rhport, max3421_ep_t *ep, bool switch_ep, bool in_isr) {
+  if (ep->hxfr_bm.ep_num == 0 ) {
     // setup
-    if (ep->is_setup) {
-      peraddr_write(rhport, ep->daddr, in_isr);
-      fifo_write(rhport, SUDFIFO_ADDR, ep->buf, 8, in_isr);
-      hxfr_write(rhport, HXFR_SETUP, in_isr);
+    if (ep->hxfr_bm.is_setup) {
+      xact_setup(rhport, ep, in_isr);
       return;
     }
 
     // status
     if (ep->buf == NULL || ep->total_len == 0) {
-      uint8_t const hxfr = HXFR_HS | (ep->ep_dir ? 0 : HXFR_OUT_NIN);
+      uint8_t const hxfr = HXFR_HS | (ep->hxfr_bm.is_out ? HXFR_OUT_NIN : 0);
       peraddr_write(rhport, ep->daddr, in_isr);
       hxfr_write(rhport, hxfr, in_isr);
       return;
     }
   }
 
-  if (ep->ep_dir) {
-    xact_in(rhport, ep, switch_ep, in_isr);
-  }else {
+  if (ep->hxfr_bm.is_out) {
     xact_out(rhport, ep, switch_ep, in_isr);
+  }else {
+    xact_in(rhport, ep, switch_ep, in_isr);
   }
 }
 
@@ -633,24 +677,21 @@ bool hcd_edpt_xfer(uint8_t rhport, uint8_t daddr, uint8_t ep_addr, uint8_t * buf
   TU_VERIFY(ep);
 
   // control transfer can switch direction
-  ep->ep_dir = ep_dir ? 1u : 0u;
+  ep->hxfr_bm.is_out = ep_dir ? 0u : 1u;
 
   ep->buf = buffer;
   ep->total_len = buflen;
   ep->xferred_len = 0;
-  ep->xfer_complete = 0;
-  ep->xfer_pending = 1;
+  ep->state = EP_STATE_ATTEMPT_1;
 
-  if ( ep_num == 0 ) {
-    ep->is_setup = 0;
+  if (ep_num == 0) {
+    ep->hxfr_bm.is_setup = 0;
     ep->data_toggle = 1;
   }
 
   // carry out transfer if not busy
-  if ( !atomic_flag_test_and_set(&_hcd_data.busy) ) {
-    xact_inout(rhport, ep, true, false);
-  } else {
-    return true;
+  if (!atomic_flag_test_and_set(&_hcd_data.busy)) {
+    xact_generic(rhport, ep, true, false);
   }
 
   return true;
@@ -673,17 +714,16 @@ bool hcd_setup_send(uint8_t rhport, uint8_t daddr, uint8_t const setup_packet[8]
   max3421_ep_t* ep = find_opened_ep(daddr, 0, 0);
   TU_ASSERT(ep);
 
-  ep->ep_dir = 0;
-  ep->is_setup = 1;
+  ep->hxfr_bm.is_out = 1;
+  ep->hxfr_bm.is_setup = 1;
   ep->buf = (uint8_t*)(uintptr_t) setup_packet;
   ep->total_len = 8;
   ep->xferred_len = 0;
-  ep->xfer_complete = 0;
-  ep->xfer_pending = 1;
+  ep->state = EP_STATE_ATTEMPT_1;
 
   // carry out transfer if not busy
-  if ( !atomic_flag_test_and_set(&_hcd_data.busy) ) {
-    xact_inout(rhport, ep, true, false);
+  if (!atomic_flag_test_and_set(&_hcd_data.busy)) {
+    xact_setup(rhport, ep, false);
   }
 
   return true;
@@ -748,22 +788,23 @@ static void handle_connect_irq(uint8_t rhport, bool in_isr) {
 }
 
 static void xfer_complete_isr(uint8_t rhport, max3421_ep_t *ep, xfer_result_t result, uint8_t hrsl, bool in_isr) {
-  uint8_t const ep_addr = tu_edpt_addr(ep->ep_num, ep->ep_dir);
+  uint8_t const ep_dir = 1-ep->hxfr_bm.is_out;
+  uint8_t const ep_addr = tu_edpt_addr(ep->hxfr_bm.ep_num, ep_dir);
 
   // save data toggle
-  if (ep->ep_dir) {
+  if (ep_dir) {
     ep->data_toggle = (hrsl & HRSL_RCVTOGRD) ? 1u : 0u;
   }else {
     ep->data_toggle = (hrsl & HRSL_SNDTOGRD) ? 1u : 0u;
   }
 
-  ep->xfer_pending = 0;
+  ep->state = EP_STATE_IDLE;
   hcd_event_xfer_complete(ep->daddr, ep_addr, ep->xferred_len, result, in_isr);
 
   // Find next pending endpoint
-  max3421_ep_t *next_ep = find_next_pending_ep(ep);
+  max3421_ep_t * next_ep = find_next_pending_ep(ep);
   if (next_ep) {
-    xact_inout(rhport, next_ep, true, in_isr);
+    xact_generic(rhport, next_ep, true, in_isr);
   }else {
     // no more pending
     atomic_flag_clear(&_hcd_data.busy);
@@ -793,20 +834,23 @@ static void handle_xfer_done(uint8_t rhport, bool in_isr) {
 
     case HRSL_NAK:
       if (ep_num == 0) {
-        // NAK on control, retry immediately
+        // control endpoint -> retry immediately
         hxfr_write(rhport, _hcd_data.hxfr, in_isr);
-      }else {
-        // NAK on non-control, find next pending to switch
-        max3421_ep_t *next_ep = find_next_pending_ep(ep);
+      } else {
+        if (ep->state < EP_STATE_ATTEMPT_MAX) {
+          ep->state++;
+        }
 
+        max3421_ep_t * next_ep = find_next_pending_ep(ep);
         if (ep == next_ep) {
-          // this endpoint is only one pending, retry immediately
+          // this endpoint is only one pending -> retry immediately
           hxfr_write(rhport, _hcd_data.hxfr, in_isr);
-        }else if (next_ep) {
-          // switch to next pending TODO could have issue with double buffered if not clear previously out data
-          xact_inout(rhport, next_ep, true, in_isr);
-        }else {
-          TU_ASSERT(false,);
+        } else if (next_ep) {
+          // switch to next pending endpoint TODO could have issue with double buffered if not clear previously out data
+          xact_generic(rhport, next_ep, true, in_isr);
+        } else {
+          // no more pending in this frame -> clear busy
+          atomic_flag_clear(&_hcd_data.busy);
         }
       }
       return;
@@ -828,12 +872,14 @@ static void handle_xfer_done(uint8_t rhport, bool in_isr) {
 
   if (ep_dir) {
     // IN transfer: fifo data is already received in RCVDAV IRQ
-    if ( hxfr_type & HXFR_HS ) {
-      ep->xfer_complete = 1;
+
+    // mark control handshake as complete
+    if (hxfr_type & HXFR_HS) {
+      ep->state = EP_STATE_COMPLETE;
     }
 
     // short packet or all bytes transferred
-    if ( ep->xfer_complete ) {
+    if (ep->state == EP_STATE_COMPLETE) {
       xfer_complete_isr(rhport, ep, xfer_result, hrsl, in_isr);
     }else {
       // more to transfer
@@ -867,13 +913,13 @@ static void handle_xfer_done(uint8_t rhport, bool in_isr) {
 void print_hirq(uint8_t hirq) {
   TU_LOG3_HEX(hirq);
 
-  if (hirq & HIRQ_HXFRDN_IRQ) TU_LOG3(" HXFRDN");
-  if (hirq & HIRQ_FRAME_IRQ)  TU_LOG3(" FRAME");
-  if (hirq & HIRQ_CONDET_IRQ) TU_LOG3(" CONDET");
-  if (hirq & HIRQ_SUSDN_IRQ)  TU_LOG3(" SUSDN");
-  if (hirq & HIRQ_SNDBAV_IRQ) TU_LOG3(" SNDBAV");
-  if (hirq & HIRQ_RCVDAV_IRQ) TU_LOG3(" RCVDAV");
-  if (hirq & HIRQ_RWU_IRQ)    TU_LOG3(" RWU");
+  if (hirq & HIRQ_HXFRDN_IRQ)   TU_LOG3(" HXFRDN");
+  if (hirq & HIRQ_FRAME_IRQ)    TU_LOG3(" FRAME");
+  if (hirq & HIRQ_CONDET_IRQ)   TU_LOG3(" CONDET");
+  if (hirq & HIRQ_SUSDN_IRQ)    TU_LOG3(" SUSDN");
+  if (hirq & HIRQ_SNDBAV_IRQ)   TU_LOG3(" SNDBAV");
+  if (hirq & HIRQ_RCVDAV_IRQ)   TU_LOG3(" RCVDAV");
+  if (hirq & HIRQ_RWU_IRQ)      TU_LOG3(" RWU");
   if (hirq & HIRQ_BUSEVENT_IRQ) TU_LOG3(" BUSEVENT");
 
   TU_LOG3("\r\n");
@@ -890,6 +936,25 @@ void hcd_int_handler(uint8_t rhport, bool in_isr) {
 
   if (hirq & HIRQ_FRAME_IRQ) {
     _hcd_data.frame_count++;
+
+    max3421_ep_t* ep_retry = NULL;
+
+    // reset all endpoints attempt counter
+    for (size_t i = 0; i < CFG_TUH_MAX3421_ENDPOINT_TOTAL; i++) {
+      max3421_ep_t* ep = &_hcd_data.ep[i];
+      if (ep->packet_size && ep->state > EP_STATE_ATTEMPT_1) {
+        ep->state = EP_STATE_ATTEMPT_1;
+
+        if (ep_retry == NULL) {
+          ep_retry = ep;
+        }
+      }
+    }
+
+    // start usb transfer if not busy
+    if (ep_retry != NULL && !atomic_flag_test_and_set(&_hcd_data.busy)) {
+      xact_generic(rhport, ep_retry, true, in_isr);
+    }
   }
 
   if (hirq & HIRQ_CONDET_IRQ) {
@@ -898,17 +963,17 @@ void hcd_int_handler(uint8_t rhport, bool in_isr) {
 
   // queue more transfer in handle_xfer_done() can cause hirq to be set again while external IRQ may not catch and/or
   // not call this handler again. So we need to loop until all IRQ are cleared
-  while ( hirq & (HIRQ_RCVDAV_IRQ | HIRQ_HXFRDN_IRQ) ) {
-    if ( hirq & HIRQ_RCVDAV_IRQ ) {
+  while (hirq & (HIRQ_RCVDAV_IRQ | HIRQ_HXFRDN_IRQ)) {
+    if (hirq & HIRQ_RCVDAV_IRQ) {
       uint8_t const ep_num = _hcd_data.hxfr & HXFR_EPNUM_MASK;
-      max3421_ep_t *ep = find_opened_ep(_hcd_data.peraddr, ep_num, 1);
+      max3421_ep_t* ep = find_opened_ep(_hcd_data.peraddr, ep_num, 1);
       uint8_t xact_len = 0;
 
       // RCVDAV_IRQ can trigger 2 times (dual buffered)
-      while ( hirq & HIRQ_RCVDAV_IRQ ) {
+      while (hirq & HIRQ_RCVDAV_IRQ) {
         uint8_t rcvbc = reg_read(rhport, RCVBC_ADDR, in_isr);
         xact_len = (uint8_t) tu_min16(rcvbc, ep->total_len - ep->xferred_len);
-        if ( xact_len ) {
+        if (xact_len) {
           fifo_read(rhport, ep->buf, xact_len, in_isr);
           ep->buf += xact_len;
           ep->xferred_len += xact_len;
@@ -919,12 +984,12 @@ void hcd_int_handler(uint8_t rhport, bool in_isr) {
         hirq = reg_read(rhport, HIRQ_ADDR, in_isr);
       }
 
-      if ( xact_len < ep->packet_size || ep->xferred_len >= ep->total_len ) {
-        ep->xfer_complete = 1;
+      if (xact_len < ep->packet_size || ep->xferred_len >= ep->total_len) {
+        ep->state = EP_STATE_COMPLETE;
       }
     }
 
-    if ( hirq & HIRQ_HXFRDN_IRQ ) {
+    if (hirq & HIRQ_HXFRDN_IRQ) {
       hirq_write(rhport, HIRQ_HXFRDN_IRQ, in_isr);
       handle_xfer_done(rhport, in_isr);
     }
diff --git a/src/portable/chipidea/ci_fs/ci_fs_kinetis.h b/src/portable/chipidea/ci_fs/ci_fs_kinetis.h
index cd21af1c7..31e14a546 100644
--- a/src/portable/chipidea/ci_fs/ci_fs_kinetis.h
+++ b/src/portable/chipidea/ci_fs/ci_fs_kinetis.h
@@ -36,14 +36,12 @@
 #define CI_FS_REG(_port)        ((ci_fs_regs_t*) USB0_BASE)
 #define CI_REG                  CI_FS_REG(0)
 
-void dcd_int_enable(uint8_t rhport)
-{
+void dcd_int_enable(uint8_t rhport) {
   (void) rhport;
   NVIC_EnableIRQ(USB0_IRQn);
 }
 
-void dcd_int_disable(uint8_t rhport)
-{
+void dcd_int_disable(uint8_t rhport) {
   (void) rhport;
   NVIC_DisableIRQ(USB0_IRQn);
 }
diff --git a/src/portable/chipidea/ci_fs/dcd_ci_fs.c b/src/portable/chipidea/ci_fs/dcd_ci_fs.c
index 9327e09d8..a68ecf8c9 100644
--- a/src/portable/chipidea/ci_fs/dcd_ci_fs.c
+++ b/src/portable/chipidea/ci_fs/dcd_ci_fs.c
@@ -271,9 +271,21 @@ void dcd_init(uint8_t rhport)
 {
   (void) rhport;
 
+  // save crystal-less setting (if available)
+  #if defined(FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED) && FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED == 1
+  uint32_t clk_recover_irc_en = CI_REG->CLK_RECOVER_IRC_EN;
+  uint32_t clk_recover_ctrl = CI_REG->CLK_RECOVER_CTRL;
+  #endif
+
   CI_REG->USBTRC0 |= USB_USBTRC0_USBRESET_MASK;
   while (CI_REG->USBTRC0 & USB_USBTRC0_USBRESET_MASK);
 
+  // restore crystal-less setting (if available)
+  #if defined(FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED) && FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED == 1
+  CI_REG->CLK_RECOVER_IRC_EN = clk_recover_irc_en;
+  CI_REG->CLK_RECOVER_CTRL  |= clk_recover_ctrl;
+  #endif
+
   tu_memclr(&_dcd, sizeof(_dcd));
   CI_REG->USBTRC0 |= TU_BIT(6); /* software must set this bit to 1 */
   CI_REG->BDT_PAGE1 = (uint8_t)((uintptr_t)_dcd.bdt >> 8);
diff --git a/src/portable/espressif/esp32sx/dcd_esp32sx.c b/src/portable/espressif/esp32sx/dcd_esp32sx.c
index 1fcfb6241..bfc0baa56 100644
--- a/src/portable/espressif/esp32sx/dcd_esp32sx.c
+++ b/src/portable/espressif/esp32sx/dcd_esp32sx.c
@@ -31,16 +31,26 @@
 #if (((CFG_TUSB_MCU == OPT_MCU_ESP32S2) ||  (CFG_TUSB_MCU == OPT_MCU_ESP32S3)) && CFG_TUD_ENABLED)
 
 // Espressif
-#include "freertos/xtensa_api.h"
+#include "xtensa_api.h"
 #include "esp_intr_alloc.h"
 #include "esp_log.h"
 #include "soc/dport_reg.h"
 #include "soc/gpio_sig_map.h"
 #include "soc/usb_periph.h"
+#include "soc/usb_reg.h"
+#include "soc/usb_struct.h"
 #include "soc/periph_defs.h" // for interrupt source
 
 #include "device/dcd.h"
 
+#ifndef USB_OUT_EP_NUM
+#define USB_OUT_EP_NUM ((int) (sizeof(USB0.out_ep_reg) / sizeof(USB0.out_ep_reg[0])))
+#endif
+
+#ifndef USB_IN_EP_NUM
+#define USB_IN_EP_NUM ((int) (sizeof(USB0.in_ep_reg) / sizeof(USB0.in_ep_reg[0])))
+#endif
+
 // Max number of bi-directional endpoints including EP0
 // Note: ESP32S2 specs say there are only up to 5 IN active endpoints include EP0
 // We should probably prohibit enabling Endpoint IN > 4 (not done yet)
diff --git a/src/portable/nordic/nrf5x/dcd_nrf5x.c b/src/portable/nordic/nrf5x/dcd_nrf5x.c
index 4e702aed4..cf9e5923b 100644
--- a/src/portable/nordic/nrf5x/dcd_nrf5x.c
+++ b/src/portable/nordic/nrf5x/dcd_nrf5x.c
@@ -246,7 +246,7 @@ static void xact_in_dma(uint8_t epnum)
 //--------------------------------------------------------------------+
 void dcd_init (uint8_t rhport)
 {
-  TU_LOG1("dcd init\r\n");
+  TU_LOG2("dcd init\r\n");
   (void) rhport;
 }
 
@@ -681,7 +681,7 @@ void dcd_int_handler(uint8_t rhport)
 
   if ( int_status & USBD_INTEN_USBEVENT_Msk )
   {
-    TU_LOG(2, "EVENTCAUSE = 0x%04lX\r\n", NRF_USBD->EVENTCAUSE);
+    TU_LOG(3, "EVENTCAUSE = 0x%04lX\r\n", NRF_USBD->EVENTCAUSE);
 
     enum { EVT_CAUSE_MASK = USBD_EVENTCAUSE_SUSPEND_Msk | USBD_EVENTCAUSE_RESUME_Msk | USBD_EVENTCAUSE_USBWUALLOWED_Msk };
     uint32_t const evt_cause = NRF_USBD->EVENTCAUSE & EVT_CAUSE_MASK;
diff --git a/src/portable/nxp/khci/dcd_khci.c b/src/portable/nxp/khci/dcd_khci.c
index 5c65ea33d..dc71117b3 100644
--- a/src/portable/nxp/khci/dcd_khci.c
+++ b/src/portable/nxp/khci/dcd_khci.c
@@ -269,9 +269,21 @@ void dcd_init(uint8_t rhport)
 {
   (void) rhport;
 
+  // save crystal-less setting (if available)
+  #if defined(FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED) && FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED == 1
+  uint32_t clk_recover_irc_en = KHCI->CLK_RECOVER_IRC_EN;
+  uint32_t clk_recover_ctrl = KHCI->CLK_RECOVER_CTRL;
+  #endif
+
   KHCI->USBTRC0 |= USB_USBTRC0_USBRESET_MASK;
   while (KHCI->USBTRC0 & USB_USBTRC0_USBRESET_MASK);
 
+  // restore crystal-less setting (if available)
+  #if defined(FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED) && FSL_FEATURE_USB_KHCI_IRC48M_MODULE_CLOCK_ENABLED == 1
+  KHCI->CLK_RECOVER_IRC_EN = clk_recover_irc_en;
+  KHCI->CLK_RECOVER_CTRL  |= clk_recover_ctrl;
+  #endif
+
   tu_memclr(&_dcd, sizeof(_dcd));
   KHCI->USBTRC0 |= TU_BIT(6); /* software must set this bit to 1 */
   KHCI->BDTPAGE1 = (uint8_t)((uintptr_t)_dcd.bdt >>  8);
diff --git a/src/portable/raspberrypi/rp2040/dcd_rp2040.c b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
index e8cee73fd..5c564cb1c 100644
--- a/src/portable/raspberrypi/rp2040/dcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
@@ -48,7 +48,7 @@
  *------------------------------------------------------------------*/
 
 // Init these in dcd_init
-static uint8_t *next_buffer_ptr;
+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];
@@ -56,79 +56,70 @@ static struct hw_endpoint hw_endpoints[USB_MAX_ENDPOINTS][2];
 // 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)
-{
+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];
 }
 
-static struct hw_endpoint *hw_endpoint_get_by_addr(uint8_t ep_addr)
-{
+TU_ATTR_ALWAYS_INLINE static inline struct hw_endpoint* hw_endpoint_get_by_addr(uint8_t ep_addr) {
   uint8_t num = tu_edpt_number(ep_addr);
   tusb_dir_t dir = tu_edpt_dir(ep_addr);
   return hw_endpoint_get_by_num(num, dir);
 }
 
-static void _hw_endpoint_alloc(struct hw_endpoint *ep, uint8_t transfer_type)
-{
+static void _hw_endpoint_alloc(struct hw_endpoint* ep, uint8_t transfer_type) {
   // size must be multiple of 64
   uint size = tu_div_ceil(ep->wMaxPacketSize, 64) * 64u;
 
   // double buffered Bulk endpoint
-  if ( transfer_type == TUSB_XFER_BULK )
-  {
+  if (transfer_type == TUSB_XFER_BULK) {
     size *= 2u;
   }
 
   ep->hw_data_buf = next_buffer_ptr;
   next_buffer_ptr += size;
 
-  assert(((uintptr_t )next_buffer_ptr & 0b111111u) == 0);
+  assert(((uintptr_t) next_buffer_ptr & 0b111111u) == 0);
   uint dpram_offset = hw_data_offset(ep->hw_data_buf);
   hard_assert(hw_data_offset(next_buffer_ptr) <= USB_DPRAM_MAX);
 
   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;
+  uint32_t const reg = EP_CTRL_ENABLE_BITS | ((uint) transfer_type << EP_CTRL_BUFFER_TYPE_LSB) | dpram_offset;
 
   *ep->endpoint_control = reg;
 }
 
-static void _hw_endpoint_close(struct hw_endpoint *ep)
-{
-    // Clear hardware registers and then zero the struct
-    // Clears endpoint enable
-    *ep->endpoint_control = 0;
-    // Clears buffer available, etc
-    *ep->buffer_control = 0;
-    // Clear any endpoint state
-    memset(ep, 0, sizeof(struct hw_endpoint));
+static void _hw_endpoint_close(struct hw_endpoint* ep) {
+  // Clear hardware registers and then zero the struct
+  // Clears endpoint enable
+  *ep->endpoint_control = 0;
+  // Clears buffer available, etc
+  *ep->buffer_control = 0;
+  // Clear any endpoint state
+  memset(ep, 0, sizeof(struct hw_endpoint));
 
-    // Reclaim buffer space if all endpoints are closed
-    bool reclaim_buffers = true;
-    for ( uint8_t i = 1; i < USB_MAX_ENDPOINTS; i++ )
-    {
-        if (hw_endpoint_get_by_num(i, TUSB_DIR_OUT)->hw_data_buf != NULL || hw_endpoint_get_by_num(i, TUSB_DIR_IN)->hw_data_buf != NULL)
-        {
-            reclaim_buffers = false;
-            break;
-        }
-    }
-    if (reclaim_buffers)
-    {
-        next_buffer_ptr = &usb_dpram->epx_data[0];
+  // Reclaim buffer space if all endpoints are closed
+  bool reclaim_buffers = true;
+  for (uint8_t i = 1; i < USB_MAX_ENDPOINTS; i++) {
+    if (hw_endpoint_get_by_num(i, TUSB_DIR_OUT)->hw_data_buf != NULL ||
+        hw_endpoint_get_by_num(i, TUSB_DIR_IN)->hw_data_buf != NULL) {
+      reclaim_buffers = false;
+      break;
     }
+  }
+  if (reclaim_buffers) {
+    next_buffer_ptr = &usb_dpram->epx_data[0];
+  }
 }
 
-static void hw_endpoint_close(uint8_t ep_addr)
-{
-    struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
-    _hw_endpoint_close(ep);
+static void hw_endpoint_close(uint8_t ep_addr) {
+  struct hw_endpoint* ep = hw_endpoint_get_by_addr(ep_addr);
+  _hw_endpoint_close(ep);
 }
 
-static void hw_endpoint_init(uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t transfer_type)
-{
-  struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
+static void hw_endpoint_init(uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t transfer_type) {
+  struct hw_endpoint* ep = hw_endpoint_get_by_addr(ep_addr);
 
   const uint8_t num = tu_edpt_number(ep_addr);
   const tusb_dir_t dir = tu_edpt_dir(ep_addr);
@@ -143,35 +134,26 @@ static void hw_endpoint_init(uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t t
   ep->transfer_type = transfer_type;
 
   // Every endpoint has a buffer control register in dpram
-  if ( dir == TUSB_DIR_IN )
-  {
+  if (dir == TUSB_DIR_IN) {
     ep->buffer_control = &usb_dpram->ep_buf_ctrl[num].in;
-  }
-  else
-  {
+  } else {
     ep->buffer_control = &usb_dpram->ep_buf_ctrl[num].out;
   }
 
   // Clear existing buffer control state
   *ep->buffer_control = 0;
 
-  if ( num == 0 )
-  {
+  if (num == 0) {
     // EP0 has no endpoint control register because the buffer offsets are fixed
     ep->endpoint_control = NULL;
 
     // Buffer offset is fixed (also double buffered)
     ep->hw_data_buf = (uint8_t*) &usb_dpram->ep0_buf_a[0];
-  }
-  else
-  {
+  } else {
     // Set the endpoint control register (starts at EP1, hence num-1)
-    if ( dir == TUSB_DIR_IN )
-    {
+    if (dir == TUSB_DIR_IN) {
       ep->endpoint_control = &usb_dpram->ep_ctrl[num - 1].in;
-    }
-    else
-    {
+    } else {
       ep->endpoint_control = &usb_dpram->ep_ctrl[num - 1].out;
     }
 
@@ -180,76 +162,82 @@ static void hw_endpoint_init(uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t t
   }
 }
 
-static void hw_endpoint_xfer(uint8_t ep_addr, uint8_t *buffer, uint16_t total_bytes)
-{
-    struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
-    hw_endpoint_xfer_start(ep, buffer, total_bytes);
+static void hw_endpoint_xfer(uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes) {
+  struct hw_endpoint* ep = hw_endpoint_get_by_addr(ep_addr);
+  hw_endpoint_xfer_start(ep, buffer, total_bytes);
 }
 
-static void __tusb_irq_path_func(hw_handle_buff_status)(void)
-{
-    uint32_t remaining_buffers = usb_hw->buf_status;
-    pico_trace("buf_status = 0x%08lx\r\n", remaining_buffers);
-    uint bit = 1u;
-    for (uint8_t i = 0; remaining_buffers && i < USB_MAX_ENDPOINTS * 2; i++)
-    {
-        if (remaining_buffers & bit)
-        {
-            // clear this in advance
-            usb_hw_clear->buf_status = bit;
+static void __tusb_irq_path_func(hw_handle_buff_status)(void) {
+  uint32_t remaining_buffers = usb_hw->buf_status;
+  pico_trace("buf_status = 0x%08lx\r\n", remaining_buffers);
+  uint bit = 1u;
+  for (uint8_t i = 0; remaining_buffers && i < USB_MAX_ENDPOINTS * 2; i++) {
+    if (remaining_buffers & bit) {
+      // clear this in advance
+      usb_hw_clear->buf_status = bit;
 
-            // IN transfer for even i, OUT transfer for odd i
-            struct hw_endpoint *ep = hw_endpoint_get_by_num(i >> 1u, (i & 1u) ? TUSB_DIR_OUT : TUSB_DIR_IN);
+      // IN transfer for even i, OUT transfer for odd i
+      struct hw_endpoint* ep = hw_endpoint_get_by_num(i >> 1u, (i & 1u) ? TUSB_DIR_OUT : TUSB_DIR_IN);
 
-            // Continue xfer
-            bool done = hw_endpoint_xfer_continue(ep);
-            if (done)
-            {
-                // Notify
-                dcd_event_xfer_complete(0, ep->ep_addr, ep->xferred_len, XFER_RESULT_SUCCESS, true);
-                hw_endpoint_reset_transfer(ep);
-            }
-            remaining_buffers &= ~bit;
-        }
-        bit <<= 1u;
+      // Continue xfer
+      bool done = hw_endpoint_xfer_continue(ep);
+      if (done) {
+        // Notify
+        dcd_event_xfer_complete(0, ep->ep_addr, ep->xferred_len, XFER_RESULT_SUCCESS, true);
+        hw_endpoint_reset_transfer(ep);
+      }
+      remaining_buffers &= ~bit;
     }
+    bit <<= 1u;
+  }
 }
 
-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
-    uint8_t addrs[] = {0x0, 0x80};
-    for (uint i = 0 ; i < TU_ARRAY_SIZE(addrs); i++)
-    {
-        struct hw_endpoint *ep = hw_endpoint_get_by_addr(addrs[i]);
-        ep->next_pid = 1u;
+TU_ATTR_ALWAYS_INLINE static inline void reset_ep0(void) {
+  // If we have finished this transfer on EP0 set pid back to 1 for next
+  // setup transfer. Also clear a stall in case
+  for (uint8_t dir = 0; dir < 2; dir++) {
+    struct hw_endpoint* ep = hw_endpoint_get_by_num(0, dir);
+    if (ep->active) {
+      // Abort any pending transfer from a prior control transfer per USB specs
+      // Due to Errata RP2040-E2: ABORT flag is only applicable for B2 and later (unusable for B0, B1).
+      // Which means we are not guaranteed to safely abort pending transfer on B0 and B1.
+      uint32_t const abort_mask = (dir ? USB_EP_ABORT_EP0_IN_BITS : USB_EP_ABORT_EP0_OUT_BITS);
+      if (rp2040_chip_version() >= 2) {
+        usb_hw_set->abort = abort_mask;
+        while ((usb_hw->abort_done & abort_mask) != abort_mask) {}
+      }
+
+      _hw_endpoint_buffer_control_set_value32(ep, USB_BUF_CTRL_DATA1_PID | USB_BUF_CTRL_SEL);
+      hw_endpoint_reset_transfer(ep);
+
+      if (rp2040_chip_version() >= 2) {
+        usb_hw_clear->abort_done = abort_mask;
+        usb_hw_clear->abort = abort_mask;
+      }
     }
+    ep->next_pid = 1u;
+  }
 }
 
-static void __tusb_irq_path_func(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++ )
-  {
+  for (uint8_t i = 0; i < USB_MAX_ENDPOINTS - 1; i++) {
     usb_dpram->ep_ctrl[i].in = 0;
     usb_dpram->ep_ctrl[i].out = 0;
   }
 
   // clear non-control hw endpoints
-  tu_memclr(hw_endpoints[1], sizeof(hw_endpoints) - 2*sizeof(hw_endpoint_t));
+  tu_memclr(hw_endpoints[1], sizeof(hw_endpoints) - 2 * sizeof(hw_endpoint_t));
 
   // reclaim buffer space
   next_buffer_ptr = &usb_dpram->epx_data[0];
 }
 
-static void __tusb_irq_path_func(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;
 
-  if ( status & USB_INTF_DEV_SOF_BITS )
-  {
+  if (status & USB_INTF_DEV_SOF_BITS) {
     bool keep_sof_alive = false;
 
     handled |= USB_INTF_DEV_SOF_BITS;
@@ -258,20 +246,17 @@ static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
     // Errata 15 workaround for Device Bulk-In endpoint
     e15_last_sof = time_us_32();
 
-    for ( uint8_t i = 0; i < USB_MAX_ENDPOINTS; i++ )
-    {
-      struct hw_endpoint * ep = hw_endpoint_get_by_num(i, TUSB_DIR_IN);
+    for (uint8_t i = 0; i < USB_MAX_ENDPOINTS; i++) {
+      struct hw_endpoint* ep = hw_endpoint_get_by_num(i, TUSB_DIR_IN);
 
       // Active Bulk IN endpoint requires SOF
-      if ( (ep->transfer_type == TUSB_XFER_BULK) && ep->active )
-      {
+      if ((ep->transfer_type == TUSB_XFER_BULK) && ep->active) {
         keep_sof_alive = true;
 
         hw_endpoint_lock_update(ep, 1);
 
         // Deferred enable?
-        if ( ep->pending )
-        {
+        if (ep->pending) {
           ep->pending = 0;
           hw_endpoint_start_next_buffer(ep);
         }
@@ -282,26 +267,24 @@ static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
 #endif
 
     // 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;
+    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 )
-  {
+  if (status & USB_INTS_BUFF_STATUS_BITS) {
     handled |= USB_INTS_BUFF_STATUS_BITS;
     hw_handle_buff_status();
   }
 
-  if ( status & USB_INTS_SETUP_REQ_BITS )
-  {
+  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);
+    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();
+    reset_ep0();
 
     // Pass setup packet to tiny usb
     dcd_event_setup_received(0, setup, true);
@@ -329,8 +312,7 @@ static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
 #endif
 
   // SE0 for 2.5 us or more (will last at least 10ms)
-  if ( status & USB_INTS_BUS_RESET_BITS )
-  {
+  if (status & USB_INTS_BUS_RESET_BITS) {
     pico_trace("BUS RESET\r\n");
 
     handled |= USB_INTS_BUS_RESET_BITS;
@@ -342,7 +324,7 @@ static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
 
 #if TUD_OPT_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();
+    if (usb_hw->sie_ctrl & USB_SIE_CTRL_PULLUP_EN_BITS) rp2040_usb_device_enumeration_fix();
 #endif
   }
 
@@ -354,22 +336,19 @@ static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
    * because without VBUS detection, it is impossible to tell the difference between
    * being disconnected and suspended.
    */
-  if ( status & USB_INTS_DEV_SUSPEND_BITS )
-  {
+  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 )
-  {
+  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 )
-  {
+  if (status ^ handled) {
     panic("Unhandled IRQ 0x%x\n", (uint) (status ^ handled));
   }
 }
@@ -390,10 +369,11 @@ static void __tusb_irq_path_func(dcd_rp2040_irq)(void)
 #define PICO_SHARED_IRQ_HANDLER_HIGHEST_ORDER_PRIORITY 0xff
 #endif
 
-void dcd_init (uint8_t rhport)
-{
+void dcd_init(uint8_t rhport) {
   assert(rhport == 0);
 
+  TU_LOG(2, "Chip Version B%u\r\n", rp2040_chip_version());
+
   // Reset hardware to default state
   rp2040_usb_init();
 
@@ -405,7 +385,7 @@ void dcd_init (uint8_t rhport)
   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));
+  tu_memclr(hw_endpoints[0], 2 * sizeof(hw_endpoint_t));
   hw_endpoint_init(0x0, 64, TUSB_XFER_CONTROL);
   hw_endpoint_init(0x80, 64, TUSB_XFER_CONTROL);
 
@@ -420,27 +400,24 @@ void dcd_init (uint8_t rhport)
   // for the global interrupt enable...
   // Note: Force VBUS detect cause disconnection not detectable
   usb_hw->sie_ctrl = USB_SIE_CTRL_EP0_INT_1BUF_BITS;
-  usb_hw->inte     = USB_INTS_BUFF_STATUS_BITS | USB_INTS_BUS_RESET_BITS | USB_INTS_SETUP_REQ_BITS |
-                     USB_INTS_DEV_SUSPEND_BITS | USB_INTS_DEV_RESUME_FROM_HOST_BITS |
-                     (FORCE_VBUS_DETECT ? 0 : USB_INTS_DEV_CONN_DIS_BITS);
+  usb_hw->inte = USB_INTS_BUFF_STATUS_BITS | USB_INTS_BUS_RESET_BITS | USB_INTS_SETUP_REQ_BITS |
+                 USB_INTS_DEV_SUSPEND_BITS | USB_INTS_DEV_RESUME_FROM_HOST_BITS |
+                 (FORCE_VBUS_DETECT ? 0 : USB_INTS_DEV_CONN_DIS_BITS);
 
   dcd_connect(rhport);
 }
 
-void dcd_int_enable(__unused uint8_t rhport)
-{
-    assert(rhport == 0);
-    irq_set_enabled(USBCTRL_IRQ, true);
+void dcd_int_enable(__unused uint8_t rhport) {
+  assert(rhport == 0);
+  irq_set_enabled(USBCTRL_IRQ, true);
 }
 
-void dcd_int_disable(__unused uint8_t rhport)
-{
-    assert(rhport == 0);
-    irq_set_enabled(USBCTRL_IRQ, false);
+void dcd_int_disable(__unused uint8_t rhport) {
+  assert(rhport == 0);
+  irq_set_enabled(USBCTRL_IRQ, false);
 }
 
-void dcd_set_address (__unused uint8_t rhport, __unused uint8_t dev_addr)
-{
+void dcd_set_address(__unused uint8_t rhport, __unused uint8_t dev_addr) {
   assert(rhport == 0);
 
   // Can't set device address in hardware until status xfer has complete
@@ -448,8 +425,7 @@ void dcd_set_address (__unused uint8_t rhport, __unused uint8_t dev_addr)
   hw_endpoint_xfer(0x80, NULL, 0);
 }
 
-void dcd_remote_wakeup(__unused uint8_t rhport)
-{
+void dcd_remote_wakeup(__unused uint8_t rhport) {
   pico_info("dcd_remote_wakeup %d\n", rhport);
   assert(rhport == 0);
 
@@ -460,100 +436,88 @@ void dcd_remote_wakeup(__unused uint8_t rhport)
 }
 
 // disconnect by disabling internal pull-up resistor on D+/D-
-void dcd_disconnect(__unused uint8_t rhport)
-{
+void dcd_disconnect(__unused uint8_t rhport) {
   (void) rhport;
   usb_hw_clear->sie_ctrl = USB_SIE_CTRL_PULLUP_EN_BITS;
 }
 
 // connect by enabling internal pull-up resistor on D+/D-
-void dcd_connect(__unused uint8_t rhport)
-{
+void dcd_connect(__unused uint8_t rhport) {
   (void) rhport;
   usb_hw_set->sie_ctrl = USB_SIE_CTRL_PULLUP_EN_BITS;
 }
 
-void dcd_sof_enable(uint8_t rhport, bool en)
-{
+void dcd_sof_enable(uint8_t rhport, bool en) {
   (void) rhport;
 
   _sof_enable = en;
 
-  if (en)
-  {
+  if (en) {
     usb_hw_set->inte = USB_INTS_DEV_SOF_BITS;
-  }else
-  {
+  }
+#if !TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
+  else {
     // 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
   }
+#endif
 }
 
 /*------------------------------------------------------------------*/
 /* DCD Endpoint port
  *------------------------------------------------------------------*/
 
-void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const * request)
-{
+void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const* request) {
   (void) rhport;
 
-  if ( request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_DEVICE &&
-       request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD &&
-       request->bRequest == TUSB_REQ_SET_ADDRESS )
-  {
+  if (request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_DEVICE &&
+      request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD &&
+      request->bRequest == TUSB_REQ_SET_ADDRESS) {
     usb_hw->dev_addr_ctrl = (uint8_t) request->wValue;
   }
 }
 
-bool dcd_edpt_open (__unused uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
-{
-    assert(rhport == 0);
-    hw_endpoint_init(desc_edpt->bEndpointAddress, tu_edpt_packet_size(desc_edpt), desc_edpt->bmAttributes.xfer);
-    return true;
+bool dcd_edpt_open(__unused uint8_t rhport, tusb_desc_endpoint_t const* desc_edpt) {
+  assert(rhport == 0);
+  hw_endpoint_init(desc_edpt->bEndpointAddress, tu_edpt_packet_size(desc_edpt), desc_edpt->bmAttributes.xfer);
+  return true;
 }
 
-void dcd_edpt_close_all (uint8_t rhport)
-{
+void dcd_edpt_close_all(uint8_t rhport) {
   (void) rhport;
 
   // may need to use EP Abort
   reset_non_control_endpoints();
 }
 
-bool dcd_edpt_xfer(__unused uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
-{
-    assert(rhport == 0);
-    hw_endpoint_xfer(ep_addr, buffer, total_bytes);
-    return true;
+bool dcd_edpt_xfer(__unused uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes) {
+  assert(rhport == 0);
+  hw_endpoint_xfer(ep_addr, buffer, total_bytes);
+  return true;
 }
 
-void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
-{
+void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr) {
   (void) rhport;
 
-  if ( tu_edpt_number(ep_addr) == 0 )
-  {
+  if (tu_edpt_number(ep_addr) == 0) {
     // A stall on EP0 has to be armed so it can be cleared on the next setup packet
-    usb_hw_set->ep_stall_arm = (tu_edpt_dir(ep_addr) == TUSB_DIR_IN) ? USB_EP_STALL_ARM_EP0_IN_BITS : USB_EP_STALL_ARM_EP0_OUT_BITS;
+    usb_hw_set->ep_stall_arm = (tu_edpt_dir(ep_addr) == TUSB_DIR_IN) ? USB_EP_STALL_ARM_EP0_IN_BITS
+                                                                     : USB_EP_STALL_ARM_EP0_OUT_BITS;
   }
 
-  struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
+  struct hw_endpoint* ep = hw_endpoint_get_by_addr(ep_addr);
 
   // stall and clear current pending buffer
   // may need to use EP_ABORT
   _hw_endpoint_buffer_control_set_value32(ep, USB_BUF_CTRL_STALL);
 }
 
-void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
-{
+void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr) {
   (void) rhport;
 
-  if (tu_edpt_number(ep_addr))
-  {
-    struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
+  if (tu_edpt_number(ep_addr)) {
+    struct hw_endpoint* ep = hw_endpoint_get_by_addr(ep_addr);
 
     // clear stall also reset toggle to DATA0, ready for next transfer
     ep->next_pid = 0;
@@ -561,16 +525,13 @@ void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
   }
 }
 
-void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr)
-{
-    (void) rhport;
-
-    pico_trace("dcd_edpt_close %02x\r\n", ep_addr);
-    hw_endpoint_close(ep_addr);
+void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr) {
+  (void) rhport;
+  pico_trace("dcd_edpt_close %02x\r\n", ep_addr);
+  hw_endpoint_close(ep_addr);
 }
 
-void __tusb_irq_path_func(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 08aef9314..b351a9a07 100644
--- a/src/portable/raspberrypi/rp2040/hcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/hcd_rp2040.c
@@ -113,7 +113,7 @@ static void __tusb_irq_path_func(_handle_buff_status_bit)(uint bit, struct hw_en
 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);
+  pico_trace("buf_status 0x%08lx\n", remaining_buffers);
 
   // Check EPX first
   uint bit = 0b1;
@@ -325,10 +325,8 @@ static void _hw_endpoint_init(struct hw_endpoint *ep, uint8_t dev_addr, uint8_t
   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);
+  pico_trace("hw_endpoint_init dev %d ep %02X xfer %d\n", ep->dev_addr, ep->ep_addr, ep->transfer_type);
+  pico_trace("dev %d ep %02X setup buffer @ 0x%p\n", ep->dev_addr, 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));
@@ -343,7 +341,7 @@ static void _hw_endpoint_init(struct hw_endpoint *ep, uint8_t dev_addr, uint8_t
     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);
+  pico_trace("endpoint control (0x%p) <- 0x%lx\n", ep->endpoint_control, ep_reg);
   ep->configured = true;
 
   if ( ep != &epx )
diff --git a/src/portable/raspberrypi/rp2040/rp2040_usb.c b/src/portable/raspberrypi/rp2040/rp2040_usb.c
index a512dc34f..1ca711c77 100644
--- a/src/portable/raspberrypi/rp2040/rp2040_usb.c
+++ b/src/portable/raspberrypi/rp2040/rp2040_usb.c
@@ -35,26 +35,18 @@
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF PROTOTYPE
 //--------------------------------------------------------------------+
-
-// Direction strings for debug
-const char *ep_dir_string[] = {
-        "out",
-        "in",
-};
-
-static void _hw_endpoint_xfer_sync(struct hw_endpoint *ep);
+static void _hw_endpoint_xfer_sync(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);
+  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)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool is_host_mode(void) {
   return (usb_hw->main_ctrl & USB_MAIN_CTRL_HOST_NDEVICE_BITS) ? true : false;
 }
 
@@ -62,8 +54,7 @@ TU_ATTR_ALWAYS_INLINE static inline bool is_host_mode(void)
 // Implementation
 //--------------------------------------------------------------------+
 
-void rp2040_usb_init(void)
-{
+void rp2040_usb_init(void) {
   // Reset usb controller
   reset_block(RESETS_RESET_USBCTRL_BITS);
   unreset_block_wait(RESETS_RESET_USBCTRL_BITS);
@@ -88,46 +79,33 @@ void rp2040_usb_init(void)
   TU_LOG2_INT(sizeof(hw_endpoint_t));
 }
 
-void __tusb_irq_path_func(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;
   ep->xferred_len = 0;
   ep->user_buf = 0;
 }
 
-void __tusb_irq_path_func(_hw_endpoint_buffer_control_update32)(struct hw_endpoint *ep, uint32_t and_mask, uint32_t or_mask)
-{
+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 )
-  {
+  if (and_mask) {
     value = *ep->buffer_control & and_mask;
   }
 
-  if ( or_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)]);
+    if (or_mask & USB_BUF_CTRL_AVAIL) {
+      if (*ep->buffer_control & USB_BUF_CTRL_AVAIL) {
+        panic("ep %02X was already available", ep->ep_addr);
       }
       *ep->buffer_control = value & ~USB_BUF_CTRL_AVAIL;
-      // 12 cycle delay.. (should be good for 48*12Mhz = 576Mhz)
+      // 4.1.2.5.1 Con-current access: 12 cycles (should be good for 48*12Mhz = 576Mhz) after write to buffer control
       // Don't need delay in host mode as host is in charge
-#if !CFG_TUH_ENABLED
-            __asm volatile (
-                    "b 1f\n"
-                    "1: b 1f\n"
-                    "1: b 1f\n"
-                    "1: b 1f\n"
-                    "1: b 1f\n"
-                    "1: b 1f\n"
-                    "1:\n"
-                    : : : "memory");
-#endif
+      if ( !is_host_mode()) {
+        busy_wait_at_least_cycles(12);
+      }
     }
   }
 
@@ -135,10 +113,9 @@ void __tusb_irq_path_func(_hw_endpoint_buffer_control_update32)(struct hw_endpoi
 }
 
 // prepare buffer, return buffer control
-static uint32_t __tusb_irq_path_func(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);
+  ep->remaining_len = (uint16_t) (ep->remaining_len - buflen);
 
   uint32_t buf_ctrl = buflen | USB_BUF_CTRL_AVAIL;
 
@@ -146,10 +123,9 @@ static uint32_t __tusb_irq_path_func(prepare_ep_buffer)(struct hw_endpoint *ep,
   buf_ctrl |= ep->next_pid ? USB_BUF_CTRL_DATA1_PID : USB_BUF_CTRL_DATA0_PID;
   ep->next_pid ^= 1u;
 
-  if ( !ep->rx )
-  {
+  if (!ep->rx) {
     // Copy data from user buffer to hw buffer
-    memcpy(ep->hw_data_buf + buf_id*64, ep->user_buf, buflen);
+    memcpy(ep->hw_data_buf + buf_id * 64, ep->user_buf, buflen);
     ep->user_buf += buflen;
 
     // Mark as full
@@ -159,8 +135,7 @@ static uint32_t __tusb_irq_path_func(prepare_ep_buffer)(struct hw_endpoint *ep,
   // Is this the last buffer? Only really matters for host mode. Will trigger
   // the trans complete irq but also stop it polling. We only really care about
   // trans complete for setup packets being sent
-  if (ep->remaining_len == 0)
-  {
+  if (ep->remaining_len == 0) {
     buf_ctrl |= USB_BUF_CTRL_LAST;
   }
 
@@ -170,8 +145,7 @@ static uint32_t __tusb_irq_path_func(prepare_ep_buffer)(struct hw_endpoint *ep,
 }
 
 // Prepare buffer control register value
-void __tusb_irq_path_func(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
@@ -186,8 +160,7 @@ void __tusb_irq_path_func(hw_endpoint_start_next_buffer)(struct hw_endpoint *ep)
   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)
-  {
+  if (ep->remaining_len && !force_single) {
     // Use buffer 1 (double buffered) if there is still data
     // TODO: Isochronous for buffer1 bit-field is different than CBI (control bulk, interrupt)
 
@@ -196,8 +169,7 @@ void __tusb_irq_path_func(hw_endpoint_start_next_buffer)(struct hw_endpoint *ep)
     // Set endpoint control double buffered bit if needed
     ep_ctrl &= ~EP_CTRL_INTERRUPT_PER_BUFFER;
     ep_ctrl |= EP_CTRL_DOUBLE_BUFFERED_BITS | EP_CTRL_INTERRUPT_PER_DOUBLE_BUFFER;
-  }else
-  {
+  } else {
     // Single buffered since 1 is enough
     ep_ctrl &= ~(EP_CTRL_DOUBLE_BUFFERED_BITS | EP_CTRL_INTERRUPT_PER_DOUBLE_BUFFER);
     ep_ctrl |= EP_CTRL_INTERRUPT_PER_BUFFER;
@@ -212,35 +184,28 @@ void __tusb_irq_path_func(hw_endpoint_start_next_buffer)(struct hw_endpoint *ep)
   _hw_endpoint_buffer_control_set_value32(ep, buf_ctrl);
 }
 
-void hw_endpoint_xfer_start(struct hw_endpoint *ep, uint8_t *buffer, uint16_t total_len)
-{
+void hw_endpoint_xfer_start(struct hw_endpoint* ep, uint8_t* buffer, uint16_t total_len) {
   hw_endpoint_lock_update(ep, 1);
 
-  if ( ep->active )
-  {
+  if (ep->active) {
     // TODO: Is this acceptable for interrupt packets?
-    TU_LOG(1, "WARN: starting new transfer on already active ep %d %s\r\n", tu_edpt_number(ep->ep_addr),
-              ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
-
+    TU_LOG(1, "WARN: starting new transfer on already active ep %02X\r\n", ep->ep_addr);
     hw_endpoint_reset_transfer(ep);
   }
 
   // Fill in info now that we're kicking off the hw
   ep->remaining_len = total_len;
-  ep->xferred_len   = 0;
-  ep->active        = true;
-  ep->user_buf      = buffer;
+  ep->xferred_len = 0;
+  ep->active = true;
+  ep->user_buf = buffer;
 
-  if ( e15_is_bulkin_ep(ep) )
-  {
+  if (e15_is_bulkin_ep(ep)) {
     usb_hw_set->inte = USB_INTS_DEV_SOF_BITS;
   }
 
-  if ( e15_is_critical_frame_period(ep) )
-  {
+  if (e15_is_critical_frame_period(ep)) {
     ep->pending = 1;
-  } else
-  {
+  } else {
     hw_endpoint_start_next_buffer(ep);
   }
 
@@ -248,34 +213,30 @@ void hw_endpoint_xfer_start(struct hw_endpoint *ep, uint8_t *buffer, uint16_t to
 }
 
 // sync endpoint buffer and return transferred bytes
-static uint16_t __tusb_irq_path_func(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;
+  if (buf_id) buf_ctrl = buf_ctrl >> 16;
 
   uint16_t xferred_bytes = buf_ctrl & USB_BUF_CTRL_LEN_MASK;
 
-  if ( !ep->rx )
-  {
+  if (!ep->rx) {
     // We are continuing a transfer here. If we are TX, we have successfully
     // sent some data can increase the length we have sent
     assert(!(buf_ctrl & USB_BUF_CTRL_FULL));
 
-    ep->xferred_len = (uint16_t)(ep->xferred_len + xferred_bytes);
-  }else
-  {
+    ep->xferred_len = (uint16_t) (ep->xferred_len + xferred_bytes);
+  } else {
     // If we have received some data, so can increase the length
     // we have received AFTER we have copied it to the user buffer at the appropriate offset
     assert(buf_ctrl & USB_BUF_CTRL_FULL);
 
-    memcpy(ep->user_buf, ep->hw_data_buf + buf_id*64, xferred_bytes);
-    ep->xferred_len = (uint16_t)(ep->xferred_len + xferred_bytes);
+    memcpy(ep->user_buf, ep->hw_data_buf + buf_id * 64, xferred_bytes);
+    ep->xferred_len = (uint16_t) (ep->xferred_len + xferred_bytes);
     ep->user_buf += xferred_bytes;
   }
 
   // Short packet
-  if (xferred_bytes < ep->wMaxPacketSize)
-  {
+  if (xferred_bytes < ep->wMaxPacketSize) {
     pico_trace("  Short packet on buffer %d with %u bytes\r\n", buf_id, xferred_bytes);
     // Reduce total length as this is last packet
     ep->remaining_len = 0;
@@ -284,8 +245,7 @@ static uint16_t __tusb_irq_path_func(sync_ep_buffer)(struct hw_endpoint *ep, uin
   return xferred_bytes;
 }
 
-static void __tusb_irq_path_func(_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
 
@@ -296,14 +256,11 @@ static void __tusb_irq_path_func(_hw_endpoint_xfer_sync) (struct hw_endpoint *ep
   uint16_t buf0_bytes = sync_ep_buffer(ep, 0);
 
   // sync buffer 1 if double buffered
-  if ( (*ep->endpoint_control) & EP_CTRL_DOUBLE_BUFFERED_BITS )
-  {
-    if (buf0_bytes == ep->wMaxPacketSize)
-    {
+  if ((*ep->endpoint_control) & EP_CTRL_DOUBLE_BUFFERED_BITS) {
+    if (buf0_bytes == ep->wMaxPacketSize) {
       // sync buffer 1 if not short packet
       sync_ep_buffer(ep, 1);
-    }else
-    {
+    } else {
       // short packet on buffer 0
       // TODO couldn't figure out how to handle this case which happen with net_lwip_webserver example
       // At this time (currently trigger per 2 buffer), the buffer1 is probably filled with data from
@@ -335,14 +292,12 @@ static void __tusb_irq_path_func(_hw_endpoint_xfer_sync) (struct hw_endpoint *ep
 }
 
 // Returns true if transfer is complete
-bool __tusb_irq_path_func(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);
 
   // Part way through a transfer
-  if (!ep->active)
-  {
-    panic("Can't continue xfer on inactive ep %d %s", tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
+  if (!ep->active) {
+    panic("Can't continue xfer on inactive ep %02X", ep->ep_addr);
   }
 
   // Update EP struct from hardware state
@@ -350,21 +305,15 @@ bool __tusb_irq_path_func(hw_endpoint_xfer_continue)(struct hw_endpoint *ep)
 
   // Now we have synced our state with the hardware. Is there more data to transfer?
   // If we are done then notify tinyusb
-  if (ep->remaining_len == 0)
-  {
-    pico_trace("Completed transfer of %d bytes on ep %d %s\r\n",
-               ep->xferred_len, tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
+  if (ep->remaining_len == 0) {
+    pico_trace("Completed transfer of %d bytes on ep %02X\r\n", ep->xferred_len, ep->ep_addr);
     // Notify caller we are done so it can notify the tinyusb stack
     hw_endpoint_lock_update(ep, -1);
     return true;
-  }
-  else
-  {
-    if ( e15_is_critical_frame_period(ep) )
-    {
+  } else {
+    if (e15_is_critical_frame_period(ep)) {
       ep->pending = 1;
-    } else
-    {
+    } else {
       hw_endpoint_start_next_buffer(ep);
     }
   }
@@ -399,16 +348,14 @@ bool __tusb_irq_path_func(hw_endpoint_xfer_continue)(struct hw_endpoint *ep)
 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)
-{
+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)
-{
+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.
@@ -419,11 +366,10 @@ static bool __tusb_irq_path_func(e15_is_critical_frame_period) (struct hw_endpoi
   if (delta < 800 || delta > 998) {
     return false;
   }
-  TU_LOG(3, "Avoiding sof %lu now %lu last %lu\r\n", (usb_hw->sof_rd + 1) & USB_SOF_RD_BITS, time_us_32(), e15_last_sof);
+  TU_LOG(3, "Avoiding sof %lu now %lu last %lu\r\n", (usb_hw->sof_rd + 1) & USB_SOF_RD_BITS, time_us_32(),
+         e15_last_sof);
   return true;
 }
 
-#endif
-
-
+#endif // TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
 #endif
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index 9c37f1f98..7bf726f3f 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -109,7 +109,7 @@
 #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"
+  #include "portable/st/stm32_fsdev/dcd_stm32_fsdev.h"
 #endif
 
 /*****************************************************
@@ -200,8 +200,7 @@ static bool dcd_read_packet_memory_ff(tu_fifo_t * ff, uint16_t src, uint16_t wNB
 // Inline helper
 //--------------------------------------------------------------------+
 
-TU_ATTR_ALWAYS_INLINE static inline xfer_ctl_t* xfer_ctl_ptr(uint32_t ep_addr)
-{
+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
@@ -524,7 +523,7 @@ static void dcd_ep_ctr_tx_handler(uint32_t wIstr)
   xfer_ctl_t * xfer = xfer_ctl_ptr(ep_addr);
   if((xfer->total_len != xfer->queued_len)) /* TX not complete */
   {
-      dcd_transmit_packet(xfer, EPindex);
+    dcd_transmit_packet(xfer, EPindex);
   }
   else /* TX Complete */
   {
@@ -533,10 +532,29 @@ static void dcd_ep_ctr_tx_handler(uint32_t wIstr)
 }
 
 // Handle CTR interrupt for the RX/OUT direction
-//
 // Upon call, (wIstr & USB_ISTR_DIR) == 0U
-static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
-{
+static void dcd_ep_ctr_rx_handler(uint32_t wIstr) {
+  #ifdef FSDEV_BUS_32BIT
+  /* https://www.st.com/resource/en/errata_sheet/es0561-stm32h503cbebkbrb-device-errata-stmicroelectronics.pdf
+   * From STM32H503 errata 2.15.1: Buffer description table update completes after CTR interrupt triggers
+   * Description:
+   * - During OUT transfers, the correct transfer interrupt (CTR) is triggered a little before the last USB SRAM accesses
+   * have completed. If the software responds quickly to the interrupt, the full buffer contents may not be correct.
+   * Workaround:
+   * - Software should ensure that a small delay is included before accessing the SRAM contents. This delay
+   * should be 800 ns in Full Speed mode and 6.4 μs in Low Speed mode
+   * - Since H5 can run up to 250Mhz -> 1 cycle = 4ns. Per errata, we need to wait 200 cycles. Though executing code
+   * also takes time, so we'll wait 40 cycles (count = 20).
+   * - Since Low Speed mode is not supported/popular, we will ignore it for now.
+   *
+   * Note: this errata also seems to apply to G0, U5, H5 etc.
+   */
+  volatile uint32_t cycle_count = 20; // defined as PCD_RX_PMA_CNT in stm32 hal_driver
+  while (cycle_count > 0U) {
+    cycle_count--; // each count take 2 cycle (1 cycle for sub, 1 cycle for compare/jump)
+  }
+  #endif
+
   uint32_t EPindex = wIstr & USB_ISTR_EP_ID;
   uint32_t wEPRegVal = pcd_get_endpoint(USB, EPindex);
   uint8_t ep_addr = wEPRegVal & USB_EPADDR_FIELD;
@@ -545,8 +563,7 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
 
   // Verify the CTR_RX bit is set. This was in the ST Micro code,
   // but I'm not sure it's actually necessary?
-  if((wEPRegVal & USB_EP_CTR_RX) == 0U)
-  {
+  if((wEPRegVal & USB_EP_CTR_RX) == 0U) {
     return;
   }
 
@@ -633,26 +650,22 @@ static void dcd_ep_ctr_rx_handler(uint32_t wIstr)
   // (Based on the docs, it seems SETUP will always be accepted after CTR is cleared)
   if(ep_addr == 0u)
   {
-      // Always be prepared for a status packet...
+    // Always be prepared for a status packet...
     pcd_set_ep_rx_bufsize(USB, EPindex, CFG_TUD_ENDPOINT0_SIZE);
     pcd_clear_rx_ep_ctr(USB, EPindex);
   }
 }
 
-static void dcd_ep_ctr_handler(void)
-{
+static void dcd_ep_ctr_handler(void) {
   uint32_t wIstr;
 
   /* stay in loop while pending interrupts */
-  while (((wIstr = USB->ISTR) & USB_ISTR_CTR) != 0U)
-  {
-
-    if ((wIstr & USB_ISTR_DIR) == 0U) /* TX/IN */
-    {
+  while (((wIstr = USB->ISTR) & USB_ISTR_CTR) != 0U) {
+    if ((wIstr & USB_ISTR_DIR) == 0U) {
+      /* TX/IN */
       dcd_ep_ctr_tx_handler(wIstr);
-    }
-    else /* RX/OUT*/
-    {
+    } else {
+      /* RX/OUT*/
       dcd_ep_ctr_rx_handler(wIstr);
     }
   }
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.h
index 3f4db985d..946ad2c7c 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev_pvt_st.h
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.h
@@ -1,30 +1,32 @@
-/**
-  * 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:
-  *   1. Redistributions of source code must retain the above copyright notice,
-  *      this list of conditions and the following disclaimer.
-  *   2. Redistributions in binary form must reproduce the above copyright notice,
-  *      this list of conditions and the following disclaimer in the documentation
-  *      and/or other materials provided with the distribution.
-  *   3. Neither the name of STMicroelectronics nor the names of its contributors
-  *      may be used to endorse or promote products derived from this software
-  *      without specific prior written permission.
-  *
-  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
-  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
-  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
-  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
-  * 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.
-  *
-  */
+/*
+ * Copyright(c) 2016 STMicroelectronics
+ * Copyright(c) N Conrad
+ * Copyright (c) 2019 Ha Thach (tinyusb.org)
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *   1. Redistributions of source code must retain the above copyright notice,
+ *      this list of conditions and the following disclaimer.
+ *   2. Redistributions in binary form must reproduce the above copyright notice,
+ *      this list of conditions and the following disclaimer in the documentation
+ *      and/or other materials provided with the distribution.
+ *   3. Neither the name of STMicroelectronics nor the names of its contributors
+ *      may be used to endorse or promote products derived from this software
+ *      without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * 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 is part of the TinyUSB stack.
+ */
 
 // This file contains source copied from ST's HAL, and thus should have their copyright statement.
 
@@ -113,6 +115,11 @@
 #elif CFG_TUSB_MCU == OPT_MCU_STM32H5
   #include "stm32h5xx.h"
   #define FSDEV_BUS_32BIT
+
+  #if !defined(USB_DRD_BASE) && defined(USB_DRD_FS_BASE)
+  #define USB_DRD_BASE USB_DRD_FS_BASE
+  #endif
+
   #define FSDEV_PMA_SIZE (2048u)
   #undef USB_PMAADDR
   #define USB_PMAADDR USB_DRD_PMAADDR
@@ -138,7 +145,6 @@
   #define USB_CNTR_LPMODE USB_CNTR_SUSPRDY
   #define USB_CNTR_FSUSP USB_CNTR_SUSPEN
 
-
 #elif CFG_TUSB_MCU == OPT_MCU_STM32WB
   #include "stm32wbxx.h"
   #define FSDEV_PMA_SIZE (1024u)
@@ -182,27 +188,23 @@ typedef uint16_t fsdev_bus_t;
 // Volatile is also needed to prevent the optimizer from changing access to 32-bit (as 32-bit access is forbidden)
 static __IO uint16_t * const pma = (__IO uint16_t*)USB_PMAADDR;
 
-TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t * pcd_btable_word_ptr(USB_TypeDef * USBx, size_t x)
-{
+TU_ATTR_ALWAYS_INLINE static inline __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 *= FSDEV_PMA_STRIDE;
   return &(pma[total_word_offset]);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_tx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx) {
   return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 1u);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline __IO uint16_t* pcd_ep_rx_cnt_ptr(USB_TypeDef * USBx, uint32_t bEpIdx) {
   return pcd_btable_word_ptr(USBx,(bEpIdx)*4u + 3u);
 }
 #endif
 
 /* Aligned buffer size according to hardware */
-TU_ATTR_ALWAYS_INLINE static inline uint16_t pcd_aligned_buffer_size(uint16_t size)
-{
+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;
@@ -213,9 +215,7 @@ TU_ATTR_ALWAYS_INLINE static inline uint16_t pcd_aligned_buffer_size(uint16_t si
   return numblocks * blocksize;
 }
 
-/* SetENDPOINT */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wRegValue)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wRegValue) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   __O uint32_t *reg = (__O uint32_t *)(USB_DRD_BASE + bEpIdx*4);
@@ -226,7 +226,6 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_endpoint(USB_TypeDef * USBx, ui
 #endif
 }
 
-/* GetENDPOINT */
 TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_endpoint(USB_TypeDef * USBx, uint32_t bEpIdx) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
@@ -237,8 +236,7 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_endpoint(USB_TypeDef * USBx
   return *reg;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_eptype(USB_TypeDef * USBx, uint32_t bEpIdx, 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, bEpIdx);
   regVal &= (uint32_t)USB_EP_T_MASK;
   regVal |= wType;
@@ -246,20 +244,19 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_eptype(USB_TypeDef * USBx, uint
   pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_eptype(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_eptype(USB_TypeDef * USBx, uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EP_T_FIELD;
   return regVal;
 }
+
 /**
   * @brief  Clears bit CTR_RX / CTR_TX in the endpoint register.
   * @param  USBx USB peripheral instance register address.
   * @param  bEpIdx Endpoint Number.
   * @retval None
   */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal &= ~USB_EP_CTR_RX;
@@ -267,22 +264,21 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_ep_ctr(USB_TypeDef * USBx,
   pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_ep_ctr(USB_TypeDef * USBx, uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal &= ~USB_EP_CTR_TX;
   regVal |= USB_EP_CTR_RX; // preserve CTR_RX (clears on writing 0)
   pcd_set_endpoint(USBx, bEpIdx,regVal);
 }
+
 /**
   * @brief  gets counter of the tx buffer.
   * @param  USBx USB peripheral instance register address.
   * @param  bEpIdx Endpoint Number.
   * @retval Counter value
   */
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   return (pma32[2*bEpIdx] & 0x03FF0000) >> 16;
@@ -292,8 +288,7 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_cnt(USB_TypeDef * USB
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   return (pma32[2*bEpIdx + 1] & 0x03FF0000) >> 16;
@@ -310,8 +305,7 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_cnt(USB_TypeDef * USB
   * @param  bAddr Address.
   * @retval None
   */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_address(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t bAddr)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_address(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t bAddr) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal |= bAddr;
@@ -319,8 +313,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_address(USB_TypeDef * USBx,
   pcd_set_endpoint(USBx, bEpIdx,regVal);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_address(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_address(USB_TypeDef * USBx, uint32_t bEpIdx) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   return pma32[2*bEpIdx] & 0x0000FFFFu ;
@@ -329,8 +322,7 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_tx_address(USB_TypeDef *
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_address(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_address(USB_TypeDef * USBx, uint32_t bEpIdx) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   return pma32[2*bEpIdx + 1] & 0x0000FFFFu;
@@ -339,8 +331,7 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_address(USB_TypeDef *
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_address(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t addr)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_address(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t addr) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   pma32[2*bEpIdx] = (pma32[2*bEpIdx] & 0xFFFF0000u) | (addr & 0x0000FFFCu);
@@ -349,8 +340,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_address(USB_TypeDef * USB
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_address(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t addr)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_address(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t addr) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   pma32[2*bEpIdx + 1] = (pma32[2*bEpIdx + 1] & 0xFFFF0000u) | (addr & 0x0000FFFCu);
@@ -359,8 +349,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_address(USB_TypeDef * USB
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   pma32[2*bEpIdx] = (pma32[2*bEpIdx] & ~0x03FF0000u) | ((wCount & 0x3FFu) << 16);
@@ -370,8 +359,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_cnt(USB_TypeDef * USBx, u
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_cnt(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount) {
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
   pma32[2*bEpIdx + 1] = (pma32[2*bEpIdx + 1] & ~0x03FF0000u) | ((wCount & 0x3FFu) << 16);
@@ -381,8 +369,8 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_cnt(USB_TypeDef * USBx, u
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_blsize_num_blocks(USB_TypeDef * USBx, uint32_t rxtx_idx, uint32_t blocksize, uint32_t numblocks)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_blsize_num_blocks(USB_TypeDef * USBx, uint32_t rxtx_idx,
+                                                                      uint32_t blocksize, uint32_t numblocks) {
   /* Encode into register. When BLSIZE==1, we need to subtract 1 block count */
 #ifdef FSDEV_BUS_32BIT
   (void) USBx;
@@ -393,8 +381,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_blsize_num_blocks(USB_TypeDe
 #endif
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_bufsize(USB_TypeDef * USBx, uint32_t rxtx_idx, uint32_t wCount)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_bufsize(USB_TypeDef * USBx, uint32_t rxtx_idx, uint32_t wCount) {
   wCount = pcd_aligned_buffer_size(wCount);
 
   /* We assume that the buffer size is already aligned to hardware requirements. */
@@ -408,13 +395,11 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_bufsize(USB_TypeDef * USBx,
   pcd_set_ep_blsize_num_blocks(USBx, rxtx_idx, blocksize, numblocks);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_bufsize(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_bufsize(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount) {
   pcd_set_ep_bufsize(USBx, 2*bEpIdx, wCount);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_bufsize(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_bufsize(USB_TypeDef * USBx, uint32_t bEpIdx, uint32_t wCount) {
   pcd_set_ep_bufsize(USBx, 2*bEpIdx + 1, wCount);
 }
 
@@ -425,8 +410,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_bufsize(USB_TypeDef * USB
   * @param  wState new state
   * @retval None
   */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wState)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx,  uint32_t bEpIdx, uint32_t wState) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPTX_DTOGMASK;
 
@@ -443,7 +427,7 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx
 
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
   pcd_set_endpoint(USBx, bEpIdx, regVal);
-} /* pcd_set_ep_tx_status */
+}
 
 /**
   * @brief  sets the status for rx transfer (bits STAT_TX[1:0])
@@ -453,31 +437,27 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_tx_status(USB_TypeDef * USBx
   * @retval None
   */
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpIdx, 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, bEpIdx);
   regVal &= USB_EPRX_DTOGMASK;
 
   /* toggle first bit ? */
-  if((USB_EPRX_DTOG1 & wState)!= 0U)
-  {
+  if((USB_EPRX_DTOG1 & wState)!= 0U) {
     regVal ^= USB_EPRX_DTOG1;
   }
   /* toggle second bit ? */
-  if((USB_EPRX_DTOG2 & wState)!= 0U)
-  {
+  if((USB_EPRX_DTOG2 & wState)!= 0U) {
     regVal ^= USB_EPRX_DTOG2;
   }
 
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
   pcd_set_endpoint(USBx, bEpIdx, regVal);
-} /* pcd_set_ep_rx_status */
+}
 
-TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_status(USB_TypeDef * USBx,  uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   return (regVal & USB_EPRX_STAT) >> (12u);
-} /* pcd_get_ep_rx_status */
+}
 
 
 /**
@@ -486,16 +466,14 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t pcd_get_ep_rx_status(USB_TypeDef *
   * @param  bEpIdx Endpoint Number.
   * @retval None
   */
-TU_ATTR_ALWAYS_INLINE static inline void pcd_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX|USB_EP_DTOG_RX;
   pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPREG_MASK;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX|USB_EP_DTOG_TX;
@@ -508,21 +486,16 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_tx_dtog(USB_TypeDef * USBx,  uint32
   * @param  bEpIdx Endpoint Number.
   * @retval None
   */
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_rx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  if((regVal & USB_EP_DTOG_RX) != 0)
-  {
+  if((regVal & USB_EP_DTOG_RX) != 0) {
     pcd_rx_dtog(USBx,bEpIdx);
   }
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_dtog(USB_TypeDef * USBx,  uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
-  if((regVal & USB_EP_DTOG_TX) != 0)
-  {
+  if((regVal & USB_EP_DTOG_TX) != 0) {
     pcd_tx_dtog(USBx,bEpIdx);
   }
 }
@@ -533,17 +506,15 @@ TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_tx_dtog(USB_TypeDef * USBx,
   * @param  bEpIdx Endpoint Number.
   * @retval None
   */
-
-TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_kind(USB_TypeDef * USBx,  uint32_t bEpIdx)
-{
+TU_ATTR_ALWAYS_INLINE static inline void pcd_set_ep_kind(USB_TypeDef * USBx,  uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal |= USB_EP_KIND;
   regVal &= USB_EPREG_MASK;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
   pcd_set_endpoint(USBx, bEpIdx, regVal);
 }
-TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_ep_kind(USB_TypeDef * USBx, uint32_t bEpIdx)
-{
+
+TU_ATTR_ALWAYS_INLINE static inline void pcd_clear_ep_kind(USB_TypeDef * USBx, uint32_t bEpIdx) {
   uint32_t regVal = pcd_get_endpoint(USBx, bEpIdx);
   regVal &= USB_EPKIND_MASK;
   regVal |= USB_EP_CTR_RX|USB_EP_CTR_TX;
diff --git a/src/portable/synopsys/dwc2/dcd_dwc2.c b/src/portable/synopsys/dwc2/dcd_dwc2.c
index 3e15d51c6..cf8f3be50 100644
--- a/src/portable/synopsys/dwc2/dcd_dwc2.c
+++ b/src/portable/synopsys/dwc2/dcd_dwc2.c
@@ -102,7 +102,13 @@ static bool _out_ep_closed;                   // Flag to check if RX FIFO size n
 // SOF enabling flag - required for SOF to not get disabled in ISR when SOF was enabled by
 static bool _sof_en;
 
-// Calculate the RX FIFO size according to recommendations from reference manual
+// Calculate the RX FIFO size according to minimum recommendations from reference manual
+// RxFIFO = (5 * number of control endpoints + 8) +
+//          ((largest USB packet used / 4) + 1 for status information) +
+//          (2 * number of OUT endpoints) + 1 for Global NAK
+// with number of control endpoints = 1 we have
+// RxFIFO = 15 + (largest USB packet used / 4) + 2 * number of OUT endpoints
+// we double the largest USB packet size to be able to hold up to 2 packets
 static inline uint16_t calc_grxfsiz(uint16_t max_ep_size, uint8_t ep_count) {
   return 15 + 2 * (max_ep_size / 4) + 2 * ep_count;
 }
@@ -121,6 +127,141 @@ static void update_grxfsiz(uint8_t rhport) {
   dwc2->grxfsiz = calc_grxfsiz(max_epsize, ep_count);
 }
 
+static bool fifo_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t packet_size) {
+  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
+  uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
+
+  TU_ASSERT(epnum < ep_count);
+
+  uint16_t const fifo_size = tu_div_ceil(packet_size, 4);
+
+  // "USB Data FIFOs" section in reference manual
+  // Peripheral FIFO architecture
+  //
+  // --------------- 320 or 1024 ( 1280 or 4096 bytes )
+  // | IN FIFO 0   |
+  // --------------- (320 or 1024) - 16
+  // | IN FIFO 1   |
+  // --------------- (320 or 1024) - 16 - x
+  // |   . . . .   |
+  // --------------- (320 or 1024) - 16 - x - y - ... - z
+  // | IN FIFO MAX |
+  // ---------------
+  // |    FREE     |
+  // --------------- GRXFSIZ
+  // | OUT FIFO    |
+  // | ( Shared )  |
+  // --------------- 0
+  //
+  // In FIFO is allocated by following rules:
+  // - IN EP 1 gets FIFO 1, IN EP "n" gets FIFO "n".
+  if (dir == TUSB_DIR_OUT) {
+    // Calculate required size of RX FIFO
+    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_controller[rhport].ep_fifo_size / 4);
+
+      // Enlarge RX FIFO
+      dwc2->grxfsiz = sz;
+    }
+  } else {
+    // Check if free space is available
+    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 %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_controller[rhport].ep_fifo_size / 4 - _allocated_fifo_words_tx);
+  }
+
+  return true;
+}
+
+static void edpt_activate(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc) {
+  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
+  uint8_t const epnum = tu_edpt_number(p_endpoint_desc->bEndpointAddress);
+  uint8_t const dir = tu_edpt_dir(p_endpoint_desc->bEndpointAddress);
+
+  xfer_ctl_t* xfer = XFER_CTL_BASE(epnum, dir);
+  xfer->max_size = tu_edpt_packet_size(p_endpoint_desc);
+  xfer->interval = p_endpoint_desc->bInterval;
+
+  // USBAEP, EPTYP, SD0PID_SEVNFRM, MPSIZ are the same for IN and OUT endpoints.
+  uint32_t const dxepctl = (1 << DOEPCTL_USBAEP_Pos) |
+                           (p_endpoint_desc->bmAttributes.xfer << DOEPCTL_EPTYP_Pos) |
+                           (p_endpoint_desc->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS ? DOEPCTL_SD0PID_SEVNFRM : 0) |
+                           (xfer->max_size << DOEPCTL_MPSIZ_Pos);
+
+  if (dir == TUSB_DIR_OUT) {
+    dwc2->epout[epnum].doepctl |= dxepctl;
+    dwc2->daintmsk |= TU_BIT(DAINTMSK_OEPM_Pos + epnum);
+  } else {
+    dwc2->epin[epnum].diepctl |= dxepctl | (epnum << DIEPCTL_TXFNUM_Pos);
+    dwc2->daintmsk |= (1 << (DAINTMSK_IEPM_Pos + epnum));
+  }
+}
+
+static void edpt_disable(uint8_t rhport, uint8_t ep_addr, bool stall) {
+  (void) rhport;
+
+  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir = tu_edpt_dir(ep_addr);
+
+  if (dir == TUSB_DIR_IN) {
+    dwc2_epin_t* epin = dwc2->epin;
+
+    // Only disable currently enabled non-control endpoint
+    if ((epnum == 0) || !(epin[epnum].diepctl & DIEPCTL_EPENA)) {
+      epin[epnum].diepctl |= DIEPCTL_SNAK | (stall ? DIEPCTL_STALL : 0);
+    } else {
+      // Stop transmitting packets and NAK IN xfers.
+      epin[epnum].diepctl |= DIEPCTL_SNAK;
+      while ((epin[epnum].diepint & DIEPINT_INEPNE) == 0) {}
+
+      // Disable the endpoint.
+      epin[epnum].diepctl |= DIEPCTL_EPDIS | (stall ? DIEPCTL_STALL : 0);
+      while ((epin[epnum].diepint & DIEPINT_EPDISD_Msk) == 0) {}
+
+      epin[epnum].diepint = DIEPINT_EPDISD;
+    }
+
+    // Flush the FIFO, and wait until we have confirmed it cleared.
+    dwc2->grstctl = ((epnum << GRSTCTL_TXFNUM_Pos) | GRSTCTL_TXFFLSH);
+    while ((dwc2->grstctl & GRSTCTL_TXFFLSH_Msk) != 0) {}
+  } else {
+    dwc2_epout_t* epout = dwc2->epout;
+
+    // Only disable currently enabled non-control endpoint
+    if ((epnum == 0) || !(epout[epnum].doepctl & DOEPCTL_EPENA)) {
+      epout[epnum].doepctl |= stall ? DOEPCTL_STALL : 0;
+    } else {
+      // Asserting GONAK is required to STALL an OUT endpoint.
+      // Simpler to use polling here, we don't use the "B"OUTNAKEFF interrupt
+      // anyway, and it can't be cleared by user code. If this while loop never
+      // finishes, we have bigger problems than just the stack.
+      dwc2->dctl |= DCTL_SGONAK;
+      while ((dwc2->gintsts & GINTSTS_BOUTNAKEFF_Msk) == 0) {}
+
+      // Ditto here- disable the endpoint.
+      epout[epnum].doepctl |= DOEPCTL_EPDIS | (stall ? DOEPCTL_STALL : 0);
+      while ((epout[epnum].doepint & DOEPINT_EPDISD_Msk) == 0) {}
+
+      epout[epnum].doepint = DOEPINT_EPDISD;
+
+      // Allow other OUT endpoints to keep receiving.
+      dwc2->dctl |= DCTL_CGONAK;
+    }
+  }
+}
+
 // Start of Bus Reset
 static void bus_reset(uint8_t rhport) {
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
@@ -139,6 +280,14 @@ static void bus_reset(uint8_t rhport) {
     dwc2->epout[n].doepctl |= DOEPCTL_SNAK;
   }
 
+  // flush all TX fifo and wait for it cleared
+  dwc2->grstctl = GRSTCTL_TXFFLSH | (0x10u << GRSTCTL_TXFNUM_Pos);
+  while (dwc2->grstctl & GRSTCTL_TXFFLSH_Msk) {}
+
+  // flush RX fifo and wait for it cleared
+  dwc2->grstctl = GRSTCTL_RXFFLSH;
+  while (dwc2->grstctl & GRSTCTL_RXFFLSH_Msk) {}
+
   // 2. Set up interrupt mask
   dwc2->daintmsk = TU_BIT(DAINTMSK_OEPM_Pos) | TU_BIT(DAINTMSK_IEPM_Pos);
   dwc2->doepmsk = DOEPMSK_STUPM | DOEPMSK_XFRCM;
@@ -269,7 +418,6 @@ static void edpt_schedule_packets(uint8_t rhport, uint8_t const epnum, uint8_t c
 /* Controller API
  *------------------------------------------------------------------*/
 #if CFG_TUSB_DEBUG >= DWC2_DEBUG
-
 void print_dwc2_info(dwc2_regs_t* dwc2) {
   // print guid, gsnpsid, ghwcfg1, ghwcfg2, ghwcfg3, ghwcfg4
   // use dwc2_info.py/md for bit-field value and comparison with other ports
@@ -280,7 +428,6 @@ void print_dwc2_info(dwc2_regs_t* dwc2) {
   }
   TU_LOG(DWC2_DEBUG, "0x%08lX\r\n", p[5]);
 }
-
 #endif
 
 static void reset_core(dwc2_regs_t* dwc2) {
@@ -462,9 +609,7 @@ void dcd_init(uint8_t rhport) {
   dwc2->gotgint |= int_mask;
 
   // Required as part of core initialization.
-  // TODO: How should mode mismatch be handled? It will cause
-  // the core to stop working/require reset.
-  dwc2->gintmsk = GINTMSK_OTGINT | GINTMSK_MMISM | GINTMSK_RXFLVLM |
+  dwc2->gintmsk = GINTMSK_OTGINT | GINTMSK_RXFLVLM |
                   GINTMSK_USBSUSPM | GINTMSK_USBRST | GINTMSK_ENUMDNEM | GINTMSK_WUIM;
 
   // Enable global interrupt
@@ -547,84 +692,8 @@ void dcd_sof_enable(uint8_t rhport, bool en) {
  *------------------------------------------------------------------*/
 
 bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const* desc_edpt) {
-  (void) 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 < ep_count);
-
-  xfer_ctl_t* xfer = XFER_CTL_BASE(epnum, dir);
-  xfer->max_size = tu_edpt_packet_size(desc_edpt);
-  xfer->interval = desc_edpt->bInterval;
-
-  uint16_t const fifo_size = tu_div_ceil(xfer->max_size, 4);
-
-  if (dir == TUSB_DIR_OUT) {
-    // Calculate required size of RX FIFO
-    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_controller[rhport].ep_fifo_size / 4);
-
-      // Enlarge RX FIFO
-      dwc2->grxfsiz = sz;
-    }
-
-    dwc2->epout[epnum].doepctl |= (1 << DOEPCTL_USBAEP_Pos) |
-                                  (desc_edpt->bmAttributes.xfer << DOEPCTL_EPTYP_Pos) |
-                                  (desc_edpt->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS ? DOEPCTL_SD0PID_SEVNFRM : 0) |
-                                  (xfer->max_size << DOEPCTL_MPSIZ_Pos);
-
-    dwc2->daintmsk |= TU_BIT(DAINTMSK_OEPM_Pos + epnum);
-  } else {
-    // "USB Data FIFOs" section in reference manual
-    // Peripheral FIFO architecture
-    //
-    // --------------- 320 or 1024 ( 1280 or 4096 bytes )
-    // | IN FIFO 0   |
-    // --------------- (320 or 1024) - 16
-    // | IN FIFO 1   |
-    // --------------- (320 or 1024) - 16 - x
-    // |   . . . .   |
-    // --------------- (320 or 1024) - 16 - x - y - ... - z
-    // | IN FIFO MAX |
-    // ---------------
-    // |    FREE     |
-    // --------------- GRXFSIZ
-    // | OUT FIFO    |
-    // | ( Shared )  |
-    // --------------- 0
-    //
-    // In FIFO is allocated by following rules:
-    // - 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_controller[rhport].ep_fifo_size / 4);
-
-    _allocated_fifo_words_tx += fifo_size;
-
-    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_controller[rhport].ep_fifo_size / 4 - _allocated_fifo_words_tx);
-
-    dwc2->epin[epnum].diepctl |= (1 << DIEPCTL_USBAEP_Pos) |
-                                 (epnum << DIEPCTL_TXFNUM_Pos) |
-                                 (desc_edpt->bmAttributes.xfer << DIEPCTL_EPTYP_Pos) |
-                                 (desc_edpt->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS ? DIEPCTL_SD0PID_SEVNFRM : 0) |
-                                 (xfer->max_size << DIEPCTL_MPSIZ_Pos);
-
-    dwc2->daintmsk |= (1 << (DAINTMSK_IEPM_Pos + epnum));
-  }
-
+  TU_ASSERT(fifo_alloc(rhport, desc_edpt->bEndpointAddress, tu_edpt_packet_size(desc_edpt)));
+  edpt_activate(rhport, desc_edpt);
   return true;
 }
 
@@ -650,6 +719,20 @@ void dcd_edpt_close_all(uint8_t rhport) {
   _allocated_fifo_words_tx = 16;
 }
 
+bool dcd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size) {
+  TU_ASSERT(fifo_alloc(rhport, ep_addr, largest_packet_size));
+  return true;
+}
+
+bool dcd_edpt_iso_activate(uint8_t rhport,  tusb_desc_endpoint_t const * p_endpoint_desc) {
+  // Disable EP to clear potential incomplete transfers
+  edpt_disable(rhport, p_endpoint_desc->bEndpointAddress, false);
+
+  edpt_activate(rhport, p_endpoint_desc);
+
+  return true;
+}
+
 bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes) {
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir = tu_edpt_dir(ep_addr);
@@ -707,71 +790,13 @@ bool dcd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t* ff, uint16_t
   return true;
 }
 
-static void dcd_edpt_disable(uint8_t rhport, uint8_t ep_addr, bool stall) {
-  (void) rhport;
-
-  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir = tu_edpt_dir(ep_addr);
-
-  if (dir == TUSB_DIR_IN) {
-    dwc2_epin_t* epin = dwc2->epin;
-
-    // Only disable currently enabled non-control endpoint
-    if ((epnum == 0) || !(epin[epnum].diepctl & DIEPCTL_EPENA)) {
-      epin[epnum].diepctl |= DIEPCTL_SNAK | (stall ? DIEPCTL_STALL : 0);
-    } else {
-      // Stop transmitting packets and NAK IN xfers.
-      epin[epnum].diepctl |= DIEPCTL_SNAK;
-      while ((epin[epnum].diepint & DIEPINT_INEPNE) == 0) {}
-
-      // Disable the endpoint.
-      epin[epnum].diepctl |= DIEPCTL_EPDIS | (stall ? DIEPCTL_STALL : 0);
-      while ((epin[epnum].diepint & DIEPINT_EPDISD_Msk) == 0) {}
-
-      epin[epnum].diepint = DIEPINT_EPDISD;
-    }
-
-    // Flush the FIFO, and wait until we have confirmed it cleared.
-    dwc2->grstctl = ((epnum << GRSTCTL_TXFNUM_Pos) | GRSTCTL_TXFFLSH);
-    while ((dwc2->grstctl & GRSTCTL_TXFFLSH_Msk) != 0) {}
-  } else {
-    dwc2_epout_t* epout = dwc2->epout;
-
-    // Only disable currently enabled non-control endpoint
-    if ((epnum == 0) || !(epout[epnum].doepctl & DOEPCTL_EPENA)) {
-      epout[epnum].doepctl |= stall ? DOEPCTL_STALL : 0;
-    } else {
-      // Asserting GONAK is required to STALL an OUT endpoint.
-      // Simpler to use polling here, we don't use the "B"OUTNAKEFF interrupt
-      // anyway, and it can't be cleared by user code. If this while loop never
-      // finishes, we have bigger problems than just the stack.
-      dwc2->dctl |= DCTL_SGONAK;
-      while ((dwc2->gintsts & GINTSTS_BOUTNAKEFF_Msk) == 0) {}
-
-      // Ditto here- disable the endpoint.
-      epout[epnum].doepctl |= DOEPCTL_EPDIS | (stall ? DOEPCTL_STALL : 0);
-      while ((epout[epnum].doepint & DOEPINT_EPDISD_Msk) == 0) {}
-
-      epout[epnum].doepint = DOEPINT_EPDISD;
-
-      // Allow other OUT endpoints to keep receiving.
-      dwc2->dctl |= DCTL_CGONAK;
-    }
-  }
-}
-
-/**
- * Close an endpoint.
- */
 void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr) {
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir = tu_edpt_dir(ep_addr);
 
-  dcd_edpt_disable(rhport, ep_addr, false);
+  edpt_disable(rhport, ep_addr, false);
 
   // Update max_size
   xfer_status[epnum][dir].max_size = 0;  // max_size = 0 marks a disabled EP - required for changing FIFO allocation
@@ -789,7 +814,7 @@ void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr) {
 }
 
 void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr) {
-  dcd_edpt_disable(rhport, ep_addr, true);
+  edpt_disable(rhport, ep_addr, true);
 }
 
 void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr) {