finally get rp2040 host epx working with 2-sof solution for switching

3 files changed, 181 insertions, 135 deletions

diff --git a/src/portable/raspberrypi/rp2040/dcd_rp2040.c b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
index d4c1bf708..8814f95d1 100644
--- a/src/portable/raspberrypi/rp2040/dcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
@@ -164,7 +164,7 @@ static void __tusb_irq_path_func(handle_hw_buff_status)(void) {
   while (buf_status) {
     // ctz/clz is faster than loop which has only a few bit set in general
     const uint8_t i = (uint8_t) __builtin_ctz(buf_status);
-    const uint bit = TU_BIT(i);
+    const uint32_t bit = TU_BIT(i);
 
     // IN transfer for even i, OUT transfer for odd i
     const uint8_t    epnum   = i >> 1u;
diff --git a/src/portable/raspberrypi/rp2040/hcd_rp2040.c b/src/portable/raspberrypi/rp2040/hcd_rp2040.c
index 3a99e7275..fb1676f50 100644
--- a/src/portable/raspberrypi/rp2040/hcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/hcd_rp2040.c
@@ -1,3 +1,4 @@
+
 /*
  * The MIT License (MIT)
  *
@@ -29,23 +30,23 @@
 
 #if CFG_TUH_ENABLED && (CFG_TUSB_MCU == OPT_MCU_RP2040) && !CFG_TUH_RPI_PIO_USB && !CFG_TUH_MAX3421
 
-#include "pico.h"
+  #include "pico.h"
 
-#if defined(PICO_RP2350) && PICO_RP2350 == 1
-#define HAS_STOP_EPX_ON_NAK
-#endif
+  #if defined(PICO_RP2350) && PICO_RP2350 == 1
+    #define HAS_STOP_EPX_ON_NAK
+  #endif
 
 // port 0 is native USB port, other is counted as software PIO
   #define RHPORT_NATIVE 0
 
-//--------------------------------------------------------------------+
+  //--------------------------------------------------------------------+
   // INCLUDE
-//--------------------------------------------------------------------+
-#include "rp2040_usb.h"
-#include "osal/osal.h"
+  //--------------------------------------------------------------------+
+  #include "rp2040_usb.h"
+  #include "osal/osal.h"
 
-#include "host/hcd.h"
-#include "host/usbh.h"
+  #include "host/hcd.h"
+  #include "host/usbh.h"
 
 //--------------------------------------------------------------------+
 //
@@ -55,6 +56,9 @@
 static hw_endpoint_t  ep_pool[USB_MAX_ENDPOINTS];
 static hw_endpoint_t *epx = &ep_pool[0]; // current active endpoint
 
+  #ifndef HAS_STOP_EPX_ON_NAK
+static volatile bool epx_switch_request = false;
+  #endif
 
 enum {
   SIE_CTRL_SPEED_DISCONNECT = 0,
@@ -67,7 +71,7 @@ enum {
 //--------------------------------------------------------------------+
 
 static hw_endpoint_t *edpt_alloc(void) {
-  for (uint i = 0; i < TU_ARRAY_SIZE(ep_pool); i++) {
+  for (uint i = 1; i < TU_ARRAY_SIZE(ep_pool); i++) {
     hw_endpoint_t *ep = &ep_pool[i];
     if (ep->max_packet_size == 0) {
       return ep;
@@ -89,11 +93,11 @@ static hw_endpoint_t *edpt_find(uint8_t daddr, uint8_t ep_addr) {
 }
 
 TU_ATTR_ALWAYS_INLINE static inline io_rw_32 *dpram_int_ep_ctrl(uint8_t int_num) {
-  return &usbh_dpram->int_ep_ctrl[int_num-1].ctrl;
+  return &usbh_dpram->int_ep_ctrl[int_num - 1].ctrl;
 }
 
-TU_ATTR_ALWAYS_INLINE static inline io_rw_32 * dpram_int_ep_buffer_ctrl(uint8_t int_num) {
-  return &usbh_dpram->int_ep_buffer_ctrl[int_num-1].ctrl;
+TU_ATTR_ALWAYS_INLINE static inline io_rw_32 *dpram_int_ep_buffer_ctrl(uint8_t int_num) {
+  return &usbh_dpram->int_ep_buffer_ctrl[int_num - 1].ctrl;
 }
 
 //--------------------------------------------------------------------+
@@ -113,30 +117,41 @@ TU_ATTR_ALWAYS_INLINE static inline bool need_pre(uint8_t dev_addr) {
 //--------------------------------------------------------------------+
 // EPX
 //--------------------------------------------------------------------+
+TU_ATTR_ALWAYS_INLINE static inline void sie_stop_xfer(void) {
+  uint32_t sie_ctrl = (usb_hw->sie_ctrl & SIE_CTRL_BASE_MASK) | USB_SIE_CTRL_STOP_TRANS_BITS;
+  usb_hw->sie_ctrl  = sie_ctrl;
+  while (usb_hw->sie_ctrl & USB_SIE_CTRL_STOP_TRANS_BITS) {}
+}
+
 TU_ATTR_ALWAYS_INLINE static inline void sie_start_xfer(bool send_setup, bool is_rx, bool need_pre) {
-  uint32_t value = usb_hw->sie_ctrl & SIE_CTRL_BASE_MASK; // preserve base bits
+  uint32_t sie_ctrl = usb_hw->sie_ctrl & SIE_CTRL_BASE_MASK; // preserve base bits
   if (send_setup) {
-    value |= USB_SIE_CTRL_SEND_SETUP_BITS;
+    sie_ctrl |= USB_SIE_CTRL_SEND_SETUP_BITS;
   } else {
-    value |= (is_rx ? USB_SIE_CTRL_RECEIVE_DATA_BITS : USB_SIE_CTRL_SEND_DATA_BITS);
+    sie_ctrl |= (is_rx ? USB_SIE_CTRL_RECEIVE_DATA_BITS : USB_SIE_CTRL_SEND_DATA_BITS);
   }
   if (need_pre) {
-    value |= USB_SIE_CTRL_PREAMBLE_EN_BITS;
+    sie_ctrl |= USB_SIE_CTRL_PREAMBLE_EN_BITS;
   }
 
   // START_TRANS bit on SIE_CTRL has the same behavior as the AVAILABLE bit
   // described in RP2040 Datasheet, release 2.1, section "4.1.2.5.1. Concurrent access".
   // We write everything except the START_TRANS bit first, then wait some cycles.
-  usb_hw->sie_ctrl = value;
+  usb_hw->sie_ctrl = sie_ctrl;
   busy_wait_at_least_cycles(12);
-  usb_hw->sie_ctrl = value | USB_SIE_CTRL_START_TRANS_BITS;
+  usb_hw->sie_ctrl = sie_ctrl | USB_SIE_CTRL_START_TRANS_BITS;
+}
+
+TU_ATTR_ALWAYS_INLINE static inline void epx_start_xfer(hw_endpoint_t *ep, bool is_setup) {
+  usb_hw->dev_addr_ctrl = (uint32_t)(ep->dev_addr | (tu_edpt_number(ep->ep_addr) << USB_ADDR_ENDP_ENDPOINT_LSB));
+  sie_start_xfer(is_setup, tu_edpt_dir(ep->ep_addr) == TUSB_DIR_IN, ep->need_pre);
 }
 
 // prepare epx_ctrl register for new endpoint
 TU_ATTR_ALWAYS_INLINE static inline void epx_ctrl_prepare(hw_endpoint_t *ep) {
-  // RP2040-E4: USB host writes status to upper half of buffer control in single buffered mode.
+  // RP2040-E4: USB host writes status to the upper half of buffer control in single buffered mode.
   // The buffer selector toggles even in single-buffered mode, so the previous transfer's status
-  // may have been written to BUF1 half, leaving BUF0 with stale AVAILABLE bit. Clear it here.
+  // may have been written to BUF1 half, leaving BUF0 with a stale AVAILABLE bit. Clear it here.
   #if defined(PICO_RP2040) && PICO_RP2040 == 1
   usbh_dpram->epx_buf_ctrl = 0;
   #endif
@@ -147,23 +162,46 @@ TU_ATTR_ALWAYS_INLINE static inline void epx_ctrl_prepare(hw_endpoint_t *ep) {
   usbh_dpram->epx_ctrl = ep_ctrl;
 }
 
-// save on-going context
-static void __tusb_irq_path_func(epx_save_context)(void) {
-  const uint32_t buf_ctrl = usbh_dpram->epx_buf_ctrl;
-  const uint16_t buf0_len = buf_ctrl & USB_BUF_CTRL_LEN_MASK; // TODO handle double buffered case
-  epx->remaining_len      = (uint16_t)(epx->remaining_len + buf0_len);
-  epx->next_pid           = (buf_ctrl & USB_BUF_CTRL_DATA1_PID) ? 1 : 0;
-  if (tu_edpt_dir(epx->ep_addr) == TUSB_DIR_OUT) {
-    epx->user_buf -= buf0_len;
-  }
-  epx->pending = 1;
-  epx->active  = false;
+// Save buffer context for EPX preemption (called after STOP_TRANS).
+// Undo PID toggle and buffer accounting for buffers NOT completed on the wire.
+// A buffer completed on wire means: controller reached STATUS phase (ACK received).
+//   OUT completed: FULL cleared to 0 in STATUS phase (was 1 when armed)
+//   IN  completed: FULL set to 1 in STATUS phase (was 0 when armed)
+// So undo when: AVAIL=1 (never started), or (OUT: FULL=1) or (IN: FULL=0)
+static void __tusb_irq_path_func(epx_save_context)(hw_endpoint_t *ep) {
+  uint32_t   buf_ctrl = usbh_dpram->epx_buf_ctrl;
+  const bool is_out   = (tu_edpt_dir(ep->ep_addr) == TUSB_DIR_OUT);
+
+  do {
+    const uint16_t bc16 = (uint16_t)buf_ctrl;
+    if (bc16) {
+      const bool avail = (bc16 & USB_BUF_CTRL_AVAIL);
+      const bool full  = (bc16 & USB_BUF_CTRL_FULL);
+      if (avail || (is_out ? full : !full)) {
+        const uint16_t buf_len = bc16 & USB_BUF_CTRL_LEN_MASK;
+        ep->remaining_len += buf_len;
+        ep->next_pid ^= 1u;
+        if (is_out) {
+          ep->user_buf -= buf_len;
+        }
+      }
+    }
+
+    if (usbh_dpram->epx_ctrl & EP_CTRL_DOUBLE_BUFFERED_BITS) {
+      buf_ctrl >>= 16;
+    } else {
+      buf_ctrl = 0;
+    }
+  } while (buf_ctrl > 0);
 
   usbh_dpram->epx_buf_ctrl = 0;
+
+  ep->pending = 1;
+  ep->active  = false;
 }
 
 // All non-interrupt endpoints use shared EPX.
-// Save current EPX context, mark pending, switch to ep
+// Save the current EPX context, mark pending, switch to ep
 static void __tusb_irq_path_func(epx_switch_ep)(hw_endpoint_t *ep) {
   const bool is_setup = (ep->pending == 2);
 
@@ -172,19 +210,17 @@ static void __tusb_irq_path_func(epx_switch_ep)(hw_endpoint_t *ep) {
   ep->active  = true;
 
   if (is_setup) {
-    usb_hw->dev_addr_ctrl = ep->dev_addr;
-    sie_start_xfer(true, false, ep->need_pre);
+    // panic("new setup \n");
+    epx_start_xfer(ep, true);
   } else {
-    const uint8_t ep_num  = tu_edpt_number(ep->ep_addr);
-    const bool    is_rx   = tu_edpt_dir(ep->ep_addr) == TUSB_DIR_IN;
-    io_rw_32     *ep_reg  = &usbh_dpram->epx_ctrl;
-    io_rw_32     *buf_reg = &usbh_dpram->epx_buf_ctrl;
+    io_rw_32 *ep_reg  = &usbh_dpram->epx_ctrl;
+    io_rw_32 *buf_reg = &usbh_dpram->epx_buf_ctrl;
 
     epx_ctrl_prepare(ep);
-    rp2usb_buffer_start(ep, ep_reg, buf_reg, is_rx, ep->transfer_type == TUSB_XFER_INTERRUPT);
+    rp2usb_buffer_start(ep, ep_reg, buf_reg, tu_edpt_dir(ep->ep_addr) == TUSB_DIR_IN,
+                         ep->transfer_type == TUSB_XFER_INTERRUPT);
 
-    usb_hw->dev_addr_ctrl = (uint32_t)(ep->dev_addr | (ep_num << USB_ADDR_ENDP_ENDPOINT_LSB));
-    sie_start_xfer(false, is_rx, ep->need_pre); // start transfer
+    epx_start_xfer(ep, false);
   }
 }
 
@@ -208,16 +244,14 @@ static hw_endpoint_t *__tusb_irq_path_func(epx_next_pending)(hw_endpoint_t *cur_
 //--------------------------------------------------------------------+
 // Interrupt handlers
 //--------------------------------------------------------------------+
-static void __tusb_irq_path_func(xfer_complete_isr)(hw_endpoint_t *ep, xfer_result_t xfer_result) {
+static void __tusb_irq_path_func(xfer_complete_isr)(hw_endpoint_t *ep, xfer_result_t xfer_result, bool is_more) {
   // Mark transfer as done before we tell the tinyusb stack
-  uint8_t dev_addr    = ep->dev_addr;
-  uint8_t ep_addr     = ep->ep_addr;
-  uint    xferred_len = ep->xferred_len;
+  uint xferred_len = ep->xferred_len;
   rp2usb_reset_transfer(ep);
-  hcd_event_xfer_complete(dev_addr, ep_addr, xferred_len, xfer_result, true);
+  hcd_event_xfer_complete(ep->dev_addr, ep->ep_addr, xferred_len, xfer_result, true);
 
   // Carry more transfer on epx
-  if (ep == epx) {
+  if (is_more) {
     hw_endpoint_t *next_ep = epx_next_pending(epx);
     if (next_ep != NULL) {
       epx_switch_ep(next_ep);
@@ -231,26 +265,31 @@ static void __tusb_irq_path_func(handle_buf_status_isr)(void) {
     BUF_STATUS_EPX = 1u
   };
 
-  // Check EPX first (bit 0). EPX is currently single-buffered, always use buf_id=0.3
+  // Check EPX first (bit 0).
   // Double-buffered: if both buffers completed at once, buf_status re-sets
   // immediately after clearing (datasheet Table 406). Process the second buffer too.
   while (usb_hw->buf_status & BUF_STATUS_EPX) {
-    const uint8_t buf_id = (usb_hw->buf_cpu_should_handle & BUF_STATUS_EPX) ? 1 : 0;
+    const uint8_t buf_id     = (usb_hw->buf_cpu_should_handle & BUF_STATUS_EPX) ? 1 : 0;
     usb_hw_clear->buf_status = 1u; // clear
 
     io_rw_32 *ep_reg  = &usbh_dpram->epx_ctrl;
     io_rw_32 *buf_reg = &usbh_dpram->epx_buf_ctrl;
+  #ifndef HAS_STOP_EPX_ON_NAK
+    // Any packet completion (mid-transfer or final) means data is flowing.
+    // Clear switch request so the 2-SOF fallback only fires for NAK-retrying endpoints.
+    epx_switch_request = false;
+  #endif
     if (rp2usb_xfer_continue(epx, ep_reg, buf_reg, buf_id, tu_edpt_dir(epx->ep_addr) == TUSB_DIR_IN)) {
-      xfer_complete_isr(epx, XFER_RESULT_SUCCESS);
+      xfer_complete_isr(epx, XFER_RESULT_SUCCESS, true);
     }
   }
 
   // Check "interrupt" (asynchronous) endpoints for both IN and OUT
-  uint32_t buf_status = usb_hw->buf_status & (uint32_t)~BUF_STATUS_EPX;
+  uint32_t buf_status = usb_hw->buf_status & ~(uint32_t)BUF_STATUS_EPX;
   while (buf_status) {
     // ctz/clz is faster than loop which has only a few bit set in general
-    const uint8_t idx = (uint8_t) __builtin_ctz(buf_status);
-    const uint bit = TU_BIT(idx);
+    const uint8_t  idx       = (uint8_t)__builtin_ctz(buf_status);
+    const uint32_t bit       = TU_BIT(idx);
     usb_hw_clear->buf_status = bit;
     buf_status &= ~bit;
 
@@ -258,7 +297,7 @@ static void __tusb_irq_path_func(handle_buf_status_isr)(void) {
     // EPX  is bit 0. Bit 1 is not used
     // IEP1 IN/OUT is bit 2, 3
     // IEP2 IN/OUT is bit 4, 5 etc
-    const uint8_t    epnum = idx >> 1u;
+    const uint8_t epnum = idx >> 1u;
     for (size_t e = 0; e < TU_ARRAY_SIZE(ep_pool); e++) {
       hw_endpoint_t *ep = &ep_pool[e];
       if (ep->interrupt_num == epnum) {
@@ -266,7 +305,7 @@ static void __tusb_irq_path_func(handle_buf_status_isr)(void) {
         io_rw_32  *buf_reg = dpram_int_ep_buffer_ctrl(ep->interrupt_num);
         const bool done    = rp2usb_xfer_continue(ep, ep_reg, buf_reg, 0, tu_edpt_dir(ep->ep_addr) == TUSB_DIR_IN);
         if (done) {
-          xfer_complete_isr(ep, XFER_RESULT_SUCCESS);
+          xfer_complete_isr(ep, XFER_RESULT_SUCCESS, false);
         }
         break;
       }
@@ -293,69 +332,80 @@ static void __tusb_irq_path_func(hcd_rp2040_irq)(void) {
   }
 
   if (status & USB_INTS_STALL_BITS) {
-    // We have rx'd a stall from the device
-    // NOTE THIS SHOULD HAVE PRIORITY OVER BUFF_STATUS
-    // AND TRANS_COMPLETE as the stall is an alternative response
-    // to one of those events
     usb_hw_clear->sie_status = USB_SIE_STATUS_STALL_REC_BITS;
-    xfer_complete_isr(epx, XFER_RESULT_STALLED);
+    xfer_complete_isr(epx, XFER_RESULT_STALLED, true);
   }
 
-  if (status & USB_INTS_BUFF_STATUS_BITS) {
-    handle_buf_status_isr();
+  if (status & USB_INTS_ERROR_RX_TIMEOUT_BITS) {
+    usb_hw_clear->sie_status = USB_SIE_STATUS_RX_TIMEOUT_BITS;
+
+    const uint32_t sie_ctrl = (usb_hw->sie_ctrl & SIE_CTRL_BASE_MASK) | USB_SIE_CTRL_STOP_TRANS_BITS;
+    usb_hw->sie_ctrl        = sie_ctrl;
+    // while (usb_hw->sie_ctrl & USB_SIE_CTRL_STOP_TRANS_BITS) {}
+
+    // Even if STOP_TRANS bit is clear, controller maybe in middle of retrying and may re-raise timeout once extra time
+    // Only handle if epx is active, don't carry more epx transfer since STOP_TRANS is raced and not safe.
+    if (epx->active) {
+      xfer_complete_isr(epx, XFER_RESULT_FAILED, false);
+    }
   }
 
   if (status & USB_INTS_TRANS_COMPLETE_BITS) {
+    // only applies for epx, interrupt endpoint does not seem to raise this
     usb_hw_clear->sie_status = USB_SIE_STATUS_TRANS_COMPLETE_BITS;
-
-    // only handle a setup packet
     if (usb_hw->sie_ctrl & USB_SIE_CTRL_SEND_SETUP_BITS) {
-      epx->xferred_len = 8;
-      xfer_complete_isr(epx, XFER_RESULT_SUCCESS);
-    } else {
-      // Don't care. Will handle this in buff status
+      uint32_t sie_ctrl = usb_hw->sie_ctrl & SIE_CTRL_BASE_MASK;
+      usb_hw->sie_ctrl  = sie_ctrl; // clear setup bit
+      epx->xferred_len  = 8;
+      xfer_complete_isr(epx, XFER_RESULT_SUCCESS, true);
     }
   }
 
+  if (status & USB_INTS_BUFF_STATUS_BITS) {
+    handle_buf_status_isr();
+  }
+
+  // SOF-based round-robin MUST run BEFORE BUFF_STATUS to avoid processing
+  // buf_status on the wrong EPX after a completion+switch in handle_buf_status_isr.
   #ifdef HAS_STOP_EPX_ON_NAK
   if (status & USB_INTS_EPX_STOPPED_ON_NAK_BITS) {
     usb_hw_clear->nak_poll = USB_NAK_POLL_EPX_STOPPED_ON_NAK_BITS;
     hw_endpoint_t *next_ep = epx_next_pending(epx);
     if (next_ep != NULL) {
-      epx_save_context();
+      epx_save_context(epx);
       epx_switch_ep(next_ep);
     } else {
-      // No pending endpoint, this is the only active one: disable stop-on-NAK, continue current transfer
       usb_hw_clear->nak_poll = USB_NAK_POLL_STOP_EPX_ON_NAK_BITS;
       sie_start_xfer(false, TUSB_DIR_IN == tu_edpt_dir(epx->ep_addr), epx->need_pre);
     }
   }
   #else
-  // RP2040: on SOF, stop and switch if there's a pending ep
+  // RP2040: on SOF, switch EPX if another endpoint is pending.
+  // First SOF sets epx_switch_request. If a transfer completes before next SOF, the flag is
+  // cleared (data is flowing, no need to force-switch). Second SOF with flag still set means
+  // no data exchanged (endpoint NAK-retrying): STOP_TRANS is safe and we switch.
+  // This avoids stopping mid-data-transfer which corrupts double-buffered PID tracking.
   if (status & USB_INTS_HOST_SOF_BITS) {
     (void)usb_hw->sof_rd; // clear SOF by reading SOF_RD
     hw_endpoint_t *next_ep = epx_next_pending(epx);
     if (next_ep == NULL) {
-      // no more pending --> disable SOF
       usb_hw_clear->inte = USB_INTE_HOST_SOF_BITS;
       usb_hw->nak_poll   = USB_NAK_POLL_RESET;
-    } else {
-      // stop transfer if is active
-      if (epx->active) {
-        usb_hw_set->sie_ctrl = USB_SIE_CTRL_STOP_TRANS_BITS;
-        while (usb_hw->sie_ctrl & USB_SIE_CTRL_STOP_TRANS_BITS) {}
+      epx_switch_request = false;
+    } else if (epx->active) {
+      if (epx_switch_request) {
+        // Second SOF with no transfer completion: endpoint is NAK-retrying, safe to switch.
+        epx_switch_request = false;
+        sie_stop_xfer();
+        epx_save_context(epx);
+        epx_switch_ep(next_ep);
+      } else {
+        epx_switch_request = true;
       }
-
-      epx_save_context();
-      epx_switch_ep(next_ep);
     }
   }
   #endif
 
-  if (status & USB_INTS_ERROR_RX_TIMEOUT_BITS) {
-    usb_hw_clear->sie_status = USB_SIE_STATUS_RX_TIMEOUT_BITS;
-  }
-
   if (status & USB_INTS_ERROR_DATA_SEQ_BITS) {
     usb_hw_clear->sie_status = USB_SIE_STATUS_DATA_SEQ_ERROR_BITS;
     panic("Data Seq Error \n");
@@ -371,9 +421,9 @@ void __tusb_irq_path_func(hcd_int_handler)(uint8_t rhport, bool in_isr) {
 //--------------------------------------------------------------------+
 // HCD API
 //--------------------------------------------------------------------+
-bool hcd_init(uint8_t rhport, const tusb_rhport_init_t* rh_init) {
-  (void) rhport;
-  (void) rh_init;
+bool hcd_init(uint8_t rhport, const tusb_rhport_init_t *rh_init) {
+  (void)rhport;
+  (void)rh_init;
   pico_trace("hcd_init %d\n", rhport);
   assert(rhport == 0);
 
@@ -392,24 +442,20 @@ bool hcd_init(uint8_t rhport, const tusb_rhport_init_t* rh_init) {
 
   // Enable in host mode with SOF / Keep alive on
   usb_hw->main_ctrl = USB_MAIN_CTRL_CONTROLLER_EN_BITS | USB_MAIN_CTRL_HOST_NDEVICE_BITS;
-  usb_hw->sie_ctrl = SIE_CTRL_BASE;
-  usb_hw->inte = USB_INTE_BUFF_STATUS_BITS      |
-                 USB_INTE_HOST_CONN_DIS_BITS    |
-                 USB_INTE_HOST_RESUME_BITS      |
-                 USB_INTE_STALL_BITS            |
-                 USB_INTE_TRANS_COMPLETE_BITS   |
-                 USB_INTE_ERROR_RX_TIMEOUT_BITS |
-                 USB_INTE_ERROR_DATA_SEQ_BITS   ;
+  usb_hw->sie_ctrl  = SIE_CTRL_BASE;
+  usb_hw->inte      = USB_INTE_BUFF_STATUS_BITS | USB_INTE_HOST_CONN_DIS_BITS | USB_INTE_HOST_RESUME_BITS |
+                 USB_INTE_STALL_BITS | USB_INTE_TRANS_COMPLETE_BITS | USB_INTE_ERROR_RX_TIMEOUT_BITS |
+                 USB_INTE_ERROR_DATA_SEQ_BITS;
 
-#ifdef HAS_STOP_EPX_ON_NAK
+  #ifdef HAS_STOP_EPX_ON_NAK
   usb_hw_set->inte = USB_INTE_EPX_STOPPED_ON_NAK_BITS;
-#endif
+  #endif
 
   return true;
 }
 
 bool hcd_deinit(uint8_t rhport) {
-  (void) rhport;
+  (void)rhport;
   irq_remove_handler(USBCTRL_IRQ, hcd_rp2040_irq);
   reset_block(RESETS_RESET_USBCTRL_BITS);
   unreset_block_wait(RESETS_RESET_USBCTRL_BITS);
@@ -417,7 +463,7 @@ bool hcd_deinit(uint8_t rhport) {
 }
 
 void hcd_port_reset(uint8_t rhport) {
-  (void) rhport;
+  (void)rhport;
   // TODO: Nothing to do here yet. Perhaps need to reset some state?
 }
 
@@ -445,7 +491,10 @@ tusb_speed_t hcd_port_speed_get(uint8_t rhport) {
 // Close all opened endpoint belong to this device
 void hcd_device_close(uint8_t rhport, uint8_t dev_addr) {
   (void)rhport;
-  (void)dev_addr;
+
+  if (dev_addr == 0) {
+    return; // address 0 is for device enumeration
+  }
 
   // reset epx if it is currently active with unplugged device
   if (epx->max_packet_size > 0 && epx->dev_addr == dev_addr) {
@@ -462,13 +511,13 @@ void hcd_device_close(uint8_t rhport, uint8_t dev_addr) {
 
       if (ep->interrupt_num) {
         // disable interrupt endpoint
-        usb_hw_clear->int_ep_ctrl                       = 1u << ep->interrupt_num;
+        usb_hw_clear->int_ep_ctrl                       = TU_BIT(ep->interrupt_num);
         usb_hw->int_ep_addr_ctrl[ep->interrupt_num - 1] = 0;
 
         io_rw_32 *ep_reg  = dpram_int_ep_ctrl(ep->interrupt_num);
         io_rw_32 *buf_reg = dpram_int_ep_buffer_ctrl(ep->interrupt_num);
-        *buf_reg = 0;
-        *ep_reg = 0;
+        *buf_reg          = 0;
+        *ep_reg           = 0;
       }
 
       ep->max_packet_size = 0; // mark as unused
@@ -498,13 +547,17 @@ void hcd_int_disable(uint8_t rhport) {
 bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, const tusb_desc_endpoint_t *ep_desc) {
   (void)rhport;
   pico_trace("hcd_edpt_open dev_addr %d, ep_addr %d\n", dev_addr, ep_desc->bEndpointAddress);
-  hw_endpoint_t *ep = edpt_alloc();
+  hw_endpoint_t *ep;
+  if (dev_addr == 0) {
+    ep = &ep_pool[0];
+  } else {
+    ep = edpt_alloc();
+  }
   TU_ASSERT(ep);
 
   const uint8_t  ep_addr         = ep_desc->bEndpointAddress;
   const uint16_t max_packet_size = tu_edpt_packet_size(ep_desc);
   const uint8_t  transfer_type   = ep_desc->bmAttributes.xfer;
-  // const uint8_t    bmInterval     = ep_desc->bInterval;
 
   ep->max_packet_size = max_packet_size;
   ep->ep_addr         = ep_addr;
@@ -532,7 +585,7 @@ bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, const tusb_desc_endpoint_t
     ep->dpram_buf    = (uint8_t *)(USBCTRL_DPRAM_BASE + USB_DPRAM_MAX - (int_idx + 1u) * 64u);
     uint32_t ep_ctrl = EP_CTRL_ENABLE_BITS | EP_CTRL_INTERRUPT_PER_BUFFER |
                        (TUSB_XFER_INTERRUPT << EP_CTRL_BUFFER_TYPE_LSB) | hw_data_offset(ep->dpram_buf) |
-                       (uint32_t)((ep_desc->bInterval - 1) << EP_CTRL_HOST_INTERRUPT_INTERVAL_LSB);
+                       ((uint32_t)(ep_desc->bInterval - 1) << EP_CTRL_HOST_INTERRUPT_INTERVAL_LSB);
     usbh_dpram->int_ep_ctrl[int_idx].ctrl = ep_ctrl;
 
     //------------- address control -------------//
@@ -547,7 +600,7 @@ bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, const tusb_desc_endpoint_t
     usb_hw->int_ep_addr_ctrl[int_idx] = addr_ctrl;
 
     // Finally, activate interrupt endpoint
-    usb_hw_set->int_ep_ctrl = 1u << ep->interrupt_num;
+    usb_hw_set->int_ep_ctrl = TU_BIT(ep->interrupt_num);
   }
 
   return true;
@@ -560,6 +613,14 @@ bool hcd_edpt_close(uint8_t rhport, uint8_t daddr, uint8_t ep_addr) {
   return false; // TODO not implemented yet
 }
 
+bool hcd_edpt_abort_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr) {
+  (void)rhport;
+  (void)dev_addr;
+  (void)ep_addr;
+  // TODO not implemented yet
+  return false;
+}
+
 bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t *buffer, uint16_t buflen) {
   (void)rhport;
 
@@ -594,19 +655,14 @@ bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t *b
       usb_hw_set->inte = USB_INTE_HOST_SOF_BITS;
   #endif
     } else {
-      const uint8_t    ep_num  = tu_edpt_number(ep->ep_addr);
-      const tusb_dir_t ep_dir  = tu_edpt_dir(ep->ep_addr);
-      const bool       is_rx   = (ep_dir == TUSB_DIR_IN);
-      io_rw_32        *ep_reg  = &usbh_dpram->epx_ctrl;
-      io_rw_32        *buf_reg = &usbh_dpram->epx_buf_ctrl;
+      io_rw_32 *ep_reg  = &usbh_dpram->epx_ctrl;
+      io_rw_32 *buf_reg = &usbh_dpram->epx_buf_ctrl;
 
       epx = ep;
 
       epx_ctrl_prepare(ep);
       rp2usb_xfer_start(ep, ep_reg, buf_reg, buffer, NULL, buflen); // prepare bufctrl
-
-      usb_hw->dev_addr_ctrl = (uint32_t)(ep->dev_addr | (ep_num << USB_ADDR_ENDP_ENDPOINT_LSB));
-      sie_start_xfer(false, is_rx, ep->need_pre);                   // start transfer
+      epx_start_xfer(ep, false);
     }
     rp2usb_critical_exit();
   }
@@ -614,27 +670,19 @@ bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t *b
   return true;
 }
 
-bool hcd_edpt_abort_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr) {
-  (void)rhport;
-  (void)dev_addr;
-  (void)ep_addr;
-  // TODO not implemented yet
-  return false;
-}
-
 bool hcd_setup_send(uint8_t rhport, uint8_t dev_addr, const uint8_t setup_packet[8]) {
   (void)rhport;
 
-  // Copy data into setup packet buffer (usbh only schedules one setup at a time)
-  for (uint8_t i = 0; i < 8; i++) {
-    usbh_dpram->setup_packet[i] = setup_packet[i];
-  }
-
   hw_endpoint_t *ep = edpt_find(dev_addr, 0x00);
   TU_ASSERT(ep);
 
   rp2usb_critical_enter();
 
+  // Copy data into setup packet buffer (usbh only schedules one setup at a time)
+  for (uint8_t i = 0; i < 8; i++) {
+    usbh_dpram->setup_packet[i] = setup_packet[i];
+  }
+
   ep->ep_addr       = 0; // setup is OUT
   ep->remaining_len = 8;
   ep->xferred_len   = 0;
@@ -651,9 +699,7 @@ bool hcd_setup_send(uint8_t rhport, uint8_t dev_addr, const uint8_t setup_packet
   } else {
     epx        = ep;
     ep->active = true;
-
-    usb_hw->dev_addr_ctrl = dev_addr;                 // Set device address
-    sie_start_xfer(true, false, ep->need_pre); // start transfer
+    epx_start_xfer(ep, true);
   }
 
   rp2usb_critical_exit();
diff --git a/src/portable/raspberrypi/rp2040/rp2040_usb.c b/src/portable/raspberrypi/rp2040/rp2040_usb.c
index 21237b059..156be62e4 100644
--- a/src/portable/raspberrypi/rp2040/rp2040_usb.c
+++ b/src/portable/raspberrypi/rp2040/rp2040_usb.c
@@ -197,7 +197,7 @@ void __tusb_irq_path_func(rp2usb_buffer_start)(hw_endpoint_t *ep, io_rw_32 *ep_r
       ep_ctrl |= EP_CTRL_DOUBLE_BUFFERED_BITS;
     } else {
       // Only buf0 used: clear DOUBLE_BUFFERED so controller doesn't toggle buffer selector
-      ep_ctrl &= ~EP_CTRL_DOUBLE_BUFFERED_BITS;
+      ep_ctrl &= ~(uint32_t)EP_CTRL_DOUBLE_BUFFERED_BITS;
     }
     *ep_reg = ep_ctrl;
   }