Merge pull request #331 from adafruit/develop

enhance image_upload example

Author: hathach

cores/nRF5/utility/adafruit_fifo.cpp

@@ -86,45 +86,21 @@ Adafruit_FIFO::~Adafruit_FIFO()
   if (_buffer) rtos_free(_buffer);
 }
 
-bool Adafruit_FIFO::_mutex_lock(bool isr)
-{
-  (void) isr;
-  return xSemaphoreTake(_mutex, portMAX_DELAY);
-}
-
-bool Adafruit_FIFO::_mutex_unlock(bool isr)
-{
-  (void) isr;
-  return xSemaphoreGive(_mutex);
-}
-
-
 /******************************************************************************/
 /*!
     @brief  Clear the FIFO
 */
 /******************************************************************************/
 void Adafruit_FIFO::clear(void)
 {
-  _mutex_lock(false);
+  _mutex_lock();
   _rd_idx = _wr_idx = _count = 0;
-  _mutex_unlock(false);
+  _mutex_unlock();
 }
 
-/******************************************************************************/
-/*!
-    @brief  Write an item to the FIFO
 
-    @param[in] item
-               Memory address of the item
-*/
-/******************************************************************************/
-uint16_t Adafruit_FIFO::write(void const* item)
+void Adafruit_FIFO::_push(void const* item)
 {
-  if ( full() && !_overwritable ) return 0;
-
-  _mutex_lock(false);
-
   memcpy( _buffer + (_wr_idx * _item_size),
           item,
           _item_size);
@@ -139,8 +115,25 @@ uint16_t Adafruit_FIFO::write(void const* item)
   {
     _count++;
   }
+}
 
-  _mutex_unlock(false);
+/******************************************************************************/
+/*!
+    @brief  Write an item to the FIFO
+
+    @param[in] item
+               Memory address of the item
+*/
+/******************************************************************************/
+uint16_t Adafruit_FIFO::write(void const* item)
+{
+  if ( full() && !_overwritable ) return 0;
+
+  _mutex_lock();
+
+  _push(item);
+
+  _mutex_unlock();
 
   return 1;
 }
@@ -157,20 +150,38 @@ uint16_t Adafruit_FIFO::write(void const* item)
     @return    Number of written items
 */
 /******************************************************************************/
-uint16_t Adafruit_FIFO::write(void const * data, uint16_t n)
+uint16_t Adafruit_FIFO::write(void const * data, uint16_t count)
 {
-  if ( n == 0 ) return 0;
+  if ( count == 0 ) return 0;
+
+  _mutex_lock();
 
-  uint8_t* buf = (uint8_t*) data;
+  // Not overwritable limit up to full
+  if (!_overwritable) count = min(count, remaining());
 
-  uint16_t len = 0;
-  while( (len < n) && write(buf) )
+  uint8_t const* buf8 = (uint8_t const*) data;
+  uint16_t n = 0;
+
+  while (n < count)
   {
-    len++;
-    buf += _item_size;
+    _push(buf8);
+
+    n++;
+    buf8 += _item_size;
   }
 
-  return len;
+  _mutex_unlock();
+
+  return n;
+}
+
+void Adafruit_FIFO::_pull(void * buffer)
+{
+  memcpy(buffer,
+         _buffer + (_rd_idx * _item_size),
+         _item_size);
+  _rd_idx = (_rd_idx + 1) % _depth;
+  _count--;
 }
 
 /******************************************************************************/
@@ -185,15 +196,11 @@ uint16_t Adafruit_FIFO::read(void* buffer)
 {
   if( empty() ) return 0;
 
-  _mutex_lock(false);
+  _mutex_lock();
 
-  memcpy(buffer,
-         _buffer + (_rd_idx * _item_size),
-         _item_size);
-  _rd_idx = (_rd_idx + 1) % _depth;
-  _count--;
+  _pull(buffer);
 
-  _mutex_unlock(false);
+  _mutex_unlock();
 
   return 1;
 }
@@ -210,42 +217,31 @@ uint16_t Adafruit_FIFO::read(void* buffer)
     @return    Number of read items
 */
 /******************************************************************************/
-uint16_t Adafruit_FIFO::read(void * buffer, uint16_t n)
+uint16_t Adafruit_FIFO::read(void * buffer, uint16_t count)
 {
-  if( n == 0 ) return 0;
+  if( count == 0 ) return 0;
 
-  uint8_t* buf = (uint8_t*) buffer;
+  _mutex_lock();
 
-  uint16_t len = 0;
-  while( (len < n) && read(buf) )
-  {
-    len++;
-    buf += _item_size;
-  }
+  /* Limit up to fifo's count */
+  if ( count > _count ) count = _count;
 
-  return len;
-}
+  uint8_t* buf8 = (uint8_t*) buffer;
+  uint16_t n = 0;
 
-/******************************************************************************/
-/*!
-    @brief  Read an item without removing it from the FIFO
+  while (n < count)
+  {
+    _pull(buf8);
 
-    @param[in] buffer
-               Memory address to store item
-*/
-/******************************************************************************/
-bool Adafruit_FIFO::peek(void* buffer)
-{
-  if( empty() ) return false;
+    n++;
+    buf8 += _item_size;
+  }
 
-  memcpy(buffer,
-         _buffer + (_rd_idx * _item_size),
-         _item_size);
+  _mutex_unlock();
 
-  return true;
+  return n;
 }
 
-
 /******************************************************************************/
 /*!
     @brief  Read an item without removing it from the FIFO at the specific index

cores/nRF5/utility/adafruit_fifo.h

@@ -54,8 +54,11 @@ class Adafruit_FIFO
 
     SemaphoreHandle_t _mutex;
 
-    bool _mutex_lock(bool isr);
-    bool _mutex_unlock(bool isr);
+    bool _mutex_lock  (void) { return xSemaphoreTake(_mutex, portMAX_DELAY); }
+    bool _mutex_unlock(void) { return xSemaphoreGive(_mutex); }
+
+    void _pull(void * buffer);
+    void _push(void const* item);
 
   public:
     // Constructor
@@ -75,8 +78,8 @@ class Adafruit_FIFO
     uint16_t read(void* buffer);
     uint16_t read(void * buffer, uint16_t n);
 
-    bool peek(void* buffer);
-    bool peekAt(uint16_t position, void * p_buffer);
+    bool peekAt(uint16_t position, void * buffer);
+    bool peek(void* buffer) { return peekAt(0, buffer); }
 
     inline bool     empty(void)     { return _count == 0;      }
     inline bool     full(void)      { return _count == _depth; }