Merge branch 'master' into vendor_class_zero_length_transfer

6 files changed, 1630 insertions, 1057 deletions

diff --git a/src/portable/raspberrypi/pio_usb/dcd_pio_usb.c b/src/portable/raspberrypi/pio_usb/dcd_pio_usb.c
new file mode 100644
index 000000000..e6daf6827
--- /dev/null
+++ b/src/portable/raspberrypi/pio_usb/dcd_pio_usb.c
@@ -0,0 +1,210 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2018, hathach (tinyusb.org)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * This file is part of the TinyUSB stack.
+ */
+
+#include "tusb_option.h"
+
+#if CFG_TUD_ENABLED && (CFG_TUSB_MCU == OPT_MCU_RP2040) && CFG_TUD_RPI_PIO_USB
+
+#include "pico.h"
+#include "pio_usb.h"
+#include "pio_usb_ll.h"
+
+#include "device/dcd.h"
+
+//--------------------------------------------------------------------+
+// MACRO TYPEDEF CONSTANT ENUM DECLARATION
+//--------------------------------------------------------------------+
+
+#define RHPORT_OFFSET     1
+#define RHPORT_PIO(_x)    ((_x)-RHPORT_OFFSET)
+
+//-------------  -------------//
+static usb_device_t *usb_device = NULL;
+static usb_descriptor_buffers_t desc;
+
+/*------------------------------------------------------------------*/
+/* Device API
+ *------------------------------------------------------------------*/
+
+// Initialize controller to device mode
+void dcd_init (uint8_t rhport)
+{
+  (void) rhport;
+
+  static pio_usb_configuration_t config = PIO_USB_DEFAULT_CONFIG;
+  usb_device = pio_usb_device_init(&config, &desc);
+}
+
+// Enable device interrupt
+void dcd_int_enable (uint8_t rhport)
+{
+  (void) rhport;
+}
+
+// Disable device interrupt
+void dcd_int_disable (uint8_t rhport)
+{
+  (void) rhport;
+}
+
+// Receive Set Address request, mcu port must also include status IN response
+void dcd_set_address (uint8_t rhport, uint8_t dev_addr)
+{
+  // must be called before queuing status
+  pio_usb_device_set_address(dev_addr);
+  dcd_edpt_xfer(rhport, 0x80, NULL, 0);
+}
+
+// Wake up host
+void dcd_remote_wakeup (uint8_t rhport)
+{
+  (void) rhport;
+}
+
+// Connect by enabling internal pull-up resistor on D+/D-
+void dcd_connect(uint8_t rhport)
+{
+  (void) rhport;
+}
+
+// Disconnect by disabling internal pull-up resistor on D+/D-
+void dcd_disconnect(uint8_t rhport)
+{
+  (void) rhport;
+}
+
+//--------------------------------------------------------------------+
+// Endpoint API
+//--------------------------------------------------------------------+
+
+// Configure endpoint's registers according to descriptor
+bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_ep)
+{
+  (void) rhport;
+  return pio_usb_device_endpoint_open((uint8_t const*) desc_ep);
+}
+
+void dcd_edpt_close_all (uint8_t rhport)
+{
+  (void) rhport;
+}
+
+// Submit a transfer, When complete dcd_event_xfer_complete() is invoked to notify the stack
+bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
+{
+  (void) rhport;
+  endpoint_t *ep = pio_usb_device_get_endpoint_by_address(ep_addr);
+  return pio_usb_ll_transfer_start(ep, buffer, total_bytes);
+}
+
+// Submit a transfer where is managed by FIFO, When complete dcd_event_xfer_complete() is invoked to notify the stack - optional, however, must be listed in usbd.c
+//bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16_t total_bytes)
+//{
+//  (void) rhport;
+//  (void) ep_addr;
+//  (void) ff;
+//  (void) total_bytes;
+//  return false;
+//}
+
+// Stall endpoint
+void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+  endpoint_t *ep = pio_usb_device_get_endpoint_by_address(ep_addr);
+  ep->has_transfer = false;
+  ep->stalled = true;
+}
+
+// clear stall, data toggle is also reset to DATA0
+void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+  endpoint_t *ep = pio_usb_device_get_endpoint_by_address(ep_addr);
+  ep->data_id = 0;
+  ep->stalled = false;
+}
+
+//--------------------------------------------------------------------+
+//
+//--------------------------------------------------------------------+
+
+static void __no_inline_not_in_flash_func(handle_endpoint_irq)(uint8_t tu_rhport, xfer_result_t result, volatile uint32_t* ep_reg)
+{
+  const uint32_t ep_all = *ep_reg;
+
+  for(uint8_t ep_idx = 0; ep_idx < PIO_USB_EP_POOL_CNT; ep_idx++)
+  {
+    uint32_t const mask = (1u << ep_idx);
+
+    if (ep_all & mask)
+    {
+      endpoint_t* ep = PIO_USB_ENDPOINT(ep_idx);
+      dcd_event_xfer_complete(tu_rhport, ep->ep_num, ep->actual_len, result, true);
+    }
+  }
+
+  // clear all
+  (*ep_reg) &= ~ep_all;
+}
+
+// IRQ Handler
+void __no_inline_not_in_flash_func(pio_usb_device_irq_handler)(uint8_t root_id)
+{
+  uint8_t const tu_rhport = root_id + 1;
+  root_port_t* rport = PIO_USB_ROOT_PORT(root_id);
+  uint32_t const ints = rport->ints;
+
+  if (ints & PIO_USB_INTS_RESET_END_BITS)
+  {
+    dcd_event_bus_reset(tu_rhport, TUSB_SPEED_FULL, true);
+  }
+
+  if (ints & PIO_USB_INTS_SETUP_REQ_BITS)
+  {
+    dcd_event_setup_received(tu_rhport, rport->setup_packet, true);
+  }
+
+  if ( ints & PIO_USB_INTS_ENDPOINT_COMPLETE_BITS )
+  {
+    handle_endpoint_irq(tu_rhport, XFER_RESULT_SUCCESS, &rport->ep_complete);
+  }
+
+  if ( ints & PIO_USB_INTS_ENDPOINT_STALLED_BITS )
+  {
+    handle_endpoint_irq(tu_rhport, XFER_RESULT_STALLED, &rport->ep_stalled);
+  }
+
+  if ( ints & PIO_USB_INTS_ENDPOINT_ERROR_BITS )
+  {
+    handle_endpoint_irq(tu_rhport, XFER_RESULT_FAILED, &rport->ep_error);
+  }
+
+  // clear all
+  rport->ints &= ~ints;
+}
+
+#endif
diff --git a/src/portable/raspberrypi/pio_usb/hcd_pio_usb.c b/src/portable/raspberrypi/pio_usb/hcd_pio_usb.c
new file mode 100644
index 000000000..f4de3c51d
--- /dev/null
+++ b/src/portable/raspberrypi/pio_usb/hcd_pio_usb.c
@@ -0,0 +1,209 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2021 Ha Thach (tinyusb.org)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * This file is part of the TinyUSB stack.
+ */
+
+#include "tusb_option.h"
+
+#if CFG_TUH_ENABLED && (CFG_TUSB_MCU == OPT_MCU_RP2040) && CFG_TUH_RPI_PIO_USB
+
+#include "pico.h"
+#include "pio_usb.h"
+#include "pio_usb_ll.h"
+
+//--------------------------------------------------------------------+
+// INCLUDE
+//--------------------------------------------------------------------+
+#include "osal/osal.h"
+
+#include "host/hcd.h"
+#include "host/usbh.h"
+
+#define RHPORT_OFFSET     1
+#define RHPORT_PIO(_x)    ((_x)-RHPORT_OFFSET)
+
+static pio_usb_configuration_t pio_host_cfg = PIO_USB_DEFAULT_CONFIG;
+
+//--------------------------------------------------------------------+
+// HCD API
+//--------------------------------------------------------------------+
+bool hcd_configure(uint8_t rhport, uint32_t cfg_id, const void *cfg_param) {
+  (void) rhport;
+  TU_VERIFY(cfg_id == TUH_CFGID_RPI_PIO_USB_CONFIGURATION);
+  memcpy(&pio_host_cfg, cfg_param, sizeof(pio_usb_configuration_t));
+  return true;
+}
+
+bool hcd_init(uint8_t rhport) {
+  (void) rhport;
+
+  // To run USB SOF interrupt in core1, call this init in core1
+  pio_usb_host_init(&pio_host_cfg);
+
+  return true;
+}
+
+void hcd_port_reset(uint8_t rhport) {
+  uint8_t const pio_rhport = RHPORT_PIO(rhport);
+  pio_usb_host_port_reset_start(pio_rhport);
+}
+
+void hcd_port_reset_end(uint8_t rhport) {
+  uint8_t const pio_rhport = RHPORT_PIO(rhport);
+  pio_usb_host_port_reset_end(pio_rhport);
+}
+
+bool hcd_port_connect_status(uint8_t rhport) {
+  uint8_t const pio_rhport = RHPORT_PIO(rhport);
+
+  root_port_t *root = PIO_USB_ROOT_PORT(pio_rhport);
+  port_pin_status_t line_state = pio_usb_bus_get_line_state(root);
+
+  return line_state != PORT_PIN_SE0;
+}
+
+tusb_speed_t hcd_port_speed_get(uint8_t rhport) {
+  // TODO determine link speed
+  uint8_t const pio_rhport = RHPORT_PIO(rhport);
+  return PIO_USB_ROOT_PORT(pio_rhport)->is_fullspeed ? TUSB_SPEED_FULL : TUSB_SPEED_LOW;
+}
+
+// Close all opened endpoint belong to this device
+void hcd_device_close(uint8_t rhport, uint8_t dev_addr) {
+  uint8_t const pio_rhport = RHPORT_PIO(rhport);
+  pio_usb_host_close_device(pio_rhport, dev_addr);
+}
+
+uint32_t hcd_frame_number(uint8_t rhport) {
+  (void) rhport;
+  return pio_usb_host_get_frame_number();
+}
+
+void hcd_int_enable(uint8_t rhport) {
+  (void) rhport;
+}
+
+void hcd_int_disable(uint8_t rhport) {
+  (void) rhport;
+}
+
+//--------------------------------------------------------------------+
+// Endpoint API
+//--------------------------------------------------------------------+
+
+bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const *desc_ep) {
+  hcd_devtree_info_t dev_tree;
+  hcd_devtree_get_info(dev_addr, &dev_tree);
+  bool const need_pre = (dev_tree.hub_addr && dev_tree.speed == TUSB_SPEED_LOW);
+
+  uint8_t const pio_rhport = RHPORT_PIO(rhport);
+  return pio_usb_host_endpoint_open(pio_rhport, dev_addr, (uint8_t const *) desc_ep, need_pre);
+}
+
+bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t *buffer, uint16_t buflen) {
+  uint8_t const pio_rhport = RHPORT_PIO(rhport);
+  return pio_usb_host_endpoint_transfer(pio_rhport, dev_addr, ep_addr, buffer, buflen);
+}
+
+bool hcd_edpt_abort_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr) {
+  uint8_t const pio_rhport = RHPORT_PIO(rhport);
+  return pio_usb_host_endpoint_abort_transfer(pio_rhport, dev_addr, ep_addr);
+}
+
+bool hcd_setup_send(uint8_t rhport, uint8_t dev_addr, uint8_t const setup_packet[8]) {
+  uint8_t const pio_rhport = RHPORT_PIO(rhport);
+  return pio_usb_host_send_setup(pio_rhport, dev_addr, setup_packet);
+}
+
+//bool hcd_edpt_busy(uint8_t dev_addr, uint8_t ep_addr)
+//{
+//    // EPX is shared, so multiple device addresses and endpoint addresses share that
+//    // so if any transfer is active on epx, we are busy. Interrupt endpoints have their own
+//    // EPX so ep->active will only be busy if there is a pending transfer on that interrupt endpoint
+//    // on that device
+//    pico_trace("hcd_edpt_busy dev addr %d ep_addr 0x%x\r\n", dev_addr, ep_addr);
+//    struct hw_endpoint *ep = get_dev_ep(dev_addr, ep_addr);
+//    assert(ep);
+//    bool busy = ep->active;
+//    pico_trace("busy == %d\r\n", busy);
+//    return busy;
+//}
+
+bool hcd_edpt_clear_stall(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr) {
+  (void) rhport;
+  (void) dev_addr;
+  (void) ep_addr;
+
+  return true;
+}
+
+static void __no_inline_not_in_flash_func(handle_endpoint_irq)(root_port_t *rport, xfer_result_t result,
+                                                               volatile uint32_t *ep_reg) {
+  (void) rport;
+  const uint32_t ep_all = *ep_reg;
+
+  for ( uint8_t ep_idx = 0; ep_idx < PIO_USB_EP_POOL_CNT; ep_idx++ ) {
+    uint32_t const mask = (1u << ep_idx);
+
+    if ( ep_all & mask ) {
+      endpoint_t * ep = PIO_USB_ENDPOINT(ep_idx);
+      hcd_event_xfer_complete(ep->dev_addr, ep->ep_num, ep->actual_len, result, true);
+    }
+  }
+
+  // clear all
+  (*ep_reg) &= ~ep_all;
+}
+
+// IRQ Handler
+void __no_inline_not_in_flash_func(pio_usb_host_irq_handler)(uint8_t root_id) {
+  uint8_t const tu_rhport = root_id + 1;
+  root_port_t *rport = PIO_USB_ROOT_PORT(root_id);
+  uint32_t const ints = rport->ints;
+
+  if ( ints & PIO_USB_INTS_ENDPOINT_COMPLETE_BITS ) {
+    handle_endpoint_irq(rport, XFER_RESULT_SUCCESS, &rport->ep_complete);
+  }
+
+  if ( ints & PIO_USB_INTS_ENDPOINT_STALLED_BITS ) {
+    handle_endpoint_irq(rport, XFER_RESULT_STALLED, &rport->ep_stalled);
+  }
+
+  if ( ints & PIO_USB_INTS_ENDPOINT_ERROR_BITS ) {
+    handle_endpoint_irq(rport, XFER_RESULT_FAILED, &rport->ep_error);
+  }
+
+  if ( ints & PIO_USB_INTS_CONNECT_BITS ) {
+    hcd_event_device_attach(tu_rhport, true);
+  }
+
+  if ( ints & PIO_USB_INTS_DISCONNECT_BITS ) {
+    hcd_event_device_remove(tu_rhport, true);
+  }
+
+  // clear all
+  rport->ints &= ~ints;
+}
+
+#endif
diff --git a/src/portable/raspberrypi/rp2040/dcd_rp2040.c b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
index 0048270a6..bc0deee32 100644
--- a/src/portable/raspberrypi/rp2040/dcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
@@ -26,9 +26,10 @@
 
 #include "tusb_option.h"
 
-#if TUSB_OPT_DEVICE_ENABLED && CFG_TUSB_MCU == OPT_MCU_RP2040
+#if CFG_TUD_ENABLED && (CFG_TUSB_MCU == OPT_MCU_RP2040) && !CFG_TUD_RPI_PIO_USB
 
 #include "pico.h"
+#include "hardware/sync.h"
 #include "rp2040_usb.h"
 
 #if TUD_OPT_RP2040_USB_DEVICE_ENUMERATION_FIX
@@ -37,345 +38,319 @@
 
 #include "device/dcd.h"
 
+// Current implementation force vbus detection as always present, causing device think it is always plugged into host.
+// Therefore it cannot detect disconnect event, mistaken it as suspend.
+// Note: won't work if change to 0 (for now)
+#define FORCE_VBUS_DETECT   1
+
 /*------------------------------------------------------------------*/
 /* Low level controller
  *------------------------------------------------------------------*/
 
-#define usb_hw_set hw_set_alias(usb_hw)
-#define usb_hw_clear hw_clear_alias(usb_hw)
-
 // Init these in dcd_init
-static uint8_t *next_buffer_ptr;
+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] = {0};
+static struct hw_endpoint hw_endpoints[USB_MAX_ENDPOINTS][2];
 
-static inline struct hw_endpoint *hw_endpoint_get_by_num(uint8_t num, tusb_dir_t dir)
-{
-    return &hw_endpoints[num][dir];
+// SOF may be used by remote wakeup as RESUME, this indicate whether SOF is actually used by usbd
+static bool _sof_enable = false;
+
+TU_ATTR_ALWAYS_INLINE static inline struct hw_endpoint* hw_endpoint_get_by_num(uint8_t num, tusb_dir_t dir) {
+  return &hw_endpoints[num][dir];
 }
 
-static 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);
+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)
-{
-    uint16_t size = tu_min16(64, ep->wMaxPacketSize);
+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;
 
-    // Assumes single buffered for now
-    ep->hw_data_buf = next_buffer_ptr;
-    next_buffer_ptr += size;
-    // Bits 0-5 are ignored by the controller so make sure these are 0
-    if ((uintptr_t)next_buffer_ptr & 0b111111u)
-    {
-        // Round up to the next 64
-        uint32_t fixptr = (uintptr_t)next_buffer_ptr;
-        fixptr &= ~0b111111u;
-        fixptr += 64;
-        pico_info("Rounding non 64 byte boundary buffer up from %x to %x\n", (uintptr_t)next_buffer_ptr, fixptr);
-        next_buffer_ptr = (uint8_t*)fixptr;
-    }
-    assert(((uintptr_t)next_buffer_ptr & 0b111111u) == 0);
-    uint dpram_offset = hw_data_offset(ep->hw_data_buf);
-    assert(hw_data_offset(next_buffer_ptr) <= USB_DPRAM_MAX);
+  // double buffered Bulk endpoint
+  if (transfer_type == TUSB_XFER_BULK) {
+    size *= 2u;
+  }
 
-    pico_info("Alloced %d bytes at offset 0x%x (0x%p) for ep %d %s\n",
-                size,
-                dpram_offset,
-                ep->hw_data_buf,
-                ep->num,
-                ep_dir_string[ep->in]);
+  ep->hw_data_buf = next_buffer_ptr;
+  next_buffer_ptr += size;
 
-    // Fill in endpoint control register with buffer offset
-    uint32_t reg =  EP_CTRL_ENABLE_BITS
-                  | EP_CTRL_INTERRUPT_PER_BUFFER
-                  | (ep->transfer_type << EP_CTRL_BUFFER_TYPE_LSB)
-                  | dpram_offset;
+  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);
 
-    *ep->endpoint_control = reg;
-}
+  pico_info("  Allocated %d bytes at offset 0x%x (0x%p)\r\n", size, dpram_offset, ep->hw_data_buf);
 
-static void _hw_endpoint_init(struct hw_endpoint *ep, uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t transfer_type)
-{
-    const uint8_t num = tu_edpt_number(ep_addr);
-    const tusb_dir_t dir = tu_edpt_dir(ep_addr);
-    ep->ep_addr = ep_addr;
-    // For device, IN is a tx transfer and OUT is an rx transfer
-    ep->rx = (dir == TUSB_DIR_OUT);
-    // Response to a setup packet on EP0 starts with pid of 1
-    ep->next_pid = num == 0 ? 1u : 0u;
+  // 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;
 
-    // Add some checks around the max packet size
-    if (transfer_type == TUSB_XFER_ISOCHRONOUS)
-    {
-        if (wMaxPacketSize > USB_MAX_ISO_PACKET_SIZE)
-        {
-            panic("Isochronous wMaxPacketSize %d too large", wMaxPacketSize);
-        }
-    }
-    else
-    {
-        if (wMaxPacketSize > USB_MAX_PACKET_SIZE)
-        {
-            panic("Isochronous wMaxPacketSize %d too large", wMaxPacketSize);
-        }
-    }
+  *ep->endpoint_control = reg;
+}
 
-    ep->wMaxPacketSize = wMaxPacketSize;
-    ep->transfer_type = transfer_type;
+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));
 
-    // Every endpoint has a buffer control register in dpram
-    if (dir == TUSB_DIR_IN)
-    {
-        ep->buffer_control = &usb_dpram->ep_buf_ctrl[num].in;
-    }
-    else
-    {
-        ep->buffer_control = &usb_dpram->ep_buf_ctrl[num].out;
+  // 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];
+  }
+}
 
-    // Clear existing buffer control state
-    *ep->buffer_control = 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);
+}
 
-    if (num == 0)
-    {
-        // EP0 has no endpoint control register because
-        // the buffer offsets are fixed
-        ep->endpoint_control = NULL;
+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);
 
-        // Buffer offset is fixed
-        ep->hw_data_buf = (uint8_t*)&usb_dpram->ep0_buf_a[0];
-    }
-    else
-    {
-        // Set the endpoint control register (starts at EP1, hence num-1)
-        if (dir == TUSB_DIR_IN)
-        {
-            ep->endpoint_control = &usb_dpram->ep_ctrl[num-1].in;
-        }
-        else
-        {
-            ep->endpoint_control = &usb_dpram->ep_ctrl[num-1].out;
-        }
+  const uint8_t num = tu_edpt_number(ep_addr);
+  const tusb_dir_t dir = tu_edpt_dir(ep_addr);
 
-        // Now if it hasn't already been done
-        //alloc a buffer and fill in endpoint control register
-        if(!(ep->configured))
-        {
-            _hw_endpoint_alloc(ep);
-        }
-    }
+  ep->ep_addr = ep_addr;
 
-    ep->configured = true;
-}
+  // For device, IN is a tx transfer and OUT is an rx transfer
+  ep->rx = (dir == TUSB_DIR_OUT);
 
-#if 0 // todo unused
-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));
-}
+  ep->next_pid = 0u;
+  ep->wMaxPacketSize = wMaxPacketSize;
+  ep->transfer_type = transfer_type;
 
-static void hw_endpoint_close(uint8_t ep_addr)
-{
-    struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
-    _hw_endpoint_close(ep);
-}
-#endif
+  // Every endpoint has a buffer control register in dpram
+  if (dir == TUSB_DIR_IN) {
+    ep->buffer_control = &usb_dpram->ep_buf_ctrl[num].in;
+  } else {
+    ep->buffer_control = &usb_dpram->ep_buf_ctrl[num].out;
+  }
 
-static void hw_endpoint_init(uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t bmAttributes)
-{
-    struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
-    _hw_endpoint_init(ep, ep_addr, wMaxPacketSize, bmAttributes);
-}
+  // Clear existing buffer control state
+  *ep->buffer_control = 0;
 
-static void hw_endpoint_xfer(uint8_t ep_addr, uint8_t *buffer, uint16_t total_bytes, bool start)
-{
-    struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
-    _hw_endpoint_xfer(ep, buffer, total_bytes, start);
-}
+  if (num == 0) {
+    // EP0 has no endpoint control register because the buffer offsets are fixed
+    ep->endpoint_control = NULL;
 
-static void hw_handle_buff_status(void)
-{
-    uint32_t remaining_buffers = usb_hw->buf_status;
-    pico_trace("buf_status 0x%08x\n", remaining_buffers);
-    uint bit = 1u;
-    for (uint i = 0; remaining_buffers && i < USB_MAX_ENDPOINTS * 2; i++)
-    {
-        if (remaining_buffers & bit)
-        {
-            uint __unused which = (usb_hw->buf_cpu_should_handle & bit) ? 1 : 0;
-            // Should be single buffered
-            assert(which == 0);
-            // 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));
-            // Continue xfer
-            bool done = _hw_endpoint_xfer_continue(ep);
-            if (done)
-            {
-                // Notify
-                dcd_event_xfer_complete(0, ep->ep_addr, ep->len, XFER_RESULT_SUCCESS, true);
-                hw_endpoint_reset_transfer(ep);
-            }
-            remaining_buffers &= ~bit;
-        }
-        bit <<= 1u;
+    // Buffer offset is fixed (also double buffered)
+    ep->hw_data_buf = (uint8_t*) &usb_dpram->ep0_buf_a[0];
+  } else {
+    // Set the endpoint control register (starts at EP1, hence num-1)
+    if (dir == TUSB_DIR_IN) {
+      ep->endpoint_control = &usb_dpram->ep_ctrl[num - 1].in;
+    } else {
+      ep->endpoint_control = &usb_dpram->ep_ctrl[num - 1].out;
     }
-}
 
-static 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
-    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;
-        ep->stalled  = 0;
-    }
+    // alloc a buffer and fill in endpoint control register
+    _hw_endpoint_alloc(ep, transfer_type);
+  }
 }
 
-static void ep0_0len_status(void)
-{
-    // Send 0len complete response on EP0 IN
-    reset_ep0();
-    hw_endpoint_xfer(0x80, NULL, 0, true);
+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_stall(struct hw_endpoint *ep)
-{
-    assert(!ep->stalled);
-    if (tu_edpt_number(ep->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->ep_addr) == TUSB_DIR_IN) ? USB_EP_STALL_ARM_EP0_IN_BITS : USB_EP_STALL_ARM_EP0_OUT_BITS;
+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);
+
+      // 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;
     }
-    _hw_endpoint_buffer_control_set_mask32(ep, USB_BUF_CTRL_STALL);
-    ep->stalled = true;
+    bit <<= 1u;
+  }
 }
 
-static void hw_endpoint_stall(uint8_t ep_addr)
-{
-    struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
-    _hw_endpoint_stall(ep);
-}
+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) {}
+      }
 
-static void _hw_endpoint_clear_stall(struct hw_endpoint *ep)
-{
-    if (tu_edpt_number(ep->ep_addr) == 0)
-    {
-        // Probably already been cleared but no harm
-        usb_hw_clear->ep_stall_arm = (tu_edpt_dir(ep->ep_addr) == TUSB_DIR_IN) ? USB_EP_STALL_ARM_EP0_IN_BITS : USB_EP_STALL_ARM_EP0_OUT_BITS;
+      _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;
+      }
     }
-    _hw_endpoint_buffer_control_clear_mask32(ep, USB_BUF_CTRL_STALL);
-    ep->stalled = false;
+    ep->next_pid = 1u;
+  }
 }
 
-static void hw_endpoint_clear_stall(uint8_t ep_addr)
-{
-    struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
-    _hw_endpoint_clear_stall(ep);
+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++) {
+    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));
+
+  // reclaim buffer space
+  next_buffer_ptr = &usb_dpram->epx_data[0];
 }
 
-static void dcd_rp2040_irq(void)
-{
-    uint32_t status = usb_hw->ints;
-    uint32_t handled = 0;
+static void __tusb_irq_path_func(dcd_rp2040_irq)(void) {
+  uint32_t const status = usb_hw->ints;
+  uint32_t handled = 0;
 
-    if (status & USB_INTS_SETUP_REQ_BITS)
-    {
-        handled |= USB_INTS_SETUP_REQ_BITS;
-        uint8_t const *setup = (uint8_t const *)&usb_dpram->setup_packet;
-        // Clear stall bits and reset pid
-        reset_ep0();
-        // Pass setup packet to tiny usb
-        dcd_event_setup_received(0, setup, true);
-        usb_hw_clear->sie_status = USB_SIE_STATUS_SETUP_REC_BITS;
-    }
+  if (status & USB_INTF_DEV_SOF_BITS) {
+    bool keep_sof_alive = false;
 
-    if (status & USB_INTS_BUFF_STATUS_BITS)
-    {
-        handled |= USB_INTS_BUFF_STATUS_BITS;
-        hw_handle_buff_status();
-    }
+    handled |= USB_INTF_DEV_SOF_BITS;
 
-    // SE0 for 2 us or more, usually together with Bus Reset
-    if (status & USB_INTS_DEV_CONN_DIS_BITS)
-    {
-        handled |= USB_INTS_DEV_CONN_DIS_BITS;
+#if TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
+    // Errata 15 workaround for Device Bulk-In endpoint
+    e15_last_sof = time_us_32();
 
-        if ( usb_hw->sie_status & USB_SIE_STATUS_CONNECTED_BITS )
-        {
-          // Connected: nothing to do
-        }else
-        {
-          // Disconnected
-          dcd_event_bus_signal(0, DCD_EVENT_UNPLUGGED, true);
-        }
+    for (uint8_t i = 0; i < USB_MAX_ENDPOINTS; i++) {
+      struct hw_endpoint* ep = hw_endpoint_get_by_num(i, TUSB_DIR_IN);
 
-        usb_hw_clear->sie_status = USB_SIE_STATUS_CONNECTED_BITS;
-    }
+      // Active Bulk IN endpoint requires SOF
+      if ((ep->transfer_type == TUSB_XFER_BULK) && ep->active) {
+        keep_sof_alive = true;
 
-    // SE0 for 2.5 us or more
-    if (status & USB_INTS_BUS_RESET_BITS)
-    {
-        pico_trace("BUS RESET\n");
-        usb_hw->dev_addr_ctrl = 0;
-        handled |= USB_INTS_BUS_RESET_BITS;
-        dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
-        usb_hw_clear->sie_status = USB_SIE_STATUS_BUS_RESET_BITS;
+        hw_endpoint_lock_update(ep, 1);
 
-#if TUD_OPT_RP2040_USB_DEVICE_ENUMERATION_FIX
-        // Only run enumeration walk-around if pull up is enabled
-        if ( usb_hw->sie_ctrl & USB_SIE_CTRL_PULLUP_EN_BITS )
-        {
-          rp2040_usb_device_enumeration_fix();
+        // Deferred enable?
+        if (ep->pending) {
+          ep->pending = 0;
+          hw_endpoint_start_next_buffer(ep);
         }
-#endif
+
+        hw_endpoint_lock_update(ep, -1);
+      }
     }
+#endif
 
-#if 0
-    // TODO Enable SUSPEND & RESUME interrupt and test later on with/without VBUS detection
+    // 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;
 
-    /* Note from pico datasheet 4.1.2.6.4 (v1.2)
-     * If you enable the suspend interrupt, it is likely you will see a suspend interrupt when
-     * the device is first connected but the bus is idle. The bus can be idle for a few ms before
-     * the host begins sending start of frame packets. You will also see a suspend interrupt
-     * when the device is disconnected if you do not have a VBUS detect circuit connected. This is
-     * because without VBUS detection, it is impossible to tell the difference between
-     * being disconnected and suspended.
-     */
-    if (status & USB_INTS_DEV_SUSPEND_BITS)
-    {
-        handled |= USB_INTS_DEV_SUSPEND_BITS;
-        dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
-        usb_hw_clear->sie_status = USB_SIE_STATUS_SUSPENDED_BITS;
-    }
+    dcd_event_sof(0, usb_hw->sof_rd & USB_SOF_RD_BITS, true);
+  }
+
+  // xfer events are handled before setup req. So if a transfer completes immediately
+  // before closing the EP, the events will be delivered in same order.
+  if (status & USB_INTS_BUFF_STATUS_BITS) {
+    handled |= USB_INTS_BUFF_STATUS_BITS;
+    hw_handle_buff_status();
+  }
 
-    if (status & USB_INTS_DEV_RESUME_FROM_HOST_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);
+
+    // reset pid to both 1 (data and ack)
+    reset_ep0();
+
+    // Pass setup packet to tiny usb
+    dcd_event_setup_received(0, setup, true);
+    usb_hw_clear->sie_status = USB_SIE_STATUS_SETUP_REC_BITS;
+  }
+
+#if FORCE_VBUS_DETECT == 0
+  // Since we force VBUS detect On, device will always think it is connected and
+  // couldn't distinguish between disconnect and suspend
+  if (status & USB_INTS_DEV_CONN_DIS_BITS)
+  {
+    handled |= USB_INTS_DEV_CONN_DIS_BITS;
+
+    if ( usb_hw->sie_status & USB_SIE_STATUS_CONNECTED_BITS )
+    {
+      // Connected: nothing to do
+    }else
     {
-        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;
+      // Disconnected
+      dcd_event_bus_signal(0, DCD_EVENT_UNPLUGGED, true);
     }
+
+    usb_hw_clear->sie_status = USB_SIE_STATUS_CONNECTED_BITS;
+  }
 #endif
 
-    if (status ^ handled)
-    {
-        panic("Unhandled IRQ 0x%x\n", (uint) (status ^ handled));
-    }
+  // SE0 for 2.5 us or more (will last at least 10ms)
+  if (status & USB_INTS_BUS_RESET_BITS) {
+    pico_trace("BUS RESET\r\n");
+
+    handled |= USB_INTS_BUS_RESET_BITS;
+
+    usb_hw->dev_addr_ctrl = 0;
+    reset_non_control_endpoints();
+    dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
+    usb_hw_clear->sie_status = USB_SIE_STATUS_BUS_RESET_BITS;
+
+#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();
+#endif
+  }
+
+  /* Note from pico datasheet 4.1.2.6.4 (v1.2)
+   * If you enable the suspend interrupt, it is likely you will see a suspend interrupt when
+   * the device is first connected but the bus is idle. The bus can be idle for a few ms before
+   * the host begins sending start of frame packets. You will also see a suspend interrupt
+   * when the device is disconnected if you do not have a VBUS detect circuit connected. This is
+   * because without VBUS detection, it is impossible to tell the difference between
+   * being disconnected and suspended.
+   */
+  if (status & USB_INTS_DEV_SUSPEND_BITS) {
+    handled |= USB_INTS_DEV_SUSPEND_BITS;
+    dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
+    usb_hw_clear->sie_status = USB_SIE_STATUS_SUSPENDED_BITS;
+  }
+
+  if (status & USB_INTS_DEV_RESUME_FROM_HOST_BITS) {
+    handled |= USB_INTS_DEV_RESUME_FROM_HOST_BITS;
+    dcd_event_bus_signal(0, DCD_EVENT_RESUME, true);
+    usb_hw_clear->sie_status = USB_SIE_STATUS_RESUME_BITS;
+  }
+
+  if (status ^ handled) {
+    panic("Unhandled IRQ 0x%x\n", (uint) (status ^ handled));
+  }
 }
 
 #define USB_INTS_ERROR_BITS ( \
@@ -388,144 +363,190 @@ static void dcd_rp2040_irq(void)
 /*------------------------------------------------------------------*/
 /* Controller API
  *------------------------------------------------------------------*/
-void dcd_init (uint8_t rhport)
-{
-    pico_trace("dcd_init %d\n", rhport);
-    assert(rhport == 0);
 
-    // Reset hardware to default state
-    rp2040_usb_init();
+// older SDK
+#ifndef PICO_SHARED_IRQ_HANDLER_HIGHEST_ORDER_PRIORITY
+#define PICO_SHARED_IRQ_HANDLER_HIGHEST_ORDER_PRIORITY 0xff
+#endif
 
-    irq_set_exclusive_handler(USBCTRL_IRQ, dcd_rp2040_irq);
-    memset(hw_endpoints, 0, sizeof(hw_endpoints));
-    next_buffer_ptr = &usb_dpram->epx_data[0];
+void dcd_init(uint8_t rhport) {
+  assert(rhport == 0);
+
+  TU_LOG(2, "Chip Version B%u\r\n", rp2040_chip_version());
 
-    // EP0 always exists so init it now
-    // EP0 OUT
-    hw_endpoint_init(0x0, 64, 0);
-    // EP0 IN
-    hw_endpoint_init(0x80, 64, 0);
+  // Reset hardware to default state
+  rp2040_usb_init();
 
-    // Initializes the USB peripheral for device mode and enables it.
-    // Don't need to enable the pull up here. Force VBUS
-    usb_hw->main_ctrl = USB_MAIN_CTRL_CONTROLLER_EN_BITS;
+#if FORCE_VBUS_DETECT
+  // Force VBUS detect so the device thinks it is plugged into a host
+  usb_hw->pwr = USB_USB_PWR_VBUS_DETECT_BITS | USB_USB_PWR_VBUS_DETECT_OVERRIDE_EN_BITS;
+#endif
+
+  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));
+  hw_endpoint_init(0x0, 64, TUSB_XFER_CONTROL);
+  hw_endpoint_init(0x80, 64, TUSB_XFER_CONTROL);
+
+  // Init non-control endpoints
+  reset_non_control_endpoints();
+
+  // Initializes the USB peripheral for device mode and enables it.
+  // Don't need to enable the pull up here. Force VBUS
+  usb_hw->main_ctrl = USB_MAIN_CTRL_CONTROLLER_EN_BITS;
+
+  // Enable individual controller IRQS here. Processor interrupt enable will be used
+  // 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);
+
+  dcd_connect(rhport);
+}
+
+bool dcd_deinit(uint8_t rhport) {
+  (void) rhport;
 
-    // Enable individual controller IRQS here. Processor interrupt enable will be used
-    // for the global interrupt enable...
-    // TODO Enable SUSPEND & RESUME interrupt
-    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_CONN_DIS_BITS /* | USB_INTS_DEV_SUSPEND_BITS | USB_INTS_DEV_RESUME_FROM_HOST_BITS */  ;
+  reset_non_control_endpoints();
+  irq_remove_handler(USBCTRL_IRQ, dcd_rp2040_irq);
 
-    dcd_connect(rhport);
+  // reset usb hardware into initial state
+  reset_block(RESETS_RESET_USBCTRL_BITS);
+  unreset_block_wait(RESETS_RESET_USBCTRL_BITS);
+
+  return true;
 }
 
-void dcd_int_enable(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(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 (uint8_t rhport, uint8_t dev_addr)
-{
-    pico_trace("dcd_set_address %d %d\n", rhport, dev_addr);
-    assert(rhport == 0);
+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
-    ep0_0len_status();
+  // Can't set device address in hardware until status xfer has complete
+  // Send 0len complete response on EP0 IN
+  hw_endpoint_xfer(0x80, NULL, 0);
 }
 
-void dcd_remote_wakeup(uint8_t rhport)
-{
-    pico_info("dcd_remote_wakeup %d\n", rhport);
-    assert(rhport == 0);
-    usb_hw_set->sie_ctrl = USB_SIE_CTRL_RESUME_BITS;
+void dcd_remote_wakeup(__unused uint8_t rhport) {
+  pico_info("dcd_remote_wakeup %d\n", rhport);
+  assert(rhport == 0);
+
+  // since RESUME interrupt is not triggered if we are the one initiate
+  // briefly enable SOF to notify usbd when bus is ready
+  usb_hw_set->inte = USB_INTS_DEV_SOF_BITS;
+  usb_hw_set->sie_ctrl = USB_SIE_CTRL_RESUME_BITS;
 }
 
 // disconnect by disabling internal pull-up resistor on D+/D-
-void dcd_disconnect(uint8_t rhport)
-{
-    pico_info("dcd_disconnect %d\n", rhport);
-    assert(rhport == 0);
-    usb_hw_clear->sie_ctrl = USB_SIE_CTRL_PULLUP_EN_BITS;
+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(uint8_t rhport)
-{
-    pico_info("dcd_connect %d\n", rhport);
-    assert(rhport == 0);
-    usb_hw_set->sie_ctrl = USB_SIE_CTRL_PULLUP_EN_BITS;
+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) rhport;
+
+  _sof_enable = en;
+
+  if (en) {
+    usb_hw_set->inte = USB_INTS_DEV_SOF_BITS;
+  }
+#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.
+    usb_hw_clear->inte = USB_INTS_DEV_SOF_BITS;
+  }
+#endif
 }
 
 /*------------------------------------------------------------------*/
 /* DCD Endpoint port
  *------------------------------------------------------------------*/
 
-void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const * request)
-{
-    pico_trace("dcd_edpt0_status_complete %d\n", rhport);
-    assert(rhport == 0);
+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)
-    {
-        pico_trace("Set HW address %d\n", assigned_address);
-        usb_hw->dev_addr_ctrl = (uint8_t) request->wValue;
-    }
-
-    reset_ep0();
+  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 (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
-{
-    pico_info("dcd_edpt_open %d %02x\n", rhport, desc_edpt->bEndpointAddress);
-    assert(rhport == 0);
-    hw_endpoint_init(desc_edpt->bEndpointAddress, desc_edpt->wMaxPacketSize.size, 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;
 }
 
-bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
-{
-    assert(rhport == 0);
-    // True means start new xfer
-    hw_endpoint_xfer(ep_addr, buffer, total_bytes, true);
-    return true;
+void dcd_edpt_close_all(uint8_t rhport) {
+  (void) rhport;
+
+  // may need to use EP Abort
+  reset_non_control_endpoints();
 }
 
-void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
-{
-    pico_trace("dcd_edpt_stall %d %02x\n", rhport, ep_addr);
-    assert(rhport == 0);
-    hw_endpoint_stall(ep_addr);
+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_clear_stall (uint8_t rhport, uint8_t ep_addr)
-{
-    pico_trace("dcd_edpt_clear_stall %d %02x\n", rhport, ep_addr);
-    assert(rhport == 0);
-    hw_endpoint_clear_stall(ep_addr);
+void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr) {
+  (void) rhport;
+
+  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;
+  }
+
+  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) rhport;
 
-void dcd_edpt_close (uint8_t rhport, uint8_t ep_addr)
-{
-    // usbd.c says: In progress transfers on this EP may be delivered after this call
-    pico_trace("dcd_edpt_close %d %02x\n", rhport, 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;
+    _hw_endpoint_buffer_control_clear_mask32(ep, USB_BUF_CTRL_STALL);
+  }
+}
 
+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_int_handler(uint8_t rhport)
-{
-    (void) rhport;
-    dcd_rp2040_irq();
+void __tusb_irq_path_func(dcd_int_handler)(uint8_t rhport) {
+  (void) rhport;
+  dcd_rp2040_irq();
 }
 
 #endif
diff --git a/src/portable/raspberrypi/rp2040/hcd_rp2040.c b/src/portable/raspberrypi/rp2040/hcd_rp2040.c
index 3c62b5732..222dbbbf0 100644
--- a/src/portable/raspberrypi/rp2040/hcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/hcd_rp2040.c
@@ -2,6 +2,7 @@
  * The MIT License (MIT)
  *
  * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ * Copyright (c) 2021 Ha Thach (tinyusb.org) for Double Buffered
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -26,7 +27,7 @@
 
 #include "tusb_option.h"
 
-#if TUSB_OPT_HOST_ENABLED && CFG_TUSB_MCU == OPT_MCU_RP2040
+#if CFG_TUH_ENABLED && (CFG_TUSB_MCU == OPT_MCU_RP2040) && !CFG_TUH_RPI_PIO_USB && !CFG_TUH_MAX3421
 
 #include "pico.h"
 #include "rp2040_usb.h"
@@ -38,9 +39,9 @@
 
 #include "host/hcd.h"
 #include "host/usbh.h"
-#include "host/usbh_hcd.h"
 
-#define ROOT_PORT 0
+// port 0 is native USB port, other is counted as software PIO
+#define RHPORT_NATIVE 0
 
 //--------------------------------------------------------------------+
 // Low level rp2040 controller functions
@@ -52,315 +53,323 @@
 static_assert(PICO_USB_HOST_INTERRUPT_ENDPOINTS <= USB_MAX_ENDPOINTS, "");
 
 // Host mode uses one shared endpoint register for non-interrupt endpoint
-struct hw_endpoint eps[1 + PICO_USB_HOST_INTERRUPT_ENDPOINTS];
-#define epx (eps[0])
-
-#define usb_hw_set hw_set_alias(usb_hw)
-#define usb_hw_clear hw_clear_alias(usb_hw)
-
-// Used for hcd pipe busy.
-// todo still a bit wasteful
-// top bit set if valid
-uint8_t dev_ep_map[CFG_TUSB_HOST_DEVICE_MAX][1 + PICO_USB_HOST_INTERRUPT_ENDPOINTS][2];
+static struct hw_endpoint ep_pool[1 + PICO_USB_HOST_INTERRUPT_ENDPOINTS];
+#define epx (ep_pool[0])
 
 // Flags we set by default in sie_ctrl (we add other bits on top)
-static uint32_t sie_ctrl_base = USB_SIE_CTRL_SOF_EN_BITS | 
-                                USB_SIE_CTRL_KEEP_ALIVE_EN_BITS | 
-                                USB_SIE_CTRL_PULLDOWN_EN_BITS | 
-                                USB_SIE_CTRL_EP0_INT_1BUF_BITS;
+enum {
+  SIE_CTRL_BASE = USB_SIE_CTRL_SOF_EN_BITS      | USB_SIE_CTRL_KEEP_ALIVE_EN_BITS |
+                  USB_SIE_CTRL_PULLDOWN_EN_BITS | USB_SIE_CTRL_EP0_INT_1BUF_BITS
+};
 
 static struct hw_endpoint *get_dev_ep(uint8_t dev_addr, uint8_t ep_addr)
 {
-    uint8_t num = tu_edpt_number(ep_addr);
-    if (num == 0) {
-        return &epx;
-    }
-    uint8_t in = (ep_addr & TUSB_DIR_IN_MASK) ? 1 : 0;
-    uint mapping = dev_ep_map[dev_addr-1][num][in];
-    pico_trace("Get dev addr %d ep %d = %d\n", dev_addr, ep_addr, mapping);
-    return mapping >= 128 ? eps + (mapping & 0x7fu) : NULL;
-}
+  uint8_t num = tu_edpt_number(ep_addr);
+  if ( num == 0 ) return &epx;
 
-static void set_dev_ep(uint8_t dev_addr, uint8_t ep_addr, struct hw_endpoint *ep)
-{
-    uint8_t num = tu_edpt_number(ep_addr);
-    uint8_t in = (ep_addr & TUSB_DIR_IN_MASK) ? 1 : 0;
-    uint32_t index = ep - eps;
-    hard_assert(index < TU_ARRAY_SIZE(eps));
-    // todo revisit why dev_addr can be 0 here
-    if (dev_addr) {
-        dev_ep_map[dev_addr-1][num][in] = 128u | index;
-    }
-    pico_trace("Set dev addr %d ep %d = %d\n", dev_addr, ep_addr, index);
+  for ( uint32_t i = 1; i < TU_ARRAY_SIZE(ep_pool); i++ )
+  {
+    struct hw_endpoint *ep = &ep_pool[i];
+    if ( ep->configured && (ep->dev_addr == dev_addr) && (ep->ep_addr == ep_addr) ) return ep;
+  }
+
+  return NULL;
 }
 
-static inline uint8_t dev_speed(void)
+TU_ATTR_ALWAYS_INLINE static inline uint8_t dev_speed(void)
 {
-    return (usb_hw->sie_status & USB_SIE_STATUS_SPEED_BITS) >> USB_SIE_STATUS_SPEED_LSB;
+  return (usb_hw->sie_status & USB_SIE_STATUS_SPEED_BITS) >> USB_SIE_STATUS_SPEED_LSB;
 }
 
-static bool need_pre(uint8_t dev_addr)
+TU_ATTR_ALWAYS_INLINE static inline bool need_pre(uint8_t dev_addr)
 {
-    // If this device is different to the speed of the root device
-    // (i.e. is a low speed device on a full speed hub) then need pre
-    return hcd_port_speed_get(0) != tuh_device_get_speed(dev_addr);
+  // If this device is different to the speed of the root device
+  // (i.e. is a low speed device on a full speed hub) then need pre
+  return hcd_port_speed_get(0) != tuh_speed_get(dev_addr);
 }
 
-static void hw_xfer_complete(struct hw_endpoint *ep, xfer_result_t xfer_result)
+static void __tusb_irq_path_func(hw_xfer_complete)(struct hw_endpoint *ep, xfer_result_t xfer_result)
 {
-    // Mark transfer as done before we tell the tinyusb stack
-    uint8_t dev_addr = ep->dev_addr;
-    uint8_t ep_addr = ep->ep_addr;
-    uint total_len = ep->total_len;
-    hw_endpoint_reset_transfer(ep);
-    hcd_event_xfer_complete(dev_addr, ep_addr, total_len, xfer_result, true);
+  // Mark transfer as done before we tell the tinyusb stack
+  uint8_t dev_addr = ep->dev_addr;
+  uint8_t ep_addr = ep->ep_addr;
+  uint xferred_len = ep->xferred_len;
+  hw_endpoint_reset_transfer(ep);
+  hcd_event_xfer_complete(dev_addr, ep_addr, xferred_len, xfer_result, true);
 }
 
-static void _handle_buff_status_bit(uint bit, struct hw_endpoint *ep)
+static void __tusb_irq_path_func(_handle_buff_status_bit)(uint bit, struct hw_endpoint *ep)
 {
-    usb_hw_clear->buf_status = bit;
-    bool done = _hw_endpoint_xfer_continue(ep);
-    if (done)
-    {
-        hw_xfer_complete(ep, XFER_RESULT_SUCCESS);
-    }
+  usb_hw_clear->buf_status = bit;
+  // EP may have been stalled?
+  assert(ep->active);
+  bool done = hw_endpoint_xfer_continue(ep);
+  if ( done )
+  {
+    hw_xfer_complete(ep, XFER_RESULT_SUCCESS);
+  }
 }
 
-static void hw_handle_buff_status(void)
+static void __tusb_irq_path_func(hw_handle_buff_status)(void)
 {
-    uint32_t remaining_buffers = usb_hw->buf_status;
-    pico_trace("buf_status 0x%08x\n", remaining_buffers);
+  uint32_t remaining_buffers = usb_hw->buf_status;
+  pico_trace("buf_status 0x%08lx\n", remaining_buffers);
+
+  // Check EPX first
+  uint bit = 0b1;
+  if ( remaining_buffers & bit )
+  {
+    remaining_buffers &= ~bit;
+    struct hw_endpoint * ep = &epx;
 
-    // Check EPX first
-    uint bit = 0b1;
-    if (remaining_buffers & bit)
+    uint32_t ep_ctrl = *ep->endpoint_control;
+    if ( ep_ctrl & EP_CTRL_DOUBLE_BUFFERED_BITS )
     {
-        remaining_buffers &= ~bit;
-        struct hw_endpoint *ep = &epx;
-        _handle_buff_status_bit(bit, ep);
+      TU_LOG(3, "Double Buffered: ");
     }
-
-    // Check interrupt endpoints
-    for (uint i = 1; i <= USB_HOST_INTERRUPT_ENDPOINTS && remaining_buffers; i++)
+    else
     {
-        // EPX is bit 0
-        // IEP1 is bit 2
-        // IEP2 is bit 4
-        // IEP3 is bit 6
-        // etc
-        bit = 1 << (i*2);
-
-        if (remaining_buffers & bit)
-        {
-            remaining_buffers &= ~bit;
-            _handle_buff_status_bit(bit, &eps[i]);
-        }
+      TU_LOG(3, "Single Buffered: ");
     }
+    TU_LOG_HEX(3, ep_ctrl);
+
+    _handle_buff_status_bit(bit, ep);
+  }
 
-    if (remaining_buffers)
+  // Check "interrupt" (asynchronous) endpoints for both IN and OUT
+  for ( uint i = 1; i <= USB_HOST_INTERRUPT_ENDPOINTS && remaining_buffers; i++ )
+  {
+    // EPX is bit 0 & 1
+    // IEP1 IN  is bit 2
+    // IEP1 OUT is bit 3
+    // IEP2 IN  is bit 4
+    // IEP2 OUT is bit 5
+    // IEP3 IN  is bit 6
+    // IEP3 OUT is bit 7
+    // etc
+    for ( uint j = 0; j < 2; j++ )
     {
-        panic("Unhandled buffer %d\n", remaining_buffers);
+      bit = 1 << (i * 2 + j);
+      if ( remaining_buffers & bit )
+      {
+        remaining_buffers &= ~bit;
+        _handle_buff_status_bit(bit, &ep_pool[i]);
+      }
     }
+  }
+
+  if ( remaining_buffers )
+  {
+    panic("Unhandled buffer %d\n", remaining_buffers);
+  }
 }
 
-static void hw_trans_complete(void)
+static void __tusb_irq_path_func(hw_trans_complete)(void)
 {
+  if (usb_hw->sie_ctrl & USB_SIE_CTRL_SEND_SETUP_BITS)
+  {
+    pico_trace("Sent setup packet\n");
     struct hw_endpoint *ep = &epx;
     assert(ep->active);
+    // Set transferred length to 8 for a setup packet
+    ep->xferred_len = 8;
+    hw_xfer_complete(ep, XFER_RESULT_SUCCESS);
+  }
+  else
+  {
+    // Don't care. Will handle this in buff status
+    return;
+  }
+}
 
-    if (ep->sent_setup)
+static void __tusb_irq_path_func(hcd_rp2040_irq)(void)
+{
+  uint32_t status = usb_hw->ints;
+  uint32_t handled = 0;
+
+  if ( status & USB_INTS_HOST_CONN_DIS_BITS )
+  {
+    handled |= USB_INTS_HOST_CONN_DIS_BITS;
+
+    if ( dev_speed() )
     {
-        pico_trace("Sent setup packet\n");
-        hw_xfer_complete(ep, XFER_RESULT_SUCCESS);
+      hcd_event_device_attach(RHPORT_NATIVE, true);
     }
     else
     {
-        // Don't care. Will handle this in buff status
-        return;
+      hcd_event_device_remove(RHPORT_NATIVE, true);
     }
-}
 
-static void hcd_rp2040_irq(void)
-{
-    uint32_t status = usb_hw->ints;
-    uint32_t handled = 0;
+    // Clear speed change interrupt
+    usb_hw_clear->sie_status = USB_SIE_STATUS_SPEED_BITS;
+  }
 
-    if (status & USB_INTS_HOST_CONN_DIS_BITS)
-    {
-        handled |= USB_INTS_HOST_CONN_DIS_BITS;
-        
-        if (dev_speed())
-        {
-            hcd_event_device_attach(ROOT_PORT, true);
-        }
-        else
-        {
-            hcd_event_device_remove(ROOT_PORT, true);
-        }
+  if ( status & USB_INTS_STALL_BITS )
+  {
+    // We have rx'd a stall from the device
+    // NOTE THIS SHOULD HAVE PRIORITY OVER BUFF_STATUS
+    // AND TRANS_COMPLETE as the stall is an alternative response
+    // to one of those events
+    pico_trace("Stall REC\n");
+    handled |= USB_INTS_STALL_BITS;
+    usb_hw_clear->sie_status = USB_SIE_STATUS_STALL_REC_BITS;
+    hw_xfer_complete(&epx, XFER_RESULT_STALLED);
+  }
 
-        // Clear speed change interrupt
-        usb_hw_clear->sie_status = USB_SIE_STATUS_SPEED_BITS;
-    }
+  if ( status & USB_INTS_BUFF_STATUS_BITS )
+  {
+    handled |= USB_INTS_BUFF_STATUS_BITS;
+    TU_LOG(2, "Buffer complete\r\n");
+    hw_handle_buff_status();
+  }
 
-    if (status & USB_INTS_TRANS_COMPLETE_BITS)
-    {
-        handled |= USB_INTS_TRANS_COMPLETE_BITS;
-        usb_hw_clear->sie_status = USB_SIE_STATUS_TRANS_COMPLETE_BITS;
-        hw_trans_complete();
-    }
-
-    if (status & USB_INTS_BUFF_STATUS_BITS)
-    {
-        handled |= USB_INTS_BUFF_STATUS_BITS;
-        // print_bufctrl32(*epx.buffer_control);
-        hw_handle_buff_status();
-    }
+  if ( status & USB_INTS_TRANS_COMPLETE_BITS )
+  {
+    handled |= USB_INTS_TRANS_COMPLETE_BITS;
+    usb_hw_clear->sie_status = USB_SIE_STATUS_TRANS_COMPLETE_BITS;
+    TU_LOG(2, "Transfer complete\r\n");
+    hw_trans_complete();
+  }
 
-    if (status & USB_INTS_STALL_BITS)
-    {
-        // We have rx'd a stall from the device
-        pico_trace("Stall REC\n");
-        handled |= USB_INTS_STALL_BITS;
-        usb_hw_clear->sie_status = USB_SIE_STATUS_STALL_REC_BITS;
-        hw_xfer_complete(&epx, XFER_RESULT_STALLED);
-    }
+  if ( status & USB_INTS_ERROR_RX_TIMEOUT_BITS )
+  {
+    handled |= USB_INTS_ERROR_RX_TIMEOUT_BITS;
+    usb_hw_clear->sie_status = USB_SIE_STATUS_RX_TIMEOUT_BITS;
+  }
 
-    if (status & USB_INTS_ERROR_RX_TIMEOUT_BITS)
-    {
-        handled |= USB_INTS_ERROR_RX_TIMEOUT_BITS;
-        usb_hw_clear->sie_status = USB_SIE_STATUS_RX_TIMEOUT_BITS;
-    }
+  if ( status & USB_INTS_ERROR_DATA_SEQ_BITS )
+  {
+    usb_hw_clear->sie_status = USB_SIE_STATUS_DATA_SEQ_ERROR_BITS;
+    TU_LOG(3, "  Seq Error: [0] = 0x%04u  [1] = 0x%04x\r\n",
+           tu_u32_low16(*epx.buffer_control),
+           tu_u32_high16(*epx.buffer_control));
+    panic("Data Seq Error \n");
+  }
 
-    if (status & USB_INTS_ERROR_DATA_SEQ_BITS)
-    {
-        usb_hw_clear->sie_status = USB_SIE_STATUS_DATA_SEQ_ERROR_BITS;
-        // print_bufctrl32(*epx.buffer_control);
-        panic("Data Seq Error \n");
-    }
+  if ( status ^ handled )
+  {
+    panic("Unhandled IRQ 0x%x\n", (uint) (status ^ handled));
+  }
+}
 
-    if (status ^ handled)
-    {
-        panic("Unhandled IRQ 0x%x\n", (uint) (status ^ handled));
-    }
+void __tusb_irq_path_func(hcd_int_handler)(uint8_t rhport, bool in_isr) {
+  (void) rhport;
+  (void) in_isr;
+  hcd_rp2040_irq();
 }
 
 static struct hw_endpoint *_next_free_interrupt_ep(void)
 {
-    struct hw_endpoint *ep = NULL;
-    for (uint i = 1; i < TU_ARRAY_SIZE(eps); i++)
+  struct hw_endpoint * ep = NULL;
+  for ( uint i = 1; i < TU_ARRAY_SIZE(ep_pool); i++ )
+  {
+    ep = &ep_pool[i];
+    if ( !ep->configured )
     {
-        ep = &eps[i];
-        if (!ep->configured)
-        {
-            // Will be configured by _hw_endpoint_init / _hw_endpoint_allocate
-            ep->interrupt_num = i - 1;
-            return ep;
-        }
+      // Will be configured by _hw_endpoint_init / _hw_endpoint_allocate
+      ep->interrupt_num = (uint8_t) (i - 1);
+      return ep;
     }
-    return ep;
+  }
+  return ep;
 }
 
 static struct hw_endpoint *_hw_endpoint_allocate(uint8_t transfer_type)
 {
-    struct hw_endpoint *ep = NULL;
-    if (transfer_type == TUSB_XFER_INTERRUPT)
-    {
-        ep = _next_free_interrupt_ep();
-        pico_info("Allocate interrupt ep %d\n", ep->interrupt_num);
-        assert(ep);
-        ep->buffer_control = &usbh_dpram->int_ep_buffer_ctrl[ep->interrupt_num].ctrl;
-        ep->endpoint_control = &usbh_dpram->int_ep_ctrl[ep->interrupt_num].ctrl;
-        // 0x180 for epx
-        // 0x1c0 for intep0
-        // 0x200 for intep1
-        // etc
-        ep->hw_data_buf = &usbh_dpram->epx_data[64 * (ep->interrupt_num + 1)];
-    }
-    else
-    {
-        ep = &epx;
-        ep->buffer_control = &usbh_dpram->epx_buf_ctrl;
-        ep->endpoint_control = &usbh_dpram->epx_ctrl;
-        ep->hw_data_buf = &usbh_dpram->epx_data[0];
-    }
-    return ep;
-}
+  struct hw_endpoint * ep = NULL;
 
-static void _hw_endpoint_init(struct hw_endpoint *ep, uint8_t dev_addr, uint8_t ep_addr, uint wMaxPacketSize, uint8_t transfer_type, uint8_t bmInterval)
-{
-    // Already has data buffer, endpoint control, and buffer control allocated at this point
-    assert(ep->endpoint_control);
-    assert(ep->buffer_control);
-    assert(ep->hw_data_buf);
+  if ( transfer_type != TUSB_XFER_CONTROL )
+  {
+    // Note: even though datasheet name these "Interrupt" endpoints. These are actually
+    // "Asynchronous" endpoints and can be used for other type such as: Bulk  (ISO need confirmation)
+    ep = _next_free_interrupt_ep();
+    pico_info("Allocate %s ep %d\n", tu_edpt_type_str(transfer_type), ep->interrupt_num);
+    assert(ep);
+    ep->buffer_control = &usbh_dpram->int_ep_buffer_ctrl[ep->interrupt_num].ctrl;
+    ep->endpoint_control = &usbh_dpram->int_ep_ctrl[ep->interrupt_num].ctrl;
+    // 0 for epx (double buffered): TODO increase to 1024 for ISO
+    // 2x64 for intep0
+    // 3x64 for intep1
+    // etc
+    ep->hw_data_buf = &usbh_dpram->epx_data[64 * (ep->interrupt_num + 2)];
+  }
+  else
+  {
+    ep = &epx;
+    ep->buffer_control = &usbh_dpram->epx_buf_ctrl;
+    ep->endpoint_control = &usbh_dpram->epx_ctrl;
+    ep->hw_data_buf = &usbh_dpram->epx_data[0];
+  }
 
-    uint8_t const num = tu_edpt_number(ep_addr);
-    tusb_dir_t const dir = tu_edpt_dir(ep_addr);
+  return ep;
+}
 
-    ep->ep_addr = ep_addr;
-    ep->dev_addr = dev_addr;
+static void _hw_endpoint_init(struct hw_endpoint *ep, uint8_t dev_addr, uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t transfer_type, uint8_t bmInterval)
+{
+  // Already has data buffer, endpoint control, and buffer control allocated at this point
+  assert(ep->endpoint_control);
+  assert(ep->buffer_control);
+  assert(ep->hw_data_buf);
 
-    // For host, IN to host == RX, anything else rx == false
-    ep->rx = (dir == TUSB_DIR_IN);
+  uint8_t const num = tu_edpt_number(ep_addr);
+  tusb_dir_t const dir = tu_edpt_dir(ep_addr);
 
-    // Response to a setup packet on EP0 starts with pid of 1
-    ep->next_pid = num == 0 ? 1u : 0u;
-    ep->wMaxPacketSize = wMaxPacketSize;
-    ep->transfer_type = transfer_type;
+  ep->ep_addr = ep_addr;
+  ep->dev_addr = dev_addr;
 
-    pico_trace("hw_endpoint_init dev %d ep %d %s xfer %d\n", ep->dev_addr, tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)], ep->transfer_type);
-    pico_trace("dev %d ep %d %s setup buffer @ 0x%p\n", ep->dev_addr, tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)], ep->hw_data_buf);
-    uint dpram_offset = hw_data_offset(ep->hw_data_buf);
-    // Bits 0-5 should be 0
-    assert(!(dpram_offset & 0b111111));
+  // For host, IN to host == RX, anything else rx == false
+  ep->rx = (dir == TUSB_DIR_IN);
 
-    // Fill in endpoint control register with buffer offset
-    uint32_t ep_reg =  EP_CTRL_ENABLE_BITS
-                  | EP_CTRL_INTERRUPT_PER_BUFFER
-                  | (ep->transfer_type << EP_CTRL_BUFFER_TYPE_LSB)
-                  | dpram_offset;
-    ep_reg |= bmInterval ? (bmInterval - 1) << EP_CTRL_HOST_INTERRUPT_INTERVAL_LSB : 0;
-    *ep->endpoint_control = ep_reg;
-    pico_trace("endpoint control (0x%p) <- 0x%x\n", ep->endpoint_control, ep_reg);
-    ep->configured = true;
+  // Response to a setup packet on EP0 starts with pid of 1
+  ep->next_pid = (num == 0 ? 1u : 0u);
+  ep->wMaxPacketSize = wMaxPacketSize;
+  ep->transfer_type = transfer_type;
 
-    if (bmInterval)
-    {
-        // This is an interrupt endpoint
-        // so need to set up interrupt endpoint address control register with:
-        // device address
-        // endpoint number / direction
-        // preamble
-        uint32_t reg = dev_addr | (num << USB_ADDR_ENDP1_ENDPOINT_LSB);
-        // Assert the interrupt endpoint is IN_TO_HOST
-        assert(dir == TUSB_DIR_IN);
+  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));
 
-        if (need_pre(dev_addr))
-        {
-            reg |= USB_ADDR_ENDP1_INTEP_PREAMBLE_BITS;
-        }
-        usb_hw->int_ep_addr_ctrl[ep->interrupt_num] = reg;
+  // Fill in endpoint control register with buffer offset
+  uint32_t ep_reg = EP_CTRL_ENABLE_BITS
+                    | EP_CTRL_INTERRUPT_PER_BUFFER
+                    | (ep->transfer_type << EP_CTRL_BUFFER_TYPE_LSB)
+                    | dpram_offset;
+  if ( bmInterval )
+  {
+    ep_reg |= (uint32_t) ((bmInterval - 1) << EP_CTRL_HOST_INTERRUPT_INTERVAL_LSB);
+  }
+  *ep->endpoint_control = ep_reg;
+  pico_trace("endpoint control (0x%p) <- 0x%lx\n", ep->endpoint_control, ep_reg);
+  ep->configured = true;
 
-        // Finally, enable interrupt that endpoint
-        usb_hw_set->int_ep_ctrl = 1 << (ep->interrupt_num + 1);
+  if ( ep != &epx )
+  {
+    // Endpoint has its own addr_endp and interrupt bits to be setup!
+    // This is an interrupt/async endpoint. so need to set up ADDR_ENDP register with:
+    // - device address
+    // - endpoint number / direction
+    // - preamble
+    uint32_t reg = (uint32_t) (dev_addr | (num << USB_ADDR_ENDP1_ENDPOINT_LSB));
 
-        // If it's an interrupt endpoint we need to set up the buffer control
-        // register
+    if ( dir == TUSB_DIR_OUT )
+    {
+      reg |= USB_ADDR_ENDP1_INTEP_DIR_BITS;
+    }
 
+    if ( need_pre(dev_addr) )
+    {
+      reg |= USB_ADDR_ENDP1_INTEP_PREAMBLE_BITS;
     }
-}
+    usb_hw->int_ep_addr_ctrl[ep->interrupt_num] = reg;
 
-static void hw_endpoint_init(uint8_t dev_addr, const tusb_desc_endpoint_t *ep_desc)
-{
-    // Allocated differently based on if it's an interrupt endpoint or not
-    struct hw_endpoint *ep = _hw_endpoint_allocate(ep_desc->bmAttributes.xfer);
-    _hw_endpoint_init(ep,
-        dev_addr,
-        ep_desc->bEndpointAddress,
-        ep_desc->wMaxPacketSize.size,
-        ep_desc->bmAttributes.xfer,
-        ep_desc->bInterval);
-    // Map this struct to ep@device address
-    set_dev_ep(dev_addr, ep_desc->bEndpointAddress, ep);
+    // Finally, enable interrupt that endpoint
+    usb_hw_set->int_ep_ctrl = 1 << (ep->interrupt_num + 1);
+
+    // If it's an interrupt endpoint we need to set up the buffer control
+    // register
+  }
 }
 
 //--------------------------------------------------------------------+
@@ -368,177 +377,269 @@ static void hw_endpoint_init(uint8_t dev_addr, const tusb_desc_endpoint_t *ep_de
 //--------------------------------------------------------------------+
 bool hcd_init(uint8_t rhport)
 {
-    pico_trace("hcd_init %d\n", rhport);
-    assert(rhport == 0);
+  (void) rhport;
+  pico_trace("hcd_init %d\n", rhport);
+  assert(rhport == 0);
+
+  // Reset any previous state
+  rp2040_usb_init();
+
+  // Force VBUS detect to always present, for now we assume vbus is always provided (without using VBUS En)
+  usb_hw->pwr = USB_USB_PWR_VBUS_DETECT_BITS | USB_USB_PWR_VBUS_DETECT_OVERRIDE_EN_BITS;
 
-    // Reset any previous state
-    rp2040_usb_init();
+  // Remove shared irq if it was previously added so as not to fill up shared irq slots
+  irq_remove_handler(USBCTRL_IRQ, hcd_rp2040_irq);
 
-    irq_set_exclusive_handler(USBCTRL_IRQ, hcd_rp2040_irq);
+  irq_add_shared_handler(USBCTRL_IRQ, hcd_rp2040_irq, PICO_SHARED_IRQ_HANDLER_HIGHEST_ORDER_PRIORITY);
 
-    // clear epx and interrupt eps
-    memset(&eps, 0, sizeof(eps));
+  // clear epx and interrupt eps
+  memset(&ep_pool, 0, sizeof(ep_pool));
 
-    // Enable in host mode with SOF / Keep alive on
-    usb_hw->main_ctrl = USB_MAIN_CTRL_CONTROLLER_EN_BITS | USB_MAIN_CTRL_HOST_NDEVICE_BITS;
-    usb_hw->sie_ctrl = sie_ctrl_base;
-    usb_hw->inte = USB_INTE_BUFF_STATUS_BITS      | 
-                   USB_INTE_HOST_CONN_DIS_BITS    | 
-                   USB_INTE_HOST_RESUME_BITS      | 
-                   USB_INTE_STALL_BITS            | 
-                   USB_INTE_TRANS_COMPLETE_BITS   |
-                   USB_INTE_ERROR_RX_TIMEOUT_BITS |
-                   USB_INTE_ERROR_DATA_SEQ_BITS   ;
+  // Enable in host mode with SOF / Keep alive on
+  usb_hw->main_ctrl = USB_MAIN_CTRL_CONTROLLER_EN_BITS | USB_MAIN_CTRL_HOST_NDEVICE_BITS;
+  usb_hw->sie_ctrl = SIE_CTRL_BASE;
+  usb_hw->inte = USB_INTE_BUFF_STATUS_BITS      |
+                 USB_INTE_HOST_CONN_DIS_BITS    |
+                 USB_INTE_HOST_RESUME_BITS      |
+                 USB_INTE_STALL_BITS            |
+                 USB_INTE_TRANS_COMPLETE_BITS   |
+                 USB_INTE_ERROR_RX_TIMEOUT_BITS |
+                 USB_INTE_ERROR_DATA_SEQ_BITS   ;
 
-    return true;
+  return true;
+}
+
+bool hcd_deinit(uint8_t rhport) {
+  (void) rhport;
+
+  irq_remove_handler(USBCTRL_IRQ, hcd_rp2040_irq);
+  reset_block(RESETS_RESET_USBCTRL_BITS);
+  unreset_block_wait(RESETS_RESET_USBCTRL_BITS);
+
+  return true;
 }
 
 void hcd_port_reset(uint8_t rhport)
 {
-    pico_trace("hcd_port_reset\n");
-    assert(rhport == 0);
-    // TODO: Nothing to do here yet. Perhaps need to reset some state?
+  (void) rhport;
+  pico_trace("hcd_port_reset\n");
+  assert(rhport == 0);
+  // TODO: Nothing to do here yet. Perhaps need to reset some state?
+}
+
+void hcd_port_reset_end(uint8_t rhport)
+{
+  (void) rhport;
 }
 
 bool hcd_port_connect_status(uint8_t rhport)
 {
-    pico_trace("hcd_port_connect_status\n");
-    assert(rhport == 0);
-    return usb_hw->sie_status & USB_SIE_STATUS_SPEED_BITS;
+  (void) rhport;
+  pico_trace("hcd_port_connect_status\n");
+  assert(rhport == 0);
+  return usb_hw->sie_status & USB_SIE_STATUS_SPEED_BITS;
 }
 
 tusb_speed_t hcd_port_speed_get(uint8_t rhport)
 {
-    pico_trace("hcd_port_speed_get\n");
-    assert(rhport == 0);
-    // TODO: Should enumval this register
-    switch (dev_speed())
-    {
-        case 1:
-            return TUSB_SPEED_LOW;
-        case 2:
-            return TUSB_SPEED_FULL;
-        default:
-            panic("Invalid speed\n");
-    }
+  (void) rhport;
+  assert(rhport == 0);
+
+  // TODO: Should enumval this register
+  switch ( dev_speed() )
+  {
+    case 1:
+      return TUSB_SPEED_LOW;
+    case 2:
+      return TUSB_SPEED_FULL;
+    default:
+      panic("Invalid speed\n");
+      // return TUSB_SPEED_INVALID;
+  }
 }
 
 // Close all opened endpoint belong to this device
 void hcd_device_close(uint8_t rhport, uint8_t dev_addr)
 {
-    pico_trace("hcd_device_close %d\n", dev_addr);
+  pico_trace("hcd_device_close %d\n", dev_addr);
+  (void) rhport;
+
+  if (dev_addr == 0) return;
+
+  for (size_t i = 1; i < TU_ARRAY_SIZE(ep_pool); i++)
+  {
+    hw_endpoint_t* ep = &ep_pool[i];
+
+    if (ep->dev_addr == dev_addr && ep->configured)
+    {
+      // in case it is an interrupt endpoint, disable it
+      usb_hw_clear->int_ep_ctrl = (1 << (ep->interrupt_num + 1));
+      usb_hw->int_ep_addr_ctrl[ep->interrupt_num] = 0;
+
+      // unconfigure the endpoint
+      ep->configured = false;
+      *ep->endpoint_control = 0;
+      *ep->buffer_control = 0;
+      hw_endpoint_reset_transfer(ep);
+    }
+  }
+}
+
+uint32_t hcd_frame_number(uint8_t rhport)
+{
+  (void) rhport;
+  return usb_hw->sof_rd;
 }
 
 void hcd_int_enable(uint8_t rhport)
 {
-    assert(rhport == 0);
-    irq_set_enabled(USBCTRL_IRQ, true);
+  (void) rhport;
+  assert(rhport == 0);
+  irq_set_enabled(USBCTRL_IRQ, true);
 }
 
 void hcd_int_disable(uint8_t rhport)
 {
-    // todo we should check this is disabling from the correct core; note currently this is never called
-    assert(rhport == 0);
-    irq_set_enabled(USBCTRL_IRQ, false);
+  (void) rhport;
+  // todo we should check this is disabling from the correct core; note currently this is never called
+  assert(rhport == 0);
+  irq_set_enabled(USBCTRL_IRQ, false);
 }
 
-bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t * buffer, uint16_t buflen)
+//--------------------------------------------------------------------+
+// Endpoint API
+//--------------------------------------------------------------------+
+
+bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const * ep_desc)
 {
-    pico_info("hcd_edpt_xfer dev_addr %d, ep_addr 0x%x, len %d\n", dev_addr, ep_addr, buflen);
-    
-    // Get appropriate ep. Either EPX or interrupt endpoint
-    struct hw_endpoint *ep = get_dev_ep(dev_addr, ep_addr);
-    assert(ep);
+  (void) rhport;
 
-    if (ep_addr != ep->ep_addr)
-    {
-        // Direction has flipped so re init it but with same properties
-        // TODO treat IN and OUT as invidual endpoints
-        _hw_endpoint_init(ep, dev_addr, ep_addr, ep->wMaxPacketSize, ep->transfer_type, 0);
-    }
+  pico_trace("hcd_edpt_open dev_addr %d, ep_addr %d\n", dev_addr, ep_desc->bEndpointAddress);
 
-    // True indicates this is the start of the transfer
-    _hw_endpoint_xfer(ep, buffer, buflen, true);
+  // Allocated differently based on if it's an interrupt endpoint or not
+  struct hw_endpoint *ep = _hw_endpoint_allocate(ep_desc->bmAttributes.xfer);
+  TU_ASSERT(ep);
 
-    // If a normal transfer (non-interrupt) then initiate using
-    // sie ctrl registers. Otherwise interrupt ep registers should
-    // already be configured
-    if (ep == &epx) {
-        // That has set up buffer control, endpoint control etc
-        // for host we have to initiate the transfer
-        usb_hw->dev_addr_ctrl = dev_addr | (tu_edpt_number(ep_addr) << USB_ADDR_ENDP_ENDPOINT_LSB);
-        uint32_t flags = USB_SIE_CTRL_START_TRANS_BITS | sie_ctrl_base;
-        flags |= ep->rx ? USB_SIE_CTRL_RECEIVE_DATA_BITS : USB_SIE_CTRL_SEND_DATA_BITS;
-        // Set pre if we are a low speed device on full speed hub
-        flags |= need_pre(dev_addr) ? USB_SIE_CTRL_PREAMBLE_EN_BITS : 0;
-        usb_hw->sie_ctrl = flags;
-    }
+  _hw_endpoint_init(ep,
+                    dev_addr,
+                    ep_desc->bEndpointAddress,
+                    tu_edpt_packet_size(ep_desc),
+                    ep_desc->bmAttributes.xfer,
+                    ep_desc->bInterval);
 
-    return true;
+  return true;
 }
 
-bool hcd_setup_send(uint8_t rhport, uint8_t dev_addr, uint8_t const setup_packet[8])
+bool hcd_edpt_xfer(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr, uint8_t * buffer, uint16_t buflen)
 {
-    pico_info("hcd_setup_send dev_addr %d\n", dev_addr);
-    
-    // Copy data into setup packet buffer
-    memcpy((void*)&usbh_dpram->setup_packet[0], setup_packet, 8);
+  (void) rhport;
+
+  pico_trace("hcd_edpt_xfer dev_addr %d, ep_addr 0x%x, len %d\n", dev_addr, ep_addr, buflen);
+
+  uint8_t const ep_num = tu_edpt_number(ep_addr);
+  tusb_dir_t const ep_dir = tu_edpt_dir(ep_addr);
+
+  // Get appropriate ep. Either EPX or interrupt endpoint
+  struct hw_endpoint *ep = get_dev_ep(dev_addr, ep_addr);
+
+  TU_ASSERT(ep);
+
+  // EP should be inactive
+  assert(!ep->active);
+
+  // Control endpoint can change direction 0x00 <-> 0x80
+  if ( ep_addr != ep->ep_addr )
+  {
+    assert(ep_num == 0);
 
-    // Configure EP0 struct with setup info for the trans complete
-    struct hw_endpoint *ep = _hw_endpoint_allocate(0);
-    // EP0 out
-    _hw_endpoint_init(ep, dev_addr, 0x00, ep->wMaxPacketSize, 0, 0);
-    assert(ep->configured);
-    ep->total_len = 8;
-    ep->transfer_size = 8;
-    ep->active = true;
-    ep->sent_setup = true;
+    // Direction has flipped on endpoint control so re init it but with same properties
+    _hw_endpoint_init(ep, dev_addr, ep_addr, ep->wMaxPacketSize, ep->transfer_type, 0);
+  }
 
-    // Set device address
-    usb_hw->dev_addr_ctrl = dev_addr;
-    // Set pre if we are a low speed device on full speed hub
-    uint32_t flags = sie_ctrl_base | USB_SIE_CTRL_SEND_SETUP_BITS | USB_SIE_CTRL_START_TRANS_BITS;
-    flags |= need_pre(dev_addr) ? USB_SIE_CTRL_PREAMBLE_EN_BITS : 0;
+  // If a normal transfer (non-interrupt) then initiate using
+  // sie ctrl registers. Otherwise interrupt ep registers should
+  // already be configured
+  if ( ep == &epx )
+  {
+    hw_endpoint_xfer_start(ep, buffer, buflen);
+
+    // That has set up buffer control, endpoint control etc
+    // for host we have to initiate the transfer
+    usb_hw->dev_addr_ctrl = (uint32_t) (dev_addr | (ep_num << USB_ADDR_ENDP_ENDPOINT_LSB));
+
+    uint32_t flags = USB_SIE_CTRL_START_TRANS_BITS | SIE_CTRL_BASE |
+                     (ep_dir ? USB_SIE_CTRL_RECEIVE_DATA_BITS : USB_SIE_CTRL_SEND_DATA_BITS) |
+                     (need_pre(dev_addr) ? USB_SIE_CTRL_PREAMBLE_EN_BITS : 0);
+    // START_TRANS bit on SIE_CTRL seems to exhibit 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 = flags & ~USB_SIE_CTRL_START_TRANS_BITS;
+    busy_wait_at_least_cycles(12);
     usb_hw->sie_ctrl = flags;
-    return true;
-}
+  }else
+  {
+    hw_endpoint_xfer_start(ep, buffer, buflen);
+  }
 
-uint32_t hcd_frame_number(uint8_t rhport)
-{
-    return usb_hw->sof_rd;
+  return true;
 }
 
-bool hcd_edpt_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_endpoint_t const * ep_desc)
-{
-    pico_trace("hcd_edpt_open dev_addr %d, ep_addr %d\n", dev_addr, ep_desc->bEndpointAddress);
-    hw_endpoint_init(dev_addr, ep_desc);
-    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_edpt_busy(uint8_t dev_addr, uint8_t ep_addr)
+bool hcd_setup_send(uint8_t rhport, uint8_t dev_addr, uint8_t const setup_packet[8])
 {
-    // EPX is shared, so multiple device addresses and endpoint addresses share that
-    // so if any transfer is active on epx, we are busy. Interrupt endpoints have their own
-    // EPX so ep->active will only be busy if there is a pending transfer on that interrupt endpoint
-    // on that device
-    pico_trace("hcd_edpt_busy dev addr %d ep_addr 0x%x\n", dev_addr, ep_addr);
-    struct hw_endpoint *ep = get_dev_ep(dev_addr, ep_addr);
-    assert(ep);
-    bool busy = ep->active;
-    pico_trace("busy == %d\n", busy);
-    return busy;
-}
+  (void) rhport;
 
-bool hcd_edpt_stalled(uint8_t dev_addr, uint8_t ep_addr)
-{
-    panic("hcd_pipe_stalled");
-    return false;
+  // Copy data into setup packet buffer
+  for ( uint8_t i = 0; i < 8; i++ )
+  {
+    usbh_dpram->setup_packet[i] = setup_packet[i];
+  }
+
+  // Configure EP0 struct with setup info for the trans complete
+  struct hw_endpoint * ep = _hw_endpoint_allocate(0);
+  TU_ASSERT(ep);
+
+  // EPX should be inactive
+  assert(!ep->active);
+
+  // EP0 out
+  _hw_endpoint_init(ep, dev_addr, 0x00, ep->wMaxPacketSize, 0, 0);
+  assert(ep->configured);
+
+  ep->remaining_len = 8;
+  ep->active = true;
+
+  // Set device address
+  usb_hw->dev_addr_ctrl = dev_addr;
+
+  // Set pre if we are a low speed device on full speed hub
+  uint32_t const flags = SIE_CTRL_BASE | USB_SIE_CTRL_SEND_SETUP_BITS | USB_SIE_CTRL_START_TRANS_BITS |
+                         (need_pre(dev_addr) ? USB_SIE_CTRL_PREAMBLE_EN_BITS : 0);
+
+  // START_TRANS bit on SIE_CTRL seems to exhibit 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 = flags & ~USB_SIE_CTRL_START_TRANS_BITS;
+  busy_wait_at_least_cycles(12);
+  usb_hw->sie_ctrl = flags;
+
+  return true;
 }
 
-bool hcd_edpt_clear_stall(uint8_t dev_addr, uint8_t ep_addr)
-{
-    panic("hcd_clear_stall");
-    return true;
+bool hcd_edpt_clear_stall(uint8_t rhport, uint8_t dev_addr, uint8_t ep_addr) {
+  (void) rhport;
+  (void) dev_addr;
+  (void) ep_addr;
+
+  panic("hcd_clear_stall");
+  // return true;
 }
 
 #endif
diff --git a/src/portable/raspberrypi/rp2040/rp2040_usb.c b/src/portable/raspberrypi/rp2040/rp2040_usb.c
index a0263612f..1ca711c77 100644
--- a/src/portable/raspberrypi/rp2040/rp2040_usb.c
+++ b/src/portable/raspberrypi/rp2040/rp2040_usb.c
@@ -2,6 +2,7 @@
  * The MIT License (MIT)
  *
  * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ * Copyright (c) 2021 Ha Thach (tinyusb.org) for Double Buffered
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -31,296 +32,344 @@
 #include <stdlib.h>
 #include "rp2040_usb.h"
 
-// Direction strings for debug
-const char *ep_dir_string[] = {
-        "out",
-        "in",
-};
+//--------------------------------------------------------------------+
+// MACRO CONSTANT TYPEDEF PROTOTYPE
+//--------------------------------------------------------------------+
+static void _hw_endpoint_xfer_sync(struct hw_endpoint* ep);
 
-static inline void _hw_endpoint_lock_update(struct hw_endpoint *ep, int delta) {
-    // todo add critsec as necessary to prevent issues between worker and IRQ...
-    //  note that this is perhaps as simple as disabling IRQs because it would make
-    //  sense to have worker and IRQ on same core, however I think using critsec is about equivalent.
-}
+#if TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
+  static bool e15_is_bulkin_ep(struct hw_endpoint* ep);
+  static bool e15_is_critical_frame_period(struct hw_endpoint* ep);
+#else
+  #define e15_is_bulkin_ep(x)             (false)
+  #define e15_is_critical_frame_period(x) (false)
+#endif
 
-#if TUSB_OPT_HOST_ENABLED
-static inline void _hw_endpoint_update_last_buf(struct hw_endpoint *ep)
-{
-    ep->last_buf = (ep->len + ep->transfer_size == ep->total_len);
+// if usb hardware is in host mode
+TU_ATTR_ALWAYS_INLINE static inline bool is_host_mode(void) {
+  return (usb_hw->main_ctrl & USB_MAIN_CTRL_HOST_NDEVICE_BITS) ? true : false;
 }
-#endif
 
-void rp2040_usb_init(void)
-{
-    // Reset usb controller
-    reset_block(RESETS_RESET_USBCTRL_BITS);
-    unreset_block_wait(RESETS_RESET_USBCTRL_BITS);
+//--------------------------------------------------------------------+
+// Implementation
+//--------------------------------------------------------------------+
 
-    // Clear any previous state just in case
-    memset(usb_hw, 0, sizeof(*usb_hw));
-    memset(usb_dpram, 0, sizeof(*usb_dpram));
+void rp2040_usb_init(void) {
+  // Reset usb controller
+  reset_block(RESETS_RESET_USBCTRL_BITS);
+  unreset_block_wait(RESETS_RESET_USBCTRL_BITS);
 
-    // Mux the controller to the onboard usb phy
-    usb_hw->muxing    = USB_USB_MUXING_TO_PHY_BITS    | USB_USB_MUXING_SOFTCON_BITS;
+#ifdef __GNUC__
+  // Clear any previous state just in case
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Warray-bounds"
+#if __GNUC__ > 6
+#pragma GCC diagnostic ignored "-Wstringop-overflow"
+#endif
+#endif
+  memset(usb_hw, 0, sizeof(*usb_hw));
+  memset(usb_dpram, 0, sizeof(*usb_dpram));
+#ifdef __GNUC__
+#pragma GCC diagnostic pop
+#endif
 
-    // Force VBUS detect so the device thinks it is plugged into a host
-    // TODO support VBUs detect
-    usb_hw->pwr       = USB_USB_PWR_VBUS_DETECT_BITS  | USB_USB_PWR_VBUS_DETECT_OVERRIDE_EN_BITS;
-}
+  // Mux the controller to the onboard usb phy
+  usb_hw->muxing = USB_USB_MUXING_TO_PHY_BITS | USB_USB_MUXING_SOFTCON_BITS;
 
-void hw_endpoint_reset_transfer(struct hw_endpoint *ep)
-{
-    ep->stalled = false;
-    ep->active = false;
-#if TUSB_OPT_HOST_ENABLED
-    ep->sent_setup = false;
-#endif
-    ep->total_len = 0;
-    ep->len = 0;
-    ep->transfer_size = 0;
-    ep->user_buf = 0;
+  TU_LOG2_INT(sizeof(hw_endpoint_t));
 }
 
-void _hw_endpoint_buffer_control_update32(struct hw_endpoint *ep, uint32_t and_mask, uint32_t or_mask) {
-    uint32_t value = 0;
-    if (and_mask) {
-        value = *ep->buffer_control & and_mask;
-    }
-    if (or_mask) {
-        value |= or_mask;
-        if (or_mask & USB_BUF_CTRL_AVAIL) {
-            if (*ep->buffer_control & USB_BUF_CTRL_AVAIL) {
-                panic("ep %d %s was already available", tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
-            }
-            *ep->buffer_control = value & ~USB_BUF_CTRL_AVAIL;
-            // 12 cycle delay.. (should be good for 48*12Mhz = 576Mhz)
-            // Don't need delay in host mode as host is in charge
-#if !TUSB_OPT_HOST_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
-        }
-    }
-    *ep->buffer_control = value;
+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 _hw_endpoint_start_next_buffer(struct hw_endpoint *ep)
-{
-    // Prepare buffer control register value
-    uint32_t val = ep->transfer_size | USB_BUF_CTRL_AVAIL;
+void __tusb_irq_path_func(_hw_endpoint_buffer_control_update32)(struct hw_endpoint* ep, uint32_t and_mask,
+                                                               uint32_t or_mask) {
+  uint32_t value = 0;
+
+  if (and_mask) {
+    value = *ep->buffer_control & and_mask;
+  }
 
-    if (!ep->rx)
-    {
-        // Copy data from user buffer to hw buffer
-        memcpy(ep->hw_data_buf, &ep->user_buf[ep->len], ep->transfer_size);
-        // Mark as full
-        val |= USB_BUF_CTRL_FULL;
+  if (or_mask) {
+    value |= or_mask;
+    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;
+      // 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 ( !is_host_mode()) {
+        busy_wait_at_least_cycles(12);
+      }
     }
+  }
 
-    // PID
-    val |= ep->next_pid ? USB_BUF_CTRL_DATA1_PID : USB_BUF_CTRL_DATA0_PID;
+  *ep->buffer_control = value;
+}
 
-#if TUSB_OPT_DEVICE_ENABLED
-    ep->next_pid ^= 1u;
+// prepare buffer, return buffer control
+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);
 
-#else
-    // For Host (also device but since we dictate the endpoint size, following scenario does not occur)
-    // Next PID depends on the number of packet in case wMaxPacketSize < 64 (e.g Interrupt Endpoint 8, or 12)
-    // Special case with control status stage where PID is always DATA1
-    if ( ep->transfer_size == 0 )
-    {
-      // ZLP also toggle data
-      ep->next_pid ^= 1u;
-    }else
-    {
-      uint32_t packet_count = 1 + ((ep->transfer_size - 1) / ep->wMaxPacketSize);
+  uint32_t buf_ctrl = buflen | USB_BUF_CTRL_AVAIL;
 
-      if ( packet_count & 0x01 )
-      {
-        ep->next_pid ^= 1u;
-      }
-    }
-#endif
+  // PID
+  buf_ctrl |= ep->next_pid ? USB_BUF_CTRL_DATA1_PID : USB_BUF_CTRL_DATA0_PID;
+  ep->next_pid ^= 1u;
 
+  if (!ep->rx) {
+    // Copy data from user buffer to hw buffer
+    memcpy(ep->hw_data_buf + buf_id * 64, ep->user_buf, buflen);
+    ep->user_buf += buflen;
 
-#if TUSB_OPT_HOST_ENABLED
-    // 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->last_buf)
-    {
-        pico_trace("Last buf (%d bytes left)\n", ep->transfer_size);
-        val |= USB_BUF_CTRL_LAST;
-    }
-#endif
+    // Mark as full
+    buf_ctrl |= USB_BUF_CTRL_FULL;
+  }
 
-    // Finally, write to buffer_control which will trigger the transfer
-    // the next time the controller polls this dpram address
-    _hw_endpoint_buffer_control_set_value32(ep, val);
-    pico_trace("buffer control (0x%p) <- 0x%x\n", ep->buffer_control, val);
-    //print_bufctrl16(val);
+  // 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) {
+    buf_ctrl |= USB_BUF_CTRL_LAST;
+  }
+
+  if (buf_id) buf_ctrl = buf_ctrl << 16;
+
+  return buf_ctrl;
 }
 
+// Prepare buffer control register value
+void __tusb_irq_path_func(hw_endpoint_start_next_buffer)(struct hw_endpoint* ep) {
+  uint32_t ep_ctrl = *ep->endpoint_control;
 
-void _hw_endpoint_xfer_start(struct hw_endpoint *ep, uint8_t *buffer, uint16_t total_len)
-{
-    _hw_endpoint_lock_update(ep, 1);
-    pico_trace("Start transfer of total len %d on ep %d %s\n", total_len, tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
-    if (ep->active)
-    {
-        // TODO: Is this acceptable for interrupt packets?
-        pico_warn("WARN: starting new transfer on already active ep %d %s\n", tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
+  // always compute and start with buffer 0
+  uint32_t buf_ctrl = prepare_ep_buffer(ep, 0) | USB_BUF_CTRL_SEL;
 
-        hw_endpoint_reset_transfer(ep);
-    }
+  // For now: skip double buffered for OUT endpoint in Device mode, since
+  // host could send < 64 bytes and cause short packet on buffer0
+  // NOTE: this could happen to Host mode IN endpoint
+  // Also, Host mode "interrupt" endpoint hardware is only single buffered,
+  // NOTE2: Currently Host bulk is implemented using "interrupt" endpoint
+  bool const is_host = is_host_mode();
+  bool const force_single = (!is_host && !tu_edpt_dir(ep->ep_addr)) ||
+                            (is_host && tu_edpt_number(ep->ep_addr) != 0);
 
-    // Fill in info now that we're kicking off the hw
-    ep->total_len = total_len;
-    ep->len = 0;
+  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)
 
-    // Limit by packet size but not less 64 (i.e low speed 8 bytes EP0)
-    ep->transfer_size = tu_min16(total_len, tu_max16(64, ep->wMaxPacketSize));
+    buf_ctrl |= prepare_ep_buffer(ep, 1);
 
-    ep->active = true;
-    ep->user_buf = buffer;
-#if TUSB_OPT_HOST_ENABLED
-    // Recalculate if this is the last buffer
-    _hw_endpoint_update_last_buf(ep);
-    ep->buf_sel = 0;
-#endif
+    // 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 {
+    // 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;
+  }
 
-    _hw_endpoint_start_next_buffer(ep);
-    _hw_endpoint_lock_update(ep, -1);
+  *ep->endpoint_control = ep_ctrl;
+
+  TU_LOG(3, "  Prepare BufCtrl: [0] = 0x%04x  [1] = 0x%04x\r\n", tu_u32_low16(buf_ctrl), tu_u32_high16(buf_ctrl));
+
+  // Finally, write to buffer_control which will trigger the transfer
+  // the next time the controller polls this dpram address
+  _hw_endpoint_buffer_control_set_value32(ep, buf_ctrl);
 }
 
-void _hw_endpoint_xfer_sync(struct hw_endpoint *ep)
-{
-    // Update hw endpoint struct with info from hardware
-    // after a buff status interrupt
+void hw_endpoint_xfer_start(struct hw_endpoint* ep, uint8_t* buffer, uint16_t total_len) {
+  hw_endpoint_lock_update(ep, 1);
 
-    // Get the buffer state and amount of bytes we have
-    // transferred
-    uint32_t buf_ctrl = _hw_endpoint_buffer_control_get_value32(ep);
-    uint16_t transferred_bytes = buf_ctrl & USB_BUF_CTRL_LEN_MASK;
+  if (ep->active) {
+    // TODO: Is this acceptable for interrupt packets?
+    TU_LOG(1, "WARN: starting new transfer on already active ep %02X\r\n", ep->ep_addr);
+    hw_endpoint_reset_transfer(ep);
+  }
 
-#if TUSB_OPT_HOST_ENABLED
-    // RP2040-E4
-    // tag::host_buf_sel_fix[]
-    // TODO need changes to support double buffering
-    if (ep->buf_sel == 1)
-    {
-        // Host can erroneously write status to top half of buf_ctrl register
-        buf_ctrl = buf_ctrl >> 16;
+  // 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;
 
-        // update buf1 -> buf0 to prevent panic with "already available"
-        *ep->buffer_control = buf_ctrl;
-    }
-    // Flip buf sel for host
-    ep->buf_sel ^= 1u;
-    // end::host_buf_sel_fix[]
-#endif
+  if (e15_is_bulkin_ep(ep)) {
+    usb_hw_set->inte = USB_INTS_DEV_SOF_BITS;
+  }
+
+  if (e15_is_critical_frame_period(ep)) {
+    ep->pending = 1;
+  } else {
+    hw_endpoint_start_next_buffer(ep);
+  }
+
+  hw_endpoint_lock_update(ep, -1);
+}
+
+// sync endpoint buffer and return transferred bytes
+static uint16_t __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;
 
-    // We are continuing a transfer here. If we are TX, we have successfullly
+  uint16_t xferred_bytes = buf_ctrl & USB_BUF_CTRL_LEN_MASK;
+
+  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
-    if (!ep->rx)
-    {
-        assert(!(buf_ctrl & USB_BUF_CTRL_FULL));
-        pico_trace("tx %d bytes (buf_ctrl 0x%08x)\n", transferred_bytes, buf_ctrl);
-        ep->len += transferred_bytes;
-    }
-    else
-    {
-        // If we are OUT we have recieved some data, so can increase the length
-        // we have recieved AFTER we have copied it to the user buffer at the appropriate
-        // offset
-        pico_trace("rx %d bytes (buf_ctrl 0x%08x)\n", transferred_bytes, buf_ctrl);
-        assert(buf_ctrl & USB_BUF_CTRL_FULL);
-        memcpy(&ep->user_buf[ep->len], ep->hw_data_buf, transferred_bytes);
-        ep->len += transferred_bytes;
-    }
+    assert(!(buf_ctrl & USB_BUF_CTRL_FULL));
 
-    // Sometimes the host will send less data than we expect...
-    // If this is a short out transfer update the total length of the transfer
-    // to be the current length
-    if ((ep->rx) && (transferred_bytes < ep->transfer_size))
-    {
-        pico_trace("Short rx transfer\n");
-        // Reduce total length as this is last packet
-        ep->total_len = ep->len;
-    }
+    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);
+    ep->user_buf += xferred_bytes;
+  }
+
+  // Short packet
+  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;
+  }
+
+  return xferred_bytes;
 }
 
-// Returns true if transfer is complete
-bool _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);
-    }
+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
 
-    // Update EP struct from hardware state
-    _hw_endpoint_xfer_sync(ep);
+  uint32_t __unused buf_ctrl = _hw_endpoint_buffer_control_get_value32(ep);
+  TU_LOG(3, "  Sync BufCtrl: [0] = 0x%04x  [1] = 0x%04x\r\n", tu_u32_low16(buf_ctrl), tu_u32_high16(buf_ctrl));
 
-    // Now we have synced our state with the hardware. Is there more data to transfer?
-    // Limit by packet size but not less 64 (i.e low speed 8 bytes EP0)
-    uint16_t remaining_bytes = ep->total_len - ep->len;
-    ep->transfer_size = tu_min16(remaining_bytes, tu_max16(64, ep->wMaxPacketSize));
-#if TUSB_OPT_HOST_ENABLED
-    _hw_endpoint_update_last_buf(ep);
-#endif
+  // always sync buffer 0
+  uint16_t buf0_bytes = sync_ep_buffer(ep, 0);
 
-    // Can happen because of programmer error so check for it
-    if (ep->len > ep->total_len)
-    {
-        panic("Transferred more data than expected");
-    }
+  // sync buffer 1 if double buffered
+  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 {
+      // 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
+      // the next transfer (not current one). For now we disable double buffered for device OUT
+      // NOTE this could happen to Host IN
+#if 0
+      uint8_t const ep_num = tu_edpt_number(ep->ep_addr);
+      uint8_t const dir =  (uint8_t) tu_edpt_dir(ep->ep_addr);
+      uint8_t const ep_id = 2*ep_num + (dir ? 0 : 1);
+
+      // abort queued transfer on buffer 1
+      usb_hw->abort |= TU_BIT(ep_id);
+
+      while ( !(usb_hw->abort_done & TU_BIT(ep_id)) ) {}
 
-    // If we are done then notify tinyusb
-    if (ep->len == ep->total_len)
-    {
-        pico_trace("Completed transfer of %d bytes on ep %d %s\n",
-                   ep->len, tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
-        // Notify caller we are done so it can notify the tinyusb
-        // stack
-        _hw_endpoint_lock_update(ep, -1);
-        return true;
+      uint32_t ep_ctrl = *ep->endpoint_control;
+      ep_ctrl &= ~(EP_CTRL_DOUBLE_BUFFERED_BITS | EP_CTRL_INTERRUPT_PER_DOUBLE_BUFFER);
+      ep_ctrl |= EP_CTRL_INTERRUPT_PER_BUFFER;
+
+      _hw_endpoint_buffer_control_set_value32(ep, 0);
+
+      usb_hw->abort &= ~TU_BIT(ep_id);
+
+      TU_LOG(3, "----SHORT PACKET buffer0 on EP %02X:\r\n", ep->ep_addr);
+      TU_LOG(3, "  BufCtrl: [0] = 0x%04x  [1] = 0x%04x\r\n", tu_u32_low16(buf_ctrl), tu_u32_high16(buf_ctrl));
+#endif
     }
-    else
-    {
-        _hw_endpoint_start_next_buffer(ep);
+  }
+}
+
+// Returns true if transfer is complete
+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 %02X", ep->ep_addr);
+  }
+
+  // Update EP struct from hardware state
+  _hw_endpoint_xfer_sync(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 %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)) {
+      ep->pending = 1;
+    } else {
+      hw_endpoint_start_next_buffer(ep);
     }
+  }
 
-    _hw_endpoint_lock_update(ep, -1);
-    // More work to do
-    return false;
+  hw_endpoint_lock_update(ep, -1);
+  // More work to do
+  return false;
 }
 
-void _hw_endpoint_xfer(struct hw_endpoint *ep, uint8_t *buffer, uint16_t total_len, bool start)
-{
-    // Trace
-    pico_trace("hw_endpoint_xfer ep %d %s", tu_edpt_number(ep->ep_addr), ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
-    pico_trace(" total_len %d, start=%d\n", total_len, start);
+//--------------------------------------------------------------------+
+// Errata 15
+//--------------------------------------------------------------------+
 
-    assert(ep->configured);
+#if TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
 
+/* Don't mark IN buffers as available during the last 200us of a full-speed
+   frame. This avoids a situation seen with the USB2.0 hub on a Raspberry
+   Pi 4 where a late IN token before the next full-speed SOF can cause port
+   babble and a corrupt ACK packet. The nature of the data corruption has a
+   chance to cause device lockup.
 
-    if (start)
-    {
-        _hw_endpoint_xfer_start(ep, buffer, total_len);
-    }
-    else
-    {
-        _hw_endpoint_xfer_continue(ep);
-    }
+   Use the next SOF to mark delayed buffers as available. This reduces
+   available Bulk IN bandwidth by approximately 20%, and requires that the
+   SOF interrupt is enabled while these transfers are ongoing.
+
+   Inherit the top-level enable from the corresponding Pico-SDK flag.
+   Applications that will not use the device in a situation where it could
+   be plugged into a Pi 4 or Pi 400 (for example, when directly connected
+   to a commodity hub or other host) can turn off the flag in the SDK.
+*/
+
+volatile uint32_t e15_last_sof = 0;
+
+// check if Errata 15 is needed for this endpoint i.e device bulk-in
+static bool __tusb_irq_path_func(e15_is_bulkin_ep)(struct hw_endpoint* ep) {
+  return (!is_host_mode() && tu_edpt_dir(ep->ep_addr) == TUSB_DIR_IN &&
+          ep->transfer_type == TUSB_XFER_BULK);
+}
+
+// check if we need to apply Errata 15 workaround : i.e
+// Endpoint is BULK IN and is currently in critical frame period i.e 20% of last usb frame
+static bool __tusb_irq_path_func(e15_is_critical_frame_period)(struct hw_endpoint* ep) {
+  TU_VERIFY(e15_is_bulkin_ep(ep));
+
+  /* Avoid the last 200us (uframe 6.5-7) of a frame, up to the EOF2 point.
+   * The device state machine cannot recover from receiving an incorrect PID
+   * when it is expecting an ACK.
+   */
+  uint32_t delta = time_us_32() - e15_last_sof;
+  if (delta < 800 || delta > 998) {
+    return false;
+  }
+  TU_LOG(3, "Avoiding sof %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 // TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
 #endif
diff --git a/src/portable/raspberrypi/rp2040/rp2040_usb.h b/src/portable/raspberrypi/rp2040/rp2040_usb.h
index 32d20051f..d4d29a816 100644
--- a/src/portable/raspberrypi/rp2040/rp2040_usb.h
+++ b/src/portable/raspberrypi/rp2040/rp2040_usb.h
@@ -11,150 +11,133 @@
 #include "hardware/structs/usb.h"
 #include "hardware/irq.h"
 #include "hardware/resets.h"
+#include "hardware/timer.h"
 
 #if defined(PICO_RP2040_USB_DEVICE_ENUMERATION_FIX) && !defined(TUD_OPT_RP2040_USB_DEVICE_ENUMERATION_FIX)
 #define TUD_OPT_RP2040_USB_DEVICE_ENUMERATION_FIX PICO_RP2040_USB_DEVICE_ENUMERATION_FIX
 #endif
 
+#if defined(PICO_RP2040_USB_DEVICE_UFRAME_FIX) && !defined(TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX)
+#define TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX PICO_RP2040_USB_DEVICE_UFRAME_FIX
+#endif
 
-#if false && !defined(NDEBUG)
-#define pico_trace(format,args...) printf(format, ## args)
-#else
-#define pico_trace(format,...) ((void)0)
+#if TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
+#undef PICO_RP2040_USB_FAST_IRQ
+#define PICO_RP2040_USB_FAST_IRQ 1
 #endif
 
-#if false && !defined(NDEBUG)
-#define pico_info(format,args...) printf(format, ## args)
-#else
-#define pico_info(format,...) ((void)0)
+#ifndef PICO_RP2040_USB_FAST_IRQ
+#define PICO_RP2040_USB_FAST_IRQ 0
 #endif
 
-#if false && !defined(NDEBUG)
-#define pico_warn(format,args...) printf(format, ## args)
+#if PICO_RP2040_USB_FAST_IRQ
+#define __tusb_irq_path_func(x) __no_inline_not_in_flash_func(x)
 #else
-#define pico_warn(format,...) ((void)0)
+#define __tusb_irq_path_func(x) x
 #endif
 
+#define usb_hw_set    ((usb_hw_t *) hw_set_alias_untyped(usb_hw))
+#define usb_hw_clear  ((usb_hw_t *) hw_clear_alias_untyped(usb_hw))
+
+#define pico_info(...)  TU_LOG(2, __VA_ARGS__)
+#define pico_trace(...) TU_LOG(3, __VA_ARGS__)
+
 // Hardware information per endpoint
-struct hw_endpoint
+typedef struct hw_endpoint
 {
     // Is this a valid struct
     bool configured;
-    
+
     // Transfer direction (i.e. IN is rx for host but tx for device)
     // allows us to common up transfer functions
     bool rx;
-    
+
     uint8_t ep_addr;
     uint8_t next_pid;
 
     // Endpoint control register
     io_rw_32 *endpoint_control;
+
     // Buffer control register
     io_rw_32 *buffer_control;
 
     // Buffer pointer in usb dpram
     uint8_t *hw_data_buf;
 
-    // Have we been stalled
-    bool stalled;
-
-    // Current transfer information
-    bool active;
-    uint16_t total_len;
-    uint16_t len;
-    // Amount of data with the hardware
-    uint16_t transfer_size;
     // User buffer in main memory
     uint8_t *user_buf;
 
+    // Current transfer information
+    uint16_t remaining_len;
+    uint16_t xferred_len;
+
     // Data needed from EP descriptor
     uint16_t wMaxPacketSize;
+
+    // Endpoint is in use
+    bool active;
+
     // Interrupt, bulk, etc
     uint8_t transfer_type;
-    
-#if TUSB_OPT_HOST_ENABLED
-    // Only needed for host mode
-    bool last_buf;
-    // RP2040-E4: HOST BUG. Host will incorrect write status to top half of buffer
-    // control register when doing transfers > 1 packet
-    uint8_t buf_sel;
+
+    // Transfer scheduled but not active
+    uint8_t pending;
+
+#if CFG_TUH_ENABLED
     // Only needed for host
     uint8_t dev_addr;
-    bool sent_setup;
+
     // If interrupt endpoint
     uint8_t interrupt_num;
 #endif
-};
+
+} hw_endpoint_t;
+
+#if TUD_OPT_RP2040_USB_DEVICE_UFRAME_FIX
+extern volatile uint32_t e15_last_sof;
+#endif
 
 void rp2040_usb_init(void);
 
+void hw_endpoint_xfer_start(struct hw_endpoint *ep, uint8_t *buffer, uint16_t total_len);
+bool hw_endpoint_xfer_continue(struct hw_endpoint *ep);
 void hw_endpoint_reset_transfer(struct hw_endpoint *ep);
-void _hw_endpoint_xfer(struct hw_endpoint *ep, uint8_t *buffer, uint16_t total_len, bool start);
-void _hw_endpoint_start_next_buffer(struct hw_endpoint *ep);
-void _hw_endpoint_xfer_start(struct hw_endpoint *ep, uint8_t *buffer, uint16_t total_len);
-void _hw_endpoint_xfer_sync(struct hw_endpoint *ep);
-bool _hw_endpoint_xfer_continue(struct hw_endpoint *ep);
-void _hw_endpoint_buffer_control_update32(struct hw_endpoint *ep, uint32_t and_mask, uint32_t or_mask);
-static inline uint32_t _hw_endpoint_buffer_control_get_value32(struct hw_endpoint *ep) {
-    return *ep->buffer_control;
-}
-static inline void _hw_endpoint_buffer_control_set_value32(struct hw_endpoint *ep, uint32_t value) {
-    return _hw_endpoint_buffer_control_update32(ep, 0, value);
-}
-static inline void _hw_endpoint_buffer_control_set_mask32(struct hw_endpoint *ep, uint32_t value) {
-    return _hw_endpoint_buffer_control_update32(ep, ~value, value);
-}
-static inline void _hw_endpoint_buffer_control_clear_mask32(struct hw_endpoint *ep, uint32_t value) {
-    return _hw_endpoint_buffer_control_update32(ep, ~value, 0);
-}
+void hw_endpoint_start_next_buffer(struct hw_endpoint *ep);
 
-static inline uintptr_t hw_data_offset(uint8_t *buf)
-{
-    // Remove usb base from buffer pointer
-    return (uintptr_t)buf ^ (uintptr_t)usb_dpram;
+TU_ATTR_ALWAYS_INLINE static inline void hw_endpoint_lock_update(__unused struct hw_endpoint * ep, __unused int delta) {
+  // todo add critsec as necessary to prevent issues between worker and IRQ...
+  //  note that this is perhaps as simple as disabling IRQs because it would make
+  //  sense to have worker and IRQ on same core, however I think using critsec is about equivalent.
 }
 
-extern const char *ep_dir_string[];
+void _hw_endpoint_buffer_control_update32(struct hw_endpoint *ep, uint32_t and_mask, uint32_t or_mask);
 
-typedef union TU_ATTR_PACKED
+TU_ATTR_ALWAYS_INLINE static inline uint32_t _hw_endpoint_buffer_control_get_value32 (struct hw_endpoint *ep)
 {
-  uint16_t u16;
-  struct TU_ATTR_PACKED
-  {
-    uint16_t xfer_len     : 10;
-    uint16_t available    : 1;
-    uint16_t stall        : 1;
-    uint16_t reset_bufsel : 1;
-    uint16_t data_toggle  : 1;
-    uint16_t last_buf     : 1;
-    uint16_t full         : 1;
-  };
-} rp2040_buffer_control_t;
-
-TU_VERIFY_STATIC(sizeof(rp2040_buffer_control_t) == 2, "size is not correct");
+  return *ep->buffer_control;
+}
 
-static inline void print_bufctrl16(uint32_t u16)
+TU_ATTR_ALWAYS_INLINE static inline void _hw_endpoint_buffer_control_set_value32 (struct hw_endpoint *ep, uint32_t value)
 {
-  rp2040_buffer_control_t bufctrl;
-
-  bufctrl.u16 = u16;
-
-  TU_LOG(2, "len = %u, available = %u, stall = %u, reset = %u, toggle = %u, last = %u, full = %u\r\n",
-         bufctrl.xfer_len, bufctrl.available, bufctrl.stall, bufctrl.reset_bufsel, bufctrl.data_toggle, bufctrl.last_buf, bufctrl.full);
+  _hw_endpoint_buffer_control_update32(ep, 0, value);
 }
 
-static inline void print_bufctrl32(uint32_t u32)
+TU_ATTR_ALWAYS_INLINE static inline void _hw_endpoint_buffer_control_set_mask32 (struct hw_endpoint *ep, uint32_t value)
 {
-  uint16_t u16;
+  _hw_endpoint_buffer_control_update32(ep, ~value, value);
+}
 
-  u16 = u32 >> 16;
-  TU_LOG(2, "Buffer Control 1 0x%x: ", u16);
-  print_bufctrl16(u16);
+TU_ATTR_ALWAYS_INLINE static inline void _hw_endpoint_buffer_control_clear_mask32 (struct hw_endpoint *ep, uint32_t value)
+{
+  _hw_endpoint_buffer_control_update32(ep, ~value, 0);
+}
 
-  u16 = u32 & 0x0000ffff;
-  TU_LOG(2, "Buffer Control 0 0x%x: ", u16);
-  print_bufctrl16(u16);
+static inline uintptr_t hw_data_offset (uint8_t *buf)
+{
+  // Remove usb base from buffer pointer
+  return (uintptr_t) buf ^ (uintptr_t) usb_dpram;
 }
 
+extern const char *ep_dir_string[];
+
 #endif