Diff: kal_buf_circ.cpp

Revision:

9:0140247bab90

Child:

10:c87566aded6e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kal_buf_circ.cpp	Mon Jan 25 09:45:10 2021 +0000
@@ -0,0 +1,378 @@
+/// @copyright
+/// ========================================================================={{{
+/// Copyright (c) 2019 WizziLab                                                /
+/// All rights reserved                                                        /
+///                                                                            /
+/// IMPORTANT: This Software may not be modified, copied or distributed unless /
+/// embedded on a WizziLab product. Other than for the foregoing purpose, this /
+/// Software and/or its documentation may not be used, reproduced, copied,     /
+/// prepared derivative works of, modified, performed, distributed, displayed  /
+/// or sold for any purpose. For the sole purpose of embedding this Software   /
+/// on a WizziLab product, copy, modification and distribution of this         /
+/// Software is granted provided that the following conditions are respected:  /
+///                                                                            /
+/// *  Redistributions of source code must retain the above copyright notice,  /
+///    this list of conditions and the following disclaimer                    /
+///                                                                            /
+/// *  Redistributions in binary form must reproduce the above copyright       /
+///    notice, this list of conditions and the following disclaimer in the     /
+///    documentation and/or other materials provided with the distribution.    /
+///                                                                            /
+/// *  The name of WizziLab can not be used to endorse or promote products     /
+///    derived from this software without specific prior written permission.   /
+///                                                                            /
+/// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS        /
+/// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED  /
+/// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR /
+/// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR          /
+/// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,      /
+/// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,        /
+/// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,            /
+/// OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY     /
+/// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING    /
+/// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS         /
+/// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.               /
+/// WIZZILAB HAS NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES,       /
+/// ENHANCEMENTS OR MODIFICATIONS.                                             /
+///                                                                            /
+/// Should you have any questions regarding your right to use this Software,   /
+/// contact WizziLab at www.wizzilab.com.                                      /
+///                                                                            /
+/// =========================================================================}}}
+/// @endcopyright
+
+/// XXX: New code for reference. Still to be tested.
+
+#include "kal_buf_circ.h"
+
+#if defined(KAL_MALLOC) && defined(KAL_FREE)
+#ifndef CIRCULAR_BUFFER_MALLOC
+#define CIRCULAR_BUFFER_MALLOC(_s) KAL_MALLOC(_s)
+#endif
+#ifndef CIRCULAR_BUFFER_FREE
+#define CIRCULAR_BUFFER_FREE(_p) KAL_FREE(_p)
+#endif
+#else
+#define CIRCULAR_BUFFER_STATIC_ONLY
+#endif
+
+static __inline uint32_t kal_buf_circ_next_head(kal_buf_circ_static_handle_t* h, uint32_t nb_elements)
+{
+    uint32_t next = h->head + (nb_elements * h->element_size);
+    return (next >= h->max_length)? next - h->max_length : next;
+}
+
+static __inline uint32_t kal_buf_circ_next_tail(kal_buf_circ_static_handle_t* h, uint32_t nb_elements)
+{
+    uint32_t next = h->tail + (nb_elements * h->element_size);
+    return (next >= h->max_length)? next - h->max_length : next;
+}
+
+#ifndef CIRCULAR_BUFFER_STATIC_ONLY
+kal_buf_circ_handle_t kal_buf_circ_create(uint32_t nb_elements, uint32_t element_size)
+{
+    // Malloc the h and the buffer
+    kal_buf_circ_static_handle_t* h = CIRCULAR_BUFFER_MALLOC(sizeof(kal_buf_circ_static_handle_t));
+
+    h->buffer = (uint8_t*)CIRCULAR_BUFFER_MALLOC((nb_elements + 1) * element_size);
+    h->head = 0;
+    h->tail = 0;
+    h->element_size = element_size;
+    h->max_length = (nb_elements + 1) * element_size;
+    h->is_static = false;
+
+    return (kal_buf_circ_handle_t)h;
+}
+#endif
+
+int kal_buf_circ_create_static(kal_buf_circ_static_handle_t* static_handle, uint8_t* buffer, uint32_t nb_elements, uint32_t element_size)
+{
+    // h and buffer are passed as parameters.
+    kal_buf_circ_static_handle_t* h = static_handle;
+
+    h->buffer = buffer;
+    h->head = 0;
+    h->tail = 0;
+    h->element_size = element_size;
+    h->max_length = (nb_elements + 1) * element_size;
+    h->is_static = true;
+
+    return 0;
+}
+
+int kal_buf_circ_delete(kal_buf_circ_handle_t handle)
+{
+    kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle;
+
+    if (h->is_static)
+    {
+        // Set h to 0
+        memset(h->buffer, 0, h->max_length);
+        memset(h, 0, sizeof(kal_buf_circ_static_handle_t));
+    }
+#ifndef CIRCULAR_BUFFER_STATIC_ONLY
+    else
+    {
+        CIRCULAR_BUFFER_FREE(h->buffer);
+        CIRCULAR_BUFFER_FREE(h);
+    }
+#endif
+
+    return 0;
+}
+
+int kal_buf_circ_reset(kal_buf_circ_handle_t handle)
+{
+    kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle;
+
+    h->head = 0;
+    h->tail = 0;
+    memset(h->buffer, 0, h->max_length);
+
+    return 0;
+}
+
+__inline int kal_buf_circ_full(kal_buf_circ_handle_t handle)
+{
+    kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle;
+
+    // if the next head is the tail, circular buffer is full
+    return (kal_buf_circ_next_head(h, 1) == h->tail)? true : false;
+}
+
+__inline int kal_buf_circ_empty(kal_buf_circ_handle_t handle)
+{
+    kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle;
+
+    // if the head isn't ahead of the tail, we don't have any elements
+    return (h->head == h->tail)? true : false;
+}
+
+uint32_t kal_buf_circ_size(kal_buf_circ_handle_t handle)
+{
+    kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle;
+
+    // head is ahead of tail
+    if (h->head >= h->tail)
+    {
+        // return the difference
+        return (h->head - h->tail) / h->element_size;
+    }
+    else
+    {
+        // else add max_length
+        return (h->max_length + h->head - h->tail) / h->element_size;
+    }
+}
+
+int kal_buf_circ_push(kal_buf_circ_handle_t handle, uint8_t* p)
+{
+    kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle;
+
+    // check if circular buffer is full
+    if (kal_buf_circ_full(handle))
+    {
+        return -1; // and return with an error.
+    }
+
+    // next is where head will point to after this write.
+    uint32_t next = kal_buf_circ_next_head(h, 1);
+
+    // Load data and then move
+    memcpy(&(h->buffer[h->head]), p, h->element_size);
+
+    // head to next data offset.
+    h->head = next;
+
+    // return success to indicate successful push.
+    return 0;
+}
+
+int kal_buf_circ_pop(kal_buf_circ_handle_t handle, uint8_t* p)
+{
+    kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle;
+
+    // check if circular buffer is empty
+    if (kal_buf_circ_empty(handle))
+    {
+        return -1;          // and return with an error
+    }
+
+    // next is where tail will point to after this read.
+    uint32_t next = kal_buf_circ_next_tail(h, 1);
+
+    if (p)
+    {
+        // Read data and then move
+        memcpy(p, &(h->buffer[h->tail]), h->element_size);
+    }
+
+    // tail to next data offset.
+    h->tail = next;
+
+    // return success to indicate successful pop.
+    return 0;
+}
+
+int kal_buf_circ_peek(kal_buf_circ_handle_t handle, uint8_t* p)
+{
+    kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle;
+
+    // check if circular buffer is empty
+    if (kal_buf_circ_empty(handle))
+    {
+        return -1;          // and return with an error
+    }
+
+    // Read data
+    memcpy(p, &(h->buffer[h->tail]), h->element_size);
+
+    // return success to indicate successful peek.
+    return 0;
+}
+
+int kal_buf_circ_put(kal_buf_circ_handle_t handle, uint8_t* p, uint32_t nb_elements)
+{
+    kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle;
+
+    // Check if there is enough available spots in the buffer
+    if (nb_elements + kal_buf_circ_size(handle) >= h->max_length / h->element_size)
+    {
+        return -1;          // and return with an error
+    }
+
+    // check if write should be done in two parts
+    uint32_t available_now = (h->max_length - h->head) / h->element_size;
+    if (nb_elements > available_now)
+    {
+        // write first part up to max_length
+        if (kal_buf_circ_put(handle, p, available_now))
+        {
+            return -1;
+        }
+
+        // increment write pointer
+        p += available_now * h->element_size;
+        // subtract what we just wrote
+        nb_elements -= available_now;
+    }
+
+    // next is where head will point to after this write.
+    uint32_t next = kal_buf_circ_next_head(h, nb_elements);
+
+    // write data
+    memcpy(&(h->buffer[h->head]), p, nb_elements * h->element_size);
+
+    // head to next data offset.
+    h->head = next;
+
+    // return success to indicate successful get.
+    return 0;
+}
+
+int kal_buf_circ_get(kal_buf_circ_handle_t handle, uint8_t* p, uint32_t nb_elements)
+{
+    kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle;
+
+    // Check if there is enough elements in the buffer
+    if (nb_elements > kal_buf_circ_size(handle))
+    {
+        return -1;          // and return with an error
+    }
+
+    // check if read should be done in two parts
+    uint32_t available_now = (h->max_length - h->tail) / h->element_size;
+    if (nb_elements > available_now)
+    {
+        // get first part up to max_length
+        if (kal_buf_circ_get(handle, p, available_now))
+        {
+            return -1;
+        }
+
+        if (p)
+        {
+            // increment read pointer
+            p += available_now * h->element_size;
+        }
+        // subtract what we just read
+        nb_elements -= available_now;
+    }
+
+    // next is where tail will point to after this read.
+    uint32_t next = kal_buf_circ_next_tail(h, nb_elements);
+
+    if (p)
+    {
+        // read data
+        memcpy(p, &(h->buffer[h->tail]), nb_elements * h->element_size);
+    }
+
+    // tail to next data offset.
+    h->tail = next;
+
+    // return success to indicate successful get.
+    return 0;
+}
+
+int kal_buf_circ_fetch(kal_buf_circ_handle_t handle, uint8_t* p, uint32_t nb_elements)
+{
+    kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle;
+
+    // Check if there is enough elements in the buffer
+    if (nb_elements > kal_buf_circ_size(handle))
+    {
+        return -1; // and return with an error
+    }
+
+    // check if read should be done in two parts
+    uint32_t available_now = (h->max_length - h->tail) / h->element_size;
+    if (nb_elements > available_now)
+    {
+        // get first part from tail up to max_length
+        memcpy(p, &(h->buffer[h->tail]), available_now * h->element_size);
+
+        // increment read pointer
+        p += available_now * h->element_size;
+
+        // subtract what we just read
+        nb_elements -= available_now;
+
+        // get second part from 0 up to remaining length
+        memcpy(p, &(h->buffer[0]), nb_elements * h->element_size);
+    }
+    else
+    {
+        // get data from tail
+        memcpy(p, &(h->buffer[h->tail]), nb_elements * h->element_size);
+    }
+
+    // return success to indicate successful fetch.
+    return 0;
+}
+
+int kal_buf_circ_erase(kal_buf_circ_handle_t handle)
+{
+    kal_buf_circ_static_handle_t* h = (kal_buf_circ_static_handle_t*)handle;
+
+    // check if circular buffer is empty
+    if (kal_buf_circ_empty(handle))
+    {
+        return -1; // and return with an error
+    }
+
+    // head is at 0, we wrap back
+    if (h->head == 0)
+    {
+        h->head = h->max_length - h->element_size;
+    }
+    // else go back one element
+    else
+    {
+        h->head = h->head - h->element_size;
+    }
+
+    // return success to indicate successful erase.
+    return 0;
+}
+