From 50c9be73c83ff2d55012aba77d6193942debfd49 Mon Sep 17 00:00:00 2001 From: iabdalkader Date: Sat, 12 Jul 2025 19:59:12 +0200 Subject: [PATCH] common: Add SPSC lock-free queue. Signed-off-by: iabdalkader --- common/common.mk | 1 + common/queue.c | 190 +++++++++++++++++++++++++++++++++++++++++++++++ common/queue.h | 61 +++++++++++++++ 3 files changed, 252 insertions(+) create mode 100644 common/queue.c create mode 100644 common/queue.h diff --git a/common/common.mk b/common/common.mk index 71949aaec..c5c9c05ff 100644 --- a/common/common.mk +++ b/common/common.mk @@ -38,6 +38,7 @@ COMMON_SRC_C += \ unaligned_memcpy.c \ usbdbg.c \ vospi.c \ + queue.c \ CFLAGS += -I$(TOP_DIR)/common OMV_FIRM_OBJ += $(addprefix $(BUILD)/common/, $(COMMON_SRC_C:.c=.o)) diff --git a/common/queue.c b/common/queue.c new file mode 100644 index 000000000..14232c833 --- /dev/null +++ b/common/queue.c @@ -0,0 +1,190 @@ +/* + * SPDX-License-Identifier: MIT + * + * Copyright (C) 2013-2024 OpenMV, LLC. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * Single-producer, single-consumer, lock-free bounded Queue. + * + * ARM processors have weak memory ordering and may reorder memory operations + * for performance. The compiler can also reorder operations. This queue uses + * acquire/release semantics to ensure proper synchronization between producer + * and consumer threads. + */ +#include +#include +#include "queue.h" + +void queue_init(queue_t **q, size_t capacity, void *buffer) { + if (!q || !buffer) { + return; + } + + *q = (queue_t *) buffer; + (*q)->capacity = capacity; + queue_flush(*q); +} + +queue_t *queue_alloc(size_t capacity) { + if (capacity == 0) { + return NULL; + } + + void *buffer = malloc(queue_calc_size(capacity)); + if (!buffer) { + return NULL; + } + + queue_t *q; + queue_init(&q, capacity, buffer); + return q; +} + +void queue_destroy(queue_t *q) { + free(q); +} + +void queue_flush(queue_t *q) { + if (!q) { + return; + } + + #ifndef HAVE_STDATOMIC_H + q->head = 0; + q->tail = 0; + #else + atomic_store_explicit(&q->head, 0, memory_order_relaxed); + atomic_store_explicit(&q->tail, 0, memory_order_relaxed); + #endif +} + +bool queue_push(queue_t *q, void *item) { + if (!q || !item) { + return false; + } + + #ifndef HAVE_STDATOMIC_H + size_t new_tail = (q->tail + 1) % (q->capacity + 1); + + if (new_tail == q->head) { + return false; // Queue full + } + q->items[q->tail] = item; + q->tail = new_tail; + #else + size_t old_tail = atomic_load_explicit(&q->tail, memory_order_relaxed); + size_t new_tail = (old_tail + 1) % (q->capacity + 1); + + // Ensure that the previous head is consumed. + if (new_tail == atomic_load_explicit(&q->head, memory_order_acquire)) { + return false; // Queue full + } + + q->items[old_tail] = item; + // Release ensures the write completes before advancing tail + atomic_store_explicit(&q->tail, new_tail, memory_order_release); + #endif + + return true; +} + +void *queue_pop(queue_t *q, bool peek) { + if (queue_is_empty(q)) { + return NULL; + } + + #ifndef HAVE_STDATOMIC_H + void *item = q->items[q->head]; + if (!peek) { + q->head = (q->head + 1) % (q->capacity + 1); + } + #else + size_t old_head = atomic_load_explicit(&q->head, memory_order_relaxed); + size_t new_head = (old_head + 1) % (q->capacity + 1); + void *item = q->items[old_head]; + + if (!peek) { + // Release ensures the read completes before advancing head + atomic_store_explicit(&q->head, new_head, memory_order_release); + } + #endif + + return item; +} + +bool queue_is_empty(const queue_t *q) { + if (!q) { + return true; + } + + #ifndef HAVE_STDATOMIC_H + size_t head = q->head; + size_t tail = q->tail; + #else + // Ensure the current thread sees all prior updates to head and tail + size_t head = atomic_load_explicit(&q->head, memory_order_acquire); + size_t tail = atomic_load_explicit(&q->tail, memory_order_acquire); + #endif + return head == tail; +} + +size_t queue_size(const queue_t *q) { + if (!q) { + return 0; + } + + #ifndef HAVE_STDATOMIC_H + size_t head = q->head; + size_t tail = q->tail; + #else + // Ensure the current thread sees all prior updates to head and tail + size_t head = atomic_load_explicit(&q->head, memory_order_acquire); + size_t tail = atomic_load_explicit(&q->tail, memory_order_acquire); + #endif + + // Calculate size considering the circular buffer + if (tail >= head) { + return tail - head; + } else { + return (q->capacity + 1) - head + tail; + } +} + +// Swap the last item of q0 (tail - 1) with the first item of q1 (head). +// Currently, this is only used for a special case in the framebuffer, +// and it's only safe under very specific conditions. +void *queue_swap(queue_t *q0, queue_t *q1) { + #ifndef HAVE_STDATOMIC_H + size_t tail = (q0->tail == 0) ? q0->capacity : q0->tail - 1; + size_t head = q1->head; + #else + size_t tail = atomic_load_explicit(&q0->tail, memory_order_acquire); + size_t head = atomic_load_explicit(&q1->head, memory_order_acquire); + tail = (tail == 0) ? q0->capacity : tail - 1; + #endif + + void *item0 = q0->items[tail]; + void *item1 = q1->items[head]; + + q0->items[tail] = item1; + q1->items[head] = item0; + + return item0; +} diff --git a/common/queue.h b/common/queue.h new file mode 100644 index 000000000..7bca4b724 --- /dev/null +++ b/common/queue.h @@ -0,0 +1,61 @@ +/* + * SPDX-License-Identifier: MIT + * + * Copyright (C) 2013-2024 OpenMV, LLC. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * Single-producer, single-consumer, lock-free bounded Queue. + */ +#ifndef __QUEUE_H__ +#define __QUEUE_H__ + +#include +#include + +#if __STDC_VERSION__ < 201112L +typedef size_t queue_index_t; +#warning "Atomics not supported" +#else +#include +#define HAVE_STDATOMIC_H +typedef atomic_size_t queue_index_t; +#endif + +typedef struct { + size_t capacity; + queue_index_t head; + queue_index_t tail; // TODO pad with cache line if needed. + void *items[]; +} queue_t; + +// One extra slot is used to distinguish full from empty. +#define queue_calc_size(capacity) \ + (sizeof(queue_t) + ((capacity) + 1) * sizeof(void *)) + +void queue_init(queue_t **q, size_t capacity, void *buffer); +void queue_flush(queue_t *q); +queue_t *queue_alloc(size_t capacity); +void queue_destroy(queue_t *q); +bool queue_is_empty(const queue_t *q); +bool queue_push(queue_t *q, void *item); +void *queue_pop(queue_t *q, bool peek); +size_t queue_size(const queue_t *q); +void *queue_swap(queue_t *q0, queue_t *q1); +#endif // __QUEUE_H__