| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // ====================================================================== | ||
| 2 | // \title AosFramer.cpp | ||
| 3 | // \author Will MacCormack (Aos Modifications) | ||
| 4 | // \brief cpp file for AosFramer component implementation class | ||
| 5 | // \details modified from thomas-bc's TmFramer | ||
| 6 | // ====================================================================== | ||
| 7 | |||
| 8 | #include "Svc/Ccsds/AosFramer/AosFramer.hpp" | ||
| 9 | #include "Svc/Ccsds/Utils/CRC16.hpp" | ||
| 10 | #include "config/FppConstantsAc.hpp" | ||
| 11 | |||
| 12 | namespace Svc { | ||
| 13 | |||
| 14 | namespace Ccsds { | ||
| 15 | // ---------------------------------------------------------------------- | ||
| 16 | // Component construction and destruction | ||
| 17 | // ---------------------------------------------------------------------- | ||
| 18 | |||
| 19 |
3/9✓ Branch 18 taken 7 times.
✓ Branch 21 taken 7 times.
✓ Branch 22 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
|
14 | AosFramer ::AosFramer(const char* const compName) : AosFramerComponentBase(compName) { |
| 20 | // Default to FECF on, Max Sized if you don't override w/ another configure call | ||
| 21 |
1/1✓ Branch 4 taken 7 times.
|
7 | configure(ComCfg::AosMaxFrameFixedSize, true); |
| 22 | 7 | } | |
| 23 | |||
| 24 |
3/4✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 11 taken 7 times.
✓ Branch 12 taken 7 times.
|
28 | AosFramer ::~AosFramer() {} |
| 25 | |||
| 26 | 16 | void AosFramer::configure(const U32 fixedFrameSize, | |
| 27 | const bool frameErrorControlField, | ||
| 28 | const U16 spacecraftId, | ||
| 29 | const U8 vcId, | ||
| 30 | const U8 idlePvns) { | ||
| 31 | // fixedFrameSize must be less than or equal to the maximum defined in ComCfg.fpp | ||
| 32 | 16 | FW_ASSERT(fixedFrameSize <= ComCfg::AosMaxFrameFixedSize, static_cast<FwAssertArgType>(fixedFrameSize)); | |
| 33 | |||
| 34 | // AOS Frame Fixed Size must be at least large enough to hold header, trailer and data | ||
| 35 | 16 | FW_ASSERT(fixedFrameSize > AOSHeader::SERIALIZED_SIZE + M_PDUHeader::SERIALIZED_SIZE + | |
| 36 | (frameErrorControlField ? AOSTrailer::SERIALIZED_SIZE : 0), | ||
| 37 | static_cast<FwAssertArgType>(fixedFrameSize)); | ||
| 38 | |||
| 39 | // Spacecraft ID is 10 bits (per CCSDS 732.0-B-5 Section 4.1.2.2) | ||
| 40 | 16 | FW_ASSERT((spacecraftId & 0xFC00) == 0, static_cast<FwAssertArgType>(spacecraftId)); | |
| 41 | |||
| 42 | // Virtual Channel ID is 6 bits (per CCSDS 732.0-B-5 Section 4.1.2.3) | ||
| 43 | 16 | FW_ASSERT((vcId & 0xC0) == 0, static_cast<FwAssertArgType>(vcId)); | |
| 44 | |||
| 45 | // AOS Framer must be provided with a protocol to use for Idle Packets | ||
| 46 | // Currently, only SPP idle packing is supported | ||
| 47 | // EPP is more optimal, however | ||
| 48 | 16 | FW_ASSERT((idlePvns & PvnBitfield::SPP_MASK) != 0, static_cast<FwAssertArgType>(idlePvns)); | |
| 49 | |||
| 50 | // FECF is constant for a given Physical Channel during a Mission Phase (4.1.6.1.3) | ||
| 51 | 16 | this->m_fecf = frameErrorControlField; | |
| 52 | 16 | this->m_spacecraftId = spacecraftId; | |
| 53 | |||
| 54 | // For each vc, init the buffer objects | ||
| 55 |
2/2✓ Branch 0 taken 16 times.
✓ Branch 1 taken 16 times.
|
32 | for (U8 ind = 0; ind < sizeof(this->m_vcs) / sizeof(AosVc); ind++) { |
| 56 | 16 | AosVc& currentVc = this->m_vcs[ind]; | |
| 57 | |||
| 58 | // Write the index for mapping a vc struct onto an output port | ||
| 59 | 16 | currentVc.vc_struct_index = ind; | |
| 60 | 16 | currentVc.virtualChannelId = vcId; | |
| 61 |
2/2✓ Branch 4 taken 16 times.
✓ Branch 13 taken 16 times.
|
16 | currentVc.frame.buffer = {currentVc.frame.backer, fixedFrameSize}; |
| 62 | // Set the bitmask of PVNs to use for idle packets | ||
| 63 | 16 | currentVc.idle_packet_types = idlePvns; | |
| 64 | } | ||
| 65 | 16 | } | |
| 66 | |||
| 67 | // ---------------------------------------------------------------------- | ||
| 68 | // Handler implementations for typed input ports | ||
| 69 | // ---------------------------------------------------------------------- | ||
| 70 | |||
| 71 | 23 | void AosFramer ::dataIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { | |
| 72 | // Get the VC Struct for this buffer | ||
| 73 | 23 | const U8 vcId = context.get_vcId(); | |
| 74 | 23 | AosVc& currentVc = this->get_vc_struct(context); | |
| 75 | 23 | FW_ASSERT(currentVc.virtualChannelId == vcId); | |
| 76 | |||
| 77 | // Ensure rest of the com stack is complying with the [communications adapter | ||
| 78 | // interface](docs/reference/communication-adapter-interface.md) | ||
| 79 | 23 | FW_ASSERT(currentVc.frame.state == BufferOwnershipState::OWNED, | |
| 80 | static_cast<FwAssertArgType>(currentVc.frame.state)); | ||
| 81 | |||
| 82 | // AOS Header & M_PDU Header | ||
| 83 | // If this is the first fresh packet, set the M_PDU firstHeaderPointer | ||
| 84 |
3/4✗ Branch 1 not taken.
✓ Branch 2 taken 23 times.
✓ Branch 3 taken 14 times.
✓ Branch 4 taken 9 times.
|
23 | if (!currentVc.past_first_fresh_packet) { |
| 85 | // setup our headers if we have a packet + context ready to go | ||
| 86 | 14 | setup_header(context); | |
| 87 | 14 | setup_m_pdu_header(context); | |
| 88 | |||
| 89 | // We now will only work with fresh packets for the rest of this frame | ||
| 90 | 14 | currentVc.past_first_fresh_packet = true; | |
| 91 | } | ||
| 92 | |||
| 93 | // Pack this packet | ||
| 94 | 23 | pack_pad_send(data, context); | |
| 95 | 23 | } | |
| 96 | |||
| 97 | 16 | void AosFramer::compute_and_inject_fecf(AosVc& currentVc) { | |
| 98 | // ------------------------------------------------- | ||
| 99 | // Trailer (CRC) | ||
| 100 | // ------------------------------------------------- | ||
| 101 |
1/1✓ Branch 2 taken 16 times.
|
16 | AOSTrailer trailer; |
| 102 | |||
| 103 |
1/1✓ Branch 6 taken 16 times.
|
16 | const FwSizeType fecfStart = currentVc.frame.buffer.getSize() - AOSTrailer::SERIALIZED_SIZE; |
| 104 | // Compute CRC over the entire frame buffer minus the FECF trailer (Standard 4.1.6) | ||
| 105 |
2/2✓ Branch 6 taken 16 times.
✓ Branch 9 taken 16 times.
|
16 | U16 crc = Ccsds::Utils::CRC16::compute(currentVc.frame.buffer.getData(), static_cast<U32>(fecfStart)); |
| 106 | // Set the Frame Error Control Field (FECF) | ||
| 107 |
1/1✓ Branch 2 taken 16 times.
|
16 | trailer.set_fecf(crc); |
| 108 | // Move the serializer pointer to the end of the location where the trailer will be | ||
| 109 | // serialized | ||
| 110 |
1/1✓ Branch 4 taken 16 times.
|
16 | auto frameSerializer = currentVc.frame.buffer.getSerializer(); |
| 111 |
1/1✓ Branch 2 taken 16 times.
|
16 | Fw::SerializeStatus status = frameSerializer.moveSerToOffset(fecfStart); |
| 112 | 16 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 113 | |||
| 114 |
1/1✓ Branch 2 taken 16 times.
|
16 | status = frameSerializer.serializeFrom(trailer); |
| 115 | 16 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 116 | 32 | } | |
| 117 | |||
| 118 | 2 | void AosFramer ::comStatusIn_handler(FwIndexType portNum, Fw::Success& condition) { | |
| 119 | // We just ask upstream for more packets | ||
| 120 | // comQueue decides to which VCs to allocate comStatus success | ||
| 121 |
1/2✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
2 | if (this->isConnected_comStatusOut_OutputPort(portNum)) { |
| 122 | // Forward the comStatus upstream | ||
| 123 | 2 | this->comStatusOut_out(portNum, condition); | |
| 124 | } | ||
| 125 | 2 | } | |
| 126 | |||
| 127 | 5 | void AosFramer ::dataReturnIn_handler(FwIndexType portNum, | |
| 128 | Fw::Buffer& frameBuffer, | ||
| 129 | const ComCfg::FrameContext& context) { | ||
| 130 | // Get the VC Struct for this buffer | ||
| 131 | 5 | AosVc& currentVc = this->get_vc_struct(context); | |
| 132 | |||
| 133 | // Assert that the returned buffer is the member, and set ownership state | ||
| 134 | 5 | FW_ASSERT(buffer_belongs(frameBuffer, currentVc.frame.backer, sizeof(currentVc.frame.backer))); | |
| 135 | 5 | currentVc.frame.state = BufferOwnershipState::OWNED; | |
| 136 | |||
| 137 | // If we have an outstanding packet from the prior frame, pack it | ||
| 138 |
2/2✓ Branch 6 taken 3 times.
✓ Branch 7 taken 2 times.
|
5 | if (currentVc.outstanding.packet.isValid()) { |
| 139 | 3 | this->pack_pad_send(currentVc.outstanding.packet, currentVc.outstanding.context, currentVc.outstanding.offset); | |
| 140 | } | ||
| 141 | 5 | } | |
| 142 | |||
| 143 | 113 | AosFramer::AosVc& AosFramer ::get_vc_struct(const ComCfg::FrameContext& context) { | |
| 144 | // MultiVc support would require looking up a vc struct ind given the vcIndex | ||
| 145 | // (unless you force them to be continuous and start at zero) | ||
| 146 | 113 | const U8 ind = 0; | |
| 147 | 113 | AosVc& currentVc = this->m_vcs[ind]; | |
| 148 | |||
| 149 | // Ensure configure was called | ||
| 150 | 113 | FW_ASSERT(currentVc.vc_struct_index == ind); | |
| 151 | 113 | FW_ASSERT(currentVc.virtualChannelId == context.get_vcId()); | |
| 152 | 113 | return currentVc; | |
| 153 | } | ||
| 154 | |||
| 155 | 16 | void AosFramer ::setup_header(const ComCfg::FrameContext& context) { | |
| 156 | // Get the VC Struct for this vc | ||
| 157 |
1/1✓ Branch 4 taken 16 times.
|
16 | AosVc& currentVc = this->get_vc_struct(context); |
| 158 | |||
| 159 | // ----------------------------------------------- | ||
| 160 | // Header | ||
| 161 | // ----------------------------------------------- | ||
| 162 |
1/1✓ Branch 2 taken 16 times.
|
16 | AOSHeader header; |
| 163 | |||
| 164 | // GVCID (Global Virtual Channel ID) (Standard 4.1.2.2 and 4.1.2.3) | ||
| 165 | 16 | U16 globalVcId = static_cast<U16>((context.get_vcId() << AOSHeaderSubfields::virtualChannelIdOffset) | | |
| 166 | 16 | ((m_spacecraftId & 0x00FF) << AOSHeaderSubfields::spacecraftIdLsbOffset) | | |
| 167 |
2/4✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
|
32 | ((Tfvn::AOS & 0x3) << AOSHeaderSubfields::frameVersionOffset)); |
| 168 | |||
| 169 | // Virtual Channel Frame Count (4.1.2.4) | ||
| 170 | 16 | U32 frameCountAndSignaling = static_cast<U32>((currentVc.virtualFrameCount & 0x00FFFFFFU) | |
| 171 | 16 | << static_cast<FwSizeType>(AOSHeaderSubfields::vcFrameCountOffset)); | |
| 172 | |||
| 173 | // Virtual Channel Frame Count Cycle Use Flag (4.1.2.5.3) | ||
| 174 | 16 | frameCountAndSignaling |= static_cast<U32>(1 << AOSHeaderSubfields::cycleCountFlagOffset); | |
| 175 | |||
| 176 | // Spacecraft ID MSB (4.1.2.5.4) | ||
| 177 | 16 | frameCountAndSignaling |= | |
| 178 | 16 | static_cast<U32>((m_spacecraftId & 0x0300) >> (8 - AOSHeaderSubfields::spacecraftIdMsbOffset)); | |
| 179 | |||
| 180 | // Virtual Channel Frame Cycle Count (4.1.2.5.5) | ||
| 181 | 16 | frameCountAndSignaling |= static_cast<U32>((currentVc.virtualFrameCount & 0x0F000000) >> 24); | |
| 182 | |||
| 183 |
1/1✓ Branch 2 taken 16 times.
|
16 | header.set_globalVcId(globalVcId); |
| 184 |
1/1✓ Branch 2 taken 16 times.
|
16 | header.set_frameCountAndSignaling(frameCountAndSignaling); |
| 185 | |||
| 186 | // Perform the modulo at serialization time makes vc cycle count easier to make optional | ||
| 187 | 16 | currentVc.virtualFrameCount++; // U24 base Frame Count | |
| 188 | // Extended by 4 bit VC Cycle Count | ||
| 189 | // intended to wrap around (modulo 268,435,456) | ||
| 190 | |||
| 191 | // ----------------------------------------------- | ||
| 192 | // Write Header | ||
| 193 | // ----------------------------------------------- | ||
| 194 | Fw::SerializeStatus status; | ||
| 195 | // Use our member Fw::Buffer | ||
| 196 |
1/1✓ Branch 4 taken 16 times.
|
16 | auto frameSerializer = currentVc.frame.buffer.getSerializer(); |
| 197 | |||
| 198 |
1/1✓ Branch 2 taken 16 times.
|
16 | status = frameSerializer.moveSerToOffset(0); |
| 199 | 16 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 200 | |||
| 201 |
1/1✓ Branch 2 taken 16 times.
|
16 | status = frameSerializer.serializeFrom(header); |
| 202 | 16 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 203 | 32 | } | |
| 204 | |||
| 205 | 16 | void AosFramer ::setup_m_pdu_header(const ComCfg::FrameContext& context, bool no_fresh) { | |
| 206 | // Get the VC Struct for this vc | ||
| 207 |
1/1✓ Branch 4 taken 16 times.
|
16 | AosVc& currentVc = this->get_vc_struct(context); |
| 208 | |||
| 209 |
1/1✓ Branch 2 taken 16 times.
|
16 | M_PDUHeader muxedPdu; |
| 210 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 15 times.
|
16 | if (no_fresh) { |
| 211 | // All Ones since no packets start in this frame (no fresh packets) | ||
| 212 |
1/1✓ Branch 2 taken 1 times.
|
1 | muxedPdu.set_firstHeaderPointer(0xFFFF); |
| 213 | } else { | ||
| 214 | // Called at first fresh packet (not a packet tail) | ||
| 215 | // So current payload offset is the pointer to first packet header | ||
| 216 |
1/1✓ Branch 4 taken 15 times.
|
15 | muxedPdu.set_firstHeaderPointer(currentVc.current_payload_offset); |
| 217 | } | ||
| 218 | |||
| 219 |
1/1✓ Branch 4 taken 16 times.
|
16 | auto frameSerializer = currentVc.frame.buffer.getSerializer(); |
| 220 |
1/1✓ Branch 2 taken 16 times.
|
16 | Fw::SerializeStatus status = frameSerializer.moveSerToOffset(AOSHeader::SERIALIZED_SIZE); |
| 221 | 16 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 222 | |||
| 223 |
1/1✓ Branch 2 taken 16 times.
|
16 | status = frameSerializer.serializeFrom(muxedPdu); |
| 224 | 16 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 225 | 32 | } | |
| 226 | |||
| 227 | 69 | FwSizeType AosFramer::get_min_size() { | |
| 228 |
2/4✗ Branch 3 not taken.
✓ Branch 4 taken 69 times.
✓ Branch 5 taken 69 times.
✗ Branch 6 not taken.
|
69 | return AOSHeader::SERIALIZED_SIZE + M_PDUHeader::SERIALIZED_SIZE + (this->m_fecf ? AOSTrailer::SERIALIZED_SIZE : 0); |
| 229 | } | ||
| 230 | |||
| 231 | 29 | bool AosFramer::buffer_belongs(const Fw::Buffer& buffer, U8 const* start, FwSizeType size) { | |
| 232 |
4/4✓ Branch 4 taken 7 times.
✓ Branch 5 taken 22 times.
✓ Branch 11 taken 6 times.
✓ Branch 12 taken 1 times.
|
29 | return (buffer.getData() >= start && buffer.getData() < start + size); |
| 233 | } | ||
| 234 | |||
| 235 | 26 | void AosFramer::check_and_send_vc(AosFramer::AosVc& currentVc) { | |
| 236 | 26 | const FwSizeType maxPayload = currentVc.frame.buffer.getSize() - get_min_size(); | |
| 237 | |||
| 238 | // Check if we've filled up the M_PDU payload | ||
| 239 |
2/2✓ Branch 2 taken 16 times.
✓ Branch 3 taken 10 times.
|
26 | if (currentVc.current_payload_offset == maxPayload) { |
| 240 | // Write the headers if we haven't already (continuing packet only) | ||
| 241 |
3/4✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 15 times.
|
16 | if (!currentVc.past_first_fresh_packet) { |
| 242 | // Setup the AOS header right before we send with the context cached in outstanding | ||
| 243 | 1 | setup_header(currentVc.outstanding.context); | |
| 244 | |||
| 245 | // Setup the M_PDU header w/ all zero (only continuing packets) | ||
| 246 | 1 | setup_m_pdu_header(currentVc.outstanding.context, true); | |
| 247 | } | ||
| 248 | |||
| 249 | // Compute Trailer | ||
| 250 |
2/4✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
|
16 | if (this->m_fecf) { |
| 251 | 16 | compute_and_inject_fecf(currentVc); | |
| 252 | } | ||
| 253 | |||
| 254 | // Ensure we aren't double sending | ||
| 255 | 16 | FW_ASSERT(currentVc.frame.state == BufferOwnershipState::OWNED); | |
| 256 | 16 | currentVc.frame.state = BufferOwnershipState::NOT_OWNED; | |
| 257 | |||
| 258 | // Clean up our per frame vc values | ||
| 259 | 16 | currentVc.current_payload_offset = 0; | |
| 260 | 16 | currentVc.past_first_fresh_packet = false; | |
| 261 | |||
| 262 | // Write the completed frame | ||
| 263 | 16 | this->dataOut_out(0, currentVc.frame.buffer, currentVc.outstanding.context); | |
| 264 |
1/2✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
|
10 | } else if (this->isConnected_comStatusOut_OutputPort(currentVc.vc_struct_index)) { |
| 265 | // We got more room, so ask for more bytes | ||
| 266 |
1/1✓ Branch 2 taken 10 times.
|
10 | Fw::Success condition = Fw::Success::SUCCESS; |
| 267 |
1/1✓ Branch 6 taken 10 times.
|
10 | this->comStatusOut_out(currentVc.vc_struct_index, condition); |
| 268 | 10 | } | |
| 269 | 26 | } | |
| 270 | |||
| 271 | 26 | void AosFramer ::pack_pad_send(Fw::Buffer& data, const ComCfg::FrameContext& context, FwSizeType dataOffset) { | |
| 272 | // Pack this packet into the M_PDU | ||
| 273 | 26 | pack_packet(data, context, dataOffset); | |
| 274 | |||
| 275 | // Get the VC Struct for this buffer | ||
| 276 | 26 | AosVc& currentVc = this->get_vc_struct(context); | |
| 277 | |||
| 278 | // Pack with idle packets if sendNow and not full already | ||
| 279 | // TODO: Add configurable time elapsed & buffer remaining based idle packing | ||
| 280 |
2/2✓ Branch 4 taken 16 times.
✓ Branch 5 taken 10 times.
|
26 | if (context.get_sendNow()) { |
| 281 | // Compute some sizes to check if we've got space left to pad | ||
| 282 | 16 | const FwSizeType min_size = get_min_size(); | |
| 283 | 16 | FW_ASSERT(currentVc.frame.buffer.getSize() >= min_size, | |
| 284 | static_cast<FwAssertArgType>(currentVc.frame.buffer.getSize()), | ||
| 285 | static_cast<FwAssertArgType>(min_size)); | ||
| 286 | 16 | const FwSizeType maxPayload = currentVc.frame.buffer.getSize() - min_size; | |
| 287 | |||
| 288 |
2/2✓ Branch 2 taken 14 times.
✓ Branch 3 taken 2 times.
|
16 | if (currentVc.current_payload_offset < maxPayload) { |
| 289 | // As per TM Standard 4.2.2.5, fill the rest of the data field with an Idle Packet | ||
| 290 | 14 | fill_with_idle_packet(currentVc, context); | |
| 291 | } | ||
| 292 | } | ||
| 293 | |||
| 294 | // Send the frame if we've filled it | ||
| 295 | 26 | check_and_send_vc(currentVc); | |
| 296 | 26 | } | |
| 297 | |||
| 298 | 27 | void AosFramer ::pack_packet(Fw::Buffer& data, const ComCfg::FrameContext& context, FwSizeType dataOffset) { | |
| 299 | // Ensure the packet is starting within the frame | ||
| 300 | 27 | const FwSizeType min_size = get_min_size(); | |
| 301 | |||
| 302 | // Get the VC Struct for this buffer | ||
| 303 |
1/1✓ Branch 4 taken 27 times.
|
27 | AosVc& currentVc = this->get_vc_struct(context); |
| 304 | |||
| 305 |
1/1✓ Branch 6 taken 27 times.
|
27 | const FwSizeType maxPayload = currentVc.frame.buffer.getSize() - min_size; |
| 306 | 27 | const FwSizeType bytesAvailable = maxPayload - currentVc.current_payload_offset; | |
| 307 | 27 | FW_ASSERT(bytesAvailable < currentVc.frame.buffer.getSize(), | |
| 308 | static_cast<FwAssertArgType>(min_size + currentVc.current_payload_offset)); | ||
| 309 | |||
| 310 | // ------------------------------------------------- | ||
| 311 | // Payload Packet | ||
| 312 | // ------------------------------------------------- | ||
| 313 | Fw::SerializeStatus status; | ||
| 314 | // Use our member Fw::Buffer | ||
| 315 |
1/1✓ Branch 4 taken 27 times.
|
27 | auto frameSerializer = currentVc.frame.buffer.getSerializer(); |
| 316 |
1/1✓ Branch 4 taken 27 times.
|
27 | status = frameSerializer.moveSerToOffset(START_OF_PAYLOAD + currentVc.current_payload_offset); |
| 317 | 27 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 318 | |||
| 319 |
1/1✓ Branch 4 taken 27 times.
|
27 | const U8* dataStart = data.getData() + dataOffset; |
| 320 | // min of (remaining bytes in buffer and available bytes in frame) | ||
| 321 |
1/1✓ Branch 4 taken 27 times.
|
27 | FwSizeType dataSize = data.getSize() - dataOffset; |
| 322 | |||
| 323 | // Determine if we can write the whole packet or not | ||
| 324 |
2/2✓ Branch 0 taken 24 times.
✓ Branch 1 taken 3 times.
|
27 | if (dataSize <= bytesAvailable) { |
| 325 | // Whole packet fits, no need to store to outstanding | ||
| 326 | 24 | currentVc.outstanding.offset = 0; | |
| 327 | } else { | ||
| 328 | // We'll only write a subset (clamp our write to the available size) | ||
| 329 | 3 | dataSize = bytesAvailable; | |
| 330 | |||
| 331 | // We'll pick up serialization from here later | ||
| 332 | 3 | currentVc.outstanding.offset = dataOffset + dataSize; | |
| 333 |
1/1✓ Branch 7 taken 3 times.
|
3 | currentVc.outstanding.packet = data; |
| 334 | } | ||
| 335 | |||
| 336 |
1/1✓ Branch 2 taken 27 times.
|
27 | status = frameSerializer.serializeFrom(dataStart, dataSize, Fw::Serialization::OMIT_LENGTH); |
| 337 | 27 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 338 | |||
| 339 | // Shift our offset into the M_PDU payload region by how many bytes we wrote | ||
| 340 | 27 | currentVc.current_payload_offset = static_cast<U16>(currentVc.current_payload_offset + dataSize); | |
| 341 | |||
| 342 | // Context must always be present once any packet payload bytes are written | ||
| 343 |
1/1✓ Branch 6 taken 27 times.
|
27 | currentVc.outstanding.context = context; |
| 344 | |||
| 345 | // Return the buffer if no bytes are outstanding | ||
| 346 |
2/2✓ Branch 2 taken 24 times.
✓ Branch 3 taken 3 times.
|
27 | if (currentVc.outstanding.offset == 0) { |
| 347 | // Return the buffer if this isn't the SPP Idle buff | ||
| 348 |
1/1✓ Branch 4 taken 24 times.
|
24 | const bool isSppIdleBuffer = buffer_belongs(data, currentVc.spp_idle.backer, sizeof(currentVc.spp_idle.backer)); |
| 349 |
2/2✓ Branch 0 taken 23 times.
✓ Branch 1 taken 1 times.
|
24 | if (!isSppIdleBuffer) { |
| 350 |
1/1✓ Branch 5 taken 23 times.
|
23 | this->dataReturnOut_out(0, data, |
| 351 | context); // return ownership of the original data buffer | ||
| 352 | } | ||
| 353 | |||
| 354 |
2/2✓ Branch 2 taken 24 times.
✓ Branch 11 taken 24 times.
|
24 | currentVc.outstanding.packet = {}; |
| 355 | 24 | currentVc.outstanding.offset = 0; | |
| 356 | } | ||
| 357 | 54 | } | |
| 358 | |||
| 359 | 14 | void AosFramer ::serialize_idle_spp_packet(Fw::SerialBufferBase& serializer, U16 length) { | |
| 360 | // APID to use for this Idle Packet | ||
| 361 | 14 | constexpr U16 idleApid = static_cast<U16>(ComCfg::Apid::SPP_IDLE_PACKET); | |
| 362 | |||
| 363 | // Length token is defined as the number of bytes of payload data minus 1 | ||
| 364 | 14 | const U16 lengthToken = static_cast<U16>(length - SpacePacketHeader::SERIALIZED_SIZE - 1); | |
| 365 | |||
| 366 |
1/1✓ Branch 2 taken 14 times.
|
14 | SpacePacketHeader header; |
| 367 |
1/1✓ Branch 2 taken 14 times.
|
14 | header.set_packetIdentification(idleApid); |
| 368 |
1/1✓ Branch 2 taken 14 times.
|
14 | header.set_packetSequenceControl( |
| 369 | 0x3 << SpacePacketSubfields::SeqFlagsOffset); // Sequence Flags = 0b11 (unsegmented) & unused Seq count | ||
| 370 |
1/1✓ Branch 2 taken 14 times.
|
14 | header.set_packetDataLength(lengthToken); |
| 371 | // Serialize header into frame | ||
| 372 |
1/1✓ Branch 7 taken 14 times.
|
14 | Fw::SerializeStatus status = serializer.serializeFrom(header); |
| 373 | 14 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 374 | |||
| 375 | // Fill with idle pattern | ||
| 376 |
2/2✓ Branch 0 taken 18119 times.
✓ Branch 1 taken 14 times.
|
18133 | for (U16 i = static_cast<U16>(SpacePacketHeader::SERIALIZED_SIZE); i < length; i++) { |
| 377 |
1/1✓ Branch 7 taken 18119 times.
|
18119 | status = serializer.serializeFrom(SPP_IDLE_DATA_PATTERN); // Idle data |
| 378 | 18119 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 379 | } | ||
| 380 | 28 | } | |
| 381 | |||
| 382 | 14 | void AosFramer ::fill_with_idle_packet(AosVc& vc, const ComCfg::FrameContext& context) { | |
| 383 | // Bytes that aren't actual PDUs | ||
| 384 |
2/4✗ Branch 3 not taken.
✓ Branch 4 taken 14 times.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
|
14 | const U16 overhead = START_OF_PAYLOAD + (m_fecf ? AOSTrailer::SERIALIZED_SIZE : 0); |
| 385 | // Bytes for all the PDUs in this VC Frame | ||
| 386 | 14 | FW_ASSERT(vc.frame.buffer.getSize() > 0, static_cast<FwAssertArgType>(vc.frame.buffer.getSize())); | |
| 387 | 14 | FW_ASSERT(vc.frame.buffer.getSize() < std::numeric_limits<U16>::max(), | |
| 388 | static_cast<FwAssertArgType>(vc.frame.buffer.getSize())); | ||
| 389 |
1/1✓ Branch 6 taken 14 times.
|
14 | const U16 pduSize = static_cast<U16>(vc.frame.buffer.getSize() - overhead); |
| 390 | |||
| 391 | // How many bytes are left over | ||
| 392 | 14 | const U16 idlePacketSize = static_cast<U16>(pduSize - vc.current_payload_offset); | |
| 393 | |||
| 394 | // Grab a serializer @ the current offset | ||
| 395 |
1/1✓ Branch 4 taken 14 times.
|
14 | auto frameSerializer = vc.frame.buffer.getSerializer(); |
| 396 |
1/1✓ Branch 4 taken 14 times.
|
14 | Fw::SerializeStatus status = frameSerializer.moveSerToOffset(START_OF_PAYLOAD + vc.current_payload_offset); |
| 397 | 14 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 398 | |||
| 399 | // If there wasn't a fresh packet before this | ||
| 400 | // set the M_PDU firstHeaderPointer | ||
| 401 |
3/4✗ Branch 1 not taken.
✓ Branch 2 taken 14 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 13 times.
|
14 | if (!vc.past_first_fresh_packet) { |
| 402 | // setup our headers if we have a packet + context ready to go | ||
| 403 |
1/1✓ Branch 4 taken 1 times.
|
1 | setup_header(context); |
| 404 |
1/1✓ Branch 4 taken 1 times.
|
1 | setup_m_pdu_header(context); |
| 405 | |||
| 406 | // This idle packet is the fresh packet | ||
| 407 | 1 | vc.past_first_fresh_packet = true; | |
| 408 | } | ||
| 409 | |||
| 410 | // EPP-only idle fill is not yet supported; configure() requires the SPP bit | ||
| 411 |
1/2✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
|
14 | if (vc.idle_packet_types & PvnBitfield::EPP_MASK) { |
| 412 | // TODO: Serialize an EPP of the right size once EPP idle is supported | ||
| 413 | } | ||
| 414 | // While we are using only SPP, we have to comply w/ the min SPP packet size | ||
| 415 | // We'll stripe this packet onto the next frame of this VC if we have to | ||
| 416 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 13 times.
|
14 | else if (idlePacketSize < 7) { |
| 417 | // Serialize the Idle packet into the spp_idle_backer | ||
| 418 | |||
| 419 | // Make sure we aren't overwriting a packet fragment | ||
| 420 | 1 | FW_ASSERT(!vc.outstanding.packet.isValid()); | |
| 421 | |||
| 422 | // Setup the PDU to point at our idle buffer | ||
| 423 |
2/2✓ Branch 4 taken 1 times.
✓ Branch 13 taken 1 times.
|
1 | vc.outstanding.packet = {vc.spp_idle.backer, MIN_SPP_LENGTH}; |
| 424 | |||
| 425 | // Write the SPP Idle to the outstanding packet | ||
| 426 |
1/1✓ Branch 4 taken 1 times.
|
1 | auto pduSerializer = vc.outstanding.packet.getSerializer(); |
| 427 |
1/1✓ Branch 4 taken 1 times.
|
1 | serialize_idle_spp_packet(pduSerializer, MIN_SPP_LENGTH); |
| 428 | |||
| 429 | // Remove the sendNow param | ||
| 430 | // The idle fragment that spills onto the next doesn't need to be sentNow | ||
| 431 |
1/1✓ Branch 2 taken 1 times.
|
1 | ComCfg::FrameContext filtered_context = context; |
| 432 |
1/1✓ Branch 2 taken 1 times.
|
1 | filtered_context.set_sendNow(false); |
| 433 | |||
| 434 | // Use the normal pack command since we will have leftovers | ||
| 435 | // There won't be an outstanding packet since we'd use those bytes instead | ||
| 436 |
1/1✓ Branch 7 taken 1 times.
|
1 | pack_packet(vc.outstanding.packet, filtered_context); |
| 437 | 1 | } else { | |
| 438 | // Serialize an idle packet right into the frame | ||
| 439 |
1/1✓ Branch 4 taken 13 times.
|
13 | serialize_idle_spp_packet(frameSerializer, idlePacketSize); |
| 440 | |||
| 441 | // Increment the offset since we serialized directly into the frame | ||
| 442 | 13 | vc.current_payload_offset = static_cast<U16>(vc.current_payload_offset + idlePacketSize); | |
| 443 | } | ||
| 444 | 28 | } | |
| 445 | |||
| 446 | } // namespace Ccsds | ||
| 447 | } // namespace Svc | ||
| 448 |