GCC Code Coverage Report

Directory: ./
File: Os/Generic/Types/AtomicQueue.cpp
Date: 2026-09-03 21:13:48
Exec Total Coverage
Lines: 157 169 92.9%
Functions: 13 14 92.9%
Branches: 54 71 76.1%
Line Branch Exec Source
1 // ======================================================================
2 // \title AtomicQueue.cpp
3 // \author B. Duckett
4 // \brief Lock-free MPMC circular buffer with embedded buffer storage
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 #include <Fw/Types/Assert.hpp>
14 #include <Os/Generic/Types/AtomicQueue.hpp>
15 #include <cstdio>
16 #include <cstring>
17 #include <new>
18
19 namespace Types {
20
21 ✗ U32 AtomicQueue::computeChecksum(const U8* buffer, FwSizeType size) {
22 ✗ FW_ASSERT(buffer != nullptr);
23 ✗ U32 sum = 0;
24 ✗ for (FwSizeType i = 0; i < size; ++i) {
25 ✗ sum += buffer[i];
26 ✗ sum = (sum << 1) | (sum >> 31); // Rotate left
27 }
28 ✗ return sum;
29 }
30
31 1439 AtomicQueue::AtomicQueue()
32 1439 : m_slots(nullptr),
33 1439 m_bufferMemory(nullptr),
34 1439 m_capacity(0),
35 1439 m_bufferSize(0),
36 1439 m_mask(0),
37 1439 m_enqueuePos(0),
38 1439 m_dequeuePos(0),
39 1439 m_allocator(nullptr),
40 1439 m_allocatorId(0),
41 1439 m_notFullSem(nullptr) {}
42
43 1439 AtomicQueue::~AtomicQueue() {
44 1439 this->teardown();
45 1439 }
46
47 1435 void AtomicQueue::create(FwSizeType numBuffers,
48 FwSizeType bufferSize,
49 Fw::MemAllocator& allocator,
50 FwEnumStoreType allocatorId) {
51 1435 FW_ASSERT(numBuffers > 0, static_cast<FwAssertArgType>(numBuffers));
52 1435 FW_ASSERT(bufferSize > 0, static_cast<FwAssertArgType>(bufferSize));
53
54 1435 this->m_capacity = numBuffers;
55 1435 this->m_bufferSize = bufferSize;
56 1435 this->m_allocator = &allocator;
57 1435 this->m_allocatorId = allocatorId;
58
59 // Optimization: use bitwise AND for power-of-2, otherwise modulo
60 1435 bool isPowerOf2 = (numBuffers & (numBuffers - 1)) == 0;
61
2/2
✓ Branch 0 taken 168 times.
✓ Branch 1 taken 1267 times.
 1435 this->m_mask = isPowerOf2 ? (numBuffers - 1) : 0;
62
63 // Allocate slot array (with overflow check)
64 1435 FW_ASSERT(numBuffers <= std::numeric_limits<FwSizeType>::max() / sizeof(Slot),
65 static_cast<FwAssertArgType>(numBuffers), static_cast<FwAssertArgType>(sizeof(Slot)));
66 1435 FwSizeType slotsSize = numBuffers * sizeof(Slot);
67
1/1
✓ Branch 4 taken 1435 times.
 1435 void* slotMem = allocator.checkedAllocate(allocatorId, slotsSize, alignof(Slot));
68 1435 FW_ASSERT(slotMem != nullptr, static_cast<FwAssertArgType>(numBuffers), static_cast<FwAssertArgType>(bufferSize));
69 1435 this->m_slots = static_cast<Slot*>(slotMem);
70
71 // Allocate contiguous buffer memory for all slots (with overflow check)
72 1435 FW_ASSERT(numBuffers <= std::numeric_limits<FwSizeType>::max() / bufferSize,
73 static_cast<FwAssertArgType>(numBuffers), static_cast<FwAssertArgType>(bufferSize));
74 1435 FwSizeType totalBufferSize = numBuffers * bufferSize;
75
1/1
✓ Branch 4 taken 1435 times.
 1435 void* bufferMem = allocator.checkedAllocate(allocatorId, totalBufferSize, 64);
76 1435 FW_ASSERT(bufferMem != nullptr, static_cast<FwAssertArgType>(numBuffers), static_cast<FwAssertArgType>(bufferSize));
77 1435 this->m_bufferMemory = static_cast<U8*>(bufferMem);
78
79 // Initialize all slots with placement new and assign buffer pointers
80
2/2
✓ Branch 0 taken 25687 times.
✓ Branch 1 taken 1435 times.
 27122 for (FwSizeType i = 0; i < numBuffers; ++i) {
81 25687 Slot* slot = new (&this->m_slots[i]) Slot();
82
83 // Assign buffer from contiguous memory block
84 25687 slot->buffer = this->m_bufferMemory + (i * bufferSize);
85 25687 slot->size = 0;
86 25687 slot->sequence.store(i, std::memory_order_relaxed);
87
88 // Runtime verification that sequence atomics are lock-free
89 // This is critical for ISR safety and lock-free guarantee
90 25687 FW_ASSERT(slot->sequence.is_lock_free(), static_cast<FwAssertArgType>(i),
91 static_cast<FwAssertArgType>(numBuffers));
92 }
93
94 // Create semaphore for blocking enqueue support (all platforms)
95 // Allocate semaphore using provided allocator
96 1435 FwSizeType semSize = sizeof(Os::CountingSemaphore);
97
1/1
✓ Branch 4 taken 1435 times.
 1435 void* semMem = allocator.checkedAllocate(allocatorId, semSize, alignof(Os::CountingSemaphore));
98 1435 FW_ASSERT(semMem != nullptr, static_cast<FwAssertArgType>(numBuffers));
99
100 // Use placement new to construct semaphore with initial count = numBuffers (all slots available)
101
1/3
✓ Branch 3 taken 1435 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 1435 this->m_notFullSem = new (semMem) Os::CountingSemaphore(static_cast<U32>(numBuffers));
102 1435 FW_ASSERT(this->m_notFullSem != nullptr, static_cast<FwAssertArgType>(numBuffers));
103
104 1435 this->m_enqueuePos.store(0, std::memory_order_relaxed);
105 1435 this->m_dequeuePos.store(0, std::memory_order_relaxed);
106 1435 }
107
108 4294 void AtomicQueue::teardown() {
109 // Destroy and deallocate semaphore
110
2/2
✓ Branch 2 taken 1435 times.
✓ Branch 3 taken 2859 times.
 4294 if (this->m_notFullSem != nullptr) {
111 1435 FW_ASSERT(this->m_allocator != nullptr, 0);
112
113 // Call destructor
114 1435 this->m_notFullSem->~CountingSemaphore();
115
116 // Deallocate memory
117 1435 this->m_allocator->deallocate(this->m_allocatorId, this->m_notFullSem);
118 1435 this->m_notFullSem = nullptr;
119 }
120
121 // Destroy slots and deallocate memory
122
2/2
✓ Branch 1 taken 1435 times.
✓ Branch 2 taken 2859 times.
 4294 if (this->m_slots != nullptr) {
123 1435 FW_ASSERT(this->m_capacity > 0, static_cast<FwAssertArgType>(this->m_capacity));
124 1435 FW_ASSERT(this->m_allocator != nullptr, 0);
125
126 // Call destructors on slots
127
2/2
✓ Branch 2 taken 25687 times.
✓ Branch 3 taken 1435 times.
 27122 for (FwSizeType i = 0; i < this->m_capacity; ++i) {
128 25687 FW_ASSERT(i < this->m_capacity, static_cast<FwAssertArgType>(i),
129 static_cast<FwAssertArgType>(this->m_capacity));
130 25687 this->m_slots[i].~Slot();
131 }
132
133 // Deallocate buffer memory
134
1/2
✓ Branch 2 taken 1435 times.
✗ Branch 3 not taken.
 1435 if (this->m_bufferMemory != nullptr) {
135 1435 this->m_allocator->deallocate(this->m_allocatorId, this->m_bufferMemory);
136 1435 this->m_bufferMemory = nullptr;
137 }
138
139 // Deallocate slot array
140 1435 this->m_allocator->deallocate(this->m_allocatorId, this->m_slots);
141 1435 this->m_slots = nullptr;
142 }
143
144 4294 this->m_enqueuePos.store(0, std::memory_order_relaxed);
145 4294 this->m_dequeuePos.store(0, std::memory_order_relaxed);
146 4294 this->m_capacity = 0;
147 4294 this->m_bufferSize = 0;
148 4294 this->m_mask = 0;
149 4294 this->m_allocator = nullptr;
150 4294 }
151
152 323438 bool AtomicQueue::enqueueInternal(const U8* buffer, FwSizeType size) {
153 323438 FW_ASSERT(this->m_slots != nullptr, 0);
154 323506 FW_ASSERT(buffer != nullptr, 0);
155 323506 FW_ASSERT(size > 0, static_cast<FwAssertArgType>(size));
156 323506 FW_ASSERT(size <= this->m_bufferSize, static_cast<FwAssertArgType>(size),
157 static_cast<FwAssertArgType>(this->m_bufferSize));
158
159
2/2
✓ Branch 0 taken 322219 times.
✓ Branch 1 taken 803 times.
 323022 for (FwSizeType retry = 0; retry < MAX_CAS_RETRIES; ++retry) {
160 322219 FW_ASSERT(retry < MAX_CAS_RETRIES, static_cast<FwAssertArgType>(retry));
161
162 // acquire-release on slot->sequence ensures coherence, enqueuePos & dequeuePos are relaxed since strong memory
163 // ordering is not required
164
165 // Get current enqueue position
166 322219 FwSizeType pos = this->m_enqueuePos.load(std::memory_order_relaxed);
167
168 // Check against dequeue position to prevent lapping (detect full queue)
169 318897 FwSizeType deqPos = this->m_dequeuePos.load(std::memory_order_relaxed);
170 318944 FwSignedSizeType queueDiff = static_cast<FwSignedSizeType>(pos) - static_cast<FwSignedSizeType>(deqPos);
171
2/2
✓ Branch 2 taken 193481 times.
✓ Branch 3 taken 129621 times.
 318944 if (queueDiff >= static_cast<FwSignedSizeType>(this->m_capacity)) {
172 193481 return false; // Queue is full
173 }
174
175
1/1
✓ Branch 3 taken 129580 times.
 129621 Slot* slot = &this->m_slots[this->getIndex(pos)];
176 129637 FwSizeType seq = slot->sequence.load(std::memory_order_acquire);
177
178 // Check if slot is ready for write (seq == pos means available)
179 129125 FwSignedSizeType diff = static_cast<FwSignedSizeType>(seq) - static_cast<FwSignedSizeType>(pos);
180
181
2/2
✓ Branch 0 taken 128121 times.
✓ Branch 1 taken 1004 times.
 129125 if (diff == 0) {
182 // Slot available, try to claim it
183
1/2
✓ Branch 4 taken 128953 times.
✗ Branch 5 not taken.
 256077 if (this->m_enqueuePos.compare_exchange_weak(pos, pos + 1, std::memory_order_release,
184 std::memory_order_relaxed)) {
185 // Claimed the slot, copy message data
186 128953 FW_ASSERT(slot->buffer != nullptr, static_cast<FwAssertArgType>(pos));
187
2/4
✗ Branch 1 not taken.
✓ Branch 2 taken 128567 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 128567 times.
 129030 (void)std::memcpy(slot->buffer, buffer, size);
188 128567 slot->size = size;
189
190 // Mark slot as ready for read
191 128646 slot->sequence.store(pos + 1, std::memory_order_release);
192 128181 return true;
193 }
194
1/2
✓ Branch 0 taken 1124 times.
✗ Branch 1 not taken.
 1004 } else if (diff < 0) {
195 // Queue is full (wrapped around)
196 1124 return false;
197 }
198 // else: another producer claimed this slot, retry
199 }
200
201 803 return false;
202 }
203
204 319516 bool AtomicQueue::enqueue(const U8* buffer, FwSizeType size) {
205 319516 bool success = this->enqueueInternal(buffer, size);
206
207 // Decrement semaphore to track available slots (if semaphore exists)
208 // This ensures blocking sends see correct availability even when
209 // queue is filled via non-blocking sends.
210 //
211 // NOTE: tryWait() may fail due to race conditions when multiple threads
212 // concurrently enqueue. This is acceptable - the semaphore is a best-effort
213 // hint for blocking operations. The lock-free atomics in enqueueInternal()
214 // are the authoritative source of queue state.
215
4/4
✓ Branch 0 taken 128855 times.
✓ Branch 1 taken 193647 times.
✓ Branch 4 taken 128636 times.
✓ Branch 5 taken 8 times.
 322502 if (success && this->m_notFullSem != nullptr) {
216 128636 (void)this->m_notFullSem->tryWait();
217 }
218
219 322042 return success;
220 }
221
222 9 bool AtomicQueue::enqueueBlocking(const U8* buffer, FwSizeType size, bool blockIfFull) {
223 9 FW_ASSERT(this->m_slots != nullptr, 0);
224 9 FW_ASSERT(buffer != nullptr, 0);
225
226 // If not blocking or no semaphore, just use non-blocking enqueue
227
3/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 4 times.
 9 if (!blockIfFull || this->m_notFullSem == nullptr) {
228 5 return this->enqueue(buffer, size);
229 }
230
231 // Wait-first pattern: reserve slot via semaphore, then enqueue
232 // This prevents semaphore count drift under contention
233
1/2
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
 4 for (FwSizeType attempt = 0; attempt < MAX_CAS_RETRIES; ++attempt) {
234 4 FW_ASSERT(attempt < MAX_CAS_RETRIES, static_cast<FwAssertArgType>(attempt));
235
236 // Reserve a slot by waiting on semaphore (blocks until space available)
237 4 Os::CountingSemaphoreInterface::Status status = this->m_notFullSem->wait();
238 4 FW_ASSERT(status == Os::CountingSemaphoreInterface::Status::OP_OK, static_cast<FwAssertArgType>(status));
239
240 // Try to enqueue (should succeed since we reserved a slot)
241 // Use internal method to avoid double-decrementing semaphore
242
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 if (this->enqueueInternal(buffer, size)) {
243 4 return true; // Success
244 }
245
246 // Extremely rare: slot was stolen between wait() and enqueue()
247 // Return the semaphore permit and retry
248 ✗ status = this->m_notFullSem->post();
249 ✗ FW_ASSERT(status == Os::CountingSemaphoreInterface::Status::OP_OK, static_cast<FwAssertArgType>(status));
250 }
251
252 // Exceeded retry limit (should never happen in practice)
253 ✗ return false;
254 }
255
256 130018 bool AtomicQueue::dequeue(U8* buffer, FwSizeType capacity, FwSizeType& actualSize) {
257 130018 FW_ASSERT(this->m_slots != nullptr, 0);
258 130116 FW_ASSERT(buffer != nullptr, 0);
259 130116 FW_ASSERT(capacity > 0, static_cast<FwAssertArgType>(capacity));
260
261
2/2
✓ Branch 0 taken 138567 times.
✓ Branch 1 taken 469 times.
 139036 for (FwSizeType retry = 0; retry < MAX_CAS_RETRIES; ++retry) {
262 138567 FW_ASSERT(retry < MAX_CAS_RETRIES, static_cast<FwAssertArgType>(retry));
263
264 // acquire-release on slot->sequence ensures coherence, enqueuePos & dequeuePos are relaxed since strong memory
265 // ordering is not required
266
267 // Get current dequeue & enqueue positions
268 138567 FwSizeType pos = this->m_dequeuePos.load(std::memory_order_relaxed);
269
1/1
✓ Branch 3 taken 137725 times.
 137993 Slot* slot = &this->m_slots[this->getIndex(pos)];
270 137755 FwSizeType seq = slot->sequence.load(std::memory_order_acquire);
271
272 // Check if slot is ready for read (seq == pos + 1 means data available)
273 137423 FwSignedSizeType diff = static_cast<FwSignedSizeType>(seq) - static_cast<FwSignedSizeType>(pos + 1);
274
275
2/2
✓ Branch 0 taken 135777 times.
✓ Branch 1 taken 1646 times.
 137423 if (diff == 0) {
276 // Slot has data, try to claim it
277
2/2
✓ Branch 4 taken 128341 times.
✓ Branch 5 taken 6439 times.
 270557 if (this->m_dequeuePos.compare_exchange_weak(pos, pos + 1, std::memory_order_release,
278 std::memory_order_relaxed)) {
279 // Claimed the slot, check size and copy data
280 // Note: sequence.load(acquire) at 10 lines above already synchronizes with
281 // enqueue's sequence.store(release), ensuring size & buffer coherency
282 128341 FW_ASSERT(slot->buffer != nullptr, static_cast<FwAssertArgType>(pos));
283 128342 FW_ASSERT(slot->size > 0, static_cast<FwAssertArgType>(slot->size));
284 128342 FW_ASSERT(slot->size <= capacity, static_cast<FwAssertArgType>(slot->size),
285 static_cast<FwAssertArgType>(capacity));
286
287 128338 actualSize = slot->size;
288
2/4
✗ Branch 2 not taken.
✓ Branch 3 taken 128337 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 128337 times.
 128339 (void)std::memcpy(buffer, slot->buffer, actualSize);
289
290 // Mark slot as available for next cycle (pos + capacity)
291 128337 slot->sequence.store(pos + this->m_capacity, std::memory_order_release);
292
293 // Post semaphore to wake up blocked enqueue threads (if semaphore exists)
294 // ISR-SAFETY NOTE: Calling from ISR depends on platform semaphore implementation.
295 // See header comments for details.
296
2/2
✓ Branch 2 taken 128280 times.
✓ Branch 3 taken 46 times.
 128299 if (this->m_notFullSem != nullptr) {
297
1/1
✓ Branch 6 taken 127487 times.
 128280 Os::CountingSemaphoreInterface::Status status = this->m_notFullSem->post();
298 127487 FW_ASSERT(status == Os::CountingSemaphoreInterface::Status::OP_OK,
299 static_cast<FwAssertArgType>(status));
300 }
301
302 127460 return true;
303 }
304
1/2
✓ Branch 0 taken 2287 times.
✗ Branch 1 not taken.
 1646 } else if (diff < 0) {
305 // Queue is empty (no data written yet)
306 2287 return false;
307 }
308 // else: another consumer claimed this slot, retry
309 }
310
311 469 return false;
312 }
313
314 7 bool AtomicQueue::isFull() const {
315 // Queue is full if enqueue is exactly capacity ahead of dequeue
316 // Return false for uninitialized queue
317
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
 7 if (this->m_capacity == 0) {
318 ✗ return false;
319 }
320 7 return this->getSize() >= this->m_capacity;
321 }
322
323 12 bool AtomicQueue::isEmpty() const {
324 12 return this->getSize() == 0;
325 }
326
327 214291 FwSizeType AtomicQueue::getSize() const {
328 // Safe to call on uninitialized queue
329
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 214800 times.
 214291 if (this->m_capacity == 0) {
330 1 return 0;
331 }
332 // A nonzero capacity implies the queue was created with backing storage
333 214800 FW_ASSERT(this->m_slots != nullptr);
334
335 214775 FwSizeType enq = this->m_enqueuePos.load(std::memory_order_relaxed);
336 213640 FwSizeType deq = this->m_dequeuePos.load(std::memory_order_relaxed);
337 213533 FwSignedSizeType diff = static_cast<FwSignedSizeType>(enq) - static_cast<FwSignedSizeType>(deq);
338
339 // Two independent relaxed loads provide no cross-variable consistency guarantee.
340 // Restrict to [0, capacity] rather than asserting — diff can be transiently negative
341 // or > capacity on concurrent access even though no real program state has that.
342
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 213532 times.
 213533 if (diff < 0) {
343 1 return 0;
344 }
345
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 214610 times.
 213532 if (static_cast<FwSizeType>(diff) > this->m_capacity) {
346 ✗ return this->m_capacity;
347 }
348 214610 return static_cast<FwSizeType>(diff);
349 }
350
351 4 FwSizeType AtomicQueue::getCapacity() const {
352 // Safe to call on uninitialized queue - returns 0 if not created
353 4 return this->m_capacity;
354 }
355
356 269680 FwSizeType AtomicQueue::getBufferSize() const {
357 // Safe to call on uninitialized queue - returns 0 if not created
358 269680 return this->m_bufferSize;
359 }
360
361 } // namespace Types
362