refactor copy to and from fifo

1 files changed, 46 insertions, 39 deletions

diff --git a/src/common/tusb_fifo.c b/src/common/tusb_fifo.c
index 3ed610021..6ab158cca 100644
--- a/src/common/tusb_fifo.c
+++ b/src/common/tusb_fifo.c
@@ -71,33 +71,38 @@ bool tu_fifo_config(tu_fifo_t *f, void* buffer, uint16_t depth, uint16_t item_si
   return true;
 }
 
+static inline uint16_t _ff_mod(uint16_t idx, uint16_t depth)
+{
+  return (idx < depth) ? idx : (idx-depth);
+}
+
 // retrieve data from fifo
-static void _tu_ff_pull(tu_fifo_t* f, void * buffer)
+static inline void _ff_pull(tu_fifo_t* f, void * buffer, uint16_t n)
 {
   memcpy(buffer,
          f->buffer + (f->rd_idx * f->item_size),
-         f->item_size);
+         f->item_size*n);
 
-  f->rd_idx = (f->rd_idx + 1) % f->depth;
-  f->count--;
+  f->rd_idx = _ff_mod(f->rd_idx + n, f->depth);
+  f->count -= n;
 }
 
 // send data to fifo
-static void _tu_ff_push(tu_fifo_t* f, void const * data)
+static inline void _ff_push(tu_fifo_t* f, void const * data, uint16_t n)
 {
-  memcpy( f->buffer + (f->wr_idx * f->item_size),
-          data,
-          f->item_size);
+  memcpy(f->buffer + (f->wr_idx * f->item_size),
+         data,
+         f->item_size*n);
 
-  f->wr_idx = (f->wr_idx + 1) % f->depth;
+  f->wr_idx = _ff_mod(f->wr_idx + n, f->depth);
 
   if (tu_fifo_full(f))
   {
-    f->rd_idx = f->wr_idx; // keep the full state (rd == wr && len = size)
+    f->rd_idx = f->wr_idx; // keep the full state (rd == wr && count = depth)
   }
   else
   {
-    f->count++;
+    f->count += n;
   }
 }
 
@@ -123,7 +128,7 @@ bool tu_fifo_read(tu_fifo_t* f, void * buffer)
 
   tu_fifo_lock(f);
 
-  _tu_ff_pull(f, buffer);
+  _ff_pull(f, buffer, 1);
 
   tu_fifo_unlock(f);
 
@@ -152,26 +157,24 @@ uint16_t tu_fifo_read_n (tu_fifo_t* f, void * buffer, uint16_t count)
 
   tu_fifo_lock(f);
 
-  /* Limit up to fifo's count */
-  if(count > f->count)
-    count = f->count;
+  // Limit up to fifo's count
+  if(count > f->count) count = f->count;
 
   if(count + f->rd_idx <= f->depth)
   {
-    memcpy(buffer, f->buffer + f->rd_idx * f->item_size, count * f->item_size);
+    _ff_pull(f, buffer, count);
   }
   else
   {
-    uint16_t part1 = (f->depth - f->rd_idx) * f->item_size;
-    memcpy(buffer, f->buffer + f->rd_idx * f->item_size, part1);
-    memcpy((uint8_t*)buffer + part1, f->buffer, count * f->item_size - part1);
+    uint16_t const part1 = f->depth - f->rd_idx;
+
+    // Part 1: from rd_idx to end
+    _ff_pull(f, buffer, part1);
+    buffer = ((uint8_t*) buffer) + part1*f->item_size;
+
+    // Part 2: start to remaining
+    _ff_pull(f, buffer, count-part1);
   }
-  
-  f->rd_idx += count;
-  if (f->rd_idx >= f->depth)
-    f->rd_idx -= f->depth;
-  
-  f->count -= count;
 
   tu_fifo_unlock(f);
 
@@ -196,12 +199,16 @@ bool tu_fifo_peek_at(tu_fifo_t* f, uint16_t pos, void * p_buffer)
 {
   if ( pos >= f->count ) return false;
 
+  tu_fifo_lock(f);
+
   // rd_idx is pos=0
-  uint16_t index = (f->rd_idx + pos) % f->depth;
+  uint16_t index = _ff_mod(f->rd_idx + pos, f->depth);
   memcpy(p_buffer,
          f->buffer + (index * f->item_size),
          f->item_size);
 
+  tu_fifo_unlock(f);
+
   return true;
 }
 
@@ -228,7 +235,7 @@ bool tu_fifo_write (tu_fifo_t* f, const void * data)
 
   tu_fifo_lock(f);
 
-  _tu_ff_push(f, data);
+  _ff_push(f, data, 1);
 
   tu_fifo_unlock(f);
 
@@ -257,14 +264,15 @@ uint16_t tu_fifo_write_n (tu_fifo_t* f, const void * data, uint16_t count)
   tu_fifo_lock(f);
 
   uint8_t const* buf8 = (uint8_t const*) data;
-  // Not overwritable limit up to full
+
   if (!f->overwritable)
   {
+    // Not overwritable limit up to full
     count = tu_min16(count, tu_fifo_remaining(f));
   }
-  // Only copy last part
   else if (count > f->depth)
   {
+    // Only copy last part
     buf8 = buf8 + (count - f->depth) * f->item_size;
     count = f->depth;
     f->wr_idx = 0;
@@ -274,21 +282,20 @@ uint16_t tu_fifo_write_n (tu_fifo_t* f, const void * data, uint16_t count)
 
   if (count + f->wr_idx <= f->depth )
   {
-    memcpy(f->buffer + f->wr_idx * f->item_size, buf8, count * f->item_size);
+    _ff_push(f, buf8, count);
   }
   else
   {
-    uint16_t part1 = (f->depth - f->wr_idx) * f->item_size;
-    memcpy(f->buffer + f->wr_idx * f->item_size, buf8, part1);
-    memcpy(f->buffer, buf8 + part1, count * f->item_size - part1);
+    uint16_t const part1 = f->depth - f->wr_idx;
+
+    // Part 1: from wr_idx to end
+    _ff_push(f, buf8, part1);
+    buf8 += part1*f->item_size;
+
+    // Part 2: start to remaining
+    _ff_push(f, buf8, count-part1);
   }
   
-  f->wr_idx += count;
-  if (f->wr_idx >= f->depth)
-    f->wr_idx -= f->depth;
-  
-  f->count += count;
-  
   tu_fifo_unlock(f);
 
   return count;