GCC Code Coverage Report

Directory:	Os/
File:	Generic/PriorityQueue.cpp
Date:	2026-09-23 21:11:56

	Total	Coverage
Lines:	164	0.0%
Functions:	13	0.0%
Branches:	106	0.0%

  
      Line
      Branch
      Exec
      Source
    
      // ======================================================================
    
      // \title Os/Generic/PriorityQueue.cpp
    
      // \brief priority queue implementation for Os::Queue
    
      // ======================================================================
    
      #include "Os/Generic/PriorityQueue.hpp"
    
      #include <algorithm>
    
      #include <cstring>
    
      #include "Fw/LanguageHelpers.hpp"
    
      #include "Fw/Types/Assert.hpp"
    
      #include "Fw/Types/MemAllocator.hpp"
    
      #include "config/MemoryAllocatorTypeEnumAc.hpp"
    
      namespace Os {
    
      namespace Generic {
    
      ✗
      FwSizeType PriorityQueueHandle ::find_index() {
    
      ✗
          FW_ASSERT(this->m_depth > 0);
    
      ✗
          FwSizeType index = this->m_indices[this->m_startIndex % this->m_depth];
    
      ✗
          this->m_startIndex = (this->m_startIndex + 1) % this->m_depth;
    
      ✗
          return index;
    
      }
    
      ✗
      void PriorityQueueHandle ::return_index(FwSizeType index) {
    
      ✗
          FW_ASSERT(this->m_depth > 0);
    
      ✗
          this->m_indices[this->m_stopIndex % this->m_depth] = index;
    
      ✗
          this->m_stopIndex = (this->m_stopIndex + 1) % this->m_depth;
    
      ✗
      }
    
      ✗
      void PriorityQueueHandle ::store_data(FwSizeType index, const U8* data, FwSizeType size) {
    
      ✗
          FW_ASSERT(size <= this->m_maxSize);
    
      ✗
          FW_ASSERT(index < this->m_depth);
    
      ✗
          FwSizeType offset = this->m_maxSize * index;
    
      ✗
          (void)::memcpy(this->m_data + offset, data, static_cast<size_t>(size));
    
      ✗
          this->m_sizes[index] = size;
    
      ✗
      }
    
      ✗
      void PriorityQueueHandle ::load_data(FwSizeType index, U8* destination, FwSizeType size) {
    
      ✗
          FW_ASSERT(size <= this->m_maxSize);
    
      ✗
          FW_ASSERT(index < this->m_depth);
    
      ✗
          FwSizeType offset = this->m_maxSize * index;
    
      ✗
          (void)::memcpy(destination, this->m_data + offset, static_cast<size_t>(size));
    
      ✗
      }
    
      ✗
      PriorityQueue::~PriorityQueue() {}
    
      ✗
      QueueInterface::Status PriorityQueue::create(FwEnumStoreType id,
    
                                                   const Fw::ConstStringBase& name,
    
                                                   FwSizeType depth,
    
                                                   FwSizeType messageSize) {
    
      ✗
          const FwEnumStoreType identifier = id;
    
      ✗
          QueueInterface::Status status = Os::QueueInterface::Status::OP_OK;
    
          // Ensure we are created exactly once
    
      ✗
          FW_ASSERT(this->m_handle.m_indices == nullptr);
    
      ✗
          FW_ASSERT(this->m_handle.m_sizes == nullptr);
    
      ✗
          FW_ASSERT(this->m_handle.m_data == nullptr);
    
          // Get the memory allocator configured for priority queues
    
      ✗
          Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator(
    
      ✗
              Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE);
    
          // Allocate indices list
    
      ✗
          void* allocation = nullptr;
    
      ✗
          FwSizeType size = 0;
    
      ✗
          FwSizeType* indices = nullptr;
    
      ✗
          FwSizeType* sizes = nullptr;
    
      ✗
          U8* data = nullptr;
    
      ✗
          U8* heap_pointer = nullptr;
    
          // Prevent integer overflow when computing allocation size (depth * sizeof(FwSizeType))
    
      ✗
          FW_ASSERT(depth < std::numeric_limits<FwSizeType>::max() / sizeof(FwSizeType));
    
          // Allocate indices list and construct it when valid
    
      ✗
          size = depth * sizeof(FwSizeType);
    
      ✗
          allocation = allocator.allocate(identifier, size, alignof(FwSizeType));
    
      ✗
          if (allocation == nullptr) {
    
      ✗
              status = QueueInterface::Status::ALLOCATION_FAILED;
    
      ✗
          } else if (size < (depth * sizeof(FwSizeType))) {
    
      ✗
              allocator.deallocate(identifier, allocation);
    
      ✗
              status = QueueInterface::Status::ALLOCATION_FAILED;
    
          } else {
    
      ✗
              indices = Fw::arrayPlacementNew<FwSizeType>(Fw::ByteArray(static_cast<U8*>(allocation), size), depth);
    
          }
    
          // Allocate sizes list and construct it when valid
    
      ✗
          if (status == QueueInterface::Status::OP_OK) {
    
      ✗
              size = depth * sizeof(FwSizeType);
    
      ✗
              allocation = allocator.allocate(identifier, size, alignof(FwSizeType));
    
      ✗
              if (allocation == nullptr) {
    
      ✗
                  allocator.deallocate(identifier, indices);
    
      ✗
                  status = QueueInterface::Status::ALLOCATION_FAILED;
    
      ✗
              } else if (size < (depth * sizeof(FwSizeType))) {
    
      ✗
                  allocator.deallocate(identifier, indices);
    
      ✗
                  allocator.deallocate(identifier, allocation);
    
      ✗
                  status = QueueInterface::Status::ALLOCATION_FAILED;
    
              } else {
    
      ✗
                  sizes = Fw::arrayPlacementNew<FwSizeType>(Fw::ByteArray(static_cast<U8*>(allocation), size), depth);
    
              }
    
          }
    
          // Allocate data
    
      ✗
          if (status == QueueInterface::Status::OP_OK) {
    
              // Prevent integer overflow when computing allocation size (depth * messageSize)
    
      ✗
              FW_ASSERT((depth == 0) || (messageSize <= std::numeric_limits<FwSizeType>::max() / depth));
    
      ✗
              size = depth * messageSize;
    
      ✗
              allocation = allocator.allocate(identifier, size, alignof(U8));
    
      ✗
              if (allocation == nullptr) {
    
      ✗
                  allocator.deallocate(identifier, indices);
    
      ✗
                  allocator.deallocate(identifier, sizes);
    
      ✗
                  status = QueueInterface::Status::ALLOCATION_FAILED;
    
      ✗
              } else if (size < (depth * messageSize)) {
    
      ✗
                  allocator.deallocate(identifier, indices);
    
      ✗
                  allocator.deallocate(identifier, sizes);
    
      ✗
                  allocator.deallocate(identifier, allocation);
    
      ✗
                  status = QueueInterface::Status::ALLOCATION_FAILED;
    
              } else {
    
      ✗
                  data = static_cast<U8*>(allocation);
    
              }
    
          }
    
          // Allocate data for max heap
    
      ✗
          if (status == QueueInterface::Status::OP_OK) {
    
      ✗
              size = Types::MaxHeap::ELEMENT_SIZE * depth;
    
      ✗
              allocation = allocator.allocate(identifier, size, Types::MaxHeap::ALIGNMENT);
    
      ✗
              if (allocation == nullptr) {
    
      ✗
                  allocator.deallocate(identifier, indices);
    
      ✗
                  allocator.deallocate(identifier, sizes);
    
      ✗
                  allocator.deallocate(identifier, data);
    
      ✗
                  status = QueueInterface::Status::ALLOCATION_FAILED;
    
      ✗
              } else if (size < (Types::MaxHeap::ELEMENT_SIZE * depth)) {
    
      ✗
                  allocator.deallocate(identifier, indices);
    
      ✗
                  allocator.deallocate(identifier, sizes);
    
      ✗
                  allocator.deallocate(identifier, data);
    
      ✗
                  allocator.deallocate(identifier, allocation);
    
      ✗
                  status = QueueInterface::Status::ALLOCATION_FAILED;
    
              } else {
    
      ✗
                  heap_pointer = static_cast<U8*>(allocation);
    
      ✗
                  this->m_handle.m_heap.create(depth, Fw::ByteArray(static_cast<U8*>(allocation), size));
    
              }
    
          }
    
          // Set up structures when all allocations succeeded
    
      ✗
          if (status == QueueInterface::Status::OP_OK) {
    
              // Assign initial indices and sizes
    
      ✗
              for (FwSizeType i = 0; i < depth; i++) {
    
      ✗
                  indices[i] = i;
    
      ✗
                  sizes[i] = 0;
    
              }
    
              // Set local tracking variables
    
      ✗
              this->m_handle.m_id = id;
    
      ✗
              this->m_handle.m_maxSize = messageSize;
    
      ✗
              this->m_handle.m_indices = indices;
    
      ✗
              this->m_handle.m_data = data;
    
      ✗
              this->m_handle.m_sizes = sizes;
    
      ✗
              this->m_handle.m_heap_pointer = heap_pointer;
    
      ✗
              this->m_handle.m_startIndex = 0;
    
      ✗
              this->m_handle.m_stopIndex = 0;
    
      ✗
              this->m_handle.m_depth = depth;
    
      ✗
              this->m_handle.m_highMark = 0;
    
          }
    
      ✗
          return status;
    
      }
    
      ✗
      void PriorityQueue::teardown() {
    
      ✗
          this->teardownInternal();
    
      ✗
      }
    
      ✗
      void PriorityQueue::teardownInternal() {
    
      ✗
          if (this->m_handle.m_data != nullptr) {
    
              // All backing arrays are allocated together in create()
    
      ✗
              FW_ASSERT(this->m_handle.m_indices != nullptr);
    
      ✗
              FW_ASSERT(this->m_handle.m_sizes != nullptr);
    
      ✗
              FW_ASSERT(this->m_handle.m_heap_pointer != nullptr);
    
      ✗
              const FwEnumStoreType identifier = this->m_handle.m_id;
    
      ✗
              Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator(
    
      ✗
                  Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE);
    
      ✗
              allocator.deallocate(identifier, this->m_handle.m_data);
    
      ✗
              allocator.deallocate(identifier, this->m_handle.m_indices);
    
      ✗
              allocator.deallocate(identifier, this->m_handle.m_sizes);
    
      ✗
              this->m_handle.m_heap.teardown();
    
      ✗
              allocator.deallocate(identifier, this->m_handle.m_heap_pointer);
    
              // Set these pointers to nullptr
    
      ✗
              this->m_handle.m_data = nullptr;
    
      ✗
              this->m_handle.m_indices = nullptr;
    
      ✗
              this->m_handle.m_sizes = nullptr;
    
          }
    
      ✗
      }
    
      ✗
      QueueInterface::Status PriorityQueue::send(const U8* buffer,
    
                                                 FwSizeType size,
    
                                                 FwQueuePriorityType priority,
    
                                                 QueueInterface::BlockingType blockType) {
    
          // Check for sizing problem before locking
    
      ✗
          if (size > this->m_handle.m_maxSize) {
    
      ✗
              return QueueInterface::Status::SIZE_MISMATCH;
    
          }
    
          // Artificial block scope for scope lock ensuring an unlock in all cases and ensuring an unlock before notify
    
          {
    
      ✗
              Os::ScopeLock lock(this->m_handle.m_data_lock);
    
      ✗
              if (this->m_handle.m_heap.isFull() and blockType == BlockingType::NONBLOCKING) {
    
      ✗
                  return QueueInterface::Status::FULL;
    
              }
    
              // Will loop and block until full is false
    
              // @non-terminating@: condition-variable wait loop
    
      ✗
              while (this->m_handle.m_heap.isFull()) {
    
      ✗
                  this->m_handle.m_full.wait(this->m_handle.m_data_lock);
    
              }
    
      ✗
              FwSizeType index = this->m_handle.find_index();
    
              // Space must exist, push must work
    
      ✗
              const bool pushed = this->m_handle.m_heap.push(priority, index);
    
      ✗
              FW_ASSERT(pushed);
    
      ✗
              this->m_handle.store_data(index, buffer, size);
    
      ✗
              this->m_handle.m_sizes[index] = size;
    
      ✗
              this->m_handle.m_highMark = std::max(this->m_handle.m_highMark, this->getMessagesAvailable());
    
      ✗
          }
    
      ✗
          this->m_handle.m_empty.notify();
    
      ✗
          return QueueInterface::Status::OP_OK;
    
      }
    
      ✗
      QueueInterface::Status PriorityQueue::receive(U8* destination,
    
                                                    FwSizeType capacity,
    
                                                    QueueInterface::BlockingType blockType,
    
                                                    FwSizeType& actualSize,
    
                                                    FwQueuePriorityType& priority) {
    
          {
    
      ✗
              Os::ScopeLock lock(this->m_handle.m_data_lock);
    
      ✗
              if (this->m_handle.m_heap.isEmpty() and blockType == BlockingType::NONBLOCKING) {
    
      ✗
                  return QueueInterface::Status::EMPTY;
    
              }
    
              // Loop and lock while empty
    
              // @non-terminating@: condition-variable wait loop
    
      ✗
              while (this->m_handle.m_heap.isEmpty()) {
    
      ✗
                  this->m_handle.m_empty.wait(this->m_handle.m_data_lock);
    
              }
    
      ✗
              FwSizeType index;
    
              // Message must exist, so pop must pass and size must be valid
    
      ✗
              const bool popped = this->m_handle.m_heap.pop(priority, index);
    
      ✗
              FW_ASSERT(popped);
    
      ✗
              actualSize = this->m_handle.m_sizes[index];
    
      ✗
              FW_ASSERT(actualSize <= capacity);
    
      ✗
              this->m_handle.load_data(index, destination, actualSize);
    
      ✗
              this->m_handle.return_index(index);
    
      ✗
          }
    
      ✗
          this->m_handle.m_full.notify();
    
      ✗
          return QueueInterface::Status::OP_OK;
    
      }
    
      ✗
      FwSizeType PriorityQueue::getMessagesAvailable() const {
    
      ✗
          return this->m_handle.m_heap.getSize();
    
      }
    
      ✗
      FwSizeType PriorityQueue::getMessageHighWaterMark() const {
    
          // Safe to cast away const in this context because scope lock will restore unlocked state on return
    
      ✗
          Os::ScopeLock lock(const_cast<Mutex&>(this->m_handle.m_data_lock));
    
      ✗
          return this->m_handle.m_highMark;
    
      ✗
      }
    
      ✗
      QueueHandle* PriorityQueue::getHandle() {
    
      ✗
          return &this->m_handle;
    
      }
    
      }  // namespace Generic
    
      }  // namespace Os

Line	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	✗	FwSizeType PriorityQueueHandle ::find_index() {
17	✗	FW_ASSERT(this->m_depth > 0);
18	✗	FwSizeType index = this->m_indices[this->m_startIndex % this->m_depth];
19	✗	this->m_startIndex = (this->m_startIndex + 1) % this->m_depth;
20	✗	return index;
21		}
22
23	✗	void PriorityQueueHandle ::return_index(FwSizeType index) {
24	✗	FW_ASSERT(this->m_depth > 0);
25	✗	this->m_indices[this->m_stopIndex % this->m_depth] = index;
26	✗	this->m_stopIndex = (this->m_stopIndex + 1) % this->m_depth;
27	✗	}
28
29	✗	void PriorityQueueHandle ::store_data(FwSizeType index, const U8* data, FwSizeType size) {
30	✗	FW_ASSERT(size <= this->m_maxSize);
31	✗	FW_ASSERT(index < this->m_depth);
32
33	✗	FwSizeType offset = this->m_maxSize * index;
34	✗	(void)::memcpy(this->m_data + offset, data, static_cast<size_t>(size));
35	✗	this->m_sizes[index] = size;
36	✗	}
37
38	✗	void PriorityQueueHandle ::load_data(FwSizeType index, U8* destination, FwSizeType size) {
39	✗	FW_ASSERT(size <= this->m_maxSize);
40	✗	FW_ASSERT(index < this->m_depth);
41	✗	FwSizeType offset = this->m_maxSize * index;
42	✗	(void)::memcpy(destination, this->m_data + offset, static_cast<size_t>(size));
43	✗	}
44
45	✗	PriorityQueue::~PriorityQueue() {}
46
47	✗	QueueInterface::Status PriorityQueue::create(FwEnumStoreType id,
48		const Fw::ConstStringBase& name,
49		FwSizeType depth,
50		FwSizeType messageSize) {
51	✗	const FwEnumStoreType identifier = id;
52	✗	QueueInterface::Status status = Os::QueueInterface::Status::OP_OK;
53		// Ensure we are created exactly once
54	✗	FW_ASSERT(this->m_handle.m_indices == nullptr);
55	✗	FW_ASSERT(this->m_handle.m_sizes == nullptr);
56	✗	FW_ASSERT(this->m_handle.m_data == nullptr);
57
58		// Get the memory allocator configured for priority queues
59	✗	Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator(
60	✗	Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE);
61
62		// Allocate indices list
63	✗	void* allocation = nullptr;
64	✗	FwSizeType size = 0;
65	✗	FwSizeType* indices = nullptr;
66	✗	FwSizeType* sizes = nullptr;
67	✗	U8* data = nullptr;
68	✗	U8* heap_pointer = nullptr;
69
70		// Prevent integer overflow when computing allocation size (depth * sizeof(FwSizeType))
71	✗	FW_ASSERT(depth < std::numeric_limits<FwSizeType>::max() / sizeof(FwSizeType));
72
73		// Allocate indices list and construct it when valid
74	✗	size = depth * sizeof(FwSizeType);
75	✗	allocation = allocator.allocate(identifier, size, alignof(FwSizeType));
76	✗	if (allocation == nullptr) {
77	✗	status = QueueInterface::Status::ALLOCATION_FAILED;
78	✗	} else if (size < (depth * sizeof(FwSizeType))) {
79	✗	allocator.deallocate(identifier, allocation);
80	✗	status = QueueInterface::Status::ALLOCATION_FAILED;
81		} else {
82	✗	indices = Fw::arrayPlacementNew<FwSizeType>(Fw::ByteArray(static_cast<U8*>(allocation), size), depth);
83		}
84
85		// Allocate sizes list and construct it when valid
86	✗	if (status == QueueInterface::Status::OP_OK) {
87	✗	size = depth * sizeof(FwSizeType);
88	✗	allocation = allocator.allocate(identifier, size, alignof(FwSizeType));
89	✗	if (allocation == nullptr) {
90	✗	allocator.deallocate(identifier, indices);
91	✗	status = QueueInterface::Status::ALLOCATION_FAILED;
92	✗	} 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	✗	sizes = Fw::arrayPlacementNew<FwSizeType>(Fw::ByteArray(static_cast<U8*>(allocation), size), depth);
98		}
99		}
100		// Allocate data
101	✗	if (status == QueueInterface::Status::OP_OK) {
102		// Prevent integer overflow when computing allocation size (depth * messageSize)
103	✗	FW_ASSERT((depth == 0) \|\| (messageSize <= std::numeric_limits<FwSizeType>::max() / depth));
104	✗	size = depth * messageSize;
105	✗	allocation = allocator.allocate(identifier, size, alignof(U8));
106	✗	if (allocation == nullptr) {
107	✗	allocator.deallocate(identifier, indices);
108	✗	allocator.deallocate(identifier, sizes);
109	✗	status = QueueInterface::Status::ALLOCATION_FAILED;
110	✗	} 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	✗	data = static_cast<U8*>(allocation);
117		}
118		}
119		// Allocate data for max heap
120	✗	if (status == QueueInterface::Status::OP_OK) {
121	✗	size = Types::MaxHeap::ELEMENT_SIZE * depth;
122	✗	allocation = allocator.allocate(identifier, size, Types::MaxHeap::ALIGNMENT);
123	✗	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	✗	} 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	✗	heap_pointer = static_cast<U8*>(allocation);
136	✗	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	✗	if (status == QueueInterface::Status::OP_OK) {
141		// Assign initial indices and sizes
142	✗	for (FwSizeType i = 0; i < depth; i++) {
143	✗	indices[i] = i;
144	✗	sizes[i] = 0;
145		}
146		// Set local tracking variables
147	✗	this->m_handle.m_id = id;
148	✗	this->m_handle.m_maxSize = messageSize;
149	✗	this->m_handle.m_indices = indices;
150	✗	this->m_handle.m_data = data;
151	✗	this->m_handle.m_sizes = sizes;
152	✗	this->m_handle.m_heap_pointer = heap_pointer;
153	✗	this->m_handle.m_startIndex = 0;
154	✗	this->m_handle.m_stopIndex = 0;
155	✗	this->m_handle.m_depth = depth;
156	✗	this->m_handle.m_highMark = 0;
157		}
158	✗	return status;
159		}
160
161	✗	void PriorityQueue::teardown() {
162	✗	this->teardownInternal();
163	✗	}
164
165	✗	void PriorityQueue::teardownInternal() {
166	✗	if (this->m_handle.m_data != nullptr) {
167		// All backing arrays are allocated together in create()
168	✗	FW_ASSERT(this->m_handle.m_indices != nullptr);
169	✗	FW_ASSERT(this->m_handle.m_sizes != nullptr);
170	✗	FW_ASSERT(this->m_handle.m_heap_pointer != nullptr);
171	✗	const FwEnumStoreType identifier = this->m_handle.m_id;
172	✗	Fw::MemAllocator& allocator = Fw::MemAllocatorRegistry::getInstance().getAnAllocator(
173	✗	Fw::MemoryAllocation::MemoryAllocatorType::OS_GENERIC_PRIORITY_QUEUE);
174	✗	allocator.deallocate(identifier, this->m_handle.m_data);
175	✗	allocator.deallocate(identifier, this->m_handle.m_indices);
176	✗	allocator.deallocate(identifier, this->m_handle.m_sizes);
177	✗	this->m_handle.m_heap.teardown();
178	✗	allocator.deallocate(identifier, this->m_handle.m_heap_pointer);
179
180		// Set these pointers to nullptr
181	✗	this->m_handle.m_data = nullptr;
182	✗	this->m_handle.m_indices = nullptr;
183	✗	this->m_handle.m_sizes = nullptr;
184		}
185	✗	}
186
187	✗	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	✗	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	✗	Os::ScopeLock lock(this->m_handle.m_data_lock);
198	✗	if (this->m_handle.m_heap.isFull() and blockType == BlockingType::NONBLOCKING) {
199	✗	return QueueInterface::Status::FULL;
200		}
201		// Will loop and block until full is false
202		// @non-terminating@: condition-variable wait loop
203	✗	while (this->m_handle.m_heap.isFull()) {
204	✗	this->m_handle.m_full.wait(this->m_handle.m_data_lock);
205		}
206	✗	FwSizeType index = this->m_handle.find_index();
207
208		// Space must exist, push must work
209	✗	const bool pushed = this->m_handle.m_heap.push(priority, index);
210	✗	FW_ASSERT(pushed);
211	✗	this->m_handle.store_data(index, buffer, size);
212	✗	this->m_handle.m_sizes[index] = size;
213	✗	this->m_handle.m_highMark = std::max(this->m_handle.m_highMark, this->getMessagesAvailable());
214	✗	}
215	✗	this->m_handle.m_empty.notify();
216	✗	return QueueInterface::Status::OP_OK;
217		}
218
219	✗	QueueInterface::Status PriorityQueue::receive(U8* destination,
220		FwSizeType capacity,
221		QueueInterface::BlockingType blockType,
222		FwSizeType& actualSize,
223		FwQueuePriorityType& priority) {
224		{
225	✗	Os::ScopeLock lock(this->m_handle.m_data_lock);
226	✗	if (this->m_handle.m_heap.isEmpty() and blockType == BlockingType::NONBLOCKING) {
227	✗	return QueueInterface::Status::EMPTY;
228		}
229		// Loop and lock while empty
230		// @non-terminating@: condition-variable wait loop
231	✗	while (this->m_handle.m_heap.isEmpty()) {
232	✗	this->m_handle.m_empty.wait(this->m_handle.m_data_lock);
233		}
234
235	✗	FwSizeType index;
236		// Message must exist, so pop must pass and size must be valid
237	✗	const bool popped = this->m_handle.m_heap.pop(priority, index);
238	✗	FW_ASSERT(popped);
239	✗	actualSize = this->m_handle.m_sizes[index];
240	✗	FW_ASSERT(actualSize <= capacity);
241	✗	this->m_handle.load_data(index, destination, actualSize);
242	✗	this->m_handle.return_index(index);
243	✗	}
244	✗	this->m_handle.m_full.notify();
245	✗	return QueueInterface::Status::OP_OK;
246		}
247
248	✗	FwSizeType PriorityQueue::getMessagesAvailable() const {
249	✗	return this->m_handle.m_heap.getSize();
250		}
251
252	✗	FwSizeType PriorityQueue::getMessageHighWaterMark() const {
253		// Safe to cast away const in this context because scope lock will restore unlocked state on return
254	✗	Os::ScopeLock lock(const_cast<Mutex&>(this->m_handle.m_data_lock));
255	✗	return this->m_handle.m_highMark;
256	✗	}
257
258	✗	QueueHandle* PriorityQueue::getHandle() {
259	✗	return &this->m_handle;
260		}
261
262		} // namespace Generic
263		} // namespace Os
264