| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // ====================================================================== | ||
| 2 | // \title ComQueue.cpp | ||
| 3 | // \author vbai | ||
| 4 | // \brief cpp file for ComQueue component implementation class | ||
| 5 | // ====================================================================== | ||
| 6 | |||
| 7 | #include <Fw/Com/ComPacket.hpp> | ||
| 8 | #include <Fw/Types/Assert.hpp> | ||
| 9 | #include <Svc/ComQueue/ComQueue.hpp> | ||
| 10 | #include <type_traits> | ||
| 11 | #include "Fw/Types/BasicTypes.hpp" | ||
| 12 | |||
| 13 | namespace Svc { | ||
| 14 | |||
| 15 | // ---------------------------------------------------------------------- | ||
| 16 | // Construction, initialization, and destruction | ||
| 17 | // ---------------------------------------------------------------------- | ||
| 18 | |||
| 19 | using FwUnsignedIndexType = std::make_unsigned<FwIndexType>::type; | ||
| 20 | |||
| 21 | 1 | ComQueue ::QueueConfigurationTable ::QueueConfigurationTable() { | |
| 22 | static_assert(static_cast<FwUnsignedIndexType>(std::numeric_limits<FwIndexType>::max()) >= | ||
| 23 | FW_NUM_ARRAY_ELEMENTS(this->entries), | ||
| 24 | "Number of entries must fit into FwIndexType"); | ||
| 25 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
|
4 | for (FwIndexType i = 0; i < static_cast<FwIndexType>(FW_NUM_ARRAY_ELEMENTS(this->entries)); i++) { |
| 26 | 3 | this->entries[i].priority = 0; | |
| 27 | 3 | this->entries[i].depth = 0; | |
| 28 | 3 | this->entries[i].mode = Types::QUEUE_FIFO; | |
| 29 | 3 | this->entries[i].overflowMode = Types::QUEUE_DROP_NEWEST; | |
| 30 | } | ||
| 31 | 1 | } | |
| 32 | |||
| 33 | 1 | ComQueue ::ComQueue(const char* const compName) | |
| 34 | : ComQueueComponentBase(compName), | ||
| 35 | 1 | m_state(WAITING), | |
| 36 | 1 | m_buffer_state(OWNED), | |
| 37 | 1 | m_allocationId(static_cast<FwEnumStoreType>(-1)), | |
| 38 | 1 | m_allocator(nullptr), | |
| 39 |
4/4✓ Branch 2 taken 1 times.
✓ Branch 5 taken 3 times.
✓ Branch 7 taken 3 times.
✓ Branch 8 taken 1 times.
|
4 | m_allocation(nullptr) { |
| 40 | // Initialize throttles to "off" | ||
| 41 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
|
4 | for (FwIndexType i = 0; i < TOTAL_PORT_COUNT; i++) { |
| 42 | 3 | this->m_throttle[i] = false; | |
| 43 | } | ||
| 44 | |||
| 45 | static_assert(TOTAL_PORT_COUNT >= 1, "ComQueue must have more than one port"); | ||
| 46 | 1 | } | |
| 47 | |||
| 48 | 2 | ComQueue ::~ComQueue() {} | |
| 49 | |||
| 50 | 1 | void ComQueue ::cleanup() { | |
| 51 | // Deallocate memory ignoring error conditions | ||
| 52 |
2/4✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | if ((this->m_allocator != nullptr) && (this->m_allocation != nullptr)) { |
| 53 | 1 | this->m_allocator->deallocate(this->m_allocationId, this->m_allocation); | |
| 54 | } | ||
| 55 | 1 | } | |
| 56 | |||
| 57 | 1 | void ComQueue::configure(const QueueConfigurationTable& queueConfig, | |
| 58 | FwEnumStoreType allocationId, | ||
| 59 | Fw::MemAllocator& allocator) { | ||
| 60 | 1 | FwIndexType currentPriorityIndex = 0; | |
| 61 | 1 | FwSizeType totalAllocation = 0; | |
| 62 | |||
| 63 | // Store/initialize allocator members | ||
| 64 | 1 | this->m_allocator = &allocator; | |
| 65 | 1 | this->m_allocationId = allocationId; | |
| 66 | 1 | this->m_allocation = nullptr; | |
| 67 | |||
| 68 | // Initializes the sorted queue metadata list in priority (sorted) order. This is accomplished by walking the | ||
| 69 | // priority values in priority order from 0 to TOTAL_PORT_COUNT. At each priory value, the supplied queue | ||
| 70 | // configuration table is walked and any entry matching the current priority values is used to add queue metadata to | ||
| 71 | // the prioritized list. This results in priority-sorted queue metadata objects that index back into the unsorted | ||
| 72 | // queue data structures. | ||
| 73 | // | ||
| 74 | // The total allocation size is tracked for passing to the allocation call and is a summation of | ||
| 75 | // (depth * message size) for each prioritized metadata object of (depth * message size) | ||
| 76 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
|
4 | for (FwIndexType currentPriority = 0; currentPriority < TOTAL_PORT_COUNT; currentPriority++) { |
| 77 | // Walk each queue configuration entry and add them into the prioritized metadata list when matching the current | ||
| 78 | // priority value | ||
| 79 | 12 | for (FwIndexType entryIndex = 0; | |
| 80 |
2/2✓ Branch 1 taken 9 times.
✓ Branch 2 taken 3 times.
|
12 | entryIndex < static_cast<FwIndexType>(FW_NUM_ARRAY_ELEMENTS(queueConfig.entries)); entryIndex++) { |
| 81 | // Check for valid configuration entry | ||
| 82 | 9 | FW_ASSERT(queueConfig.entries[entryIndex].priority < TOTAL_PORT_COUNT, | |
| 83 | static_cast<FwAssertArgType>(queueConfig.entries[entryIndex].priority), | ||
| 84 | static_cast<FwAssertArgType>(TOTAL_PORT_COUNT), static_cast<FwAssertArgType>(entryIndex)); | ||
| 85 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 6 times.
|
9 | if (currentPriority == queueConfig.entries[entryIndex].priority) { |
| 86 | // Set up the queue metadata object in order to track priority, depth, index into the queue list of the | ||
| 87 | // backing queue object, and message size. Both index and message size are calculated where priority and | ||
| 88 | // depth are copied from the configuration object. | ||
| 89 | 3 | QueueMetadata& entry = this->m_prioritizedList[currentPriorityIndex]; | |
| 90 | 3 | entry.priority = queueConfig.entries[entryIndex].priority; | |
| 91 | 3 | entry.depth = queueConfig.entries[entryIndex].depth; | |
| 92 | 3 | entry.mode = queueConfig.entries[entryIndex].mode; | |
| 93 | 3 | entry.overflowMode = queueConfig.entries[entryIndex].overflowMode; | |
| 94 | 3 | entry.index = entryIndex; | |
| 95 | // Message size is determined by the type of object being stored, which in turn is determined by the | ||
| 96 | // index of the entry. Those lower than COM_PORT_COUNT are Fw::ComBuffers and those larger Fw::Buffer. | ||
| 97 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
|
3 | entry.msgSize = (entryIndex < COM_PORT_COUNT) ? static_cast<FwSizeType>(Fw::ComBuffer::SERIALIZED_SIZE) |
| 98 | : static_cast<FwSizeType>(Fw::Buffer::SERIALIZED_SIZE); | ||
| 99 | // Overflow checks. A depth of 0 disables the queue and contributes no storage. | ||
| 100 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
3 | if (entry.depth > 0) { |
| 101 | 3 | FW_ASSERT((std::numeric_limits<FwSizeType>::max() / entry.depth) >= entry.msgSize, | |
| 102 | static_cast<FwAssertArgType>(entry.depth), static_cast<FwAssertArgType>(entry.msgSize)); | ||
| 103 | 3 | FW_ASSERT(std::numeric_limits<FwSizeType>::max() - (entry.depth * entry.msgSize) >= | |
| 104 | totalAllocation); | ||
| 105 | 3 | totalAllocation += entry.depth * entry.msgSize; | |
| 106 | } | ||
| 107 | 3 | currentPriorityIndex++; | |
| 108 | } | ||
| 109 | } | ||
| 110 | } | ||
| 111 | // At least one queue must be enabled; an all-zero table is the default-constructed (unconfigured) table | ||
| 112 | 1 | FW_ASSERT(totalAllocation > 0); | |
| 113 | // Allocate a single chunk of memory from the memory allocator. Memory recover is neither needed nor used. | ||
| 114 | 1 | bool recoverable = false; | |
| 115 | 1 | FwSizeType actualAllocation = totalAllocation; | |
| 116 |
1/1✓ Branch 1 taken 1 times.
|
1 | this->m_allocation = this->m_allocator->allocate(this->m_allocationId, actualAllocation, recoverable); |
| 117 | 1 | FW_ASSERT(this->m_allocation != nullptr); | |
| 118 | 1 | FW_ASSERT(actualAllocation >= totalAllocation, static_cast<FwAssertArgType>(actualAllocation), | |
| 119 | static_cast<FwAssertArgType>(totalAllocation)); | ||
| 120 | |||
| 121 | // Each of the backing queue objects must be supplied memory to store the queued messages. These data regions are | ||
| 122 | // sub-portions of the total allocated data. This memory is passed out by looping through each queue in prioritized | ||
| 123 | // order and passing out the memory to each queue's setup method. | ||
| 124 | 1 | FwSizeType allocationOffset = 0; | |
| 125 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
|
4 | for (FwIndexType i = 0; i < TOTAL_PORT_COUNT; i++) { |
| 126 | // Get current queue's allocation size and safety check the values | ||
| 127 | 3 | FwSizeType allocationSize = this->m_prioritizedList[i].depth * this->m_prioritizedList[i].msgSize; | |
| 128 | 3 | FW_ASSERT(this->m_prioritizedList[i].index < static_cast<FwIndexType>(FW_NUM_ARRAY_ELEMENTS(this->m_queues)), | |
| 129 | static_cast<FwAssertArgType>(this->m_prioritizedList[i].index)); | ||
| 130 | 3 | FW_ASSERT((allocationSize + allocationOffset) <= totalAllocation, static_cast<FwAssertArgType>(allocationSize), | |
| 131 | static_cast<FwAssertArgType>(allocationOffset), static_cast<FwAssertArgType>(totalAllocation)); | ||
| 132 | |||
| 133 | // Setup queue's memory allocation, depth, and message size. Setup is skipped for a disabled (depth 0) queue | ||
| 134 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
3 | if (this->m_prioritizedList[i].depth > 0) { |
| 135 | 3 | this->m_queues[this->m_prioritizedList[i].index].setup( | |
| 136 | 3 | reinterpret_cast<U8*>(this->m_allocation) + allocationOffset, allocationSize, | |
| 137 | 3 | this->m_prioritizedList[i].depth, this->m_prioritizedList[i].msgSize, this->m_prioritizedList[i].mode, | |
| 138 |
1/1✓ Branch 1 taken 3 times.
|
3 | this->m_prioritizedList[i].overflowMode); |
| 139 | } | ||
| 140 | 3 | allocationOffset += allocationSize; | |
| 141 | } | ||
| 142 | // Safety check that all memory was used as expected | ||
| 143 | 1 | FW_ASSERT(allocationOffset == totalAllocation, static_cast<FwAssertArgType>(allocationOffset), | |
| 144 | static_cast<FwAssertArgType>(totalAllocation)); | ||
| 145 | 1 | } | |
| 146 | |||
| 147 | // ---------------------------------------------------------------------- | ||
| 148 | // Handler implementations for commands | ||
| 149 | // ---------------------------------------------------------------------- | ||
| 150 | |||
| 151 | ✗ | void ComQueue ::FLUSH_QUEUE_cmdHandler(FwOpcodeType opCode, | |
| 152 | U32 cmdSeq, | ||
| 153 | const Svc::QueueType& queueType, | ||
| 154 | FwIndexType index) { | ||
| 155 | // Acquire the queue that we need to drain | ||
| 156 | ✗ | FwIndexType queueIndex = this->getQueueNum(queueType, index); | |
| 157 | |||
| 158 | // Validate queue index | ||
| 159 | ✗ | if (queueIndex < 0 || queueIndex >= TOTAL_PORT_COUNT) { | |
| 160 | ✗ | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::VALIDATION_ERROR); | |
| 161 | ✗ | return; | |
| 162 | } | ||
| 163 | ✗ | FW_ASSERT(queueIndex >= 0 && queueIndex < TOTAL_PORT_COUNT, static_cast<FwAssertArgType>(queueIndex)); | |
| 164 | |||
| 165 | ✗ | this->drainQueue(queueIndex); | |
| 166 | ✗ | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); | |
| 167 | } | ||
| 168 | |||
| 169 | ✗ | void ComQueue ::FLUSH_ALL_QUEUES_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) { | |
| 170 | ✗ | for (FwIndexType i = 0; i < TOTAL_PORT_COUNT; i++) { | |
| 171 | ✗ | this->drainQueue(i); | |
| 172 | } | ||
| 173 | ✗ | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); | |
| 174 | ✗ | } | |
| 175 | |||
| 176 | ✗ | void ComQueue::SET_QUEUE_PRIORITY_cmdHandler(FwOpcodeType opCode, | |
| 177 | U32 cmdSeq, | ||
| 178 | const Svc::QueueType& queueType, | ||
| 179 | FwIndexType index, | ||
| 180 | FwIndexType newPriority) { | ||
| 181 | // Acquire the queue we are to reprioritize | ||
| 182 | ✗ | FwIndexType queueIndex = this->getQueueNum(queueType, index); | |
| 183 | |||
| 184 | // Validate queue index | ||
| 185 | ✗ | if (queueIndex < 0 || queueIndex >= TOTAL_PORT_COUNT) { | |
| 186 | ✗ | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::VALIDATION_ERROR); | |
| 187 | ✗ | return; | |
| 188 | } | ||
| 189 | |||
| 190 | // Validate priority range | ||
| 191 | ✗ | if (newPriority < 0 || newPriority >= TOTAL_PORT_COUNT) { | |
| 192 | ✗ | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::VALIDATION_ERROR); | |
| 193 | ✗ | return; | |
| 194 | } | ||
| 195 | |||
| 196 | // Find our queue in the prioritized list & update the priority | ||
| 197 | ✗ | for (FwIndexType prioIndex = 0; prioIndex < TOTAL_PORT_COUNT; prioIndex++) { | |
| 198 | // Each entry must reference a valid queue index | ||
| 199 | ✗ | FW_ASSERT(m_prioritizedList[prioIndex].index >= 0 && m_prioritizedList[prioIndex].index < TOTAL_PORT_COUNT, | |
| 200 | static_cast<FwAssertArgType>(m_prioritizedList[prioIndex].index)); | ||
| 201 | // If the port based index matches, then update | ||
| 202 | ✗ | if (m_prioritizedList[prioIndex].index == queueIndex) { | |
| 203 | ✗ | m_prioritizedList[prioIndex].priority = newPriority; | |
| 204 | ✗ | break; // Since we shouldn't find more than one queue at this port index | |
| 205 | } | ||
| 206 | } | ||
| 207 | |||
| 208 | // Re-sort the prioritized list to maintain priority ordering | ||
| 209 | // Using simple bubble sort since TOTAL_PORT_COUNT is typically small | ||
| 210 | ✗ | for (FwIndexType i = 0; i < TOTAL_PORT_COUNT - 1; i++) { | |
| 211 | ✗ | for (FwIndexType j = 0; (j < TOTAL_PORT_COUNT - i - 1) && (j < TOTAL_PORT_COUNT - 1); j++) { | |
| 212 | ✗ | if (m_prioritizedList[j].priority > m_prioritizedList[j + 1].priority) { | |
| 213 | // Swap metadata | ||
| 214 | ✗ | QueueMetadata temp = m_prioritizedList[j]; | |
| 215 | ✗ | m_prioritizedList[j] = m_prioritizedList[j + 1]; | |
| 216 | ✗ | m_prioritizedList[j + 1] = temp; | |
| 217 | } | ||
| 218 | } | ||
| 219 | } | ||
| 220 | |||
| 221 | // Emit event for successful priority change | ||
| 222 | ✗ | this->log_ACTIVITY_HI_QueuePriorityChanged(queueType, index, newPriority); | |
| 223 | |||
| 224 | // Send command response | ||
| 225 | ✗ | this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK); | |
| 226 | } | ||
| 227 | |||
| 228 | // ---------------------------------------------------------------------- | ||
| 229 | // Handler implementations for user-defined typed input ports | ||
| 230 | // ---------------------------------------------------------------------- | ||
| 231 | |||
| 232 | 703 | void ComQueue::comPacketQueueIn_handler(const FwIndexType portNum, Fw::ComBuffer& data, U32 context) { | |
| 233 | // Ensure that the port number of comPacketQueueIn is consistent with the expectation | ||
| 234 | 703 | FW_ASSERT(portNum >= 0 && portNum < COM_PORT_COUNT, static_cast<FwAssertArgType>(portNum)); | |
| 235 | 703 | (void)this->enqueue(portNum, data); | |
| 236 | 703 | } | |
| 237 | |||
| 238 | ✗ | void ComQueue::bufferQueueIn_handler(const FwIndexType portNum, Fw::Buffer& fwBuffer) { | |
| 239 | ✗ | FW_ASSERT(std::numeric_limits<FwIndexType>::max() - COM_PORT_COUNT > portNum); | |
| 240 | ✗ | const FwIndexType queueNum = static_cast<FwIndexType>(portNum + COM_PORT_COUNT); | |
| 241 | // Ensure that the port number of bufferQueueIn is consistent with the expectation | ||
| 242 | ✗ | FW_ASSERT(portNum >= 0 && portNum < BUFFER_PORT_COUNT, static_cast<FwAssertArgType>(portNum)); | |
| 243 | ✗ | FW_ASSERT(queueNum < TOTAL_PORT_COUNT); | |
| 244 | ✗ | bool success = this->enqueue(queueNum, fwBuffer); | |
| 245 | ✗ | if (!success) { | |
| 246 | ✗ | this->bufferReturnOut_out(portNum, fwBuffer); | |
| 247 | } | ||
| 248 | ✗ | } | |
| 249 | |||
| 250 | 704 | void ComQueue::comStatusIn_handler(const FwIndexType portNum, Fw::Success& condition) { | |
| 251 |
1/2✓ Branch 0 taken 704 times.
✗ Branch 1 not taken.
|
704 | switch (this->m_state) { |
| 252 | // On success, the queue should be processed. On failure, the component should still wait. | ||
| 253 | 704 | case WAITING: | |
| 254 |
1/2✓ Branch 0 taken 704 times.
✗ Branch 1 not taken.
|
704 | if (condition.e == Fw::Success::SUCCESS) { |
| 255 | 704 | this->m_state = READY; | |
| 256 | 704 | this->processQueue(); | |
| 257 | // A message may or may not be sent. Thus, READY or WAITING are acceptable final states. | ||
| 258 | 704 | FW_ASSERT((this->m_state == WAITING || this->m_state == READY), | |
| 259 | static_cast<FwAssertArgType>(this->m_state)); | ||
| 260 | } else { | ||
| 261 | ✗ | this->m_state = WAITING; | |
| 262 | } | ||
| 263 | 704 | break; | |
| 264 | // Both READY and unknown states should not be possible at this point. To receive a status message we must be | ||
| 265 | // one of the WAITING or RETRY states. | ||
| 266 | ✗ | default: | |
| 267 | ✗ | FW_ASSERT(false, static_cast<FwAssertArgType>(this->m_state)); | |
| 268 | ✗ | break; | |
| 269 | } | ||
| 270 | 704 | } | |
| 271 | |||
| 272 | 241 | void ComQueue::run_handler(const FwIndexType portNum, U32 context) { | |
| 273 | // Downlink the high-water marks for the Fw::ComBuffer array types. Disabled (depth 0) queues report 0. | ||
| 274 |
1/1✓ Branch 1 taken 241 times.
|
241 | ComQueueDepth comQueueDepth; |
| 275 | FW_ASSERT(comQueueDepth.SIZE <= COM_PORT_COUNT, static_cast<FwAssertArgType>(comQueueDepth.SIZE)); | ||
| 276 |
2/2✓ Branch 0 taken 482 times.
✓ Branch 1 taken 241 times.
|
723 | for (U32 i = 0; i < comQueueDepth.SIZE; i++) { |
| 277 | 482 | const FwIndexType queueNum = static_cast<FwIndexType>(i); | |
| 278 |
1/1✓ Branch 1 taken 482 times.
|
482 | comQueueDepth[i] = 0; |
| 279 |
2/3✓ Branch 1 taken 482 times.
✓ Branch 3 taken 482 times.
✗ Branch 4 not taken.
|
482 | if (this->getQueueDepth(queueNum) > 0) { |
| 280 |
2/2✓ Branch 1 taken 482 times.
✓ Branch 4 taken 482 times.
|
482 | comQueueDepth[i] = static_cast<U32>(this->m_queues[queueNum].get_high_water_mark()); |
| 281 |
1/1✓ Branch 1 taken 482 times.
|
482 | this->m_queues[queueNum].clear_high_water_mark(); |
| 282 | } | ||
| 283 | } | ||
| 284 |
2/2✓ Branch 1 taken 241 times.
✓ Branch 4 taken 241 times.
|
241 | this->tlmWrite_comQueueDepth(comQueueDepth); |
| 285 | |||
| 286 | // Downlink the high-water marks for the Fw::Buffer array types | ||
| 287 |
1/1✓ Branch 1 taken 241 times.
|
241 | BuffQueueDepth buffQueueDepth; |
| 288 | FW_ASSERT((buffQueueDepth.SIZE + COM_PORT_COUNT) <= TOTAL_PORT_COUNT, | ||
| 289 | static_cast<FwAssertArgType>(buffQueueDepth.SIZE)); | ||
| 290 |
2/2✓ Branch 0 taken 241 times.
✓ Branch 1 taken 241 times.
|
482 | for (U32 i = 0; i < buffQueueDepth.SIZE; i++) { |
| 291 | 241 | const FwIndexType queueNum = static_cast<FwIndexType>(i + COM_PORT_COUNT); | |
| 292 |
1/1✓ Branch 1 taken 241 times.
|
241 | buffQueueDepth[i] = 0; |
| 293 |
2/3✓ Branch 1 taken 241 times.
✓ Branch 3 taken 241 times.
✗ Branch 4 not taken.
|
241 | if (this->getQueueDepth(queueNum) > 0) { |
| 294 |
2/2✓ Branch 1 taken 241 times.
✓ Branch 4 taken 241 times.
|
241 | buffQueueDepth[i] = static_cast<U32>(this->m_queues[queueNum].get_high_water_mark()); |
| 295 |
1/1✓ Branch 1 taken 241 times.
|
241 | this->m_queues[queueNum].clear_high_water_mark(); |
| 296 | } | ||
| 297 | } | ||
| 298 |
2/2✓ Branch 1 taken 241 times.
✓ Branch 4 taken 241 times.
|
241 | this->tlmWrite_buffQueueDepth(buffQueueDepth); |
| 299 | 241 | } | |
| 300 | |||
| 301 | 703 | void ComQueue ::dataReturnIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { | |
| 302 | static_assert(std::numeric_limits<FwIndexType>::is_signed, "FwIndexType must be signed"); | ||
| 303 | // This handler runs on the returning caller's thread: take ownership atomically | ||
| 304 | 703 | const BufferState previousState = this->m_buffer_state.exchange(OWNED); | |
| 305 | 703 | FW_ASSERT(previousState == UNOWNED, static_cast<FwAssertArgType>(previousState)); | |
| 306 | // For the buffer queues, the index of the queue is portNum offset by COM_PORT_COUNT since | ||
| 307 | // the first COM_PORT_COUNT queues are for ComBuffer. So we have for buffer queues: | ||
| 308 | // queueNum = portNum + COM_PORT_COUNT | ||
| 309 | // Since queueNum is used as APID, we can retrieve the original portNum like such: | ||
| 310 | 703 | FwIndexType bufferReturnPortNum = static_cast<FwIndexType>(context.get_comQueueIndex() - ComQueue::COM_PORT_COUNT); | |
| 311 | // Failing this assert means that context.apid was modified since ComQueue set it, which should not happen | ||
| 312 | 703 | FW_ASSERT(bufferReturnPortNum < BUFFER_PORT_COUNT, static_cast<FwAssertArgType>(bufferReturnPortNum)); | |
| 313 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 703 times.
|
703 | if (bufferReturnPortNum >= 0) { |
| 314 | // It is a coding error not to connect the associated bufferReturnOut port for each dataReturnIn port | ||
| 315 | ✗ | FW_ASSERT(this->isConnected_bufferReturnOut_OutputPort(bufferReturnPortNum), | |
| 316 | static_cast<FwAssertArgType>(bufferReturnPortNum)); | ||
| 317 | // If this is a buffer port, return the buffer to the BufferDownlink | ||
| 318 | ✗ | this->bufferReturnOut_out(bufferReturnPortNum, data); | |
| 319 | } | ||
| 320 | 703 | } | |
| 321 | |||
| 322 | // ---------------------------------------------------------------------- | ||
| 323 | // Hook implementations for typed async input ports | ||
| 324 | // ---------------------------------------------------------------------- | ||
| 325 | |||
| 326 | ✗ | void ComQueue::bufferQueueIn_overflowHook(FwIndexType portNum, Fw::Buffer& fwBuffer) { | |
| 327 | ✗ | FW_ASSERT(portNum >= 0 && portNum < BUFFER_PORT_COUNT, static_cast<FwAssertArgType>(portNum)); | |
| 328 | ✗ | this->bufferReturnOut_out(portNum, fwBuffer); | |
| 329 | ✗ | } | |
| 330 | |||
| 331 | // ---------------------------------------------------------------------- | ||
| 332 | // Private helper methods | ||
| 333 | // ---------------------------------------------------------------------- | ||
| 334 | |||
| 335 | 703 | bool ComQueue::enqueue(const FwIndexType queueNum, const Fw::ComBuffer& data) { | |
| 336 | // Enqueue the given message onto the matching queue. When no space is available then emit the queue overflow event, | ||
| 337 | // set the appropriate throttle, and move on. A disabled (depth 0) queue has no space and always overflows. | ||
| 338 | 703 | FW_ASSERT(queueNum >= 0 && queueNum < COM_PORT_COUNT, static_cast<FwAssertArgType>(queueNum)); | |
| 339 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 703 times.
|
703 | if (this->getQueueDepth(queueNum) == 0) { |
| 340 | ✗ | return this->handleEnqueueStatus(queueNum, QueueType::COM_QUEUE, queueNum, false, | |
| 341 | ✗ | Fw::FW_SERIALIZE_NO_ROOM_LEFT); | |
| 342 | } | ||
| 343 | |||
| 344 | 703 | const Fw::SerializeStatus status = this->m_queues[queueNum].enqueue(data); | |
| 345 |
2/2✓ Branch 1 taken 703 times.
✓ Branch 4 taken 703 times.
|
703 | return this->handleEnqueueStatus(queueNum, QueueType::COM_QUEUE, queueNum, false, status); |
| 346 | } | ||
| 347 | |||
| 348 | ✗ | bool ComQueue::enqueue(const FwIndexType queueNum, const Fw::Buffer& data) { | |
| 349 | // Enqueue the given message onto the matching queue. When no space is available then emit the queue overflow event, | ||
| 350 | // set the appropriate throttle, and move on. A disabled (depth 0) queue has no space and always overflows. | ||
| 351 | ✗ | FW_ASSERT(queueNum >= COM_PORT_COUNT && queueNum < TOTAL_PORT_COUNT, static_cast<FwAssertArgType>(queueNum)); | |
| 352 | ✗ | const FwIndexType portNum = static_cast<FwIndexType>(queueNum - COM_PORT_COUNT); | |
| 353 | ✗ | if (this->getQueueDepth(queueNum) == 0) { | |
| 354 | ✗ | return this->handleEnqueueStatus(queueNum, QueueType::BUFFER_QUEUE, portNum, false, | |
| 355 | ✗ | Fw::FW_SERIALIZE_NO_ROOM_LEFT); | |
| 356 | } | ||
| 357 | |||
| 358 | // For buffer queues with DROP_OLDEST, check if the queue is full before enqueuing. | ||
| 359 | // If full, dequeue the oldest entry first so we can return buffer ownership before | ||
| 360 | // Queue::enqueue() silently discards it via rotate. This prevents buffer-pool leaks. | ||
| 361 | ✗ | bool preEmptiveOverflow = false; | |
| 362 | ✗ | Types::Queue& queue = this->m_queues[queueNum]; | |
| 363 | ✗ | for (FwIndexType i = 0; i < TOTAL_PORT_COUNT; i++) { | |
| 364 | ✗ | if (this->m_prioritizedList[i].index == queueNum && | |
| 365 | ✗ | this->m_prioritizedList[i].overflowMode == Types::QUEUE_DROP_OLDEST && | |
| 366 | ✗ | queue.getQueueSize() >= this->m_prioritizedList[i].depth) { | |
| 367 | // Queue is full and will drop oldest; remove the front entry to return ownership. | ||
| 368 | // popFront() always removes from the front (oldest) regardless of queue mode, | ||
| 369 | // matching the rotate-based removal that Queue::enqueue() uses for DROP_OLDEST. | ||
| 370 | ✗ | Fw::Buffer droppedBuffer; | |
| 371 | ✗ | Fw::SerializeStatus dequeueStatus = queue.popFront(droppedBuffer); | |
| 372 | ✗ | FW_ASSERT(dequeueStatus == Fw::FW_SERIALIZE_OK, static_cast<FwAssertArgType>(dequeueStatus)); | |
| 373 | ✗ | this->bufferReturnOut_out(portNum, droppedBuffer); | |
| 374 | ✗ | preEmptiveOverflow = true; | |
| 375 | ✗ | break; | |
| 376 | ✗ | } | |
| 377 | } | ||
| 378 | |||
| 379 | ✗ | const Fw::SerializeStatus status = this->m_queues[queueNum].enqueue(data); | |
| 380 | ✗ | return this->handleEnqueueStatus(queueNum, QueueType::BUFFER_QUEUE, portNum, preEmptiveOverflow, status); | |
| 381 | } | ||
| 382 | |||
| 383 | 703 | bool ComQueue::handleEnqueueStatus(const FwIndexType queueNum, | |
| 384 | QueueType queueType, | ||
| 385 | const FwIndexType portNum, | ||
| 386 | const bool preEmptiveOverflow, | ||
| 387 | const Fw::SerializeStatus status) { | ||
| 388 |
3/6✓ Branch 0 taken 703 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 703 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 703 times.
|
703 | if (preEmptiveOverflow || status == Fw::FW_SERIALIZE_NO_ROOM_LEFT || |
| 389 | status == Fw::FW_SERIALIZE_DISCARDED_EXISTING) { | ||
| 390 | ✗ | if (!this->m_throttle[queueNum]) { | |
| 391 | ✗ | this->log_WARNING_HI_QueueOverflow(queueType, portNum); | |
| 392 | ✗ | this->m_throttle[queueNum] = true; | |
| 393 | } | ||
| 394 | } | ||
| 395 | |||
| 396 | // When the component is already in READY state process the queue to send out the next available message immediately | ||
| 397 |
2/2✓ Branch 0 taken 395 times.
✓ Branch 1 taken 308 times.
|
703 | if (this->m_state == READY) { |
| 398 | 395 | this->processQueue(); | |
| 399 | } | ||
| 400 | |||
| 401 | // Check if the buffer was accepted or must be returned | ||
| 402 | 703 | return status != Fw::FW_SERIALIZE_NO_ROOM_LEFT; | |
| 403 | } | ||
| 404 | |||
| 405 | 703 | void ComQueue::sendComBuffer(Fw::ComBuffer& comBuffer, FwIndexType queueIndex) { | |
| 406 | 703 | FW_ASSERT(this->m_state == READY); | |
| 407 |
2/2✓ Branch 1 taken 703 times.
✓ Branch 5 taken 703 times.
|
703 | Fw::Buffer outBuffer(comBuffer.getBuffAddr(), static_cast<Fw::Buffer::SizeType>(comBuffer.getSize())); |
| 408 | |||
| 409 | // Context value is used to determine what to do when the buffer returns on the dataReturnIn port | ||
| 410 |
1/1✓ Branch 1 taken 703 times.
|
703 | ComCfg::FrameContext context; |
| 411 | 703 | FwPacketDescriptorType descriptor = 0; | |
| 412 |
1/1✓ Branch 1 taken 703 times.
|
703 | Fw::SerializeStatus status = comBuffer.deserializeTo(descriptor); |
| 413 | 703 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, static_cast<FwAssertArgType>(status)); | |
| 414 |
1/1✓ Branch 1 taken 703 times.
|
703 | context.set_apid(static_cast<ComCfg::Apid::T>(descriptor)); |
| 415 |
1/1✓ Branch 1 taken 703 times.
|
703 | context.set_comQueueIndex(queueIndex); |
| 416 | 703 | const BufferState previousState = this->m_buffer_state.exchange(UNOWNED); | |
| 417 | 703 | FW_ASSERT(previousState == OWNED, static_cast<FwAssertArgType>(previousState)); | |
| 418 |
1/1✓ Branch 1 taken 703 times.
|
703 | this->dataOut_out(0, outBuffer, context); |
| 419 | // Set state to WAITING for the status to come back | ||
| 420 | 703 | this->m_state = WAITING; | |
| 421 | 703 | } | |
| 422 | |||
| 423 | ✗ | void ComQueue::sendBuffer(Fw::Buffer& buffer, FwIndexType queueIndex) { | |
| 424 | // Retry buffer expected to be cleared as we are either transferring ownership or have already deallocated it. | ||
| 425 | ✗ | FW_ASSERT(this->m_state == READY); | |
| 426 | |||
| 427 | // Context value is used to determine what to do when the buffer returns on the dataReturnIn port | ||
| 428 | ✗ | ComCfg::FrameContext context; | |
| 429 | FwPacketDescriptorType descriptor; | ||
| 430 | ✗ | Fw::SerializeStatus status = buffer.getDeserializer().deserializeTo(descriptor); | |
| 431 | ✗ | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, static_cast<FwAssertArgType>(status)); | |
| 432 | ✗ | context.set_apid(static_cast<ComCfg::Apid::T>(descriptor)); | |
| 433 | ✗ | context.set_comQueueIndex(queueIndex); | |
| 434 | ✗ | const BufferState previousState = this->m_buffer_state.exchange(UNOWNED); | |
| 435 | ✗ | FW_ASSERT(previousState == OWNED, static_cast<FwAssertArgType>(previousState)); | |
| 436 | ✗ | this->dataOut_out(0, buffer, context); | |
| 437 | // Set state to WAITING for the status to come back | ||
| 438 | ✗ | this->m_state = WAITING; | |
| 439 | ✗ | } | |
| 440 | |||
| 441 | ✗ | void ComQueue::drainQueue(FwIndexType index) { | |
| 442 | ✗ | FW_ASSERT(index >= 0 && index < TOTAL_PORT_COUNT, static_cast<FwAssertArgType>(index)); | |
| 443 | // A disabled (depth 0) queue holds no messages and has no backing storage to drain | ||
| 444 | ✗ | if (this->getQueueDepth(index) == 0) { | |
| 445 | ✗ | return; | |
| 446 | } | ||
| 447 | ✗ | Types::Queue& queue = this->m_queues[index]; | |
| 448 | |||
| 449 | // Read all messages from the queue and discard them | ||
| 450 | ✗ | Fw::SerializeStatus status = Fw::FW_SERIALIZE_OK; | |
| 451 | ✗ | const FwSizeType available = queue.getQueueSize(); | |
| 452 | ✗ | for (FwSizeType i = 0; (i < available) && (status == Fw::FW_SERIALIZE_OK); i++) { | |
| 453 | ✗ | if (index < COM_PORT_COUNT) { | |
| 454 | // Dequeueing deserializes the persisted Fw::ComBuffer from the queue's storage | ||
| 455 | ✗ | Fw::ComBuffer comBuffer; | |
| 456 | ✗ | status = queue.dequeue(comBuffer); | |
| 457 | ✗ | } else { | |
| 458 | // For buffer queues, if the buffer requires ownership return, return it via the bufferReturnOut port | ||
| 459 | // Dequeueing deserializes the persisted Fw::Buffer from the queue's storage | ||
| 460 | ✗ | Fw::Buffer buffer; | |
| 461 | ✗ | status = queue.dequeue(buffer); | |
| 462 | ✗ | this->bufferReturnOut_out(static_cast<FwIndexType>(index - COM_PORT_COUNT), buffer); | |
| 463 | ✗ | } | |
| 464 | } | ||
| 465 | } | ||
| 466 | |||
| 467 | 1099 | void ComQueue::processQueue() { | |
| 468 | 1099 | FwIndexType priorityIndex = 0; | |
| 469 | 1099 | FwIndexType sendPriority = 0; | |
| 470 | // Check that we are in the appropriate state | ||
| 471 | 1099 | FW_ASSERT(this->m_state == READY); | |
| 472 | |||
| 473 | // Walk all the queues in priority order. Send the first message that is available in priority order. No balancing | ||
| 474 | // is done within this loop. | ||
| 475 |
2/2✓ Branch 0 taken 2369 times.
✓ Branch 1 taken 396 times.
|
2765 | for (priorityIndex = 0; priorityIndex < TOTAL_PORT_COUNT; priorityIndex++) { |
| 476 | 2369 | QueueMetadata& entry = this->m_prioritizedList[priorityIndex]; | |
| 477 | 2369 | Types::Queue& queue = this->m_queues[entry.index]; | |
| 478 | |||
| 479 | // Continue onto next prioritized queue if the current queue is disabled (depth 0) or holds no items | ||
| 480 |
5/6✓ Branch 0 taken 2369 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1666 times.
✓ Branch 4 taken 703 times.
✓ Branch 5 taken 1666 times.
✓ Branch 6 taken 703 times.
|
2369 | if ((entry.depth == 0) || (queue.getQueueSize() == 0)) { |
| 481 | 1666 | continue; | |
| 482 | } | ||
| 483 | |||
| 484 | // Send out the message based on the type | ||
| 485 |
1/2✓ Branch 0 taken 703 times.
✗ Branch 1 not taken.
|
703 | if (entry.index < COM_PORT_COUNT) { |
| 486 | // Dequeue deserializes the persisted Fw::ComBuffer from the queue's storage | ||
| 487 | 703 | FW_ASSERT(this->m_buffer_state.load() == OWNED); | |
| 488 | 703 | auto dequeue_status = queue.dequeue(this->m_dequeued_com_buffer); | |
| 489 | 703 | FW_ASSERT(dequeue_status == Fw::SerializeStatus::FW_SERIALIZE_OK, | |
| 490 | static_cast<FwAssertArgType>(dequeue_status)); | ||
| 491 | 703 | this->sendComBuffer(this->m_dequeued_com_buffer, entry.index); | |
| 492 | } else { | ||
| 493 | ✗ | Fw::Buffer buffer; | |
| 494 | ✗ | auto dequeue_status = queue.dequeue(buffer); | |
| 495 | ✗ | FW_ASSERT(dequeue_status == Fw::SerializeStatus::FW_SERIALIZE_OK, | |
| 496 | static_cast<FwAssertArgType>(dequeue_status)); | ||
| 497 | ✗ | this->sendBuffer(buffer, entry.index); | |
| 498 | ✗ | } | |
| 499 | |||
| 500 | // Update the throttle and the index that was just sent | ||
| 501 | 703 | this->m_throttle[entry.index] = false; | |
| 502 | |||
| 503 | // Priority used in the next loop | ||
| 504 | 703 | sendPriority = entry.priority; | |
| 505 | 703 | break; | |
| 506 | } | ||
| 507 | |||
| 508 | // Starting on the priority entry after the one dispatched and continuing through the end of the set of entries that | ||
| 509 | // share the same priority, rotate those entries such that the currently dispatched queue is last and the rest are | ||
| 510 | // shifted up by one. This effectively round-robins the queues of the same priority. | ||
| 511 | 1099 | for (priorityIndex++; | |
| 512 |
3/4✓ Branch 0 taken 464 times.
✓ Branch 1 taken 635 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 464 times.
|
1099 | priorityIndex < TOTAL_PORT_COUNT && (this->m_prioritizedList[priorityIndex].priority == sendPriority); |
| 513 | priorityIndex++) { | ||
| 514 | // Swap the previous entry with this one. | ||
| 515 | ✗ | QueueMetadata temp = this->m_prioritizedList[priorityIndex]; | |
| 516 | ✗ | this->m_prioritizedList[priorityIndex] = this->m_prioritizedList[priorityIndex - 1]; | |
| 517 | ✗ | this->m_prioritizedList[priorityIndex - 1] = temp; | |
| 518 | } | ||
| 519 | 1099 | } | |
| 520 | |||
| 521 | ✗ | FwIndexType ComQueue::getQueueNum(Svc::QueueType queueType, FwIndexType portNum) { | |
| 522 | // Validate against the per-type port count before folding so an invalid index cannot alias another queue type | ||
| 523 | ✗ | const FwIndexType portCount = (queueType == QueueType::COM_QUEUE) ? COM_PORT_COUNT : BUFFER_PORT_COUNT; | |
| 524 | ✗ | if (portNum < 0 || portNum >= portCount) { | |
| 525 | ✗ | return -1; | |
| 526 | } | ||
| 527 | ✗ | return static_cast<FwIndexType>(portNum + ((queueType == QueueType::COM_QUEUE) ? 0 : COM_PORT_COUNT)); | |
| 528 | } | ||
| 529 | |||
| 530 | 1426 | FwSizeType ComQueue::getQueueDepth(const FwIndexType queueNum) const { | |
| 531 | 1426 | FW_ASSERT(queueNum >= 0 && queueNum < TOTAL_PORT_COUNT, static_cast<FwAssertArgType>(queueNum)); | |
| 532 |
1/2✓ Branch 0 taken 2627 times.
✗ Branch 1 not taken.
|
2627 | for (FwIndexType i = 0; i < TOTAL_PORT_COUNT; i++) { |
| 533 |
2/2✓ Branch 0 taken 1426 times.
✓ Branch 1 taken 1201 times.
|
2627 | if (this->m_prioritizedList[i].index == queueNum) { |
| 534 | 1426 | return this->m_prioritizedList[i].depth; | |
| 535 | } | ||
| 536 | } | ||
| 537 | // configure() places exactly one metadata entry per queue in the prioritized list | ||
| 538 | ✗ | FW_ASSERT(false, static_cast<FwAssertArgType>(queueNum)); | |
| 539 | ✗ | return 0; | |
| 540 | } | ||
| 541 | } // end namespace Svc | ||
| 542 |