F´ Flight Software - C/C++ Documentation  devel
A framework for building embedded system applications to NASA flight quality standards.
PriorityQueue.cpp
Go to the documentation of this file.
1 // ======================================================================
2 // \title Os/Generic/PriorityQueue.cpp
3 // \brief priority queue implementation for Os::Queue
4 // ======================================================================
5 
6 #include "PriorityQueue.hpp"
7 #include <Fw/Types/Assert.hpp>
8 #include <cstring>
9 #include <new>
10 
11 namespace Os {
12 namespace Generic {
13 
15  FwSizeType index = this->m_indices[this->m_startIndex % this->m_depth];
16  this->m_startIndex = (this->m_startIndex + 1) % this->m_depth;
17  return index;
18 }
19 
21  this->m_indices[this->m_stopIndex % this->m_depth] = index;
22  this->m_stopIndex = (this->m_stopIndex + 1) % this->m_depth;
23 }
24 
25 void PriorityQueueHandle ::store_data(FwSizeType index, const U8* data, FwSizeType size) {
26  FW_ASSERT(size <= this->m_maxSize);
27  FW_ASSERT(index < this->m_depth);
28 
29  FwSizeType offset = this->m_maxSize * index;
30  ::memcpy(this->m_data + offset, data, size);
31  this->m_sizes[index] = size;
32 }
33 
34 void PriorityQueueHandle ::load_data(FwSizeType index, U8* destination, FwSizeType size) {
35  FW_ASSERT(size <= this->m_maxSize);
36  FW_ASSERT(index < this->m_depth);
37  FwSizeType offset = this->m_maxSize * index;
38  ::memcpy(destination, this->m_data + offset, size);
39 }
40 
42  delete[] this->m_handle.m_data;
43  delete[] this->m_handle.m_indices;
44  delete[] this->m_handle.m_sizes;
45 }
46 
48  // Ensure we are created exactly once
49  FW_ASSERT(this->m_handle.m_indices == nullptr);
50  FW_ASSERT(this->m_handle.m_sizes == nullptr);
51  FW_ASSERT(this->m_handle.m_data == nullptr);
52 
53  // Allocate indices list
54  FwSizeType* indices = new (std::nothrow) FwSizeType[depth];
55  if (indices == nullptr) {
56  return QueueInterface::Status::UNKNOWN_ERROR;
57  }
58  // Allocate sizes list or clean-up
59  FwSizeType* sizes = new (std::nothrow) FwSizeType[depth];
60  if (sizes == nullptr) {
61  delete[] indices;
62  return QueueInterface::Status::UNKNOWN_ERROR;
63  }
64  // Allocate sizes list or clean-up
65  U8* data = new (std::nothrow) U8[depth * messageSize];
66  if (data == nullptr) {
67  delete[] indices;
68  delete[] sizes;
69  return QueueInterface::Status::UNKNOWN_ERROR;
70  }
71  // Allocate max heap or clean-up
72  bool created = this->m_handle.m_heap.create(depth);
73  if (not created) {
74  delete[] indices;
75  delete[] sizes;
76  delete[] data;
77  return QueueInterface::Status::UNKNOWN_ERROR;
78  }
79  // Assign initial indices and sizes
80  for (FwSizeType i = 0; i < depth; i++) {
81  indices[i] = i;
82  sizes[i] = 0;
83  }
84  // Set local tracking variables
85  this->m_handle.m_maxSize = messageSize;
86  this->m_handle.m_indices = indices;
87  this->m_handle.m_data = data;
88  this->m_handle.m_sizes = sizes;
89  this->m_handle.m_startIndex = 0;
90  this->m_handle.m_stopIndex = 0;
91  this->m_handle.m_depth = depth;
92  this->m_handle.m_highMark = 0;
93 
95 }
96 
98  FwSizeType size,
99  FwQueuePriorityType priority,
100  QueueInterface::BlockingType blockType) {
101  // Check for sizing problem before locking
102  if (size > this->m_handle.m_maxSize) {
103  return QueueInterface::Status::SIZE_MISMATCH;
104  }
105  // Artificial block scope for scope lock ensuring an unlock in all cases and ensuring an unlock before notify
106  {
107  Os::ScopeLock lock(this->m_handle.m_data_lock);
108  if (this->m_handle.m_heap.isFull() and blockType == BlockingType::NONBLOCKING) {
109  return QueueInterface::Status::FULL;
110  }
111  // Will loop and block until full is false
112  while (this->m_handle.m_heap.isFull()) {
113  this->m_handle.m_full.wait(this->m_handle.m_data_lock);
114  }
115  FwSizeType index = this->m_handle.find_index();
116 
117  // Space must exist, push must work
118  FW_ASSERT(this->m_handle.m_heap.push(priority, index));
119  this->m_handle.store_data(index, buffer, size);
120  this->m_handle.m_sizes[index] = size;
121  this->m_handle.m_highMark = FW_MAX(this->m_handle.m_highMark, this->getMessagesAvailable());
122  }
123  this->m_handle.m_empty.notify();
125 }
126 
128  FwSizeType capacity,
130  FwSizeType& actualSize,
131  FwQueuePriorityType& priority) {
132  {
133  Os::ScopeLock lock(this->m_handle.m_data_lock);
134  if (this->m_handle.m_heap.isEmpty() and blockType == BlockingType::NONBLOCKING) {
135  return QueueInterface::Status::EMPTY;
136  }
137  // Loop and lock while empty
138  while (this->m_handle.m_heap.isEmpty()) {
140  }
141 
142  FwSizeType index;
143  // Message must exist, so pop must pass and size must be valid
144  FW_ASSERT(this->m_handle.m_heap.pop(priority, index));
145  actualSize = this->m_handle.m_sizes[index];
146  FW_ASSERT(actualSize <= capacity);
147  this->m_handle.load_data(index, destination, actualSize);
148  this->m_handle.return_index(index);
149  }
150  this->m_handle.m_full.notify();
152 }
153 
155  return this->m_handle.m_heap.getSize();
156 }
157 
159  // Safe to cast away const in this context because scope lock will restore unlocked state on return
160  Os::ScopeLock lock(const_cast<Mutex&>(this->m_handle.m_data_lock));
161  return this->m_handle.m_highMark;
162 }
163 
165  return &this->m_handle;
166 }
167 
168 } // namespace Generic
169 } // namespace Os
#define FW_ASSERT(...)
Definition: Assert.hpp:14
uint8_t U8
8-bit unsigned integer
Definition: BasicTypes.h:30
#define FW_MAX(a, b)
MAX macro.
Definition: BasicTypes.h:71
PlatformSizeType FwSizeType
Definition: FpConfig.h:35
PlatformQueuePriorityType FwQueuePriorityType
Definition: FpConfig.h:55
void notify() override
notify a single waiter on this condition variable
Definition: Condition.cpp:20
void wait(Os::Mutex &mutex) override
wait on a condition variable
Definition: Condition.cpp:11
Status receive(U8 *destination, FwSizeType capacity, BlockingType blockType, FwSizeType &actualSize, FwQueuePriorityType &priority) override
receive a message from the queue
Status send(const U8 *buffer, FwSizeType size, FwQueuePriorityType priority, BlockingType blockType) override
send a message into the queue
virtual ~PriorityQueue()
default queue destructor
PriorityQueueHandle m_handle
Status create(const Fw::StringBase &name, FwSizeType depth, FwSizeType messageSize) override
create queue storage
FwSizeType getMessagesAvailable() const override
get number of messages available
FwSizeType getMessageHighWaterMark() const override
get maximum messages stored at any given time
QueueHandle * getHandle() override
return the underlying queue handle (implementation specific)
QueueHandle parent class.
Definition: Queue.hpp:19
BlockingType
message type
Definition: Queue.hpp:44
Status
status returned from the queue send function
Definition: Queue.hpp:30
locks a mutex within the current scope
Definition: Mutex.hpp:79
FwSizeType getSize() const
Get the current number of elements on the heap.
Definition: MaxHeap.cpp:144
bool isFull()
Is the heap full?
Definition: MaxHeap.cpp:134
bool isEmpty()
Is the heap empty?
Definition: MaxHeap.cpp:139
bool pop(FwQueuePriorityType &value, FwSizeType &id)
Pop an item from the heap.
Definition: MaxHeap.cpp:105
bool create(FwSizeType capacity)
MaxHeap creation.
Definition: MaxHeap.cpp:45
bool push(FwQueuePriorityType value, FwSizeType id)
Push an item onto the heap.
Definition: MaxHeap.cpp:57
@ OP_OK
Operation succeeded.
Definition: Os.hpp:26
Os::Mutex m_data_lock
Lock against data manipulation.
void store_data(FwSizeType index, const U8 *source, FwSizeType size)
store data into a set index in the data store
FwSizeType m_startIndex
Start index of the circular data structure.
void load_data(FwSizeType index, U8 *destination, FwSizeType capacity)
load data from a set index in the data store
void return_index(FwSizeType index)
return index to the circular data structure
FwSizeType * m_sizes
Size store for each method.
FwSizeType m_maxSize
Maximum size allowed of a message.
FwSizeType find_index()
find an available index to store data from the list
Types::MaxHeap m_heap
MaxHeap data store for tracking priority.
FwSizeType * m_indices
List of indices into data.
Os::ConditionVariable m_empty
Queue empty condition variable to support blocking.
FwSizeType m_stopIndex
End index of the circular data structure.
U8 * m_data
Pointer to data allocation.
FwSizeType m_depth
Depth of the queue.
FwSizeType m_highMark
Message count high water mark.
Os::ConditionVariable m_full
Queue full condition variable to support blocking.