Initial Release

src/common/pico_util/CMakeLists.txt

@@ -0,0 +1,15 @@
+if (NOT TARGET pico_util_headers)
+    add_library(pico_util_headers INTERFACE)
+    target_include_directories(pico_util_headers INTERFACE ${CMAKE_CURRENT_LIST_DIR}/include)
+    target_link_libraries(pico_util_headers INTERFACE pico_base_headers hardware_sync)
+endif()
+
+if (NOT TARGET pico_util)
+    add_library(pico_util INTERFACE)
+    target_sources(pico_util INTERFACE
+            ${CMAKE_CURRENT_LIST_DIR}/datetime.c
+            ${CMAKE_CURRENT_LIST_DIR}/pheap.c
+            ${CMAKE_CURRENT_LIST_DIR}/queue.c
+    )
+    target_link_libraries(pico_util INTERFACE pico_util_headers)
+endif()

src/common/pico_util/datetime.c

@@ -0,0 +1,41 @@
+#include "pico/util/datetime.h"
+
+#include <stdio.h>
+
+static const char *DATETIME_MONTHS[12] = {
+        "January",
+        "February",
+        "March",
+        "April",
+        "May",
+        "June",
+        "July",
+        "August",
+        "September",
+        "October",
+        "November",
+        "December"
+};
+
+static const char *DATETIME_DOWS[7] = {
+        "Sunday",
+        "Monday",
+        "Tuesday",
+        "Wednesday",
+        "Thursday",
+        "Friday",
+        "Saturday",
+};
+
+void datetime_to_str(char *buf, uint buf_size, const datetime_t *t) {
+    snprintf(buf,
+             buf_size,
+             "%s %d %s %d:%02d:%02d %d",
+             DATETIME_DOWS[t->dotw],
+             t->day,
+             DATETIME_MONTHS[t->month - 1],
+             t->hour,
+             t->min,
+             t->sec,
+             t->year);
+};

src/common/pico_util/doc.h

@@ -0,0 +1,4 @@
+/**
+ * \defgroup pico_util pico_util
+ * \brief Useful data structures and utility functions
+ */

src/common/pico_util/include/pico/util/datetime.h

@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_DATETIME_H
+#define _PICO_DATETIME_H
+
+#include "pico.h"
+
+/** \file datetime.h
+ * \defgroup util_datetime datetime
+ * \brief Date/Time formatting
+ * \ingroup pico_util
+ */
+
+/*! \brief  Convert a datetime_t structure to a string
+ *  \ingroup util_datetime
+ *
+ * \param buf character buffer to accept generated string
+ * \param buf_size The size of the passed in buffer
+ * \param t The datetime to be converted.
+ */
+void datetime_to_str(char *buf, uint buf_size, const datetime_t *t);
+
+#endif

src/common/pico_util/include/pico/util/pheap.h

@@ -0,0 +1,155 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_UTIL_PHEAP_H
+#define _PICO_UTIL_PHEAP_H
+
+#include "pico.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// PICO_CONFIG: PARAM_ASSERTIONS_ENABLED_PHEAP, Enable/disable assertions in the pheap module, type=bool, default=0, group=pico_util
+#ifndef PARAM_ASSERTIONS_ENABLED_PHEAP
+#define PARAM_ASSERTIONS_ENABLED_PHEAP 0
+#endif
+
+/**
+ * \file pheap.h
+ * \defgroup util_pheap pheap
+ * Pairing Heap Implementation
+ * \ingroup pico_util
+ *
+ * pheap defines a simple pairing heap. the implementation simply tracks array indexes, it is up to
+ * the user to provide storage for heap entries and a comparison function.
+ *
+ * NOTE: this class is not safe for concurrent usage. It should be externally protected. Furthermore
+ * if used concurrently, the caller needs to protect around their use of the returned id.
+ * for example, ph_remove_head returns the id of an element that is no longer in the heap.
+ *
+ * The user can still use this to look at the data in their companion array, however obviously further operations
+ * on the heap may cause them to overwrite that data as the id may be reused on subsequent operations
+ *
+ */
+// PICO_CONFIG: PICO_PHEAP_MAX_ENTRIES, Maximum number of entries in the pheap, min=1, max=65534, default=255, group=pico_util
+#ifndef PICO_PHEAP_MAX_ENTRIES
+#define PICO_PHEAP_MAX_ENTRIES 255
+#endif
+
+// public heap_node ids are numbered from 1 (0 means none)
+#if PICO_PHEAP_MAX_ENTRIES < 256
+typedef uint8_t pheap_node_id_t;
+#elif PICO_PHEAP_MAX_ENTRIES < 65535
+typedef uint16_t pheap_node_id_t;
+#else
+#error invalid PICO_PHEAP_MAX_ENTRIES
+#endif
+
+typedef struct pheap_node {
+    pheap_node_id_t child, sibling, parent;
+} pheap_node_t;
+
+// return true if a < b in natural order
+typedef bool (*pheap_comparator)(void *user_data, pheap_node_id_t a, pheap_node_id_t b);
+
+typedef struct pheap {
+    pheap_node_t *nodes;
+    pheap_comparator comparator;
+    void *user_data;
+    pheap_node_id_t max_nodes;
+    pheap_node_id_t root_id;
+    // we remove from head and add to tail to stop reusing the same ids
+    pheap_node_id_t free_head_id;
+    pheap_node_id_t free_tail_id;
+} pheap_t;
+
+pheap_t *ph_create(uint max_nodes, pheap_comparator comparator, void *user_data);
+
+void ph_clear(pheap_t *heap);
+
+void ph_destroy(pheap_t *heap);
+
+static inline pheap_node_t *ph_get_node(pheap_t *heap, pheap_node_id_t id) {
+    assert(id && id <= heap->max_nodes);
+    return heap->nodes + id - 1;
+}
+
+static void ph_add_child_node(pheap_t *heap, pheap_node_id_t parent_id, pheap_node_id_t child_id) {
+    pheap_node_t *n = ph_get_node(heap, parent_id);
+    assert(parent_id);
+    assert(child_id);
+    assert(parent_id != child_id);
+    pheap_node_t *c = ph_get_node(heap, child_id);
+    c->parent = parent_id;
+    if (!n->child) {
+        n->child = child_id;
+    } else {
+        c->sibling = n->child;
+        n->child = child_id;
+    }
+}
+
+static pheap_node_id_t ph_merge_nodes(pheap_t *heap, pheap_node_id_t a, pheap_node_id_t b) {
+    if (!a) return b;
+    if (!b) return a;
+    if (heap->comparator(heap->user_data, a, b)) {
+        ph_add_child_node(heap, a, b);
+        return a;
+    } else {
+        ph_add_child_node(heap, b, a);
+        return b;
+    }
+}
+
+static inline pheap_node_id_t ph_new_node(pheap_t *heap) {
+    if (!heap->free_head_id) return 0;
+    pheap_node_id_t id = heap->free_head_id;
+    heap->free_head_id = ph_get_node(heap, id)->sibling;
+    if (!heap->free_head_id) heap->free_tail_id = 0;
+    return id;
+}
+
+// note this will callback the comparator for the node
+// returns the (new) root of the heap
+static inline pheap_node_id_t ph_insert(pheap_t *heap, pheap_node_id_t id) {
+    assert(id);
+    pheap_node_t *hn = ph_get_node(heap, id);
+    hn->child = hn->sibling = hn->parent = 0;
+    heap->root_id = ph_merge_nodes(heap, heap->root_id, id);
+    return heap->root_id;
+}
+
+static inline pheap_node_id_t ph_peek_head(pheap_t *heap) {
+    return heap->root_id;
+}
+
+pheap_node_id_t ph_remove_head_reserve(pheap_t *heap, bool reserve);
+
+static inline pheap_node_id_t ph_remove_head(pheap_t *heap) {
+    return ph_remove_head_reserve(heap, false);
+}
+
+static inline bool ph_contains(pheap_t *heap, pheap_node_id_t id) {
+    return id == heap->root_id || ph_get_node(heap, id)->parent;
+}
+
+bool ph_delete(pheap_t *heap, pheap_node_id_t id);
+
+static inline void ph_add_to_free_list(pheap_t *heap, pheap_node_id_t id) {
+    assert(id && !ph_contains(heap, id));
+    if (heap->free_tail_id) {
+        ph_get_node(heap, heap->free_tail_id)->sibling = id;
+    }
+    heap->free_tail_id = id;
+}
+
+void ph_dump(pheap_t *heap, void (*dump_key)(pheap_node_id_t, void *), void *user_data);
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/common/pico_util/include/pico/util/queue.h

@@ -0,0 +1,184 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _UTIL_QUEUE_H
+#define _UTIL_QUEUE_H
+
+#include "pico.h"
+#include "hardware/sync.h"
+
+/** \file queue.h
+ * \defgroup queue queue
+ * Multi-core and IRQ safe queue implementation.
+ *
+ * Note that this queue stores values of a specified size, and pushed values are copied into the queue
+ * \ingroup pico_util
+ */
+
+typedef struct {
+    spin_lock_t *lock;
+    uint8_t *data;
+    uint16_t wptr;
+    uint16_t rptr;
+    uint16_t element_size;
+    uint16_t element_count;
+} queue_t;
+
+/*! \brief Initialise a queue with a specific spinlock for concurrency protection
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param element_size Size of each value in the queue
+ * \param element_count Maximum number of entries in the queue
+ * \param spinlock_num The spin ID used to protect the queue
+ */
+void queue_init_with_spinlock(queue_t *q, uint element_size, uint element_count, uint spinlock_num);
+
+/*! \brief Initialise a queue, allocating a (possibly shared) spinlock
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param element_size Size of each value in the queue
+ * \param element_count Maximum number of entries in the queue
+ */
+static inline void queue_init(queue_t *q, uint element_size, uint element_count) {
+    return queue_init_with_spinlock(q, element_size, element_count, next_striped_spin_lock_num());
+}
+
+/*! \brief Destroy the specified queue.
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ *
+ * Does not deallocate the queue_t structure itself.
+ */
+void queue_free(queue_t *q);
+
+/*! \brief Unsafe check of level of the specified queue.
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \return Number of entries in the queue
+ *
+ * This does not use the spinlock, so may return incorrect results if the
+ * spin lock is not externally locked
+ */
+static inline uint queue_get_level_unsafe(queue_t *q) {
+    int32_t rc = (int32_t)q->wptr - (int32_t)q->rptr;
+    if (rc < 0) {
+        rc += + q->element_count + 1;
+    }
+    return rc;
+}
+
+/*! \brief Check of level of the specified queue.
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \return Number of entries in the queue
+ */
+static inline uint queue_get_level(queue_t *q) {
+    uint32_t save = spin_lock_blocking(q->lock);
+    uint level = queue_get_level_unsafe(q);
+    spin_unlock(q->lock, save);
+    return level;
+}
+
+/*! \brief Check if queue is empty
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \return true if queue is empty, false otherwise
+ *
+ * This function is interrupt and multicore safe.
+ */
+static inline bool queue_is_empty(queue_t *q) {
+    return queue_get_level(q) == 0;
+}
+
+/*! \brief Check if queue is full
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \return true if queue is full, false otherwise
+ *
+ * This function is interrupt and multicore safe.
+ */
+static inline bool queue_is_full(queue_t *q) {
+    return queue_get_level(q) == q->element_count;
+}
+
+// nonblocking queue access functions:
+
+/*! \brief Non-blocking add value queue if not full
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param data Pointer to value to be copied into the queue
+ * \return true if the value was added
+ *
+ * If the queue is full this function will return immediately with false, otherwise
+ * the data is copied into a new value added to the queue, and this function will return true.
+ */
+bool queue_try_add(queue_t *q, void *data);
+
+/*! \brief Non-blocking removal of entry from the queue if non empty
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param data Pointer to the location to receive the removed value
+ * \return true if a value was removed
+ *
+ * If the queue is not empty function will copy the removed value into the location provided and return
+ * immediately with true, otherwise the function will return immediately with false.
+ */
+bool queue_try_remove(queue_t *q, void *data);
+
+/*! \brief Non-blocking peek at the next item to be removed from the queue
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param data Pointer to the location to receive the peeked value
+ * \return true if there was a value to peek
+ *
+ * If the queue is not empty this function will return immediately with true with the peeked entry
+ * copied into the location specified by the data parameter, otherwise the function will return false.
+ */
+bool queue_try_peek(queue_t *q, void *data);
+
+// blocking queue access functions:
+
+/*! \brief Blocking add of value to queue
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param data Pointer to value to be copied into the queue
+ *
+ * If the queue is full this function will block, until a removal happens on the queue
+ */
+void queue_add_blocking(queue_t *q, void *data);
+
+/*! \brief Blocking remove entry from queue
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param data Pointer to the location to receive the removed value
+ *
+ * If the queue is empty this function will block until a value is added.
+ */
+void queue_remove_blocking(queue_t *q, void *data);
+
+/*! \brief Blocking peek at next value to be removed from queue
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \param data Pointer to the location to receive the peeked value
+ *
+ * If the queue is empty function will block until a value is added
+ */
+void queue_peek_blocking(queue_t *q, void *data);
+
+#endif

src/common/pico_util/pheap.c

@@ -0,0 +1,130 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "pico/util/pheap.h"
+
+pheap_t *ph_create(uint max_nodes, pheap_comparator comparator, void *user_data) {
+    invalid_params_if(PHEAP, !max_nodes || max_nodes >= (1u << sizeof(pheap_node_id_t)));
+    pheap_t *heap = calloc(1, sizeof(pheap_t));
+    heap->max_nodes = max_nodes;
+    heap->comparator = comparator;
+    heap->nodes = calloc(max_nodes, sizeof(pheap_node_t));
+    heap->user_data = user_data;
+    ph_clear(heap);
+    return heap;
+}
+
+void ph_clear(pheap_t *heap) {
+    heap->root_id = 0;
+    heap->free_head_id = 1;
+    heap->free_tail_id = heap->max_nodes;
+    for(uint i = 1; i < heap->max_nodes; i++) {
+        ph_get_node(heap, i)->sibling = i + 1;
+    }
+    ph_get_node(heap, heap->max_nodes)->sibling = 0;
+}
+
+void ph_destroy(pheap_t *heap) {
+    free(heap->nodes);
+    free(heap);
+}
+
+pheap_node_id_t ph_merge_two_pass(pheap_t *heap, pheap_node_id_t id) {
+    if (!id || !ph_get_node(heap, id)->sibling) {
+        return id;
+    } else {
+        pheap_node_id_t a, b, new_node;
+        a = id;
+        b = ph_get_node(heap, id)->sibling;
+        new_node = ph_get_node(heap, b)->sibling;
+        ph_get_node(heap, a)->sibling = ph_get_node(heap, b)->sibling = 0;
+        return ph_merge_nodes(heap, ph_merge_nodes(heap, a, b), ph_merge_two_pass(heap, new_node));
+    }
+}
+
+static pheap_node_id_t ph_remove_any_head(pheap_t *heap, pheap_node_id_t root_id, bool reserve) {
+    assert(root_id);
+//    printf("Removing head %d (parent %d sibling %d)\n", root_id, ph_get_node(heap, root_id)->parent, ph_get_node(heap, root_id)->sibling);
+    assert(!ph_get_node(heap, root_id)->sibling);
+    assert(!ph_get_node(heap, root_id)->parent);
+    pheap_node_id_t new_root_id = ph_merge_two_pass(heap, ph_get_node(heap, root_id)->child);
+    if (!reserve) {
+        if (heap->free_tail_id) {
+            ph_get_node(heap, heap->free_tail_id)->sibling = root_id;
+        }
+        heap->free_tail_id = root_id;
+    }
+    if (new_root_id) ph_get_node(heap, new_root_id)->parent = 0;
+    ph_get_node(heap, root_id)->sibling = 0;
+    return new_root_id;
+}
+
+pheap_node_id_t ph_remove_head_reserve(pheap_t *heap, bool reserve) {
+    pheap_node_id_t old_root_id = ph_peek_head(heap);
+    heap->root_id = ph_remove_any_head(heap, old_root_id, reserve);
+    return old_root_id;
+}
+
+#include <stdio.h>
+bool ph_delete(pheap_t *heap, pheap_node_id_t id) {
+    // 1) trivial cases
+    if (!id) return false;
+    if (id == heap->root_id) {
+        ph_remove_head(heap);
+        return true;
+    }
+    // 2) unlink the node from the tree
+    pheap_node_t *node = ph_get_node(heap, id);
+    if (!node->parent) return false; // not in tree
+    pheap_node_t *parent = ph_get_node(heap, node->parent);
+    if (parent->child == id) {
+        parent->child = node->sibling;
+    } else {
+        pheap_node_id_t prev_sibling_id = parent->child;
+        bool __unused found = false;
+        do {
+            pheap_node_t *prev_sibling = ph_get_node(heap, prev_sibling_id);
+            if (prev_sibling->sibling == id) {
+                prev_sibling->sibling = node->sibling;
+                found = true;
+                break;
+            }
+            prev_sibling_id = prev_sibling->sibling;
+        } while (prev_sibling_id);
+        assert(found);
+    }
+    node->sibling = node->parent = 0;
+//    ph_dump(heap, NULL, NULL);
+    // 3) remove it from the head of its own subtree
+    pheap_node_id_t new_sub_tree = ph_remove_any_head(heap, id, false);
+    assert(new_sub_tree != heap->root_id);
+    heap->root_id = ph_merge_nodes(heap, heap->root_id, new_sub_tree);
+    return true;
+}
+
+static uint ph_dump_node(pheap_t *heap, pheap_node_id_t id, void (*dump_key)(pheap_node_id_t, void *), void *user_data, uint indent) {
+    uint count = 0;
+    if (id) {
+        count++;
+        for (uint i = 0; i < indent * 2; i++) {
+            putchar(' ');
+        }
+        pheap_node_t *node = ph_get_node(heap, id);
+        printf("%d (c=%d s=%d p=%d) ", id, node->child, node->sibling, node->parent);
+        if (dump_key) dump_key(id, user_data);
+        printf("\n");
+        count += ph_dump_node(heap, node->child, dump_key, user_data, indent + 1);
+        count += ph_dump_node(heap, node->sibling, dump_key, user_data, indent);
+    }
+    return count;
+}
+
+void ph_dump(pheap_t *heap, void (*dump_key)(pheap_node_id_t, void *), void *user_data) {
+    uint count = ph_dump_node(heap, heap->root_id, dump_key, user_data, 0);
+    printf("node_count %d\n", count);
+}

src/common/pico_util/queue.c

@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include "pico/util/queue.h"
+
+void queue_init_with_spinlock(queue_t *q, uint element_size, uint element_count, uint spinlock_num) {
+    q->lock = spin_lock_instance(spinlock_num);
+    q->data = (uint8_t *)calloc(element_count + 1, element_size);
+    q->element_count = element_count;
+    q->element_size = element_size;
+    q->wptr = 0;
+    q->rptr = 0;
+}
+
+void queue_free(queue_t *q) {
+    free(q->data);
+}
+
+static inline void *element_ptr(queue_t *q, uint index) {
+    assert(index <= q->element_count);
+    return q->data + index * q->element_size;
+}
+
+static inline uint16_t inc_index(queue_t *q, uint16_t index) {
+    if (++index > q->element_count) { // > because we have element_count + 1 elements
+        index = 0;
+    }
+    return index;
+}
+
+bool queue_try_add(queue_t *q, void *data) {
+    bool success = false;
+    uint32_t flags = spin_lock_blocking(q->lock);
+    if (queue_get_level_unsafe(q) != q->element_count) {
+        memcpy(element_ptr(q, q->wptr), data, q->element_size);
+        q->wptr = inc_index(q, q->wptr);
+        success = true;
+    }
+    spin_unlock(q->lock, flags);
+    if (success) __sev();
+    return success;
+}
+
+bool queue_try_remove(queue_t *q, void *data) {
+    bool success = false;
+    uint32_t flags = spin_lock_blocking(q->lock);
+    if (queue_get_level_unsafe(q) != 0) {
+        memcpy(data, element_ptr(q, q->rptr), q->element_size);
+        q->rptr = inc_index(q, q->rptr);
+        success = true;
+    }
+    spin_unlock(q->lock, flags);
+    if (success) __sev();
+    return success;
+}
+
+bool queue_try_peek(queue_t *q, void *data) {
+    bool success = false;
+    uint32_t flags = spin_lock_blocking(q->lock);
+    if (queue_get_level_unsafe(q) != 0) {
+        memcpy(data, element_ptr(q, q->rptr), q->element_size);
+        success = true;
+    }
+    spin_unlock(q->lock, flags);
+    return success;
+}
+
+void queue_add_blocking(queue_t *q, void *data) {
+    bool done;
+    do {
+        done = queue_try_add(q, data);
+        if (done) break;
+        __wfe();
+    } while (true);
+}
+
+void queue_remove_blocking(queue_t *q, void *data) {
+    bool done;
+    do {
+        done = queue_try_remove(q, data);
+        if (done) break;
+        __wfe();
+    } while (true);
+}
+
+void queue_peek_blocking(queue_t *q, void *data) {
+    bool done;
+    do {
+        done = queue_try_peek(q, data);
+        if (done) break;
+        __wfe();
+    } while (true);
+}