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_util/queue.c b/src/common/pico_util/queue.c
new file mode 100644
index 0000000..a5c8e18
--- /dev/null
+++ b/src/common/pico_util/queue.c
@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include "pico/util/queue.h"
+
+void queue_init_with_spinlock(queue_t *q, uint element_size, uint element_count, uint spinlock_num) {
+ lock_init(&q->core, spinlock_num);
+ q->data = (uint8_t *)calloc(element_count + 1, element_size);
+ q->element_count = (uint16_t)element_count;
+ q->element_size = (uint16_t)element_size;
+ q->wptr = 0;
+ q->rptr = 0;
+}
+
+void queue_free(queue_t *q) {
+ free(q->data);
+}
+
+static inline void *element_ptr(queue_t *q, uint index) {
+ assert(index <= q->element_count);
+ return q->data + index * q->element_size;
+}
+
+static inline uint16_t inc_index(queue_t *q, uint16_t index) {
+ if (++index > q->element_count) { // > because we have element_count + 1 elements
+ index = 0;
+ }
+
+#if PICO_QUEUE_MAX_LEVEL
+ uint16_t level = queue_get_level_unsafe(q);
+ if (level > q->max_level) {
+ q->max_level = level;
+ }
+#endif
+
+ return index;
+}
+
+static bool queue_add_internal(queue_t *q, const void *data, bool block) {
+ do {
+ uint32_t save = spin_lock_blocking(q->core.spin_lock);
+ if (queue_get_level_unsafe(q) != q->element_count) {
+ memcpy(element_ptr(q, q->wptr), data, q->element_size);
+ q->wptr = inc_index(q, q->wptr);
+ lock_internal_spin_unlock_with_notify(&q->core, save);
+ return true;
+ }
+ if (block) {
+ lock_internal_spin_unlock_with_wait(&q->core, save);
+ } else {
+ spin_unlock(q->core.spin_lock, save);
+ return false;
+ }
+ } while (true);
+}
+
+static bool queue_remove_internal(queue_t *q, void *data, bool block) {
+ do {
+ uint32_t save = spin_lock_blocking(q->core.spin_lock);
+ if (queue_get_level_unsafe(q) != 0) {
+ memcpy(data, element_ptr(q, q->rptr), q->element_size);
+ q->rptr = inc_index(q, q->rptr);
+ lock_internal_spin_unlock_with_notify(&q->core, save);
+ return true;
+ }
+ if (block) {
+ lock_internal_spin_unlock_with_wait(&q->core, save);
+ } else {
+ spin_unlock(q->core.spin_lock, save);
+ return false;
+ }
+ } while (true);
+}
+
+static bool queue_peek_internal(queue_t *q, void *data, bool block) {
+ do {
+ uint32_t save = spin_lock_blocking(q->core.spin_lock);
+ if (queue_get_level_unsafe(q) != 0) {
+ memcpy(data, element_ptr(q, q->rptr), q->element_size);
+ lock_internal_spin_unlock_with_notify(&q->core, save);
+ return true;
+ }
+ if (block) {
+ lock_internal_spin_unlock_with_wait(&q->core, save);
+ } else {
+ spin_unlock(q->core.spin_lock, save);
+ return false;
+ }
+ } while (true);
+}
+
+bool queue_try_add(queue_t *q, const void *data) {
+ return queue_add_internal(q, data, false);
+}
+
+bool queue_try_remove(queue_t *q, void *data) {
+ return queue_remove_internal(q, data, false);
+}
+
+bool queue_try_peek(queue_t *q, void *data) {
+ return queue_peek_internal(q, data, false);
+}
+
+void queue_add_blocking(queue_t *q, const void *data) {
+ queue_add_internal(q, data, true);
+}
+
+void queue_remove_blocking(queue_t *q, void *data) {
+ queue_remove_internal(q, data, true);
+}
+
+void queue_peek_blocking(queue_t *q, void *data) {
+ queue_peek_internal(q, data, true);
+}