| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // ====================================================================== | ||
| 2 | // \title Os/Generic/PriorityMemQueue.hpp | ||
| 3 | // \author B. Duckett | ||
| 4 | // \brief hpp 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 | #ifndef OS_GENERIC_PRIORITYMEMQUEUE_HPP | ||
| 12 | #define OS_GENERIC_PRIORITYMEMQUEUE_HPP | ||
| 13 | |||
| 14 | #include <atomic> | ||
| 15 | #include "Fw/Types/MemAllocator.hpp" | ||
| 16 | #include "Os/CountingSemaphore.hpp" | ||
| 17 | #include "Os/Generic/Types/AtomicQueue.hpp" | ||
| 18 | #include "Os/Queue.hpp" | ||
| 19 | |||
| 20 | namespace Os { | ||
| 21 | namespace Generic { | ||
| 22 | |||
| 23 | // Constants | ||
| 24 | namespace Queue { | ||
| 25 | // Maximum number of priority levels supported per queue (0-31). | ||
| 26 | // Limited to 32 to: | ||
| 27 | // - Fit priority bitmask in a 32-bit atomic for efficient lock-free operations | ||
| 28 | // - Meet typical F' component needs (most use 2-4 priorities) | ||
| 29 | // WARNING: Standard F' components may use priorities outside this range. | ||
| 30 | // Ensure FPP priority assignments are within [0, MAX_PRIORITIES-1]. | ||
| 31 | constexpr static FwSizeType MAX_PRIORITIES = 32; | ||
| 32 | constexpr static FwSizeType DEFAULT_PRIORITY = 0; | ||
| 33 | } // namespace Queue | ||
| 34 | |||
| 35 | // Forward declarations | ||
| 36 | class PriorityMemQueue; | ||
| 37 | |||
| 38 | //! \brief critical data stored for priority queue | ||
| 39 | //! | ||
| 40 | //! The priority queue uses AtomicQueue for ISR-safe + SMP-safe operation. | ||
| 41 | //! Each priority has its own AtomicQueue. Priority tracking uses atomic | ||
| 42 | //! bitmasks and a counting semaphore for receive notification. | ||
| 43 | struct PriorityMemQueueHandle : public QueueHandle { | ||
| 44 | // Sparse priority support: map priority→index into m_atomicQueues | ||
| 45 | // Reduces memory waste when using non-consecutive priorities (e.g., {0, 15, 31}) | ||
| 46 | I8 m_priorityMap[Queue::MAX_PRIORITIES]; // Priority→index mapping (-1 = unused) | ||
| 47 | Types::AtomicQueue* m_atomicQueues = nullptr; // Array sized to actual configured priorities | ||
| 48 | |||
| 49 | // Rule of Five: prevent accidental copy/move of resource-owning handle | ||
| 50 | PriorityMemQueueHandle(const PriorityMemQueueHandle&) = delete; | ||
| 51 | PriorityMemQueueHandle& operator=(const PriorityMemQueueHandle&) = delete; | ||
| 52 | PriorityMemQueueHandle(PriorityMemQueueHandle&&) = delete; | ||
| 53 | PriorityMemQueueHandle& operator=(PriorityMemQueueHandle&&) = delete; | ||
| 54 | FwQueuePriorityType m_maxPriority = 0; // Highest priority value (for iteration) | ||
| 55 | FwSizeType m_numActivePriorities = 0; // Number of configured priorities | ||
| 56 | Os::CountingSemaphore* m_notEmptySem = nullptr; // Counting semaphore signaling messages available | ||
| 57 | FwEnumStoreType m_id = 0; // Queue identifier | ||
| 58 | FwEnumStoreType m_allocatorId = 0; // Allocator ID for memory operations | ||
| 59 | std::atomic<U32> m_priorityMask{0}; // Bit mask of enabled priorities | ||
| 60 | std::atomic<U32>* m_highWaterMarks = nullptr; // Array indexed by priority map | ||
| 61 | |||
| 62 | //! \brief Constructor | ||
| 63 | ✗ | PriorityMemQueueHandle() { | |
| 64 | // Initialize priority map to all -1 (unused) | ||
| 65 | ✗ | for (FwSizeType i = 0; i < Queue::MAX_PRIORITIES; ++i) { | |
| 66 | ✗ | m_priorityMap[i] = -1; | |
| 67 | } | ||
| 68 | ✗ | } | |
| 69 | |||
| 70 | //! \brief Initialize the handle | ||
| 71 | void init(); | ||
| 72 | |||
| 73 | //! \brief Allocate arrays for priority data (sparse allocation) | ||
| 74 | //! Uses m_priorityMap to determine which priorities are configured | ||
| 75 | //! \param allocator: memory allocator to use | ||
| 76 | //! \param allocatorId: ID for memory allocation | ||
| 77 | //! \return true if successful, false otherwise | ||
| 78 | bool allocateArrays(Fw::MemAllocator& allocator, FwEnumStoreType allocatorId); | ||
| 79 | |||
| 80 | //! \brief Deallocate arrays for priority data | ||
| 81 | //! \param allocator: memory allocator to use | ||
| 82 | //! \param allocatorId: ID for memory deallocation | ||
| 83 | void deallocateArrays(Fw::MemAllocator& allocator, FwEnumStoreType allocatorId); | ||
| 84 | |||
| 85 | //! \brief Enable a specific priority (in the handle ) | ||
| 86 | //! | ||
| 87 | //! \param priority: priority to enable | ||
| 88 | void enablePriority(FwQueuePriorityType priority); | ||
| 89 | |||
| 90 | //! \brief Disable a specific priority | ||
| 91 | //! | ||
| 92 | //! \param priority: priority to disable | ||
| 93 | void disablePriority(FwQueuePriorityType priority); | ||
| 94 | |||
| 95 | //! \brief Get array index for a priority (inline for performance) | ||
| 96 | //! \param priority: priority to look up | ||
| 97 | //! \return array index, or -1 if priority not configured | ||
| 98 | ✗ | I8 getPriorityIndex(FwQueuePriorityType priority) const { | |
| 99 | // Bounds check before array access (buffer overflow protection) | ||
| 100 | ✗ | FW_ASSERT(priority < Queue::MAX_PRIORITIES, priority); | |
| 101 | ✗ | return this->m_priorityMap[priority]; | |
| 102 | } | ||
| 103 | }; | ||
| 104 | //! \brief AtomicQueue-based priority queue implementation | ||
| 105 | //! | ||
| 106 | //! \warning This Priority Queue is ISR safe and SMP safe | ||
| 107 | //! | ||
| 108 | //! An OS-agnostic implementation of a priority queue using AtomicQueue for ISR-safe + SMP-safe operation. | ||
| 109 | //! Each priority has its own AtomicQueue. Uses counting semaphores for blocking operations. | ||
| 110 | //! | ||
| 111 | //! \warning allocates memory using MemAllocator | ||
| 112 | |||
| 113 | class PriorityMemQueue : public Os::QueueInterface { | ||
| 114 | public: | ||
| 115 | //! \brief Configuration for a priority level | ||
| 116 | struct QueuePriorityConfig { | ||
| 117 | FwQueuePriorityType priority; | ||
| 118 | FwSizeType maxMsgSize; | ||
| 119 | FwSizeType numMsgs; | ||
| 120 | }; | ||
| 121 | |||
| 122 | //!\brief Configuration for a queue with multiple priorities | ||
| 123 | struct QueueConfig { | ||
| 124 | FwEnumStoreType instanceId; // Per component creates pass in instance ID | ||
| 125 | FwSizeType numPriorities; // Number of priorities | ||
| 126 | QueuePriorityConfig* priorityConfigs; // Array of priority configurations | ||
| 127 | }; | ||
| 128 | //! \brief Register per-priority sizes for component instances which do | ||
| 129 | //! queueing with multiple priorities | ||
| 130 | //! \warning This function must be called before any queues are created | ||
| 131 | //! \note Queue size is specified separately from create() to avoid changes | ||
| 132 | //! to the Os::Queue interface | ||
| 133 | //! \param queueConfigs: configuration for the queue | ||
| 134 | //! \param numQueueConfigs: number of queue configurations | ||
| 135 | //! \param required: If true, fatal if a non-default priority is enqueued | ||
| 136 | //! \param allocatorId: ID to use for memory allocation | ||
| 137 | static void configure(QueueConfig* queueConfigs, | ||
| 138 | FwSizeType numQueueConfigs, | ||
| 139 | bool required, | ||
| 140 | FwEnumStoreType allocatorId); | ||
| 141 | |||
| 142 | //! \brief Reset static configuration (test environments only) | ||
| 143 | //! | ||
| 144 | //! Deallocates internal config tracking memory and resets state. | ||
| 145 | //! \warning Only call this in test harnesses after all queues are destroyed | ||
| 146 | static void resetConfig(); | ||
| 147 | |||
| 148 | public: | ||
| 149 | //! \brief queue interface constructor - initializes handle | ||
| 150 | PriorityMemQueue(); | ||
| 151 | |||
| 152 | //! \brief default queue destructor | ||
| 153 | virtual ~PriorityMemQueue(); | ||
| 154 | |||
| 155 | //! \brief copy constructor is forbidden | ||
| 156 | explicit PriorityMemQueue(const QueueInterface& other) = delete; | ||
| 157 | |||
| 158 | //! \brief copy constructor is forbidden | ||
| 159 | explicit PriorityMemQueue(const QueueInterface* other) = delete; | ||
| 160 | |||
| 161 | //! \brief assignment operator is forbidden | ||
| 162 | PriorityMemQueue& operator=(const QueueInterface& other) override = delete; | ||
| 163 | |||
| 164 | //! \brief create queue storage | ||
| 165 | //! | ||
| 166 | //! Creates a queue ensuring sufficient storage to hold `depth` messages of `messageSize` size each. | ||
| 167 | //! | ||
| 168 | //! \warning allocates memory through the memory allocator registry | ||
| 169 | //! | ||
| 170 | //! \param id: identifier for the queue, used for memory allocation | ||
| 171 | //! \param name: name of queue | ||
| 172 | //! \param depth: depth of queue in number of messages | ||
| 173 | //! \param messageSize: size of an individual message | ||
| 174 | //! \return: status of the creation | ||
| 175 | Status create(FwEnumStoreType id, | ||
| 176 | const Fw::ConstStringBase& name, | ||
| 177 | FwSizeType depth, | ||
| 178 | FwSizeType messageSize) override; | ||
| 179 | |||
| 180 | //! \brief teardown the queue | ||
| 181 | //! | ||
| 182 | //! Allow for queues to deallocate resources as part of system shutdown. This delegates to the underlying queue | ||
| 183 | //! implementation. | ||
| 184 | void teardown() override; | ||
| 185 | |||
| 186 | //! \brief teardown the queue | ||
| 187 | //! | ||
| 188 | //! Allow for queues to deallocate resources as part of system shutdown. This delegates to the underlying queue | ||
| 189 | //! implementation. | ||
| 190 | //! | ||
| 191 | //! Note: this is a helper to allow this to be called from the destructor. | ||
| 192 | void teardownInternal(); | ||
| 193 | |||
| 194 | //! \brief send a message into the queue | ||
| 195 | //! | ||
| 196 | //! Send a message into the queue, providing the message data, size, priority, and blocking type. When | ||
| 197 | //! `blockType` is set to BLOCKING, this call will block on queue full. Otherwise, this will return an error | ||
| 198 | //! status on queue full. | ||
| 199 | //! | ||
| 200 | //! \warning It is invalid to send a null buffer | ||
| 201 | //! \warning This method will block if the queue is full and blockType is set to BLOCKING | ||
| 202 | //! \warning A BLOCKING send can still return FULL: the underlying AtomicQueue bounds its | ||
| 203 | //! reserve-retry cycle, so sustained producer contention that loses every attempt | ||
| 204 | //! reports FULL rather than blocking indefinitely | ||
| 205 | //! | ||
| 206 | //! \param buffer: message data | ||
| 207 | //! \param size: size of message data | ||
| 208 | //! \param priority: priority of the message | ||
| 209 | //! \param blockType: BLOCKING to block for space or NONBLOCKING to return error when queue is full | ||
| 210 | //! \return: status of the send | ||
| 211 | Status send(const U8* buffer, FwSizeType size, FwQueuePriorityType priority, BlockingType blockType) override; | ||
| 212 | |||
| 213 | //! \brief receive a message from the queue | ||
| 214 | //! | ||
| 215 | //! Receive a message from the queue, providing the message destination, capacity, priority, and blocking type. | ||
| 216 | //! When `blockType` is set to BLOCKING, this call will block on queue empty. Otherwise, this will return an | ||
| 217 | //! error status on queue empty. Actual size received and priority of message is set on success status. | ||
| 218 | //! | ||
| 219 | //! \warning It is invalid to send a null buffer | ||
| 220 | //! \warning This method will block if the queue is empty and blockType is set to BLOCKING | ||
| 221 | //! | ||
| 222 | //! \param destination: destination for message data | ||
| 223 | //! \param capacity: maximum size of message data | ||
| 224 | //! \param blockType: BLOCKING to wait for message or NONBLOCKING to return error when queue is empty | ||
| 225 | //! \param actualSize: (output) actual size of message read | ||
| 226 | //! \param priority: (output) priority of message read | ||
| 227 | //! \return: status of the send | ||
| 228 | Status receive(U8* destination, | ||
| 229 | FwSizeType capacity, | ||
| 230 | BlockingType blockType, | ||
| 231 | FwSizeType& actualSize, | ||
| 232 | FwQueuePriorityType& priority) override; | ||
| 233 | |||
| 234 | //! \brief get number of messages available | ||
| 235 | //! | ||
| 236 | //! \return number of messages available | ||
| 237 | FwSizeType getMessagesAvailable() const override; | ||
| 238 | |||
| 239 | //! \brief get maximum messages stored at any given time | ||
| 240 | //! | ||
| 241 | //! Returns the maximum number of messages in this queue at any given time. This is the high-water mark for this | ||
| 242 | //! queue. | ||
| 243 | //! \return queue message high-water mark | ||
| 244 | FwSizeType getMessageHighWaterMark() const override; | ||
| 245 | |||
| 246 | QueueHandle* getHandle() override; | ||
| 247 | |||
| 248 | PriorityMemQueueHandle m_handle; | ||
| 249 | |||
| 250 | private: | ||
| 251 | //! \brief Static configuration storage | ||
| 252 | //! \warning must be initialized by config | ||
| 253 | static QueueConfig* s_configs; | ||
| 254 | static FwSizeType s_numConfigs; | ||
| 255 | static bool s_requirePrioritySizing; | ||
| 256 | static std::atomic<bool>* s_configsUsed; | ||
| 257 | static bool s_configured; | ||
| 258 | static FwEnumStoreType s_allocatorId; | ||
| 259 | |||
| 260 | private: | ||
| 261 | //! \brief Find the highest priority with available messages | ||
| 262 | //! \return highest priority with available messages, or MAX_PRIORITIES if none | ||
| 263 | FwQueuePriorityType findHighestPriority(U32 priorities = 0); | ||
| 264 | |||
| 265 | //! \brief Check if a priority is enabled | ||
| 266 | //! \param priority: priority to check | ||
| 267 | //! \return true if the priority is enabled, false otherwise | ||
| 268 | bool isPriorityEnabled(FwQueuePriorityType priority); | ||
| 269 | |||
| 270 | //! \brief Enable or disable a priority (internal) | ||
| 271 | //! \param priority: priority to enable or disable | ||
| 272 | //! \param enabled: true to enable, false to disable | ||
| 273 | void setPriorityEnabled(FwQueuePriorityType priority, bool enabled); | ||
| 274 | |||
| 275 | //! \brief Get the memory allocator for this queue | ||
| 276 | //! \return reference to the memory allocator | ||
| 277 | Fw::MemAllocator& getAllocator(); | ||
| 278 | |||
| 279 | private: | ||
| 280 | //! \brief Find a matching configuration for the given ID | ||
| 281 | //! | ||
| 282 | //! \param id: identifier for the queue | ||
| 283 | //! \return: pointer to the matching configuration, or nullptr if none found | ||
| 284 | QueueConfig* findMatchingConfig(FwEnumStoreType id); | ||
| 285 | |||
| 286 | //! \brief Create queues based on configuration | ||
| 287 | //! | ||
| 288 | //! \param queueConfig: configuration for the queue | ||
| 289 | //! \param allocator: memory allocator to use | ||
| 290 | //! \param allocatorId: ID to use for memory allocation | ||
| 291 | //! \return: status of the creation | ||
| 292 | QueueInterface::Status createConfiguredQueues(QueueConfig* queueConfig, | ||
| 293 | Fw::MemAllocator& allocator, | ||
| 294 | FwEnumStoreType allocatorId); | ||
| 295 | |||
| 296 | //! \brief Create a single default priority queue | ||
| 297 | //! | ||
| 298 | //! \param depth: depth of queue in number of messages | ||
| 299 | //! \param messageSize: size of an individual message | ||
| 300 | //! \param allocator: memory allocator to use | ||
| 301 | //! \param allocatorId: ID to use for memory allocation | ||
| 302 | //! \return: status of the creation | ||
| 303 | QueueInterface::Status createDefaultQueue(FwSizeType depth, | ||
| 304 | FwSizeType messageSize, | ||
| 305 | Fw::MemAllocator& allocator, | ||
| 306 | FwEnumStoreType allocatorId); | ||
| 307 | |||
| 308 | //! \brief Create a single priority queue | ||
| 309 | //! | ||
| 310 | //! \param priority: priority of the queue | ||
| 311 | //! \param maxMsgSize: maximum size of a message | ||
| 312 | //! \param numMsgs: number of messages to allocate space for | ||
| 313 | //! \param allocator: memory allocator to use | ||
| 314 | //! \param allocatorId: ID to use for memory allocation | ||
| 315 | //! \return: status of the creation | ||
| 316 | QueueInterface::Status createPriorityQueue(FwQueuePriorityType priority, | ||
| 317 | FwSizeType maxMsgSize, | ||
| 318 | FwSizeType numMsgs, | ||
| 319 | Fw::MemAllocator& allocator, | ||
| 320 | FwEnumStoreType allocatorId); | ||
| 321 | }; | ||
| 322 | } // namespace Generic | ||
| 323 | } // namespace Os | ||
| 324 | |||
| 325 | #endif // OS_GENERIC_PRIORITYMEMQUEUE_HPP | ||
| 326 |