420 lines
16 KiB
C
420 lines
16 KiB
C
/*
|
|
* 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;
|
|
uint8_t core_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;
|
|
pool->core_num = (uint8_t) get_core_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;
|
|
}
|
|
|
|
uint alarm_pool_core_num(alarm_pool_t *pool) {
|
|
return pool->core_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
|
|
}
|