common: Add SPSC lock-free queue.

Signed-off-by: iabdalkader <i.abdalkader@gmail.com>
This commit is contained in:
iabdalkader 2025-07-12 19:59:12 +02:00
parent d4186a4408
commit 50c9be73c8
3 changed files with 252 additions and 0 deletions

View File

@ -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))

190
common/queue.c Normal file
View File

@ -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 <stdio.h>
#include <stdlib.h>
#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;
}

61
common/queue.h Normal file
View File

@ -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 <stddef.h>
#include <stdbool.h>
#if __STDC_VERSION__ < 201112L
typedef size_t queue_index_t;
#warning "Atomics not supported"
#else
#include <stdatomic.h>
#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__