Merge 9f1958bf3f19e360cf7c990eedb4568423a28580 into 5592322465b449ef01ca5b4290f2f03fdff71381

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
  *

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);
 }
 
@@ -99,25 +112,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);
 }