enable dedidcated hwfifo for musb with odd access with 16-bit and 8-bit

3 files changed, 69 insertions, 79 deletions

diff --git a/src/common/tusb_fifo.c b/src/common/tusb_fifo.c
index ee07f66df..a92435912 100644
--- a/src/common/tusb_fifo.c
+++ b/src/common/tusb_fifo.c
@@ -123,8 +123,8 @@ void tu_fifo_set_overwritable(tu_fifo_t *f, bool overwritable) {
   #define HWFIFO_ADDR_NEXT(_hwfifo, _const) HWFIFO_ADDR_NEXT_N(_hwfifo, _const, CFG_TUSB_FIFO_HWFIFO_ADDR_STRIDE)
 
   #ifndef CFG_TUSB_FIFO_HWFIFO_CUSTOM_WRITE
-static void stride_write(volatile void *hwfifo, const void *src, uint8_t data_stride) {
-  #if CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE & 4
+static inline void stride_write(volatile void *hwfifo, const void *src, uint8_t data_stride) {
+    #if CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE & 4
   if (data_stride == 4) {
     *((volatile uint32_t *)hwfifo) = tu_unaligned_read32(src);
   }
@@ -147,6 +147,25 @@ void tu_hwfifo_write(volatile void *hwfifo, const uint8_t *src, uint16_t len, co
     HWFIFO_ADDR_NEXT(hwfifo, );
   }
 
+    #ifdef CFG_TUSB_FIFO_HWFIFO_DATA_ODD_16BIT_ACCESS
+  // 16-bit access is allowed for odd bytes
+  if (len >= 2) {
+    *((volatile uint16_t *)hwfifo) = tu_unaligned_read16(src);
+    src += 2;
+    len -= 2;
+    HWFIFO_ADDR_NEXT_N(hwfifo, , 2);
+  }
+    #endif
+
+    #ifdef CFG_TUSB_FIFO_HWFIFO_DATA_ODD_8BIT_ACCESS
+  // 8-bit access is allowed for odd bytes
+  while (len > 0) {
+    *((volatile uint8_t *)hwfifo) = *src++;
+    len--;
+    HWFIFO_ADDR_NEXT_N(hwfifo, , 1);
+  }
+    #else
+
   // Write odd bytes i.e 1 byte for 16 bit or 1-3 bytes for 32 bit
   if (len > 0) {
     uint32_t tmp = 0u;
@@ -154,21 +173,30 @@ void tu_hwfifo_write(volatile void *hwfifo, const uint8_t *src, uint16_t len, co
     stride_write(hwfifo, &tmp, data_stride);
     HWFIFO_ADDR_NEXT(hwfifo, );
   }
+    #endif
 }
   #endif
 
   #ifndef CFG_TUSB_FIFO_HWFIFO_CUSTOM_READ
-static void stride_read(const volatile void *hwfifo, void *dest, uint8_t data_stride) {
-  #if CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE & 4
-  if (data_stride == 4) {
+static inline void stride_read(const volatile void *hwfifo, void *dest, uint8_t data_stride) {
+  (void)data_stride; // possible unused
+    #if CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE & 4
+      #if CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE != 4
+  if (data_stride == 4)
+      #endif
+  {
     tu_unaligned_write32(dest, *((const volatile uint32_t *)hwfifo));
   }
-  #endif
-  #if CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE & 2
-  if (data_stride == 2) {
+    #endif
+
+    #if CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE & 2
+      #if CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE != 2
+  if (data_stride == 2)
+      #endif
+  {
     tu_unaligned_write16(dest, *((const volatile uint16_t *)hwfifo));
   }
-  #endif
+    #endif
 }
 
 void tu_hwfifo_read(const volatile void *hwfifo, uint8_t *dest, uint16_t len, const tu_hwfifo_access_t *access_mode) {
@@ -181,6 +209,24 @@ void tu_hwfifo_read(const volatile void *hwfifo, uint8_t *dest, uint16_t len, co
     HWFIFO_ADDR_NEXT(hwfifo, const);
   }
 
+    #ifdef CFG_TUSB_FIFO_HWFIFO_DATA_ODD_16BIT_ACCESS
+  // 16-bit access is allowed for odd bytes
+  if (len >= 2) {
+    tu_unaligned_write16(dest, *((const volatile uint16_t *)hwfifo));
+    dest += 2;
+    len -= 2;
+    HWFIFO_ADDR_NEXT_N(hwfifo, const, 2);
+  }
+    #endif
+
+    #ifdef CFG_TUSB_FIFO_HWFIFO_DATA_ODD_8BIT_ACCESS
+  // 8-bit access is allowed for odd bytes
+  while (len > 0) {
+    *dest++ = *((const volatile uint8_t *)hwfifo);
+    len--;
+    HWFIFO_ADDR_NEXT_N(hwfifo, const, 1);
+  }
+    #else
   // Read odd bytes i.e 1 byte for 16 bit or 1-3 bytes for 32 bit
   if (len > 0) {
     uint32_t tmp;
@@ -188,6 +234,7 @@ void tu_hwfifo_read(const volatile void *hwfifo, uint8_t *dest, uint16_t len, co
     memcpy(dest, &tmp, len);
     HWFIFO_ADDR_NEXT(hwfifo, const);
   }
+    #endif
 }
   #endif
 
diff --git a/src/portable/mentor/musb/dcd_musb.c b/src/portable/mentor/musb/dcd_musb.c
index ad20d64bd..3827be318 100644
--- a/src/portable/mentor/musb/dcd_musb.c
+++ b/src/portable/mentor/musb/dcd_musb.c
@@ -170,68 +170,6 @@ TU_ATTR_ALWAYS_INLINE static inline void hwfifo_flush(musb_regs_t* musb, unsigne
   }
 }
 
-static void pipe_write_packet(void *buf, volatile void *fifo, unsigned len)
-{
-  volatile hw_fifo_t *reg = (volatile hw_fifo_t*)fifo;
-  uintptr_t addr = (uintptr_t)buf;
-  while (len >= 4) {
-    reg->u32 = *(uint32_t const *)addr;
-    addr += 4;
-    len  -= 4;
-  }
-  if (len >= 2) {
-    reg->u16 = *(uint16_t const *)addr;
-    addr += 2;
-    len  -= 2;
-  }
-  if (len) {
-    reg->u8 = *(uint8_t const *)addr;
-  }
-}
-
-static void pipe_read_packet(void *buf, volatile void *fifo, unsigned len)
-{
-  volatile hw_fifo_t *reg = (volatile hw_fifo_t*)fifo;
-  uintptr_t addr = (uintptr_t)buf;
-  while (len >= 4) {
-    *(uint32_t *)addr = reg->u32;
-    addr += 4;
-    len  -= 4;
-  }
-  if (len >= 2) {
-    *(uint16_t *)addr = reg->u16;
-    addr += 2;
-    len  -= 2;
-  }
-  if (len) {
-    *(uint8_t *)addr = reg->u8;
-  }
-}
-
-static void pipe_read_write_packet_ff(tu_fifo_t *f, volatile void *fifo, unsigned len, unsigned dir)
-{
-  static const struct {
-    void (*tu_fifo_get_info)(tu_fifo_t *f, tu_fifo_buffer_info_t *info);
-    void (*tu_fifo_advance)(tu_fifo_t *f, uint16_t n);
-    void (*pipe_read_write)(void *buf, volatile void *fifo, unsigned len);
-  } ops[] = {
-    /* OUT */ {tu_fifo_get_write_info,tu_fifo_advance_write_pointer,pipe_read_packet},
-    /* IN  */ {tu_fifo_get_read_info, tu_fifo_advance_read_pointer, pipe_write_packet},
-  };
-  tu_fifo_buffer_info_t info;
-  ops[dir].tu_fifo_get_info(f, &info);
-  unsigned total_len = len;
-  len = TU_MIN(total_len, info.linear.len);
-  ops[dir].pipe_read_write(info.linear.ptr, fifo, len);
-  unsigned rem = total_len - len;
-  if (rem) {
-    len = TU_MIN(rem, info.wrapped.len);
-    ops[dir].pipe_read_write(info.wrapped.ptr, fifo, len);
-    rem -= len;
-  }
-  ops[dir].tu_fifo_advance(f, total_len - rem);
-}
-
 static void process_setup_packet(uint8_t rhport) {
   musb_regs_t* musb_regs = MUSB_REGS(rhport);
 
@@ -277,9 +215,9 @@ static bool handle_xfer_in(uint8_t rhport, uint_fast8_t ep_addr)
   // TU_LOG1("   %p mps %d len %d rem %d\r\n", buf, mps, len, rem);
   if (len) {
     if (_dcd.pipe_buf_is_fifo[TUSB_DIR_IN] & TU_BIT(epnum_minus1)) {
-      pipe_read_write_packet_ff(buf, fifo_ptr, len, TUSB_DIR_IN);
+      tu_hwfifo_write_from_fifo(fifo_ptr, (tu_fifo_t *)buf, len, NULL);
     } else {
-      pipe_write_packet(buf, fifo_ptr, len);
+      tu_hwfifo_write(fifo_ptr, buf, len, NULL);
       pipe->buf       = buf + len;
     }
     pipe->remaining = rem - len;
@@ -308,9 +246,9 @@ static bool handle_xfer_out(uint8_t rhport, uint_fast8_t ep_addr)
   volatile void *fifo_ptr = &musb_regs->fifo[epnum];
   if (len) {
     if (_dcd.pipe_buf_is_fifo[TUSB_DIR_OUT] & TU_BIT(epnum_minus1)) {
-      pipe_read_write_packet_ff(buf, fifo_ptr, len, TUSB_DIR_OUT);
+      tu_hwfifo_read_to_fifo(fifo_ptr, (tu_fifo_t *)buf, len, NULL);
     } else {
-      pipe_read_packet(buf, fifo_ptr, len);
+      tu_hwfifo_read(fifo_ptr, buf, len, NULL);
       pipe->buf       = buf + len;
     }
     pipe->remaining = rem - len;
@@ -378,7 +316,7 @@ static bool edpt0_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t *buffer, uint16_
     const unsigned len = TU_MIN(TU_MIN(rem, 64), total_bytes);
     volatile void *fifo_ptr = &musb_regs->fifo[0];
     if (dir_in) {
-      pipe_write_packet(buffer, fifo_ptr, len);
+      tu_hwfifo_write(fifo_ptr, buffer, len, NULL);
 
       _dcd.pipe0.buf       = buffer + len;
       _dcd.pipe0.length    = len;
@@ -458,7 +396,7 @@ static void process_ep0(uint8_t rhport)
       const unsigned rem = _dcd.pipe0.remaining;
       const unsigned len = TU_MIN(TU_MIN(rem, 64), vld);
       volatile void *fifo_ptr = &musb_regs->fifo[0];
-      pipe_read_packet(_dcd.pipe0.buf, fifo_ptr, len);
+      tu_hwfifo_read(fifo_ptr, _dcd.pipe0.buf, len, NULL);
 
       _dcd.pipe0.remaining = rem - len;
       _dcd.remaining_ctrl -= len;
diff --git a/src/tusb_option.h b/src/tusb_option.h
index 453dddb7c..87aba6a6c 100644
--- a/src/tusb_option.h
+++ b/src/tusb_option.h
@@ -365,13 +365,18 @@
 
 //------------ MUSB --------------//
 #if defined(TUP_USBIP_MUSB)
-  #define CFG_TUD_EDPT_DEDICATED_HWFIFO 0 // need testing to enable
+  #define CFG_TUD_EDPT_DEDICATED_HWFIFO              1
+  #define CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE           4 // 32 bit data
+  #define CFG_TUSB_FIFO_HWFIFO_DATA_ODD_16BIT_ACCESS   // allow odd 16bit access
+  #define CFG_TUSB_FIFO_HWFIFO_DATA_ODD_8BIT_ACCESS    // allow odd 8bit access
+  #define CFG_TUSB_FIFO_HWFIFO_ADDR_STRIDE           0 // fixed hwfifo
+
 #endif
 
 //------------ RUSB2 --------------//
 #if defined(TUP_USBIP_RUSB2)
   #define CFG_TUD_EDPT_DEDICATED_HWFIFO     1
-  #define CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE  (2 + (TUD_OPT_HIGH_SPEED ? 4 : 0)) // 16 bit and 32 bit data if highspeed
+  #define CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE  (2 | (TUD_OPT_HIGH_SPEED ? 4 : 0)) // 16 bit and 32 bit data if highspeed
   #define CFG_TUSB_FIFO_HWFIFO_ADDR_STRIDE  0
   #define CFG_TUSB_FIFO_HWFIFO_CUSTOM_WRITE // custom write since rusb2 can change access width 32 -> 16 and can write
                                             // odd byte with byte access