From d3aa6f7f9830440d0948d7f3af32e6bca4900132 Mon Sep 17 00:00:00 2001 From: Graham Sanderson Date: Thu, 25 Feb 2021 08:40:03 -0600 Subject: [PATCH] No malloc for default alarm pool an pheap docs/cleanup (#143) * Statically allocate the default timer pool (to avoid pulling in malloc); doxygen for pheap (and some function name changes) * fix comments --- src/common/pico_time/time.c | 92 ++++++---- .../pico_util/include/pico/util/pheap.h | 164 ++++++++++++++++-- src/common/pico_util/pheap.c | 26 +-- 3 files changed, 225 insertions(+), 57 deletions(-) diff --git a/src/common/pico_time/time.c b/src/common/pico_time/time.c index 0c94e12..32f11e6 100644 --- a/src/common/pico_time/time.c +++ b/src/common/pico_time/time.c @@ -33,26 +33,25 @@ typedef struct alarm_pool { } alarm_pool_t; #if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED -static alarm_pool_t *default_alarm_pool; +// To avoid bringing in calloc, we statically allocate the arrays and the heap +PHEAP_DEFINE_STATIC(default_alarm_pool_heap, PICO_TIME_DEFAULT_ALARM_POOL_MAX_TIMERS); +static alarm_pool_entry_t default_alarm_pool_entries[PICO_TIME_DEFAULT_ALARM_POOL_MAX_TIMERS]; +static uint8_t default_alarm_pool_entry_ids_high[PICO_TIME_DEFAULT_ALARM_POOL_MAX_TIMERS]; + +static alarm_pool_t default_alarm_pool = { + .heap = &default_alarm_pool_heap, + .entries = default_alarm_pool_entries, + .entry_ids_high = default_alarm_pool_entry_ids_high, +}; + +static inline bool default_alarm_pool_initialized(void) { + return default_alarm_pool.lock != NULL; +} #endif + static alarm_pool_t *pools[NUM_TIMERS]; +static void alarm_pool_post_alloc_init(alarm_pool_t *pool, uint hardware_alarm_num); -void alarm_pool_init_default() { -#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED - // allow multiple calls for ease of use from host tests - if (!default_alarm_pool) { - default_alarm_pool = alarm_pool_create(PICO_TIME_DEFAULT_ALARM_POOL_HARDWARE_ALARM_NUM, - PICO_TIME_DEFAULT_ALARM_POOL_MAX_TIMERS); - } -#endif -} - -#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED -alarm_pool_t *alarm_pool_get_default() { - assert(default_alarm_pool); - return default_alarm_pool; -} -#endif static inline alarm_pool_entry_t *get_entry(alarm_pool_t *pool, pheap_node_id_t id) { assert(id && id <= pool->heap->max_nodes); @@ -73,11 +72,30 @@ static inline alarm_id_t make_public_id(uint8_t id_high, pheap_node_id_t id) { return (alarm_id_t)(((uint)id_high << 8u * sizeof(id)) | id); } +void alarm_pool_init_default() { +#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED + // allow multiple calls for ease of use from host tests + if (!default_alarm_pool_initialized()) { + ph_post_alloc_init(default_alarm_pool.heap, PICO_TIME_DEFAULT_ALARM_POOL_MAX_TIMERS, + timer_pool_entry_comparator, &default_alarm_pool); + alarm_pool_post_alloc_init(&default_alarm_pool, + PICO_TIME_DEFAULT_ALARM_POOL_HARDWARE_ALARM_NUM); + } +#endif +} + +#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED +alarm_pool_t *alarm_pool_get_default() { + assert(default_alarm_pool_initialized()); + return &default_alarm_pool; +} +#endif + static pheap_node_id_t add_alarm_under_lock(alarm_pool_t *pool, absolute_time_t time, alarm_callback_t callback, void *user_data, pheap_node_id_t reuse_id, bool create_if_past, bool *missed) { pheap_node_id_t id; if (reuse_id) { - assert(!ph_contains(pool->heap, reuse_id)); + assert(!ph_contains_node(pool->heap, reuse_id)); id = reuse_id; } else { id = ph_new_node(pool->heap); @@ -87,10 +105,10 @@ static pheap_node_id_t add_alarm_under_lock(alarm_pool_t *pool, absolute_time_t entry->target = time; entry->callback = callback; entry->user_data = user_data; - if (id == ph_insert(pool->heap, id)) { + if (id == ph_insert_node(pool->heap, id)) { bool is_missed = hardware_alarm_set_target(pool->hardware_alarm_num, time); if (is_missed && !create_if_past) { - ph_delete(pool->heap, id); + ph_remove_and_free_node(pool->heap, id); } if (missed) *missed = is_missed; } @@ -114,8 +132,8 @@ static void alarm_pool_alarm_callback(uint alarm_num) { if (next_id) { alarm_pool_entry_t *entry = get_entry(pool, next_id); if (absolute_time_diff_us(now, entry->target) <= 0) { - // we reserve the id in case we need to re-add the timer - pheap_node_id_t __unused removed_id = ph_remove_head_reserve(pool->heap, true); + // we don't free the id in case we need to re-add the timer + pheap_node_id_t __unused removed_id = ph_remove_head(pool->heap, false); assert(removed_id == next_id); // will be true under lock target = entry->target; callback = entry->callback; @@ -143,7 +161,7 @@ static void alarm_pool_alarm_callback(uint alarm_num) { true, NULL); } else { // need to return the id to the heap - ph_add_to_free_list(pool->heap, next_id); + ph_free_node(pool->heap, next_id); (*get_entry_id_high(pool, next_id))++; // we bump it for next use of id } pool->alarm_in_progress = 0; @@ -155,20 +173,30 @@ static void alarm_pool_alarm_callback(uint alarm_num) { // note the timer is create with IRQs on this core alarm_pool_t *alarm_pool_create(uint hardware_alarm_num, uint max_timers) { - hardware_alarm_claim(hardware_alarm_num); - hardware_alarm_cancel(hardware_alarm_num); - hardware_alarm_set_callback(hardware_alarm_num, alarm_pool_alarm_callback); - alarm_pool_t *pool = (alarm_pool_t *)malloc(sizeof(alarm_pool_t)); - pool->lock = spin_lock_instance(next_striped_spin_lock_num()); + alarm_pool_t *pool = (alarm_pool_t *) malloc(sizeof(alarm_pool_t)); pool->heap = ph_create(max_timers, timer_pool_entry_comparator, pool); pool->entries = (alarm_pool_entry_t *)calloc(max_timers, sizeof(alarm_pool_entry_t)); pool->entry_ids_high = (uint8_t *)calloc(max_timers, sizeof(uint8_t)); - pool->hardware_alarm_num = (uint8_t)hardware_alarm_num; - pools[hardware_alarm_num] = pool; + alarm_pool_post_alloc_init(pool, hardware_alarm_num); return pool; } +void alarm_pool_post_alloc_init(alarm_pool_t *pool, uint hardware_alarm_num) { + hardware_alarm_claim(hardware_alarm_num); + hardware_alarm_cancel(hardware_alarm_num); + hardware_alarm_set_callback(hardware_alarm_num, alarm_pool_alarm_callback); + pool->lock = spin_lock_instance(next_striped_spin_lock_num()); + pool->hardware_alarm_num = (uint8_t) hardware_alarm_num; + pools[hardware_alarm_num] = pool; +} + void alarm_pool_destroy(alarm_pool_t *pool) { +#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED + if (pool == &default_alarm_pool) { + assert(false); // attempt to delete default alarm pool + return; + } +#endif assert(pools[pool->hardware_alarm_num] == pool); pools[pool->hardware_alarm_num] = NULL; // todo clear out timers @@ -219,12 +247,12 @@ bool alarm_pool_cancel_alarm(alarm_pool_t *pool, alarm_id_t alarm_id) { bool rc = false; uint32_t save = spin_lock_blocking(pool->lock); pheap_node_id_t id = (pheap_node_id_t) alarm_id; - if (ph_contains(pool->heap, id)) { + if (ph_contains_node(pool->heap, id)) { assert(alarm_id != pool->alarm_in_progress); // it shouldn't be in the heap if it is in progress // check we have the right high value uint8_t id_high = (uint8_t)((uint)alarm_id >> 8u * sizeof(pheap_node_id_t)); if (id_high == *get_entry_id_high(pool, id)) { - rc = ph_delete(pool->heap, id); + rc = ph_remove_and_free_node(pool->heap, id); // note we don't bother to remove the actual hardware alarm timeout... // it will either do callbacks or not depending on other alarms, and reset the next timeout itself assert(rc); diff --git a/src/common/pico_util/include/pico/util/pheap.h b/src/common/pico_util/include/pico/util/pheap.h index 59617e7..25351b4 100644 --- a/src/common/pico_util/include/pico/util/pheap.h +++ b/src/common/pico_util/include/pico/util/pheap.h @@ -29,7 +29,7 @@ extern "C" { * * 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. + * 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 @@ -53,7 +53,11 @@ typedef struct pheap_node { pheap_node_id_t child, sibling, parent; } pheap_node_t; -// return true if a < b in natural order +/** + * 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 { @@ -67,17 +71,42 @@ typedef struct pheap { 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); @@ -93,6 +122,7 @@ static void ph_add_child_node(pheap_t *heap, pheap_node_id_t parent_id, pheap_no } } +// 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; @@ -105,17 +135,34 @@ static pheap_node_id_t ph_merge_nodes(pheap_t *heap, pheap_node_id_t a, pheap_no } } +/** + * 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; - heap->free_head_id = ph_get_node(heap, id)->sibling; + 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; } -// 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) { +/** + * 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; @@ -123,31 +170,120 @@ static inline pheap_node_id_t ph_insert(pheap_t *heap, pheap_node_id_t 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; } -pheap_node_id_t ph_remove_head_reserve(pheap_t *heap, bool reserve); +/** + * 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); -static inline pheap_node_id_t ph_remove_head(pheap_t *heap) { - return ph_remove_head_reserve(heap, false); +/** + * 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); } -static inline bool ph_contains(pheap_t *heap, pheap_node_id_t id) { +/** + * 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; } -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)); +/** + * 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; } -void ph_dump(pheap_t *heap, void (*dump_key)(pheap_node_id_t, void *), void *user_data); +/** + * 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 diff --git a/src/common/pico_util/pheap.c b/src/common/pico_util/pheap.c index 6ddc0cb..fc80435 100644 --- a/src/common/pico_util/pheap.c +++ b/src/common/pico_util/pheap.c @@ -9,14 +9,19 @@ #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))); + 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->nodes = calloc(max_nodes, sizeof(pheap_node_t)); heap->user_data = user_data; ph_clear(heap); - return heap; } void ph_clear(pheap_t *heap) { @@ -47,13 +52,13 @@ pheap_node_id_t ph_merge_two_pass(pheap_t *heap, pheap_node_id_t id) { } } -static pheap_node_id_t ph_remove_any_head(pheap_t *heap, pheap_node_id_t root_id, bool reserve) { +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 (!reserve) { + if (free) { if (heap->free_tail_id) { ph_get_node(heap, heap->free_tail_id)->sibling = root_id; } @@ -64,18 +69,17 @@ static pheap_node_id_t ph_remove_any_head(pheap_t *heap, pheap_node_id_t root_id return new_root_id; } -pheap_node_id_t ph_remove_head_reserve(pheap_t *heap, bool reserve) { +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, reserve); + heap->root_id = ph_remove_any_head(heap, old_root_id, free); return old_root_id; } -#include -bool ph_delete(pheap_t *heap, pheap_node_id_t 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_head(heap); + ph_remove_and_free_head(heap); return true; } // 2) unlink the node from the tree @@ -101,7 +105,7 @@ bool ph_delete(pheap_t *heap, pheap_node_id_t id) { 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); + 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;