| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // ====================================================================== | ||
| 2 | // \title AosDeframer.cpp | ||
| 3 | // \author Will MacCormack | ||
| 4 | // \brief cpp file for AosDeframer component implementation class | ||
| 5 | // | ||
| 6 | // Deframer for the AOS Space Data Link Protocol per CCSDS 732.0-B-5. | ||
| 7 | // Supports M_PDU data field service with: | ||
| 8 | // - Frame Error Control Field (FECF) validation (Section 4.1.6) | ||
| 9 | // - Space Packet Protocol (SPP) extraction (CCSDS 133.0-B-2) | ||
| 10 | // - Encapsulation Packet Protocol (EPP) extraction (CCSDS 133.1-B-3) | ||
| 11 | // ====================================================================== | ||
| 12 | #include "Svc/Ccsds/AosDeframer/AosDeframer.hpp" | ||
| 13 | #include <cstring> | ||
| 14 | #include <limits> | ||
| 15 | #include "Svc/Ccsds/Types/EppLengthOfLengthEnumAc.hpp" | ||
| 16 | #include "Svc/Ccsds/Types/EppProtocolIdEnumAc.hpp" | ||
| 17 | #include "Svc/Ccsds/Types/SpacePacketHeaderSerializableAc.hpp" | ||
| 18 | #include "Svc/Ccsds/Utils/CRC16.hpp" | ||
| 19 | #include "config/FppConstantsAc.hpp" | ||
| 20 | |||
| 21 | namespace Svc { | ||
| 22 | namespace Ccsds { | ||
| 23 | |||
| 24 | // ---------------------------------------------------------------------- | ||
| 25 | // Component construction and destruction | ||
| 26 | // ---------------------------------------------------------------------- | ||
| 27 | |||
| 28 | 36 | AosDeframer::AosDeframer(const char* const compName) | |
| 29 | : AosDeframerComponentBase(compName), | ||
| 30 | 36 | m_fixedFrameSize(ComCfg::AosMaxFrameFixedSize), | |
| 31 | 36 | m_fecfEnabled(true), | |
| 32 | 36 | m_spacecraftId(ComCfg::SpacecraftId), | |
| 33 |
3/7✓ Branch 14 taken 36 times.
✓ Branch 17 taken 36 times.
✓ Branch 18 taken 36 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
|
108 | m_crcErrorCount(0) { |
| 34 | // Initialize VC struct | ||
| 35 |
2/2✓ Branch 0 taken 36 times.
✓ Branch 1 taken 36 times.
|
72 | for (U8 vcInd = 0; vcInd < AosDeframer_NumVcs; vcInd++) { |
| 36 | 36 | m_vcs[vcInd].vcStructIndex = vcInd; | |
| 37 | } | ||
| 38 | 36 | } | |
| 39 | |||
| 40 |
3/4✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✓ Branch 11 taken 36 times.
✓ Branch 12 taken 36 times.
|
144 | AosDeframer::~AosDeframer() {} |
| 41 | |||
| 42 | 36 | void AosDeframer::configure(U32 fixedFrameSize, bool frameErrorControlField, U16 spacecraftId, U8 vcId, U8 pvnMask) { | |
| 43 | // Validate frame size is within bounds | ||
| 44 | 36 | FW_ASSERT(fixedFrameSize <= ComCfg::AosMaxFrameFixedSize, static_cast<FwAssertArgType>(fixedFrameSize), | |
| 45 | static_cast<FwAssertArgType>(ComCfg::AosMaxFrameFixedSize)); | ||
| 46 | |||
| 47 | // Frame must be large enough for header + M_PDU header + optional trailer | ||
| 48 |
2/2✓ Branch 0 taken 35 times.
✓ Branch 1 taken 1 times.
|
36 | const FwSizeType minSize = AOSHeader::SERIALIZED_SIZE + M_PDUHeader::SERIALIZED_SIZE + |
| 49 | (frameErrorControlField ? AOSTrailer::SERIALIZED_SIZE : 0); | ||
| 50 | 36 | FW_ASSERT(fixedFrameSize > minSize, static_cast<FwAssertArgType>(fixedFrameSize), | |
| 51 | static_cast<FwAssertArgType>(minSize)); | ||
| 52 | |||
| 53 | // Spacecraft ID is 10 bits (per CCSDS 732.0-B-5 Section 4.1.2.2) | ||
| 54 | 36 | FW_ASSERT((spacecraftId & 0xFC00) == 0, static_cast<FwAssertArgType>(spacecraftId)); | |
| 55 | |||
| 56 | // Virtual Channel ID is 6 bits (per CCSDS 732.0-B-5 Section 4.1.2.3) | ||
| 57 | 36 | FW_ASSERT((vcId & 0xC0) == 0, static_cast<FwAssertArgType>(vcId)); | |
| 58 | |||
| 59 | // pvnMask must only contain valid PVN bits and at least one must be set | ||
| 60 | 36 | FW_ASSERT((pvnMask & PvnBitfield::VALID_MASK) != 0, static_cast<FwAssertArgType>(pvnMask)); | |
| 61 | 36 | FW_ASSERT((pvnMask & ~PvnBitfield::VALID_MASK) == 0, static_cast<FwAssertArgType>(pvnMask)); | |
| 62 | |||
| 63 | // Spanning packet reassembly requires dynamic backing via allocator ports | ||
| 64 | 36 | FW_ASSERT(this->isConnected_allocate_OutputPort(0)); | |
| 65 | 36 | FW_ASSERT(this->isConnected_deallocate_OutputPort(0)); | |
| 66 | |||
| 67 | 36 | m_fixedFrameSize = fixedFrameSize; | |
| 68 | 36 | m_fecfEnabled = frameErrorControlField; | |
| 69 | 36 | m_spacecraftId = spacecraftId; | |
| 70 | |||
| 71 | // Zero out FECF error counter on (re)configure | ||
| 72 | 36 | m_crcErrorCount = 0; | |
| 73 | |||
| 74 | // Populate the (single) VC struct | ||
| 75 | 36 | m_vcs[0].virtualChannelId = vcId; | |
| 76 | 36 | m_vcs[0].pvnMask = pvnMask; | |
| 77 | |||
| 78 | // Clear out all VC stats | ||
| 79 |
2/2✓ Branch 0 taken 36 times.
✓ Branch 1 taken 36 times.
|
72 | for (U8 vcInd = 0; vcInd < AosDeframer_NumVcs; vcInd++) { |
| 80 | 36 | m_vcs[vcInd].framesProcessed = 0; | |
| 81 | 36 | m_vcs[vcInd].packetsExtracted = 0; | |
| 82 | 36 | m_vcs[vcInd].vcFrameCount = 0; | |
| 83 | |||
| 84 | // Clear out the spanningPacket | ||
| 85 | 36 | this->abandonSpanningPacket(m_vcs[vcInd]); | |
| 86 | } | ||
| 87 | 36 | } | |
| 88 | |||
| 89 | // ---------------------------------------------------------------------- | ||
| 90 | // Handler implementations for user-defined typed input ports | ||
| 91 | // ---------------------------------------------------------------------- | ||
| 92 | |||
| 93 | 60 | void AosDeframer::dataIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) { | |
| 94 | // Per CCSDS 732.0-B-5, AOS frames are fixed-size | ||
| 95 | // Verify we have received a complete frame | ||
| 96 | 60 | FW_ASSERT(m_fixedFrameSize > 0, static_cast<FwAssertArgType>(m_fixedFrameSize)); | |
| 97 | |||
| 98 |
3/3✓ Branch 4 taken 60 times.
✓ Branch 10 taken 1 times.
✓ Branch 11 taken 59 times.
|
60 | if (data.getSize() < m_fixedFrameSize) { |
| 99 |
2/2✓ Branch 12 taken 1 times.
✓ Branch 15 taken 1 times.
|
1 | this->log_WARNING_HI_InvalidFrameLength(data.getSize(), m_fixedFrameSize); |
| 100 |
2/2✓ Branch 5 taken 1 times.
✓ Branch 8 taken 1 times.
|
1 | this->notifyErrorIfConnected(Ccsds::FrameError::AOS_INVALID_LENGTH); |
| 101 |
1/1✓ Branch 5 taken 1 times.
|
1 | this->dataReturnOut_out(0, data, context); |
| 102 | 1 | return; | |
| 103 | } | ||
| 104 | |||
| 105 | // Validate FECF if enabled (Section 4.1.6) | ||
| 106 | // FrameDetector + FrameAccumulator or Lower Protocol Layer should enforce whole AOS Frames | ||
| 107 |
3/4✗ Branch 3 not taken.
✓ Branch 4 taken 59 times.
✓ Branch 5 taken 58 times.
✓ Branch 6 taken 1 times.
|
59 | if (m_fecfEnabled) { |
| 108 |
1/1✓ Branch 4 taken 58 times.
|
58 | const bool fecfValid = this->validateFecf(data); |
| 109 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 56 times.
|
58 | if (!fecfValid) { |
| 110 |
1/1✓ Branch 5 taken 2 times.
|
2 | this->dataReturnOut_out(0, data, context); |
| 111 | 2 | return; | |
| 112 | } | ||
| 113 | } | ||
| 114 | |||
| 115 | // Create a mutable context for extracted packet info | ||
| 116 |
1/1✓ Branch 2 taken 57 times.
|
57 | ComCfg::FrameContext packetContext = context; |
| 117 | // Parse and validate the AOS Primary Header (Section 4.1.2) | ||
| 118 | // Note: parseAndValidateHeader handles warning events and errorNotify for header failures. | ||
| 119 |
1/1✓ Branch 4 taken 57 times.
|
57 | AosDeframerVc* vc = this->parseAndValidateHeader(data, packetContext); |
| 120 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 54 times.
|
57 | if (vc == nullptr) { |
| 121 |
1/1✓ Branch 5 taken 3 times.
|
3 | this->dataReturnOut_out(0, data, context); |
| 122 | 3 | return; | |
| 123 | } | ||
| 124 | |||
| 125 | // Set the default context only if we haven't for this packet already | ||
| 126 | // Otherwise our PVN tracker gets overwritten | ||
| 127 |
3/3✓ Branch 6 taken 54 times.
✓ Branch 8 taken 40 times.
✓ Branch 9 taken 14 times.
|
54 | if (!vc->spanningPacket.buffer.isValid()) { |
| 128 |
1/1✓ Branch 6 taken 40 times.
|
40 | vc->spanningPacket.context = packetContext; |
| 129 | } | ||
| 130 | |||
| 131 | // Extract packets from the M_PDU data zone | ||
| 132 |
1/1✓ Branch 5 taken 54 times.
|
54 | this->extractPackets(*vc, data); |
| 133 | |||
| 134 | // Return the frame buffer | ||
| 135 |
1/1✓ Branch 5 taken 54 times.
|
54 | this->dataReturnOut_out(0, data, context); |
| 136 | |||
| 137 | // Update telemetry | ||
| 138 |
2/2✓ Branch 6 taken 54 times.
✓ Branch 15 taken 54 times.
|
54 | this->tlmWrite_FramesProcessed(++vc->framesProcessed); |
| 139 | 57 | } | |
| 140 | |||
| 141 | 1 | void AosDeframer::dataReturnIn_handler(FwIndexType portNum, Fw::Buffer& fwBuffer, const ComCfg::FrameContext& context) { | |
| 142 | // Deallocate this dynamically allocated packet | ||
| 143 | 1 | this->deallocate_out(0, fwBuffer); | |
| 144 | 1 | } | |
| 145 | |||
| 146 | // ---------------------------------------------------------------------- | ||
| 147 | // Private helper methods | ||
| 148 | // ---------------------------------------------------------------------- | ||
| 149 | |||
| 150 | 10 | void AosDeframer::notifyErrorIfConnected(Ccsds::FrameError error) { | |
| 151 |
1/2✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
10 | if (this->isConnected_errorNotify_OutputPort(0)) { |
| 152 | 10 | this->errorNotify_out(0, error); | |
| 153 | } | ||
| 154 | 10 | } | |
| 155 | |||
| 156 | 153 | void AosDeframer::abandonSpanningPacket(AosDeframerVc& vc) { | |
| 157 |
2/2✓ Branch 6 taken 2 times.
✓ Branch 7 taken 151 times.
|
153 | if (vc.spanningPacket.buffer.isValid()) { |
| 158 |
2/2✓ Branch 15 taken 2 times.
✓ Branch 21 taken 2 times.
|
4 | this->log_WARNING_HI_SpanningPacketAbandoned(vc.virtualChannelId, vc.spanningPacket.context.get_pvn(), |
| 159 | vc.spanningPacket.bytesReceived, | ||
| 160 | 2 | vc.spanningPacket.buffer.getSize()); | |
| 161 | 2 | this->deallocate_out(0, vc.spanningPacket.buffer); | |
| 162 | } | ||
| 163 |
2/2✓ Branch 2 taken 153 times.
✓ Branch 11 taken 153 times.
|
153 | vc.spanningPacket.buffer = Fw::Buffer(); |
| 164 | 153 | vc.spanningPacket.bytesReceived = 0; | |
| 165 | 153 | vc.spanningPacket.context.set_pvn(ComCfg::Pvn::INVALID_UNINITIALIZED); | |
| 166 | 153 | } | |
| 167 | |||
| 168 | 55 | AosDeframer::AosDeframerVc* AosDeframer::getVcStruct(const U8 vcId) { | |
| 169 |
2/2✓ Branch 0 taken 55 times.
✓ Branch 1 taken 1 times.
|
56 | for (U8 vcInd = 0; vcInd < AosDeframer_NumVcs; vcInd++) { |
| 170 |
2/2✓ Branch 4 taken 54 times.
✓ Branch 5 taken 1 times.
|
55 | if (m_vcs[vcInd].virtualChannelId == vcId) { |
| 171 | 54 | return &m_vcs[vcInd]; | |
| 172 | } | ||
| 173 | } | ||
| 174 | |||
| 175 | 1 | return nullptr; | |
| 176 | } | ||
| 177 | |||
| 178 | 57 | AosDeframer::AosDeframerVc* AosDeframer::parseAndValidateHeader(Fw::Buffer& data, ComCfg::FrameContext& context) { | |
| 179 | // Deserialize the AOS Primary Header (per CCSDS 732.0-B-5 Section 4.1.2) | ||
| 180 |
1/1✓ Branch 2 taken 57 times.
|
57 | AOSHeader header; |
| 181 |
2/2✓ Branch 2 taken 57 times.
✓ Branch 8 taken 57 times.
|
57 | Fw::SerializeStatus status = data.getDeserializer().deserializeTo(header); |
| 182 | // We already checked that a header fits into fixedFrameSize & that this frame is >= fixedFrameSize | ||
| 183 | 57 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, static_cast<FwAssertArgType>(status)); | |
| 184 | |||
| 185 | // Extract Transfer Frame Version Number (Section 4.1.2.2.2) | ||
| 186 | // AOS uses Tfvn::AOS = 0x1 ('01' binary) | ||
| 187 | 57 | U8 tfvn = static_cast<U8>((header.get_globalVcId() & AOSHeaderSubfields::frameVersionMask) >> | |
| 188 | 57 | AOSHeaderSubfields::frameVersionOffset); | |
| 189 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 56 times.
|
57 | if (tfvn != static_cast<U8>(Tfvn::AOS)) { |
| 190 |
1/1✓ Branch 5 taken 1 times.
|
1 | this->log_WARNING_HI_InvalidTfvn(tfvn, static_cast<U8>(Tfvn::AOS)); |
| 191 |
2/2✓ Branch 5 taken 1 times.
✓ Branch 8 taken 1 times.
|
1 | this->notifyErrorIfConnected(Ccsds::FrameError::AOS_INVALID_VERSION); |
| 192 | 1 | return nullptr; | |
| 193 | } | ||
| 194 | |||
| 195 | // Extract Spacecraft ID (Section 4.1.2.2) | ||
| 196 | // SCID is split: 8 LS bits in globalVcId, 2 MS bits in signaling field | ||
| 197 | // We extract and do logical OR in a single operation to appease GCC warnings related to int promotion in |= | ||
| 198 | const U16 spacecraftId = | ||
| 199 | 56 | static_cast<U16>(((header.get_globalVcId() & AOSHeaderSubfields::spacecraftIdLsbMask) >> | |
| 200 | 56 | AOSHeaderSubfields::spacecraftIdLsbOffset) | | |
| 201 | 56 | ((header.get_frameCountAndSignaling() & AOSHeaderSubfields::spacecraftIdMsbMask) | |
| 202 | 56 | << (8 - AOSHeaderSubfields::spacecraftIdMsbOffset))); | |
| 203 | |||
| 204 |
2/2✓ Branch 4 taken 1 times.
✓ Branch 5 taken 55 times.
|
56 | if (spacecraftId != m_spacecraftId) { |
| 205 |
1/1✓ Branch 9 taken 1 times.
|
1 | this->log_WARNING_LO_InvalidSpacecraftId(spacecraftId, m_spacecraftId); |
| 206 |
2/2✓ Branch 5 taken 1 times.
✓ Branch 8 taken 1 times.
|
1 | this->notifyErrorIfConnected(Ccsds::FrameError::AOS_INVALID_SCID); |
| 207 | 1 | return nullptr; | |
| 208 | } | ||
| 209 | |||
| 210 | // Extract Virtual Channel ID (Section 4.1.2.3) | ||
| 211 | 55 | U8 vcId = static_cast<U8>(header.get_globalVcId() & AOSHeaderSubfields::virtualChannelIdMask); | |
| 212 | 55 | AosDeframerVc* vc = this->getVcStruct(vcId); | |
| 213 | |||
| 214 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 54 times.
|
55 | if (vc == nullptr) { |
| 215 | // TODO: Multi VC | Handle logging all valid vcIds | ||
| 216 |
1/1✓ Branch 9 taken 1 times.
|
1 | this->log_ACTIVITY_LO_InvalidVcId(vcId, m_vcs[0].virtualChannelId); |
| 217 |
2/2✓ Branch 5 taken 1 times.
✓ Branch 8 taken 1 times.
|
1 | this->notifyErrorIfConnected(Ccsds::FrameError::AOS_INVALID_VCID); |
| 218 | 1 | return vc; | |
| 219 | } | ||
| 220 | |||
| 221 | // Extract Virtual Channel Frame Count (Section 4.1.2.4) | ||
| 222 | // 24 bits in the upper 3 bytes of frameCountAndSignaling | ||
| 223 | 54 | U32 rxVcFrameCount = (header.get_frameCountAndSignaling() & AOSHeaderSubfields::vcFrameCountMask) >> | |
| 224 | 54 | AOSHeaderSubfields::vcFrameCountOffset; | |
| 225 | |||
| 226 | // Default Frame Count is a 24 bit counter (e.g. modulo 2^24) | ||
| 227 | 54 | U32 frameCountMask = 0x00FF'FFFF; | |
| 228 | |||
| 229 | // Extract VC Frame Count Cycle if in use (Section 4.1.2.5.3) | ||
| 230 |
1/2✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
|
54 | if ((header.get_frameCountAndSignaling() & AOSHeaderSubfields::cycleCountFlagMask) != 0) { |
| 231 | 54 | const U8 rxVcFrameCountCycle = header.get_frameCountAndSignaling() & AOSHeaderSubfields::vcFrameCountCycleMask; | |
| 232 | // Extend the 24-bit frame count with the 4-bit cycle count | ||
| 233 | 54 | rxVcFrameCount |= static_cast<U32>(rxVcFrameCountCycle) << 24; | |
| 234 | // Add the 4 additional bits to our modulo | ||
| 235 | 54 | frameCountMask |= 0x0F00'0000; | |
| 236 | } | ||
| 237 | |||
| 238 | // Gap detect after the first accepted frame on a VC | ||
| 239 |
2/2✓ Branch 2 taken 24 times.
✓ Branch 3 taken 30 times.
|
54 | if (vc->framesProcessed > 0U) { |
| 240 | 24 | const U32 expectedVcFrameCount = vc->vcFrameCount + 1U; | |
| 241 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 21 times.
|
24 | if (rxVcFrameCount != (expectedVcFrameCount & frameCountMask)) { |
| 242 |
1/1✓ Branch 5 taken 3 times.
|
3 | this->log_WARNING_HI_VcFrameCountGap(vcId, rxVcFrameCount, expectedVcFrameCount); |
| 243 |
2/2✓ Branch 5 taken 3 times.
✓ Branch 8 taken 3 times.
|
3 | this->notifyErrorIfConnected(Ccsds::FrameError::AOS_VC_FRAME_COUNT_GAP); |
| 244 | // Other errors will implicitly drop their spanning packet once we finally lock back onto a valid frame | ||
| 245 |
1/1✓ Branch 5 taken 3 times.
|
3 | this->abandonSpanningPacket(*vc); |
| 246 | } | ||
| 247 | } | ||
| 248 | |||
| 249 | // Store VC frame count in the VC struct for reference (e.g. gap detection) | ||
| 250 | 54 | vc->vcFrameCount = rxVcFrameCount; | |
| 251 |
2/2✓ Branch 6 taken 54 times.
✓ Branch 11 taken 54 times.
|
54 | this->tlmWrite_LatestVcFrameCount(vc->vcFrameCount); |
| 252 | |||
| 253 | // Update context with extracted values | ||
| 254 |
1/1✓ Branch 4 taken 54 times.
|
54 | context.set_vcId(vcId); |
| 255 | |||
| 256 | 54 | return vc; | |
| 257 | 57 | } | |
| 258 | |||
| 259 | 58 | bool AosDeframer::validateFecf(Fw::Buffer& data) { | |
| 260 | // Per CCSDS 732.0-B-5 Section 4.1.6, FECF is a 16-bit CRC | ||
| 261 | // computed over all preceding bits in the frame | ||
| 262 | |||
| 263 | 58 | const FwSizeType crcDataLen = m_fixedFrameSize - AOSTrailer::SERIALIZED_SIZE; | |
| 264 |
2/2✓ Branch 4 taken 58 times.
✓ Branch 7 taken 58 times.
|
58 | U16 computedCrc = Ccsds::Utils::CRC16::compute(data.getData(), static_cast<U32>(crcDataLen)); |
| 265 | |||
| 266 | // Deserialize the trailer | ||
| 267 |
1/1✓ Branch 2 taken 58 times.
|
58 | AOSTrailer trailer; |
| 268 |
1/1✓ Branch 2 taken 58 times.
|
58 | auto deserializer = data.getDeserializer(); |
| 269 |
1/1✓ Branch 2 taken 58 times.
|
58 | Fw::SerializeStatus status = deserializer.moveDeserToOffset(crcDataLen); |
| 270 | 58 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 271 |
1/1✓ Branch 2 taken 58 times.
|
58 | status = deserializer.deserializeTo(trailer); |
| 272 | 58 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 273 | |||
| 274 | 58 | U16 transmittedCrc = trailer.get_fecf(); | |
| 275 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 56 times.
|
58 | if (transmittedCrc != computedCrc) { |
| 276 |
1/1✓ Branch 5 taken 2 times.
|
2 | this->log_WARNING_HI_InvalidFecf(transmittedCrc, computedCrc); |
| 277 |
2/2✓ Branch 5 taken 2 times.
✓ Branch 8 taken 2 times.
|
2 | this->notifyErrorIfConnected(Ccsds::FrameError::AOS_INVALID_CRC); |
| 278 |
2/2✓ Branch 6 taken 2 times.
✓ Branch 17 taken 2 times.
|
2 | this->tlmWrite_CrcErrorCount(++m_crcErrorCount); |
| 279 | 2 | return false; | |
| 280 | } | ||
| 281 | |||
| 282 | 56 | return true; | |
| 283 | 58 | } | |
| 284 | |||
| 285 | 84 | FwSizeType AosDeframer::appendToSpanningPacket(AosDeframerVc& vc, U8* data, FwSizeType size) { | |
| 286 | 84 | FW_ASSERT(data != nullptr); | |
| 287 | 84 | FW_ASSERT(size > 0, static_cast<FwAssertArgType>(size)); | |
| 288 | |||
| 289 | // How much the outer func needs to seek forward in the AOS frame | ||
| 290 | 84 | FwSizeType seekForward = 0; | |
| 291 | |||
| 292 | // We work out of the static header buffer until we know the full packet size | ||
| 293 |
2/2✓ Branch 6 taken 71 times.
✓ Branch 7 taken 13 times.
|
84 | if (!vc.spanningPacket.buffer.isValid()) { |
| 294 | // (Keep) packing whatever we've got into the static header buffer | ||
| 295 | 71 | const FwSizeType headerCap = AosDeframerVc::SpanningPacketState::HEADER_BUF_SIZE; | |
| 296 | // Pack the lesser of how much we have & how much room we have | ||
| 297 | 71 | const FwSizeType toHeader = FW_MIN(size, headerCap - vc.spanningPacket.bytesReceived); | |
| 298 |
1/2✓ Branch 0 taken 71 times.
✗ Branch 1 not taken.
|
71 | if (toHeader > 0) { |
| 299 | 71 | FW_ASSERT(vc.spanningPacket.bytesReceived < headerCap, | |
| 300 | static_cast<FwAssertArgType>(vc.spanningPacket.bytesReceived), | ||
| 301 | static_cast<FwAssertArgType>(headerCap)); | ||
| 302 | 71 | FW_ASSERT(toHeader <= headerCap, static_cast<FwAssertArgType>(toHeader), | |
| 303 | static_cast<FwAssertArgType>(headerCap)); | ||
| 304 | 71 | FW_ASSERT(vc.spanningPacket.bytesReceived + toHeader <= headerCap, | |
| 305 | static_cast<FwAssertArgType>(vc.spanningPacket.bytesReceived), | ||
| 306 | static_cast<FwAssertArgType>(toHeader), static_cast<FwAssertArgType>(headerCap)); | ||
| 307 |
2/4✗ Branch 5 not taken.
✓ Branch 6 taken 71 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 71 times.
|
71 | (void)::memcpy(vc.spanningPacket.headerBuf + vc.spanningPacket.bytesReceived, data, toHeader); |
| 308 | 71 | vc.spanningPacket.bytesReceived += toHeader; | |
| 309 | |||
| 310 | // We'll work w/ everything past the copied header if we get a clean parse | ||
| 311 | 71 | data += toHeader; | |
| 312 | 71 | size -= toHeader; | |
| 313 | 71 | seekForward += toHeader; | |
| 314 | } | ||
| 315 | |||
| 316 | // Attempt to find a size w/ what we have in our header buff (zero means we ran out of frame before valid | ||
| 317 | // packet) | ||
| 318 | 71 | const FwSizeType packetSize = sizePacket(vc, vc.spanningPacket.headerBuf, vc.spanningPacket.bytesReceived); | |
| 319 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 44 times.
|
71 | if (packetSize == 0) { |
| 320 | 27 | return 0; | |
| 321 | } | ||
| 322 | |||
| 323 | // A packet may declare fewer bytes than the header buffer holds, so give the surplus back | ||
| 324 | // to the caller as the next packet instead of copying it past the end of this one | ||
| 325 |
2/2✓ Branch 2 taken 1 times.
✓ Branch 3 taken 43 times.
|
44 | if (packetSize < vc.spanningPacket.bytesReceived) { |
| 326 | 1 | const FwSizeType surplus = vc.spanningPacket.bytesReceived - packetSize; | |
| 327 | // Prior calls left bytesReceived <= packetSize, so the surplus came from this call | ||
| 328 | 1 | FW_ASSERT(surplus <= toHeader, static_cast<FwAssertArgType>(surplus), | |
| 329 | static_cast<FwAssertArgType>(toHeader)); | ||
| 330 | 1 | vc.spanningPacket.bytesReceived = packetSize; | |
| 331 | 1 | data -= surplus; | |
| 332 | 1 | size += surplus; | |
| 333 | 1 | seekForward -= surplus; | |
| 334 | } | ||
| 335 | |||
| 336 | // Try to allocate a buffer for the whole packet. If this size is invalid (too large) or if the buffer | ||
| 337 | // manager is out of memory, this is handled below. | ||
| 338 |
2/2✓ Branch 3 taken 44 times.
✓ Branch 12 taken 44 times.
|
44 | vc.spanningPacket.buffer = this->allocate_out(0, packetSize); |
| 339 |
5/6✓ Branch 6 taken 40 times.
✓ Branch 7 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 40 times.
✓ Branch 16 taken 4 times.
✓ Branch 17 taken 40 times.
|
44 | if ((not vc.spanningPacket.buffer.isValid()) || (vc.spanningPacket.buffer.getSize() < packetSize)) { |
| 340 |
2/2✓ Branch 12 taken 4 times.
✓ Branch 18 taken 4 times.
|
4 | this->log_WARNING_HI_SpanningPacketAllocFailed(vc.virtualChannelId, vc.spanningPacket.context.get_pvn(), |
| 341 | packetSize); | ||
| 342 | // Save before abandon clears it -— needed for the correct seek offset below | ||
| 343 | 4 | const FwSizeType remainingBody = packetSize - vc.spanningPacket.bytesReceived; | |
| 344 | 4 | this->abandonSpanningPacket(vc); | |
| 345 | |||
| 346 | // Seek past the failed packet (header bytes already consumed + remaining body) | ||
| 347 | 4 | const FwSizeType remainingLength = seekForward + remainingBody; | |
| 348 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
|
4 | if (remainingBody > size) { |
| 349 | 3 | return 0; | |
| 350 | } else { | ||
| 351 | 1 | return remainingLength; | |
| 352 | } | ||
| 353 | } | ||
| 354 | |||
| 355 | // Load the header into the dynamic buffer | ||
| 356 | 40 | FW_ASSERT(vc.spanningPacket.bytesReceived <= AosDeframerVc::SpanningPacketState::HEADER_BUF_SIZE, | |
| 357 | static_cast<FwAssertArgType>(vc.spanningPacket.bytesReceived), | ||
| 358 | AosDeframerVc::SpanningPacketState::HEADER_BUF_SIZE); | ||
| 359 | // Destination buffer must be large enough for the accumulated header bytes. | ||
| 360 | // Protect against any future regression in sizeEppPacket/sizeSppPacket | ||
| 361 | // reintroducing an overflow that makes packetSize < bytesReceived. | ||
| 362 | 40 | FW_ASSERT(vc.spanningPacket.bytesReceived <= vc.spanningPacket.buffer.getSize(), | |
| 363 | static_cast<FwAssertArgType>(vc.spanningPacket.bytesReceived), | ||
| 364 | static_cast<FwAssertArgType>(vc.spanningPacket.buffer.getSize())); | ||
| 365 |
2/4✗ Branch 10 not taken.
✓ Branch 11 taken 40 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 40 times.
|
40 | (void)::memcpy(vc.spanningPacket.buffer.getData(), vc.spanningPacket.headerBuf, |
| 366 | vc.spanningPacket.bytesReceived); | ||
| 367 | } | ||
| 368 | |||
| 369 | // Already have the dynamic buffer, so fill away | ||
| 370 | 53 | const FwSizeType spaceLeft = vc.spanningPacket.buffer.getSize() - vc.spanningPacket.bytesReceived; | |
| 371 | // Copy what we got | ||
| 372 |
2/2✓ Branch 0 taken 16 times.
✓ Branch 1 taken 37 times.
|
53 | const FwSizeType toBody = FW_MIN(size, spaceLeft); |
| 373 |
2/2✓ Branch 0 taken 52 times.
✓ Branch 1 taken 1 times.
|
53 | if (toBody > 0) { |
| 374 | 52 | FW_ASSERT(vc.spanningPacket.bytesReceived + toBody <= vc.spanningPacket.buffer.getSize(), | |
| 375 | static_cast<FwAssertArgType>(vc.spanningPacket.bytesReceived), static_cast<FwAssertArgType>(toBody), | ||
| 376 | static_cast<FwAssertArgType>(vc.spanningPacket.buffer.getSize())); | ||
| 377 |
2/4✗ Branch 9 not taken.
✓ Branch 10 taken 52 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 52 times.
|
52 | (void)::memcpy(vc.spanningPacket.buffer.getData() + vc.spanningPacket.bytesReceived, data, toBody); |
| 378 | 52 | vc.spanningPacket.bytesReceived += toBody; | |
| 379 | 52 | seekForward += toBody; | |
| 380 | } | ||
| 381 | |||
| 382 | // Check if the spanning packet is now complete | ||
| 383 |
3/4✓ Branch 6 taken 53 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 37 times.
✓ Branch 9 taken 16 times.
|
106 | if (vc.spanningPacket.buffer.getSize() > 0 && |
| 384 |
2/2✓ Branch 8 taken 37 times.
✓ Branch 9 taken 16 times.
|
53 | vc.spanningPacket.bytesReceived >= vc.spanningPacket.buffer.getSize()) { |
| 385 | 37 | this->dataOut_out(0, vc.spanningPacket.buffer, vc.spanningPacket.context); | |
| 386 |
2/2✓ Branch 6 taken 37 times.
✓ Branch 15 taken 37 times.
|
37 | this->tlmWrite_PacketsExtracted(++vc.packetsExtracted); |
| 387 | |||
| 388 | // Ownership of the buffer has transferred downstream; clear local handle before consolidating state reset. | ||
| 389 |
2/2✓ Branch 2 taken 37 times.
✓ Branch 11 taken 37 times.
|
37 | vc.spanningPacket.buffer = Fw::Buffer(); |
| 390 | // Buffer won't be returned now since we cleared the handle | ||
| 391 | 37 | this->abandonSpanningPacket(vc); | |
| 392 | } | ||
| 393 | |||
| 394 | 53 | return seekForward; | |
| 395 | } | ||
| 396 | |||
| 397 | 54 | void AosDeframer::extractPackets(AosDeframerVc& vc, Fw::Buffer& data) { | |
| 398 | // Parse M_PDU header (per CCSDS 732.0-B-5 Section 4.1.4.2.2) | ||
| 399 |
1/1✓ Branch 2 taken 54 times.
|
54 | M_PDUHeader mpduHeader; |
| 400 |
1/1✓ Branch 2 taken 54 times.
|
54 | auto deserializer = data.getDeserializer(); |
| 401 |
1/1✓ Branch 2 taken 54 times.
|
54 | Fw::SerializeStatus status = deserializer.moveDeserToOffset(AOSHeader::SERIALIZED_SIZE); |
| 402 | 54 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 403 |
1/1✓ Branch 2 taken 54 times.
|
54 | status = deserializer.deserializeTo(mpduHeader); |
| 404 | 54 | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 405 | |||
| 406 | 54 | U16 firstHeaderPointer = mpduHeader.get_firstHeaderPointer(); | |
| 407 | |||
| 408 | // Calculate data zone boundaries | ||
| 409 | 54 | const FwSizeType dataZoneStart = AOSHeader::SERIALIZED_SIZE + M_PDUHeader::SERIALIZED_SIZE; | |
| 410 |
3/4✗ Branch 7 not taken.
✓ Branch 8 taken 54 times.
✓ Branch 9 taken 53 times.
✓ Branch 10 taken 1 times.
|
54 | const FwSizeType dataZoneEnd = m_fixedFrameSize - (m_fecfEnabled ? AOSTrailer::SERIALIZED_SIZE : 0); |
| 411 | 54 | const FwSizeType dataZoneSize = dataZoneEnd - dataZoneStart; | |
| 412 |
1/1✓ Branch 4 taken 54 times.
|
54 | U8* dataZone = data.getData() + dataZoneStart; |
| 413 | |||
| 414 | // Handle special First Header Pointer values (Section 4.1.4.2.2.4) | ||
| 415 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 52 times.
|
54 | if (firstHeaderPointer == M_PDUSubfields::FHP_IDLE_DATA_ONLY) { |
| 416 | // Frame contains only idle data | ||
| 417 |
1/1✓ Branch 7 taken 2 times.
|
2 | this->log_ACTIVITY_LO_IdleFrame(vc.virtualChannelId); |
| 418 | 2 | return; | |
| 419 | } | ||
| 420 | // Handle continuation data (data before First Header Pointer) | ||
| 421 |
2/2✓ Branch 0 taken 13 times.
✓ Branch 1 taken 39 times.
|
52 | else if (firstHeaderPointer == M_PDUSubfields::FHP_NO_PACKET_START) { |
| 422 | // Entire data zone is continuation of previous packet | ||
| 423 |
2/2✓ Branch 2 taken 12 times.
✓ Branch 3 taken 1 times.
|
13 | if (vc.spanningPacket.bytesReceived > 0) { |
| 424 |
1/1✓ Branch 4 taken 12 times.
|
12 | (void)this->appendToSpanningPacket(vc, dataZone, dataZoneSize); |
| 425 | } | ||
| 426 | // If no spanning packet active, this continuation data cannot be used | ||
| 427 | 13 | return; | |
| 428 | } | ||
| 429 | |||
| 430 | // Guard against First Header Pointer pointing out of bounds (untrusted input) | ||
| 431 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 38 times.
|
39 | if (firstHeaderPointer >= dataZoneSize) { |
| 432 |
1/1✓ Branch 7 taken 1 times.
|
1 | this->log_WARNING_HI_InvalidFhp(vc.virtualChannelId, firstHeaderPointer, dataZoneSize); |
| 433 |
2/2✓ Branch 5 taken 1 times.
✓ Branch 8 taken 1 times.
|
1 | this->notifyErrorIfConnected(Ccsds::FrameError::AOS_INVALID_LENGTH); |
| 434 | // Abandon any existing data since this frame (and any continuing packets) are garbage now | ||
| 435 |
1/1✓ Branch 4 taken 1 times.
|
1 | this->abandonSpanningPacket(vc); |
| 436 | 1 | return; | |
| 437 | } | ||
| 438 | |||
| 439 | // There is continuation data before the first packet header | ||
| 440 |
4/4✓ Branch 0 taken 6 times.
✓ Branch 1 taken 32 times.
✓ Branch 4 taken 4 times.
✓ Branch 5 taken 2 times.
|
38 | if (firstHeaderPointer > 0 && vc.spanningPacket.bytesReceived > 0) { |
| 441 |
1/1✓ Branch 4 taken 4 times.
|
4 | (void)this->appendToSpanningPacket(vc, dataZone, static_cast<FwSizeType>(firstHeaderPointer)); |
| 442 | // We must be done w/ the prior packet since we have a FHP | ||
| 443 |
1/1✓ Branch 4 taken 4 times.
|
4 | this->abandonSpanningPacket(vc); |
| 444 | } | ||
| 445 | |||
| 446 | // Move to first packet header | ||
| 447 | 38 | FwSizeType currentOffset = firstHeaderPointer; | |
| 448 | |||
| 449 | // Max Bound is a sequence of 1 byte EPP Idle Packets | ||
| 450 | 38 | const FwIndexType maxIters = static_cast<FwIndexType>(dataZoneSize - firstHeaderPointer); | |
| 451 | |||
| 452 | // Extract packets starting at First Header Pointer | ||
| 453 | // (All fresh packets from here on out) | ||
| 454 |
3/4✓ Branch 0 taken 77 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 68 times.
✓ Branch 3 taken 9 times.
|
77 | for (FwIndexType iter = 0; iter < maxIters && currentOffset < dataZoneSize; iter++) { |
| 455 | // Clear out any prior packet data | ||
| 456 |
1/1✓ Branch 4 taken 68 times.
|
68 | this->abandonSpanningPacket(vc); |
| 457 | |||
| 458 | 68 | U8* packetStart = dataZone + currentOffset; | |
| 459 | 68 | FwSizeType remainingBytes = dataZoneSize - currentOffset; | |
| 460 | |||
| 461 |
1/1✓ Branch 4 taken 68 times.
|
68 | FwSizeType packetSize = this->appendToSpanningPacket(vc, packetStart, remainingBytes); |
| 462 | |||
| 463 |
2/2✓ Branch 0 taken 29 times.
✓ Branch 1 taken 39 times.
|
68 | if (packetSize == 0) { |
| 464 | // Break out of loop since we ran out of data | ||
| 465 | 29 | return; | |
| 466 | } | ||
| 467 | |||
| 468 | 39 | currentOffset += packetSize; | |
| 469 | } | ||
| 470 | 99 | } | |
| 471 | |||
| 472 | 71 | FwSizeType AosDeframer::sizePacket(AosDeframerVc& vc, U8* packetStart, FwSizeType remainingBytes) { | |
| 473 | 71 | FW_ASSERT(packetStart != nullptr); | |
| 474 | 71 | FW_ASSERT(remainingBytes > 0, static_cast<FwAssertArgType>(remainingBytes)); | |
| 475 | |||
| 476 | // Determine packet type from PVN (upper 3 bits of first byte) | ||
| 477 | 71 | U8 pvn = getPacketVersion(packetStart[0]); | |
| 478 | // Default to invalid, override if valid (non-idle) packet | ||
| 479 |
1/1✓ Branch 6 taken 71 times.
|
71 | vc.spanningPacket.context.set_pvn(ComCfg::Pvn::INVALID_UNINITIALIZED); |
| 480 | |||
| 481 | // Check if this pvn is disabled | ||
| 482 |
3/4✗ Branch 0 not taken.
✓ Branch 1 taken 71 times.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 69 times.
|
71 | if (~vc.pvnMask & (1 << pvn)) { |
| 483 |
1/1✓ Branch 7 taken 2 times.
|
2 | this->log_WARNING_HI_DisabledPvn(vc.virtualChannelId, pvn); |
| 484 | 2 | return 0; | |
| 485 | } | ||
| 486 | |||
| 487 |
1/1✓ Branch 2 taken 69 times.
|
69 | ComCfg::Pvn pvnEnum = static_cast<ComCfg::Pvn::T>(pvn); |
| 488 |
1/1✓ Branch 8 taken 69 times.
|
69 | vc.spanningPacket.context.set_pvn(pvnEnum); |
| 489 | |||
| 490 | // Size the Packet (so we can alloc a buffer) | ||
| 491 |
2/3✓ Branch 2 taken 37 times.
✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
|
69 | switch (pvnEnum) { |
| 492 | 37 | case ComCfg::Pvn::SPACE_PACKET_PROTOCOL: | |
| 493 |
1/1✓ Branch 4 taken 37 times.
|
37 | return sizeSppPacket(packetStart, remainingBytes); |
| 494 | break; | ||
| 495 | 32 | case ComCfg::Pvn::ENCAPSULATION_PACKET_PROTOCOL: | |
| 496 |
1/1✓ Branch 4 taken 32 times.
|
32 | return sizeEppPacket(packetStart, remainingBytes); |
| 497 | break; | ||
| 498 | ✗ | default: | |
| 499 | // User should only configure AOS Deframer to accept SPP &/| EPP | ||
| 500 | ✗ | FW_ASSERT(false, pvn); | |
| 501 | ✗ | return 0; | |
| 502 | } | ||
| 503 | 69 | } | |
| 504 | |||
| 505 | 37 | FwSizeType AosDeframer::sizeSppPacket(U8* payloadStart, FwSizeType payloadSize) { | |
| 506 | 37 | FW_ASSERT(payloadStart != nullptr); | |
| 507 |
1/1✓ Branch 2 taken 37 times.
|
37 | SpacePacketHeader header; |
| 508 | |||
| 509 |
1/1✓ Branch 2 taken 37 times.
|
37 | Fw::Buffer data(payloadStart, payloadSize); |
| 510 |
2/2✓ Branch 2 taken 37 times.
✓ Branch 8 taken 37 times.
|
37 | Fw::SerializeStatus status = data.getDeserializer().deserializeTo(header); |
| 511 | |||
| 512 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 36 times.
|
37 | if (status != Fw::FW_SERIALIZE_OK) { |
| 513 | 1 | return 0; // Incomplete header - spans to next frame | |
| 514 | } | ||
| 515 | |||
| 516 | // Per CCSDS 133.0-B-2 Section 4.1.3.5.2, packet data length = (actual length - 1) | ||
| 517 | // packetDataLength is a 16-bit field (max 65535); SERIALIZED_SIZE is a small constant. | ||
| 518 | // Guarantee at compile time that the maximum possible sum fits in FwSizeType. If | ||
| 519 | // FwSizeType is ever narrowed below 17 bits, this fails to build and the addition | ||
| 520 | // below must be guarded the same way sizeEppPacket is. | ||
| 521 | 36 | constexpr FwSizeType MAX_LENGTH = std::numeric_limits<FwSizeType>::max() - SpacePacketHeader::SERIALIZED_SIZE; | |
| 522 | static_assert(MAX_LENGTH >= std::numeric_limits<U16>::max() + 1, | ||
| 523 | "FwSizeType must be wide enough to hold the maximum SPP packet size without overflow"); | ||
| 524 | 36 | FwSizeType totalPacketSize = SpacePacketHeader::SERIALIZED_SIZE + header.get_packetDataLength() + 1; | |
| 525 | |||
| 526 | // TODO: Unify Deframers | bring the whole spp processing into this component | ||
| 527 | // since we're only missing seq count logic? | ||
| 528 | |||
| 529 | // Check for idle packet (APID = 0x7FF per CCSDS 133.0-B-2) | ||
| 530 | 36 | U16 apid = static_cast<U16>(header.get_packetIdentification() & SpacePacketSubfields::ApidMask); | |
| 531 | |||
| 532 | // Idle means this is the last packet in the frame | ||
| 533 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 35 times.
|
36 | if (apid == static_cast<U16>(ComCfg::Apid::SPP_IDLE_PACKET)) { |
| 534 | 1 | return 0; | |
| 535 | } | ||
| 536 | |||
| 537 | 35 | return totalPacketSize; | |
| 538 | 37 | } | |
| 539 | |||
| 540 | 32 | FwSizeType AosDeframer::sizeEppPacket(const U8* const payloadStart, FwSizeType payloadSize) { | |
| 541 | // Per CCSDS 133.1-B-3 Section 4.1.2.1.1, EPP minimum header is 1 byte | ||
| 542 | // Since we identified this as an EPP we had the 1 byte to read the PVN already | ||
| 543 | 32 | FW_ASSERT(payloadStart != nullptr); | |
| 544 | 32 | FW_ASSERT(payloadSize > 0, static_cast<FwAssertArgType>(payloadSize)); | |
| 545 | |||
| 546 | // Parse first byte | ||
| 547 | 32 | U8 firstByte = payloadStart[0]; | |
| 548 | 32 | U8 protocolId = static_cast<U8>((firstByte & EPPSubfields::protocolIdMask) >> EPPSubfields::protocolIdOffset); | |
| 549 | |||
| 550 | 32 | FwSizeType totalPacketSize = 0; | |
| 551 | |||
| 552 | // Idle means this is the last packet in the frame | ||
| 553 |
2/2✓ Branch 0 taken 21 times.
✓ Branch 1 taken 11 times.
|
32 | if (protocolId == static_cast<U8>(EppProtocolId::Idle)) { |
| 554 | 21 | return 0; | |
| 555 | } | ||
| 556 | |||
| 557 | // Encapsulation Idle Packet per CCSDS 133.1-B-3 Section 4.1.3.2 | ||
| 558 | 11 | U8 lengthOfLength = firstByte & EPPSubfields::lengthOfLengthMask; | |
| 559 | |||
| 560 | 11 | U8 lengthOffset = 1U; | |
| 561 | |||
| 562 | // If length of length is 2 or more then there's an extra byte of extension/user defined (4.1.2.1.1) | ||
| 563 |
2/2✓ Branch 0 taken 8 times.
✓ Branch 1 taken 3 times.
|
11 | if (lengthOfLength >= EppLengthOfLength::Two) { |
| 564 | 8 | lengthOffset = static_cast<U8>(lengthOffset + 1U); | |
| 565 | } | ||
| 566 | |||
| 567 | // If length of length is 4 then we add 2 bytes for the ccsds reserved field (4.1.2.1.1) | ||
| 568 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 7 times.
|
11 | if (lengthOfLength == EppLengthOfLength::Four) { |
| 569 | 4 | lengthOffset = static_cast<U8>(lengthOffset + 2U); | |
| 570 | // '0d3' on the wire, but means 4 | ||
| 571 | 4 | lengthOfLength = 4; | |
| 572 | } | ||
| 573 | |||
| 574 | // Bytes to get to length + length of length | ||
| 575 | 11 | const U8 headerLength = static_cast<U8>(lengthOffset + lengthOfLength); | |
| 576 | |||
| 577 | // Validate and read length field | ||
| 578 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 9 times.
|
11 | if (payloadSize < headerLength) { |
| 579 | 2 | return 0; // Incomplete | |
| 580 | } | ||
| 581 | |||
| 582 | // Read length field (big-endian) | ||
| 583 | 9 | U32 packetDataLength = 0; | |
| 584 |
2/2✓ Branch 0 taken 21 times.
✓ Branch 1 taken 9 times.
|
30 | for (U8 i = 0; i < lengthOfLength; i++) { |
| 585 | 21 | packetDataLength = (packetDataLength << 8) | payloadStart[lengthOffset + i]; | |
| 586 | } | ||
| 587 | |||
| 588 | // Guard against integer overflow and return 0 as incomplete/invalid (if true). | ||
| 589 | // This fires on 32-bit targets (FwSizeType = U32) where the sum would wrap. | ||
| 590 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 9 times.
|
9 | if (packetDataLength > (std::numeric_limits<FwSizeType>::max() - static_cast<FwSizeType>(headerLength))) { |
| 591 | ✗ | return 0; | |
| 592 | } | ||
| 593 | |||
| 594 | // Cast both operands to FwSizeType BEFORE adding. | ||
| 595 | // Without the cast, C++ computes headerLength(U8) + packetDataLength(U32) in U32 | ||
| 596 | // arithmetic, which wraps on both 32-bit and 64-bit hosts even when FwSizeType is | ||
| 597 | // 64 bits wide. The guard above is not sufficient on its own: on 64-bit it never | ||
| 598 | // fires (packetDataLength can never exceed UINT64_MAX-8), so the unguarded addition | ||
| 599 | // would still silently truncate to 4 on a 64-bit host. | ||
| 600 | 9 | totalPacketSize = static_cast<FwSizeType>(headerLength) + static_cast<FwSizeType>(packetDataLength); | |
| 601 | |||
| 602 | 9 | return totalPacketSize; | |
| 603 | } | ||
| 604 | |||
| 605 | 71 | U8 AosDeframer::getPacketVersion(U8 firstByte) { | |
| 606 | // PVN is the upper 3 bits per both CCSDS 133.0-B-2 and 133.1-B-3 | ||
| 607 | // EPP's Subfield array is done in bytes | ||
| 608 | 71 | return static_cast<U8>(firstByte >> EPPSubfields::packetVersionOffset); | |
| 609 | } | ||
| 610 | |||
| 611 | } // namespace Ccsds | ||
| 612 | } // namespace Svc | ||
| 613 |