1 files changed, 147 insertions, 73 deletions
diff --git a/tinyusb/common/fifo.c b/tinyusb/common/fifo.c
index b0f822182..dd7a66298 100644
--- a/tinyusb/common/fifo.c
+++ b/tinyusb/common/fifo.c
@@ -7,7 +7,7 @@
 
     Software License Agreement (BSD License)
 
-    Copyright (c) 2013, hathach (tinyusb.org)
+    Copyright (c) 2018, hathach (tinyusb.org)
     All rights reserved.
 
     Redistribution and use in source and binary forms, with or without
@@ -35,18 +35,36 @@
 	This file is part of the tinyusb stack.
 */
 /**************************************************************************/
-#include <string.h>
+
 #include "fifo.h"
 
-//static inline void mutex_lock   (fifo_t* f) ATTR_ALWAYS_INLINE;
-//static inline void mutex_unlock (fifo_t* f) ATTR_ALWAYS_INLINE;
-#define mutex_lock(f)
-#define mutex_unlock(f)
+/*------------------------------------------------------------------*/
+/*
+ *------------------------------------------------------------------*/
+#if CFG_FIFO_MUTEX
 
-static inline bool is_fifo_initalized(fifo_t* f) ATTR_ALWAYS_INLINE;
+#define mutex_lock_if_needed(_ff)     if (_ff->mutex) fifo_mutex_lock(_ff->mutex)
+#define mutex_unlock_if_needed(_ff)   if (_ff->mutex) fifo_mutex_unlock(_ff->mutex)
 
+#else
 
-/**************************************************************************/
+#define mutex_lock_if_needed(_ff)
+#define mutex_unlock_if_needed(_ff)
+
+#endif
+
+static inline uint16_t min16_of(uint16_t x, uint16_t y)
+{
+  return (x < y) ? x : y;
+}
+
+static inline bool fifo_initalized(fifo_t* f)
+{
+  return (f->buffer != NULL) && (f->depth > 0) && (f->item_size > 0);
+}
+
+
+/******************************************************************************/
 /*!
     @brief Read one byte out of the RX buffer.
 
@@ -56,20 +74,18 @@ static inline bool is_fifo_initalized(fifo_t* f) ATTR_ALWAYS_INLINE;
 
     @param[in]  f
                 Pointer to the FIFO buffer to manipulate
-    @param[in]  data
+    @param[in]  p_buffer
                 Pointer to the place holder for data read from the buffer
 
     @returns TRUE if the queue is not empty
 */
-/**************************************************************************/
+/******************************************************************************/
 bool fifo_read(fifo_t* f, void * p_buffer)
 {
-  if( !is_fifo_initalized(f) || fifo_is_empty(f) )
-  {
-    return false;
-  }
+  if( !fifo_initalized(f) ) return false;
+  if( fifo_is_empty(f) ) return false;
 
-  mutex_lock(f);
+  mutex_lock_if_needed(f);
 
   memcpy(p_buffer,
          f->buffer + (f->rd_idx * f->item_size),
@@ -77,36 +93,108 @@ bool fifo_read(fifo_t* f, void * p_buffer)
   f->rd_idx = (f->rd_idx + 1) % f->depth;
   f->count--;
 
-  mutex_unlock(f);
+  mutex_unlock_if_needed(f);
 
   return true;
 }
 
-/**************************************************************************/
+/******************************************************************************/
+/*!
+    @brief This function will read n elements into the array index specified by
+    the write pointer and increment the write index. If the write index
+    exceeds the max buffer size, then it will roll over to zero.
+
+    @param[in]  f
+                Pointer to the FIFO buffer to manipulate
+    @param[in]  p_data
+                The pointer to data location
+    @param[in]  count
+                Number of element that buffer can afford
+
+    @returns number of bytes read from the FIFO
+*/
+/******************************************************************************/
+uint16_t fifo_read_n (fifo_t* f, void * p_buffer, uint16_t count)
+{
+  if( !fifo_initalized(f) ) return false;
+  if( fifo_is_empty(f) ) return false;
+
+  /* Limit up to fifo's count */
+  count = min16_of(count, f->count);
+  if( count == 0 ) return 0;
+
+  mutex_lock_if_needed(f);
+
+  /* Could copy up to 2 portions marked as 'x' if queue is wrapped around
+   * case 1: ....RxxxxW.......
+   * case 2: xxxxxW....Rxxxxxx
+   */
+//  uint16_t index2upper = min16_of(count, f->count-f->rd_idx);
+
+  uint8_t* p_buf = (uint8_t*) p_buffer;
+  uint16_t len = 0;
+  while( (len < count) && fifo_read(f, p_buf) )
+  {
+    len++;
+    p_buf += f->item_size;
+  }
+
+  mutex_unlock_if_needed(f);
+
+  return len;
+}
+
+/******************************************************************************/
 /*!
-    @brief Write one byte into the RX buffer.
+    @brief Reads one item without removing it from the FIFO
+
+    @param[in]  f
+                Pointer to the FIFO buffer to manipulate
+    @param[in]  position
+                Position to read from in the FIFO buffer
+    @param[in]  p_buffer
+                Pointer to the place holder for data read from the buffer
 
-    This function will write one byte into the array index specified by
+    @returns TRUE if the queue is not empty
+*/
+/******************************************************************************/
+bool fifo_peek_at(fifo_t* f, uint16_t position, void * p_buffer)
+{
+  if ( !fifo_initalized(f) ) return false;
+  if ( position >= f->count ) return false;
+
+  // rd_idx is position=0
+  uint16_t index = (f->rd_idx + position) % f->depth;
+  memcpy(p_buffer,
+         f->buffer + (index * f->item_size),
+         f->item_size);
+
+  return true;
+}
+
+/******************************************************************************/
+/*!
+    @brief Write one element into the RX buffer.
+
+    This function will write one element into the array index specified by
     the write pointer and increment the write index. If the write index
     exceeds the max buffer size, then it will roll over to zero.
 
     @param[in]  f
                 Pointer to the FIFO buffer to manipulate
-    @param[in]  data
+    @param[in]  p_data
                 The byte to add to the FIFO
 
     @returns TRUE if the data was written to the FIFO (overwrittable
              FIFO will always return TRUE)
 */
-/**************************************************************************/
+/******************************************************************************/
 bool fifo_write(fifo_t* f, void const * p_data)
 {
-  if ( !is_fifo_initalized(f) || (fifo_is_full(f) && !f->overwritable) )
-  {
-    return false;
-  }
+  if ( !fifo_initalized(f) ) return false;
+  if ( fifo_is_full(f) && !f->overwritable ) return false;
 
-  mutex_lock(f);
+  mutex_lock_if_needed(f);
 
   memcpy( f->buffer + (f->wr_idx * f->item_size),
           p_data,
@@ -117,75 +205,61 @@ bool fifo_write(fifo_t* f, void const * p_data)
   if (fifo_is_full(f))
   {
     f->rd_idx = f->wr_idx; // keep the full state (rd == wr && len = size)
-  }else
+  }
+  else
   {
     f->count++;
   }
 
-  mutex_unlock(f);
+  mutex_unlock_if_needed(f);
 
   return true;
 }
 
-/**************************************************************************/
+/******************************************************************************/
 /*!
-    @brief Clear the fifo read and write pointers and set length to zero
+    @brief This function will write n elements into the array index specified by
+    the write pointer and increment the write index. If the write index
+    exceeds the max buffer size, then it will roll over to zero.
 
     @param[in]  f
                 Pointer to the FIFO buffer to manipulate
+    @param[in]  p_data
+                The pointer to data to add to the FIFO
+    @param[in]  count
+                Number of element
+    @return Number of written elements
 */
-/**************************************************************************/
-void fifo_clear(fifo_t *f)
+/******************************************************************************/
+uint16_t fifo_write_n(fifo_t* f, void const * p_data, uint16_t count)
 {
-  mutex_lock(f);
+  if ( count == 0 ) return 0;
 
-  f->rd_idx = f->wr_idx = f->count = 0;
+  uint8_t* p_buf = (uint8_t*) p_data;
 
-  mutex_unlock(f);
-}
+  uint16_t len = 0;
+  while( (len < count) && fifo_write(f, p_buf) )
+  {
+    len++;
+    p_buf += f->item_size;
+  }
 
-//--------------------------------------------------------------------+
-// HELPER FUNCTIONS
-//--------------------------------------------------------------------+
+  return len;
+}
 
-/**************************************************************************/
+/******************************************************************************/
 /*!
-    @brief Disables the IRQ specified in the FIFO's 'irq' field
-           to prevent reads/write issues with interrupts
+    @brief Clear the fifo read and write pointers and set length to zero
 
     @param[in]  f
-                Pointer to the FIFO that should be protected
+                Pointer to the FIFO buffer to manipulate
 */
-/**************************************************************************/
-//static inline void mutex_lock (fifo_t* f)
-//{
-//  if (f->irq > 0)
-//  {
-//    #if !defined (_TEST_)
-//    NVIC_DisableIRQ(f->irq);
-//    #endif
-//  }
-//}
-
-/**************************************************************************/
-/*!
-    @brief Re-enables the IRQ specified in the FIFO's 'irq' field
+/******************************************************************************/
+void fifo_clear(fifo_t *f)
+{
+  mutex_lock_if_needed(f);
 
-    @param[in]  f
-                Pointer to the FIFO that should be protected
-*/
-/**************************************************************************/
-//static inline void mutex_unlock (fifo_t* f)
-//{
-//  if (f->irq > 0)
-//  {
-//    #if !defined (_TEST_)
-//    NVIC_EnableIRQ(f->irq);
-//    #endif
-//  }
-//}
+  f->rd_idx = f->wr_idx = f->count = 0;
 
-static inline bool is_fifo_initalized(fifo_t* f)
-{
-  return !( f->buffer == NULL || f->depth == 0 || f->item_size == 0);
+  mutex_unlock_if_needed(f);
 }