GCC Code Coverage Report

Line	Branch	Exec	Source
1			// ======================================================================
2			// \title Os/Generic/LocklessPriorityQueue.cpp
3			// \brief lockless ISR-safe priority queue implementation for Os::Queue
4			// ======================================================================
5			#include "Os/Generic/LocklessPriorityQueue.hpp"
6			#include <atomic>
7			#include <cstring>
8			#include <limits>
9			#include "Fw/LanguageHelpers.hpp"
10			#include "Fw/Time/TimeInterval.hpp"
11			#include "Fw/Types/Assert.hpp"
12			#include "Fw/Types/ByteArray.hpp"
13			#include "Fw/Types/MemAllocator.hpp"
14			#include "Os/Task.hpp"
15			#include "config/MemoryAllocatorTypeEnumAc.hpp"
16
17			namespace Os {
18			namespace Generic {
19
20			namespace {
21
22			//! Extract the state portion of a packed state-tag word.
23		✗	constexpr LocklessStateTagType stateOf(LocklessStateTagType packed) {
24		✗	return packed & LocklessSlot::STATE_MASK;
25			}
26
27			//! Extract the tag portion of a packed state-tag word.
28		✗	constexpr LocklessStateTagType tagOf(LocklessStateTagType packed) {
29		✗	return packed >> LocklessSlot::STATE_BITS;
30			}
31
32			//! Pack a (state, tag) pair into a single word.
33		✗	constexpr LocklessStateTagType packStateTag(LocklessStateTagType state, LocklessStateTagType tag) {
34		✗	return (tag << LocklessSlot::STATE_BITS) | (state & LocklessSlot::STATE_MASK);
35			}
36
37			} // namespace
38
39		✗	bool LocklessPriorityQueue::isCandidatePreferred(FwQueuePriorityType candidatePriority,
40			U32 candidateSequence,
41			FwQueuePriorityType bestPriority,
42			U32 bestSequence) {
43		✗	bool preferred = false;
44		✗	if (candidatePriority > bestPriority) {
45		✗	preferred = true;
46		✗	} else if (candidatePriority == bestPriority) {
47			// Modular subtraction: if (candidate - best) interpreted unsigned has its top bit set,
48			// candidate is "older" (smaller in the wrap-aware ordering) than best.
49		✗	const U32 difference = candidateSequence - bestSequence;
50		✗	const U32 topBit = static_cast<U32>(1) << (std::numeric_limits<U32>::digits - 1);
51		✗	preferred = (difference & topBit) != 0;
52			}
53		✗	return preferred;
54			}
55
56		✗	LocklessSlot::LocklessSlot()
57		✗	: m_stateTag(packStateTag(LOCKLESS_SLOT_FREE, 0)), m_sequence(0), m_size(0), m_priority(0) {}
58
59		✗	LocklessPriorityQueueHandle::LocklessPriorityQueueHandle()
60			: QueueHandle(),
61		✗	m_slots(nullptr),
62		✗	m_slotsAllocation(nullptr),
63		✗	m_data(nullptr),
64		✗	m_depth(0),
65		✗	m_messageSize(0),
66		✗	m_sequence(0),
67		✗	m_count(0),
68		✗	m_available(0),
69		✗	m_highMark(0),
70		✗	m_id(0) {}
71
72		✗	LocklessPriorityQueue::~LocklessPriorityQueue() {
73			// Intentionally empty: cleanup is the responsibility of an explicit `teardown()` call.
74			// Freeing here would call the `Fw::MemAllocatorRegistry` singleton, whose destruction
75			// order relative to global queue objects is unspecified (matches
76			// `Os::Generic::PriorityQueue::~PriorityQueue()`).
77		✗	}
78
79		✗	QueueInterface::Status LocklessPriorityQueue::create(FwEnumStoreType id,
80			const Fw::ConstStringBase& name,
81			FwSizeType depth,
82			FwSizeType messageSize) {
83			static_cast<void>(name);
84
85			// Ensure that the queue has not already been created. A double create would leak memory and
86			// is a programming error.
87		✗	FW_ASSERT(this->m_handle.m_slots == nullptr);
88		✗	FW_ASSERT(this->m_handle.m_data == nullptr);
89
90			// The state-tag word relies on the underlying atomic being lock-free for ISR safety. The
91			// static_assert in the header covers platforms with a compile-time guarantee; this runtime
92			// check is the authoritative gate (`is_always_lock_free` is C++17 and this code targets
93			// C++14).
94		✗	std::atomic<LocklessStateTagType> probe(0);
95		✗	FW_ASSERT(probe.is_lock_free());
96		✗	std::atomic<FwQueuePriorityType> priorityProbe(0);
97		✗	FW_ASSERT(priorityProbe.is_lock_free());
98		✗	std::atomic<U32> counterProbe(0);
99		✗	FW_ASSERT(counterProbe.is_lock_free());
100		✗	FW_ASSERT(depth > 0);
101		✗	FW_ASSERT(messageSize > 0);
102
103			// Guard the multiplications used to compute allocation sizes against overflow.
104		✗	const FwSizeType maxSize = std::numeric_limits<FwSizeType>::max();
105		✗	FW_ASSERT(depth <= (maxSize / sizeof(LocklessSlot)));
106		✗	FW_ASSERT(depth <= (maxSize / messageSize));
107
108			// The modular comparison in isCandidatePreferred is exact only while queued equal-priority
109			// messages span less than half the U32 sequence domain (SDD section 7); depth bounds message
110			// count, not sequence spread.
111		✗	FW_ASSERT(depth < (std::numeric_limits<U32>::max() / 2));
112
113		✗	Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator(
114			Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE);
115
116		✗	LocklessSlot* slots = nullptr;
117		✗	void* slotsAllocation = nullptr;
118		✗	U8* data = nullptr;
119		✗	QueueInterface::Status status = QueueInterface::Status::OP_OK;
120
121			// Allocate the slot array. Request padding for manual alignment: allocators are not required
122			// to honor the alignment argument (e.g. Fw::MallocAllocator ignores it).
123		✗	FW_ASSERT(depth * sizeof(LocklessSlot) <= (maxSize - alignof(LocklessSlot)));
124		✗	FwSizeType slotBytesRequested = (depth * sizeof(LocklessSlot)) + alignof(LocklessSlot);
125		✗	FwSizeType slotBytesAllocated = slotBytesRequested;
126		✗	slotsAllocation = allocator.allocate(id, slotBytesAllocated, alignof(LocklessSlot));
127		✗	if (slotsAllocation == nullptr) {
128		✗	status = QueueInterface::Status::ALLOCATION_FAILED;
129		✗	} else if (slotBytesAllocated < slotBytesRequested) {
130		✗	allocator.deallocate(id, slotsAllocation);
131		✗	status = QueueInterface::Status::ALLOCATION_FAILED;
132			} else {
133		✗	const PlatformPointerCastType base = reinterpret_cast<PlatformPointerCastType>(slotsAllocation);
134		✗	const PlatformPointerCastType aligned =
135		✗	(base + (alignof(LocklessSlot) - 1)) & ~static_cast<PlatformPointerCastType>(alignof(LocklessSlot) - 1);
136		✗	const FwSizeType offset = static_cast<FwSizeType>(aligned - base);
137		✗	slots = Fw::arrayPlacementNew<LocklessSlot>(
138			Fw::ByteArray(static_cast<U8*>(slotsAllocation) + offset, slotBytesAllocated - offset), depth);
139			}
140
141			// Allocate the message-data region.
142		✗	if (status == QueueInterface::Status::OP_OK) {
143		✗	FwSizeType dataBytesRequested = depth * messageSize;
144		✗	FwSizeType dataBytesAllocated = dataBytesRequested;
145		✗	void* dataAllocation = allocator.allocate(id, dataBytesAllocated, alignof(U8));
146		✗	if (dataAllocation == nullptr) {
147		✗	Fw::arrayPlacementDestruct<LocklessSlot>(slots, depth);
148		✗	allocator.deallocate(id, slotsAllocation);
149		✗	status = QueueInterface::Status::ALLOCATION_FAILED;
150		✗	} else if (dataBytesAllocated < dataBytesRequested) {
151		✗	Fw::arrayPlacementDestruct<LocklessSlot>(slots, depth);
152		✗	allocator.deallocate(id, slotsAllocation);
153		✗	allocator.deallocate(id, dataAllocation);
154		✗	status = QueueInterface::Status::ALLOCATION_FAILED;
155			} else {
156		✗	data = static_cast<U8*>(dataAllocation);
157			}
158			}
159
160			// Publish the configured handle once both allocations succeeded.
161		✗	if (status == QueueInterface::Status::OP_OK) {
162		✗	this->m_handle.m_id = id;
163		✗	this->m_handle.m_messageSize = messageSize;
164		✗	this->m_handle.m_depth = depth;
165		✗	this->m_handle.m_slots = slots;
166		✗	this->m_handle.m_slotsAllocation = slotsAllocation;
167		✗	this->m_handle.m_data = data;
168		✗	this->m_handle.m_sequence.store(0, std::memory_order_relaxed);
169		✗	this->m_handle.m_count.store(0, std::memory_order_relaxed);
170		✗	this->m_handle.m_available.store(0, std::memory_order_relaxed);
171		✗	this->m_handle.m_highMark.store(0, std::memory_order_relaxed);
172			}
173		✗	return status;
174			}
175
176		✗	void LocklessPriorityQueue::teardown() {
177		✗	if (this->m_handle.m_slots != nullptr) {
178		✗	Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator(
179			Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE);
180		✗	Fw::arrayPlacementDestruct<LocklessSlot>(this->m_handle.m_slots, this->m_handle.m_depth);
181		✗	allocator.deallocate(this->m_handle.m_id, this->m_handle.m_slotsAllocation);
182		✗	if (this->m_handle.m_data != nullptr) {
183		✗	allocator.deallocate(this->m_handle.m_id, this->m_handle.m_data);
184			}
185		✗	this->m_handle.m_slots = nullptr;
186		✗	this->m_handle.m_slotsAllocation = nullptr;
187		✗	this->m_handle.m_data = nullptr;
188		✗	this->m_handle.m_depth = 0;
189		✗	this->m_handle.m_messageSize = 0;
190		✗	this->m_handle.m_count.store(0, std::memory_order_relaxed);
191		✗	this->m_handle.m_available.store(0, std::memory_order_relaxed);
192		✗	this->m_handle.m_highMark.store(0, std::memory_order_relaxed);
193		✗	this->m_handle.m_sequence.store(0, std::memory_order_relaxed);
194			}
195		✗	}
196
197		✗	QueueInterface::Status LocklessPriorityQueue::send(const U8* buffer,
198			FwSizeType size,
199			FwQueuePriorityType priority,
200			QueueInterface::BlockingType blockType) {
201			// Programming-error checks: queue must be created and inputs must be well-formed. These are
202			// preconditions, not untrusted-input checks.
203		✗	FW_ASSERT(this->m_handle.m_slots != nullptr);
204		✗	FW_ASSERT(this->m_handle.m_data != nullptr);
205		✗	FW_ASSERT(buffer != nullptr);
206
207			// Reject oversized messages without touching the queue.
208		✗	if (size > this->m_handle.m_messageSize) {
209		✗	return QueueInterface::Status::SIZE_MISMATCH;
210			}
211
212		✗	const FwSizeType depth = this->m_handle.m_depth;
213		✗	const bool blocking = (blockType == QueueInterface::BlockingType::BLOCKING);
214
215			// Bounded for non-blocking, unbounded for blocking (explicit user contract).
216		✗	for (FwSizeType pass = 0; blocking || (pass < MAX_RETRY_PASSES); pass++) {
217		✗	for (FwSizeType i = 0; i < depth; i++) {
218		✗	LocklessSlot& slot = this->m_handle.m_slots[i];
219		✗	LocklessStateTagType packed = slot.m_stateTag.load(std::memory_order_acquire);
220		✗	if (stateOf(packed) != LOCKLESS_SLOT_FREE) {
221		✗	continue;
222			}
223		✗	const LocklessStateTagType desired = packStateTag(LOCKLESS_SLOT_WRITING, tagOf(packed) + 1);
224		✗	if (slot.m_stateTag.compare_exchange_strong(packed, desired, std::memory_order_acq_rel,
225			std::memory_order_relaxed)) {
226		✗	if (size > 0) {
227		✗	const FwSizeType offset = i * this->m_handle.m_messageSize;
228		✗	static_cast<void>(::memcpy(this->m_handle.m_data + offset, buffer, static_cast<size_t>(size)));
229			}
230		✗	slot.m_size = size;
231		✗	slot.m_priority.store(priority, std::memory_order_relaxed);
232		✗	slot.m_sequence.store(this->m_handle.m_sequence.fetch_add(1, std::memory_order_relaxed),
233			std::memory_order_relaxed);
234
235			// Increment the occupancy count *before* publishing READY. This guarantees a
236			// consumer's decrement (which can only follow a READY observation) never precedes
237			// this increment, so m_count cannot transiently underflow.
238		✗	const U32 nextCount = this->m_handle.m_count.fetch_add(1, std::memory_order_acq_rel) + 1;
239
240		✗	slot.m_stateTag.store(packStateTag(LOCKLESS_SLOT_READY, tagOf(desired) + 1), std::memory_order_release);
241
242			// Increment the receivable count only after READY is published, so a nonzero
243			// getMessagesAvailable() implies at least one message has been made receivable.
244		✗	static_cast<void>(this->m_handle.m_available.fetch_add(1, std::memory_order_acq_rel));
245
246			// Raise the high-water mark after publication so the message is never invisible
247			// while the producer runs this loop. Each strong-CAS failure strictly raises
248			// prevMark (mark only increases, capped at depth), so at most depth iterations run.
249		✗	U32 prevMark = this->m_handle.m_highMark.load(std::memory_order_relaxed);
250		✗	for (FwSizeType markPass = 0; (markPass < depth) && (nextCount > prevMark); markPass++) {
251		✗	if (this->m_handle.m_highMark.compare_exchange_strong(
252			prevMark, nextCount, std::memory_order_relaxed, std::memory_order_relaxed)) {
253		✗	break;
254			}
255			}
256		✗	return QueueInterface::Status::OP_OK;
257			}
258			}
259			// No free slot found this pass. Blocking callers back off; non-blocking callers retry.
260		✗	if (blocking) {
261		✗	static_cast<void>(Os::Task::delay(Fw::TimeInterval(0, LOCKLESS_QUEUE_BLOCKING_BACKOFF_US)));
262			}
263			}
264		✗	return QueueInterface::Status::FULL;
265			}
266
267		✗	QueueInterface::Status LocklessPriorityQueue::receive(U8* destination,
268			FwSizeType capacity,
269			QueueInterface::BlockingType blockType,
270			FwSizeType& actualSize,
271			FwQueuePriorityType& priority) {
272			// Programming-error checks. These mirror the assertions in send().
273		✗	FW_ASSERT(this->m_handle.m_slots != nullptr);
274		✗	FW_ASSERT(this->m_handle.m_data != nullptr);
275		✗	FW_ASSERT(destination != nullptr);
276
277		✗	const FwSizeType depth = this->m_handle.m_depth;
278		✗	const bool blocking = (blockType == QueueInterface::BlockingType::BLOCKING);
279
280			// Bounded for non-blocking, unbounded for blocking (explicit user contract).
281		✗	for (FwSizeType pass = 0; blocking || (pass < MAX_RETRY_PASSES); pass++) {
282			// Fast path: skip the O(depth) scan while no message is receivable.
283		✗	if (this->m_handle.m_available.load(std::memory_order_acquire) == 0) {
284		✗	if (blocking) {
285		✗	static_cast<void>(Os::Task::delay(Fw::TimeInterval(0, LOCKLESS_QUEUE_BLOCKING_BACKOFF_US)));
286			}
287		✗	continue;
288			}
289		✗	FwSizeType bestIndex = depth;
290		✗	FwQueuePriorityType bestPriority = FwQueuePriorityType();
291		✗	U32 bestSequence = 0;
292		✗	LocklessStateTagType bestPacked = 0;
293
294		✗	for (FwSizeType i = 0; i < depth; i++) {
295		✗	LocklessSlot& slot = this->m_handle.m_slots[i];
296		✗	LocklessStateTagType packed = slot.m_stateTag.load(std::memory_order_acquire);
297		✗	if (stateOf(packed) != LOCKLESS_SLOT_READY) {
298		✗	continue;
299			}
300		✗	const FwQueuePriorityType candidatePriority = slot.m_priority.load(std::memory_order_relaxed);
301		✗	const U32 candidateSequence = slot.m_sequence.load(std::memory_order_relaxed);
302			// Recheck: if the state-tag changed, the relaxed priority/sequence reads above may
303			// belong to a recycled slot; discard the candidate and keep scanning.
304		✗	const LocklessStateTagType packedRecheck = slot.m_stateTag.load(std::memory_order_acquire);
305		✗	if (packed != packedRecheck) {
306		✗	continue;
307			}
308		✗	if ((bestIndex == depth) ||
309		✗	isCandidatePreferred(candidatePriority, candidateSequence, bestPriority, bestSequence)) {
310		✗	bestIndex = i;
311		✗	bestPriority = candidatePriority;
312		✗	bestSequence = candidateSequence;
313		✗	bestPacked = packed;
314			}
315			}
316
317		✗	if (bestIndex == depth) {
318		✗	if (blocking) {
319		✗	static_cast<void>(Os::Task::delay(Fw::TimeInterval(0, LOCKLESS_QUEUE_BLOCKING_BACKOFF_US)));
320			}
321		✗	continue;
322			}
323
324		✗	const LocklessStateTagType desired = packStateTag(LOCKLESS_SLOT_READING, tagOf(bestPacked) + 1);
325		✗	if (this->m_handle.m_slots[bestIndex].m_stateTag.compare_exchange_strong(
326			bestPacked, desired, std::memory_order_acq_rel, std::memory_order_relaxed)) {
327			// Decrement the receivable count at the successful READY->READING claim: this
328			// message can no longer complete another receive.
329		✗	static_cast<void>(this->m_handle.m_available.fetch_sub(1, std::memory_order_acq_rel));
330		✗	LocklessSlot& slot = this->m_handle.m_slots[bestIndex];
331		✗	const FwSizeType storedSize = slot.m_size;
332		✗	FW_ASSERT(storedSize <= capacity);
333		✗	if (storedSize > 0) {
334		✗	const FwSizeType offset = bestIndex * this->m_handle.m_messageSize;
335			static_cast<void>(
336		✗	::memcpy(destination, this->m_handle.m_data + offset, static_cast<size_t>(storedSize)));
337			}
338		✗	actualSize = storedSize;
339		✗	priority = slot.m_priority.load(std::memory_order_relaxed);
340			// Decrement the occupancy count *before* releasing the slot to FREE. This keeps
341			// m_count (and therefore the high-water mark) at or below the queue depth: a producer
342			// can only re-claim and re-count this slot after observing FREE, which follows the
343			// decrement.
344		✗	static_cast<void>(this->m_handle.m_count.fetch_sub(1, std::memory_order_acq_rel));
345		✗	slot.m_stateTag.store(packStateTag(LOCKLESS_SLOT_FREE, tagOf(desired) + 1), std::memory_order_release);
346		✗	return QueueInterface::Status::OP_OK;
347			}
348			// CAS failed — another consumer claimed this slot; yield and rescan.
349		✗	if (blocking) {
350		✗	static_cast<void>(Os::Task::delay(Fw::TimeInterval(0, 0)));
351			}
352			}
353		✗	return QueueInterface::Status::EMPTY;
354			}
355
356		✗	FwSizeType LocklessPriorityQueue::getMessagesAvailable() const {
357		✗	return static_cast<FwSizeType>(this->m_handle.m_available.load(std::memory_order_acquire));
358			}
359
360		✗	FwSizeType LocklessPriorityQueue::getMessageHighWaterMark() const {
361		✗	return static_cast<FwSizeType>(this->m_handle.m_highMark.load(std::memory_order_acquire));
362			}
363
364		✗	QueueHandle* LocklessPriorityQueue::getHandle() {
365		✗	return &this->m_handle;
366			}
367
368			} // namespace Generic
369			} // namespace Os
370