Squashed 'third_party/pico-sdk/' content from commit 2062372d2
Change-Id: Ic20f199d3ed0ea8d3a6a1bbf513f875ec7500cc6
git-subtree-dir: third_party/pico-sdk
git-subtree-split: 2062372d203b372849d573f252cf7c6dc2800c0a
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/src/common/pico_util/CMakeLists.txt b/src/common/pico_util/CMakeLists.txt
new file mode 100644
index 0000000..3eb6998
--- /dev/null
+++ b/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)
+ pico_add_impl_library(pico_util)
+ 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()
diff --git a/src/common/pico_util/datetime.c b/src/common/pico_util/datetime.c
new file mode 100644
index 0000000..e035515
--- /dev/null
+++ b/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);
+};
\ No newline at end of file
diff --git a/src/common/pico_util/doc.h b/src/common/pico_util/doc.h
new file mode 100644
index 0000000..4485b5d
--- /dev/null
+++ b/src/common/pico_util/doc.h
@@ -0,0 +1,4 @@
+/**
+ * \defgroup pico_util pico_util
+ * \brief Useful data structures and utility functions
+ */
diff --git a/src/common/pico_util/include/pico/util/datetime.h b/src/common/pico_util/include/pico/util/datetime.h
new file mode 100644
index 0000000..bb32835
--- /dev/null
+++ b/src/common/pico_util/include/pico/util/datetime.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef _PICO_DATETIME_H
+#define _PICO_DATETIME_H
+
+#include "pico.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \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);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/src/common/pico_util/include/pico/util/pheap.h b/src/common/pico_util/include/pico/util/pheap.h
new file mode 100644
index 0000000..25351b4
--- /dev/null
+++ b/src/common/pico_util/include/pico/util/pheap.h
@@ -0,0 +1,291 @@
+/*
+ * 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_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;
+
+/**
+ * A user comparator function for nodes in a pairing heap.
+ *
+ * \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;
+
+/**
+ * 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
+ *
+ * \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);
+
+/**
+ * Removes all nodes from the pairing heap
+ * \param heap the heap
+ */
+void ph_clear(pheap_t *heap);
+
+/**
+ * De-allocates a pairing heap
+ *
+ * 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;
+ }
+}
+
+/**
+ * Allocate a new node from the unused space in the heap
+ *
+ * \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;
+}
+
+/**
+ * Inserts a node into the heap.
+ *
+ * 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;
+}
+
+/**
+ * Returns the head node in the heap, i.e. the node
+ * which compares first, but without removing it from the heap.
+ *
+ * \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;
+}
+
+/**
+ * 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.
+ *
+ * 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);
+
+/**
+ * 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.
+ *
+ * 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);
+}
+
+/**
+ * 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()
+ *
+ * @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);
+
+/**
+ * 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())
+ *
+ * @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;
+}
+
+
+/**
+ * Free a node that is not currently in the heap, but has been allocated
+ *
+ * @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;
+ }
+ heap->free_tail_id = id;
+}
+
+/**
+ * Print a representation of the heap for debugging
+ *
+ * @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);
+
+/**
+ * Initialize a statically allocated heap (ph_create() using the C heap).
+ * The heap member `nodes` must be allocated of size max_nodes.
+ *
+ * @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);
+
+/**
+ * Define a statically allocated pairing heap. This must be initialized
+ * by ph_post_alloc_init
+ */
+#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
diff --git a/src/common/pico_util/include/pico/util/queue.h b/src/common/pico_util/include/pico/util/queue.h
new file mode 100644
index 0000000..097578a
--- /dev/null
+++ b/src/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 _UTIL_QUEUE_H
+#define _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
+ * 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) {
+ 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 (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;
+}
+
+/*! \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
+ */
+#if PICO_QUEUE_MAX_LEVEL
+static inline uint queue_get_max_level(queue_t *q) {
+ return q->max_level;
+}
+#endif
+
+/*! \brief Reset the highest level reached of the specified queue.
+ * \ingroup queue
+ *
+ * \param q Pointer to a queue_t structure, used as a handle
+ */
+#if PICO_QUEUE_MAX_LEVEL
+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
+ * \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, 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
+ *
+ * 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);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/src/common/pico_util/pheap.c b/src/common/pico_util/pheap.c
new file mode 100644
index 0000000..fc80435
--- /dev/null
+++ b/src/common/pico_util/pheap.c
@@ -0,0 +1,134 @@
+/*
+ * 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;
+ }
+ 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);
+}
\ No newline at end of file
diff --git a/src/common/pico_util/queue.c b/src/common/pico_util/queue.c
new file mode 100644
index 0000000..a5c8e18
--- /dev/null
+++ b/src/common/pico_util/queue.c
@@ -0,0 +1,119 @@
+/*
+ * 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) {
+ 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) {
+ 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);
+}