add tu_scatter_read32(), tu_scatter_write32() to simplify tu_fifo

4 files changed, 192 insertions, 81 deletions

diff --git a/src/common/tusb_common.h b/src/common/tusb_common.h
index f377d5272..b53fa5c02 100644
--- a/src/common/tusb_common.h
+++ b/src/common/tusb_common.h
@@ -329,6 +329,39 @@ TU_ATTR_ALWAYS_INLINE static inline void tu_unaligned_write16(void *mem, uint16_
 
 #endif
 
+// scatter read 4 bytes from two buffers. Parameter are not checked
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tu_scatter_read32(const uint8_t *buf1, uint8_t len1, const uint8_t *buf2,
+                                                               uint8_t len2) {
+  uint32_t result = 0;
+  uint8_t  shift  = 0;
+
+  for (uint8_t i = 0; i < len1; ++i) {
+    result |= ((uint32_t)buf1[i]) << shift;
+    shift += 8;
+  }
+
+  for (uint8_t i = 0; i < len2; ++i) {
+    result |= ((uint32_t)buf2[i]) << shift;
+    shift += 8;
+  }
+
+  return result;
+}
+
+// scatter write 4 bytes 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) {
+    buf1[i] = (uint8_t)(value & 0xFF);
+    value >>= 8;
+  }
+
+  for (uint8_t i = 0; i < len2; ++i) {
+    buf2[i] = (uint8_t)(value & 0xFF);
+    value >>= 8;
+  }
+}
+
 //--------------------------------------------------------------------+
 // Descriptor helper
 //--------------------------------------------------------------------+
diff --git a/src/common/tusb_fifo.c b/src/common/tusb_fifo.c
index 6bc384be3..aa8a97979 100644
--- a/src/common/tusb_fifo.c
+++ b/src/common/tusb_fifo.c
@@ -85,16 +85,13 @@ bool tu_fifo_config(tu_fifo_t *f, void *buffer, uint16_t depth, uint16_t item_si
 // copy data to/from fifo without updating read/write pointers
 //--------------------------------------------------------------------+
 #ifdef CFG_TUSB_FIFO_ACCESS_FIXED_ADDR_RW32
-// Intended to be used to read from hardware USB FIFO in e.g. STM32 where all data is read from a constant address
-// Code adapted from dcd_synopsys.c
-// TODO generalize with configurable 1 byte or 4 byte each read
-static void _ff_push_const_addr(uint8_t *ff_buf, const void *app_buf, uint16_t len) {
-  const volatile uint32_t *reg_rx = (volatile const uint32_t *)app_buf;
-
+// Copy to fifo from fixed address buffer (usually a rx register) with TU_FIFO_FIXED_ADDR_RW32 mode
+static void ff_push_fixed_addr_rw32(uint8_t *ff_buf, const volatile uint32_t *reg_rx, uint16_t len) {
   // Reading full available 32 bit words from const app address
   uint16_t full_words = len >> 2;
   while (full_words--) {
-    tu_unaligned_write32(ff_buf, *reg_rx);
+    const uint32_t tmp32 = *reg_rx;
+    tu_unaligned_write32(ff_buf, tmp32);
     ff_buf += 4;
   }
 
@@ -106,11 +103,8 @@ static void _ff_push_const_addr(uint8_t *ff_buf, const void *app_buf, uint16_t l
   }
 }
 
-// Intended to be used to write to hardware USB FIFO in e.g. STM32
-// where all data is written to a constant address in full word copies
-static void _ff_pull_const_addr(void *app_buf, const uint8_t *ff_buf, uint16_t len) {
-  volatile uint32_t *reg_tx = (volatile uint32_t *)app_buf;
-
+// Copy from fifo to fixed address buffer (usually a tx register) with TU_FIFO_FIXED_ADDR_RW32 mode
+static void ff_pull_fixed_addr_rw32(volatile uint32_t *reg_tx, const uint8_t *ff_buf, uint16_t len) {
   // Write full available 32 bit words to const address
   uint16_t full_words = len >> 2;
   while (full_words--) {
@@ -118,25 +112,23 @@ static void _ff_pull_const_addr(void *app_buf, const uint8_t *ff_buf, uint16_t l
     ff_buf += 4;
   }
 
-  // Write the remaining 1-3 bytes into const address
+  // Write the remaining 1-3 bytes
   const uint8_t bytes_rem = len & 0x03;
   if (bytes_rem) {
     uint32_t tmp32 = 0;
     memcpy(&tmp32, ff_buf, bytes_rem);
-
     *reg_tx = tmp32;
   }
 }
 #endif
 
 // send one item to fifo WITHOUT updating write pointer
-static inline void _ff_push(tu_fifo_t *f, const void *app_buf, uint16_t rel) {
+static inline void ff_push(tu_fifo_t *f, const void *app_buf, uint16_t rel) {
   memcpy(f->buffer + (rel * f->item_size), app_buf, f->item_size);
 }
 
 // send n items to fifo WITHOUT updating write pointer
-static void _ff_push_n(tu_fifo_t *f, const void *app_buf, uint16_t n, uint16_t wr_ptr,
-                       tu_fifo_access_mode_t copy_mode) {
+static void ff_push_n(tu_fifo_t *f, const void *app_buf, uint16_t n, uint16_t wr_ptr, tu_fifo_access_mode_t copy_mode) {
   const uint16_t lin_count  = f->depth - wr_ptr;
   const uint16_t wrap_count = n - lin_count;
 
@@ -153,61 +145,45 @@ static void _ff_push_n(tu_fifo_t *f, const void *app_buf, uint16_t n, uint16_t w
         memcpy(ff_buf, app_buf, n * f->item_size);
       } else {
         // Wrap around
-
-        // Write data to linear part of buffer
-        memcpy(ff_buf, app_buf, lin_bytes);
-
-        // Write data wrapped around
-        // TU_ASSERT(nWrap_bytes <= f->depth, );
-        memcpy(f->buffer, ((const uint8_t *)app_buf) + lin_bytes, wrap_bytes);
+        memcpy(ff_buf, app_buf, lin_bytes);                                    // linear part
+        memcpy(f->buffer, ((const uint8_t *)app_buf) + lin_bytes, wrap_bytes); // wrapped part
       }
       break;
 
-#ifdef  CFG_TUSB_FIFO_ACCESS_FIXED_ADDR_RW32
-    case TU_FIFO_FIXED_ADDR_RW32:
-      // Intended for hardware buffers from which it can be read word by word only
+#ifdef CFG_TUSB_FIFO_ACCESS_FIXED_ADDR_RW32
+    case TU_FIFO_FIXED_ADDR_RW32: {
+      const volatile uint32_t *reg_rx = (volatile const uint32_t *)app_buf;
       if (n <= lin_count) {
         // Linear only
-        _ff_push_const_addr(ff_buf, app_buf, n * f->item_size);
+        ff_push_fixed_addr_rw32(ff_buf, reg_rx, n * f->item_size);
       } else {
-        // Wrap around case
+        // Wrap around
 
         // Write full words to linear part of buffer
-        uint16_t nLin_4n_bytes = lin_bytes & 0xFFFC;
-        _ff_push_const_addr(ff_buf, app_buf, nLin_4n_bytes);
-        ff_buf += nLin_4n_bytes;
+        uint16_t lin_4n_bytes = lin_bytes & 0xFFFC;
+        ff_push_fixed_addr_rw32(ff_buf, reg_rx, lin_4n_bytes);
+        ff_buf += lin_4n_bytes;
 
         // There could be odd 1-3 bytes before the wrap-around boundary
-        uint8_t rem = lin_bytes & 0x03;
+        const uint8_t rem = lin_bytes & 0x03;
         if (rem > 0) {
-          const volatile uint32_t *rx_fifo = (volatile const uint32_t *)app_buf;
+          const uint8_t  remrem = (uint8_t)tu_min16(wrap_bytes, 4 - rem);
+          const uint32_t tmp32  = *reg_rx;
+          tu_scatter_write32(tmp32, ff_buf, rem, f->buffer, remrem);
 
-          uint8_t remrem = (uint8_t)tu_min16(wrap_bytes, 4 - rem);
           wrap_bytes -= remrem;
-
-          uint32_t tmp32  = *rx_fifo;
-          uint8_t *src_u8 = ((uint8_t *)&tmp32);
-
-          // Write 1-3 bytes before wrapped boundary
-          while (rem--) {
-            *ff_buf++ = *src_u8++;
-          }
-
-          // Read more bytes to beginning to complete a word
-          ff_buf = f->buffer;
-          while (remrem--) {
-            *ff_buf++ = *src_u8++;
-          }
+          ff_buf = f->buffer + remrem; // wrap around
         } else {
           ff_buf = f->buffer; // wrap around to beginning
         }
 
         // Write data wrapped part
         if (wrap_bytes > 0) {
-          _ff_push_const_addr(ff_buf, app_buf, wrap_bytes);
+          ff_push_fixed_addr_rw32(ff_buf, reg_rx, wrap_bytes);
         }
       }
       break;
+    }
 #endif
 
     default:
@@ -216,12 +192,12 @@ static void _ff_push_n(tu_fifo_t *f, const void *app_buf, uint16_t n, uint16_t w
 }
 
 // get one item from fifo WITHOUT updating read pointer
-TU_ATTR_ALWAYS_INLINE static inline void _ff_pull(const tu_fifo_t *f, void *buf, uint16_t ptr) {
+TU_ATTR_ALWAYS_INLINE static inline void ff_pull(const tu_fifo_t *f, void *buf, uint16_t ptr) {
   memcpy(buf, f->buffer + (ptr * f->item_size), f->item_size);
 }
 
 // get n items from fifo WITHOUT updating read pointer
-static void _ff_pull_n(tu_fifo_t *f, void *app_buf, uint16_t n, uint16_t rd_ptr, tu_fifo_access_mode_t copy_mode) {
+static void ff_pull_n(tu_fifo_t *f, void *app_buf, uint16_t n, uint16_t rd_ptr, tu_fifo_access_mode_t copy_mode) {
   const uint16_t lin_count  = f->depth - rd_ptr;
   const uint16_t wrap_count = n - lin_count; // only used if wrapped
 
@@ -244,51 +220,41 @@ static void _ff_pull_n(tu_fifo_t *f, void *app_buf, uint16_t n, uint16_t rd_ptr,
       break;
 
 #ifdef  CFG_TUSB_FIFO_ACCESS_FIXED_ADDR_RW32
-    case TU_FIFO_FIXED_ADDR_RW32:
+    case TU_FIFO_FIXED_ADDR_RW32: {
+      volatile uint32_t *reg_tx = (volatile uint32_t *)app_buf;
+
       if (n <= lin_count) {
         // Linear only
-        _ff_pull_const_addr(app_buf, ff_buf, n * f->item_size);
+        ff_pull_fixed_addr_rw32(reg_tx, ff_buf, n * f->item_size);
       } else {
         // Wrap around case
 
-        // Read full words from linear part of buffer
+        // Read full words from linear part
         uint16_t lin_4n_bytes = lin_bytes & 0xFFFC;
-        _ff_pull_const_addr(app_buf, ff_buf, lin_4n_bytes);
+        ff_pull_fixed_addr_rw32(reg_tx, ff_buf, lin_4n_bytes);
         ff_buf += lin_4n_bytes;
 
         // There could be odd 1-3 bytes before the wrap-around boundary
-        uint8_t rem = lin_bytes & 0x03;
+        const uint8_t rem = lin_bytes & 0x03;
         if (rem > 0) {
-          volatile uint32_t *reg_tx = (volatile uint32_t *)app_buf;
-
-          uint8_t remrem = (uint8_t)tu_min16(wrap_bytes, 4 - rem);
-          wrap_bytes -= remrem;
+          const uint8_t  remrem    = (uint8_t)tu_min16(wrap_bytes, 4 - rem);
+          const uint32_t scatter32 = tu_scatter_read32(ff_buf, rem, f->buffer, remrem);
 
-          uint32_t tmp32  = 0;
-          uint8_t *dst_u8 = (uint8_t *)&tmp32;
+          *reg_tx = scatter32;
 
-          // Read 1-3 bytes before wrapped boundary
-          while (rem--) {
-            *dst_u8++ = *ff_buf++;
-          }
-
-          // Read more bytes from beginning to complete a word
-          ff_buf = f->buffer;
-          while (remrem--) {
-            *dst_u8++ = *ff_buf++;
-          }
-
-          *reg_tx = tmp32;
+          wrap_bytes -= remrem;
+          ff_buf = f->buffer + remrem; // 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) {
-          _ff_pull_const_addr(app_buf, ff_buf, wrap_bytes);
+          ff_pull_fixed_addr_rw32(reg_tx, ff_buf, wrap_bytes);
         }
       }
       break;
+    }
 #endif
 
     default:
@@ -353,7 +319,7 @@ static bool ff_peek_local(tu_fifo_t *f, void *p_buffer, uint16_t wr_idx, uint16_
     ff_unlock(f->mutex_rd);
   }
 
-  _ff_pull(f, p_buffer, idx2ptr(f->depth, rd_idx));
+  ff_pull(f, p_buffer, idx2ptr(f->depth, rd_idx));
   return true;
 }
 
@@ -382,7 +348,7 @@ uint16_t tu_fifo_peek_n_access(tu_fifo_t *f, void *p_buffer, uint16_t n, uint16_
   }
 
   const uint16_t rd_ptr = idx2ptr(f->depth, rd_idx);
-  _ff_pull_n(f, p_buffer, n, rd_ptr, access_mode);
+  ff_pull_n(f, p_buffer, n, rd_ptr, access_mode);
 
   return n;
 }
@@ -449,7 +415,7 @@ uint16_t tu_fifo_write_n_access(tu_fifo_t *f, const void *data, uint16_t n, tu_f
     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, access_mode);
+    ff_push_n(f, buf8, n, wr_ptr, access_mode);
     f->wr_idx = advance_index(f->depth, wr_idx, n);
 
     TU_LOG(TU_FIFO_DBG, "\tnew_wr = %u\r\n", f->wr_idx);
@@ -575,7 +541,7 @@ bool tu_fifo_write(tu_fifo_t *f, const void *data) {
     ret = false;
   } else {
     uint16_t wr_ptr = idx2ptr(f->depth, wr_idx);
-    _ff_push(f, data, wr_ptr);
+    ff_push(f, data, wr_ptr);
     f->wr_idx = advance_index(f->depth, wr_idx, 1);
     ret       = true;
   }
diff --git a/test/hil/hil_test.py b/test/hil/hil_test.py
index 6b7a5ee12..78d8975c6 100755
--- a/test/hil/hil_test.py
+++ b/test/hil/hil_test.py
@@ -36,6 +36,8 @@ import warnings
 
 # Suppress pkg_resources deprecation warning from fs module
 warnings.filterwarnings("ignore", message="pkg_resources is deprecated")
+# Suppress pyfatfs unclean unmount warning
+warnings.filterwarnings("ignore", message="Filesystem was not cleanly unmounted")
 
 import serial
 import subprocess
diff --git a/test/unit-test/test/test_common_func.c b/test/unit-test/test/test_common_func.c
index 981531dd7..8afcc5b2b 100644
--- a/test/unit-test/test/test_common_func.c
+++ b/test/unit-test/test/test_common_func.c
@@ -80,3 +80,113 @@ void test_TU_ARGS_NUM(void)
   TEST_ASSERT_EQUAL(31, TU_ARGS_NUM(a1, a2, a3, a4, a5, a6, a7, a8, a9, a10, a11, a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23, a24, a25, a26, a27, a28, a29, a30, a31));
   TEST_ASSERT_EQUAL(32, TU_ARGS_NUM(a1, a2, a3, a4, a5, a6, a7, a8, a9, a10, a11, a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23, a24, a25, a26, a27, a28, a29, a30, a31, a32));
 }
+
+void test_tu_scatter_read32(void) {
+  // Test data: 0x04030201
+  uint8_t buf1[] = {0x01, 0x02, 0x03, 0x04};
+  uint8_t buf2[] = {0x05, 0x06, 0x07, 0x08};
+
+  // len1=1, len2=0: read 1 byte from buf1
+  TEST_ASSERT_EQUAL_HEX32(0x01, tu_scatter_read32(buf1, 1, buf2, 0));
+
+  // len1=1, len2=1: read 1 byte from buf1, 1 byte from buf2
+  TEST_ASSERT_EQUAL_HEX32(0x0501, tu_scatter_read32(buf1, 1, buf2, 1));
+
+  // len1=1, len2=2: read 1 byte from buf1, 2 bytes from buf2
+  TEST_ASSERT_EQUAL_HEX32(0x060501, tu_scatter_read32(buf1, 1, buf2, 2));
+
+  // len1=1, len2=3: read 1 byte from buf1, 3 bytes from buf2
+  TEST_ASSERT_EQUAL_HEX32(0x07060501, tu_scatter_read32(buf1, 1, buf2, 3));
+
+  // len1=2, len2=0: read 2 bytes from buf1
+  TEST_ASSERT_EQUAL_HEX32(0x0201, tu_scatter_read32(buf1, 2, buf2, 0));
+
+  // len1=2, len2=1: read 2 bytes from buf1, 1 byte from buf2
+  TEST_ASSERT_EQUAL_HEX32(0x050201, tu_scatter_read32(buf1, 2, buf2, 1));
+
+  // len1=2, len2=2: read 2 bytes from buf1, 2 bytes from buf2
+  TEST_ASSERT_EQUAL_HEX32(0x06050201, tu_scatter_read32(buf1, 2, buf2, 2));
+
+  // len1=3, len2=0: read 3 bytes from buf1
+  TEST_ASSERT_EQUAL_HEX32(0x030201, tu_scatter_read32(buf1, 3, buf2, 0));
+
+  // len1=3, len2=1: read 3 bytes from buf1, 1 byte from buf2
+  TEST_ASSERT_EQUAL_HEX32(0x05030201, tu_scatter_read32(buf1, 3, buf2, 1));
+}
+
+void test_tu_scatter_write32(void) {
+  uint8_t buf1[4];
+  uint8_t buf2[4];
+
+  // len1=1, len2=0: write 1 byte to buf1
+  memset(buf1, 0, sizeof(buf1));
+  memset(buf2, 0, sizeof(buf2));
+  tu_scatter_write32(0x01, buf1, 1, buf2, 0);
+  TEST_ASSERT_EQUAL_HEX8(0x01, buf1[0]);
+  TEST_ASSERT_EQUAL_HEX8(0x00, buf2[0]);
+
+  // len1=1, len2=1: write 1 byte to buf1, 1 byte to buf2
+  memset(buf1, 0, sizeof(buf1));
+  memset(buf2, 0, sizeof(buf2));
+  tu_scatter_write32(0x0201, buf1, 1, buf2, 1);
+  TEST_ASSERT_EQUAL_HEX8(0x01, buf1[0]);
+  TEST_ASSERT_EQUAL_HEX8(0x02, buf2[0]);
+
+  // len1=1, len2=2: write 1 byte to buf1, 2 bytes to buf2
+  memset(buf1, 0, sizeof(buf1));
+  memset(buf2, 0, sizeof(buf2));
+  tu_scatter_write32(0x030201, buf1, 1, buf2, 2);
+  TEST_ASSERT_EQUAL_HEX8(0x01, buf1[0]);
+  TEST_ASSERT_EQUAL_HEX8(0x02, buf2[0]);
+  TEST_ASSERT_EQUAL_HEX8(0x03, buf2[1]);
+
+  // len1=1, len2=3: write 1 byte to buf1, 3 bytes to buf2
+  memset(buf1, 0, sizeof(buf1));
+  memset(buf2, 0, sizeof(buf2));
+  tu_scatter_write32(0x04030201, buf1, 1, buf2, 3);
+  TEST_ASSERT_EQUAL_HEX8(0x01, buf1[0]);
+  TEST_ASSERT_EQUAL_HEX8(0x02, buf2[0]);
+  TEST_ASSERT_EQUAL_HEX8(0x03, buf2[1]);
+  TEST_ASSERT_EQUAL_HEX8(0x04, buf2[2]);
+
+  // len1=2, len2=0: write 2 bytes to buf1
+  memset(buf1, 0, sizeof(buf1));
+  memset(buf2, 0, sizeof(buf2));
+  tu_scatter_write32(0x0201, buf1, 2, buf2, 0);
+  TEST_ASSERT_EQUAL_HEX8(0x01, buf1[0]);
+  TEST_ASSERT_EQUAL_HEX8(0x02, buf1[1]);
+
+  // len1=2, len2=1: write 2 bytes to buf1, 1 byte to buf2
+  memset(buf1, 0, sizeof(buf1));
+  memset(buf2, 0, sizeof(buf2));
+  tu_scatter_write32(0x030201, buf1, 2, buf2, 1);
+  TEST_ASSERT_EQUAL_HEX8(0x01, buf1[0]);
+  TEST_ASSERT_EQUAL_HEX8(0x02, buf1[1]);
+  TEST_ASSERT_EQUAL_HEX8(0x03, buf2[0]);
+
+  // len1=2, len2=2: write 2 bytes to buf1, 2 bytes to buf2
+  memset(buf1, 0, sizeof(buf1));
+  memset(buf2, 0, sizeof(buf2));
+  tu_scatter_write32(0x04030201, buf1, 2, buf2, 2);
+  TEST_ASSERT_EQUAL_HEX8(0x01, buf1[0]);
+  TEST_ASSERT_EQUAL_HEX8(0x02, buf1[1]);
+  TEST_ASSERT_EQUAL_HEX8(0x03, buf2[0]);
+  TEST_ASSERT_EQUAL_HEX8(0x04, buf2[1]);
+
+  // len1=3, len2=0: write 3 bytes to buf1
+  memset(buf1, 0, sizeof(buf1));
+  memset(buf2, 0, sizeof(buf2));
+  tu_scatter_write32(0x030201, buf1, 3, buf2, 0);
+  TEST_ASSERT_EQUAL_HEX8(0x01, buf1[0]);
+  TEST_ASSERT_EQUAL_HEX8(0x02, buf1[1]);
+  TEST_ASSERT_EQUAL_HEX8(0x03, buf1[2]);
+
+  // len1=3, len2=1: write 3 bytes to buf1, 1 byte to buf2
+  memset(buf1, 0, sizeof(buf1));
+  memset(buf2, 0, sizeof(buf2));
+  tu_scatter_write32(0x04030201, buf1, 3, buf2, 1);
+  TEST_ASSERT_EQUAL_HEX8(0x01, buf1[0]);
+  TEST_ASSERT_EQUAL_HEX8(0x02, buf1[1]);
+  TEST_ASSERT_EQUAL_HEX8(0x03, buf1[2]);
+  TEST_ASSERT_EQUAL_HEX8(0x04, buf2[0]);
+}