| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // ====================================================================== | ||
| 2 | // \title Os/Generic/LocklessPriorityQueue.cpp | ||
| 3 | // \brief lockless ISR-safe priority queue implementation for Os::Queue | ||
| 4 | // ====================================================================== | ||
| 5 | #include "Os/Generic/LocklessPriorityQueue.hpp" | ||
| 6 | #include <atomic> | ||
| 7 | #include <cstring> | ||
| 8 | #include <limits> | ||
| 9 | #include "Fw/LanguageHelpers.hpp" | ||
| 10 | #include "Fw/Time/TimeInterval.hpp" | ||
| 11 | #include "Fw/Types/Assert.hpp" | ||
| 12 | #include "Fw/Types/ByteArray.hpp" | ||
| 13 | #include "Fw/Types/MemAllocator.hpp" | ||
| 14 | #include "Os/Task.hpp" | ||
| 15 | #include "config/MemoryAllocatorTypeEnumAc.hpp" | ||
| 16 | |||
| 17 | namespace Os { | ||
| 18 | namespace Generic { | ||
| 19 | |||
| 20 | namespace { | ||
| 21 | |||
| 22 | //! Extract the state portion of a packed state-tag word. | ||
| 23 | 91111659 | constexpr LocklessStateTagType stateOf(LocklessStateTagType packed) { | |
| 24 | 91111659 | return packed & LocklessSlot::STATE_MASK; | |
| 25 | } | ||
| 26 | |||
| 27 | //! Extract the tag portion of a packed state-tag word. | ||
| 28 | 8130269 | constexpr LocklessStateTagType tagOf(LocklessStateTagType packed) { | |
| 29 | 8130269 | return packed >> LocklessSlot::STATE_BITS; | |
| 30 | } | ||
| 31 | |||
| 32 | //! Pack a (state, tag) pair into a single word. | ||
| 33 | 8132365 | constexpr LocklessStateTagType packStateTag(LocklessStateTagType state, LocklessStateTagType tag) { | |
| 34 | 8132365 | return (tag << LocklessSlot::STATE_BITS) | (state & LocklessSlot::STATE_MASK); | |
| 35 | } | ||
| 36 | |||
| 37 | } // namespace | ||
| 38 | |||
| 39 | 42022872 | bool LocklessPriorityQueue::isCandidatePreferred(FwQueuePriorityType candidatePriority, | |
| 40 | U32 candidateSequence, | ||
| 41 | FwQueuePriorityType bestPriority, | ||
| 42 | U32 bestSequence) { | ||
| 43 | 42022872 | bool preferred = false; | |
| 44 |
2/2✓ Branch 0 taken 3706001 times.
✓ Branch 1 taken 38316871 times.
|
42022872 | if (candidatePriority > bestPriority) { |
| 45 | 3706001 | preferred = true; | |
| 46 |
2/2✓ Branch 0 taken 11619008 times.
✓ Branch 1 taken 26697863 times.
|
38316871 | } else if (candidatePriority == bestPriority) { |
| 47 | // Modular subtraction: if (candidate - best) interpreted unsigned has its top bit set, | ||
| 48 | // candidate is "older" (smaller in the wrap-aware ordering) than best. | ||
| 49 | 11619008 | const U32 difference = candidateSequence - bestSequence; | |
| 50 | 11619008 | const U32 topBit = static_cast<U32>(1) << (std::numeric_limits<U32>::digits - 1); | |
| 51 | 11619008 | preferred = (difference & topBit) != 0; | |
| 52 | } | ||
| 53 | 42022872 | return preferred; | |
| 54 | } | ||
| 55 | |||
| 56 | 2428 | LocklessSlot::LocklessSlot() | |
| 57 | 2428 | : m_stateTag(packStateTag(LOCKLESS_SLOT_FREE, 0)), m_sequence(0), m_size(0), m_priority(0) {} | |
| 58 | |||
| 59 | 205 | LocklessPriorityQueueHandle::LocklessPriorityQueueHandle() | |
| 60 | : QueueHandle(), | ||
| 61 | 205 | m_slots(nullptr), | |
| 62 | 205 | m_slotsAllocation(nullptr), | |
| 63 | 205 | m_data(nullptr), | |
| 64 | 205 | m_depth(0), | |
| 65 | 205 | m_messageSize(0), | |
| 66 | 205 | m_sequence(0), | |
| 67 | 205 | m_count(0), | |
| 68 | 205 | m_available(0), | |
| 69 | 205 | m_highMark(0), | |
| 70 | 205 | m_id(0) {} | |
| 71 | |||
| 72 | 410 | LocklessPriorityQueue::~LocklessPriorityQueue() { | |
| 73 | // Intentionally empty: cleanup is the responsibility of an explicit `teardown()` call. | ||
| 74 | // Freeing here would call the `Fw::MemAllocatorRegistry` singleton, whose destruction | ||
| 75 | // order relative to global queue objects is unspecified (matches | ||
| 76 | // `Os::Generic::PriorityQueue::~PriorityQueue()`). | ||
| 77 | 410 | } | |
| 78 | |||
| 79 | 194 | QueueInterface::Status LocklessPriorityQueue::create(FwEnumStoreType id, | |
| 80 | const Fw::ConstStringBase& name, | ||
| 81 | FwSizeType depth, | ||
| 82 | FwSizeType messageSize) { | ||
| 83 | static_cast<void>(name); | ||
| 84 | |||
| 85 | // Ensure that the queue has not already been created. A double create would leak memory and | ||
| 86 | // is a programming error. | ||
| 87 | 194 | FW_ASSERT(this->m_handle.m_slots == nullptr); | |
| 88 | 194 | FW_ASSERT(this->m_handle.m_data == nullptr); | |
| 89 | |||
| 90 | // The state-tag word relies on the underlying atomic being lock-free for ISR safety. The | ||
| 91 | // static_assert in the header covers platforms with a compile-time guarantee; this runtime | ||
| 92 | // check is the authoritative gate (`is_always_lock_free` is C++17 and this code targets | ||
| 93 | // C++14). | ||
| 94 | 194 | std::atomic<LocklessStateTagType> probe(0); | |
| 95 | 194 | FW_ASSERT(probe.is_lock_free()); | |
| 96 | 194 | std::atomic<FwQueuePriorityType> priorityProbe(0); | |
| 97 | 194 | FW_ASSERT(priorityProbe.is_lock_free()); | |
| 98 | 194 | std::atomic<U32> counterProbe(0); | |
| 99 | 194 | FW_ASSERT(counterProbe.is_lock_free()); | |
| 100 | 194 | FW_ASSERT(depth > 0); | |
| 101 | 194 | FW_ASSERT(messageSize > 0); | |
| 102 | |||
| 103 | // Guard the multiplications used to compute allocation sizes against overflow. | ||
| 104 | 194 | const FwSizeType maxSize = std::numeric_limits<FwSizeType>::max(); | |
| 105 | 194 | FW_ASSERT(depth <= (maxSize / sizeof(LocklessSlot))); | |
| 106 | 194 | FW_ASSERT(depth <= (maxSize / messageSize)); | |
| 107 | |||
| 108 | // The modular comparison in isCandidatePreferred is exact only while queued equal-priority | ||
| 109 | // messages span less than half the U32 sequence domain (SDD section 7); depth bounds message | ||
| 110 | // count, not sequence spread. | ||
| 111 | 194 | FW_ASSERT(depth < (std::numeric_limits<U32>::max() / 2)); | |
| 112 | |||
| 113 |
3/3✓ Branch 1 taken 194 times.
✓ Branch 5 taken 194 times.
✓ Branch 8 taken 194 times.
|
388 | Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator( |
| 114 | 194 | Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE); | |
| 115 | |||
| 116 | 194 | LocklessSlot* slots = nullptr; | |
| 117 | 194 | void* slotsAllocation = nullptr; | |
| 118 | 194 | U8* data = nullptr; | |
| 119 | 194 | QueueInterface::Status status = QueueInterface::Status::OP_OK; | |
| 120 | |||
| 121 | // Allocate the slot array. Request padding for manual alignment: allocators are not required | ||
| 122 | // to honor the alignment argument (e.g. Fw::MallocAllocator ignores it). | ||
| 123 | 194 | FW_ASSERT(depth * sizeof(LocklessSlot) <= (maxSize - alignof(LocklessSlot))); | |
| 124 | 194 | FwSizeType slotBytesRequested = (depth * sizeof(LocklessSlot)) + alignof(LocklessSlot); | |
| 125 | 194 | FwSizeType slotBytesAllocated = slotBytesRequested; | |
| 126 |
1/1✓ Branch 4 taken 194 times.
|
194 | slotsAllocation = allocator.allocate(id, slotBytesAllocated, alignof(LocklessSlot)); |
| 127 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 194 times.
|
194 | if (slotsAllocation == nullptr) { |
| 128 | ✗ | status = QueueInterface::Status::ALLOCATION_FAILED; | |
| 129 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 194 times.
|
194 | } else if (slotBytesAllocated < slotBytesRequested) { |
| 130 | ✗ | allocator.deallocate(id, slotsAllocation); | |
| 131 | ✗ | status = QueueInterface::Status::ALLOCATION_FAILED; | |
| 132 | } else { | ||
| 133 | 194 | const PlatformPointerCastType base = reinterpret_cast<PlatformPointerCastType>(slotsAllocation); | |
| 134 | 194 | const PlatformPointerCastType aligned = | |
| 135 | 194 | (base + (alignof(LocklessSlot) - 1)) & ~static_cast<PlatformPointerCastType>(alignof(LocklessSlot) - 1); | |
| 136 | 194 | const FwSizeType offset = static_cast<FwSizeType>(aligned - base); | |
| 137 |
1/1✓ Branch 4 taken 194 times.
|
194 | slots = Fw::arrayPlacementNew<LocklessSlot>( |
| 138 | Fw::ByteArray(static_cast<U8*>(slotsAllocation) + offset, slotBytesAllocated - offset), depth); | ||
| 139 | } | ||
| 140 | |||
| 141 | // Allocate the message-data region. | ||
| 142 |
1/2✓ Branch 0 taken 194 times.
✗ Branch 1 not taken.
|
194 | if (status == QueueInterface::Status::OP_OK) { |
| 143 | 194 | FwSizeType dataBytesRequested = depth * messageSize; | |
| 144 | 194 | FwSizeType dataBytesAllocated = dataBytesRequested; | |
| 145 |
1/1✓ Branch 4 taken 194 times.
|
194 | void* dataAllocation = allocator.allocate(id, dataBytesAllocated, alignof(U8)); |
| 146 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 194 times.
|
194 | if (dataAllocation == nullptr) { |
| 147 | ✗ | Fw::arrayPlacementDestruct<LocklessSlot>(slots, depth); | |
| 148 | ✗ | allocator.deallocate(id, slotsAllocation); | |
| 149 | ✗ | status = QueueInterface::Status::ALLOCATION_FAILED; | |
| 150 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 194 times.
|
194 | } else if (dataBytesAllocated < dataBytesRequested) { |
| 151 | ✗ | Fw::arrayPlacementDestruct<LocklessSlot>(slots, depth); | |
| 152 | ✗ | allocator.deallocate(id, slotsAllocation); | |
| 153 | ✗ | allocator.deallocate(id, dataAllocation); | |
| 154 | ✗ | status = QueueInterface::Status::ALLOCATION_FAILED; | |
| 155 | } else { | ||
| 156 | 194 | data = static_cast<U8*>(dataAllocation); | |
| 157 | } | ||
| 158 | } | ||
| 159 | |||
| 160 | // Publish the configured handle once both allocations succeeded. | ||
| 161 |
1/2✓ Branch 0 taken 194 times.
✗ Branch 1 not taken.
|
194 | if (status == QueueInterface::Status::OP_OK) { |
| 162 | 194 | this->m_handle.m_id = id; | |
| 163 | 194 | this->m_handle.m_messageSize = messageSize; | |
| 164 | 194 | this->m_handle.m_depth = depth; | |
| 165 | 194 | this->m_handle.m_slots = slots; | |
| 166 | 194 | this->m_handle.m_slotsAllocation = slotsAllocation; | |
| 167 | 194 | this->m_handle.m_data = data; | |
| 168 | 194 | this->m_handle.m_sequence.store(0, std::memory_order_relaxed); | |
| 169 | 194 | this->m_handle.m_count.store(0, std::memory_order_relaxed); | |
| 170 | 194 | this->m_handle.m_available.store(0, std::memory_order_relaxed); | |
| 171 | 194 | this->m_handle.m_highMark.store(0, std::memory_order_relaxed); | |
| 172 | } | ||
| 173 | 388 | return status; | |
| 174 | } | ||
| 175 | |||
| 176 | 205 | void LocklessPriorityQueue::teardown() { | |
| 177 |
2/2✓ Branch 4 taken 194 times.
✓ Branch 5 taken 11 times.
|
205 | if (this->m_handle.m_slots != nullptr) { |
| 178 |
2/2✓ Branch 3 taken 194 times.
✓ Branch 6 taken 194 times.
|
388 | Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator( |
| 179 | 194 | Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE); | |
| 180 | 194 | Fw::arrayPlacementDestruct<LocklessSlot>(this->m_handle.m_slots, this->m_handle.m_depth); | |
| 181 | 194 | allocator.deallocate(this->m_handle.m_id, this->m_handle.m_slotsAllocation); | |
| 182 |
1/2✓ Branch 4 taken 194 times.
✗ Branch 5 not taken.
|
194 | if (this->m_handle.m_data != nullptr) { |
| 183 | 194 | allocator.deallocate(this->m_handle.m_id, this->m_handle.m_data); | |
| 184 | } | ||
| 185 | 194 | this->m_handle.m_slots = nullptr; | |
| 186 | 194 | this->m_handle.m_slotsAllocation = nullptr; | |
| 187 | 194 | this->m_handle.m_data = nullptr; | |
| 188 | 194 | this->m_handle.m_depth = 0; | |
| 189 | 194 | this->m_handle.m_messageSize = 0; | |
| 190 | 194 | this->m_handle.m_count.store(0, std::memory_order_relaxed); | |
| 191 | 194 | this->m_handle.m_available.store(0, std::memory_order_relaxed); | |
| 192 | 194 | this->m_handle.m_highMark.store(0, std::memory_order_relaxed); | |
| 193 | 194 | this->m_handle.m_sequence.store(0, std::memory_order_relaxed); | |
| 194 | } | ||
| 195 | 205 | } | |
| 196 | |||
| 197 | 2109196 | QueueInterface::Status LocklessPriorityQueue::send(const U8* buffer, | |
| 198 | FwSizeType size, | ||
| 199 | FwQueuePriorityType priority, | ||
| 200 | QueueInterface::BlockingType blockType) { | ||
| 201 | // Programming-error checks: queue must be created and inputs must be well-formed. These are | ||
| 202 | // preconditions, not untrusted-input checks. | ||
| 203 | 2109196 | FW_ASSERT(this->m_handle.m_slots != nullptr); | |
| 204 | 2110032 | FW_ASSERT(this->m_handle.m_data != nullptr); | |
| 205 | 2111879 | FW_ASSERT(buffer != nullptr); | |
| 206 | |||
| 207 | // Reject oversized messages without touching the queue. | ||
| 208 |
2/2✓ Branch 4 taken 1 times.
✓ Branch 5 taken 2110964 times.
|
2111879 | if (size > this->m_handle.m_messageSize) { |
| 209 | 1 | return QueueInterface::Status::SIZE_MISMATCH; | |
| 210 | } | ||
| 211 | |||
| 212 | 2110964 | const FwSizeType depth = this->m_handle.m_depth; | |
| 213 | 2106582 | const bool blocking = (blockType == QueueInterface::BlockingType::BLOCKING); | |
| 214 | |||
| 215 | // Bounded for non-blocking, unbounded for blocking (explicit user contract). | ||
| 216 |
4/4✓ Branch 0 taken 13996 times.
✓ Branch 1 taken 4423158 times.
✓ Branch 2 taken 4052186 times.
✓ Branch 3 taken 370972 times.
|
4437154 | for (FwSizeType pass = 0; blocking || (pass < MAX_RETRY_PASSES); pass++) { |
| 217 |
2/2✓ Branch 0 taken 58207408 times.
✓ Branch 1 taken 2393071 times.
|
60600479 | for (FwSizeType i = 0; i < depth; i++) { |
| 218 | 58207408 | LocklessSlot& slot = this->m_handle.m_slots[i]; | |
| 219 | 58416665 | LocklessStateTagType packed = slot.m_stateTag.load(std::memory_order_acquire); | |
| 220 |
2/2✓ Branch 1 taken 56379595 times.
✓ Branch 2 taken 1590209 times.
|
57956972 | if (stateOf(packed) != LOCKLESS_SLOT_FREE) { |
| 221 | 56379595 | continue; | |
| 222 | } | ||
| 223 | 1590209 | const LocklessStateTagType desired = packStateTag(LOCKLESS_SLOT_WRITING, tagOf(packed) + 1); | |
| 224 |
2/2✓ Branch 2 taken 1743169 times.
✓ Branch 3 taken 32804 times.
|
3552685 | if (slot.m_stateTag.compare_exchange_strong(packed, desired, std::memory_order_acq_rel, |
| 225 | std::memory_order_relaxed)) { | ||
| 226 |
1/2✓ Branch 0 taken 1743172 times.
✗ Branch 1 not taken.
|
1743169 | if (size > 0) { |
| 227 | 1743172 | const FwSizeType offset = i * this->m_handle.m_messageSize; | |
| 228 |
2/4✗ Branch 5 not taken.
✓ Branch 6 taken 1742578 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1742578 times.
|
1742936 | static_cast<void>(::memcpy(this->m_handle.m_data + offset, buffer, static_cast<size_t>(size))); |
| 229 | } | ||
| 230 | 1742575 | slot.m_size = size; | |
| 231 | 1742833 | slot.m_priority.store(priority, std::memory_order_relaxed); | |
| 232 | 3486010 | slot.m_sequence.store(this->m_handle.m_sequence.fetch_add(1, std::memory_order_relaxed), | |
| 233 | std::memory_order_relaxed); | ||
| 234 | |||
| 235 | // Increment both counters *before* publishing READY. This guarantees that a | ||
| 236 | // consumer's decrement (which can only follow a READY observation) never races | ||
| 237 | // ahead of the matching increment, so neither m_count nor m_available can | ||
| 238 | // transiently underflow or wrap to a huge value (fixes GitHub issue #6055). | ||
| 239 | 1742692 | const U32 nextCount = this->m_handle.m_count.fetch_add(1, std::memory_order_acq_rel) + 1; | |
| 240 | 1742925 | static_cast<void>(this->m_handle.m_available.fetch_add(1, std::memory_order_acq_rel)); | |
| 241 | |||
| 242 | 1742851 | slot.m_stateTag.store(packStateTag(LOCKLESS_SLOT_READY, tagOf(desired) + 1), std::memory_order_release); | |
| 243 | |||
| 244 | // Raise the high-water mark after publication so the message is never invisible | ||
| 245 | // while the producer runs this loop. Each strong-CAS failure strictly raises | ||
| 246 | // prevMark (mark only increases, capped at depth), so at most depth iterations run. | ||
| 247 | 1742041 | U32 prevMark = this->m_handle.m_highMark.load(std::memory_order_relaxed); | |
| 248 |
4/4✓ Branch 0 taken 1741877 times.
✓ Branch 1 taken 138 times.
✓ Branch 2 taken 2134 times.
✓ Branch 3 taken 1739743 times.
|
1742015 | for (FwSizeType markPass = 0; (markPass < depth) && (nextCount > prevMark); markPass++) { |
| 249 |
2/2✓ Branch 3 taken 2132 times.
✓ Branch 4 taken 1 times.
|
4267 | if (this->m_handle.m_highMark.compare_exchange_strong( |
| 250 | prevMark, nextCount, std::memory_order_relaxed, std::memory_order_relaxed)) { | ||
| 251 | 2132 | break; | |
| 252 | } | ||
| 253 | } | ||
| 254 | 1742013 | return QueueInterface::Status::OP_OK; | |
| 255 | } | ||
| 256 | } | ||
| 257 | // No free slot found this pass. Blocking callers back off; non-blocking callers retry. | ||
| 258 |
2/2✓ Branch 0 taken 1915 times.
✓ Branch 1 taken 2391156 times.
|
2393071 | if (blocking) { |
| 259 |
2/2✓ Branch 2 taken 1915 times.
✓ Branch 6 taken 1901 times.
|
1915 | static_cast<void>(Os::Task::delay(Fw::TimeInterval(0, LOCKLESS_QUEUE_BLOCKING_BACKOFF_US))); |
| 260 | } | ||
| 261 | } | ||
| 262 | 370972 | return QueueInterface::Status::FULL; | |
| 263 | } | ||
| 264 | |||
| 265 | 2604369 | QueueInterface::Status LocklessPriorityQueue::receive(U8* destination, | |
| 266 | FwSizeType capacity, | ||
| 267 | QueueInterface::BlockingType blockType, | ||
| 268 | FwSizeType& actualSize, | ||
| 269 | FwQueuePriorityType& priority) { | ||
| 270 | // Programming-error checks. These mirror the assertions in send(). | ||
| 271 | 2604369 | FW_ASSERT(this->m_handle.m_slots != nullptr); | |
| 272 | 2608216 | FW_ASSERT(this->m_handle.m_data != nullptr); | |
| 273 | 2614758 | FW_ASSERT(destination != nullptr); | |
| 274 | |||
| 275 | // Reject undersized buffers before claiming a slot so the message stays queued. | ||
| 276 |
2/2✓ Branch 4 taken 1 times.
✓ Branch 5 taken 2595445 times.
|
2614758 | if (capacity < this->m_handle.m_messageSize) { |
| 277 | 1 | return QueueInterface::Status::SIZE_MISMATCH; | |
| 278 | } | ||
| 279 | |||
| 280 | 2595445 | const FwSizeType depth = this->m_handle.m_depth; | |
| 281 | 2600808 | const bool blocking = (blockType == QueueInterface::BlockingType::BLOCKING); | |
| 282 | |||
| 283 | // Bounded for non-blocking, unbounded for blocking (explicit user contract). | ||
| 284 |
4/4✓ Branch 0 taken 55902 times.
✓ Branch 1 taken 6800070 times.
✓ Branch 2 taken 5906896 times.
✓ Branch 3 taken 893174 times.
|
6855972 | for (FwSizeType pass = 0; blocking || (pass < MAX_RETRY_PASSES); pass++) { |
| 285 | // Fast path: skip the O(depth) scan while no message is receivable. | ||
| 286 |
2/2✓ Branch 3 taken 2886991 times.
✓ Branch 4 taken 3056727 times.
|
11906516 | if (this->m_handle.m_available.load(std::memory_order_acquire) == 0) { |
| 287 |
2/2✓ Branch 0 taken 1747 times.
✓ Branch 1 taken 2885244 times.
|
2886991 | if (blocking) { |
| 288 |
2/2✓ Branch 2 taken 1748 times.
✓ Branch 6 taken 1739 times.
|
1747 | static_cast<void>(Os::Task::delay(Fw::TimeInterval(0, LOCKLESS_QUEUE_BLOCKING_BACKOFF_US))); |
| 289 | } | ||
| 290 | 2887261 | continue; | |
| 291 | } | ||
| 292 | 3056727 | FwSizeType bestIndex = depth; | |
| 293 | 3056727 | FwQueuePriorityType bestPriority = FwQueuePriorityType(); | |
| 294 | 3056727 | U32 bestSequence = 0; | |
| 295 | 3056727 | LocklessStateTagType bestPacked = 0; | |
| 296 | |||
| 297 |
2/2✓ Branch 0 taken 53017982 times.
✓ Branch 1 taken 6687273 times.
|
59705255 | for (FwSizeType i = 0; i < depth; i++) { |
| 298 | 53017982 | LocklessSlot& slot = this->m_handle.m_slots[i]; | |
| 299 | 54297339 | LocklessStateTagType packed = slot.m_stateTag.load(std::memory_order_acquire); | |
| 300 |
2/2✓ Branch 1 taken 12526575 times.
✓ Branch 2 taken 40870032 times.
|
53329639 | if (stateOf(packed) != LOCKLESS_SLOT_READY) { |
| 301 | 12526575 | continue; | |
| 302 | } | ||
| 303 | 40870032 | const FwQueuePriorityType candidatePriority = slot.m_priority.load(std::memory_order_relaxed); | |
| 304 | 44897593 | const U32 candidateSequence = slot.m_sequence.load(std::memory_order_relaxed); | |
| 305 | // Recheck: if the state-tag changed, the relaxed priority/sequence reads above may | ||
| 306 | // belong to a recycled slot; discard the candidate and keep scanning. | ||
| 307 | 43780737 | const LocklessStateTagType packedRecheck = slot.m_stateTag.load(std::memory_order_acquire); | |
| 308 |
2/2✓ Branch 0 taken 86356 times.
✓ Branch 1 taken 43883605 times.
|
43969961 | if (packed != packedRecheck) { |
| 309 | 86356 | continue; | |
| 310 | } | ||
| 311 |
6/6✓ Branch 0 taken 41989252 times.
✓ Branch 1 taken 1894353 times.
✓ Branch 2 taken 7143894 times.
✓ Branch 3 taken 34980548 times.
✓ Branch 4 taken 9059800 times.
✓ Branch 5 taken 34958995 times.
|
86008047 | if ((bestIndex == depth) || |
| 312 | 41989252 | isCandidatePreferred(candidatePriority, candidateSequence, bestPriority, bestSequence)) { | |
| 313 | 9059800 | bestIndex = i; | |
| 314 | 9059800 | bestPriority = candidatePriority; | |
| 315 | 9059800 | bestSequence = candidateSequence; | |
| 316 | 9059800 | bestPacked = packed; | |
| 317 | } | ||
| 318 | } | ||
| 319 | |||
| 320 |
2/2✓ Branch 0 taken 36948 times.
✓ Branch 1 taken 6650325 times.
|
6687273 | if (bestIndex == depth) { |
| 321 |
2/2✓ Branch 0 taken 21 times.
✓ Branch 1 taken 36927 times.
|
36948 | if (blocking) { |
| 322 |
2/2✓ Branch 2 taken 21 times.
✓ Branch 6 taken 21 times.
|
21 | static_cast<void>(Os::Task::delay(Fw::TimeInterval(0, LOCKLESS_QUEUE_BLOCKING_BACKOFF_US))); |
| 323 | } | ||
| 324 | 36826 | continue; | |
| 325 | } | ||
| 326 | |||
| 327 | 6650325 | const LocklessStateTagType desired = packStateTag(LOCKLESS_SLOT_READING, tagOf(bestPacked) + 1); | |
| 328 |
2/2✓ Branch 7 taken 1742970 times.
✓ Branch 8 taken 1272534 times.
|
6028655 | if (this->m_handle.m_slots[bestIndex].m_stateTag.compare_exchange_strong( |
| 329 | bestPacked, desired, std::memory_order_acq_rel, std::memory_order_relaxed)) { | ||
| 330 | // Decrement the receivable count at the successful READY->READING claim. Because | ||
| 331 | // m_available is incremented before READY is published, it is always >= the number | ||
| 332 | // of genuinely READY slots, so this decrement can never underflow. | ||
| 333 | 1742970 | static_cast<void>(this->m_handle.m_available.fetch_sub(1, std::memory_order_acq_rel)); | |
| 334 | 1742886 | LocklessSlot& slot = this->m_handle.m_slots[bestIndex]; | |
| 335 | 1742724 | const FwSizeType storedSize = slot.m_size; | |
| 336 | 1742753 | FW_ASSERT(storedSize <= capacity); | |
| 337 |
1/2✓ Branch 0 taken 1742763 times.
✗ Branch 1 not taken.
|
1742753 | if (storedSize > 0) { |
| 338 | 1742763 | const FwSizeType offset = bestIndex * this->m_handle.m_messageSize; | |
| 339 | static_cast<void>( | ||
| 340 |
2/4✗ Branch 5 not taken.
✓ Branch 6 taken 1742783 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1742783 times.
|
1742764 | ::memcpy(destination, this->m_handle.m_data + offset, static_cast<size_t>(storedSize))); |
| 341 | } | ||
| 342 | 1742773 | actualSize = storedSize; | |
| 343 | 3485301 | priority = slot.m_priority.load(std::memory_order_relaxed); | |
| 344 | // Decrement the occupancy count *before* releasing the slot to FREE. This keeps | ||
| 345 | // m_count (and therefore the high-water mark) at or below the queue depth: a producer | ||
| 346 | // can only re-claim and re-count this slot after observing FREE, which follows the | ||
| 347 | // decrement. | ||
| 348 | 1742559 | static_cast<void>(this->m_handle.m_count.fetch_sub(1, std::memory_order_acq_rel)); | |
| 349 | 1742873 | slot.m_stateTag.store(packStateTag(LOCKLESS_SLOT_FREE, tagOf(desired) + 1), std::memory_order_release); | |
| 350 | 1742359 | return QueueInterface::Status::OP_OK; | |
| 351 | } | ||
| 352 | // CAS failed — another consumer claimed this slot; yield and rescan. | ||
| 353 |
2/2✓ Branch 0 taken 111 times.
✓ Branch 1 taken 1272423 times.
|
1272534 | if (blocking) { |
| 354 |
2/2✓ Branch 2 taken 111 times.
✓ Branch 6 taken 111 times.
|
111 | static_cast<void>(Os::Task::delay(Fw::TimeInterval(0, 0))); |
| 355 | } | ||
| 356 | } | ||
| 357 | 893174 | return QueueInterface::Status::EMPTY; | |
| 358 | } | ||
| 359 | |||
| 360 | 350445 | FwSizeType LocklessPriorityQueue::getMessagesAvailable() const { | |
| 361 | 700890 | return static_cast<FwSizeType>(this->m_handle.m_available.load(std::memory_order_acquire)); | |
| 362 | } | ||
| 363 | |||
| 364 | 328569 | FwSizeType LocklessPriorityQueue::getMessageHighWaterMark() const { | |
| 365 | 657138 | return static_cast<FwSizeType>(this->m_handle.m_highMark.load(std::memory_order_acquire)); | |
| 366 | } | ||
| 367 | |||
| 368 | ✗ | QueueHandle* LocklessPriorityQueue::getHandle() { | |
| 369 | ✗ | return &this->m_handle; | |
| 370 | } | ||
| 371 | |||
| 372 | } // namespace Generic | ||
| 373 | } // namespace Os | ||
| 374 |