Adding RP2350 SDK and target framework (#13988)

* Adding RP2350 SDK and target framework

* Spacing

* Removing board definitions

lib/main/pico-sdk/common/pico_util/datetime.c

@@ -0,0 +1,95 @@
+#include "pico/util/datetime.h"
+
+#include <stdio.h>
+
+#if PICO_INCLUDE_RTC_DATETIME
+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);
+};
+
+
+bool time_to_datetime(time_t time, datetime_t *dt) {
+    struct tm local;
+    if (localtime_r(&time, &local)) {
+        dt->year = (int16_t) (local.tm_year + 1900); // 0..4095
+        dt->month = (int8_t) (local.tm_mon + 1);     // 1..12, 1 is January
+        dt->day = (int8_t) local.tm_mday;            // 1..28,29,30,31 depending on month
+        dt->dotw = (int8_t) local.tm_wday;           // 0..6, 0 is Sunday
+        dt->hour = (int8_t) local.tm_hour;           // 0..23
+        dt->min = (int8_t) local.tm_min;             // 0..59
+        dt->sec = (int8_t) local.tm_sec;             // 0..59
+        return true;
+    }
+    return false;
+}
+
+bool datetime_to_time(const datetime_t *dt, time_t *time) {
+    struct tm local;
+    local.tm_year = dt->year - 1900;
+    local.tm_mon = dt->month - 1;
+    local.tm_mday = dt->day;
+    local.tm_hour = dt->hour;
+    local.tm_min = dt->min;
+    local.tm_sec = dt->sec;
+    *time = mktime(&local);
+    return *time >= 0;
+}
+
+#endif
+
+uint64_t timespec_to_ms(const struct timespec *ts) {
+    int64_t rc = ts->tv_sec * 1000;
+    rc += ts->tv_nsec / 1000000;
+    return (uint64_t) rc;
+}
+
+void ms_to_timespec(uint64_t ms, struct timespec *ts) {
+    ts->tv_sec = (time_t)((int64_t)ms / 1000);
+    ts->tv_nsec = ((long)((int64_t)ms % 1000)) * 1000000;
+}
+
+uint64_t timespec_to_us(const struct timespec *ts) {
+    int64_t rc = ts->tv_sec * 1000000;
+    rc += ts->tv_nsec / 1000;
+    return (uint64_t) rc;
+}
+
+void us_to_timespec(uint64_t ms, struct timespec *ts) {
+    ts->tv_sec = (time_t)((int64_t)ms / 1000000);
+    ts->tv_nsec = ((long)((int64_t)ms % 1000000)) * 1000;
+}
+

lib/main/pico-sdk/common/pico_util/doc.h

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

lib/main/pico-sdk/common/pico_util/include/pico/util/datetime.h

@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_UTIL_DATETIME_H
+#define _PICO_UTIL_DATETIME_H
+
+#include "pico.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file datetime.h
+ * \defgroup util_datetime datetime
+ * \brief Date/Time formatting
+ * \ingroup pico_util
+ */
+
+#if PICO_INCLUDE_RTC_DATETIME
+#include <time.h>
+
+/*! \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);
+
+bool time_to_datetime(time_t time, datetime_t *dt);
+bool datetime_to_time(const datetime_t *dt, time_t *time);
+#endif
+
+#include <sys/time.h>
+uint64_t timespec_to_ms(const struct timespec *ts);
+uint64_t timespec_to_us(const struct timespec *ts);
+void ms_to_timespec(uint64_t ms, struct timespec *ts);
+void us_to_timespec(uint64_t ms, struct timespec *ts);
+
+#ifdef __cplusplus
+}
+#endif
+#endif

lib/main/pico-sdk/common/pico_util/include/pico/util/pheap.h

@@ -0,0 +1,309 @@
+/*
+ * 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
+ * \brief 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_and_free_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;
+
+/**
+ * \brief A user comparator function for nodes in a pairing heap.
+ * \ingroup util_pheap
+ *
+ * \return true if a < b in natural order. Note this relative ordering must be stable from call to call.
+ */
+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;
+
+/**
+ * \brief Create a pairing heap, which effectively maintains an efficient sorted ordering
+ * of nodes. The heap itself stores no user per-node state, it is expected
+ * that the user maintains a companion array. A comparator function must
+ * be provided so that the heap implementation can determine the relative ordering of nodes
+ * \ingroup util_pheap
+ *
+ * \param max_nodes the maximum number of nodes that may be in the heap (this is bounded by
+ *                  PICO_PHEAP_MAX_ENTRIES which defaults to 255 to be able to store indexes
+ *                  in a single byte).
+ * \param comparator the node comparison function
+ * \param user_data a user data pointer associated with the heap that is provided in callbacks
+ * \return a newly allocated and initialized heap
+ */
+pheap_t *ph_create(uint max_nodes, pheap_comparator comparator, void *user_data);
+
+/**
+ * \brief Removes all nodes from the pairing heap
+ * \ingroup util_pheap
+ * \param heap the heap
+ */
+void ph_clear(pheap_t *heap);
+
+/**
+ * \brief De-allocates a pairing heap
+ * \ingroup util_pheap
+ *
+ * Note this method must *ONLY* be called on heaps created by ph_create()
+ * \param heap the heap
+ */
+void ph_destroy(pheap_t *heap);
+
+// internal method
+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;
+}
+
+// internal method
+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;
+    }
+}
+
+// internal method
+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;
+    }
+}
+
+/**
+ * \brief Allocate a new node from the unused space in the heap
+ * \ingroup util_pheap
+ *
+ * \param heap the heap
+ * \return an identifier for the node, or 0 if the heap is full
+ */
+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;
+    pheap_node_t *hn = ph_get_node(heap, id);
+    heap->free_head_id = hn->sibling;
+    if (!heap->free_head_id) heap->free_tail_id = 0;
+    hn->child = hn->sibling = hn->parent = 0;
+    return id;
+}
+
+/**
+ * \brief Inserts a node into the heap.
+ * \ingroup util_pheap
+ *
+ * This method inserts a node (previously allocated by ph_new_node())
+ * into the heap, determining the correct order by calling
+ * the heap's comparator
+ *
+ * \param heap the heap
+ * \param id the id of the node to insert
+ * \return the id of the new head of the pairing heap (i.e. node that compares first)
+ */
+static inline pheap_node_id_t ph_insert_node(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;
+}
+
+/**
+ * \brief Returns the head node in the heap, i.e. the node
+ * which compares first, but without removing it from the heap.
+ * \ingroup util_pheap
+ *
+ * \param heap the heap
+ * \return the current head node id
+ */
+static inline pheap_node_id_t ph_peek_head(pheap_t *heap) {
+    return heap->root_id;
+}
+
+/**
+ * \brief Remove the head node from the pairing heap. This head node is
+ * the node which compares first in the logical ordering provided
+ * by the comparator.
+ * \ingroup util_pheap
+ *
+ * Note that in the case of free == true, the returned id is no longer
+ * allocated and may be re-used by future node allocations, so the caller
+ * should retrieve any per node state from the companion array before modifying
+ * the heap further.
+ *
+ * @param heap the heap
+ * @param free true if the id is also to be freed; false if not - useful if the caller
+ *        may wish to re-insert an item with the same id)
+ * @return the old head node id.
+ */
+pheap_node_id_t ph_remove_head(pheap_t *heap, bool free);
+
+/**
+ * \brief Remove the head node from the pairing heap. This head node is
+ * the node which compares first in the logical ordering provided
+ * by the comparator.
+ * \ingroup util_pheap
+ *
+ * Note that the returned id will be freed, and thus may be re-used by future node allocations,
+ * so the caller should retrieve any per node state from the companion array before modifying
+ * the heap further.
+ *
+ * @param heap the heap
+ * @return the old head node id.
+ */
+static inline pheap_node_id_t ph_remove_and_free_head(pheap_t *heap) {
+    return ph_remove_head(heap, true);
+}
+
+/**
+ * \brief Remove and free an arbitrary node from the pairing heap. This is a more
+ * costly operation than removing the head via ph_remove_and_free_head()
+ * \ingroup util_pheap
+ *
+ * @param heap the heap
+ * @param id the id of the node to free
+ * @return true if the the node was in the heap, false otherwise
+ */
+bool ph_remove_and_free_node(pheap_t *heap, pheap_node_id_t id);
+
+/**
+ * \brief Determine if the heap contains a given node. Note containment refers
+ * to whether the node is inserted (ph_insert_node()) vs allocated (ph_new_node())
+ * \ingroup util_pheap
+ *
+ * @param heap the heap
+ * @param id the id of the node
+ * @return true if the heap contains a node with the given id, false otherwise.
+ */
+static inline bool ph_contains_node(pheap_t *heap, pheap_node_id_t id) {
+    return id == heap->root_id || ph_get_node(heap, id)->parent;
+}
+
+
+/**
+ * \brief Free a node that is not currently in the heap, but has been allocated
+ * \ingroup util_pheap
+ *
+ * @param heap the heap
+ * @param id the id of the node
+ */
+static inline void ph_free_node(pheap_t *heap, pheap_node_id_t id) {
+    assert(id && !ph_contains_node(heap, id));
+    if (heap->free_tail_id) {
+        ph_get_node(heap, heap->free_tail_id)->sibling = id;
+    }
+    if (!heap->free_head_id) {
+        assert(!heap->free_tail_id);
+        heap->free_head_id = id;
+    }
+    heap->free_tail_id = id;
+}
+
+/**
+ * \brief Print a representation of the heap for debugging
+ * \ingroup util_pheap
+ *
+ * @param heap the heap
+ * @param dump_key a method to print a node value
+ * @param user_data the user data to pass to the dump_key method
+ */
+void ph_dump(pheap_t *heap, void (*dump_key)(pheap_node_id_t id, void *user_data), void *user_data);
+
+/**
+ * \brief Initialize a statically allocated heap (ph_create() using the C heap).
+ * The heap member `nodes` must be allocated of size max_nodes.
+ * \ingroup util_pheap
+ *
+ * @param heap the heap
+ * @param max_nodes the max number of nodes in the heap (matching the size of the heap's nodes array)
+ * @param comparator the comparator for the heap
+ * @param user_data the user data for the heap.
+ */
+void ph_post_alloc_init(pheap_t *heap, uint max_nodes, pheap_comparator comparator, void *user_data);
+
+/**
+ * \brief Define a statically allocated pairing heap. This must be initialized
+ * by ph_post_alloc_init
+ * \ingroup util_pheap
+ */
+#define PHEAP_DEFINE_STATIC(name, _max_nodes) \
+    static_assert(_max_nodes && _max_nodes < (1u << (8 * sizeof(pheap_node_id_t))), ""); \
+    static pheap_node_t name ## _nodes[_max_nodes]; \
+    static pheap_t name = { \
+            .nodes = name ## _nodes, \
+            .max_nodes = _max_nodes \
+    };
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

lib/main/pico-sdk/common/pico_util/include/pico/util/queue.h

@@ -0,0 +1,227 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_UTIL_QUEUE_H
+#define _PICO_UTIL_QUEUE_H
+
+#include "pico.h"
+#include "hardware/sync.h"
+
+// PICO_CONFIG: PICO_QUEUE_MAX_LEVEL, Maintain a field for the highest level that has been reached by a queue, type=bool, default=0, advanced=true, group=queue
+#ifndef PICO_QUEUE_MAX_LEVEL
+#define PICO_QUEUE_MAX_LEVEL 0
+#endif
+
+/** \file queue.h
+ * \defgroup queue queue
+ * \brief 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
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "pico/lock_core.h"
+
+typedef struct {
+    lock_core_t core;
+    uint8_t *data;
+    uint16_t wptr;
+    uint16_t rptr;
+    uint16_t element_size;
+    uint16_t element_count;
+#if PICO_QUEUE_MAX_LEVEL
+    uint16_t max_level;
+#endif
+} 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) {
+    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 (uint)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->core.spin_lock);
+    uint level = queue_get_level_unsafe(q);
+    spin_unlock(q->core.spin_lock, save);
+    return level;
+}
+
+#if PICO_QUEUE_MAX_LEVEL
+/*! \brief Returns the highest level reached by the specified queue since it was created
+ *         or since the max level was reset
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ * \return Maximum level of the queue
+ */
+static inline uint queue_get_max_level(queue_t *q) {
+    return q->max_level;
+}
+#endif
+
+#if PICO_QUEUE_MAX_LEVEL
+/*! \brief Reset the highest level reached of the specified queue.
+ *  \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ */
+static inline void queue_reset_max_level(queue_t *q) {
+    uint32_t save = spin_lock_blocking(q->core.spin_lock);
+    q->max_level = queue_get_level_unsafe(q);
+    spin_unlock(q->core.spin_lock, save);
+}
+#endif
+
+/*! \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, const 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, or NULL if the data isn't required
+ * \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, or NULL if the data isn't required
+ * \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, const 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, or NULL if the data isn't required
+ *
+ * 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, or NULL if the data isn't required
+ *
+ * If the queue is empty function will block until a value is added
+ */
+void queue_peek_blocking(queue_t *q, void *data);
+
+#ifdef __cplusplus
+}
+#endif
+#endif

lib/main/pico-sdk/common/pico_util/pheap.c

@@ -0,0 +1,138 @@
+/*
+ * 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 << (8 * sizeof(pheap_node_id_t))));
+    pheap_t *heap = calloc(1, sizeof(pheap_t));
+    heap->nodes = calloc(max_nodes, sizeof(pheap_node_t));
+    ph_post_alloc_init(heap, max_nodes, comparator, user_data);
+    return heap;
+}
+
+void ph_post_alloc_init(pheap_t *heap, uint max_nodes, pheap_comparator comparator, void *user_data) {
+    invalid_params_if(PHEAP, !max_nodes || max_nodes >= (1u << (8 * sizeof(pheap_node_id_t))));
+    heap->max_nodes = (pheap_node_id_t) max_nodes;
+    heap->comparator = comparator;
+    heap->user_data = user_data;
+    ph_clear(heap);
+}
+
+void ph_clear(pheap_t *heap) {
+    heap->root_id = 0;
+    heap->free_head_id = 1;
+    heap->free_tail_id = heap->max_nodes;
+    for(pheap_node_id_t i = 1; i < heap->max_nodes; i++) {
+        ph_get_node(heap, i)->sibling = (pheap_node_id_t)(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 free) {
+    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 (free) {
+        if (heap->free_tail_id) {
+            ph_get_node(heap, heap->free_tail_id)->sibling = root_id;
+        }
+        if (!heap->free_head_id) {
+            assert(!heap->free_tail_id);
+            heap->free_head_id = 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(pheap_t *heap, bool free) {
+    pheap_node_id_t old_root_id = ph_peek_head(heap);
+    heap->root_id = ph_remove_any_head(heap, old_root_id, free);
+    return old_root_id;
+}
+
+bool ph_remove_and_free_node(pheap_t *heap, pheap_node_id_t id) {
+    // 1) trivial cases
+    if (!id) return false;
+    if (id == heap->root_id) {
+        ph_remove_and_free_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, true);
+    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);
+}

lib/main/pico-sdk/common/pico_util/queue.c

@@ -0,0 +1,123 @@
+/*
+ * 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) {
+    lock_init(&q->core, spinlock_num);
+    q->data = (uint8_t *)calloc(element_count + 1, element_size);
+    q->element_count = (uint16_t)element_count;
+    q->element_size = (uint16_t)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;
+    }
+
+#if PICO_QUEUE_MAX_LEVEL
+    uint16_t level = queue_get_level_unsafe(q);
+    if (level > q->max_level) {
+        q->max_level = level;
+    }
+#endif
+
+    return index;
+}
+
+static bool queue_add_internal(queue_t *q, const void *data, bool block) {
+    do {
+        uint32_t save = spin_lock_blocking(q->core.spin_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);
+            lock_internal_spin_unlock_with_notify(&q->core, save);
+            return true;
+        }
+        if (block) {
+            lock_internal_spin_unlock_with_wait(&q->core, save);
+        } else {
+            spin_unlock(q->core.spin_lock, save);
+            return false;
+        }
+    } while (true);
+}
+
+static bool queue_remove_internal(queue_t *q, void *data, bool block) {
+    do {
+        uint32_t save = spin_lock_blocking(q->core.spin_lock);
+        if (queue_get_level_unsafe(q) != 0) {
+            if (data) {
+                memcpy(data, element_ptr(q, q->rptr), q->element_size);
+            }
+            q->rptr = inc_index(q, q->rptr);
+            lock_internal_spin_unlock_with_notify(&q->core, save);
+            return true;
+        }
+        if (block) {
+            lock_internal_spin_unlock_with_wait(&q->core, save);
+        } else {
+            spin_unlock(q->core.spin_lock, save);
+            return false;
+        }
+    } while (true);
+}
+
+static bool queue_peek_internal(queue_t *q, void *data, bool block) {
+    do {
+        uint32_t save = spin_lock_blocking(q->core.spin_lock);
+        if (queue_get_level_unsafe(q) != 0) {
+            if (data) {
+                memcpy(data, element_ptr(q, q->rptr), q->element_size);
+            }
+            lock_internal_spin_unlock_with_notify(&q->core, save);
+            return true;
+        }
+        if (block) {
+            lock_internal_spin_unlock_with_wait(&q->core, save);
+        } else {
+            spin_unlock(q->core.spin_lock, save);
+            return false;
+        }
+    } while (true);
+}
+
+bool queue_try_add(queue_t *q, const void *data) {
+    return queue_add_internal(q, data, false);
+}
+
+bool queue_try_remove(queue_t *q, void *data) {
+    return queue_remove_internal(q, data, false);
+}
+
+bool queue_try_peek(queue_t *q, void *data) {
+    return queue_peek_internal(q, data, false);
+}
+
+void queue_add_blocking(queue_t *q, const void *data) {
+    queue_add_internal(q, data, true);
+}
+
+void queue_remove_blocking(queue_t *q, void *data) {
+    queue_remove_internal(q, data, true);
+}
+
+void queue_peek_blocking(queue_t *q, void *data) {
+    queue_peek_internal(q, data, true);
+}