mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
common: Add SPSC lock-free queue.
Signed-off-by: iabdalkader <i.abdalkader@gmail.com>
This commit is contained in:
parent
d4186a4408
commit
50c9be73c8
@ -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
190
common/queue.c
Normal 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
61
common/queue.h
Normal 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__
|
||||
Loading…
Reference in New Issue
Block a user