| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // ====================================================================== | ||
| 2 | // \title AtomicQueue.hpp | ||
| 3 | // \author B. Duckett | ||
| 4 | // \brief A lock-free FIFO queue using atomics for thread/ISR safety | ||
| 5 | // | ||
| 6 | // \copyright | ||
| 7 | // Copyright 2026, by the California Institute of Technology. | ||
| 8 | // ALL RIGHTS RESERVED. United States Government Sponsorship | ||
| 9 | // acknowledged. | ||
| 10 | // | ||
| 11 | // ====================================================================== | ||
| 12 | |||
| 13 | #ifndef OS_GENERIC_TYPES_ATOMIC_QUEUE_HPP | ||
| 14 | #define OS_GENERIC_TYPES_ATOMIC_QUEUE_HPP | ||
| 15 | |||
| 16 | #include <Fw/FPrimeBasicTypes.hpp> | ||
| 17 | #include <Fw/Types/Assert.hpp> | ||
| 18 | #include <Fw/Types/ByteArray.hpp> | ||
| 19 | #include <Fw/Types/MemAllocator.hpp> | ||
| 20 | #include <Os/CountingSemaphore.hpp> | ||
| 21 | #include <atomic> | ||
| 22 | |||
| 23 | // Forward declaration for test-only friend access | ||
| 24 | class AtomicQueueWrapAroundTest; | ||
| 25 | |||
| 26 | namespace Types { | ||
| 27 | |||
| 28 | //! \class AtomicQueue | ||
| 29 | //! \brief A lock-free MPMC FIFO circular buffer with fixed-size buffer storage | ||
| 30 | //! | ||
| 31 | //! This queue stores fixed-size message buffers using memcpy semantics. Each slot | ||
| 32 | //! contains an embedded buffer (not just a pointer). The circular buffer uses atomic | ||
| 33 | //! sequence numbers for lock-free coordination between multiple producers/consumers. | ||
| 34 | //! | ||
| 35 | //! Features: | ||
| 36 | //! - O(1) enqueue/dequeue with bounded CAS retries | ||
| 37 | //! - Embedded buffer storage (memcpy on send/receive) | ||
| 38 | //! - Optional blocking enqueue via Os::CountingSemaphore (platform-agnostic) | ||
| 39 | //! - Uses only word-size atomics (no DWCAS), portable to all architectures | ||
| 40 | //! | ||
| 41 | //! Pre-allocates memory for slots and buffers during create() | ||
| 42 | //! \note Power-of-2 capacity uses fast bitwise AND; other sizes use modulo (~5-20% slower) | ||
| 43 | //! | ||
| 44 | //! \warning Position Counter Wrap-Around: The m_enqueuePos and m_dequeuePos counters are | ||
| 45 | //! FwSizeType (platform word size). On 64-bit platforms (FwSizeType=U64), wrap-around | ||
| 46 | //! is functionally impossible (~584 years at 1 GHz). However, on 32-bit platforms | ||
| 47 | //! (FwSizeType=U32), wrap-around occurs after 2^32 operations (~1.2 hours at 1M ops/sec). | ||
| 48 | //! The algorithm remains correct after wrap (sequence numbers prevent ABA), but applications | ||
| 49 | //! with sustained high throughput on 32-bit systems should be aware. FwSizeType=U32 is | ||
| 50 | //! used (instead of forcing U64) to support platforms with 32-bit native word size where | ||
| 51 | //! 64-bit atomics may not be lock-free or require expensive emulation. | ||
| 52 | class AtomicQueue { | ||
| 53 | friend class ::AtomicQueueWrapAroundTest; // Test-only accessor for counter manipulation | ||
| 54 | public: | ||
| 55 | //! \brief AtomicQueue constructor | ||
| 56 | AtomicQueue(); | ||
| 57 | |||
| 58 | //! \brief AtomicQueue destructor | ||
| 59 | ~AtomicQueue(); | ||
| 60 | |||
| 61 | // Rule of Five: prevent accidental copy/move of resource-managing class | ||
| 62 | AtomicQueue(const AtomicQueue&) = delete; | ||
| 63 | AtomicQueue& operator=(const AtomicQueue&) = delete; | ||
| 64 | AtomicQueue(AtomicQueue&&) = delete; | ||
| 65 | AtomicQueue& operator=(AtomicQueue&&) = delete; | ||
| 66 | |||
| 67 | //! \brief Create the queue with embedded buffer storage | ||
| 68 | //! | ||
| 69 | //! Creates queue with blocking semaphore support for enqueueBlocking(). | ||
| 70 | //! | ||
| 71 | //! \param numBuffers maximum number of messages (any value > 0) | ||
| 72 | //! \param bufferSize size of each message buffer in bytes | ||
| 73 | //! \param allocator memory allocator for dynamic allocation | ||
| 74 | //! \param allocatorId allocator identifier for tracking | ||
| 75 | void create(FwSizeType numBuffers, FwSizeType bufferSize, Fw::MemAllocator& allocator, FwEnumStoreType allocatorId); | ||
| 76 | |||
| 77 | //! \brief Teardown the queue and free allocated memory | ||
| 78 | void teardown(); | ||
| 79 | |||
| 80 | //! \brief Enqueue a message (multi-producer safe, non-blocking, O(1)) | ||
| 81 | //! | ||
| 82 | //! Copies message data into an available slot using memcpy. Multiple producers | ||
| 83 | //! can safely call this concurrently. Never blocks. | ||
| 84 | //! | ||
| 85 | //! ISR-SAFETY: Platform-dependent (calls semaphore tryWait). ISR-safe on: | ||
| 86 | //! VxWorks, FreeRTOS, INTEGRITY, ThreadX, RTEMS, QNX, Zephyr, embOS, µC/OS. | ||
| 87 | //! NOT ISR-safe on: POSIX RT, Linux (standard/non-RT). Verify platform before ISR use. | ||
| 88 | //! | ||
| 89 | //! \param buffer source buffer to copy from | ||
| 90 | //! \param size size of data to copy (must be ≤ bufferSize) | ||
| 91 | //! \return true if successful, false if queue is full or retry limit exceeded | ||
| 92 | bool enqueue(const U8* buffer, FwSizeType size); | ||
| 93 | |||
| 94 | //! \brief Enqueue with optional blocking (multi-producer safe, O(1)) | ||
| 95 | //! | ||
| 96 | //! Copies message data into available slot. If queue is full and blocking enabled, | ||
| 97 | //! waits on semaphore until space available. | ||
| 98 | //! | ||
| 99 | //! WARNING: When blockIfFull=true, this function can block and is NOT ISR-safe. | ||
| 100 | //! For ISR contexts, always use enqueue() or set blockIfFull=false. | ||
| 101 | //! | ||
| 102 | //! Blocking is bounded, not unconditional: each reserved slot may be taken by a | ||
| 103 | //! concurrent producer between the semaphore wait and the enqueue, and the reserve-retry | ||
| 104 | //! cycle is attempted at most MAX_CAS_RETRIES times. Sustained contention that loses every | ||
| 105 | //! attempt therefore returns false even with blockIfFull=true. Callers must handle a false | ||
| 106 | //! return in both modes. | ||
| 107 | //! | ||
| 108 | //! \param buffer source buffer to copy from | ||
| 109 | //! \param size size of data to copy (must be ≤ bufferSize) | ||
| 110 | //! \param blockIfFull if true, blocks when full (caller must ensure not in ISR) | ||
| 111 | //! \return true if successful, false if the queue was full (non-blocking mode) or the | ||
| 112 | //! retry limit was exhausted (blocking mode) | ||
| 113 | bool enqueueBlocking(const U8* buffer, FwSizeType size, bool blockIfFull); | ||
| 114 | |||
| 115 | //! \brief Dequeue a message (multi-consumer safe, non-blocking, O(1)) | ||
| 116 | //! | ||
| 117 | //! Copies message data from oldest slot using memcpy. Multiple consumers can | ||
| 118 | //! safely call this concurrently. Never blocks. | ||
| 119 | //! | ||
| 120 | //! ISR-SAFETY: Depends on platform semaphore post() implementation. ISR-safe on: | ||
| 121 | //! VxWorks, FreeRTOS, Green Hills INTEGRITY, ThreadX, RTEMS, QNX Neutrino, | ||
| 122 | //! Zephyr RTOS, embOS, µC/OS-II, µC/OS-III, SafeRTOS, Azure RTOS. | ||
| 123 | //! NOT ISR-safe on: POSIX RT (strict spec), Linux (standard/non-RT), Embedded | ||
| 124 | //! Linux without RT patches. Verify platform semaphore before ISR use. | ||
| 125 | //! | ||
| 126 | //! \param buffer destination buffer to copy into | ||
| 127 | //! \param capacity size of destination buffer | ||
| 128 | //! \param actualSize output parameter for actual message size copied | ||
| 129 | //! \return true if successful, false if queue is empty or retry limit exceeded | ||
| 130 | bool dequeue(U8* buffer, FwSizeType capacity, FwSizeType& actualSize); | ||
| 131 | |||
| 132 | //! \brief Check if the queue is full | ||
| 133 | //! | ||
| 134 | //! \return true if all nodes are in use, false otherwise | ||
| 135 | bool isFull() const; | ||
| 136 | |||
| 137 | //! \brief Check if the queue is empty | ||
| 138 | //! | ||
| 139 | //! \return true if no nodes are in the queue, false otherwise | ||
| 140 | bool isEmpty() const; | ||
| 141 | |||
| 142 | //! \brief Get the current number of elements in the queue | ||
| 143 | //! | ||
| 144 | //! \return current size of the queue | ||
| 145 | FwSizeType getSize() const; | ||
| 146 | |||
| 147 | //! \brief Get the maximum capacity of the queue | ||
| 148 | //! | ||
| 149 | //! \return maximum number of buffers | ||
| 150 | FwSizeType getCapacity() const; | ||
| 151 | |||
| 152 | //! \brief Get the buffer size for each message | ||
| 153 | //! | ||
| 154 | //! \return buffer size in bytes | ||
| 155 | FwSizeType getBufferSize() const; | ||
| 156 | |||
| 157 | //! \brief Check if queue has been successfully created | ||
| 158 | //! | ||
| 159 | //! \return true if create() completed successfully, false otherwise | ||
| 160 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
1 | bool isCreated() const { return this->m_slots != nullptr && this->m_capacity > 0; } |
| 161 | |||
| 162 | private: | ||
| 163 | //! Maximum CAS retry attempts (JPL Power of Ten: bounded loops) | ||
| 164 | static constexpr FwSizeType MAX_CAS_RETRIES = 100; | ||
| 165 | |||
| 166 | //! \brief Slot structure for circular buffer with embedded buffer storage | ||
| 167 | //! | ||
| 168 | //! Each slot contains an embedded buffer and sequence number for lock-free coordination. | ||
| 169 | //! Sequence encoding: | ||
| 170 | //! - seq == index: ready for write (producer can claim) | ||
| 171 | //! - seq == index + 1: ready for read (consumer can claim) | ||
| 172 | //! - seq == index + capacity: completed read, next cycle's write position | ||
| 173 | //! | ||
| 174 | //! Memory layout: Natural alignment (~24 bytes on 64-bit platforms) | ||
| 175 | //! For large slot counts (>20K), this saves significant memory vs cache line alignment | ||
| 176 | struct Slot { | ||
| 177 | U8* buffer; // Embedded message buffer | ||
| 178 | FwSizeType size; // Actual message size stored | ||
| 179 | std::atomic<FwSizeType> sequence; // Coordination sequence number | ||
| 180 | }; | ||
| 181 | |||
| 182 | //! \brief Calculate slot index from position | ||
| 183 | //! | ||
| 184 | //! Uses bitwise AND for power-of-2 capacity (fast), modulo otherwise. | ||
| 185 | //! \param pos position value | ||
| 186 | //! \return slot index in range [0, capacity) | ||
| 187 | 92762 | inline FwSizeType getIndex(FwSizeType pos) const { | |
| 188 | 92762 | FW_ASSERT(this->m_capacity > 0); | |
| 189 |
3/4✓ Branch 2 taken 85901 times.
✓ Branch 3 taken 6885 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 6876 times.
|
92774 | return (this->m_mask != 0) ? (pos & this->m_mask) : (pos % this->m_capacity); |
| 190 | } | ||
| 191 | |||
| 192 | //! \brief Compute simple checksum for diagnostic logging | ||
| 193 | static U32 computeChecksum(const U8* buffer, FwSizeType size); | ||
| 194 | |||
| 195 | //! \brief Internal enqueue without semaphore interaction | ||
| 196 | //! | ||
| 197 | //! Used by both enqueue() and enqueueBlocking() to avoid double-decrementing | ||
| 198 | //! the semaphore. Performs the lock-free enqueue operation only. | ||
| 199 | //! | ||
| 200 | //! \param buffer source buffer to copy from | ||
| 201 | //! \param size size of data to copy | ||
| 202 | //! \return true if successful, false if queue full | ||
| 203 | bool enqueueInternal(const U8* buffer, FwSizeType size); | ||
| 204 | |||
| 205 | // Private members: | ||
| 206 | Slot* m_slots; // Circular slot array | ||
| 207 | U8* m_bufferMemory; // Contiguous buffer memory block | ||
| 208 | FwSizeType m_capacity; // Number of message buffers | ||
| 209 | FwSizeType m_bufferSize; // Size of each message buffer | ||
| 210 | FwSizeType m_mask; // Bitmask if power-of-2, else 0 | ||
| 211 | std::atomic<FwSizeType> m_enqueuePos; // Next enqueue position (producer cursor) | ||
| 212 | std::atomic<FwSizeType> m_dequeuePos; // Next dequeue position (consumer cursor) | ||
| 213 | Fw::MemAllocator* m_allocator; // Memory allocator (nullptr if not using allocator) | ||
| 214 | FwEnumStoreType m_allocatorId; // Allocator identifier for deallocation (also used to identify asserts) | ||
| 215 | Os::CountingSemaphore* m_notFullSem; // Semaphore for blocking enqueue (all platforms) | ||
| 216 | }; | ||
| 217 | |||
| 218 | } // namespace Types | ||
| 219 | |||
| 220 | #endif // OS_GENERIC_TYPES_ATOMIC_QUEUE_HPP | ||
| 221 |