Merge pull request #405 from hathach/follow-pr404

Follow pr404

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;

test/test/test_fifo.c

@@ -54,6 +54,90 @@ void test_normal(void)
   }
 }
 
+void test_item_size(void)
+{
+  TU_FIFO_DEF(ff4, FIFO_SIZE, uint32_t, false);
+  tu_fifo_clear(&ff4);
+
+  uint32_t data[20];
+  for(uint32_t i=0; i<sizeof(data)/4; i++) data[i] = i;
+
+  tu_fifo_write_n(&ff4, data, 10);
+
+  uint32_t rd[10];
+  uint16_t rd_count;
+
+  // read 0 -> 4
+  rd_count = tu_fifo_read_n(&ff4, rd, 5);
+  TEST_ASSERT_EQUAL( 5, rd_count );
+  TEST_ASSERT_EQUAL_UINT32_ARRAY( data, rd, rd_count ); // 0 -> 4
+
+  tu_fifo_write_n(&ff4, data+10, 5);
+
+  // read 5 -> 14
+  rd_count = tu_fifo_read_n(&ff4, rd, 10);
+  TEST_ASSERT_EQUAL( 10, rd_count );
+  TEST_ASSERT_EQUAL_UINT32_ARRAY( data+5, rd, rd_count ); // 5 -> 14
+}
+
+void test_read_n(void)
+{
+  // prepare data
+  uint8_t data[20];
+  for(int i=0; i<sizeof(data); i++) data[i] = i;
+
+  for(uint8_t i=0; i < FIFO_SIZE; i++) tu_fifo_write(&ff, data+i);
+
+  uint8_t rd[10];
+  uint16_t rd_count;
+
+  // case 1: Read index + count < depth
+  // read 0 -> 4
+  rd_count = tu_fifo_read_n(&ff, rd, 5);
+  TEST_ASSERT_EQUAL( 5, rd_count );
+  TEST_ASSERT_EQUAL_MEMORY( data, rd, rd_count ); // 0 -> 4
+
+  // case 2: Read index + count > depth
+  // write 10, 11, 12
+  tu_fifo_write(&ff, data+10);
+  tu_fifo_write(&ff, data+11);
+  tu_fifo_write(&ff, data+12);
+
+  rd_count = tu_fifo_read_n(&ff, rd, 7);
+  TEST_ASSERT_EQUAL( 7, rd_count );
+
+  TEST_ASSERT_EQUAL_MEMORY( data+5, rd, rd_count ); // 5 -> 11
+
+  // Should only read until empty
+  TEST_ASSERT_EQUAL( 1, tu_fifo_read_n(&ff, rd, 100) );
+}
+
+void test_write_n(void)
+{
+  // prepare data
+  uint8_t data[20];
+  for(int i=0; i<sizeof(data); i++) data[i] = i;
+
+  // case 1: wr + count < depth
+  tu_fifo_write_n(&ff, data, 8); // wr = 8, count = 8
+
+  uint8_t rd[10];
+  uint16_t rd_count;
+
+  rd_count = tu_fifo_read_n(&ff, rd, 5); // wr = 8, count = 3
+  TEST_ASSERT_EQUAL( 5, rd_count );
+  TEST_ASSERT_EQUAL_MEMORY( data, rd, rd_count ); // 0 -> 4
+
+  // case 2: wr + count > depth
+  tu_fifo_write_n(&ff, data+8, 6); // wr = 3, count = 9
+
+  for(rd_count=0; rd_count<7; rd_count++) tu_fifo_read(&ff, rd+rd_count); // wr = 3, count = 2
+
+  TEST_ASSERT_EQUAL_MEMORY( data+5, rd, rd_count); // 5 -> 11
+
+  TEST_ASSERT_EQUAL(2, tu_fifo_count(&ff));
+}
+
 void test_peek(void)
 {
   uint8_t temp;