lib: bm_fifo: a FIFO queue implementation

lemrey · lemrey · commit de7a5acfcdf2 · 2025-06-15T15:32:42.000+02:00
A simple FIFO queue implementation using a circular buffer,
that is ISR-safe.

Signed-off-by: Emanuele Di Santo &lt;emdi@nordicsemi.no&gt;
diff --git a/include/bm_fifo.h b/include/bm_fifo.h
@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2025 Nordic Semiconductor ASA
+ *
+ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
+ */
+#include <stddef.h>
+#include <stdint.h>
+
+struct bm_fifo {
+	/**
+	 * @brief FIFO buffer.
+	 */
+	void *buf;
+	/**
+	 * @brief FIFO maximum capacity (number of items).
+	 */
+	uint32_t capacity;
+	/**
+	 * @brief FIFO item size.
+	 */
+	size_t item_size;
+	/**
+	 * @brief Number of items in the queue.
+	 */
+	uint32_t count;
+	/**
+	 * @brief FIFO head.
+	 */
+	int head;
+	/**
+	 * @brief FIFO tail.
+	 */
+	int tail;
+};
+
+/**
+ * @brief Statically define a FIFO.
+ *
+ * Avoids the @ref bm_fifo_init() call.
+ */
+#define BM_FIFO_INIT(_name, _capacity, _item_size)                                                 \
+	static uint8_t _name_buf[_item_size * _capacity];                                          \
+	static struct bm_fifo _name = {                                                            \
+		.buf = _name_buf,                                                                  \
+		.item_size = _item_size,                                                           \
+		.capacity = _capacity,                                                             \
+	}
+
+/**
+ * @brief Initialize a queue.
+ *
+ * @param fifo FIFO queue.
+ * @param buf Queue buffer.
+ * @param capacity Buffer capacity, in number of items.
+ * @param item_size Size of a queue element, in bytes.
+ *
+ * @retval 0 on success.
+ * @retval -EFAULT If @p fifo or @p buf are @c NULL.
+ * @retval -EINVAL If @p capacity or @p item_size are 0.
+ */
+int bm_fifo_init(struct bm_fifo *fifo, void *buf, size_t capacity, size_t item_size);
+
+/**
+ * @brief Check whether the queue is full.
+ *
+ * @param fifo FIFO queue.
+ *
+ * @return true Queue is full.
+ * @return false Queue is not full.
+ */
+bool bm_fifo_is_full(const struct bm_fifo *const fifo);
+
+/**
+ * @brief Check whether the queue is empty.
+ *
+ * @param fifo FIFO queue.
+ *
+ * @return true Queue is empty.
+ * @return false Queue is not empty.
+ */
+bool bm_fifo_is_empty(const struct bm_fifo *const fifo);
+
+/**
+ * @brief Queue an element.
+ *
+ * The element is copied into the queue's own buffer.
+ * Interrupts are disabled during the copy.
+ *
+ * @param fifo FIFO queue.
+ * @param buf Buffer pointing to the element.
+ *
+ * @retval 0 on success.
+ * @retval -EFAULT If @p fifo or @p buf are @c NULL.
+ * @retval -ENOBUFS If there are no buffers available in the queue.
+ */
+int bm_fifo_enqueue(struct bm_fifo *fifo, void *buf);
+
+/**
+ * @brief Dequeue an element.
+ *
+ * Dequeue an element from the queue's head.
+ *
+ * @param fifo FIFO queue.
+ * @param buf Buffer to copy the element into.
+ *
+ * @retval 0 on success.
+ * @retval -EFAULT If @p fifo or @p buf are @c NULL.
+ * @retval -ENOENT If the queue is empty.
+ */
+int bm_fifo_dequeue(struct bm_fifo *fifo, void *buf);
+
+/**
+ * @brief Peek at the queue.
+ *
+ * Peek at the queue's head.
+ *
+ * @param fifo FIFO queue.
+ * @param buf Buffer to copy the element into.
+ *
+ * @retval 0 on success.
+ * @retval -EFAULT If @p fifo or @p buf are @c NULL.
+ * @retval -ENOENT If the queue is empty.
+ */
+int bm_fifo_peek(const struct bm_fifo *fifo, void *buf);
+
+/**
+ * @brief Dequeue one element and discard it.
+ *
+ * Dequeue an element and discard it.
+ *
+ * @param fifo FIFO queue.
+ *
+ * @retval 0 on success.
+ * @retval -EFAULT If @p fifo is @c NULL.
+ * @retval -ENOENT If the queue is empty.
+ */
+int bm_fifo_discard(struct bm_fifo *fifo);
+
+/**
+ * @brief Clear the queue, discarding all elements.
+ *
+ * @param fifo FIFO queue.
+ *
+ * @retval 0 on success.
+ * @retval -EFAULT If @p fifo is @c NULL.
+ */
+int bm_fifo_clear(struct bm_fifo *fifo);
diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt
@@ -11,6 +11,7 @@ add_subdirectory_ifdef(CONFIG_BLE_RACP ble_racp)
 add_subdirectory_ifdef(CONFIG_EVENT_SCHEDULER event_scheduler)
 add_subdirectory_ifdef(CONFIG_BM_BUTTONS bm_buttons)
 add_subdirectory_ifdef(CONFIG_BM_TIMER bm_timer)
+add_subdirectory_ifdef(CONFIG_BM_FIFO bm_fifo)
 add_subdirectory_ifdef(CONFIG_BLE_QWR ble_qwr)
 add_subdirectory_ifdef(CONFIG_SENSORSIM sensorsim)
 add_subdirectory_ifdef(CONFIG_NCS_BARE_METAL_BOOT_BANNER boot_banner)
diff --git a/lib/Kconfig b/lib/Kconfig
@@ -12,6 +12,7 @@ rsource "ble_racp/Kconfig"
 rsource "event_scheduler/Kconfig"
 rsource "bm_buttons/Kconfig"
 rsource "bm_timer/Kconfig"
+rsource "bm_fifo/Kconfig"
 rsource "ble_qwr/Kconfig"
 rsource "sensorsim/Kconfig"
 rsource "boot_banner/Kconfig"
diff --git a/lib/bm_fifo/CMakeLists.txt b/lib/bm_fifo/CMakeLists.txt
@@ -0,0 +1,7 @@
+#
+# Copyright (c) 2025 Nordic Semiconductor ASA
+#
+# SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
+#
+zephyr_library()
+zephyr_library_sources(bm_fifo.c)
diff --git a/lib/bm_fifo/Kconfig b/lib/bm_fifo/Kconfig
@@ -0,0 +1,18 @@
+#
+# Copyright (c) 2025 Nordic Semiconductor
+#
+# SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
+#
+menuconfig BM_FIFO
+	bool "FIFO queue library"
+	help
+	  A simple FIFO queue using a circular buffer.
+
+if BM_FIFO
+
+module=BM_FIFO
+module-dep=LOG
+module-str=FIFO library
+source "${ZEPHYR_BASE}/subsys/logging/Kconfig.template.log_config"
+
+endif
diff --git a/lib/bm_fifo/bm_fifo.c b/lib/bm_fifo/bm_fifo.c
@@ -0,0 +1,199 @@
+/*
+ * Copyright (c) 2025 Nordic Semiconductor ASA
+ *
+ * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
+ */
+#include <stddef.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <errno.h>
+#include <bm_fifo.h>
+#if CONFIG_CPU_ARM_M
+#include <cmsis_compiler.h> /* PRIMASK */
+#endif
+#include <zephyr/sys/__assert.h> /* __ASSERT */
+
+/* SoftDevice IRQs are defined as Zero latency IRQs.
+ * We can't lock those with Zephyr's irq_lock().
+ */
+#if CONFIG_CPU_ARM_M
+static int primask;
+#endif
+
+static void bm_fifo_critical_region_enter(void)
+{
+#if CONFIG_CPU_ARM_M
+	/* Save PRIMASK */
+	primask = __get_PRIMASK();
+	__disable_irq();
+#endif
+}
+
+static void bm_fifo_critical_region_exit(void)
+{
+#if CONFIG_CPU_ARM_M
+	/* Restore PRIMASK */
+	__set_PRIMASK(primask);
+#endif
+}
+
+bool bm_fifo_is_full(const struct bm_fifo *const fifo)
+{
+	__ASSERT_NO_MSG(fifo);
+	return (fifo->count == fifo->capacity);
+}
+
+bool bm_fifo_is_empty(const struct bm_fifo *const fifo)
+{
+	__ASSERT_NO_MSG(fifo);
+	return (fifo->count == 0);
+}
+
+int bm_fifo_init(struct bm_fifo *fifo, void *buf, size_t capacity, size_t item_size)
+{
+	if (!fifo) {
+		return -EFAULT;
+	}
+	if (!item_size || !capacity) {
+		return -EINVAL;
+	}
+
+	memset(fifo, 0x00, sizeof(*fifo));
+
+	fifo->buf = buf;
+	fifo->item_size = item_size;
+	fifo->capacity = capacity;
+
+	return 0;
+}
+
+int bm_fifo_enqueue(struct bm_fifo *fifo, void *buf)
+{
+	int err;
+	void *item;
+
+	if (!fifo || !buf) {
+		return -EFAULT;
+	}
+
+	bm_fifo_critical_region_enter();
+
+	if (bm_fifo_is_full(fifo)) {
+		err = -ENOBUFS;
+		goto out;
+	}
+
+	fifo->count++;
+	__ASSERT(fifo->count <= fifo->capacity, "Queue overflow");
+
+	item = (uint8_t *)fifo->buf + (fifo->tail * fifo->item_size);
+	fifo->tail = (fifo->tail + 1) % fifo->capacity;
+	memcpy(item, buf, fifo->item_size);
+
+	err = 0;
+
+out:
+	bm_fifo_critical_region_exit();
+	return err;
+}
+
+int bm_fifo_dequeue(struct bm_fifo *fifo, void *buf)
+{
+	int err;
+	void *item;
+
+	if (!fifo || !buf) {
+		return -EFAULT;
+	}
+
+	bm_fifo_critical_region_enter();
+
+	if (bm_fifo_is_empty(fifo)) {
+		err = -ENOENT;
+		goto out;
+	}
+
+	fifo->count--;
+	__ASSERT(fifo->count <= fifo->capacity, "Queue underflow");
+
+	item = (uint8_t *)fifo->buf + (fifo->head * fifo->item_size);
+	fifo->head = (fifo->head + 1) % fifo->capacity;
+	memcpy(buf, item, fifo->item_size);
+
+	err = 0;
+
+out:
+	bm_fifo_critical_region_exit();
+	return err;
+}
+
+int bm_fifo_peek(const struct bm_fifo *fifo, void *buf)
+{
+	int err;
+	void *item;
+
+	if (!fifo || !buf) {
+		return -EFAULT;
+	}
+
+	bm_fifo_critical_region_enter();
+
+	if (bm_fifo_is_empty(fifo)) {
+		err = -ENOENT;
+		goto out;
+	}
+
+	item = (uint8_t *)fifo->buf + (fifo->head * fifo->item_size);
+	memcpy(buf, item, fifo->item_size);
+
+	err = 0;
+
+out:
+	bm_fifo_critical_region_exit();
+	return err;
+}
+
+int bm_fifo_discard(struct bm_fifo *fifo)
+{
+	int err;
+
+	if (!fifo) {
+		return -EFAULT;
+	}
+
+	bm_fifo_critical_region_enter();
+
+	if (bm_fifo_is_empty(fifo)) {
+		err = -ENOENT;
+		goto out;
+	}
+
+	fifo->count--;
+	__ASSERT(fifo->count <= fifo->capacity, "Queue underflow");
+
+	fifo->head = (fifo->head + 1) % fifo->capacity;
+
+	err = 0;
+
+out:
+	bm_fifo_critical_region_exit();
+	return err;
+}
+
+int bm_fifo_clear(struct bm_fifo *fifo)
+{
+	if (!fifo) {
+		return -EFAULT;
+	}
+
+	bm_fifo_critical_region_enter();
+
+	fifo->head = 0;
+	fifo->tail = 0;
+	fifo->count = 0;
+
+	bm_fifo_critical_region_exit();
+
+	return 0;
+}
diff --git a/tests/lib/bm_fifo/CMakeLists.txt b/tests/lib/bm_fifo/CMakeLists.txt
diff --git a/tests/lib/bm_fifo/prj.conf b/tests/lib/bm_fifo/prj.conf
diff --git a/tests/lib/bm_fifo/src/unity_test.c b/tests/lib/bm_fifo/src/unity_test.c
