simplify _ff_correct_read_index()

1 files changed, 62 insertions, 45 deletions

diff --git a/src/common/tusb_fifo.c b/src/common/tusb_fifo.c
index a0ce73ba0..22d6e8e55 100644
--- a/src/common/tusb_fifo.c
+++ b/src/common/tusb_fifo.c
@@ -314,7 +314,44 @@ static void _ff_pull_n(tu_fifo_t* f, void* app_buf, uint16_t n, uint16_t rel, tu
 }
 
 //--------------------------------------------------------------------+
-// Index (free-running and real buffer pointer)
+// Helper
+//--------------------------------------------------------------------+
+
+// return only the index difference and as such can be used to determine an overflow i.e overflowable count
+TU_ATTR_ALWAYS_INLINE static inline
+uint16_t _ff_count(uint16_t depth, uint16_t wr_idx, uint16_t rd_idx)
+{
+  // In case we have non-power of two depth we need a further modification
+  if (wr_idx >= rd_idx)
+  {
+    return (uint16_t) (wr_idx - rd_idx);
+  } else
+  {
+    return (uint16_t) (2*depth - (rd_idx - wr_idx));
+  }
+}
+
+// BE AWARE - THIS FUNCTION MIGHT NOT GIVE A CORRECT ANSWERE IN CASE WRITE POINTER "OVERFLOWS"
+// Only one overflow is allowed for this function to work e.g. if depth = 100, you must not
+// write more than 2*depth-1 items in one rush without updating write pointer. Otherwise
+// write pointer wraps and you pointer states are messed up. This can only happen if you
+// use DMAs, write functions do not allow such an error.
+TU_ATTR_ALWAYS_INLINE static inline
+bool _ff_overflowed(uint16_t depth, uint16_t wr_idx, uint16_t rd_idx)
+{
+  return _ff_count(depth, wr_idx, rd_idx) > depth;
+}
+
+// return remaining slot in fifo
+TU_ATTR_ALWAYS_INLINE static inline
+uint16_t _ff_remaining(uint16_t depth, uint16_t wr_idx, uint16_t rd_idx)
+{
+  uint16_t const count = _ff_count(depth, wr_idx, rd_idx);
+  return (depth > count) ? (depth - count) : 0;
+}
+
+//--------------------------------------------------------------------+
+// Index Helper
 //--------------------------------------------------------------------+
 
 // Advance an absolute index
@@ -334,6 +371,7 @@ static uint16_t advance_index(uint16_t depth, uint16_t idx, uint16_t offset)
   return new_idx;
 }
 
+#if 0 // not used but
 // Backward an absolute index
 static uint16_t backward_index(uint16_t depth, uint16_t idx, uint16_t offset)
 {
@@ -349,6 +387,7 @@ static uint16_t backward_index(uint16_t depth, uint16_t idx, uint16_t offset)
 
   return new_idx;
 }
+#endif
 
 // index to pointer, simply an modulo with minus.
 TU_ATTR_ALWAYS_INLINE static inline
@@ -359,45 +398,24 @@ uint16_t idx2ptr(uint16_t depth, uint16_t idx)
   return idx;
 }
 
-// return only the index difference and as such can be used to determine an overflow i.e overflowable count
+// Works on local copies of w
+// When an overwritable fifo is overflowed, rd_idx will be re-index so that it forms
+// an full fifo i.e _ff_count() = depth
 TU_ATTR_ALWAYS_INLINE static inline
-uint16_t _ff_count(uint16_t depth, uint16_t wr_idx, uint16_t rd_idx)
+uint16_t _ff_correct_read_index(tu_fifo_t* f, uint16_t wr_idx)
 {
-  // In case we have non-power of two depth we need a further modification
-  if (wr_idx >= rd_idx)
+  uint16_t rd_idx;
+  if ( wr_idx >= f->depth )
   {
-    return (uint16_t) (wr_idx - rd_idx);
-  } else
+    rd_idx = wr_idx - f->depth;
+  }else
   {
-    return (uint16_t) (2*depth - (rd_idx - wr_idx));
+    rd_idx = wr_idx + f->depth;
   }
-}
 
-// BE AWARE - THIS FUNCTION MIGHT NOT GIVE A CORRECT ANSWERE IN CASE WRITE POINTER "OVERFLOWS"
-// Only one overflow is allowed for this function to work e.g. if depth = 100, you must not
-// write more than 2*depth-1 items in one rush without updating write pointer. Otherwise
-// write pointer wraps and you pointer states are messed up. This can only happen if you
-// use DMAs, write functions do not allow such an error.
-TU_ATTR_ALWAYS_INLINE static inline
-bool _ff_overflowed(uint16_t depth, uint16_t wr_idx, uint16_t rd_idx)
-{
-  return _ff_count(depth, wr_idx, rd_idx) > depth;
-}
+  f->rd_idx = rd_idx;
 
-// return remaining slot in fifo
-TU_ATTR_ALWAYS_INLINE static inline
-uint16_t _ff_remaining(uint16_t depth, uint16_t wr_idx, uint16_t rd_idx)
-{
-  uint16_t const count = _ff_count(depth, wr_idx, rd_idx);
-  return (depth > count) ? (depth - count) : 0;
-}
-
-// Works on local copies of w
-// For more details see _tu_fifo_overflow()!
-TU_ATTR_ALWAYS_INLINE static inline
-void _tu_fifo_correct_read_pointer(tu_fifo_t* f, uint16_t wr_idx)
-{
-  f->rd_idx = backward_index(f->depth, wr_idx, f->depth);
+  return rd_idx;
 }
 
 // Works on local copies of w and r
@@ -407,14 +425,14 @@ static bool _tu_fifo_peek(tu_fifo_t* f, void * p_buffer, uint16_t wr_idx, uint16
   uint16_t cnt = _ff_count(f->depth, wr_idx, rd_idx);
 
   // Check overflow and correct if required
-  if (cnt > f->depth)
+  if ( cnt > f->depth )
   {
-    _tu_fifo_correct_read_pointer(f, wr_idx);
+    rd_idx = _ff_correct_read_index(f, wr_idx);
     cnt = f->depth;
   }
 
   // Skip beginning of buffer
-  if (cnt == 0) return false;
+  if ( cnt == 0 ) return false;
 
   uint16_t rd_ptr = idx2ptr(f->depth, rd_idx);
 
@@ -431,18 +449,17 @@ static uint16_t _tu_fifo_peek_n(tu_fifo_t* f, void * p_buffer, uint16_t n, uint1
   uint16_t cnt = _ff_count(f->depth, wr_idx, rd_idx);
 
   // Check overflow and correct if required
-  if (cnt > f->depth)
+  if ( cnt > f->depth )
   {
-    _tu_fifo_correct_read_pointer(f, wr_idx);
-    rd_idx = f->rd_idx;
+    rd_idx = _ff_correct_read_index(f, wr_idx);
     cnt = f->depth;
   }
 
   // Skip beginning of buffer
-  if (cnt == 0) return 0;
+  if ( cnt == 0 ) return 0;
 
   // Check if we can read something at and after offset - if too less is available we read what remains
-  if (cnt < n) n = cnt;
+  if ( cnt < n ) n = cnt;
 
   uint16_t rd_ptr = idx2ptr(f->depth, rd_idx);
 
@@ -662,7 +679,7 @@ bool tu_fifo_overflowed(tu_fifo_t* f)
 void tu_fifo_correct_read_pointer(tu_fifo_t* f)
 {
   _ff_lock(f->mutex_rd);
-  _tu_fifo_correct_read_pointer(f, f->wr_idx);
+  _ff_correct_read_index(f, f->wr_idx);
   _ff_unlock(f->mutex_rd);
 }
 
@@ -963,10 +980,9 @@ void tu_fifo_get_read_info(tu_fifo_t *f, tu_fifo_buffer_info_t *info)
   if (cnt > f->depth)
   {
     _ff_lock(f->mutex_rd);
-    _tu_fifo_correct_read_pointer(f, wr_idx);
+    rd_idx = _ff_correct_read_index(f, wr_idx);
     _ff_unlock(f->mutex_rd);
 
-    rd_idx = f->rd_idx;
     cnt = f->depth;
   }
 
@@ -988,7 +1004,8 @@ void tu_fifo_get_read_info(tu_fifo_t *f, tu_fifo_buffer_info_t *info)
   info->ptr_lin = &f->buffer[rd_ptr];
 
   // Check if there is a wrap around necessary
-  if (wr_ptr > rd_ptr) {
+  if (wr_ptr > rd_ptr)
+  {
     // Non wrapping case
     info->len_lin  = cnt;