tinyusb-rp2040-double-buffering.diff

diff --git a/src/portable/raspberrypi/rp2040/dcd_rp2040.c b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
index 80e5d3c4..1df94e63 100644
--- a/src/portable/raspberrypi/rp2040/dcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
@@ -72,8 +72,10 @@ static void _hw_endpoint_alloc(struct hw_endpoint *ep)
     {
         size = ep->wMaxPacketSize;
     }
+    ep->hw_data_buf_len = size;
 
-    // Assumes single buffered for now
+    if (ep->double_buffered)
+        size *= 2;
     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
@@ -99,6 +101,7 @@ static void _hw_endpoint_alloc(struct hw_endpoint *ep)
 
     // Fill in endpoint control register with buffer offset
     uint32_t reg =  EP_CTRL_ENABLE_BITS
+                  | (ep->double_buffered ? EP_CTRL_DOUBLE_BUFFERED_BITS : 0u)
                   | EP_CTRL_INTERRUPT_PER_BUFFER
                   | (ep->transfer_type << EP_CTRL_BUFFER_TYPE_LSB)
                   | dpram_offset;
@@ -134,6 +137,9 @@ static void _hw_endpoint_init(struct hw_endpoint *ep, uint8_t ep_addr, uint wMax
         }
     }
 
+    if (transfer_type == TUSB_XFER_BULK)
+        ep->double_buffered = true;
+
     ep->wMaxPacketSize = wMaxPacketSize;
     ep->transfer_type = transfer_type;
 
@@ -203,10 +209,10 @@ static void hw_endpoint_init(uint8_t ep_addr, uint wMaxPacketSize, uint8_t bmAtt
     _hw_endpoint_init(ep, ep_addr, wMaxPacketSize, bmAttributes);
 }
 
-static void hw_endpoint_xfer(uint8_t ep_addr, uint8_t *buffer, uint16_t total_bytes, bool start)
+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(ep, buffer, total_bytes, start);
+    _hw_endpoint_xfer(ep, buffer, total_bytes);
 }
 
 static void hw_handle_buff_status(void)
@@ -218,15 +224,15 @@ static void hw_handle_buff_status(void)
     {
         if (remaining_buffers & bit)
         {
-            uint __unused which = (usb_hw->buf_cpu_should_handle & bit) ? 1 : 0;
-            // Should be single buffered
-            assert(which == 0);
+            uint which = (usb_hw->buf_cpu_should_handle & bit) ? 1 : 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));
+            if (!ep->double_buffered)
+                assert(which == 0);
             // Continue xfer
-            bool done = _hw_endpoint_xfer_continue(ep);
+            bool done = _hw_endpoint_xfer_continue(ep, which);
             if (done)
             {
                 // Notify
@@ -256,7 +262,7 @@ static void ep0_0len_status(void)
 {
     // Send 0len complete response on EP0 IN
     reset_ep0();
-    hw_endpoint_xfer(0x80, NULL, 0, true);
+    hw_endpoint_xfer(0x80, NULL, 0);
 }
 
 static void _hw_endpoint_stall(struct hw_endpoint *ep)
@@ -267,7 +273,7 @@ static void _hw_endpoint_stall(struct hw_endpoint *ep)
         // A stall on EP0 has to be armed so it can be cleared on the next setup packet
         usb_hw_set->ep_stall_arm = ep->in ? USB_EP_STALL_ARM_EP0_IN_BITS : USB_EP_STALL_ARM_EP0_OUT_BITS;
     }
-    _hw_endpoint_buffer_control_set_mask32(ep, USB_BUF_CTRL_STALL);
+    _hw_endpoint_buffer_control_set_mask16(ep, 0, USB_BUF_CTRL_STALL);
     ep->stalled = true;
 }
 
@@ -284,7 +290,7 @@ static void _hw_endpoint_clear_stall(struct hw_endpoint *ep)
         // Probably already been cleared but no harm
         usb_hw_clear->ep_stall_arm = ep->in ? USB_EP_STALL_ARM_EP0_IN_BITS : USB_EP_STALL_ARM_EP0_OUT_BITS;
     }
-    _hw_endpoint_buffer_control_clear_mask32(ep, USB_BUF_CTRL_STALL);
+    _hw_endpoint_buffer_control_clear_mask16(ep, 0, USB_BUF_CTRL_STALL);
     ep->stalled = false;
 }
 
@@ -370,7 +376,7 @@ void dcd_init (uint8_t rhport)
 
     // Enable individual controller IRQS here. Processor interrupt enable will be used
     // for the global interrupt enable...
-    usb_hw->sie_ctrl = USB_SIE_CTRL_EP0_INT_1BUF_BITS; 
+    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;
 
     dcd_connect(rhport);
@@ -452,8 +458,7 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
 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);
+    hw_endpoint_xfer(ep_addr, buffer, total_bytes);
     return true;
 }
 
diff --git a/src/portable/raspberrypi/rp2040/rp2040_usb.c b/src/portable/raspberrypi/rp2040/rp2040_usb.c
index a44607e9..dc4c8010 100644
--- a/src/portable/raspberrypi/rp2040/rp2040_usb.c
+++ b/src/portable/raspberrypi/rp2040/rp2040_usb.c
@@ -70,20 +70,23 @@ void hw_endpoint_reset_transfer(struct hw_endpoint *ep)
     ep->len = 0;
     ep->transfer_size = 0;
     ep->user_buf = 0;
+    ep->buffer_index = 0;
+    *ep->buffer_control |= USB_BUF_CTRL_SEL;
 }
 
-void _hw_endpoint_buffer_control_update32(struct hw_endpoint *ep, uint32_t and_mask, uint32_t or_mask) {
-    uint32_t value = 0;
+void _hw_endpoint_buffer_control_update16(struct hw_endpoint *ep, uint index, uint16_t and_mask, uint16_t or_mask) {
+    io_rw_16 *ctrl = _hw_endpoint_buffer_control_get_ptr16(ep, index);
+    uint16_t value = 0;
     if (and_mask) {
-        value = *ep->buffer_control & and_mask;
+        value = *ctrl & and_mask;
     }
     if (or_mask) {
         value |= or_mask;
         if (or_mask & USB_BUF_CTRL_AVAIL) {
-            if (*ep->buffer_control & USB_BUF_CTRL_AVAIL) {
+            if (*ctrl & USB_BUF_CTRL_AVAIL) {
                 panic("ep %d %s was already available", ep->num, ep_dir_string[ep->in]);
             }
-            *ep->buffer_control = value & ~USB_BUF_CTRL_AVAIL;
+            *ctrl = 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
 #ifndef RP2040_USB_HOST_MODE
@@ -99,18 +102,30 @@ void _hw_endpoint_buffer_control_update32(struct hw_endpoint *ep, uint32_t and_m
 #endif
         }
     }
-    *ep->buffer_control = value;
+    *ctrl = value;
+    pico_trace("ep%d buffer[%d] control (0x%p) <- 0x%04x\n", ep->num, index, ctrl, value);
 }
 
-void _hw_endpoint_start_next_buffer(struct hw_endpoint *ep)
+static uint8_t *hw_endpoint_data_buf(struct hw_endpoint *ep, uint index)
 {
+    if (index) {
+        return ep->hw_data_buf + ep->hw_data_buf_len;
+    } else {
+        return ep->hw_data_buf;
+    }
+}
+
+void _hw_endpoint_start_next_buffer(struct hw_endpoint *ep, uint index)
+{
+    ep->buffer_index = index;
+
     // Prepare buffer control register value
-    uint32_t val = ep->transfer_size | USB_BUF_CTRL_AVAIL;
+    uint16_t val = ep->transfer_size | USB_BUF_CTRL_AVAIL;
 
     if (!ep->rx)
     {
         // Copy data from user buffer to hw buffer
-        memcpy(ep->hw_data_buf, &ep->user_buf[ep->len], ep->transfer_size);
+        memcpy(hw_endpoint_data_buf(ep, index), &ep->user_buf[ep->len], ep->transfer_size);
         // Mark as full
         val |= USB_BUF_CTRL_FULL;
     }
@@ -130,8 +145,7 @@ void _hw_endpoint_start_next_buffer(struct hw_endpoint *ep)
 
     // 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);
+    _hw_endpoint_buffer_control_set_value16(ep, index, val);
 }
 
 
@@ -158,18 +172,18 @@ void _hw_endpoint_xfer_start(struct hw_endpoint *ep, uint8_t *buffer, uint16_t t
     _hw_endpoint_update_last_buf(ep);
     ep->buf_sel = 0;
 
-    _hw_endpoint_start_next_buffer(ep);
+    _hw_endpoint_start_next_buffer(ep, ep->buffer_index);
     _hw_endpoint_lock_update(ep, -1);
 }
 
-void _hw_endpoint_xfer_sync(struct hw_endpoint *ep)
+void _hw_endpoint_xfer_sync(struct hw_endpoint *ep, uint buf_index)
 {
     // Update hw endpoint struct with info from hardware
     // after a buff status interrupt
 
     // Get the buffer state and amount of bytes we have
     // transferred
-    uint32_t buf_ctrl = _hw_endpoint_buffer_control_get_value32(ep);
+    uint16_t buf_ctrl = _hw_endpoint_buffer_control_get_value16(ep, buf_index);
     uint transferred_bytes = buf_ctrl & USB_BUF_CTRL_LEN_MASK;
 
 #ifdef RP2040_USB_HOST_MODE
@@ -177,7 +191,7 @@ void _hw_endpoint_xfer_sync(struct hw_endpoint *ep)
     if (ep->buf_sel == 1)
     {
         // Host can erroneously write status to top half of buf_ctrl register
-        buf_ctrl = buf_ctrl >> 16;
+        buf_ctrl = *ep->buffer_control >> 16;
     }
     // Flip buf sel for host
     ep->buf_sel ^= 1u;
@@ -189,7 +203,7 @@ void _hw_endpoint_xfer_sync(struct hw_endpoint *ep)
     if (!ep->rx)
     {
         assert(!(buf_ctrl & USB_BUF_CTRL_FULL));
-        pico_trace("tx %d bytes (buf_ctrl 0x%08x)\n", transferred_bytes, buf_ctrl);
+        pico_trace("ep%d tx[%d] %d bytes (buf_ctrl 0x%04x)\n", ep->num, ep->buffer_index, transferred_bytes, buf_ctrl);
         ep->len += transferred_bytes;
     }
     else
@@ -197,9 +211,9 @@ void _hw_endpoint_xfer_sync(struct hw_endpoint *ep)
         // 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);
+        pico_trace("ep%d rx[%d] %d bytes (buf_ctrl 0x%04x)\n", ep->num, ep->buffer_index, transferred_bytes, buf_ctrl);
         assert(buf_ctrl & USB_BUF_CTRL_FULL);
-        memcpy(&ep->user_buf[ep->len], ep->hw_data_buf, transferred_bytes);
+        memcpy(&ep->user_buf[ep->len], hw_endpoint_data_buf(ep, buf_index), transferred_bytes);
         ep->len += transferred_bytes;
     }
 
@@ -215,7 +229,7 @@ void _hw_endpoint_xfer_sync(struct hw_endpoint *ep)
 }
 
 // Returns true if transfer is complete
-bool _hw_endpoint_xfer_continue(struct hw_endpoint *ep)
+bool _hw_endpoint_xfer_continue(struct hw_endpoint *ep, uint buf_index)
 {
     _hw_endpoint_lock_update(ep, 1);
     // Part way through a transfer
@@ -225,7 +239,7 @@ bool _hw_endpoint_xfer_continue(struct hw_endpoint *ep)
     }
 
     // Update EP struct from hardware state
-    _hw_endpoint_xfer_sync(ep);
+    _hw_endpoint_xfer_sync(ep, buf_index);
 
     // Now we have synced our state with the hardware. Is there more data to transfer?
     uint remaining_bytes = ep->total_len - ep->len;
@@ -250,7 +264,9 @@ bool _hw_endpoint_xfer_continue(struct hw_endpoint *ep)
     }
     else
     {
-        _hw_endpoint_start_next_buffer(ep);
+        if (ep->double_buffered)
+            buf_index ^= 1u;
+        _hw_endpoint_start_next_buffer(ep, buf_index);
     }
 
     _hw_endpoint_lock_update(ep, -1);
@@ -258,22 +274,14 @@ bool _hw_endpoint_xfer_continue(struct hw_endpoint *ep)
     return false;
 }
 
-void _hw_endpoint_xfer(struct hw_endpoint *ep, uint8_t *buffer, uint16_t total_len, bool start)
+void _hw_endpoint_xfer(struct hw_endpoint *ep, uint8_t *buffer, uint16_t total_len)
 {
     // Trace
     pico_trace("hw_endpoint_xfer ep %d %s", ep->num, ep_dir_string[ep->in]);
-    pico_trace(" total_len %d, start=%d\n", total_len, start);
+    pico_trace(" total_len %d\n", total_len);
 
     assert(ep->configured);
 
-
-    if (start)
-    {
-        _hw_endpoint_xfer_start(ep, buffer, total_len);
-    }
-    else
-    {
-        _hw_endpoint_xfer_continue(ep);
-    }
+    _hw_endpoint_xfer_start(ep, buffer, total_len);
 }
 
diff --git a/src/portable/raspberrypi/rp2040/rp2040_usb.h b/src/portable/raspberrypi/rp2040/rp2040_usb.h
index 7c3234e8..451e8604 100644
--- a/src/portable/raspberrypi/rp2040/rp2040_usb.h
+++ b/src/portable/raspberrypi/rp2040/rp2040_usb.h
@@ -46,11 +46,11 @@ struct hw_endpoint
     bool in;
     // EP num (not including direction)
     uint8_t num;
-    
+
     // 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;
 
@@ -61,6 +61,13 @@ struct hw_endpoint
 
     // Buffer pointer in usb dpram
     uint8_t *hw_data_buf;
+    // Size of buffer
+    uint hw_data_buf_len;
+
+    // Use double buffered transfers
+    bool double_buffered;
+    // Current buffer index
+    uint buffer_index;
 
     // Have we been stalled
     bool stalled;
@@ -83,7 +90,7 @@ struct hw_endpoint
     uint wMaxPacketSize;
     // Interrupt, bulk, etc
     uint8_t transfer_type;
-    
+
     // Only needed for host
     uint8_t dev_addr;
     bool sent_setup;
@@ -94,23 +101,26 @@ struct hw_endpoint
 void rp2040_usb_init(void);
 
 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(struct hw_endpoint *ep, uint8_t *buffer, uint16_t total_len);
+void _hw_endpoint_start_next_buffer(struct hw_endpoint *ep, uint index);
 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;
+void _hw_endpoint_xfer_sync(struct hw_endpoint *ep, uint buf_index);
+bool _hw_endpoint_xfer_continue(struct hw_endpoint *ep, uint buf_index);
+void _hw_endpoint_buffer_control_update16(struct hw_endpoint *ep, uint index, uint16_t and_mask, uint16_t or_mask);
+static inline io_rw_16 *_hw_endpoint_buffer_control_get_ptr16(struct hw_endpoint *ep, uint index) {
+    return &((io_rw_16 *) ep->buffer_control)[index];
 }
-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 uint16_t _hw_endpoint_buffer_control_get_value16(struct hw_endpoint *ep, uint index) {
+    return *_hw_endpoint_buffer_control_get_ptr16(ep, index);
 }
-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_set_value16(struct hw_endpoint *ep, uint index, uint16_t value) {
+    return _hw_endpoint_buffer_control_update16(ep, index, 0, 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);
+static inline void _hw_endpoint_buffer_control_set_mask16(struct hw_endpoint *ep, uint index, uint16_t value) {
+    return _hw_endpoint_buffer_control_update16(ep, index, ~value, value);
+}
+static inline void _hw_endpoint_buffer_control_clear_mask16(struct hw_endpoint *ep, uint index, uint16_t value) {
+    return _hw_endpoint_buffer_control_update16(ep, index, ~value, 0);
 }
 
 static inline uintptr_t hw_data_offset(uint8_t *buf)