fix ep tx with double buffered

3 files changed, 25 insertions, 36 deletions

diff --git a/src/common/tusb_common.h b/src/common/tusb_common.h
index fe5bf5f41..8490daad7 100644
--- a/src/common/tusb_common.h
+++ b/src/common/tusb_common.h
@@ -123,7 +123,7 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_align4k (uint32_t value) { retur
 TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_offset4k(uint32_t value) { return (value & 0xFFFUL); }
 
 //------------- Mathematics -------------//
-TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_abs(int32_t value) { return (uint32_t)((value < 0) ? (-value) : value); }
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_div_ceil(uint32_t v, uint32_t d) { return (v + d -1)/d; }
 
 /// inclusive range checking TODO remove
 TU_ATTR_ALWAYS_INLINE static inline bool tu_within(uint32_t lower, uint32_t value, uint32_t upper)
diff --git a/src/portable/raspberrypi/rp2040/dcd_rp2040.c b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
index 63e129e53..4f70d60cc 100644
--- a/src/portable/raspberrypi/rp2040/dcd_rp2040.c
+++ b/src/portable/raspberrypi/rp2040/dcd_rp2040.c
@@ -64,40 +64,30 @@ static struct hw_endpoint *hw_endpoint_get_by_addr(uint8_t ep_addr)
 
 static void _hw_endpoint_alloc(struct hw_endpoint *ep)
 {
-    uint16_t size = tu_min16(64, ep->wMaxPacketSize);
+  // size must be multiple of 64
+  uint16_t size = tu_div_ceil(ep->wMaxPacketSize, 64) * 64u;
 
-    // Assumes single buffered for now
-    ep->hw_data_buf = next_buffer_ptr;
-    next_buffer_ptr += size;
+  // double buffered for non-ISO endpoint
+  if ( ep->transfer_type != TUSB_XFER_ISOCHRONOUS ) size *= 2u;
 
-    // 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);
+  ep->hw_data_buf = next_buffer_ptr;
+  next_buffer_ptr += size;
+
+  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);
 
-    pico_info("Alloced %d bytes at offset 0x%x (0x%p) for ep %d %s\n",
-                size,
-                dpram_offset,
-                ep->hw_data_buf,
-                tu_edpt_number(ep->ep_addr),
-                ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
+  pico_info("Alloced %d bytes at offset 0x%x (0x%p) for ep %d %s\n",
+            size,
+            dpram_offset,
+            ep->hw_data_buf,
+            tu_edpt_number(ep->ep_addr),
+            ep_dir_string[tu_edpt_dir(ep->ep_addr)]);
 
-    // 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;
+  // Fill in endpoint control register with buffer offset
+  uint32_t const reg = EP_CTRL_ENABLE_BITS | (ep->transfer_type << EP_CTRL_BUFFER_TYPE_LSB) | dpram_offset;
 
-    *ep->endpoint_control = reg;
+  *ep->endpoint_control = reg;
 }
 
 static void _hw_endpoint_init(struct hw_endpoint *ep, uint8_t ep_addr, uint16_t wMaxPacketSize, uint8_t transfer_type)
diff --git a/src/portable/raspberrypi/rp2040/rp2040_usb.c b/src/portable/raspberrypi/rp2040/rp2040_usb.c
index de3a83be9..f8d36e5eb 100644
--- a/src/portable/raspberrypi/rp2040/rp2040_usb.c
+++ b/src/portable/raspberrypi/rp2040/rp2040_usb.c
@@ -108,7 +108,8 @@ void _hw_endpoint_buffer_control_update32(struct hw_endpoint *ep, uint32_t and_m
     *ep->buffer_control = value;
 }
 
-static uint32_t compute_buffer_control(struct hw_endpoint *ep, uint8_t buf_id)
+// prepare buffer, return buffer control
+static uint32_t 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 -= buflen;
@@ -122,14 +123,13 @@ static uint32_t compute_buffer_control(struct hw_endpoint *ep, uint8_t buf_id)
   if ( !ep->rx )
   {
     // Copy data from user buffer to hw buffer
-    memcpy(ep->hw_data_buf, ep->user_buf, buflen);
+    memcpy(ep->hw_data_buf + buf_id*64, ep->user_buf, buflen);
     ep->user_buf += buflen;
 
     // Mark as full
     buf_ctrl |= USB_BUF_CTRL_FULL;
   }
 
-#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
@@ -137,7 +137,6 @@ static uint32_t compute_buffer_control(struct hw_endpoint *ep, uint8_t buf_id)
   {
     buf_ctrl |= USB_BUF_CTRL_LAST;
   }
-#endif
 
   if (buf_id) buf_ctrl = buf_ctrl << 16;
 
@@ -150,14 +149,14 @@ static void _hw_endpoint_start_next_buffer(struct hw_endpoint *ep)
   uint32_t ep_ctrl = *ep->endpoint_control;
 
   // always compute buffer 0
-  uint32_t buf_ctrl = compute_buffer_control(ep, 0);
+  uint32_t buf_ctrl = prepare_ep_buffer(ep, 0);
 
   if(ep->remaining_len)
   {
     // Use buffer 1 (double buffered) if there is still data
     // TODO: Isochronous for buffer1 bit-field is different than CBI (control bulk, interrupt)
 
-    buf_ctrl |= compute_buffer_control(ep, 1);
+    buf_ctrl |= prepare_ep_buffer(ep, 1);
 
     // Set endpoint control double buffered bit if needed
     ep_ctrl &= ~EP_CTRL_INTERRUPT_PER_BUFFER;