| 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 | 287817 | static constexpr U32 priorityBitMask(FwQueuePriorityType priority) { | |
| 55 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 287817 times.
|
287817 | return 1U << priority; |
| 56 | } | ||
| 57 | |||
| 58 | 2132 | void PriorityMemQueueHandle::init() { | |
| 59 | 2132 | 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 |
2/2✓ Branch 2 taken 1082 times.
✓ Branch 3 taken 1050 times.
|
2132 | if (this->m_atomicQueues != nullptr) { |
| 65 |
2/2✓ Branch 2 taken 1415 times.
✓ Branch 3 taken 1082 times.
|
2497 | for (FwSizeType i = 0; i < this->m_numActivePriorities; ++i) { |
| 66 | 1415 | this->m_atomicQueues[i].teardown(); | |
| 67 | } | ||
| 68 | } | ||
| 69 | |||
| 70 | // Initialize high water marks to zero if array is allocated | ||
| 71 |
2/2✓ Branch 2 taken 1082 times.
✓ Branch 3 taken 1050 times.
|
2132 | if (this->m_highWaterMarks != nullptr) { |
| 72 |
2/2✓ Branch 2 taken 1415 times.
✓ Branch 3 taken 1082 times.
|
2497 | for (FwSizeType i = 0; i < this->m_numActivePriorities; ++i) { |
| 73 | 1415 | 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 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 2132 times.
|
2132 | 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 | 2132 | this->m_priorityMask.store(0, std::memory_order_relaxed); | |
| 86 | 2132 | } | |
| 87 | |||
| 88 | 1082 | bool PriorityMemQueueHandle::allocateArrays(Fw::MemAllocator& allocator, FwEnumStoreType allocatorId) { | |
| 89 | 1082 | this->m_allocatorId = allocatorId; | |
| 90 | |||
| 91 | // Scan m_priorityMap to count configured priorities and find max priority | ||
| 92 | 1082 | this->m_numActivePriorities = 0; | |
| 93 | 1082 | this->m_maxPriority = 0; | |
| 94 | |||
| 95 |
2/2✓ Branch 0 taken 34624 times.
✓ Branch 1 taken 1082 times.
|
35706 | for (FwSizeType p = 0; p < Os::Generic::Queue::MAX_PRIORITIES; ++p) { |
| 96 |
2/2✓ Branch 3 taken 1415 times.
✓ Branch 4 taken 33209 times.
|
34624 | if (this->m_priorityMap[p] >= 0) { |
| 97 | 1415 | this->m_numActivePriorities++; | |
| 98 |
2/2✓ Branch 2 taken 334 times.
✓ Branch 3 taken 1081 times.
|
1415 | if (static_cast<FwQueuePriorityType>(p) > this->m_maxPriority) { |
| 99 | 334 | this->m_maxPriority = static_cast<FwQueuePriorityType>(p); | |
| 100 | } | ||
| 101 | } | ||
| 102 | } | ||
| 103 | |||
| 104 | 1082 | FW_ASSERT(this->m_numActivePriorities > 0, allocatorId); | |
| 105 | |||
| 106 | // Allocate memory for atomicQueues array (sized to actual configured priorities) | ||
| 107 | 1082 | FwSizeType atomicQueuesSize = sizeof(Types::AtomicQueue) * this->m_numActivePriorities; | |
| 108 |
1/1✓ Branch 4 taken 1082 times.
|
1082 | void* atomicQueuesMem = allocator.checkedAllocate(allocatorId, atomicQueuesSize, alignof(Types::AtomicQueue)); |
| 109 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1082 times.
|
1082 | if (atomicQueuesMem == nullptr) { |
| 110 | ✗ | this->deallocateArrays(allocator, allocatorId); | |
| 111 | ✗ | return false; | |
| 112 | } | ||
| 113 | // Use placement new to construct array | ||
| 114 | 1082 | this->m_atomicQueues = static_cast<Types::AtomicQueue*>(atomicQueuesMem); | |
| 115 |
2/2✓ Branch 2 taken 1415 times.
✓ Branch 3 taken 1082 times.
|
2497 | for (FwSizeType i = 0; i < this->m_numActivePriorities; ++i) { |
| 116 |
1/3✓ Branch 5 taken 1415 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
|
1415 | new (&this->m_atomicQueues[i]) Types::AtomicQueue(); |
| 117 | } | ||
| 118 | |||
| 119 | // Allocate memory for highWaterMarks array (sized to actual configured priorities) | ||
| 120 | 1082 | FwSizeType hwmSize = sizeof(std::atomic<U32>) * this->m_numActivePriorities; | |
| 121 |
1/1✓ Branch 4 taken 1082 times.
|
1082 | void* hwmMem = allocator.checkedAllocate(allocatorId, hwmSize, alignof(std::atomic<U32>)); |
| 122 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1082 times.
|
1082 | 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 | 1082 | this->m_highWaterMarks = reinterpret_cast<std::atomic<U32>*>(hwmMem); | |
| 128 |
2/2✓ Branch 2 taken 1415 times.
✓ Branch 3 taken 1082 times.
|
2497 | for (FwSizeType i = 0; i < this->m_numActivePriorities; ++i) { |
| 129 | // Placement new constructs std::atomic in pre-allocated memory (standard F' allocator pattern) | ||
| 130 | 1415 | new (&this->m_highWaterMarks[i]) std::atomic<U32>(0); | |
| 131 | } | ||
| 132 | |||
| 133 | 1082 | return true; | |
| 134 | } | ||
| 135 | |||
| 136 | 2131 | void PriorityMemQueueHandle::deallocateArrays(Fw::MemAllocator& allocator, FwEnumStoreType allocatorId) { | |
| 137 | 2131 | FW_ASSERT(this->m_numActivePriorities <= Os::Generic::Queue::MAX_PRIORITIES, | |
| 138 | static_cast<FwAssertArgType>(this->m_numActivePriorities)); | ||
| 139 | // Deallocate arrays in reverse order | ||
| 140 |
2/2✓ Branch 2 taken 1082 times.
✓ Branch 3 taken 1049 times.
|
2131 | if (this->m_highWaterMarks != nullptr) { |
| 141 | // std::atomic<U32> is trivially destructible — no explicit destructor needed | ||
| 142 | 1082 | allocator.deallocate(allocatorId, this->m_highWaterMarks); | |
| 143 | 1082 | this->m_highWaterMarks = nullptr; | |
| 144 | } | ||
| 145 |
2/2✓ Branch 2 taken 1082 times.
✓ Branch 3 taken 1049 times.
|
2131 | if (this->m_atomicQueues != nullptr) { |
| 146 | // Explicitly destroy AtomicQueues before deallocation | ||
| 147 |
2/2✓ Branch 2 taken 1415 times.
✓ Branch 3 taken 1082 times.
|
2497 | for (FwSizeType i = 0; i < this->m_numActivePriorities; ++i) { |
| 148 | 1415 | this->m_atomicQueues[i].~AtomicQueue(); | |
| 149 | } | ||
| 150 | 1082 | allocator.deallocate(allocatorId, this->m_atomicQueues); | |
| 151 | 1082 | this->m_atomicQueues = nullptr; | |
| 152 | } | ||
| 153 | |||
| 154 | // Reset priority map | ||
| 155 |
2/2✓ Branch 0 taken 68192 times.
✓ Branch 1 taken 2131 times.
|
70323 | for (FwSizeType i = 0; i < Os::Generic::Queue::MAX_PRIORITIES; ++i) { |
| 156 | 68192 | this->m_priorityMap[i] = -1; | |
| 157 | } | ||
| 158 | |||
| 159 | 2131 | this->m_maxPriority = 0; | |
| 160 | 2131 | this->m_numActivePriorities = 0; | |
| 161 | 2131 | this->m_allocatorId = 0; | |
| 162 | 2131 | } | |
| 163 | |||
| 164 | 78783 | void PriorityMemQueueHandle::enablePriority(FwQueuePriorityType priority) { | |
| 165 | 78783 | 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 | 78783 | 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 | 78783 | (void)this->m_priorityMask.fetch_or(priorityBitMask(priority), std::memory_order_seq_cst); | |
| 172 | 78783 | } | |
| 173 | |||
| 174 | 75883 | void PriorityMemQueueHandle::disablePriority(FwQueuePriorityType priority) { | |
| 175 | 75883 | 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 | 75883 | (void)this->m_priorityMask.fetch_and(~priorityBitMask(priority), std::memory_order_seq_cst); | |
| 180 | 75883 | } | |
| 181 | |||
| 182 | 1050 | PriorityMemQueue::PriorityMemQueue() { | |
| 183 | // Initialize handle to safe defaults | ||
| 184 |
1/1✓ Branch 4 taken 1050 times.
|
1050 | this->m_handle.init(); |
| 185 | 1050 | } | |
| 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 | 2830 | void PriorityMemQueue::setPriorityEnabled(FwQueuePriorityType priority, bool enabled) { | |
| 242 | // Delegate to handle methods (SSOT) | ||
| 243 |
1/2✓ Branch 0 taken 2830 times.
✗ Branch 1 not taken.
|
2830 | if (enabled) { |
| 244 | 2830 | this->m_handle.enablePriority(priority); | |
| 245 | } else { | ||
| 246 | ✗ | this->m_handle.disablePriority(priority); | |
| 247 | } | ||
| 248 | 2830 | } | |
| 249 | |||
| 250 | 4295 | Fw::MemAllocator& PriorityMemQueue::getAllocator() { | |
| 251 |
2/2✓ Branch 3 taken 4295 times.
✓ Branch 6 taken 4295 times.
|
8590 | return Fw::MemAllocatorRegistry::getInstance().getAnAllocator( |
| 252 | 8590 | 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 | 71 | static void validateQueueConfigs(PriorityMemQueue::QueueConfig* queueConfigs, FwSizeType numQueueConfigs) { | |
| 259 |
2/2✓ Branch 0 taken 72 times.
✓ Branch 1 taken 71 times.
|
143 | for (FwSizeType i = 0; i < numQueueConfigs; ++i) { |
| 260 | 72 | PriorityMemQueue::QueueConfig* currentConfig = &queueConfigs[i]; | |
| 261 | |||
| 262 | // Assert if numPriorities is 0 or exceeds maximum | ||
| 263 | 72 | 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 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 72 times.
|
73 | for (FwSizeType j = i + 1; j < numQueueConfigs; ++j) { |
| 270 | 1 | PriorityMemQueue::QueueConfig* otherConfig = &queueConfigs[j]; | |
| 271 | 1 | FW_ASSERT(currentConfig->instanceId != otherConfig->instanceId, currentConfig->instanceId, | |
| 272 | static_cast<FwAssertArgType>(i), static_cast<FwAssertArgType>(j)); | ||
| 273 | } | ||
| 274 | |||
| 275 | // Check priority configurations | ||
| 276 | 72 | PriorityMemQueue::QueuePriorityConfig* priorityConfigs = currentConfig->priorityConfigs; | |
| 277 | 72 | FW_ASSERT(priorityConfigs != nullptr, static_cast<FwAssertArgType>(i), currentConfig->instanceId, | |
| 278 | static_cast<FwAssertArgType>(currentConfig->numPriorities)); | ||
| 279 |
2/2✓ Branch 2 taken 405 times.
✓ Branch 3 taken 72 times.
|
477 | for (FwSizeType p = 0; p < currentConfig->numPriorities; ++p) { |
| 280 | 405 | PriorityMemQueue::QueuePriorityConfig* pConfig = &priorityConfigs[p]; | |
| 281 | |||
| 282 | // Assert if maxMsgSize or numMsgs is 0 | ||
| 283 | 405 | FW_ASSERT(pConfig->maxMsgSize > 0, static_cast<FwAssertArgType>(i), static_cast<FwAssertArgType>(p), | |
| 284 | pConfig->priority); | ||
| 285 | 405 | FW_ASSERT(pConfig->numMsgs > 0, static_cast<FwAssertArgType>(i), static_cast<FwAssertArgType>(p), | |
| 286 | pConfig->priority); | ||
| 287 | 405 | 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 |
2/2✓ Branch 2 taken 3649 times.
✓ Branch 3 taken 405 times.
|
4054 | for (FwSizeType q = p + 1; q < currentConfig->numPriorities; ++q) { |
| 292 | 3649 | PriorityMemQueue::QueuePriorityConfig* qConfig = &priorityConfigs[q]; | |
| 293 | 3649 | FW_ASSERT(pConfig->priority != qConfig->priority, static_cast<FwAssertArgType>(i), | |
| 294 | currentConfig->instanceId, pConfig->priority); | ||
| 295 | } | ||
| 296 | } | ||
| 297 | } | ||
| 298 | 71 | } | |
| 299 | |||
| 300 | 1079 | 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 | 1079 | FW_ASSERT((queueConfigs != nullptr) || (numQueueConfigs == 0), 0); | |
| 306 | |||
| 307 | // Assert if already configured - that's not a supported use case | ||
| 308 | 1079 | FW_ASSERT(!s_configured, 0); | |
| 309 | |||
| 310 | 1079 | s_configured = true; | |
| 311 | |||
| 312 | // Assert if required is true but num priorities is 0 | ||
| 313 | 1079 | FW_ASSERT(!(required && numQueueConfigs == 0), required, static_cast<FwAssertArgType>(numQueueConfigs)); | |
| 314 | |||
| 315 | // Validate all configurations | ||
| 316 |
2/2✓ Branch 0 taken 71 times.
✓ Branch 1 taken 1008 times.
|
1079 | if (queueConfigs != nullptr) { |
| 317 | 71 | validateQueueConfigs(queueConfigs, numQueueConfigs); | |
| 318 | } | ||
| 319 | // Get the memory allocator configured for priority queues | ||
| 320 |
2/2✓ Branch 3 taken 1079 times.
✓ Branch 6 taken 1079 times.
|
2158 | Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator( |
| 321 | 1079 | Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE); | |
| 322 | // Allocate memory for tracking used configurations | ||
| 323 |
2/2✓ Branch 0 taken 71 times.
✓ Branch 1 taken 1008 times.
|
1079 | if (numQueueConfigs > 0) { |
| 324 | 71 | FwSizeType expSize = numQueueConfigs * sizeof(std::atomic<bool>); | |
| 325 | 71 | s_configsUsed = static_cast<std::atomic<bool>*>( | |
| 326 |
1/1✓ Branch 4 taken 71 times.
|
71 | allocator.checkedAllocate(allocatorId, expSize, alignof(std::atomic<bool>))); |
| 327 | 71 | FW_ASSERT(s_configsUsed != nullptr); | |
| 328 | // Initialize all entries to false using placement new | ||
| 329 |
2/2✓ Branch 0 taken 72 times.
✓ Branch 1 taken 71 times.
|
143 | for (FwSizeType i = 0; i < numQueueConfigs; ++i) { |
| 330 | 72 | 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 |
2/2✓ Branch 0 taken 71 times.
✓ Branch 1 taken 1008 times.
|
1079 | if (numQueueConfigs > 0) { |
| 338 | 71 | FwSizeType totalSize = numQueueConfigs * sizeof(QueueConfig); | |
| 339 |
2/2✓ Branch 0 taken 72 times.
✓ Branch 1 taken 71 times.
|
143 | for (FwSizeType i = 0; i < numQueueConfigs; ++i) { |
| 340 | 72 | totalSize += queueConfigs[i].numPriorities * sizeof(QueuePriorityConfig); | |
| 341 | } | ||
| 342 |
1/1✓ Branch 4 taken 71 times.
|
71 | void* configsMem = allocator.checkedAllocate(allocatorId, totalSize, alignof(QueueConfig)); |
| 343 | 71 | FW_ASSERT(configsMem != nullptr); | |
| 344 | 71 | s_configs = static_cast<QueueConfig*>(configsMem); | |
| 345 | 71 | QueuePriorityConfig* priorityBase = reinterpret_cast<QueuePriorityConfig*>(s_configs + numQueueConfigs); | |
| 346 |
2/2✓ Branch 0 taken 72 times.
✓ Branch 1 taken 71 times.
|
143 | for (FwSizeType i = 0; i < numQueueConfigs; ++i) { |
| 347 | 72 | s_configs[i] = queueConfigs[i]; | |
| 348 | 72 | FwSizeType priorityConfigsSize = queueConfigs[i].numPriorities * sizeof(QueuePriorityConfig); | |
| 349 |
2/4✗ Branch 3 not taken.
✓ Branch 4 taken 72 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 72 times.
|
72 | (void)memcpy(priorityBase, queueConfigs[i].priorityConfigs, priorityConfigsSize); |
| 350 | 72 | s_configs[i].priorityConfigs = priorityBase; | |
| 351 | 72 | priorityBase += queueConfigs[i].numPriorities; | |
| 352 | } | ||
| 353 | } | ||
| 354 | 1079 | s_numConfigs = numQueueConfigs; | |
| 355 | 1079 | s_requirePrioritySizing = required; | |
| 356 | 1079 | s_allocatorId = allocatorId; | |
| 357 | 1079 | } | |
| 358 | |||
| 359 | 1156 | void PriorityMemQueue::resetConfig() { | |
| 360 | // Configs are allocated if and only if a nonzero count was configured | ||
| 361 | 1156 | 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 |
3/4✓ Branch 0 taken 1085 times.
✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1085 times.
|
1156 | if (s_configsUsed != nullptr || s_configs != nullptr) { |
| 364 | // Get allocator (same as used in config()) | ||
| 365 |
2/2✓ Branch 3 taken 71 times.
✓ Branch 6 taken 71 times.
|
142 | Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator( |
| 366 | 71 | Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE); | |
| 367 | 71 | FwEnumStoreType allocatorId = s_allocatorId; | |
| 368 | |||
| 369 | // Deallocate the tracking array | ||
| 370 |
1/2✓ Branch 0 taken 71 times.
✗ Branch 1 not taken.
|
71 | if (s_configsUsed != nullptr) { |
| 371 | 71 | allocator.deallocate(allocatorId, s_configsUsed); | |
| 372 | 71 | s_configsUsed = nullptr; | |
| 373 | } | ||
| 374 | |||
| 375 | // Deallocate the single contiguous config block (QueueConfig[] + all QueuePriorityConfig[] sub-arrays) | ||
| 376 |
1/2✓ Branch 0 taken 71 times.
✗ Branch 1 not taken.
|
71 | if (s_configs != nullptr) { |
| 377 | 71 | allocator.deallocate(allocatorId, s_configs); | |
| 378 | } | ||
| 379 | } | ||
| 380 | |||
| 381 | // Reset all static state | ||
| 382 | 1156 | s_configs = nullptr; | |
| 383 | 1156 | s_numConfigs = 0; | |
| 384 | 1156 | s_requirePrioritySizing = false; | |
| 385 | 1156 | s_configured = false; | |
| 386 | 1156 | } | |
| 387 | |||
| 388 | 2104 | PriorityMemQueue::~PriorityMemQueue() { | |
| 389 | 2100 | this->teardownInternal(); | |
| 390 | 2104 | } | |
| 391 | |||
| 392 | 1082 | QueueInterface::Status PriorityMemQueue::create(FwEnumStoreType id, | |
| 393 | const Fw::ConstStringBase& name, | ||
| 394 | FwSizeType depth, | ||
| 395 | FwSizeType messageSize) { | ||
| 396 | 1082 | FW_ASSERT(depth > 0, id); | |
| 397 | 1082 | FW_ASSERT(messageSize > 0, id); | |
| 398 | |||
| 399 | // Initialize the handle ID | ||
| 400 | 1082 | this->m_handle.m_id = id; | |
| 401 | |||
| 402 | // Get the memory allocator for queue operations | ||
| 403 |
1/1✓ Branch 4 taken 1082 times.
|
1082 | Fw::MemAllocator& allocator = this->getAllocator(); |
| 404 | |||
| 405 | // Find a matching configuration if one exists | ||
| 406 |
1/1✓ Branch 4 taken 1082 times.
|
1082 | QueueConfig* queueConfig = findMatchingConfig(id); |
| 407 | |||
| 408 | // Build priority map from configuration | ||
| 409 |
2/2✓ Branch 0 taken 73 times.
✓ Branch 1 taken 1009 times.
|
1082 | if (queueConfig != nullptr) { |
| 410 | // Map each configured priority to array index | ||
| 411 |
2/2✓ Branch 2 taken 406 times.
✓ Branch 3 taken 73 times.
|
479 | for (FwSizeType i = 0; i < queueConfig->numPriorities; ++i) { |
| 412 | 406 | FwQueuePriorityType p = queueConfig->priorityConfigs[i].priority; | |
| 413 | 406 | FW_ASSERT(p < Os::Generic::Queue::MAX_PRIORITIES, id, static_cast<FwAssertArgType>(p), | |
| 414 | Os::Generic::Queue::MAX_PRIORITIES); | ||
| 415 | 406 | this->m_handle.m_priorityMap[p] = static_cast<I8>(i); | |
| 416 | } | ||
| 417 | } else { | ||
| 418 | // Default: single priority at DEFAULT_PRIORITY | ||
| 419 | 1009 | this->m_handle.m_priorityMap[Os::Generic::Queue::DEFAULT_PRIORITY] = 0; | |
| 420 | } | ||
| 421 | |||
| 422 | // Allocate arrays for priority data (reads from m_priorityMap) | ||
| 423 |
2/3✓ Branch 4 taken 1082 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 1082 times.
|
1082 | if (!this->m_handle.allocateArrays(allocator, id)) { |
| 424 | ✗ | return Os::QueueInterface::Status::ALLOCATION_FAILED; | |
| 425 | } | ||
| 426 | |||
| 427 | // Initialize the handle with allocated arrays | ||
| 428 |
1/1✓ Branch 4 taken 1082 times.
|
1082 | this->m_handle.init(); |
| 429 | |||
| 430 | // Allocate and create the not-empty semaphore (initial count 0) | ||
| 431 | 1082 | FwSizeType semSize = sizeof(Os::CountingSemaphore); | |
| 432 |
1/1✓ Branch 4 taken 1082 times.
|
1082 | void* semMem = allocator.checkedAllocate(id, semSize, alignof(Os::CountingSemaphore)); |
| 433 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1082 times.
|
1082 | if (semMem == nullptr) { |
| 434 | ✗ | this->m_handle.deallocateArrays(allocator, id); | |
| 435 | ✗ | return Os::QueueInterface::Status::ALLOCATION_FAILED; | |
| 436 | } | ||
| 437 |
1/3✓ Branch 3 taken 1082 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
1082 | this->m_handle.m_notEmptySem = new (semMem) Os::CountingSemaphore(static_cast<U32>(0)); |
| 438 | |||
| 439 | // Create the priority queues based on configuration | ||
| 440 |
2/2✓ Branch 0 taken 73 times.
✓ Branch 1 taken 1009 times.
|
1082 | if (queueConfig != nullptr) { |
| 441 | // Use the found configuration to create multiple priority queues | ||
| 442 |
1/1✓ Branch 4 taken 73 times.
|
73 | return createConfiguredQueues(queueConfig, allocator, id); |
| 443 | } else { | ||
| 444 | // No configuration found, create a single default priority queue | ||
| 445 |
1/1✓ Branch 4 taken 1009 times.
|
1009 | return createDefaultQueue(depth, messageSize, allocator, id); |
| 446 | } | ||
| 447 | } | ||
| 448 | |||
| 449 | // Helper method to find a matching configuration for the given ID | ||
| 450 | 1082 | PriorityMemQueue::QueueConfig* PriorityMemQueue::findMatchingConfig(FwEnumStoreType id) { | |
| 451 |
3/4✓ Branch 0 taken 73 times.
✓ Branch 1 taken 1009 times.
✓ Branch 2 taken 73 times.
✗ Branch 3 not taken.
|
1082 | if (s_configs != nullptr && s_configsUsed != nullptr) { |
| 452 |
1/2✓ Branch 0 taken 74 times.
✗ Branch 1 not taken.
|
74 | for (FwSizeType i = 0; i < s_numConfigs; ++i) { |
| 453 |
2/2✓ Branch 2 taken 73 times.
✓ Branch 3 taken 1 times.
|
74 | if (s_configs[i].instanceId == id) { |
| 454 | // Atomic check-and-set to claim configuration | ||
| 455 | 73 | bool expected = false; | |
| 456 |
1/2✓ Branch 3 taken 73 times.
✗ Branch 4 not taken.
|
73 | if (s_configsUsed[i].compare_exchange_strong(expected, true, std::memory_order_acq_rel)) { |
| 457 | 73 | 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 | 1009 | return nullptr; | |
| 466 | } | ||
| 467 | |||
| 468 | // Helper method to create queues based on configuration | ||
| 469 | 73 | QueueInterface::Status PriorityMemQueue::createConfiguredQueues(QueueConfig* queueConfig, | |
| 470 | Fw::MemAllocator& allocator, | ||
| 471 | FwEnumStoreType allocatorId) { | ||
| 472 | 73 | FW_ASSERT(queueConfig != nullptr, this->m_handle.m_id); | |
| 473 | 73 | FW_ASSERT(queueConfig->priorityConfigs != nullptr, this->m_handle.m_id, | |
| 474 | static_cast<FwAssertArgType>(queueConfig->numPriorities)); | ||
| 475 | |||
| 476 |
2/2✓ Branch 2 taken 406 times.
✓ Branch 3 taken 73 times.
|
479 | for (FwSizeType i = 0; i < queueConfig->numPriorities; ++i) { |
| 477 | 406 | const QueuePriorityConfig& priorityConfig = queueConfig->priorityConfigs[i]; | |
| 478 | 406 | FwQueuePriorityType priority = priorityConfig.priority; | |
| 479 | |||
| 480 | // Create and initialize the priority queue | ||
| 481 | 812 | QueueInterface::Status status = this->createPriorityQueue(priority, priorityConfig.maxMsgSize, | |
| 482 | 406 | priorityConfig.numMsgs, allocator, allocatorId); | |
| 483 | |||
| 484 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 406 times.
|
406 | if (status != Os::QueueInterface::Status::OP_OK) { |
| 485 | ✗ | return status; | |
| 486 | } | ||
| 487 | |||
| 488 | // Enable this priority | ||
| 489 | 406 | this->setPriorityEnabled(priority, true); | |
| 490 | } | ||
| 491 | |||
| 492 | 73 | return Os::QueueInterface::Status::OP_OK; | |
| 493 | } | ||
| 494 | |||
| 495 | // Helper method to create a single default priority queue | ||
| 496 | 1009 | 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 | 1009 | this->createPriorityQueue(Os::Generic::Queue::DEFAULT_PRIORITY, messageSize, depth, allocator, allocatorId); | |
| 503 |
1/2✓ Branch 0 taken 1009 times.
✗ Branch 1 not taken.
|
1009 | if (status == Os::QueueInterface::Status::OP_OK) { |
| 504 | 1009 | this->setPriorityEnabled(Os::Generic::Queue::DEFAULT_PRIORITY, true); | |
| 505 | } | ||
| 506 | 1009 | return status; | |
| 507 | } | ||
| 508 | |||
| 509 | // Helper method to create a single priority queue using AtomicQueue | ||
| 510 | 1415 | QueueInterface::Status PriorityMemQueue::createPriorityQueue(FwQueuePriorityType priority, | |
| 511 | FwSizeType maxMsgSize, | ||
| 512 | FwSizeType numMsgs, | ||
| 513 | Fw::MemAllocator& allocator, | ||
| 514 | FwEnumStoreType allocatorId) { | ||
| 515 | 1415 | FW_ASSERT(this->m_handle.m_atomicQueues != nullptr, this->m_handle.m_id, priority); | |
| 516 | 1415 | FW_ASSERT(priority < Os::Generic::Queue::MAX_PRIORITIES, this->m_handle.m_id, priority); | |
| 517 | 1415 | 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 | 1415 | I8 index = this->m_handle.getPriorityIndex(priority); | |
| 521 | 1415 | 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 | 1415 | Types::AtomicQueue* atomicQueue = &this->m_handle.m_atomicQueues[index]; | |
| 525 | |||
| 526 | // Create the AtomicQueue with the specified parameters | ||
| 527 | 1415 | atomicQueue->create(numMsgs, maxMsgSize, allocator, allocatorId); | |
| 528 | |||
| 529 | // Check if creation was successful (both capacity and slots must be initialized) | ||
| 530 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 1415 times.
|
1415 | if (!atomicQueue->isCreated()) { |
| 531 | ✗ | this->teardownInternal(); | |
| 532 | ✗ | return Os::QueueInterface::Status::ALLOCATION_FAILED; | |
| 533 | } | ||
| 534 | |||
| 535 | // Enable this priority | ||
| 536 | 1415 | this->setPriorityEnabled(priority, true); | |
| 537 | |||
| 538 | 1415 | return Os::QueueInterface::Status::OP_OK; | |
| 539 | } | ||
| 540 | |||
| 541 | 1081 | void PriorityMemQueue::teardown() { | |
| 542 | 1081 | this->teardownInternal(); | |
| 543 | 1081 | } | |
| 544 | |||
| 545 | 2131 | void PriorityMemQueue::teardownInternal() { | |
| 546 | 2131 | 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 |
2/2✓ Branch 4 taken 1082 times.
✓ Branch 5 taken 1049 times.
|
2131 | if (this->m_handle.m_atomicQueues != nullptr) { |
| 550 |
2/2✓ Branch 4 taken 1415 times.
✓ Branch 5 taken 1082 times.
|
2497 | for (FwSizeType i = 0; i < this->m_handle.m_numActivePriorities; ++i) { |
| 551 | 1415 | this->m_handle.m_atomicQueues[i].teardown(); | |
| 552 | } | ||
| 553 | } | ||
| 554 | |||
| 555 | // Delete the not-empty semaphore | ||
| 556 |
2/2✓ Branch 4 taken 1082 times.
✓ Branch 5 taken 1049 times.
|
2131 | if (this->m_handle.m_notEmptySem != nullptr) { |
| 557 | 1082 | Fw::MemAllocator& allocator = this->getAllocator(); | |
| 558 | 1082 | FW_ASSERT(this->m_handle.m_allocatorId != 0 || this->m_handle.m_notEmptySem != nullptr, this->m_handle.m_id); | |
| 559 | 1082 | this->m_handle.m_notEmptySem->~CountingSemaphore(); | |
| 560 | 1082 | allocator.deallocate(this->m_handle.m_allocatorId, this->m_handle.m_notEmptySem); | |
| 561 | 1082 | this->m_handle.m_notEmptySem = nullptr; | |
| 562 | } | ||
| 563 | |||
| 564 | // Reset handle state | ||
| 565 | 2131 | this->m_handle.m_priorityMask.store(0, std::memory_order_relaxed); | |
| 566 | |||
| 567 | // Deallocate arrays using stored allocator ID | ||
| 568 | 2131 | Fw::MemAllocator& allocator = this->getAllocator(); | |
| 569 | 2131 | this->m_handle.deallocateArrays(allocator, this->m_handle.m_allocatorId); | |
| 570 | |||
| 571 | // If we were using a configuration, mark it as unused | ||
| 572 | 2131 | FW_ASSERT(s_configs != nullptr || s_configsUsed == nullptr, this->m_handle.m_id); | |
| 573 | 2131 | FW_ASSERT(s_configsUsed != nullptr || s_configs == nullptr, this->m_handle.m_id); | |
| 574 | |||
| 575 |
3/4✓ Branch 0 taken 105 times.
✓ Branch 1 taken 2026 times.
✓ Branch 2 taken 105 times.
✗ Branch 3 not taken.
|
2131 | if (s_configs != nullptr && s_configsUsed != nullptr) { |
| 576 |
2/2✓ Branch 0 taken 108 times.
✓ Branch 1 taken 32 times.
|
140 | for (FwSizeType i = 0; i < s_numConfigs; ++i) { |
| 577 |
6/6✓ Branch 6 taken 105 times.
✓ Branch 7 taken 3 times.
✓ Branch 11 taken 73 times.
✓ Branch 12 taken 32 times.
✓ Branch 13 taken 73 times.
✓ Branch 14 taken 35 times.
|
108 | if (s_configs[i].instanceId == this->m_handle.m_id && s_configsUsed[i].load()) { |
| 578 | 73 | s_configsUsed[i].store(false); | |
| 579 | 73 | break; | |
| 580 | } | ||
| 581 | } | ||
| 582 | } | ||
| 583 | 2131 | } | |
| 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 | 272469 | static Types::AtomicQueue* resolvePriorityQueue(PriorityMemQueueHandle& handle, | |
| 592 | FwQueuePriorityType& priority, | ||
| 593 | FwEnumStoreType queueId, | ||
| 594 | bool requirePrioritySizing) { | ||
| 595 | // Look up priority in sparse map | ||
| 596 | 272469 | I8 index = handle.getPriorityIndex(priority); | |
| 597 | |||
| 598 | // If priority not configured, fall back to default | ||
| 599 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 271206 times.
|
271208 | if (index < 0) { |
| 600 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
|
2 | if (requirePrioritySizing) { |
| 601 | ✗ | FW_ASSERT(false, queueId, requirePrioritySizing, priority, handle.m_maxPriority); | |
| 602 | } | ||
| 603 | 2 | priority = Os::Generic::Queue::DEFAULT_PRIORITY; | |
| 604 | 2 | index = handle.getPriorityIndex(priority); | |
| 605 | 2 | FW_ASSERT(index >= 0, queueId, priority); | |
| 606 | } | ||
| 607 | |||
| 608 | // Get AtomicQueue at mapped index | ||
| 609 | 271208 | FW_ASSERT(index < static_cast<I8>(handle.m_numActivePriorities), queueId, static_cast<FwAssertArgType>(index), | |
| 610 | static_cast<FwAssertArgType>(handle.m_numActivePriorities)); | ||
| 611 | 271764 | Types::AtomicQueue* atomicQueue = &handle.m_atomicQueues[index]; | |
| 612 | 271866 | FW_ASSERT(atomicQueue->isCreated(), queueId, priority); | |
| 613 | 270818 | 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 | 83045 | static void updateHighWaterMark(std::atomic<U32>* highWaterMarks, | |
| 622 | FwSizeType index, | ||
| 623 | U32 currentDepth, | ||
| 624 | FwEnumStoreType queueId) { | ||
| 625 | 83045 | FW_ASSERT(highWaterMarks != nullptr, queueId, static_cast<FwAssertArgType>(index)); | |
| 626 | 83045 | 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 | 82180 | constexpr U32 MAX_CAS_RETRIES = 100; | |
| 629 |
1/2✓ Branch 0 taken 82315 times.
✗ Branch 1 not taken.
|
82178 | for (U32 casRetries = 0; casRetries < MAX_CAS_RETRIES; ++casRetries) { |
| 630 |
2/2✓ Branch 0 taken 79633 times.
✓ Branch 1 taken 2682 times.
|
82315 | if (currentDepth <= prevMax) { |
| 631 | 79633 | return; // No update needed | |
| 632 | } | ||
| 633 |
1/2✓ Branch 3 taken 2683 times.
✗ Branch 4 not taken.
|
5363 | if (highWaterMarks[index].compare_exchange_weak(prevMax, currentDepth, std::memory_order_release, |
| 634 | std::memory_order_acquire)) { | ||
| 635 | 2683 | return; // Update succeeded | |
| 636 | } | ||
| 637 | } | ||
| 638 | } | ||
| 639 | |||
| 640 | 273714 | QueueInterface::Status PriorityMemQueue::send(const U8* buffer, | |
| 641 | FwSizeType size, | ||
| 642 | FwQueuePriorityType priority, | ||
| 643 | QueueInterface::BlockingType blockType) { | ||
| 644 | // Validate input parameters | ||
| 645 | 273714 | FW_ASSERT(buffer != nullptr, this->m_handle.m_id, priority, blockType); | |
| 646 | 273796 | FW_ASSERT(size > 0, this->m_handle.m_id, static_cast<FwAssertArgType>(size), priority); | |
| 647 | |||
| 648 | // Check if priority is valid | ||
| 649 |
2/2✓ Branch 0 taken 5 times.
✓ Branch 1 taken 273433 times.
|
273438 | if (priority >= Os::Generic::Queue::MAX_PRIORITIES) { |
| 650 | 5 | return QueueInterface::Status::INVALID_PRIORITY; | |
| 651 | } | ||
| 652 | |||
| 653 | // Check if the queue is initialized | ||
| 654 |
2/2✓ Branch 4 taken 1 times.
✓ Branch 5 taken 272643 times.
|
273433 | if (this->m_handle.m_atomicQueues == nullptr) { |
| 655 | 1 | return QueueInterface::Status::UNINITIALIZED; | |
| 656 | } | ||
| 657 | |||
| 658 | // Resolve priority to valid queue | ||
| 659 | Types::AtomicQueue* atomicQueue = | ||
| 660 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 272643 times.
|
272643 | resolvePriorityQueue(this->m_handle, priority, this->m_handle.m_id, s_requirePrioritySizing); |
| 661 | |||
| 662 | // Check for sizing problem | ||
| 663 |
2/2✓ Branch 2 taken 8 times.
✓ Branch 3 taken 270444 times.
|
270041 | if (size > atomicQueue->getBufferSize()) { |
| 664 | 8 | return QueueInterface::Status::SIZE_MISMATCH; | |
| 665 | } | ||
| 666 | |||
| 667 | // Send message using AtomicQueue | ||
| 668 | bool success; | ||
| 669 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 270443 times.
|
270444 | if (blockType == QueueInterface::BlockingType::BLOCKING) { |
| 670 | 1 | success = atomicQueue->enqueueBlocking(buffer, size, true); | |
| 671 | } else { | ||
| 672 | 270443 | success = atomicQueue->enqueue(buffer, size); | |
| 673 | } | ||
| 674 | |||
| 675 |
2/2✓ Branch 0 taken 190926 times.
✓ Branch 1 taken 82399 times.
|
273325 | if (!success) { |
| 676 | 190926 | return QueueInterface::Status::FULL; | |
| 677 | } | ||
| 678 | |||
| 679 | // Update per-priority high water mark (use array index, not priority value) | ||
| 680 | 82399 | I8 index = this->m_handle.getPriorityIndex(priority); | |
| 681 | 82294 | FW_ASSERT(index >= 0, this->m_handle.m_id, priority); | |
| 682 | 82294 | U32 currentDepth = static_cast<U32>(atomicQueue->getSize()); | |
| 683 | 82281 | updateHighWaterMark(this->m_handle.m_highWaterMarks, static_cast<FwSizeType>(index), currentDepth, | |
| 684 | 82171 | this->m_handle.m_id); | |
| 685 | |||
| 686 | // Post semaphore to wake up receiver (if any) | ||
| 687 | 82199 | FW_ASSERT(this->m_handle.m_notEmptySem != nullptr, this->m_handle.m_id, priority); | |
| 688 | 82297 | Os::CountingSemaphoreInterface::Status semStatus = this->m_handle.m_notEmptySem->post(); | |
| 689 | 82297 | FW_ASSERT(semStatus == Os::CountingSemaphoreInterface::Status::OP_OK, static_cast<FwAssertArgType>(semStatus)); | |
| 690 | |||
| 691 | 82353 | return QueueInterface::Status::OP_OK; | |
| 692 | } | ||
| 693 | |||
| 694 | 82032 | QueueInterface::Status PriorityMemQueue::receive(U8* destination, | |
| 695 | FwSizeType capacity, | ||
| 696 | QueueInterface::BlockingType blockType, | ||
| 697 | FwSizeType& actualSize, | ||
| 698 | FwQueuePriorityType& priority) { | ||
| 699 | // Validate input parameters | ||
| 700 | 82032 | FW_ASSERT(destination != nullptr, blockType, this->m_handle.m_id); | |
| 701 | |||
| 702 | // Check if the queue is initialized | ||
| 703 |
2/2✓ Branch 4 taken 1 times.
✓ Branch 5 taken 82079 times.
|
82032 | if (this->m_handle.m_atomicQueues == nullptr) { |
| 704 | 1 | return QueueInterface::Status::UNINITIALIZED; | |
| 705 | } | ||
| 706 | |||
| 707 | // Check if the blocking type is valid | ||
| 708 | 82079 | 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 | 81962 | bool consumedCredit = false; | |
| 722 | |||
| 723 | // Bounded loop with compile-time limit | ||
| 724 |
2/2✓ Branch 0 taken 82728 times.
✓ Branch 1 taken 240 times.
|
82968 | for (U32 reps = 0; reps < LOOP_GUARD_LIMIT; ++reps) { |
| 725 | 82728 | U32 enabledPriorities = this->m_handle.m_priorityMask.load(std::memory_order_acquire); | |
| 726 | |||
| 727 |
2/2✓ Branch 4 taken 133319 times.
✓ Branch 5 taken 1141 times.
|
134560 | for (I32 p = this->m_handle.m_maxPriority; p >= 0; --p) { |
| 728 | 133319 | FwQueuePriorityType testPriority = static_cast<FwQueuePriorityType>(p); | |
| 729 |
2/2✓ Branch 1 taken 91 times.
✓ Branch 2 taken 133039 times.
|
133319 | if ((enabledPriorities & priorityBitMask(testPriority)) == 0) { |
| 730 | 91 | continue; | |
| 731 | } | ||
| 732 | // Look up priority in sparse map | ||
| 733 | 133039 | I8 index = this->m_handle.getPriorityIndex(testPriority); | |
| 734 | 133264 | FW_ASSERT(index >= 0, index, testPriority, this->m_handle.m_id); | |
| 735 | 133264 | FW_ASSERT(this->m_handle.m_atomicQueues != nullptr, this->m_handle.m_id, testPriority); | |
| 736 | 133729 | Types::AtomicQueue* aq = &this->m_handle.m_atomicQueues[index]; | |
| 737 | 133793 | FW_ASSERT(aq->isCreated(), this->m_handle.m_id, testPriority); | |
| 738 |
2/2✓ Branch 2 taken 50113 times.
✓ Branch 3 taken 83454 times.
|
132848 | if (aq->getSize() == 0) { |
| 739 | 50113 | continue; | |
| 740 | } | ||
| 741 | 83454 | const bool dequeued = aq->dequeue(destination, capacity, actualSize); | |
| 742 |
2/2✓ Branch 0 taken 81640 times.
✓ Branch 1 taken 2071 times.
|
83711 | 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 |
2/2✓ Branch 0 taken 80636 times.
✓ Branch 1 taken 1004 times.
|
81640 | if (!consumedCredit) { |
| 746 | 80636 | (void)this->m_handle.m_notEmptySem->tryWait(); | |
| 747 | } | ||
| 748 | 81385 | priority = testPriority; | |
| 749 | 81429 | return QueueInterface::Status::OP_OK; | |
| 750 | } | ||
| 751 | } | ||
| 752 | |||
| 753 | // No message found | ||
| 754 |
2/2✓ Branch 0 taken 1006 times.
✓ Branch 1 taken 135 times.
|
1141 | if (blockType == QueueInterface::BlockingType::BLOCKING) { |
| 755 | 1006 | FW_ASSERT(this->m_handle.m_notEmptySem != nullptr, this->m_handle.m_id); | |
| 756 | 1006 | Os::CountingSemaphoreInterface::Status semStatus = this->m_handle.m_notEmptySem->wait(); | |
| 757 | 1006 | FW_ASSERT(semStatus == Os::CountingSemaphoreInterface::Status::OP_OK, | |
| 758 | static_cast<FwAssertArgType>(semStatus)); | ||
| 759 | 1006 | consumedCredit = true; | |
| 760 | } else { | ||
| 761 | 135 | return QueueInterface::Status::EMPTY; | |
| 762 | } | ||
| 763 | } | ||
| 764 | |||
| 765 | // Should never reach here - loop guard prevents infinite loop | ||
| 766 | 240 | FW_ASSERT(false, this->m_handle.m_id, LOOP_GUARD_LIMIT); | |
| 767 | ✗ | return QueueInterface::Status::UNKNOWN_ERROR; | |
| 768 | } | ||
| 769 | |||
| 770 | 201 | FwSizeType PriorityMemQueue::getMessagesAvailable() const { | |
| 771 | 201 | FwSizeType total = 0; | |
| 772 | |||
| 773 |
1/2✓ Branch 4 taken 201 times.
✗ Branch 5 not taken.
|
201 | if (this->m_handle.m_atomicQueues != nullptr) { |
| 774 | 201 | FW_ASSERT(this->m_handle.m_numActivePriorities <= Os::Generic::Queue::MAX_PRIORITIES, | |
| 775 | static_cast<FwAssertArgType>(this->m_handle.m_numActivePriorities)); | ||
| 776 |
2/2✓ Branch 4 taken 597 times.
✓ Branch 5 taken 201 times.
|
798 | for (FwSizeType i = 0; i < this->m_handle.m_numActivePriorities; ++i) { |
| 777 | 597 | const Types::AtomicQueue* atomicQueue = &this->m_handle.m_atomicQueues[i]; | |
| 778 |
1/2✓ Branch 2 taken 597 times.
✗ Branch 3 not taken.
|
595 | if (atomicQueue->isCreated()) { |
| 779 | 597 | total += atomicQueue->getSize(); | |
| 780 | } | ||
| 781 | } | ||
| 782 | } | ||
| 783 | |||
| 784 | 201 | return total; | |
| 785 | } | ||
| 786 | |||
| 787 | 197 | 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 | 197 | U32 maxHwm = 0; | |
| 791 |
1/2✓ Branch 4 taken 197 times.
✗ Branch 5 not taken.
|
197 | if (this->m_handle.m_highWaterMarks != nullptr) { |
| 792 | 197 | FW_ASSERT(this->m_handle.m_numActivePriorities <= Os::Generic::Queue::MAX_PRIORITIES, | |
| 793 | static_cast<FwAssertArgType>(this->m_handle.m_numActivePriorities)); | ||
| 794 |
2/2✓ Branch 4 taken 591 times.
✓ Branch 5 taken 197 times.
|
788 | for (FwSizeType i = 0; i < this->m_handle.m_numActivePriorities; ++i) { |
| 795 | 591 | U32 hwm = this->m_handle.m_highWaterMarks[i].load(std::memory_order_acquire); | |
| 796 |
2/2✓ Branch 0 taken 151 times.
✓ Branch 1 taken 440 times.
|
591 | if (hwm > maxHwm) { |
| 797 | 151 | maxHwm = hwm; | |
| 798 | } | ||
| 799 | } | ||
| 800 | } | ||
| 801 | 197 | return static_cast<FwSizeType>(maxHwm); | |
| 802 | } | ||
| 803 | |||
| 804 | 87 | QueueHandle* PriorityMemQueue::getHandle() { | |
| 805 | 87 | return &this->m_handle; | |
| 806 | } | ||
| 807 | |||
| 808 | } // namespace Generic | ||
| 809 | } // namespace Os | ||
| 810 |