diff --git a/src/common/pico_util/include/pico/util/queue.h b/src/common/pico_util/include/pico/util/queue.h index 80e5a927d..68114f532 100644 --- a/src/common/pico_util/include/pico/util/queue.h +++ b/src/common/pico_util/include/pico/util/queue.h @@ -62,6 +62,29 @@ 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 Initialise a static 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 spinlock_num The spin ID used to protect the queue + * \param storage Pointer to queue storage array + * \param storage_size Size of the queue storage array. (Must be N + 1 elements.) + */ +void static_queue_init_with_spinlock(queue_t *q, uint element_size, uint spinlock_num, uint8_t *storage, uint32_t storage_size); + +/*! \brief Initialise a static 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 storage Pointer to queue storage array + * \param storage_size Size of the queue storage array. (Must be N + 1 elements.) + */ +static inline void static_queue_init(queue_t *q, uint element_size, uint8_t *storage, uint32_t storage_size) { + static_queue_init_with_spinlock(q, element_size, next_striped_spin_lock_num(), storage, storage_size); +} + /*! \brief Destroy the specified queue. * \ingroup queue * diff --git a/src/common/pico_util/queue.c b/src/common/pico_util/queue.c index a5c8e181f..b8afd860b 100644 --- a/src/common/pico_util/queue.c +++ b/src/common/pico_util/queue.c @@ -9,6 +9,7 @@ #include "pico/util/queue.h" void queue_init_with_spinlock(queue_t *q, uint element_size, uint element_count, uint spinlock_num) { + assert(q != NULL); lock_init(&q->core, spinlock_num); q->data = (uint8_t *)calloc(element_count + 1, element_size); q->element_count = (uint16_t)element_count; @@ -17,7 +18,19 @@ void queue_init_with_spinlock(queue_t *q, uint element_size, uint element_count, q->rptr = 0; } +void static_queue_init_with_spinlock(queue_t *q, uint element_size, uint spinlock_num, uint8_t *storage, uint32_t storage_size) { + assert(q != NULL); + assert((storage_size / element_size) >= 2); + lock_init(&q->core, spinlock_num); + q->data = storage; + q->element_count = (uint16_t) ((storage_size / element_size) - 1); + q->element_size = (uint16_t)element_size; + q->wptr = 0; + q->rptr = 0; +} + void queue_free(queue_t *q) { + assert(q != NULL); free(q->data); } @@ -95,25 +108,31 @@ static bool queue_peek_internal(queue_t *q, void *data, bool block) { } bool queue_try_add(queue_t *q, const void *data) { + assert(q != NULL); return queue_add_internal(q, data, false); } bool queue_try_remove(queue_t *q, void *data) { + assert(q != NULL); return queue_remove_internal(q, data, false); } bool queue_try_peek(queue_t *q, void *data) { + assert(q != NULL); return queue_peek_internal(q, data, false); } void queue_add_blocking(queue_t *q, const void *data) { + assert(q != NULL); queue_add_internal(q, data, true); } void queue_remove_blocking(queue_t *q, void *data) { + assert(q != NULL); queue_remove_internal(q, data, true); } void queue_peek_blocking(queue_t *q, void *data) { + assert(q != NULL); queue_peek_internal(q, data, true); }