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