sys/chunked_ringbuffer: add chunked ringbuffer

sys/chunked_ringbuffer/Makefile

@@ -0,0 +1 @@
+include $(RIOTBASE)/Makefile.base

sys/chunked_ringbuffer/chunked_ringbuffer.c

@@ -0,0 +1,237 @@
+/*
+ * Copyright (C) 2021 ML!PA Consulting GmbH
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser
+ * General Public License v2.1. See the file LICENSE in the top level
+ * directory for more details.
+ */
+
+/**
+ * @{
+ *
+ * @file
+ *
+ * @author  Benjamin Valentin <benjamin.valentin@ml-pa.com>
+ */
+
+#include <string.h>
+#include "atomic_utils.h"
+#include "chunked_ringbuffer.h"
+#include "irq.h"
+
+static int _get_free_chunk(chunk_ringbuf_t *rb)
+{
+    int idx = rb->chunk_cur;
+    for (int i = 0; i < CHUNK_NUM_MAX; ++i) {
+        uintptr_t _ptr = atomic_load_uintptr((uintptr_t *)&rb->chunk_start[idx]);
+        if (_ptr == 0) {
+            return idx;
+        }
+
+        if (++idx == CHUNK_NUM_MAX) {
+            idx = 0;
+        }
+    }
+
+    return -1;
+}
+
+static int _get_complete_chunk(chunk_ringbuf_t *rb)
+{
+    int idx = rb->chunk_cur;
+    for (int i = 0; i < CHUNK_NUM_MAX; ++i) {
+        uintptr_t _ptr = atomic_load_uintptr((uintptr_t *)&rb->chunk_start[idx]);
+        if (_ptr) {
+            return idx;
+        }
+
+        if (++idx == CHUNK_NUM_MAX) {
+            idx = 0;
+        }
+    }
+
+    return -1;
+}
+
+bool crb_add_bytes(chunk_ringbuf_t *rb, const void *data, size_t len)
+{
+    const uint8_t *in = data;
+    for (size_t i = 0; i < len; ++i) {
+        if (!crb_add_byte(rb ,in[i])) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+bool crb_add_chunk(chunk_ringbuf_t *rb, const void *data, size_t len)
+{
+    if (!crb_start_chunk(rb)) {
+        return false;
+    }
+
+    bool keep = crb_add_bytes(rb ,data, len);
+
+    return crb_end_chunk(rb ,keep);
+}
+
+static unsigned _get_cur_len(chunk_ringbuf_t *rb)
+{
+    if (rb->cur > rb->cur_start) {
+        return rb->cur - rb->cur_start;
+    } else {
+        /* buffer_end point to the last element */
+        return (rb->cur - rb->buffer) + 1
+             + (rb->buffer_end - rb->cur_start);
+    }
+}
+
+bool crb_end_chunk(chunk_ringbuf_t *rb, bool keep)
+{
+    int idx;
+
+    /* no chunk was started */
+    if (rb->cur_start == NULL) {
+        return false;
+    }
+
+    if (keep) {
+        idx = _get_free_chunk(rb);
+    } else {
+        idx = -1;
+    }
+
+    /* discard chunk */
+    if (idx < 0) {
+        if (rb->protect == rb->cur_start) {
+            rb->protect = NULL;
+        }
+        rb->cur = rb->cur_start;
+        rb->cur_start = NULL;
+        return false;
+    }
+
+    /* store complete chunk */
+    rb->chunk_start[idx] = rb->cur_start;
+    rb->chunk_len[idx] = _get_cur_len(rb);
+    rb->cur_start = NULL;
+
+    return true;
+}
+
+bool crb_get_chunk_size(chunk_ringbuf_t *rb, size_t *len)
+{
+    int idx = _get_complete_chunk(rb);
+    if (idx < 0) {
+        return false;
+    }
+
+    *len = rb->chunk_len[idx];
+    return true;
+}
+
+bool crb_peek_bytes(chunk_ringbuf_t *rb, void *dst, size_t offset, size_t len)
+{
+    int idx = _get_complete_chunk(rb);
+    if (idx < 0) {
+        return false;
+    }
+
+    if (offset + len > rb->chunk_len[idx]) {
+        return false;
+    }
+
+    const uint8_t *start = rb->chunk_start[idx];
+    start += offset;
+
+    if (start > rb->buffer_end) {
+        start = ((uint8_t *)rb->buffer) + (start - rb->buffer_end + 1);
+        memcpy(dst, start, len);
+    } else if (start + len <= rb->buffer_end) {
+        memcpy(dst, start, len);
+    } else {
+        size_t len_0 = 1 + rb->buffer_end - start;
+        memcpy(dst, start, len_0);
+        memcpy((uint8_t *)dst + len_0, rb->buffer, len - len_0);
+    }
+
+    return true;
+}
+
+bool crb_chunk_foreach(chunk_ringbuf_t *rb, crb_foreach_callback_t func, void *ctx)
+{
+    size_t len;
+    int idx = _get_complete_chunk(rb);
+    if (idx < 0) {
+        return false;
+    }
+
+    len = rb->chunk_len[idx];
+
+    if (rb->chunk_start[idx] + len <= rb->buffer_end) {
+        /* chunk is continuous */
+        func(ctx, rb->chunk_start[idx], len);
+    } else {
+        /* chunk wraps around */
+        size_t len_0 = 1 + rb->buffer_end - rb->chunk_start[idx];
+        func(ctx, rb->chunk_start[idx], len_0);
+        func(ctx, rb->buffer, len - len_0);
+    }
+
+    return true;
+}
+
+bool crb_consume_chunk(chunk_ringbuf_t *rb, void *dst, size_t len)
+{
+    int idx = _get_complete_chunk(rb);
+    if (idx < 0) {
+        return false;
+    }
+
+    if (len > rb->chunk_len[idx]) {
+        len = rb->chunk_len[idx];
+    }
+
+    if (dst) {
+        if (rb->chunk_start[idx] + len <= rb->buffer_end) {
+            /* chunk is continuous */
+            memcpy(dst, rb->chunk_start[idx], len);
+        } else {
+            /* chunk wraps around */
+            uint8_t *dst8 = dst;
+            size_t len_0 = 1 + rb->buffer_end - rb->chunk_start[idx];
+            memcpy(dst8, rb->chunk_start[idx], len_0);
+            memcpy(dst8 + len_0, rb->buffer, len - len_0);
+        }
+    }
+
+    unsigned state = irq_disable();
+
+    rb->chunk_start[idx] = NULL;
+
+    /* advance protect marker */
+    idx = _get_complete_chunk(rb);
+    if (idx < 0) {
+        rb->protect = rb->cur_start;
+    } else {
+        rb->protect = rb->chunk_start[idx];
+    }
+
+    /* advance first used slot nr */
+    rb->chunk_cur = (rb->chunk_cur + 1) % CHUNK_NUM_MAX;
+
+    irq_restore(state);
+
+    return true;
+}
+
+void crb_init(chunk_ringbuf_t *rb, void *buffer, size_t len)
+{
+    memset(rb ,0, sizeof(*rb));
+    rb->buffer = buffer;
+    rb->buffer_end = &rb->buffer[len - 1];
+    rb->cur = rb->buffer;
+}
+
+/** @} */

sys/include/chunked_ringbuffer.h

@@ -0,0 +1,243 @@
+/*
+ * Copyright (C) 2021 ML!PA Consulting GmbH
+ *
+ * This file is subject to the terms and conditions of the GNU Lesser
+ * General Public License v2.1. See the file LICENSE in the top level
+ * directory for more details.
+ */
+
+/**
+ * @defgroup    sys_chunk_buffer    chunked Ringbuffer
+ * @ingroup     sys
+ * @brief       Implementation of a Ringbuffer to store chunks of data
+ * @{
+ *
+ * @file
+ * @brief   Chunked Ringbuffer
+ *
+ * A chunked ringbuffer is a ringbuffer that holds chunks of data.
+ *
+ * @author  Benjamin Valentin <benjamin.valentin@ml-pa.com>
+ */
+
+#ifndef CHUNKED_RINGBUFFER_H
+#define CHUNKED_RINGBUFFER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief The maximum number of chunks that can be stored in a Chunked Ringbuffer
+ *
+ */
+#ifndef CHUNK_NUM_MAX
+#define CHUNK_NUM_MAX   (4)
+#endif
+
+/**
+ * @brief A chunked ringbuffer
+ *
+ */
+typedef struct {
+    uint8_t *buffer;        /**< pointer to the memory to hold the data */
+    uint8_t *buffer_end;    /**< last data element */
+    uint8_t *cur;           /**< current write pointer */
+    uint8_t *cur_start;     /**< start of the currently written chunk */
+    uint8_t *protect;       /**< start of the first valid chunk */
+    uint8_t *chunk_start[CHUNK_NUM_MAX];    /**< Array to hold start of done chunks */
+    uint16_t chunk_len[CHUNK_NUM_MAX];      /**< Length of valid chunks */
+    uint8_t chunk_cur;                      /**< Index of the first valid chunk */
+} chunk_ringbuf_t;
+
+/**
+ * @brief Callback function for @ref crb_chunk_foreach
+ *
+ * @param[in] ctx   Callback context
+ * @param[in] bytes Chunk data
+ * @param[in] len   Length of data
+ */
+typedef void (*crb_foreach_callback_t)(void *ctx, uint8_t *bytes, size_t len);
+
+/**
+ * @brief Initialize a Chunked Ringbuffer
+ *
+ * @param[in] rb    The Ringbuffer to work on
+ * @param buffer    The Ringbuffer work area
+ * @param len       Size of the Ringbuffer work area
+ */
+void crb_init(chunk_ringbuf_t *rb, void *buffer, size_t len);
+
+/**
+ * @brief Start a new chunk on the ringbuffer
+ *
+ * @note  This function is expected to be called in ISR context / with
+ *        interrupts disabled.
+ *
+ * @param[in] rb    The Ringbuffer to work on
+ *
+ * @return true     If a new chunk could be started
+ * @return false    If the ringbuffer is full
+ */
+static inline bool crb_start_chunk(chunk_ringbuf_t *rb)
+{
+    /* pointing to the start of the first chunk */
+    if (rb->cur == rb->protect) {
+        return false;
+    }
+
+    rb->cur_start = rb->cur;
+
+    if (rb->protect == NULL) {
+        rb->protect = rb->cur_start;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Insert a byte into the current chunk
+ *
+ * @note  This function is expected to be called in ISR context / with
+ *        interrupts disabled.
+ *
+ * @pre A new chunk has been started with @ref crb_start_chunk
+ *
+ * @param[in] rb        The Ringbuffer to work on
+ * @param[in] b         The byte to write
+ *
+ * @return true         If the byte could be written
+ * @return false        If the ringbuffer is full
+ */
+static inline bool crb_add_byte(chunk_ringbuf_t *rb, uint8_t b)
+{
+    /* if this is the first chunk, protect will be at start */
+    if (rb->cur == rb->protect &&
+        rb->cur != rb->cur_start) {
+        return false;
+    }
+
+    *rb->cur = b;
+
+    /* handle wrap around */
+    if (rb->cur == rb->buffer_end) {
+        rb->cur = rb->buffer;
+    } else {
+        ++rb->cur;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Insert a number of bytes into the current chunk
+ *
+ * @note  This function is expected to be called in ISR context / with
+ *        interrupts disabled.
+ *
+ * @pre A new chunk has been started with @ref crb_start_chunk
+ *
+ * @param[in] rb        The Ringbuffer to work on
+ * @param[in] data      The data to write
+ * @param[in] len       Size of data
+ *
+ * @return true         If the bytes could be written
+ * @return false        If the ringbuffer is full
+ */
+bool crb_add_bytes(chunk_ringbuf_t *rb, const void *data, size_t len);
+
+/**
+ * @brief Close the current chunk
+ *
+ * @note  This function is expected to be called in ISR context / with
+ *        interrupts disabled.
+ *
+ * @param[in] rb        The Ringbuffer to work on
+ * @param[in] valid     True if the chunk is valid and should be stored
+ *                      False if the current chunk should be discarded
+ *
+ * @return true         If the chunk could be stored in the valid chunk array
+ * @return false        If there is no more space in the valid chunk array
+ */
+bool crb_end_chunk(chunk_ringbuf_t *rb, bool valid);
+
+/**
+ * @brief Add a complete chunk to the Ringbuffer
+ *
+ * @note  This function is expected to be called in ISR context / with
+ *        interrupts disabled.
+ *
+ * This is a convenience function that combines @ref crb_start_chunk,
+ * @ref crb_add_bytes and @ref crb_end_chunk
+ *
+ * @param[in] rb        The Ringbuffer to work on
+ * @param[in] data      The data to write
+ * @param[in] len       Size of data
+ *
+ * @return true         If the chunk could be added to the valid chunk array
+ * @return false        There was not enough space and the chunk was discarded
+ */
+bool crb_add_chunk(chunk_ringbuf_t *rb, const void *data, size_t len);
+
+/**
+ * @brief Get the size of the first valid chunk
+ *
+ * @param[in] rb        The Ringbuffer to work on
+ * @param[out] len      Pointer to store the size of the first valid chunk
+ *
+ * @return true         If a valid chunk exists and @p size was written
+ * @return false        If no valid chunk exists
+ */
+bool crb_get_chunk_size(chunk_ringbuf_t *rb, size_t *len);
+
+/**
+ * @brief Get a number of bytes from the first valid chunk without consuming it.
+ *
+ * @param[in] rb        The Ringbuffer to work on
+ * @param[out] dst      Destination buffer
+ * @param[in] offset    Offset to the start of the chunk
+ * @param[in] len       Number of bytes to read
+ *
+ * @return true         If the data could be read
+ * @return false        If no valid chunk exists or the bytes could not be read
+ */
+bool crb_peek_bytes(chunk_ringbuf_t *rb, void *dst, size_t offset, size_t len);
+
+/**
+ * @brief Remove a chunk from the valid chunk array
+ *
+ * @param[in] rb        The Ringbuffer to work on
+ * @param[out] dst      Destination where the chunk contents should be copied to.
+ *                      May be NULL, then the chunk is just discarded.
+ * @param[in] len       Max number of bytes to read. If there are bytes left in the
+ *                      chunk beyond that, they will be discarded
+ *
+ * @return true         If a chunk was consumed
+ * @return false        If no valid chunk did exist
+ */
+bool crb_consume_chunk(chunk_ringbuf_t *rb, void *dst, size_t len);
+
+/**
+ * @brief Execute a callback for each byte in the first valid chunk
+ *        The callback function may be called twice if the chunk is non-continuous.
+ *
+ *        This function will not consume the chunk.
+ *
+ * @param[in] rb        The Ringbuffer to work on
+ * @param[in] func      The function to call for each byte
+ * @param[in] ctx       Optional function argument
+ *
+ * @return true         If a valid chunk exits on which the function was executed
+ * @return false        If no valid chunk exists
+ */
+bool crb_chunk_foreach(chunk_ringbuf_t *rb, crb_foreach_callback_t func, void *ctx);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CHUNKED_RINGBUFFER_H */
+/** @} */