support multiple data stride if configured

5 files changed, 206 insertions, 97 deletions

diff --git a/src/common/tusb_common.h b/src/common/tusb_common.h
index d74760608..17cd26cd9 100644
--- a/src/common/tusb_common.h
+++ b/src/common/tusb_common.h
@@ -348,7 +348,7 @@ TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_scatter_read32(const uint8_t *bu
   return result;
 }
 
-// scatter write 4 bytes to two buffers. Parameter are not checked
+// scatter write 4 bytes (LE) to two buffers. Parameter are not checked
 TU_ATTR_ALWAYS_INLINE static inline void tu_scatter_write32(uint32_t value, uint8_t *buf1, uint8_t len1,
                                                             uint8_t *buf2, uint8_t len2) {
   for (uint8_t i = 0; i < len1; ++i) {
diff --git a/src/common/tusb_fifo.c b/src/common/tusb_fifo.c
index fe17f1f2b..5749f3a68 100644
--- a/src/common/tusb_fifo.c
+++ b/src/common/tusb_fifo.c
@@ -114,119 +114,136 @@ void tu_fifo_set_overwritable(tu_fifo_t *f, bool overwritable) {
 // copy data to/from fifo without updating read/write pointers
 //--------------------------------------------------------------------+
 #if CFG_TUSB_FIFO_HWFIFO_API
-  #if CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE == 4
-    #define stride_unaligned_write tu_unaligned_write32
-    #define stride_unaligned_read  tu_unaligned_read32
-typedef uint32_t hwfifo_item_t;
-  #elif CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE == 2
-    #define stride_unaligned_write tu_unaligned_write16
-    #define stride_unaligned_read  tu_unaligned_read16
-typedef uint16_t hwfifo_item_t;
+
+  #if CFG_TUSB_FIFO_HWFIFO_ADDR_STRIDE
+    #define HWFIFO_ADDR_NEXT(_const, _hwfifo)                                                     \
+      _hwfifo = (_const volatile void *)((uintptr_t)(_hwfifo) + CFG_TUSB_FIFO_HWFIFO_ADDR_STRIDE)
+  #else
+    #define HWFIFO_ADDR_NEXT(_const, _hwfifo)
   #endif
 
-enum {
-  STRIDE_REMAIN_MASK = sizeof(hwfifo_item_t) - 1u
-};
+static 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);
+  }
+  #endif
+  #if CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE & 2
+  if (data_stride == 2) {
+    *((volatile uint16_t *)hwfifo) = tu_unaligned_read16(src);
+  }
+  #endif
+}
 
-void tu_hwfifo_read(const volatile void *hwfifo, uint8_t *dest, uint16_t len) {
-  const volatile hwfifo_item_t *src = (const volatile hwfifo_item_t *)hwfifo;
+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) {
+    tu_unaligned_write32(dest, *((const volatile uint32_t *)hwfifo));
+  }
+  #endif
+  #if CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE & 2
+  if (data_stride == 2) {
+    tu_unaligned_write16(dest, *((const volatile uint16_t *)hwfifo));
+  }
+  #endif
+}
 
+void tu_hwfifo_read_access_mode(const volatile void *hwfifo, uint8_t *dest, uint16_t len, uint8_t data_stride) {
   // Reading full available 16/32-bit hwfifo and write to fifo
-  while (len >= sizeof(hwfifo_item_t)) {
-    const hwfifo_item_t tmp = *src;
-    stride_unaligned_write(dest, tmp);
-    dest += sizeof(hwfifo_item_t);
-    len -= sizeof(hwfifo_item_t);
+  while (len >= data_stride) {
+    stride_read(hwfifo, dest, data_stride);
+    dest += data_stride;
+    len -= data_stride;
 
-  #if CFG_TUSB_FIFO_HWFIFO_ADDR_STRIDE
-    src = (const volatile hwfifo_item_t *)((uintptr_t)src + CFG_TUSB_FIFO_HWFIFO_ADDR_STRIDE);
-  #endif
+    HWFIFO_ADDR_NEXT(const, hwfifo);
   }
 
   // Read odd bytes i.e 1 byte for 16 bit or 1-3 bytes for 32 bit
   if (len > 0) {
   #ifdef CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE_ODD_BYTE_SUPPORT
     // odd byte access, read byte per byte e.g for rusb2. No address stride needed
-    const volatile uint8_t *src8 = (const volatile uint8_t *)src;
+    const volatile uint8_t *src8 = (const volatile uint8_t *)hwfifo;
     for (uint16_t i = 0; i < len; ++i) {
-      dest[i] = *src8;
+      dest[i] = *(src8 + 3);
     }
   #else
-    const hwfifo_item_t tmp = *src;
+    uint32_t tmp;
+    stride_read(hwfifo, &tmp, data_stride);
     memcpy(dest, &tmp, len);
   #endif
   }
 }
 
 // Copy from fifo to fixed address buffer (usually a tx register) with TU_FIFO_FIXED_ADDR_RW32 mode
-void tu_hwfifo_write(volatile void *hwfifo, const uint8_t *src, uint16_t len) {
-  volatile hwfifo_item_t *dest = (volatile hwfifo_item_t *)hwfifo;
-
+void tu_hwfifo_write_access_mode(volatile void *hwfifo, const uint8_t *src, uint16_t len, uint8_t data_stride) {
   // Write full available 16/32 bit words to dest
-  while (len >= sizeof(hwfifo_item_t)) {
-    *dest = stride_unaligned_read(src);
-    src += sizeof(hwfifo_item_t);
-    len -= sizeof(hwfifo_item_t);
+  while (len >= data_stride) {
+    stride_write(hwfifo, src, data_stride);
+    src += data_stride;
+    len -= data_stride;
 
-  #if CFG_TUSB_FIFO_HWFIFO_ADDR_STRIDE
-    dest = (volatile hwfifo_item_t *)((uintptr_t)dest + CFG_TUSB_FIFO_HWFIFO_ADDR_STRIDE);
-  #endif
+    HWFIFO_ADDR_NEXT(, hwfifo);
   }
 
   // Write odd bytes i.e 1 byte for 16 bit or 1-3 bytes for 32 bit
   if (len > 0) {
   #ifdef CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE_ODD_BYTE_SUPPORT
     // odd byte access, write byte per byte e.g for rusb2. No address stride needed
-    volatile uint8_t *dest8 = (volatile uint8_t *)dest;
+    volatile uint8_t *dest8 = (volatile uint8_t *)hwfifo;
     for (uint16_t i = 0; i < len; ++i) {
-      *dest8 = src[i];
+      *(dest8 + 3) = src[i];
     }
   #else
-    hwfifo_item_t tmp = 0u;
+    uint32_t tmp = 0u;
     memcpy(&tmp, src, len);
-    *dest = tmp;
+    stride_write(hwfifo, &tmp, data_stride);
   #endif
   }
 }
 #endif
 
 // send n items to fifo WITHOUT updating write pointer
-static void ff_push_n(const tu_fifo_t *f, const void *app_buf, uint16_t n, uint16_t wr_ptr, bool stride_mode) {
-  (void)stride_mode;
+static void ff_push_n(const tu_fifo_t *f, const void *app_buf, uint16_t n, uint16_t wr_ptr, uint8_t data_stride) {
+  (void)data_stride;
   uint16_t lin_bytes  = f->depth - wr_ptr;
   uint16_t wrap_bytes = n - lin_bytes;
   uint8_t *ff_buf     = f->buffer + wr_ptr;
 
 #if CFG_TUSB_FIFO_HWFIFO_API
-  if (stride_mode) {
-    const volatile hwfifo_item_t *hwfifo = (const volatile hwfifo_item_t *)app_buf;
+  if (data_stride) {
+    const volatile void *hwfifo = (const volatile void *)app_buf;
     if (n <= lin_bytes) {
       // Linear only case
-      tu_hwfifo_read(hwfifo, ff_buf, n);
+      tu_hwfifo_read_access_mode(hwfifo, ff_buf, n, data_stride);
     } else {
       // Wrap around case
 
       // Write full words to linear part of buffer
-      uint16_t lin_nitems_bytes = lin_bytes & ~STRIDE_REMAIN_MASK;
-      tu_hwfifo_read(hwfifo, ff_buf, lin_nitems_bytes);
-      ff_buf += lin_nitems_bytes;
+      const uint32_t odd_mask = data_stride - 1;
+      uint16_t       lin_even = lin_bytes & ~odd_mask;
+      tu_hwfifo_read_access_mode(hwfifo, ff_buf, lin_even, data_stride);
+      ff_buf += lin_even;
 
       // There could be odd 1 byte (16bit) or 1-3 bytes (32bit) before the wrap-around boundary
-      const uint8_t rem = lin_bytes & STRIDE_REMAIN_MASK;
-      if (rem > 0) {
-        const uint8_t       remrem = (uint8_t)tu_min16(wrap_bytes, sizeof(hwfifo_item_t) - rem);
-        const hwfifo_item_t tmp    = *hwfifo;
-        tu_scatter_write32(tmp, ff_buf, rem, f->buffer, remrem);
+      // combine it with the wrapped part to form a full word for data stride
+      const uint8_t lin_odd = lin_bytes & odd_mask;
+      if (lin_odd > 0) {
+        const uint8_t wrap_odd = (uint8_t)tu_min16(wrap_bytes, data_stride - lin_odd);
+        uint32_t      tmp      = 0;
+        stride_read(hwfifo, &tmp, data_stride);
+        HWFIFO_ADDR_NEXT(const, hwfifo);
+
+        tu_scatter_write32(tmp, ff_buf, lin_odd, f->buffer, wrap_odd);
 
-        wrap_bytes -= remrem;
-        ff_buf = f->buffer + remrem; // wrap around
+        wrap_bytes -= wrap_odd;
+        ff_buf = f->buffer + wrap_odd; // wrap around
       } else {
-        ff_buf = f->buffer;          // wrap around to beginning
+        ff_buf = f->buffer;            // wrap around to beginning
       }
 
       // Write data wrapped part
       if (wrap_bytes > 0) {
-        tu_hwfifo_read(hwfifo, ff_buf, wrap_bytes);
+        tu_hwfifo_read_access_mode(hwfifo, ff_buf, wrap_bytes, data_stride);
       }
     }
   } else
@@ -245,44 +262,46 @@ static void ff_push_n(const tu_fifo_t *f, const void *app_buf, uint16_t n, uint1
 }
 
 // get n items from fifo WITHOUT updating read pointer
-static void ff_pull_n(const tu_fifo_t *f, void *app_buf, uint16_t n, uint16_t rd_ptr, bool stride_mode) {
-  (void)stride_mode;
+static void ff_pull_n(const tu_fifo_t *f, void *app_buf, uint16_t n, uint16_t rd_ptr, uint8_t data_stride) {
+  (void)data_stride;
   uint16_t       lin_bytes  = f->depth - rd_ptr;
   uint16_t       wrap_bytes = n - lin_bytes; // only used if wrapped
   const uint8_t *ff_buf     = f->buffer + rd_ptr;
 
 #if CFG_TUSB_FIFO_HWFIFO_API
-  if (stride_mode) {
-    volatile hwfifo_item_t *hwfifo = (volatile hwfifo_item_t *)app_buf;
+  if (data_stride) {
+    volatile void *hwfifo = (volatile void *)app_buf;
 
     if (n <= lin_bytes) {
       // Linear only case
-      tu_hwfifo_write(hwfifo, ff_buf, n);
+      tu_hwfifo_write_access_mode(hwfifo, ff_buf, n, data_stride);
     } else {
       // Wrap around case
 
       // Read full words from linear part
-      uint16_t lin_nitems_bytes = lin_bytes & ~STRIDE_REMAIN_MASK;
-      tu_hwfifo_write(hwfifo, ff_buf, lin_nitems_bytes);
-      ff_buf += lin_nitems_bytes;
+      const uint32_t odd_mask = data_stride - 1;
+      uint16_t       lin_even = lin_bytes & ~odd_mask;
+      tu_hwfifo_write_access_mode(hwfifo, ff_buf, lin_even, data_stride);
+      ff_buf += lin_even;
 
       // There could be odd 1 byte (16bit) or 1-3 bytes (32bit) before the wrap-around boundary
-      const uint8_t rem = lin_bytes & STRIDE_REMAIN_MASK;
-      if (rem > 0) {
-        const uint8_t       remrem  = (uint8_t)tu_min16(wrap_bytes, sizeof(hwfifo_item_t) - rem);
-        const hwfifo_item_t scatter = (hwfifo_item_t)tu_scatter_read32(ff_buf, rem, f->buffer, remrem);
+      const uint8_t lin_odd = lin_bytes & odd_mask;
+      if (lin_odd > 0) {
+        const uint8_t  wrap_odd = (uint8_t)tu_min16(wrap_bytes, data_stride - lin_odd);
+        const uint32_t scatter  = tu_scatter_read32(ff_buf, lin_odd, f->buffer, wrap_odd);
 
-        *hwfifo = scatter;
+        stride_write(hwfifo, &scatter, data_stride);
+        HWFIFO_ADDR_NEXT(, hwfifo);
 
-        wrap_bytes -= remrem;
-        ff_buf = f->buffer + remrem; // wrap around
+        wrap_bytes -= wrap_odd;
+        ff_buf = f->buffer + wrap_odd; // wrap around
       } else {
-        ff_buf = f->buffer;          // wrap around to beginning
+        ff_buf = f->buffer;            // wrap around to beginning
       }
 
       // Read data wrapped part
       if (wrap_bytes > 0) {
-        tu_hwfifo_write(hwfifo, ff_buf, wrap_bytes);
+        tu_hwfifo_write_access_mode(hwfifo, ff_buf, wrap_bytes, data_stride);
       }
     }
   } else
@@ -349,7 +368,7 @@ static uint16_t correct_read_index(tu_fifo_t *f, uint16_t wr_idx) {
 // Works on local copies of w and r
 // Must be protected by read mutex since in case of an overflow read pointer gets modified
 uint16_t tu_fifo_peek_n_access_mode(tu_fifo_t *f, void *p_buffer, uint16_t n, uint16_t wr_idx, uint16_t rd_idx,
-                                    bool stride_mode) {
+                                    uint8_t data_stride) {
   uint16_t count = tu_ff_overflow_count(f->depth, wr_idx, rd_idx);
   if (count == 0) {
     return 0; // nothing to peek
@@ -366,7 +385,7 @@ uint16_t tu_fifo_peek_n_access_mode(tu_fifo_t *f, void *p_buffer, uint16_t n, ui
   }
 
   const uint16_t rd_ptr = idx2ptr(f->depth, rd_idx);
-  ff_pull_n(f, p_buffer, n, rd_ptr, stride_mode);
+  ff_pull_n(f, p_buffer, n, rd_ptr, data_stride);
 
   return n;
 }
@@ -374,17 +393,17 @@ uint16_t tu_fifo_peek_n_access_mode(tu_fifo_t *f, void *p_buffer, uint16_t n, ui
 // Read n items without removing it from the FIFO, correct read pointer if overflowed
 uint16_t tu_fifo_peek_n(tu_fifo_t *f, void *p_buffer, uint16_t n) {
   ff_lock(f->mutex_rd);
-  const uint16_t ret = tu_fifo_peek_n_access_mode(f, p_buffer, n, f->wr_idx, f->rd_idx, false);
+  const uint16_t ret = tu_fifo_peek_n_access_mode(f, p_buffer, n, f->wr_idx, f->rd_idx, 0);
   ff_unlock(f->mutex_rd);
   return ret;
 }
 
 // Read n items from fifo with access mode
-uint16_t tu_fifo_read_n_access_mode(tu_fifo_t *f, void *buffer, uint16_t n, bool stride_mode) {
+uint16_t tu_fifo_read_n_access_mode(tu_fifo_t *f, void *buffer, uint16_t n, uint8_t data_stride) {
   ff_lock(f->mutex_rd);
 
   // Peek the data: f->rd_idx might get modified in case of an overflow so we can not use a local variable
-  n         = tu_fifo_peek_n_access_mode(f, buffer, n, f->wr_idx, f->rd_idx, stride_mode);
+  n         = tu_fifo_peek_n_access_mode(f, buffer, n, f->wr_idx, f->rd_idx, data_stride);
   f->rd_idx = advance_index(f->depth, f->rd_idx, n);
 
   ff_unlock(f->mutex_rd);
@@ -392,7 +411,7 @@ uint16_t tu_fifo_read_n_access_mode(tu_fifo_t *f, void *buffer, uint16_t n, bool
 }
 
 // Write n items to fifo with access mode
-uint16_t tu_fifo_write_n_access_mode(tu_fifo_t *f, const void *data, uint16_t n, bool stride_mode) {
+uint16_t tu_fifo_write_n_access_mode(tu_fifo_t *f, const void *data, uint16_t n, uint8_t data_stride) {
   if (n == 0) {
     return 0;
   }
@@ -418,7 +437,7 @@ uint16_t tu_fifo_write_n_access_mode(tu_fifo_t *f, const void *data, uint16_t n,
     // function! Since it would end up in a race condition with read functions!
     if (n >= f->depth) {
       // Only copy last part
-      if (!stride_mode) {
+      if (!data_stride) {
         buf8 += (n - f->depth);
       } else {
         // TODO should read from hw fifo to discard data, however reading an odd number could
@@ -454,7 +473,7 @@ uint16_t tu_fifo_write_n_access_mode(tu_fifo_t *f, const void *data, uint16_t n,
     const uint16_t wr_ptr = idx2ptr(f->depth, wr_idx);
     TU_LOG(TU_FIFO_DBG, "actual_n = %u, wr_ptr = %u", n, wr_ptr);
 
-    ff_push_n(f, buf8, n, wr_ptr, stride_mode);
+    ff_push_n(f, buf8, n, wr_ptr, data_stride);
     f->wr_idx = advance_index(f->depth, wr_idx, n);
 
     TU_LOG(TU_FIFO_DBG, "\tnew_wr = %u\r\n", f->wr_idx);
diff --git a/src/common/tusb_fifo.h b/src/common/tusb_fifo.h
index e1ea1126c..f2939aec0 100644
--- a/src/common/tusb_fifo.h
+++ b/src/common/tusb_fifo.h
@@ -193,7 +193,7 @@ void tu_fifo_get_write_info(tu_fifo_t *f, tu_fifo_buffer_info_t *info);
 // peek() will correct/re-index read pointer in case of an overflowed fifo to form a full fifo
 //--------------------------------------------------------------------+
 uint16_t tu_fifo_peek_n_access_mode(tu_fifo_t *f, void *p_buffer, uint16_t n, uint16_t wr_idx, uint16_t rd_idx,
-                                    bool stride_mode);
+                                    uint8_t data_stride);
 bool     tu_fifo_peek(tu_fifo_t *f, void *p_buffer);
 uint16_t tu_fifo_peek_n(tu_fifo_t *f, void *p_buffer, uint16_t n);
 
@@ -201,10 +201,10 @@ uint16_t tu_fifo_peek_n(tu_fifo_t *f, void *p_buffer, uint16_t n);
 // Read API
 // peek() + advance read index
 //--------------------------------------------------------------------+
-uint16_t tu_fifo_read_n_access_mode(tu_fifo_t *f, void *buffer, uint16_t n, bool stride_mode);
+uint16_t tu_fifo_read_n_access_mode(tu_fifo_t *f, void *buffer, uint16_t n, uint8_t data_stride);
 bool     tu_fifo_read(tu_fifo_t *f, void *buffer);
 TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_fifo_read_n(tu_fifo_t *f, void *buffer, uint16_t n) {
-  return tu_fifo_read_n_access_mode(f, buffer, n, false);
+  return tu_fifo_read_n_access_mode(f, buffer, n, 0);
 }
 
 // discard first n items from fifo i.e advance read pointer by n with mutex
@@ -214,10 +214,10 @@ uint16_t tu_fifo_discard_n(tu_fifo_t *f, uint16_t n);
 //--------------------------------------------------------------------+
 // Write API
 //--------------------------------------------------------------------+
-uint16_t tu_fifo_write_n_access_mode(tu_fifo_t *f, const void *data, uint16_t n, bool stride_mode);
+uint16_t tu_fifo_write_n_access_mode(tu_fifo_t *f, const void *data, uint16_t n, uint8_t data_stride);
 bool     tu_fifo_write(tu_fifo_t *f, const void *data);
 TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_fifo_write_n(tu_fifo_t *f, const void *data, uint16_t n) {
-  return tu_fifo_write_n_access_mode(f, data, n, false);
+  return tu_fifo_write_n_access_mode(f, data, n, 0);
 }
 
 //--------------------------------------------------------------------+
@@ -228,20 +228,30 @@ TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_fifo_write_n(tu_fifo_t *f, const
 //--------------------------------------------------------------------+
 TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_hwfifo_write_from_fifo(volatile void *hwfifo, tu_fifo_t *f,
                                                                        uint16_t n) {
-  return tu_fifo_read_n_access_mode(f, (void *)(uintptr_t)hwfifo, n, true);
+  return tu_fifo_read_n_access_mode(f, (void *)(uintptr_t)hwfifo, n, CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE);
 }
 
 TU_ATTR_ALWAYS_INLINE static inline uint16_t tu_hwfifo_read_to_fifo(const volatile void *hwfifo, tu_fifo_t *f,
                                                                     uint16_t n) {
-  return tu_fifo_write_n_access_mode(f, (const void *)(uintptr_t)hwfifo, n, true);
+  return tu_fifo_write_n_access_mode(f, (const void *)(uintptr_t)hwfifo, n, CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE);
 }
 
 #if CFG_TUSB_FIFO_HWFIFO_API
-// read from hwfifo to buffer
-void tu_hwfifo_read(const volatile void *hwfifo, uint8_t *dest, uint16_t len);
+// read from hwfifo to buffer with access mode
+void tu_hwfifo_read_access_mode(const volatile void *hwfifo, uint8_t *dest, uint16_t len, uint8_t data_stride);
 
-// write to hwfifo from buffer
-void tu_hwfifo_write(volatile void *hwfifo, const uint8_t *src, uint16_t len);
+// read from hwfifo to buffer with default data stride
+TU_ATTR_ALWAYS_INLINE static inline void tu_hwfifo_read(const volatile void *hwfifo, uint8_t *dest, uint16_t len) {
+  tu_hwfifo_read_access_mode(hwfifo, dest, len, CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE);
+}
+
+// write to hwfifo from buffer with access mode
+void tu_hwfifo_write_access_mode(volatile void *hwfifo, const uint8_t *src, uint16_t len, uint8_t data_stride);
+
+// write to hwfifo from buffer with default data stride
+TU_ATTR_ALWAYS_INLINE static inline void tu_hwfifo_write(volatile void *hwfifo, const uint8_t *src, uint16_t len) {
+  tu_hwfifo_write_access_mode(hwfifo, src, len, CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE);
+}
 #endif
 
 //--------------------------------------------------------------------+
diff --git a/test/unit-test/project.yml b/test/unit-test/project.yml
index 186eacbf0..be3fc3de0 100644
--- a/test/unit-test/project.yml
+++ b/test/unit-test/project.yml
@@ -129,7 +129,7 @@
   :test:
     - _UNITY_TEST_
     - CFG_TUD_EDPT_DEDICATED_HWFIFO=1
-    - CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE=4
+    - CFG_TUSB_FIFO_HWFIFO_DATA_STRIDE=6
     - CFG_TUSB_FIFO_HWFIFO_ADDR_STRIDE=0
   :release: []
 
diff --git a/test/unit-test/test/test_fifo.c b/test/unit-test/test/test_fifo.c
index 453930a2f..6e1c13d6d 100644
--- a/test/unit-test/test/test_fifo.c
+++ b/test/unit-test/test/test_fifo.c
@@ -403,7 +403,7 @@ void test_write_n_fixed_addr_rw32_nowrap(void) {
 
   for (uint8_t n = 1; n <= 8; n++) {
     tu_fifo_clear(ff);
-    uint16_t written = tu_fifo_write_n_access_mode(ff, (const void *)&reg, n, true);
+    uint16_t written = tu_fifo_write_n_access_mode(ff, (const void *)&reg, n, 4);
     TEST_ASSERT_EQUAL(n, written);
     TEST_ASSERT_EQUAL(n, tu_fifo_count(ff));
 
@@ -425,7 +425,7 @@ void test_write_n_fixed_addr_rw32_wrapped(void) {
     ff->wr_idx = FIFO_SIZE - 3;
     ff->rd_idx = FIFO_SIZE - 3;
 
-    uint16_t written = tu_fifo_write_n_access_mode(ff, (const void *)&reg, n, true);
+    uint16_t written = tu_fifo_write_n_access_mode(ff, (const void *)&reg, n, 4);
     TEST_ASSERT_EQUAL(n, written);
     TEST_ASSERT_EQUAL(n, tu_fifo_count(ff));
 
@@ -445,7 +445,7 @@ void test_read_n_fixed_addr_rw32_nowrap(void) {
     tu_fifo_write_n(ff, pattern, 8);
 
     uint32_t reg      = 0;
-    uint16_t read_cnt = tu_fifo_read_n_access_mode(ff, &reg, n, true);
+    uint16_t read_cnt = tu_fifo_read_n_access_mode(ff, &reg, n, 4);
     TEST_ASSERT_EQUAL(n, read_cnt);
     TEST_ASSERT_EQUAL(8 - n, tu_fifo_count(ff));
 
@@ -469,7 +469,7 @@ void test_read_n_fixed_addr_rw32_wrapped(void) {
     }
 
     uint32_t reg      = 0;
-    uint16_t read_cnt = tu_fifo_read_n_access_mode(ff, &reg, n, true);
+    uint16_t read_cnt = tu_fifo_read_n_access_mode(ff, &reg, n, 4);
     TEST_ASSERT_EQUAL(n, read_cnt);
     TEST_ASSERT_EQUAL(0, tu_fifo_count(ff));
 
@@ -477,6 +477,86 @@ void test_read_n_fixed_addr_rw32_wrapped(void) {
   }
 }
 
+void test_write_n_fixed_addr_rw16_nowrap(void) {
+  tu_fifo_clear(ff);
+
+  volatile uint16_t reg         = 0x1122;
+  uint8_t           expected[6] = {0x22, 0x11, 0x22, 0x11, 0x22, 0x11};
+
+  for (uint8_t n = 1; n <= 6; n++) {
+    tu_fifo_clear(ff);
+    uint16_t written = tu_fifo_write_n_access_mode(ff, (const void *)&reg, n, 2);
+    TEST_ASSERT_EQUAL(n, written);
+    TEST_ASSERT_EQUAL(n, tu_fifo_count(ff));
+
+    uint8_t out[6] = {0};
+    tu_fifo_read_n(ff, out, n);
+    TEST_ASSERT_EQUAL_UINT8_ARRAY(expected, out, n);
+  }
+}
+
+void test_write_n_fixed_addr_rw16_wrapped(void) {
+  tu_fifo_clear(ff);
+
+  volatile uint16_t reg         = 0xA1B2;
+  uint8_t           expected[6] = {0xB2, 0xA1, 0xB2, 0xA1, 0xB2, 0xA1};
+
+  for (uint8_t n = 1; n <= 6; n++) {
+    tu_fifo_clear(ff);
+    // Position the fifo near the end so writes wrap
+    ff->wr_idx = FIFO_SIZE - 3;
+    ff->rd_idx = FIFO_SIZE - 3;
+
+    uint16_t written = tu_fifo_write_n_access_mode(ff, (const void *)&reg, n, 2);
+    TEST_ASSERT_EQUAL(n, written);
+    TEST_ASSERT_EQUAL(n, tu_fifo_count(ff));
+
+    uint8_t out[6] = {0};
+    tu_fifo_read_n(ff, out, n);
+    TEST_ASSERT_EQUAL_UINT8_ARRAY(expected, out, n);
+  }
+}
+
+void test_read_n_fixed_addr_rw16_nowrap(void) {
+  uint8_t pattern[6] = {0x10, 0x21, 0x32, 0x43, 0x54, 0x65};
+  uint16_t reg_expected[6] = {0x0010, 0x2110, 0x0032, 0x4332, 0x0054, 0x6554};
+
+  for (uint8_t n = 1; n <= 6; n++) {
+    tu_fifo_clear(ff);
+    tu_fifo_write_n(ff, pattern, 6);
+
+    uint16_t reg     = 0;
+    uint16_t read_cnt = tu_fifo_read_n_access_mode(ff, &reg, n, 2);
+    TEST_ASSERT_EQUAL(n, read_cnt);
+    TEST_ASSERT_EQUAL(6 - n, tu_fifo_count(ff));
+
+    TEST_ASSERT_EQUAL_HEX16(reg_expected[n - 1], reg);
+  }
+}
+
+void test_read_n_fixed_addr_rw16_wrapped(void) {
+  uint8_t pattern[6] = {0xF0, 0xE1, 0xD2, 0xC3, 0xB4, 0xA5};
+  uint16_t reg_expected[6] = {0x00F0, 0xE1F0, 0x00D2, 0xC3D2, 0x00B4, 0xA5B4};
+
+  for (uint8_t n = 1; n <= 6; n++) {
+    tu_fifo_clear(ff);
+    ff->rd_idx = FIFO_SIZE - 1;
+    ff->wr_idx = (uint16_t)(ff->rd_idx + n);
+
+    for (uint8_t i = 0; i < n; i++) {
+      uint8_t idx = (uint8_t)((ff->rd_idx + i) % FIFO_SIZE);
+      ff->buffer[idx] = pattern[i];
+    }
+
+    uint16_t reg     = 0;
+    uint16_t read_cnt = tu_fifo_read_n_access_mode(ff, &reg, n, 2);
+    TEST_ASSERT_EQUAL(n, read_cnt);
+    TEST_ASSERT_EQUAL(0, tu_fifo_count(ff));
+
+    TEST_ASSERT_EQUAL_HEX16(reg_expected[n - 1], reg);
+  }
+}
+
 void test_get_read_info_advanced_cases(void) {
   tu_fifo_clear(ff);