diff --git a/src/class/msc/msc_device.c b/src/class/msc/msc_device.c
index 7dcd4983e..6b3a392a8 100644
--- a/src/class/msc/msc_device.c
+++ b/src/class/msc/msc_device.c
@@ -323,7 +323,11 @@ bool mscd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t
         if ( p_msc->stage == MSC_STAGE_CMD )
         {
           // part of reset recovery (probably due to invalid CBW) -> prepare for new command
-          TU_ASSERT( prepare_cbw(rhport, p_msc) );
+          // Note: skip if already queued previously
+          if ( usbd_edpt_ready(rhport, p_msc->ep_out) )
+          {
+            TU_ASSERT( prepare_cbw(rhport, p_msc) );
+          }
         }
       }
     }
diff --git a/src/common/tusb_common.h b/src/common/tusb_common.h
index 1452e0105..687f980be 100644
--- a/src/common/tusb_common.h
+++ b/src/common/tusb_common.h
@@ -110,6 +110,9 @@ TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_u32_byte2(uint32_t u32) { return
 TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_u32_byte1(uint32_t u32) { return TU_U32_BYTE1(u32); }
 TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_u32_byte0(uint32_t u32) { return TU_U32_BYTE0(u32); }
 
+TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_u32_high16(uint32_t u32) { return (uint16_t) (u32 >> 16); }
+TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_u32_low16 (uint32_t u32) { return (uint16_t) (u32 & 0x0000ffffu); }
+
 TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_u16_high(uint16_t u16) { return TU_U16_HIGH(u16); }
 TU_ATTR_ALWAYS_INLINE static inline uint8_t tu_u16_low (uint16_t u16) { return TU_U16_LOW(u16); }
 
diff --git a/src/device/dcd.h b/src/device/dcd.h
index 8d042bbde..d43a0dd9a 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -153,7 +153,7 @@ bool dcd_edpt_xfer            (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer
 // This API is optional, may be useful for register-based for transferring data.
 bool dcd_edpt_xfer_fifo       (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16_t total_bytes) TU_ATTR_WEAK;
 
-// Stall endpoint
+// Stall endpoint, any queuing transfer should be removed from endpoint
 void dcd_edpt_stall           (uint8_t rhport, uint8_t ep_addr);
 
 // clear stall, data toggle is also reset to DATA0
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 7cd8fad42..a52ea9afe 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -1333,26 +1333,33 @@ bool usbd_edpt_busy(uint8_t rhport, uint8_t ep_addr)
 
 void usbd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
 {
-  TU_LOG(USBD_DBG, "    Stall EP %02X\r\n", ep_addr);
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir   = tu_edpt_dir(ep_addr);
 
-  dcd_edpt_stall(rhport, ep_addr);
-  _usbd_dev.ep_status[epnum][dir].stalled = true;
-  _usbd_dev.ep_status[epnum][dir].busy = true;
+  // only stalled if currently cleared
+  if ( !_usbd_dev.ep_status[epnum][dir].stalled )
+  {
+    TU_LOG(USBD_DBG, "    Stall EP %02X\r\n", ep_addr);
+    dcd_edpt_stall(rhport, ep_addr);
+    _usbd_dev.ep_status[epnum][dir].stalled = true;
+    _usbd_dev.ep_status[epnum][dir].busy = true;
+  }
 }
 
 void usbd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
 {
-  TU_LOG(USBD_DBG, "    Clear Stall EP %02X\r\n", ep_addr);
-
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir   = tu_edpt_dir(ep_addr);
 
-  dcd_edpt_clear_stall(rhport, ep_addr);
-  _usbd_dev.ep_status[epnum][dir].stalled = false;
-  _usbd_dev.ep_status[epnum][dir].busy = false;
+  // only clear if currently stalled
+  if ( _usbd_dev.ep_status[epnum][dir].stalled )
+  {
+    TU_LOG(USBD_DBG, "    Clear Stall EP %02X\r\n", ep_addr);
+    dcd_edpt_clear_stall(rhport, ep_addr);
+    _usbd_dev.ep_status[epnum][dir].stalled = false;
+    _usbd_dev.ep_status[epnum][dir].busy = false;
+  }
 }
 
 bool usbd_edpt_stalled(uint8_t rhport, uint8_t ep_addr)
diff --git a/src/device/usbd_pvt.h b/src/device/usbd_pvt.h
index 6a4b30956..7607b9895 100644
--- a/src/device/usbd_pvt.h
+++ b/src/device/usbd_pvt.h
@@ -81,7 +81,7 @@ bool usbd_edpt_claim(uint8_t rhport, uint8_t ep_addr);
 // Release an endpoint without submitting a transfer
 bool usbd_edpt_release(uint8_t rhport, uint8_t ep_addr);
 
-// Check if endpoint transferring is complete
+// Check if endpoint is busy transferring
 bool usbd_edpt_busy(uint8_t rhport, uint8_t ep_addr);
 
 // Stall endpoint
@@ -93,6 +93,7 @@ void usbd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr);
 // Check if endpoint is stalled
 bool usbd_edpt_stalled(uint8_t rhport, uint8_t ep_addr);
 
+// Check if endpoint is ready (not busy and not stalled)
 TU_ATTR_ALWAYS_INLINE static inline
 bool usbd_edpt_ready(uint8_t rhport, uint8_t ep_addr)
 {
diff --git a/src/portable/raspberrypi/rp2040/dcd_rp2040.c b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
index e81681d4f..a710bd557 100644
--- a/src/portable/raspberrypi/rp2040/dcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
@@ -124,9 +124,7 @@ static void hw_endpoint_init(uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t t
   // 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);
-
+  ep->next_pid = 0u;
   ep->wMaxPacketSize = wMaxPacketSize;
   ep->transfer_type = transfer_type;
 
@@ -145,8 +143,7 @@ static void hw_endpoint_init(uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t t
 
   if ( num == 0 )
   {
-    // EP0 has no endpoint control register because
-    // the buffer offsets are fixed
+    // EP0 has no endpoint control register because the buffer offsets are fixed
     ep->endpoint_control = NULL;
 
     // Buffer offset is fixed (also double buffered)
@@ -178,7 +175,7 @@ static void hw_endpoint_xfer(uint8_t ep_addr, uint8_t *buffer, uint16_t total_by
 static void hw_handle_buff_status(void)
 {
     uint32_t remaining_buffers = usb_hw->buf_status;
-    pico_trace("buf_status 0x%08x\n", remaining_buffers);
+    pico_trace("buf_status = 0x%08x\n", remaining_buffers);
     uint bit = 1u;
     for (uint i = 0; remaining_buffers && i < USB_MAX_ENDPOINTS * 2; i++)
     {
@@ -186,8 +183,10 @@ static void hw_handle_buff_status(void)
         {
             // 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)
@@ -202,7 +201,7 @@ static void hw_handle_buff_status(void)
     }
 }
 
-static void reset_ep0(void)
+static void reset_ep0_pid(void)
 {
     // If we have finished this transfer on EP0 set pid back to 1 for next
     // setup transfer. Also clear a stall in case
@@ -214,14 +213,18 @@ static void reset_ep0(void)
     }
 }
 
-static void reset_all_endpoints(void)
+static void reset_non_control_endpoints(void)
 {
-  memset(hw_endpoints, 0, sizeof(hw_endpoints));
-  next_buffer_ptr = &usb_dpram->epx_data[0];
+  // 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;
+  }
 
-  // Init Control endpoint out & in
-  hw_endpoint_init(0x0, 64, TUSB_XFER_CONTROL);
-  hw_endpoint_init(0x80, 64, TUSB_XFER_CONTROL);
+  // clear non-control hw endpoints
+  tu_memclr(hw_endpoints[1], sizeof(hw_endpoints) - 2*sizeof(hw_endpoint_t));
+  next_buffer_ptr = &usb_dpram->epx_data[0];
 }
 
 static void dcd_rp2040_irq(void)
@@ -233,8 +236,10 @@ static void dcd_rp2040_irq(void)
     {
         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();
+
+        // reset pid to both 1 (data and ack)
+        reset_ep0_pid();
+
         // Pass setup packet to tiny usb
         dcd_event_setup_received(0, setup, true);
         usb_hw_clear->sie_status = USB_SIE_STATUS_SETUP_REC_BITS;
@@ -274,7 +279,7 @@ static void dcd_rp2040_irq(void)
         handled |= USB_INTS_BUS_RESET_BITS;
 
         usb_hw->dev_addr_ctrl = 0;
-        reset_all_endpoints();
+        reset_non_control_endpoints();
         dcd_event_bus_reset(0, TUSB_SPEED_FULL, true);
         usb_hw_clear->sie_status = USB_SIE_STATUS_BUS_RESET_BITS;
 
@@ -325,7 +330,6 @@ static void dcd_rp2040_irq(void)
 
 void dcd_init (uint8_t rhport)
 {
-  pico_trace("dcd_init %d\n", rhport);
   assert(rhport == 0);
 
   // Reset hardware to default state
@@ -338,8 +342,13 @@ void dcd_init (uint8_t rhport)
 
   irq_set_exclusive_handler(USBCTRL_IRQ, dcd_rp2040_irq);
 
-  // reset endpoints
-  reset_all_endpoints();
+  // 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
@@ -370,12 +379,10 @@ void dcd_int_disable(uint8_t rhport)
 
 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);
 
   // Can't set device address in hardware until status xfer has complete
   // Send 0len complete response on EP0 IN
-  reset_ep0();
   hw_endpoint_xfer(0x80, NULL, 0);
 }
 
@@ -389,17 +396,15 @@ void dcd_remote_wakeup(uint8_t rhport)
 // 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) 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) rhport;
+  usb_hw_set->sie_ctrl = USB_SIE_CTRL_PULLUP_EN_BITS;
 }
 
 /*------------------------------------------------------------------*/
@@ -414,11 +419,8 @@ void dcd_edpt0_status_complete(uint8_t rhport, tusb_control_request_t const * re
        request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD &&
        request->bRequest == TUSB_REQ_SET_ADDRESS )
   {
-    pico_trace("Set HW address %d\n", request->wValue);
     usb_hw->dev_addr_ctrl = (uint8_t) request->wValue;
   }
-
-  reset_ep0();
 }
 
 bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
@@ -431,7 +433,9 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
 void dcd_edpt_close_all (uint8_t rhport)
 {
   (void) rhport;
-  // TODO implement dcd_edpt_close_all()
+
+  // may need to use EP Abort
+  reset_non_control_endpoints();
 }
 
 bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
@@ -443,8 +447,7 @@ bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t t
 
 void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
 {
-  pico_trace("dcd_edpt_stall %02x\n", ep_addr);
-  assert(rhport == 0);
+  (void) rhport;
 
   if ( tu_edpt_number(ep_addr) == 0 )
   {
@@ -454,22 +457,22 @@ void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
 
   struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
 
-  // TODO check with double buffered
-  _hw_endpoint_buffer_control_set_mask32(ep, USB_BUF_CTRL_STALL);
+  // 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)
 {
-  pico_trace("dcd_edpt_clear_stall %02x\n", ep_addr);
-  assert(rhport == 0);
+  (void) rhport;
 
   if (tu_edpt_number(ep_addr))
   {
     struct hw_endpoint *ep = hw_endpoint_get_by_addr(ep_addr);
 
-    // clear stall also reset toggle to DATA0
-    // TODO check with double buffered
-    _hw_endpoint_buffer_control_clear_mask32(ep, USB_BUF_CTRL_STALL | USB_BUF_CTRL_DATA1_PID);
+    // 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);
   }
 }
 
diff --git a/src/portable/raspberrypi/rp2040/hcd_rp2040.c b/src/portable/raspberrypi/rp2040/hcd_rp2040.c
index e51dfac2b..5e5bb4906 100644
--- a/src/portable/raspberrypi/rp2040/hcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/hcd_rp2040.c
@@ -201,7 +201,6 @@ static void hcd_rp2040_irq(void)
     {
         handled |= USB_INTS_BUFF_STATUS_BITS;
         TU_LOG(2, "Buffer complete\n");
-        // print_bufctrl32(*epx.buffer_control);
         hw_handle_buff_status();
     }
 
@@ -231,7 +230,7 @@ static void hcd_rp2040_irq(void)
     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);
+        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");
     }
 
diff --git a/src/portable/raspberrypi/rp2040/rp2040_usb.c b/src/portable/raspberrypi/rp2040/rp2040_usb.c
index 43554d28b..f9b4d9b17 100644
--- a/src/portable/raspberrypi/rp2040/rp2040_usb.c
+++ b/src/portable/raspberrypi/rp2040/rp2040_usb.c
@@ -67,7 +67,6 @@ void rp2040_usb_init(void)
 
 void hw_endpoint_reset_transfer(struct hw_endpoint *ep)
 {
-  ep->stalled = false;
   ep->active = false;
   ep->remaining_len = 0;
   ep->xferred_len = 0;
@@ -171,8 +170,7 @@ static void _hw_endpoint_start_next_buffer(struct hw_endpoint *ep)
 
   *ep->endpoint_control = ep_ctrl;
 
-  TU_LOG(3, "Prepare Buffer Control:\r\n");
-  print_bufctrl32(buf_ctrl);
+  TU_LOG(3, "  Prepare BufCtrl: [0] = 0x%04u  [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
@@ -231,7 +229,7 @@ static uint16_t sync_ep_buffer(struct hw_endpoint *ep, uint8_t buf_id)
   // Short packet
   if (xferred_bytes < ep->wMaxPacketSize)
   {
-    pico_trace("Short rx transfer on buffer %d with %u bytes\n", buf_id, xferred_bytes);
+    pico_trace("  Short packet on buffer %d with %u bytes\n", buf_id, xferred_bytes);
     // Reduce total length as this is last packet
     ep->remaining_len = 0;
   }
@@ -245,8 +243,7 @@ static void _hw_endpoint_xfer_sync (struct hw_endpoint *ep)
   // after a buff status interrupt
 
   uint32_t buf_ctrl = _hw_endpoint_buffer_control_get_value32(ep);
-  TU_LOG(3, "_hw_endpoint_xfer_sync:\r\n");
-  print_bufctrl32(buf_ctrl);
+  TU_LOG(3, "  Sync BufCtrl: [0] = 0x%04u  [1] = 0x%04x\r\n", tu_u32_low16(buf_ctrl), tu_u32_high16(buf_ctrl));
 
   // always sync buffer 0
   uint16_t buf0_bytes = sync_ep_buffer(ep, 0);
@@ -284,7 +281,7 @@ static void _hw_endpoint_xfer_sync (struct hw_endpoint *ep)
       usb_hw->abort &= ~TU_BIT(ep_id);
 
       TU_LOG(3, "----SHORT PACKET buffer0 on EP %02X:\r\n", ep->ep_addr);
-      print_bufctrl32(buf_ctrl);
+      TU_LOG(3, "  BufCtrl: [0] = 0x%04u  [1] = 0x%04x\r\n", tu_u32_low16(buf_ctrl), tu_u32_high16(buf_ctrl));
 #endif
     }
   }
diff --git a/src/portable/raspberrypi/rp2040/rp2040_usb.h b/src/portable/raspberrypi/rp2040/rp2040_usb.h
index 5570a731a..a9cf1dd07 100644
--- a/src/portable/raspberrypi/rp2040/rp2040_usb.h
+++ b/src/portable/raspberrypi/rp2040/rp2040_usb.h
@@ -21,7 +21,7 @@
 #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;
@@ -42,9 +42,6 @@ struct hw_endpoint
     // Buffer pointer in usb dpram
     uint8_t *hw_data_buf;
 
-    // Have we been stalled TODO remove later
-    bool stalled;
-
     // Current transfer information
     bool active;
     uint16_t remaining_len;
@@ -66,7 +63,7 @@ struct hw_endpoint
     // If interrupt endpoint
     uint8_t interrupt_num;
 #endif
-};
+} hw_endpoint_t;
 
 void rp2040_usb_init(void);
 
@@ -96,52 +93,4 @@ static inline uintptr_t hw_data_offset(uint8_t *buf)
 
 extern const char *ep_dir_string[];
 
-typedef union TU_ATTR_PACKED
-{
-  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");
-
-#if CFG_TUSB_DEBUG >= 3
-static inline void print_bufctrl16(uint32_t u16)
-{
-  rp2040_buffer_control_t bufctrl = {
-      .u16 = u16
-  };
-
-  TU_LOG(3, "len = %u, available = %u, full = %u, last = %u, stall = %u, reset = %u, toggle = %u\r\n",
-         bufctrl.xfer_len, bufctrl.available, bufctrl.full, bufctrl.last_buf, bufctrl.stall, bufctrl.reset_bufsel, bufctrl.data_toggle);
-}
-
-static inline void print_bufctrl32(uint32_t u32)
-{
-  uint16_t u16;
-
-  u16 = u32 >> 16;
-  TU_LOG(3, "  Buffer Control 1 0x%x: ", u16);
-  print_bufctrl16(u16);
-
-  u16 = u32 & 0x0000ffff;
-  TU_LOG(3, "  Buffer Control 0 0x%x: ", u16);
-  print_bufctrl16(u16);
-}
-
-#else
-
-#define print_bufctrl16(u16)
-#define print_bufctrl32(u32)
-
-#endif
-
 #endif