| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * Queue.cpp: | ||
| 3 | * | ||
| 4 | * Implementation of the queue data type. | ||
| 5 | * | ||
| 6 | * Created on: July 5th, 2022 | ||
| 7 | * Author: lestarch | ||
| 8 | * | ||
| 9 | */ | ||
| 10 | #include "Queue.hpp" | ||
| 11 | #include <Fw/Types/Assert.hpp> | ||
| 12 | |||
| 13 | namespace Types { | ||
| 14 | |||
| 15 | 3 | Queue::Queue() : m_internal(), m_message_size(0), m_mode(QUEUE_FIFO), m_overflow_mode(QUEUE_DROP_NEWEST) {} | |
| 16 | |||
| 17 | 3 | void Queue::setup(U8* const storage, | |
| 18 | const FwSizeType storage_size, | ||
| 19 | const FwSizeType depth, | ||
| 20 | const FwSizeType message_size, | ||
| 21 | const QueueMode mode, | ||
| 22 | const QueueOverflowMode overflow_mode) { | ||
| 23 | // Ensure that enough storage was supplied | ||
| 24 | 3 | const FwSizeType total_needed_size = depth * message_size; | |
| 25 | 3 | FW_ASSERT(storage_size >= total_needed_size, static_cast<FwAssertArgType>(storage_size), | |
| 26 | static_cast<FwAssertArgType>(depth), static_cast<FwAssertArgType>(message_size)); | ||
| 27 | 3 | FW_ASSERT(mode == QUEUE_FIFO || mode == QUEUE_LIFO, static_cast<FwAssertArgType>(mode)); | |
| 28 | 3 | FW_ASSERT(overflow_mode == QUEUE_DROP_OLDEST || overflow_mode == QUEUE_DROP_NEWEST, | |
| 29 | static_cast<FwAssertArgType>(overflow_mode)); | ||
| 30 | 3 | m_internal.setup(storage, total_needed_size); | |
| 31 | 3 | m_message_size = message_size; | |
| 32 | 3 | m_mode = mode; | |
| 33 | 3 | m_overflow_mode = overflow_mode; | |
| 34 | 3 | } | |
| 35 | |||
| 36 | ✗ | Fw::SerializeStatus Queue::enqueue(const U8* const message, const FwSizeType size) { | |
| 37 | ✗ | FW_ASSERT(m_message_size > 0, static_cast<FwAssertArgType>(m_message_size)); // Ensure initialization | |
| 38 | ✗ | FW_ASSERT(m_message_size == size, static_cast<FwAssertArgType>(size), | |
| 39 | static_cast<FwAssertArgType>(m_message_size)); // Message size is as expected | ||
| 40 | ✗ | FW_ASSERT(message != nullptr); | |
| 41 | ✗ | Fw::SerializeStatus status = m_internal.serialize(message, m_message_size); | |
| 42 | |||
| 43 | // If queue is full and we're in DROP_OLDEST mode, remove the oldest and try again | ||
| 44 | ✗ | if (status == Fw::FW_SERIALIZE_NO_ROOM_LEFT && m_overflow_mode == QUEUE_DROP_OLDEST) { | |
| 45 | // Remove the oldest message by rotating | ||
| 46 | ✗ | Fw::SerializeStatus rotate_status = m_internal.rotate(m_message_size); | |
| 47 | ✗ | if (rotate_status != Fw::FW_SERIALIZE_OK) { | |
| 48 | ✗ | return rotate_status; | |
| 49 | } | ||
| 50 | |||
| 51 | // Now enqueue the new message (should succeed since we just freed space) | ||
| 52 | ✗ | status = m_internal.serialize(message, m_message_size); | |
| 53 | ✗ | if (status != Fw::FW_SERIALIZE_OK) { | |
| 54 | ✗ | return status; | |
| 55 | } else { | ||
| 56 | // Let the caller know we deleted data | ||
| 57 | ✗ | return Fw::FW_SERIALIZE_DISCARDED_EXISTING; | |
| 58 | } | ||
| 59 | } | ||
| 60 | |||
| 61 | ✗ | return status; | |
| 62 | } | ||
| 63 | |||
| 64 | template <typename T> | ||
| 65 | 1336 | Fw::SerializeStatus Queue::enqueue_impl(const T& message) { | |
| 66 | 1336 | FW_ASSERT(m_message_size > 0, static_cast<FwAssertArgType>(m_message_size)); // Ensure initialization | |
| 67 | 1336 | Fw::SerializeStatus status = m_internal.serialize(message, m_message_size); | |
| 68 | |||
| 69 | // If queue is full and we're in DROP_OLDEST mode, remove the oldest and try again | ||
| 70 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 668 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
1336 | if (status == Fw::FW_SERIALIZE_NO_ROOM_LEFT && m_overflow_mode == QUEUE_DROP_OLDEST) { |
| 71 | // Remove the oldest message by rotating | ||
| 72 | ✗ | Fw::SerializeStatus rotate_status = m_internal.rotate(m_message_size); | |
| 73 | ✗ | if (rotate_status != Fw::FW_SERIALIZE_OK) { | |
| 74 | ✗ | return rotate_status; | |
| 75 | } | ||
| 76 | |||
| 77 | // Now enqueue the new message (should succeed since we just freed space) | ||
| 78 | ✗ | status = m_internal.serialize(message, m_message_size); | |
| 79 | ✗ | if (status != Fw::FW_SERIALIZE_OK) { | |
| 80 | ✗ | return status; | |
| 81 | } else { | ||
| 82 | // Let the caller know we deleted data | ||
| 83 | ✗ | return Fw::FW_SERIALIZE_DISCARDED_EXISTING; | |
| 84 | } | ||
| 85 | } | ||
| 86 | |||
| 87 | 1336 | return status; | |
| 88 | } | ||
| 89 | |||
| 90 | ✗ | Fw::SerializeStatus Queue::enqueue(const Fw::Serializable& message) { | |
| 91 | ✗ | return this->enqueue_impl(message); | |
| 92 | } | ||
| 93 | |||
| 94 | 668 | Fw::SerializeStatus Queue::enqueue(const Fw::LinearBufferBase& message) { | |
| 95 | 668 | return this->enqueue_impl(message); | |
| 96 | } | ||
| 97 | |||
| 98 | ✗ | Fw::SerializeStatus Queue::dequeue(U8* const message, const FwSizeType size) { | |
| 99 | ✗ | FW_ASSERT(m_message_size > 0); // Ensure initialization | |
| 100 | ✗ | FW_ASSERT(m_message_size <= size, static_cast<FwAssertArgType>(size), | |
| 101 | static_cast<FwAssertArgType>(m_message_size)); // Sufficient storage space for read message | ||
| 102 | ✗ | FW_ASSERT(message != nullptr); | |
| 103 | Fw::SerializeStatus result; | ||
| 104 | |||
| 105 | ✗ | if (m_mode == QUEUE_FIFO) { | |
| 106 | // FIFO: Dequeue from the front (oldest message) | ||
| 107 | ✗ | result = m_internal.peek(message, m_message_size, 0); | |
| 108 | ✗ | if (result != Fw::FW_SERIALIZE_OK) { | |
| 109 | ✗ | return result; | |
| 110 | } | ||
| 111 | ✗ | return m_internal.rotate(m_message_size); | |
| 112 | } else { | ||
| 113 | // LIFO: Dequeue from the back (newest message) | ||
| 114 | ✗ | FwSizeType current_size = m_internal.get_allocated_size(); | |
| 115 | ✗ | if (current_size < m_message_size) { | |
| 116 | ✗ | return Fw::FW_DESERIALIZE_BUFFER_EMPTY; | |
| 117 | } | ||
| 118 | ✗ | FwSizeType offset = current_size - m_message_size; | |
| 119 | ✗ | result = m_internal.peek(message, m_message_size, offset); | |
| 120 | ✗ | if (result != Fw::FW_SERIALIZE_OK) { | |
| 121 | ✗ | return result; | |
| 122 | } | ||
| 123 | ✗ | return m_internal.trim(m_message_size); | |
| 124 | } | ||
| 125 | } | ||
| 126 | |||
| 127 | template <typename T> | ||
| 128 | 1336 | Fw::SerializeStatus Queue::dequeue_impl(T& message) { | |
| 129 | 1336 | FW_ASSERT(m_message_size > 0); // Ensure initialization | |
| 130 | Fw::SerializeStatus result; | ||
| 131 | |||
| 132 |
1/2✓ Branch 0 taken 668 times.
✗ Branch 1 not taken.
|
1336 | if (m_mode == QUEUE_FIFO) { |
| 133 | // FIFO: Dequeue from the front (oldest message) | ||
| 134 | 1336 | result = m_internal.peek(message, m_message_size, 0); | |
| 135 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 668 times.
|
1336 | if (result != Fw::FW_SERIALIZE_OK) { |
| 136 | ✗ | return result; | |
| 137 | } | ||
| 138 | 1336 | return m_internal.rotate(m_message_size); | |
| 139 | } else { | ||
| 140 | // LIFO: Dequeue from the back (newest message) | ||
| 141 | ✗ | FwSizeType current_size = m_internal.get_allocated_size(); | |
| 142 | ✗ | if (current_size < m_message_size) { | |
| 143 | ✗ | return Fw::FW_DESERIALIZE_BUFFER_EMPTY; | |
| 144 | } | ||
| 145 | ✗ | FwSizeType offset = current_size - m_message_size; | |
| 146 | ✗ | result = m_internal.peek(message, m_message_size, offset); | |
| 147 | ✗ | if (result != Fw::FW_SERIALIZE_OK) { | |
| 148 | ✗ | return result; | |
| 149 | } | ||
| 150 | ✗ | return m_internal.trim(m_message_size); | |
| 151 | } | ||
| 152 | } | ||
| 153 | |||
| 154 | ✗ | Fw::SerializeStatus Queue::dequeue(Fw::Serializable& message) { | |
| 155 | ✗ | return this->dequeue_impl(message); | |
| 156 | } | ||
| 157 | |||
| 158 | 668 | Fw::SerializeStatus Queue::dequeue(Fw::LinearBufferBase& message) { | |
| 159 | 668 | return this->dequeue_impl(message); | |
| 160 | } | ||
| 161 | |||
| 162 | ✗ | Fw::SerializeStatus Queue::popFront(U8* const message, const FwSizeType size) { | |
| 163 | ✗ | FW_ASSERT(m_message_size > 0); | |
| 164 | ✗ | FW_ASSERT(m_message_size <= size, static_cast<FwAssertArgType>(size), static_cast<FwAssertArgType>(m_message_size)); | |
| 165 | ✗ | FW_ASSERT(message != nullptr); | |
| 166 | // Always remove from the front (oldest), regardless of queue mode | ||
| 167 | ✗ | Fw::SerializeStatus result = m_internal.peek(message, m_message_size, 0); | |
| 168 | ✗ | if (result != Fw::FW_SERIALIZE_OK) { | |
| 169 | ✗ | return result; | |
| 170 | } | ||
| 171 | ✗ | return m_internal.rotate(m_message_size); | |
| 172 | } | ||
| 173 | |||
| 174 | template <typename T> | ||
| 175 | ✗ | Fw::SerializeStatus Queue::popFront_impl(T& message) { | |
| 176 | ✗ | FW_ASSERT(m_message_size > 0); | |
| 177 | // Always remove from the front (oldest), regardless of queue mode | ||
| 178 | ✗ | Fw::SerializeStatus result = m_internal.peek(message, m_message_size, 0); | |
| 179 | ✗ | if (result != Fw::FW_SERIALIZE_OK) { | |
| 180 | ✗ | return result; | |
| 181 | } | ||
| 182 | ✗ | return m_internal.rotate(m_message_size); | |
| 183 | } | ||
| 184 | |||
| 185 | ✗ | Fw::SerializeStatus Queue::popFront(Fw::Serializable& message) { | |
| 186 | ✗ | return this->popFront_impl(message); | |
| 187 | } | ||
| 188 | |||
| 189 | ✗ | Fw::SerializeStatus Queue::popFront(Fw::LinearBufferBase& message) { | |
| 190 | ✗ | return this->popFront_impl(message); | |
| 191 | } | ||
| 192 | |||
| 193 | 705 | FwSizeType Queue::get_high_water_mark() const { | |
| 194 | 705 | FW_ASSERT(m_message_size > 0, static_cast<FwAssertArgType>(m_message_size)); | |
| 195 | 705 | return m_internal.get_high_water_mark() / m_message_size; | |
| 196 | } | ||
| 197 | |||
| 198 | 705 | void Queue::clear_high_water_mark() { | |
| 199 | 705 | m_internal.clear_high_water_mark(); | |
| 200 | 705 | } | |
| 201 | |||
| 202 | 2218 | FwSizeType Queue::getQueueSize() const { | |
| 203 | 2218 | FW_ASSERT(m_message_size > 0, static_cast<FwAssertArgType>(m_message_size)); | |
| 204 | 2218 | return m_internal.get_allocated_size() / m_message_size; | |
| 205 | } | ||
| 206 | |||
| 207 | } // namespace Types | ||
| 208 |