GCC Code Coverage Report

Line	Branch	Exec	Source
1			// ======================================================================
2			// \title Os/Generic/PriorityMemQueue.cpp
3			// \author B. Duckett
4			// \brief cpp file for AtomicQueue-based priority queue implementation for Os::Queue
5			//
6			// \copyright
7			// Copyright 2026, by the California Institute of Technology.
8			// ALL RIGHTS RESERVED. United States Government Sponsorship
9			// acknowledged.
10			// ======================================================================
11			#include "PriorityMemQueue.hpp"
12			#include <algorithm>
13			#include <cstdio>
14			#include <cstring>
15			#include <limits>
16			#include "Fw/LanguageHelpers.hpp"
17			#include "Fw/Types/Assert.hpp"
18			#include "Fw/Types/MemAllocator.hpp"
19			#include "config/MemoryAllocatorTypeEnumAc.hpp"
20
21			namespace Os {
22			namespace Generic {
23
24			// Arbitrary (but small) limit to prevent infinite loops
25			// Don't expect a message queue depth to ever exceed three digits
26			constexpr U32 LOOP_GUARD_LIMIT = 2000;
27
28			// ======================================================================
29			// Static Configuration State (Global)
30			// ======================================================================
31			// THREAD-SAFETY: Static configuration is designed for single-threaded
32			// initialization at system startup (before any queues are created).
33			// configure() asserts if called multiple times. Individual queue
34			// creation uses atomic s_configsUsed[] to prevent race conditions when
35			// multiple components instantiate queues concurrently.
36			//
37			// LIFECYCLE:
38			// 1. System startup: Call configure() once (single-threaded)
39			// 2. Component init: create() claims config atomically (multi-threaded safe)
40			// 3. Runtime: No modification to static state
41			// 4. Teardown: Each queue marks config unused atomically
42			// 5. Test only: resetConfig() deallocates (single-threaded after all queues destroyed)
43			// ======================================================================
44			PriorityMemQueue::QueueConfig* PriorityMemQueue::s_configs = nullptr;
45			FwSizeType PriorityMemQueue::s_numConfigs = 0;
46			bool PriorityMemQueue::s_requirePrioritySizing = false;
47			std::atomic<bool>* PriorityMemQueue::s_configsUsed = nullptr;
48			bool PriorityMemQueue::s_configured = false;
49			FwEnumStoreType PriorityMemQueue::s_allocatorId = 0;
50
51			//! \brief Get the bit mask for a priority
52			//! \param priority: priority to get mask for
53			//! \return bit mask with the priority bit set
54		✗	static constexpr U32 priorityBitMask(FwQueuePriorityType priority) {
55		✗	return 1U << priority;
56			}
57
58		✗	void PriorityMemQueueHandle::init() {
59		✗	FW_ASSERT(this->m_numActivePriorities <= Os::Generic::Queue::MAX_PRIORITIES,
60			static_cast<FwAssertArgType>(this->m_numActivePriorities));
61			// NOTE: Do NOT reset m_priorityMap here - it's already populated by create()
62
63			// If arrays were allocated, teardown AtomicQueues
64		✗	if (this->m_atomicQueues != nullptr) {
65		✗	for (FwSizeType i = 0; i < this->m_numActivePriorities; ++i) {
66		✗	this->m_atomicQueues[i].teardown();
67			}
68			}
69
70			// Initialize high water marks to zero if array is allocated
71		✗	if (this->m_highWaterMarks != nullptr) {
72		✗	for (FwSizeType i = 0; i < this->m_numActivePriorities; ++i) {
73		✗	this->m_highWaterMarks[i].store(0, std::memory_order_relaxed);
74			}
75			}
76
77			// Initialize the not-empty semaphore with count 0 (queue starts empty)
78		✗	if (this->m_notEmptySem != nullptr) {
79		✗	this->m_notEmptySem->~CountingSemaphore();
80		✗	this->m_notEmptySem = nullptr;
81			}
82
83			// Initialize atomic variables: no priority is enabled until its queue is created.
84			// A stale bit for an unconfigured priority would assert in the receive scan.
85		✗	this->m_priorityMask.store(0, std::memory_order_relaxed);
86		✗	}
87
88		✗	bool PriorityMemQueueHandle::allocateArrays(Fw::MemAllocator& allocator, FwEnumStoreType allocatorId) {
89		✗	this->m_allocatorId = allocatorId;
90
91			// Scan m_priorityMap to count configured priorities and find max priority
92		✗	this->m_numActivePriorities = 0;
93		✗	this->m_maxPriority = 0;
94
95		✗	for (FwSizeType p = 0; p < Os::Generic::Queue::MAX_PRIORITIES; ++p) {
96		✗	if (this->m_priorityMap[p] >= 0) {
97		✗	this->m_numActivePriorities++;
98		✗	if (static_cast<FwQueuePriorityType>(p) > this->m_maxPriority) {
99		✗	this->m_maxPriority = static_cast<FwQueuePriorityType>(p);
100			}
101			}
102			}
103
104		✗	FW_ASSERT(this->m_numActivePriorities > 0, allocatorId);
105
106			// Allocate memory for atomicQueues array (sized to actual configured priorities)
107		✗	FwSizeType atomicQueuesSize = sizeof(Types::AtomicQueue) * this->m_numActivePriorities;
108		✗	void* atomicQueuesMem = allocator.checkedAllocate(allocatorId, atomicQueuesSize, alignof(Types::AtomicQueue));
109		✗	if (atomicQueuesMem == nullptr) {
110		✗	this->deallocateArrays(allocator, allocatorId);
111		✗	return false;
112			}
113			// Use placement new to construct array
114		✗	this->m_atomicQueues = static_cast<Types::AtomicQueue*>(atomicQueuesMem);
115		✗	for (FwSizeType i = 0; i < this->m_numActivePriorities; ++i) {
116		✗	new (&this->m_atomicQueues[i]) Types::AtomicQueue();
117			}
118
119			// Allocate memory for highWaterMarks array (sized to actual configured priorities)
120		✗	FwSizeType hwmSize = sizeof(std::atomic<U32>) * this->m_numActivePriorities;
121		✗	void* hwmMem = allocator.checkedAllocate(allocatorId, hwmSize, alignof(std::atomic<U32>));
122		✗	if (hwmMem == nullptr) {
123		✗	this->deallocateArrays(allocator, allocatorId);
124		✗	return false;
125			}
126			// Cast required: MemAllocator returns void*, reinterpret_cast converts to typed pointer for placement new
127		✗	this->m_highWaterMarks = reinterpret_cast<std::atomic<U32>*>(hwmMem);
128		✗	for (FwSizeType i = 0; i < this->m_numActivePriorities; ++i) {
129			// Placement new constructs std::atomic in pre-allocated memory (standard F' allocator pattern)
130		✗	new (&this->m_highWaterMarks[i]) std::atomic<U32>(0);
131			}
132
133		✗	return true;
134			}
135
136		✗	void PriorityMemQueueHandle::deallocateArrays(Fw::MemAllocator& allocator, FwEnumStoreType allocatorId) {
137		✗	FW_ASSERT(this->m_numActivePriorities <= Os::Generic::Queue::MAX_PRIORITIES,
138			static_cast<FwAssertArgType>(this->m_numActivePriorities));
139			// Deallocate arrays in reverse order
140		✗	if (this->m_highWaterMarks != nullptr) {
141			// std::atomic<U32> is trivially destructible — no explicit destructor needed
142		✗	allocator.deallocate(allocatorId, this->m_highWaterMarks);
143		✗	this->m_highWaterMarks = nullptr;
144			}
145		✗	if (this->m_atomicQueues != nullptr) {
146			// Explicitly destroy AtomicQueues before deallocation
147		✗	for (FwSizeType i = 0; i < this->m_numActivePriorities; ++i) {
148		✗	this->m_atomicQueues[i].~AtomicQueue();
149			}
150		✗	allocator.deallocate(allocatorId, this->m_atomicQueues);
151		✗	this->m_atomicQueues = nullptr;
152			}
153
154			// Reset priority map
155		✗	for (FwSizeType i = 0; i < Os::Generic::Queue::MAX_PRIORITIES; ++i) {
156		✗	this->m_priorityMap[i] = -1;
157			}
158
159		✗	this->m_maxPriority = 0;
160		✗	this->m_numActivePriorities = 0;
161		✗	this->m_allocatorId = 0;
162		✗	}
163
164		✗	void PriorityMemQueueHandle::enablePriority(FwQueuePriorityType priority) {
165		✗	FW_ASSERT(priority < Os::Generic::Queue::MAX_PRIORITIES, priority, this->m_id);
166			// Enabling a priority is only allowed if it's in use
167		✗	FW_ASSERT(this->m_atomicQueues != nullptr, this->m_id, priority);
168
169			// MEMORY ORDERING: seq_cst for control path operations ensures total ordering
170			// Atomic update of priority mask using fetch_or with seq_cst (control path)
171		✗	(void)this->m_priorityMask.fetch_or(priorityBitMask(priority), std::memory_order_seq_cst);
172		✗	}
173
174		✗	void PriorityMemQueueHandle::disablePriority(FwQueuePriorityType priority) {
175		✗	FW_ASSERT(priority < Os::Generic::Queue::MAX_PRIORITIES, this->m_id);
176
177			// MEMORY ORDERING: seq_cst for control path operations ensures total ordering
178			// Atomic update of priority mask using fetch_and with seq_cst (control path)
179		✗	(void)this->m_priorityMask.fetch_and(~priorityBitMask(priority), std::memory_order_seq_cst);
180		✗	}
181
182		✗	PriorityMemQueue::PriorityMemQueue() {
183			// Initialize handle to safe defaults
184		✗	this->m_handle.init();
185		✗	}
186
187			//! \brief Find most significant bit set (IPC-style priority finding)
188			//! \param value: bit mask to search
189			//! \return bit position of MSB, or -1 if no bits set
190		✗	static inline I32 findMSB(U32 value) {
191		✗	if (value == 0) {
192		✗	return -1;
193			}
194			// Use compiler builtin for CLZ (count leading zeros) if available
195			#if defined(__GNUC__) || defined(__clang__)
196		✗	I32 msb = 31 - __builtin_clz(value);
197		✗	return msb;
198			#else
199			// Fallback: software implementation with explicit bound (32 bits maximum)
200			I32 msb = 31;
201			U32 mask = 0x80000000;
202			for (FwSizeType bit = 0; bit < 32; ++bit) {
203			if (mask & value) {
204			return msb;
205			}
206			--msb;
207			mask >>= 1;
208			}
209			return -1;
210			#endif
211			}
212
213		✗	FwQueuePriorityType PriorityMemQueue::findHighestPriority(U32 priorities) {
214			// The priority bit mask is a U32, so priorities must fit in 32 bits
215			static_assert(static_cast<FwSizeType>(Os::Generic::Queue::MAX_PRIORITIES) <= 32,
216			"MAX_PRIORITIES must fit in a U32 priority bit mask");
217			// MEMORY ORDERING: Use acquire to synchronize with priority enable/disable operations
218			// Get enabled priorities
219		✗	if (priorities == 0) {
220		✗	priorities = this->m_handle.m_priorityMask.load(std::memory_order_acquire);
221			}
222
223		✗	if (priorities == 0) {
224		✗	return Os::Generic::Queue::MAX_PRIORITIES;
225			}
226
227			// Use IPC-style MSB finding for performance
228		✗	I32 msb = findMSB(priorities);
229		✗	if (msb < 0 || msb >= static_cast<I32>(Os::Generic::Queue::MAX_PRIORITIES)) {
230		✗	return Os::Generic::Queue::MAX_PRIORITIES;
231			}
232		✗	return static_cast<FwQueuePriorityType>(msb);
233			}
234
235		✗	bool PriorityMemQueue::isPriorityEnabled(FwQueuePriorityType priority) {
236		✗	FW_ASSERT(priority < Os::Generic::Queue::MAX_PRIORITIES, this->m_handle.m_id);
237			// MEMORY ORDERING: Use acquire to synchronize with enable/disable operations
238		✗	return (this->m_handle.m_priorityMask.load(std::memory_order_acquire) & priorityBitMask(priority)) != 0;
239			}
240
241		✗	void PriorityMemQueue::setPriorityEnabled(FwQueuePriorityType priority, bool enabled) {
242			// Delegate to handle methods (SSOT)
243		✗	if (enabled) {
244		✗	this->m_handle.enablePriority(priority);
245			} else {
246		✗	this->m_handle.disablePriority(priority);
247			}
248		✗	}
249
250		✗	Fw::MemAllocator& PriorityMemQueue::getAllocator() {
251		✗	return Fw::MemAllocatorRegistry::getInstance().getAnAllocator(
252		✗	Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE);
253			}
254
255			//! \brief Validate queue configuration structures
256			//! \param queueConfigs: array of queue configurations to validate
257			//! \param numQueueConfigs: number of configurations
258		✗	static void validateQueueConfigs(PriorityMemQueue::QueueConfig* queueConfigs, FwSizeType numQueueConfigs) {
259		✗	for (FwSizeType i = 0; i < numQueueConfigs; ++i) {
260		✗	PriorityMemQueue::QueueConfig* currentConfig = &queueConfigs[i];
261
262			// Assert if numPriorities is 0 or exceeds maximum
263		✗	FW_ASSERT(
264			currentConfig->numPriorities > 0 && currentConfig->numPriorities <= Os::Generic::Queue::MAX_PRIORITIES,
265			static_cast<FwAssertArgType>(i), currentConfig->instanceId,
266			static_cast<FwAssertArgType>(currentConfig->numPriorities));
267
268			// Check for duplicate instance IDs
269		✗	for (FwSizeType j = i + 1; j < numQueueConfigs; ++j) {
270		✗	PriorityMemQueue::QueueConfig* otherConfig = &queueConfigs[j];
271		✗	FW_ASSERT(currentConfig->instanceId != otherConfig->instanceId, currentConfig->instanceId,
272			static_cast<FwAssertArgType>(i), static_cast<FwAssertArgType>(j));
273			}
274
275			// Check priority configurations
276		✗	PriorityMemQueue::QueuePriorityConfig* priorityConfigs = currentConfig->priorityConfigs;
277		✗	FW_ASSERT(priorityConfigs != nullptr, static_cast<FwAssertArgType>(i), currentConfig->instanceId,
278			static_cast<FwAssertArgType>(currentConfig->numPriorities));
279		✗	for (FwSizeType p = 0; p < currentConfig->numPriorities; ++p) {
280		✗	PriorityMemQueue::QueuePriorityConfig* pConfig = &priorityConfigs[p];
281
282			// Assert if maxMsgSize or numMsgs is 0
283		✗	FW_ASSERT(pConfig->maxMsgSize > 0, static_cast<FwAssertArgType>(i), static_cast<FwAssertArgType>(p),
284			pConfig->priority);
285		✗	FW_ASSERT(pConfig->numMsgs > 0, static_cast<FwAssertArgType>(i), static_cast<FwAssertArgType>(p),
286			pConfig->priority);
287		✗	FW_ASSERT(pConfig->priority >= 0 && pConfig->priority < Os::Generic::Queue::MAX_PRIORITIES,
288			static_cast<FwAssertArgType>(i), static_cast<FwAssertArgType>(p), pConfig->priority);
289
290			// Check for duplicate priority values
291		✗	for (FwSizeType q = p + 1; q < currentConfig->numPriorities; ++q) {
292		✗	PriorityMemQueue::QueuePriorityConfig* qConfig = &priorityConfigs[q];
293		✗	FW_ASSERT(pConfig->priority != qConfig->priority, static_cast<FwAssertArgType>(i),
294			currentConfig->instanceId, pConfig->priority);
295			}
296			}
297			}
298		✗	}
299
300		✗	void PriorityMemQueue::configure(QueueConfig* queueConfigs,
301			FwSizeType numQueueConfigs,
302			bool required,
303			FwEnumStoreType allocatorId) {
304			// Accept NULL pointer if and only if numQueueConfigs is 0
305		✗	FW_ASSERT((queueConfigs != nullptr) || (numQueueConfigs == 0), 0);
306
307			// Assert if already configured - that's not a supported use case
308		✗	FW_ASSERT(!s_configured, 0);
309
310		✗	s_configured = true;
311
312			// Assert if required is true but num priorities is 0
313		✗	FW_ASSERT(!(required && numQueueConfigs == 0), required, static_cast<FwAssertArgType>(numQueueConfigs));
314
315			// Validate all configurations
316		✗	if (queueConfigs != nullptr) {
317		✗	validateQueueConfigs(queueConfigs, numQueueConfigs);
318			}
319			// Get the memory allocator configured for priority queues
320		✗	Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator(
321			Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE);
322			// Allocate memory for tracking used configurations
323		✗	if (numQueueConfigs > 0) {
324		✗	FwSizeType expSize = numQueueConfigs * sizeof(std::atomic<bool>);
325		✗	s_configsUsed = static_cast<std::atomic<bool>*>(
326		✗	allocator.checkedAllocate(allocatorId, expSize, alignof(std::atomic<bool>)));
327		✗	FW_ASSERT(s_configsUsed != nullptr);
328			// Initialize all entries to false using placement new
329		✗	for (FwSizeType i = 0; i < numQueueConfigs; ++i) {
330		✗	new (&s_configsUsed[i]) std::atomic<bool>(false);
331			}
332			}
333
334			// Deep copy into a single contiguous block: QueueConfig[] followed by all QueuePriorityConfig[] sub-arrays.
335			static_assert(sizeof(QueueConfig) % alignof(QueuePriorityConfig) == 0,
336			"QueueConfig array must be naturally aligned with QueuePriorityConfig");
337		✗	if (numQueueConfigs > 0) {
338		✗	FwSizeType totalSize = numQueueConfigs * sizeof(QueueConfig);
339		✗	for (FwSizeType i = 0; i < numQueueConfigs; ++i) {
340		✗	totalSize += queueConfigs[i].numPriorities * sizeof(QueuePriorityConfig);
341			}
342		✗	void* configsMem = allocator.checkedAllocate(allocatorId, totalSize, alignof(QueueConfig));
343		✗	FW_ASSERT(configsMem != nullptr);
344		✗	s_configs = static_cast<QueueConfig*>(configsMem);
345		✗	QueuePriorityConfig* priorityBase = reinterpret_cast<QueuePriorityConfig*>(s_configs + numQueueConfigs);
346		✗	for (FwSizeType i = 0; i < numQueueConfigs; ++i) {
347		✗	s_configs[i] = queueConfigs[i];
348		✗	FwSizeType priorityConfigsSize = queueConfigs[i].numPriorities * sizeof(QueuePriorityConfig);
349		✗	(void)memcpy(priorityBase, queueConfigs[i].priorityConfigs, priorityConfigsSize);
350		✗	s_configs[i].priorityConfigs = priorityBase;
351		✗	priorityBase += queueConfigs[i].numPriorities;
352			}
353			}
354		✗	s_numConfigs = numQueueConfigs;
355		✗	s_requirePrioritySizing = required;
356		✗	s_allocatorId = allocatorId;
357		✗	}
358
359		✗	void PriorityMemQueue::resetConfig() {
360			// Configs are allocated if and only if a nonzero count was configured
361		✗	FW_ASSERT((s_configs != nullptr) || (s_numConfigs == 0), static_cast<FwAssertArgType>(s_numConfigs));
362			// Only call this in test environments after all queues are destroyed
363		✗	if (s_configsUsed != nullptr || s_configs != nullptr) {
364			// Get allocator (same as used in config())
365		✗	Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator(
366			Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE);
367		✗	FwEnumStoreType allocatorId = s_allocatorId;
368
369			// Deallocate the tracking array
370		✗	if (s_configsUsed != nullptr) {
371		✗	allocator.deallocate(allocatorId, s_configsUsed);
372		✗	s_configsUsed = nullptr;
373			}
374
375			// Deallocate the single contiguous config block (QueueConfig[] + all QueuePriorityConfig[] sub-arrays)
376		✗	if (s_configs != nullptr) {
377		✗	allocator.deallocate(allocatorId, s_configs);
378			}
379			}
380
381			// Reset all static state
382		✗	s_configs = nullptr;
383		✗	s_numConfigs = 0;
384		✗	s_requirePrioritySizing = false;
385		✗	s_configured = false;
386		✗	}
387
388		✗	PriorityMemQueue::~PriorityMemQueue() {
389		✗	this->teardownInternal();
390		✗	}
391
392		✗	QueueInterface::Status PriorityMemQueue::create(FwEnumStoreType id,
393			const Fw::ConstStringBase& name,
394			FwSizeType depth,
395			FwSizeType messageSize) {
396		✗	FW_ASSERT(depth > 0, id);
397		✗	FW_ASSERT(messageSize > 0, id);
398
399			// Initialize the handle ID
400		✗	this->m_handle.m_id = id;
401
402			// Get the memory allocator for queue operations
403		✗	Fw::MemAllocator& allocator = this->getAllocator();
404
405			// Find a matching configuration if one exists
406		✗	QueueConfig* queueConfig = findMatchingConfig(id);
407
408			// Build priority map from configuration
409		✗	if (queueConfig != nullptr) {
410			// Map each configured priority to array index
411		✗	for (FwSizeType i = 0; i < queueConfig->numPriorities; ++i) {
412		✗	FwQueuePriorityType p = queueConfig->priorityConfigs[i].priority;
413		✗	FW_ASSERT(p < Os::Generic::Queue::MAX_PRIORITIES, id, static_cast<FwAssertArgType>(p),
414			Os::Generic::Queue::MAX_PRIORITIES);
415		✗	this->m_handle.m_priorityMap[p] = static_cast<I8>(i);
416			}
417			} else {
418			// Default: single priority at DEFAULT_PRIORITY
419		✗	this->m_handle.m_priorityMap[Os::Generic::Queue::DEFAULT_PRIORITY] = 0;
420			}
421
422			// Allocate arrays for priority data (reads from m_priorityMap)
423		✗	if (!this->m_handle.allocateArrays(allocator, id)) {
424		✗	return Os::QueueInterface::Status::ALLOCATION_FAILED;
425			}
426
427			// Initialize the handle with allocated arrays
428		✗	this->m_handle.init();
429
430			// Allocate and create the not-empty semaphore (initial count 0)
431		✗	FwSizeType semSize = sizeof(Os::CountingSemaphore);
432		✗	void* semMem = allocator.checkedAllocate(id, semSize, alignof(Os::CountingSemaphore));
433		✗	if (semMem == nullptr) {
434		✗	this->m_handle.deallocateArrays(allocator, id);
435		✗	return Os::QueueInterface::Status::ALLOCATION_FAILED;
436			}
437		✗	this->m_handle.m_notEmptySem = new (semMem) Os::CountingSemaphore(static_cast<U32>(0));
438
439			// Create the priority queues based on configuration
440		✗	if (queueConfig != nullptr) {
441			// Use the found configuration to create multiple priority queues
442		✗	return createConfiguredQueues(queueConfig, allocator, id);
443			} else {
444			// No configuration found, create a single default priority queue
445		✗	return createDefaultQueue(depth, messageSize, allocator, id);
446			}
447			}
448
449			// Helper method to find a matching configuration for the given ID
450		✗	PriorityMemQueue::QueueConfig* PriorityMemQueue::findMatchingConfig(FwEnumStoreType id) {
451		✗	if (s_configs != nullptr && s_configsUsed != nullptr) {
452		✗	for (FwSizeType i = 0; i < s_numConfigs; ++i) {
453		✗	if (s_configs[i].instanceId == id) {
454			// Atomic check-and-set to claim configuration
455		✗	bool expected = false;
456		✗	if (s_configsUsed[i].compare_exchange_strong(expected, true, std::memory_order_acq_rel)) {
457		✗	return &s_configs[i];
458			} else {
459			// Configuration already in use, assert failure
460		✗	FW_ASSERT(false, id, s_configs[i].instanceId);
461			}
462			}
463			}
464			}
465		✗	return nullptr;
466			}
467
468			// Helper method to create queues based on configuration
469		✗	QueueInterface::Status PriorityMemQueue::createConfiguredQueues(QueueConfig* queueConfig,
470			Fw::MemAllocator& allocator,
471			FwEnumStoreType allocatorId) {
472		✗	FW_ASSERT(queueConfig != nullptr, this->m_handle.m_id);
473		✗	FW_ASSERT(queueConfig->priorityConfigs != nullptr, this->m_handle.m_id,
474			static_cast<FwAssertArgType>(queueConfig->numPriorities));
475
476		✗	for (FwSizeType i = 0; i < queueConfig->numPriorities; ++i) {
477		✗	const QueuePriorityConfig& priorityConfig = queueConfig->priorityConfigs[i];
478		✗	FwQueuePriorityType priority = priorityConfig.priority;
479
480			// Create and initialize the priority queue
481		✗	QueueInterface::Status status = this->createPriorityQueue(priority, priorityConfig.maxMsgSize,
482		✗	priorityConfig.numMsgs, allocator, allocatorId);
483
484		✗	if (status != Os::QueueInterface::Status::OP_OK) {
485		✗	return status;
486			}
487
488			// Enable this priority
489		✗	this->setPriorityEnabled(priority, true);
490			}
491
492		✗	return Os::QueueInterface::Status::OP_OK;
493			}
494
495			// Helper method to create a single default priority queue
496		✗	QueueInterface::Status PriorityMemQueue::createDefaultQueue(FwSizeType depth,
497			FwSizeType messageSize,
498			Fw::MemAllocator& allocator,
499			FwEnumStoreType allocatorId) {
500			// Create and initialize the default priority queue
501			QueueInterface::Status status =
502		✗	this->createPriorityQueue(Os::Generic::Queue::DEFAULT_PRIORITY, messageSize, depth, allocator, allocatorId);
503		✗	if (status == Os::QueueInterface::Status::OP_OK) {
504		✗	this->setPriorityEnabled(Os::Generic::Queue::DEFAULT_PRIORITY, true);
505			}
506		✗	return status;
507			}
508
509			// Helper method to create a single priority queue using AtomicQueue
510		✗	QueueInterface::Status PriorityMemQueue::createPriorityQueue(FwQueuePriorityType priority,
511			FwSizeType maxMsgSize,
512			FwSizeType numMsgs,
513			Fw::MemAllocator& allocator,
514			FwEnumStoreType allocatorId) {
515		✗	FW_ASSERT(this->m_handle.m_atomicQueues != nullptr, this->m_handle.m_id, priority);
516		✗	FW_ASSERT(priority < Os::Generic::Queue::MAX_PRIORITIES, this->m_handle.m_id, priority);
517		✗	FW_ASSERT(priority <= this->m_handle.m_maxPriority, this->m_handle.m_id, priority, this->m_handle.m_maxPriority);
518
519			// Get array index for this priority
520		✗	I8 index = this->m_handle.getPriorityIndex(priority);
521		✗	FW_ASSERT(index >= 0, this->m_handle.m_id, priority);
522
523			// Get pointer to the AtomicQueue at mapped index (already constructed in allocateArrays)
524		✗	Types::AtomicQueue* atomicQueue = &this->m_handle.m_atomicQueues[index];
525
526			// Create the AtomicQueue with the specified parameters
527		✗	atomicQueue->create(numMsgs, maxMsgSize, allocator, allocatorId);
528
529			// Check if creation was successful (both capacity and slots must be initialized)
530		✗	if (!atomicQueue->isCreated()) {
531		✗	this->teardownInternal();
532		✗	return Os::QueueInterface::Status::ALLOCATION_FAILED;
533			}
534
535			// Enable this priority
536		✗	this->setPriorityEnabled(priority, true);
537
538		✗	return Os::QueueInterface::Status::OP_OK;
539			}
540
541		✗	void PriorityMemQueue::teardown() {
542		✗	this->teardownInternal();
543		✗	}
544
545		✗	void PriorityMemQueue::teardownInternal() {
546		✗	FW_ASSERT(this->m_handle.m_atomicQueues != nullptr || this->m_handle.m_maxPriority == 0, this->m_handle.m_id);
547
548			// Teardown all AtomicQueues if arrays are allocated
549		✗	if (this->m_handle.m_atomicQueues != nullptr) {
550		✗	for (FwSizeType i = 0; i < this->m_handle.m_numActivePriorities; ++i) {
551		✗	this->m_handle.m_atomicQueues[i].teardown();
552			}
553			}
554
555			// Delete the not-empty semaphore
556		✗	if (this->m_handle.m_notEmptySem != nullptr) {
557		✗	Fw::MemAllocator& allocator = this->getAllocator();
558		✗	FW_ASSERT(this->m_handle.m_allocatorId != 0 || this->m_handle.m_notEmptySem != nullptr, this->m_handle.m_id);
559		✗	this->m_handle.m_notEmptySem->~CountingSemaphore();
560		✗	allocator.deallocate(this->m_handle.m_allocatorId, this->m_handle.m_notEmptySem);
561		✗	this->m_handle.m_notEmptySem = nullptr;
562			}
563
564			// Reset handle state
565		✗	this->m_handle.m_priorityMask.store(0, std::memory_order_relaxed);
566
567			// Deallocate arrays using stored allocator ID
568		✗	Fw::MemAllocator& allocator = this->getAllocator();
569		✗	this->m_handle.deallocateArrays(allocator, this->m_handle.m_allocatorId);
570
571			// If we were using a configuration, mark it as unused
572		✗	FW_ASSERT(s_configs != nullptr || s_configsUsed == nullptr, this->m_handle.m_id);
573		✗	FW_ASSERT(s_configsUsed != nullptr || s_configs == nullptr, this->m_handle.m_id);
574
575		✗	if (s_configs != nullptr && s_configsUsed != nullptr) {
576		✗	for (FwSizeType i = 0; i < s_numConfigs; ++i) {
577		✗	if (s_configs[i].instanceId == this->m_handle.m_id && s_configsUsed[i].load()) {
578		✗	s_configsUsed[i].store(false);
579		✗	break;
580			}
581			}
582			}
583		✗	}
584
585			//! \brief Resolve priority to a valid AtomicQueue, fallback to DEFAULT if needed
586			//! \param handle: queue handle
587			//! \param priority: input/output priority (may be modified to DEFAULT)
588			//! \param queueId: queue ID for assertions
589			//! \param requirePrioritySizing: whether to assert on fallback
590			//! \return pointer to AtomicQueue or nullptr if uninitialized
591		✗	static Types::AtomicQueue* resolvePriorityQueue(PriorityMemQueueHandle& handle,
592			FwQueuePriorityType& priority,
593			FwEnumStoreType queueId,
594			bool requirePrioritySizing) {
595			// Look up priority in sparse map
596		✗	I8 index = handle.getPriorityIndex(priority);
597
598			// If priority not configured, fall back to default
599		✗	if (index < 0) {
600		✗	if (requirePrioritySizing) {
601		✗	FW_ASSERT(false, queueId, requirePrioritySizing, priority, handle.m_maxPriority);
602			}
603		✗	priority = Os::Generic::Queue::DEFAULT_PRIORITY;
604		✗	index = handle.getPriorityIndex(priority);
605		✗	FW_ASSERT(index >= 0, queueId, priority);
606			}
607
608			// Get AtomicQueue at mapped index
609		✗	FW_ASSERT(index < static_cast<I8>(handle.m_numActivePriorities), queueId, static_cast<FwAssertArgType>(index),
610			static_cast<FwAssertArgType>(handle.m_numActivePriorities));
611		✗	Types::AtomicQueue* atomicQueue = &handle.m_atomicQueues[index];
612		✗	FW_ASSERT(atomicQueue->isCreated(), queueId, priority);
613		✗	return atomicQueue;
614			}
615
616			//! \brief Update per-priority high water mark atomically
617			//! \param highWaterMarks: array of HWM atomics
618			//! \param index: array index (not priority value)
619			//! \param currentDepth: current queue depth
620			//! \param queueId: queue ID for assertions
621		✗	static void updateHighWaterMark(std::atomic<U32>* highWaterMarks,
622			FwSizeType index,
623			U32 currentDepth,
624			FwEnumStoreType queueId) {
625		✗	FW_ASSERT(highWaterMarks != nullptr, queueId, static_cast<FwAssertArgType>(index));
626		✗	U32 prevMax = highWaterMarks[index].load(std::memory_order_acquire);
627			// This is best effort, debug data only. So if retry count is exceeded, just give up
628		✗	constexpr U32 MAX_CAS_RETRIES = 100;
629		✗	for (U32 casRetries = 0; casRetries < MAX_CAS_RETRIES; ++casRetries) {
630		✗	if (currentDepth <= prevMax) {
631		✗	return; // No update needed
632			}
633		✗	if (highWaterMarks[index].compare_exchange_weak(prevMax, currentDepth, std::memory_order_release,
634			std::memory_order_acquire)) {
635		✗	return; // Update succeeded
636			}
637			}
638			}
639
640		✗	QueueInterface::Status PriorityMemQueue::send(const U8* buffer,
641			FwSizeType size,
642			FwQueuePriorityType priority,
643			QueueInterface::BlockingType blockType) {
644			// Validate input parameters
645		✗	FW_ASSERT(buffer != nullptr, this->m_handle.m_id, priority, blockType);
646		✗	FW_ASSERT(size > 0, this->m_handle.m_id, static_cast<FwAssertArgType>(size), priority);
647
648			// Check if priority is valid
649		✗	if (priority >= Os::Generic::Queue::MAX_PRIORITIES) {
650		✗	return QueueInterface::Status::INVALID_PRIORITY;
651			}
652
653			// Check if the queue is initialized
654		✗	if (this->m_handle.m_atomicQueues == nullptr) {
655		✗	return QueueInterface::Status::UNINITIALIZED;
656			}
657
658			// Resolve priority to valid queue
659			Types::AtomicQueue* atomicQueue =
660		✗	resolvePriorityQueue(this->m_handle, priority, this->m_handle.m_id, s_requirePrioritySizing);
661
662			// Check for sizing problem
663		✗	if (size > atomicQueue->getBufferSize()) {
664		✗	return QueueInterface::Status::SIZE_MISMATCH;
665			}
666
667			// Send message using AtomicQueue
668			bool success;
669		✗	if (blockType == QueueInterface::BlockingType::BLOCKING) {
670		✗	success = atomicQueue->enqueueBlocking(buffer, size, true);
671			} else {
672		✗	success = atomicQueue->enqueue(buffer, size);
673			}
674
675		✗	if (!success) {
676		✗	return QueueInterface::Status::FULL;
677			}
678
679			// Update per-priority high water mark (use array index, not priority value)
680		✗	I8 index = this->m_handle.getPriorityIndex(priority);
681		✗	FW_ASSERT(index >= 0, this->m_handle.m_id, priority);
682		✗	U32 currentDepth = static_cast<U32>(atomicQueue->getSize());
683		✗	updateHighWaterMark(this->m_handle.m_highWaterMarks, static_cast<FwSizeType>(index), currentDepth,
684			this->m_handle.m_id);
685
686			// Post semaphore to wake up receiver (if any)
687		✗	FW_ASSERT(this->m_handle.m_notEmptySem != nullptr, this->m_handle.m_id, priority);
688		✗	Os::CountingSemaphoreInterface::Status semStatus = this->m_handle.m_notEmptySem->post();
689		✗	FW_ASSERT(semStatus == Os::CountingSemaphoreInterface::Status::OP_OK, static_cast<FwAssertArgType>(semStatus));
690
691		✗	return QueueInterface::Status::OP_OK;
692			}
693
694		✗	QueueInterface::Status PriorityMemQueue::receive(U8* destination,
695			FwSizeType capacity,
696			QueueInterface::BlockingType blockType,
697			FwSizeType& actualSize,
698			FwQueuePriorityType& priority) {
699			// Validate input parameters
700		✗	FW_ASSERT(destination != nullptr, blockType, this->m_handle.m_id);
701
702			// Check if the queue is initialized
703		✗	if (this->m_handle.m_atomicQueues == nullptr) {
704		✗	return QueueInterface::Status::UNINITIALIZED;
705			}
706
707			// Check if the blocking type is valid
708		✗	FW_ASSERT(
709			blockType == QueueInterface::BlockingType::BLOCKING || blockType == QueueInterface::BlockingType::NONBLOCKING,
710			blockType, this->m_handle.m_id);
711
712			// RECEIVE FLOW: Scan all enabled priorities from highest to lowest.
713			// For each enabled priority, use getSize() as a cheap pre-filter (2 relaxed loads).
714			// Attempt dequeue only when getSize() > 0; dequeue CAS is the authoritative check.
715			// If getSize() is transiently stale and dequeue() fails, continue to next priority.
716			// Liveness is guaranteed by the semaphore — spurious wakes just re-scan.
717			//
718			// MEMORY ORDERING: acquire on priority mask ensures visibility of queue state.
719
720			// Tracks whether this receive has already consumed a semaphore credit via a blocking wait
721		✗	bool consumedCredit = false;
722
723			// Bounded loop with compile-time limit
724		✗	for (U32 reps = 0; reps < LOOP_GUARD_LIMIT; ++reps) {
725		✗	U32 enabledPriorities = this->m_handle.m_priorityMask.load(std::memory_order_acquire);
726
727		✗	for (I32 p = this->m_handle.m_maxPriority; p >= 0; --p) {
728		✗	FwQueuePriorityType testPriority = static_cast<FwQueuePriorityType>(p);
729		✗	if ((enabledPriorities & priorityBitMask(testPriority)) == 0) {
730		✗	continue;
731			}
732			// Look up priority in sparse map
733		✗	I8 index = this->m_handle.getPriorityIndex(testPriority);
734		✗	FW_ASSERT(index >= 0, index, testPriority, this->m_handle.m_id);
735		✗	FW_ASSERT(this->m_handle.m_atomicQueues != nullptr, this->m_handle.m_id, testPriority);
736		✗	Types::AtomicQueue* aq = &this->m_handle.m_atomicQueues[index];
737		✗	FW_ASSERT(aq->isCreated(), this->m_handle.m_id, testPriority);
738		✗	if (aq->getSize() == 0) {
739		✗	continue;
740			}
741		✗	const bool dequeued = aq->dequeue(destination, capacity, actualSize);
742		✗	if (dequeued) {
743			// Balance the sender's post: consume one credit unless a blocking wait already did.
744			// A failed tryWait is harmless: the credit was consumed by another receiver
745		✗	if (!consumedCredit) {
746		✗	(void)this->m_handle.m_notEmptySem->tryWait();
747			}
748		✗	priority = testPriority;
749		✗	return QueueInterface::Status::OP_OK;
750			}
751			}
752
753			// No message found
754		✗	if (blockType == QueueInterface::BlockingType::BLOCKING) {
755		✗	FW_ASSERT(this->m_handle.m_notEmptySem != nullptr, this->m_handle.m_id);
756		✗	Os::CountingSemaphoreInterface::Status semStatus = this->m_handle.m_notEmptySem->wait();
757		✗	FW_ASSERT(semStatus == Os::CountingSemaphoreInterface::Status::OP_OK,
758			static_cast<FwAssertArgType>(semStatus));
759		✗	consumedCredit = true;
760			} else {
761		✗	return QueueInterface::Status::EMPTY;
762			}
763			}
764
765			// Should never reach here - loop guard prevents infinite loop
766		✗	FW_ASSERT(false, this->m_handle.m_id, LOOP_GUARD_LIMIT);
767		✗	return QueueInterface::Status::UNKNOWN_ERROR;
768			}
769
770		✗	FwSizeType PriorityMemQueue::getMessagesAvailable() const {
771		✗	FwSizeType total = 0;
772
773		✗	if (this->m_handle.m_atomicQueues != nullptr) {
774		✗	FW_ASSERT(this->m_handle.m_numActivePriorities <= Os::Generic::Queue::MAX_PRIORITIES,
775			static_cast<FwAssertArgType>(this->m_handle.m_numActivePriorities));
776		✗	for (FwSizeType i = 0; i < this->m_handle.m_numActivePriorities; ++i) {
777		✗	const Types::AtomicQueue* atomicQueue = &this->m_handle.m_atomicQueues[i];
778		✗	if (atomicQueue->isCreated()) {
779		✗	total += atomicQueue->getSize();
780			}
781			}
782			}
783
784		✗	return total;
785			}
786
787		✗	FwSizeType PriorityMemQueue::getMessageHighWaterMark() const {
788			// Return the maximum high water mark across all priorities
789			// MEMORY ORDERING: Use acquire to ensure visibility of latest HWM updates
790		✗	U32 maxHwm = 0;
791		✗	if (this->m_handle.m_highWaterMarks != nullptr) {
792		✗	FW_ASSERT(this->m_handle.m_numActivePriorities <= Os::Generic::Queue::MAX_PRIORITIES,
793			static_cast<FwAssertArgType>(this->m_handle.m_numActivePriorities));
794		✗	for (FwSizeType i = 0; i < this->m_handle.m_numActivePriorities; ++i) {
795		✗	U32 hwm = this->m_handle.m_highWaterMarks[i].load(std::memory_order_acquire);
796		✗	if (hwm > maxHwm) {
797		✗	maxHwm = hwm;
798			}
799			}
800			}
801		✗	return static_cast<FwSizeType>(maxHwm);
802			}
803
804		✗	QueueHandle* PriorityMemQueue::getHandle() {
805		✗	return &this->m_handle;
806			}
807
808			} // namespace Generic
809			} // namespace Os
810