GCC Code Coverage Report

Directory: ./
File: Os/Generic/PriorityQueue.cpp
Date: 2026-10-14 21:15:02
Exec Total Coverage
Lines: 139 166 83.7%
Functions: 12 13 92.3%
Branches: 71 108 65.7%
Line Branch Exec Source
1 // ======================================================================
2 // \title Os/Generic/PriorityQueue.cpp
3 // \brief priority queue implementation for Os::Queue
4 // ======================================================================
5 #include "Os/Generic/PriorityQueue.hpp"
6 #include <algorithm>
7 #include <cstring>
8 #include "Fw/LanguageHelpers.hpp"
9 #include "Fw/Types/Assert.hpp"
10 #include "Fw/Types/MemAllocator.hpp"
11 #include "config/MemoryAllocatorTypeEnumAc.hpp"
12
13 namespace Os {
14 namespace Generic {
15
16 189732 FwSizeType PriorityQueueHandle ::find_index() {
17 189732 FW_ASSERT(this->m_depth > 0);
18
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 189732 times.
 189732 FwSizeType index = this->m_indices[this->m_startIndex % this->m_depth];
19
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 189732 times.
 189732 this->m_startIndex = (this->m_startIndex + 1) % this->m_depth;
20 189732 return index;
21 }
22
23 189586 void PriorityQueueHandle ::return_index(FwSizeType index) {
24 189586 FW_ASSERT(this->m_depth > 0);
25
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 189586 times.
 189586 this->m_indices[this->m_stopIndex % this->m_depth] = index;
26
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 189586 times.
 189586 this->m_stopIndex = (this->m_stopIndex + 1) % this->m_depth;
27 189586 }
28
29 189732 void PriorityQueueHandle ::store_data(FwSizeType index, const U8* data, FwSizeType size) {
30 189732 FW_ASSERT(size <= this->m_maxSize);
31 189732 FW_ASSERT(index < this->m_depth);
32
33 189732 FwSizeType offset = this->m_maxSize * index;
34
2/4
✗ Branch 3 not taken.
✓ Branch 4 taken 189732 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 189732 times.
 189732 (void)::memcpy(this->m_data + offset, data, static_cast<size_t>(size));
35 189732 this->m_sizes[index] = size;
36 189732 }
37
38 189586 void PriorityQueueHandle ::load_data(FwSizeType index, U8* destination, FwSizeType size) {
39 189586 FW_ASSERT(size <= this->m_maxSize);
40 189586 FW_ASSERT(index < this->m_depth);
41 189586 FwSizeType offset = this->m_maxSize * index;
42
2/4
✗ Branch 3 not taken.
✓ Branch 4 taken 189586 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 189586 times.
 189586 (void)::memcpy(destination, this->m_data + offset, static_cast<size_t>(size));
43 189586 }
44
45 3620 PriorityQueue::~PriorityQueue() {}
46
47 1786 QueueInterface::Status PriorityQueue::create(FwEnumStoreType id,
48 const Fw::ConstStringBase& name,
49 FwSizeType depth,
50 FwSizeType messageSize) {
51 1786 const FwEnumStoreType identifier = id;
52 1786 QueueInterface::Status status = Os::QueueInterface::Status::OP_OK;
53 // Ensure we are created exactly once
54 1786 FW_ASSERT(this->m_handle.m_indices == nullptr);
55 1786 FW_ASSERT(this->m_handle.m_sizes == nullptr);
56 1786 FW_ASSERT(this->m_handle.m_data == nullptr);
57
58 // Get the memory allocator configured for priority queues
59
3/3
✓ Branch 1 taken 1786 times.
✓ Branch 5 taken 1786 times.
✓ Branch 8 taken 1786 times.
 3572 Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator(
60 1786 Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE);
61
62 // Allocate indices list
63 1786 void* allocation = nullptr;
64 1786 FwSizeType size = 0;
65 1786 FwSizeType* indices = nullptr;
66 1786 FwSizeType* sizes = nullptr;
67 1786 U8* data = nullptr;
68 1786 U8* heap_pointer = nullptr;
69
70 // Prevent integer overflow when computing allocation size (depth * sizeof(FwSizeType))
71 1786 FW_ASSERT(depth < std::numeric_limits<FwSizeType>::max() / sizeof(FwSizeType));
72
73 // Allocate indices list and construct it when valid
74 1786 size = depth * sizeof(FwSizeType);
75
1/1
✓ Branch 4 taken 1786 times.
 1786 allocation = allocator.allocate(identifier, size, alignof(FwSizeType));
76
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1786 times.
 1786 if (allocation == nullptr) {
77 ✗ status = QueueInterface::Status::ALLOCATION_FAILED;
78
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1786 times.
 1786 } else if (size < (depth * sizeof(FwSizeType))) {
79 ✗ allocator.deallocate(identifier, allocation);
80 ✗ status = QueueInterface::Status::ALLOCATION_FAILED;
81 } else {
82
1/1
✓ Branch 3 taken 1786 times.
 1786 indices = Fw::arrayPlacementNew<FwSizeType>(Fw::ByteArray(static_cast<U8*>(allocation), size), depth);
83 }
84
85 // Allocate sizes list and construct it when valid
86
1/2
✓ Branch 0 taken 1786 times.
✗ Branch 1 not taken.
 1786 if (status == QueueInterface::Status::OP_OK) {
87 1786 size = depth * sizeof(FwSizeType);
88
1/1
✓ Branch 4 taken 1786 times.
 1786 allocation = allocator.allocate(identifier, size, alignof(FwSizeType));
89
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1786 times.
 1786 if (allocation == nullptr) {
90 ✗ allocator.deallocate(identifier, indices);
91 ✗ status = QueueInterface::Status::ALLOCATION_FAILED;
92
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1786 times.
 1786 } else if (size < (depth * sizeof(FwSizeType))) {
93 ✗ allocator.deallocate(identifier, indices);
94 ✗ allocator.deallocate(identifier, allocation);
95 ✗ status = QueueInterface::Status::ALLOCATION_FAILED;
96 } else {
97
1/1
✓ Branch 3 taken 1786 times.
 1786 sizes = Fw::arrayPlacementNew<FwSizeType>(Fw::ByteArray(static_cast<U8*>(allocation), size), depth);
98 }
99 }
100 // Allocate data
101
1/2
✓ Branch 0 taken 1786 times.
✗ Branch 1 not taken.
 1786 if (status == QueueInterface::Status::OP_OK) {
102 // Prevent integer overflow when computing allocation size (depth * messageSize)
103 1786 FW_ASSERT((depth == 0) || (messageSize <= std::numeric_limits<FwSizeType>::max() / depth));
104 1786 size = depth * messageSize;
105
1/1
✓ Branch 4 taken 1786 times.
 1786 allocation = allocator.allocate(identifier, size, alignof(U8));
106
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1786 times.
 1786 if (allocation == nullptr) {
107 ✗ allocator.deallocate(identifier, indices);
108 ✗ allocator.deallocate(identifier, sizes);
109 ✗ status = QueueInterface::Status::ALLOCATION_FAILED;
110
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1786 times.
 1786 } else if (size < (depth * messageSize)) {
111 ✗ allocator.deallocate(identifier, indices);
112 ✗ allocator.deallocate(identifier, sizes);
113 ✗ allocator.deallocate(identifier, allocation);
114 ✗ status = QueueInterface::Status::ALLOCATION_FAILED;
115 } else {
116 1786 data = static_cast<U8*>(allocation);
117 }
118 }
119 // Allocate data for max heap
120
1/2
✓ Branch 0 taken 1786 times.
✗ Branch 1 not taken.
 1786 if (status == QueueInterface::Status::OP_OK) {
121 1786 size = Types::MaxHeap::ELEMENT_SIZE * depth;
122
1/1
✓ Branch 4 taken 1786 times.
 1786 allocation = allocator.allocate(identifier, size, Types::MaxHeap::ALIGNMENT);
123
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1786 times.
 1786 if (allocation == nullptr) {
124 ✗ allocator.deallocate(identifier, indices);
125 ✗ allocator.deallocate(identifier, sizes);
126 ✗ allocator.deallocate(identifier, data);
127 ✗ status = QueueInterface::Status::ALLOCATION_FAILED;
128
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1786 times.
 1786 } else if (size < (Types::MaxHeap::ELEMENT_SIZE * depth)) {
129 ✗ allocator.deallocate(identifier, indices);
130 ✗ allocator.deallocate(identifier, sizes);
131 ✗ allocator.deallocate(identifier, data);
132 ✗ allocator.deallocate(identifier, allocation);
133 ✗ status = QueueInterface::Status::ALLOCATION_FAILED;
134 } else {
135 1786 heap_pointer = static_cast<U8*>(allocation);
136
1/1
✓ Branch 6 taken 1786 times.
 1786 this->m_handle.m_heap.create(depth, Fw::ByteArray(static_cast<U8*>(allocation), size));
137 }
138 }
139 // Set up structures when all allocations succeeded
140
1/2
✓ Branch 0 taken 1786 times.
✗ Branch 1 not taken.
 1786 if (status == QueueInterface::Status::OP_OK) {
141 // Assign initial indices and sizes
142
2/2
✓ Branch 0 taken 21487 times.
✓ Branch 1 taken 1786 times.
 23273 for (FwSizeType i = 0; i < depth; i++) {
143 21487 indices[i] = i;
144 21487 sizes[i] = 0;
145 }
146 // Set local tracking variables
147 1786 this->m_handle.m_id = id;
148 1786 this->m_handle.m_maxSize = messageSize;
149 1786 this->m_handle.m_indices = indices;
150 1786 this->m_handle.m_data = data;
151 1786 this->m_handle.m_sizes = sizes;
152 1786 this->m_handle.m_heap_pointer = heap_pointer;
153 1786 this->m_handle.m_startIndex = 0;
154 1786 this->m_handle.m_stopIndex = 0;
155 1786 this->m_handle.m_depth = depth;
156 1786 this->m_handle.m_highMark = 0;
157 }
158 1786 return status;
159 }
160
161 1795 void PriorityQueue::teardown() {
162 1795 this->teardownInternal();
163 1795 }
164
165 1795 void PriorityQueue::teardownInternal() {
166
2/2
✓ Branch 4 taken 1786 times.
✓ Branch 5 taken 9 times.
 1795 if (this->m_handle.m_data != nullptr) {
167 // All backing arrays are allocated together in create()
168 1786 FW_ASSERT(this->m_handle.m_indices != nullptr);
169 1786 FW_ASSERT(this->m_handle.m_sizes != nullptr);
170 1786 FW_ASSERT(this->m_handle.m_heap_pointer != nullptr);
171 1786 const FwEnumStoreType identifier = this->m_handle.m_id;
172
2/2
✓ Branch 3 taken 1786 times.
✓ Branch 6 taken 1786 times.
 3572 Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator(
173 1786 Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE);
174 1786 allocator.deallocate(identifier, this->m_handle.m_data);
175 1786 allocator.deallocate(identifier, this->m_handle.m_indices);
176 1786 allocator.deallocate(identifier, this->m_handle.m_sizes);
177 1786 this->m_handle.m_heap.teardown();
178 1786 allocator.deallocate(identifier, this->m_handle.m_heap_pointer);
179
180 // Set these pointers to nullptr
181 1786 this->m_handle.m_data = nullptr;
182 1786 this->m_handle.m_indices = nullptr;
183 1786 this->m_handle.m_sizes = nullptr;
184 }
185 1795 }
186
187 252988 QueueInterface::Status PriorityQueue::send(const U8* buffer,
188 FwSizeType size,
189 FwQueuePriorityType priority,
190 QueueInterface::BlockingType blockType) {
191 // Check for sizing problem before locking
192
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 252988 times.
 252988 if (size > this->m_handle.m_maxSize) {
193 ✗ return QueueInterface::Status::SIZE_MISMATCH;
194 }
195 // Artificial block scope for scope lock ensuring an unlock in all cases and ensuring an unlock before notify
196 {
197
1/1
✓ Branch 5 taken 252988 times.
 252988 Os::ScopeLock lock(this->m_handle.m_data_lock);
198
7/7
✓ Branch 4 taken 252988 times.
✓ Branch 6 taken 63257 times.
✓ Branch 7 taken 189731 times.
✓ Branch 8 taken 63256 times.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 63256 times.
✓ Branch 11 taken 189732 times.
 252988 if (this->m_handle.m_heap.isFull() and blockType == BlockingType::NONBLOCKING) {
199 63256 return QueueInterface::Status::FULL;
200 }
201 // Will loop and block until full is false
202 // @non-terminating@: condition-variable wait loop
203
3/3
✓ Branch 4 taken 189733 times.
✓ Branch 6 taken 1 times.
✓ Branch 7 taken 189732 times.
 189733 while (this->m_handle.m_heap.isFull()) {
204
1/1
✓ Branch 9 taken 1 times.
 1 this->m_handle.m_full.wait(this->m_handle.m_data_lock);
205 }
206
1/1
✓ Branch 4 taken 189732 times.
 189732 FwSizeType index = this->m_handle.find_index();
207
208 // Space must exist, push must work
209
1/1
✓ Branch 4 taken 189732 times.
 189732 const bool pushed = this->m_handle.m_heap.push(priority, index);
210 189732 FW_ASSERT(pushed);
211
1/1
✓ Branch 4 taken 189732 times.
 189732 this->m_handle.store_data(index, buffer, size);
212 189732 this->m_handle.m_sizes[index] = size;
213
1/1
✓ Branch 8 taken 189732 times.
 189732 this->m_handle.m_highMark = std::max(this->m_handle.m_highMark, this->getMessagesAvailable());
214 252988 }
215 189732 this->m_handle.m_empty.notify();
216 189732 return QueueInterface::Status::OP_OK;
217 }
218
219 195495 QueueInterface::Status PriorityQueue::receive(U8* destination,
220 FwSizeType capacity,
221 QueueInterface::BlockingType blockType,
222 FwSizeType& actualSize,
223 FwQueuePriorityType& priority) {
224 // Reject undersized buffers before popping so the message stays queued (matches POSIX mq_receive EMSGSIZE)
225
2/2
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 195494 times.
 195495 if (capacity < this->m_handle.m_maxSize) {
226 1 return QueueInterface::Status::SIZE_MISMATCH;
227 }
228 {
229
1/1
✓ Branch 5 taken 195494 times.
 195494 Os::ScopeLock lock(this->m_handle.m_data_lock);
230
7/7
✓ Branch 4 taken 195494 times.
✓ Branch 6 taken 5909 times.
✓ Branch 7 taken 189585 times.
✓ Branch 8 taken 5908 times.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 5908 times.
✓ Branch 11 taken 189586 times.
 195494 if (this->m_handle.m_heap.isEmpty() and blockType == BlockingType::NONBLOCKING) {
231 5908 return QueueInterface::Status::EMPTY;
232 }
233 // Loop and lock while empty
234 // @non-terminating@: condition-variable wait loop
235
3/3
✓ Branch 4 taken 189587 times.
✓ Branch 6 taken 1 times.
✓ Branch 7 taken 189586 times.
 189587 while (this->m_handle.m_heap.isEmpty()) {
236
1/1
✓ Branch 9 taken 1 times.
 1 this->m_handle.m_empty.wait(this->m_handle.m_data_lock);
237 }
238
239 189586 FwSizeType index;
240 // Message must exist, so pop must pass and size must be valid
241
1/1
✓ Branch 4 taken 189586 times.
 189586 const bool popped = this->m_handle.m_heap.pop(priority, index);
242 189586 FW_ASSERT(popped);
243 189586 actualSize = this->m_handle.m_sizes[index];
244 189586 FW_ASSERT(actualSize <= capacity);
245
1/1
✓ Branch 5 taken 189586 times.
 189586 this->m_handle.load_data(index, destination, actualSize);
246
1/1
✓ Branch 4 taken 189586 times.
 189586 this->m_handle.return_index(index);
247 195494 }
248 189586 this->m_handle.m_full.notify();
249 189586 return QueueInterface::Status::OP_OK;
250 }
251
252 609364 FwSizeType PriorityQueue::getMessagesAvailable() const {
253 609364 return this->m_handle.m_heap.getSize();
254 }
255
256 404460 FwSizeType PriorityQueue::getMessageHighWaterMark() const {
257 // Safe to cast away const in this context because scope lock will restore unlocked state on return
258
1/1
✓ Branch 9 taken 404460 times.
 404460 Os::ScopeLock lock(const_cast<Mutex&>(this->m_handle.m_data_lock));
259 808920 return this->m_handle.m_highMark;
260 404460 }
261
262 ✗ QueueHandle* PriorityQueue::getHandle() {
263 ✗ return &this->m_handle;
264 }
265
266 } // namespace Generic
267 } // namespace Os
268