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_time/time.c b/src/common/pico_time/time.c
new file mode 100644
index 0000000..3a585f6
--- /dev/null
+++ b/src/common/pico_time/time.c
@@ -0,0 +1,413 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <limits.h>
+#include <inttypes.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include "pico.h"
+#include "pico/time.h"
+#include "pico/util/pheap.h"
+#include "pico/sync.h"
+
+const absolute_time_t ABSOLUTE_TIME_INITIALIZED_VAR(nil_time, 0);
+const absolute_time_t ABSOLUTE_TIME_INITIALIZED_VAR(at_the_end_of_time, INT64_MAX);
+
+typedef struct alarm_pool_entry {
+ absolute_time_t target;
+ alarm_callback_t callback;
+ void *user_data;
+} alarm_pool_entry_t;
+
+typedef struct alarm_pool {
+ pheap_t *heap;
+ spin_lock_t *lock;
+ alarm_pool_entry_t *entries;
+ // one byte per entry, used to provide more longevity to public IDs than heap node ids do
+ // (this is increment every time the heap node id is re-used)
+ uint8_t *entry_ids_high;
+ alarm_id_t alarm_in_progress; // this is set during a callback from the IRQ handler... it can be cleared by alarm_cancel to prevent repeats
+ uint8_t hardware_alarm_num;
+} alarm_pool_t;
+
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+// 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 lock_core_t sleep_notifier;
+
+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);
+
+
+static inline alarm_pool_entry_t *get_entry(alarm_pool_t *pool, pheap_node_id_t id) {
+ assert(id && id <= pool->heap->max_nodes);
+ return pool->entries + id - 1;
+}
+
+static inline uint8_t *get_entry_id_high(alarm_pool_t *pool, pheap_node_id_t id) {
+ assert(id && id <= pool->heap->max_nodes);
+ return pool->entry_ids_high + id - 1;
+}
+
+bool timer_pool_entry_comparator(void *user_data, pheap_node_id_t a, pheap_node_id_t b) {
+ alarm_pool_t *pool = (alarm_pool_t *)user_data;
+ return to_us_since_boot(get_entry(pool, a)->target) < to_us_since_boot(get_entry(pool, b)->target);
+}
+
+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);
+ }
+ lock_init(&sleep_notifier, PICO_SPINLOCK_ID_TIMER);
+#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_node(pool->heap, reuse_id));
+ id = reuse_id;
+ } else {
+ id = ph_new_node(pool->heap);
+ }
+ if (id) {
+ alarm_pool_entry_t *entry = get_entry(pool, id);
+ entry->target = time;
+ entry->callback = callback;
+ entry->user_data = user_data;
+ 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_remove_and_free_node(pool->heap, id);
+ }
+ if (missed) *missed = is_missed;
+ }
+ }
+ return id;
+}
+
+static void alarm_pool_alarm_callback(uint alarm_num) {
+ // note this is called from timer IRQ handler
+ alarm_pool_t *pool = pools[alarm_num];
+ bool again;
+ do {
+ absolute_time_t now = get_absolute_time();
+ alarm_callback_t callback = NULL;
+ absolute_time_t target = nil_time;
+ void *user_data = NULL;
+ uint8_t id_high;
+ again = false;
+ uint32_t save = spin_lock_blocking(pool->lock);
+ pheap_node_id_t next_id = ph_peek_head(pool->heap);
+ if (next_id) {
+ alarm_pool_entry_t *entry = get_entry(pool, next_id);
+ if (absolute_time_diff_us(now, entry->target) <= 0) {
+ // 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;
+ user_data = entry->user_data;
+ assert(callback);
+ id_high = *get_entry_id_high(pool, next_id);
+ pool->alarm_in_progress = make_public_id(id_high, removed_id);
+ } else {
+ if (hardware_alarm_set_target(alarm_num, entry->target)) {
+ again = true;
+ }
+ }
+ }
+ spin_unlock(pool->lock, save);
+ if (callback) {
+ int64_t repeat = callback(make_public_id(id_high, next_id), user_data);
+ save = spin_lock_blocking(pool->lock);
+ // todo think more about whether we want to keep calling
+ if (repeat < 0 && pool->alarm_in_progress) {
+ assert(pool->alarm_in_progress == make_public_id(id_high, next_id));
+ add_alarm_under_lock(pool, delayed_by_us(target, (uint64_t)-repeat), callback, user_data, next_id, true, NULL);
+ } else if (repeat > 0 && pool->alarm_in_progress) {
+ assert(pool->alarm_in_progress == make_public_id(id_high, next_id));
+ add_alarm_under_lock(pool, delayed_by_us(get_absolute_time(), (uint64_t)repeat), callback, user_data, next_id,
+ true, NULL);
+ } else {
+ // need to return the id to the heap
+ 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;
+ spin_unlock(pool->lock, save);
+ again = true;
+ }
+ } while (again);
+}
+
+// note the timer is create with IRQs on this core
+alarm_pool_t *alarm_pool_create(uint hardware_alarm_num, uint max_timers) {
+ 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));
+ 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
+ ph_destroy(pool->heap);
+ hardware_alarm_set_callback(pool->hardware_alarm_num, NULL);
+ hardware_alarm_unclaim(pool->hardware_alarm_num);
+ free(pool->entry_ids_high);
+ free(pool->entries);
+ free(pool);
+}
+
+alarm_id_t alarm_pool_add_alarm_at(alarm_pool_t *pool, absolute_time_t time, alarm_callback_t callback,
+ void *user_data, bool fire_if_past) {
+ bool missed = false;
+
+ alarm_id_t public_id;
+ do {
+ uint8_t id_high = 0;
+ uint32_t save = spin_lock_blocking(pool->lock);
+
+ pheap_node_id_t id = add_alarm_under_lock(pool, time, callback, user_data, 0, false, &missed);
+ if (id) id_high = *get_entry_id_high(pool, id);
+
+ spin_unlock(pool->lock, save);
+
+ if (!id) {
+ // no space in pheap to allocate an alarm
+ return -1;
+ }
+
+ // note that if missed was true, then the id was never added to the pheap (because we
+ // passed false for create_if_past arg above)
+ public_id = missed ? 0 : make_public_id(id_high, id);
+ if (missed && fire_if_past) {
+ // ... so if fire_if_past == true we call the callback
+ int64_t repeat = callback(public_id, user_data);
+ // if not repeated we have no id to return so set public_id to 0,
+ // otherwise we need to repeat, but will assign a new id next time
+ // todo arguably this does mean that the id passed to the first callback may differ from subsequent calls
+ if (!repeat) {
+ public_id = 0;
+ break;
+ } else if (repeat < 0) {
+ time = delayed_by_us(time, (uint64_t)-repeat);
+ } else {
+ time = delayed_by_us(get_absolute_time(), (uint64_t)repeat);
+ }
+ } else {
+ // either:
+ // a) missed == false && public_id is > 0
+ // b) missed == true && fire_if_past == false && public_id = 0
+ // but we are done in either case
+ break;
+ }
+ } while (true);
+ return public_id;
+}
+
+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_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_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);
+ }
+ } else {
+ if (alarm_id == pool->alarm_in_progress) {
+ // make sure the alarm doesn't repeat
+ pool->alarm_in_progress = 0;
+ }
+ }
+ spin_unlock(pool->lock, save);
+ return rc;
+}
+
+uint alarm_pool_hardware_alarm_num(alarm_pool_t *pool) {
+ return pool->hardware_alarm_num;
+}
+
+static void alarm_pool_dump_key(pheap_node_id_t id, void *user_data) {
+ alarm_pool_t *pool = (alarm_pool_t *)user_data;
+#if PICO_ON_DEVICE
+ printf("%lld (hi %02x)", to_us_since_boot(get_entry(pool, id)->target), *get_entry_id_high(pool, id));
+#else
+ printf("%"PRIu64, to_us_since_boot(get_entry(pool, id)->target));
+#endif
+}
+
+static int64_t repeating_timer_callback(__unused alarm_id_t id, void *user_data) {
+ repeating_timer_t *rt = (repeating_timer_t *)user_data;
+ assert(rt->alarm_id == id);
+ if (rt->callback(rt)) {
+ return rt->delay_us;
+ } else {
+ rt->alarm_id = 0;
+ return 0;
+ }
+}
+
+bool alarm_pool_add_repeating_timer_us(alarm_pool_t *pool, int64_t delay_us, repeating_timer_callback_t callback, void *user_data, repeating_timer_t *out) {
+ if (!delay_us) delay_us = 1;
+ out->pool = pool;
+ out->callback = callback;
+ out->delay_us = delay_us;
+ out->user_data = user_data;
+ out->alarm_id = alarm_pool_add_alarm_at(pool, make_timeout_time_us((uint64_t)(delay_us >= 0 ? delay_us : -delay_us)),
+ repeating_timer_callback, out, true);
+ // note that if out->alarm_id is 0, then the callback was called during the above call (fire_if_past == true)
+ // and then the callback removed itself.
+ return out->alarm_id >= 0;
+}
+
+bool cancel_repeating_timer(repeating_timer_t *timer) {
+ bool rc = false;
+ if (timer->alarm_id) {
+ rc = alarm_pool_cancel_alarm(timer->pool, timer->alarm_id);
+ timer->alarm_id = 0;
+ }
+ return rc;
+}
+
+void alarm_pool_dump(alarm_pool_t *pool) {
+ uint32_t save = spin_lock_blocking(pool->lock);
+ ph_dump(pool->heap, alarm_pool_dump_key, pool);
+ spin_unlock(pool->lock, save);
+}
+
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+static int64_t sleep_until_callback(__unused alarm_id_t id, __unused void *user_data) {
+ uint32_t save = spin_lock_blocking(sleep_notifier.spin_lock);
+ lock_internal_spin_unlock_with_notify(&sleep_notifier, save);
+ return 0;
+}
+#endif
+
+void sleep_until(absolute_time_t t) {
+#if PICO_ON_DEVICE && !defined(NDEBUG)
+ if (__get_current_exception()) {
+ panic("Attempted to sleep inside of an exception handler; use busy_wait if you must");
+ }
+#endif
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+ uint64_t t_us = to_us_since_boot(t);
+ uint64_t t_before_us = t_us - PICO_TIME_SLEEP_OVERHEAD_ADJUST_US;
+ // needs to work in the first PICO_TIME_SLEEP_OVERHEAD_ADJUST_US of boot
+ if (t_before_us > t_us) t_before_us = 0;
+ absolute_time_t t_before;
+ update_us_since_boot(&t_before, t_before_us);
+ if (absolute_time_diff_us(get_absolute_time(), t_before) > 0) {
+ if (add_alarm_at(t_before, sleep_until_callback, NULL, false) >= 0) {
+ // able to add alarm for just before the time
+ while (!time_reached(t_before)) {
+ uint32_t save = spin_lock_blocking(sleep_notifier.spin_lock);
+ lock_internal_spin_unlock_with_wait(&sleep_notifier, save);
+ }
+ }
+ }
+#else
+ // hook in case we're in RTOS; note we assume using the alarm pool is better always if available.
+ sync_internal_yield_until_before(t);
+#endif
+ // now wait until the exact time
+ busy_wait_until(t);
+}
+
+void sleep_us(uint64_t us) {
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+ sleep_until(make_timeout_time_us(us));
+#else
+ if (us < PICO_TIME_SLEEP_OVERHEAD_ADJUST_US) {
+ busy_wait_us(us);
+ } else {
+ // hook in case we're in RTOS; note we assume using the alarm pool is better always if available.
+ absolute_time_t t = make_timeout_time_us(us - PICO_TIME_SLEEP_OVERHEAD_ADJUST_US);
+ sync_internal_yield_until_before(t);
+
+ // then wait the rest of thw way
+ busy_wait_until(t);
+ }
+#endif
+}
+
+void sleep_ms(uint32_t ms) {
+ sleep_us(ms * 1000ull);
+}
+
+bool best_effort_wfe_or_timeout(absolute_time_t timeout_timestamp) {
+#if !PICO_TIME_DEFAULT_ALARM_POOL_DISABLED
+ alarm_id_t id;
+ id = add_alarm_at(timeout_timestamp, sleep_until_callback, NULL, false);
+ if (id <= 0) {
+ tight_loop_contents();
+ return time_reached(timeout_timestamp);
+ } else {
+ __wfe();
+ // we need to clean up if it wasn't us that caused the wfe; if it was this will be a noop.
+ cancel_alarm(id);
+ return time_reached(timeout_timestamp);
+ }
+#else
+ tight_loop_contents();
+ return time_reached(timeout_timestamp);
+#endif
+}