memarray: Add extend and reduce functions

sys/include/memarray.h

@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2018 Tobias Heider <heidert@nm.ifi.lmu.de>
+ *               2020 Koen Zandberg <koen@bergzand.net>
  *
  * 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
@@ -13,6 +14,7 @@
  *
  * @brief       pseudo dynamic allocation in static memory arrays
  * @author      Tobias Heider <heidert@nm.ifi.lmu.de>
+ * @author      Koen Zandberg <koen@bergzand.net>
  */
 
 #ifndef MEMARRAY_H
@@ -33,9 +35,19 @@ extern "C" {
 typedef struct {
     void *free_data;    /**< memory pool data / head of the free list */
     size_t size;        /**< size of single list element */
-    size_t num;         /**< max number of elements in list */
 } memarray_t;
 
+/**
+ * @brief Memory pool element
+ *
+ * Internal memarray element struct to increase code readability
+ *
+ * @internal
+ */
+typedef struct memarray_element {
+    struct memarray_element *next;  /**< Pointer to the next element */
+} memarray_element_t;
+
 /**
  * @brief Initialize memarray pool with free list
  *
@@ -47,7 +59,7 @@ typedef struct {
  * @param[in,out] mem    memarray pool to initialize
  * @param[in]     data   pointer to user-allocated data
  * @param[in]     size   size of a single element in data
- * @param[in]     num    number of elements in data
+ * @param[in]     num    number of elements in @p data
  */
 void memarray_init(memarray_t *mem, void *data, size_t size, size_t num);
 
@@ -110,6 +122,35 @@ static inline void memarray_free(memarray_t *mem, void *ptr)
     mem->free_data = ptr;
 }
 
+/**
+ * @brief Extend the memarray with a new memory region
+ *
+ * This function extends the memarray pool with a new memory region. The region
+ * must be able to fit the supplied number of elements of the size used when
+ * initializing this memarray.
+ *
+ * @pre `mem != NULL`
+ * @pre `data != NULL`
+ * @pre `num != 0`
+ *
+ * @param[in,out] mem   memarray pool to extend
+ * @param[in]     data  pointer to user-allocated data
+ * @param[in]     num   number of elements in @p data
+ */
+void memarray_extend(memarray_t *mem, void *data, size_t num);
+
+/**
+ * @brief Reduce the memarray space, subtracting the memory pool
+ *
+ * It is up to the user to free all chunks in the reduced pool. The function
+ * will check if all elements in the pool are freed.
+ *
+ * @param[in,out] mem   memarray pool to reduce
+ * @param[in]     data  pointer to the user-allocated data to reduce
+ * @param[in]     num   number of elements to reduce the data pool with
+ */
+int memarray_reduce(memarray_t *mem, void *data, size_t num);
+
 /**
  * @brief Returns the number of blocks available
  *

sys/memarray/memarray.c

@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2018 Tobias Heider <heidert@nm.ifi.lmu.de>
+ *               2020 Koen Zandberg <koen@bergzand.net>
  *
  * 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
@@ -8,6 +9,7 @@
 
 #include <assert.h>
 #include <string.h>
+
 #include "memarray.h"
 
 #define ENABLE_DEBUG 0
@@ -23,15 +25,81 @@ void memarray_init(memarray_t *mem, void *data, size_t size, size_t num)
 
     mem->free_data = NULL;
     mem->size = size;
-    mem->num = num;
 
+    memarray_extend(mem, data, num);
+}
+
+void memarray_extend(memarray_t *mem, void *data, size_t num)
+{
     for (uint8_t *element = data;
-         element < (uint8_t*)data + (num * size);
+         element < (uint8_t*)data + (num * mem->size);
-         element += size) {
+         element += mem->size) {
         memarray_free(mem, element);
     }
 }
 
+static bool _in_pool(const memarray_t *mem, const void *data, size_t num,
+                     const void *element)
+{
+    return element >= data &&
+        (uint8_t*)element < ((uint8_t*)data + (mem->size * num));
+}
+
+int memarray_reduce(memarray_t *mem, void *data, size_t num)
+{
+    /* Number of free chunks found inside the pool to be freed */
+    size_t remaining = num;
+
+    /* Keep a clist around of found elements in case we need to restore */
+    memarray_element_t *found_elements = NULL;
+    /* Cast it to memarray_element_t, makes things easier to read */
+    memarray_element_t **element_ptr  = (memarray_element_t**)&mem->free_data;
+
+    while (*element_ptr) {
+        DEBUG("memarray: At element %p -> %p\n",
+              (void*)*element_ptr, (void*)(*element_ptr)->next);
+        if (_in_pool(mem, data, num, *element_ptr)) {
+
+            /* Save the element */
+            memarray_element_t *found_element = *element_ptr;
+            DEBUG("memarray: Found %p in %p, at %u\n",
+                  (void*)found_element, data, (unsigned)remaining);
+
+            /* Copy pointer over to previous element remove it from the pool
+             * free list */
+            memcpy(element_ptr, (*element_ptr), sizeof(void*));
+
+            if (found_elements) {
+                found_element->next = found_elements->next;
+                found_elements->next = found_element;
+            }
+            else {
+                found_element->next = found_element;
+            }
+            found_elements = found_element;
+
+            remaining--;
+            if (remaining == 0) {
+                /* Early return if all elements are removed */
+                return 0;
+            }
+        }
+        else {
+            element_ptr = &(*element_ptr)->next;
+        }
+    }
+
+    /* Not all elements found in free list, re-add them to the memarray */
+    DEBUG("memarray: Missing elements, restoring");
+    if (found_elements) {
+        void *swap = mem->free_data;
+        mem->free_data = found_elements->next;
+        found_elements->next = swap;
+    }
+
+    return -1;
+}
+
 size_t memarray_available(memarray_t *mem)
 {
     size_t num = 0;