| 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 | ✗ | AosDeframer::AosDeframer(const char* const compName) | |
| 29 | : AosDeframerComponentBase(compName), | ||
| 30 | ✗ | m_fixedFrameSize(ComCfg::AosMaxFrameFixedSize), | |
| 31 | ✗ | m_fecfEnabled(true), | |
| 32 | ✗ | m_spacecraftId(ComCfg::SpacecraftId), | |
| 33 | ✗ | m_crcErrorCount(0) { | |
| 34 | // Initialize VC struct | ||
| 35 | ✗ | for (U8 vcInd = 0; vcInd < AosDeframer_NumVcs; vcInd++) { | |
| 36 | ✗ | m_vcs[vcInd].vcStructIndex = vcInd; | |
| 37 | } | ||
| 38 | ✗ | } | |
| 39 | |||
| 40 | ✗ | AosDeframer::~AosDeframer() {} | |
| 41 | |||
| 42 | ✗ | void AosDeframer::configure(U32 fixedFrameSize, bool frameErrorControlField, U16 spacecraftId, U8 vcId, U8 pvnMask) { | |
| 43 | // Validate frame size is within bounds | ||
| 44 | ✗ | 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 | ✗ | const FwSizeType minSize = AOSHeader::SERIALIZED_SIZE + M_PDUHeader::SERIALIZED_SIZE + | |
| 49 | (frameErrorControlField ? AOSTrailer::SERIALIZED_SIZE : 0); | ||
| 50 | ✗ | 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 | ✗ | 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 | ✗ | 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 | ✗ | FW_ASSERT((pvnMask & PvnBitfield::VALID_MASK) != 0, static_cast<FwAssertArgType>(pvnMask)); | |
| 61 | ✗ | FW_ASSERT((pvnMask & ~PvnBitfield::VALID_MASK) == 0, static_cast<FwAssertArgType>(pvnMask)); | |
| 62 | |||
| 63 | // Spanning packet reassembly requires dynamic backing via allocator ports | ||
| 64 | ✗ | FW_ASSERT(this->isConnected_allocate_OutputPort(0)); | |
| 65 | ✗ | FW_ASSERT(this->isConnected_deallocate_OutputPort(0)); | |
| 66 | |||
| 67 | ✗ | m_fixedFrameSize = fixedFrameSize; | |
| 68 | ✗ | m_fecfEnabled = frameErrorControlField; | |
| 69 | ✗ | m_spacecraftId = spacecraftId; | |
| 70 | |||
| 71 | // Zero out FECF error counter on (re)configure | ||
| 72 | ✗ | m_crcErrorCount = 0; | |
| 73 | |||
| 74 | // Populate the (single) VC struct | ||
| 75 | ✗ | m_vcs[0].virtualChannelId = vcId; | |
| 76 | ✗ | m_vcs[0].pvnMask = pvnMask; | |
| 77 | |||
| 78 | // Clear out all VC stats | ||
| 79 | ✗ | for (U8 vcInd = 0; vcInd < AosDeframer_NumVcs; vcInd++) { | |
| 80 | ✗ | m_vcs[vcInd].framesProcessed = 0; | |
| 81 | ✗ | m_vcs[vcInd].packetsExtracted = 0; | |
| 82 | ✗ | m_vcs[vcInd].vcFrameCount = 0; | |
| 83 | |||
| 84 | // Clear out the spanningPacket | ||
| 85 | ✗ | this->abandonSpanningPacket(m_vcs[vcInd]); | |
| 86 | } | ||
| 87 | ✗ | } | |
| 88 | |||
| 89 | // ---------------------------------------------------------------------- | ||
| 90 | // Handler implementations for user-defined typed input ports | ||
| 91 | // ---------------------------------------------------------------------- | ||
| 92 | |||
| 93 | ✗ | 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 | ✗ | FW_ASSERT(m_fixedFrameSize > 0, static_cast<FwAssertArgType>(m_fixedFrameSize)); | |
| 97 | |||
| 98 | ✗ | if (data.getSize() < m_fixedFrameSize) { | |
| 99 | ✗ | this->log_WARNING_HI_InvalidFrameLength(data.getSize(), m_fixedFrameSize); | |
| 100 | ✗ | this->notifyErrorIfConnected(Ccsds::FrameError::AOS_INVALID_LENGTH); | |
| 101 | ✗ | this->dataReturnOut_out(0, data, context); | |
| 102 | ✗ | 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 | ✗ | if (m_fecfEnabled) { | |
| 108 | ✗ | const bool fecfValid = this->validateFecf(data); | |
| 109 | ✗ | if (!fecfValid) { | |
| 110 | ✗ | this->dataReturnOut_out(0, data, context); | |
| 111 | ✗ | return; | |
| 112 | } | ||
| 113 | } | ||
| 114 | |||
| 115 | // Create a mutable context for extracted packet info | ||
| 116 | ✗ | 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 | ✗ | AosDeframerVc* vc = this->parseAndValidateHeader(data, packetContext); | |
| 120 | ✗ | if (vc == nullptr) { | |
| 121 | ✗ | this->dataReturnOut_out(0, data, context); | |
| 122 | ✗ | 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 | ✗ | if (!vc->spanningPacket.buffer.isValid()) { | |
| 128 | ✗ | vc->spanningPacket.context = packetContext; | |
| 129 | } | ||
| 130 | |||
| 131 | // Extract packets from the M_PDU data zone | ||
| 132 | ✗ | this->extractPackets(*vc, data); | |
| 133 | |||
| 134 | // Return the frame buffer | ||
| 135 | ✗ | this->dataReturnOut_out(0, data, context); | |
| 136 | |||
| 137 | // Update telemetry | ||
| 138 | ✗ | this->tlmWrite_FramesProcessed(++vc->framesProcessed); | |
| 139 | ✗ | } | |
| 140 | |||
| 141 | ✗ | void AosDeframer::dataReturnIn_handler(FwIndexType portNum, Fw::Buffer& fwBuffer, const ComCfg::FrameContext& context) { | |
| 142 | // Deallocate this dynamically allocated packet | ||
| 143 | ✗ | this->deallocate_out(0, fwBuffer); | |
| 144 | ✗ | } | |
| 145 | |||
| 146 | // ---------------------------------------------------------------------- | ||
| 147 | // Private helper methods | ||
| 148 | // ---------------------------------------------------------------------- | ||
| 149 | |||
| 150 | ✗ | void AosDeframer::notifyErrorIfConnected(Ccsds::FrameError error) { | |
| 151 | ✗ | if (this->isConnected_errorNotify_OutputPort(0)) { | |
| 152 | ✗ | this->errorNotify_out(0, error); | |
| 153 | } | ||
| 154 | ✗ | } | |
| 155 | |||
| 156 | ✗ | void AosDeframer::abandonSpanningPacket(AosDeframerVc& vc) { | |
| 157 | ✗ | if (vc.spanningPacket.buffer.isValid()) { | |
| 158 | ✗ | this->log_WARNING_HI_SpanningPacketAbandoned(vc.virtualChannelId, vc.spanningPacket.context.get_pvn(), | |
| 159 | vc.spanningPacket.bytesReceived, | ||
| 160 | vc.spanningPacket.buffer.getSize()); | ||
| 161 | ✗ | this->deallocate_out(0, vc.spanningPacket.buffer); | |
| 162 | } | ||
| 163 | ✗ | vc.spanningPacket.buffer = Fw::Buffer(); | |
| 164 | ✗ | vc.spanningPacket.bytesReceived = 0; | |
| 165 | ✗ | vc.spanningPacket.context.set_pvn(ComCfg::Pvn::INVALID_UNINITIALIZED); | |
| 166 | ✗ | } | |
| 167 | |||
| 168 | ✗ | AosDeframer::AosDeframerVc* AosDeframer::getVcStruct(const U8 vcId) { | |
| 169 | ✗ | for (U8 vcInd = 0; vcInd < AosDeframer_NumVcs; vcInd++) { | |
| 170 | ✗ | if (m_vcs[vcInd].virtualChannelId == vcId) { | |
| 171 | ✗ | return &m_vcs[vcInd]; | |
| 172 | } | ||
| 173 | } | ||
| 174 | |||
| 175 | ✗ | return nullptr; | |
| 176 | } | ||
| 177 | |||
| 178 | ✗ | 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 | ✗ | AOSHeader header; | |
| 181 | ✗ | Fw::SerializeStatus status = data.getDeserializer().deserializeTo(header); | |
| 182 | // We already checked that a header fits into fixedFrameSize & that this frame is >= fixedFrameSize | ||
| 183 | ✗ | 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 | ✗ | U8 tfvn = static_cast<U8>((header.get_globalVcId() & AOSHeaderSubfields::frameVersionMask) >> | |
| 188 | ✗ | AOSHeaderSubfields::frameVersionOffset); | |
| 189 | ✗ | if (tfvn != static_cast<U8>(Tfvn::AOS)) { | |
| 190 | ✗ | this->log_WARNING_HI_InvalidTfvn(tfvn, static_cast<U8>(Tfvn::AOS)); | |
| 191 | ✗ | this->notifyErrorIfConnected(Ccsds::FrameError::AOS_INVALID_VERSION); | |
| 192 | ✗ | 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 | ✗ | static_cast<U16>(((header.get_globalVcId() & AOSHeaderSubfields::spacecraftIdLsbMask) >> | |
| 200 | AOSHeaderSubfields::spacecraftIdLsbOffset) | | ||
| 201 | ✗ | ((header.get_frameCountAndSignaling() & AOSHeaderSubfields::spacecraftIdMsbMask) | |
| 202 | ✗ | << (8 - AOSHeaderSubfields::spacecraftIdMsbOffset))); | |
| 203 | |||
| 204 | ✗ | if (spacecraftId != m_spacecraftId) { | |
| 205 | ✗ | this->log_WARNING_LO_InvalidSpacecraftId(spacecraftId, m_spacecraftId); | |
| 206 | ✗ | this->notifyErrorIfConnected(Ccsds::FrameError::AOS_INVALID_SCID); | |
| 207 | ✗ | return nullptr; | |
| 208 | } | ||
| 209 | |||
| 210 | // Extract Virtual Channel ID (Section 4.1.2.3) | ||
| 211 | ✗ | U8 vcId = static_cast<U8>(header.get_globalVcId() & AOSHeaderSubfields::virtualChannelIdMask); | |
| 212 | ✗ | AosDeframerVc* vc = this->getVcStruct(vcId); | |
| 213 | |||
| 214 | ✗ | if (vc == nullptr) { | |
| 215 | // TODO: Multi VC | Handle logging all valid vcIds | ||
| 216 | ✗ | this->log_ACTIVITY_LO_InvalidVcId(vcId, m_vcs[0].virtualChannelId); | |
| 217 | ✗ | this->notifyErrorIfConnected(Ccsds::FrameError::AOS_INVALID_VCID); | |
| 218 | ✗ | 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 | ✗ | U32 rxVcFrameCount = (header.get_frameCountAndSignaling() & AOSHeaderSubfields::vcFrameCountMask) >> | |
| 224 | ✗ | AOSHeaderSubfields::vcFrameCountOffset; | |
| 225 | |||
| 226 | // Default Frame Count is a 24 bit counter (e.g. modulo 2^24) | ||
| 227 | ✗ | U32 frameCountMask = 0x00FF'FFFF; | |
| 228 | |||
| 229 | // Extract VC Frame Count Cycle if in use (Section 4.1.2.5.3) | ||
| 230 | ✗ | if ((header.get_frameCountAndSignaling() & AOSHeaderSubfields::cycleCountFlagMask) != 0) { | |
| 231 | ✗ | const U8 rxVcFrameCountCycle = header.get_frameCountAndSignaling() & AOSHeaderSubfields::vcFrameCountCycleMask; | |
| 232 | // Extend the 24-bit frame count with the 4-bit cycle count | ||
| 233 | ✗ | rxVcFrameCount |= static_cast<U32>(rxVcFrameCountCycle) << 24; | |
| 234 | // Add the 4 additional bits to our modulo | ||
| 235 | ✗ | frameCountMask |= 0x0F00'0000; | |
| 236 | } | ||
| 237 | |||
| 238 | // Gap detect after the first accepted frame on a VC | ||
| 239 | ✗ | if (vc->framesProcessed > 0U) { | |
| 240 | ✗ | const U32 expectedVcFrameCount = vc->vcFrameCount + 1U; | |
| 241 | ✗ | if (rxVcFrameCount != (expectedVcFrameCount & frameCountMask)) { | |
| 242 | ✗ | this->log_WARNING_HI_VcFrameCountGap(vcId, rxVcFrameCount, expectedVcFrameCount); | |
| 243 | ✗ | 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 | ✗ | this->abandonSpanningPacket(*vc); | |
| 246 | } | ||
| 247 | } | ||
| 248 | |||
| 249 | // Store VC frame count in the VC struct for reference (e.g. gap detection) | ||
| 250 | ✗ | vc->vcFrameCount = rxVcFrameCount; | |
| 251 | ✗ | this->tlmWrite_LatestVcFrameCount(vc->vcFrameCount); | |
| 252 | |||
| 253 | // Update context with extracted values | ||
| 254 | ✗ | context.set_vcId(vcId); | |
| 255 | |||
| 256 | ✗ | return vc; | |
| 257 | ✗ | } | |
| 258 | |||
| 259 | ✗ | 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 | ✗ | const FwSizeType crcDataLen = m_fixedFrameSize - AOSTrailer::SERIALIZED_SIZE; | |
| 264 | ✗ | U16 computedCrc = Ccsds::Utils::CRC16::compute(data.getData(), static_cast<U32>(crcDataLen)); | |
| 265 | |||
| 266 | // Deserialize the trailer | ||
| 267 | ✗ | AOSTrailer trailer; | |
| 268 | ✗ | auto deserializer = data.getDeserializer(); | |
| 269 | ✗ | Fw::SerializeStatus status = deserializer.moveDeserToOffset(crcDataLen); | |
| 270 | ✗ | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 271 | ✗ | status = deserializer.deserializeTo(trailer); | |
| 272 | ✗ | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 273 | |||
| 274 | ✗ | U16 transmittedCrc = trailer.get_fecf(); | |
| 275 | ✗ | if (transmittedCrc != computedCrc) { | |
| 276 | ✗ | this->log_WARNING_HI_InvalidFecf(transmittedCrc, computedCrc); | |
| 277 | ✗ | this->notifyErrorIfConnected(Ccsds::FrameError::AOS_INVALID_CRC); | |
| 278 | ✗ | this->tlmWrite_CrcErrorCount(++m_crcErrorCount); | |
| 279 | ✗ | return false; | |
| 280 | } | ||
| 281 | |||
| 282 | ✗ | return true; | |
| 283 | ✗ | } | |
| 284 | |||
| 285 | ✗ | FwSizeType AosDeframer::appendToSpanningPacket(AosDeframerVc& vc, U8* data, FwSizeType size) { | |
| 286 | ✗ | FW_ASSERT(data != nullptr); | |
| 287 | ✗ | FW_ASSERT(size > 0, static_cast<FwAssertArgType>(size)); | |
| 288 | |||
| 289 | // How much the outer func needs to seek forward in the AOS frame | ||
| 290 | ✗ | FwSizeType seekForward = 0; | |
| 291 | |||
| 292 | // We work out of the static header buffer until we know the full packet size | ||
| 293 | ✗ | if (!vc.spanningPacket.buffer.isValid()) { | |
| 294 | // (Keep) packing whatever we've got into the static header buffer | ||
| 295 | ✗ | const FwSizeType headerCap = AosDeframerVc::SpanningPacketState::HEADER_BUF_SIZE; | |
| 296 | // Pack the lesser of how much we have & how much room we have | ||
| 297 | ✗ | const FwSizeType toHeader = FW_MIN(size, headerCap - vc.spanningPacket.bytesReceived); | |
| 298 | ✗ | if (toHeader > 0) { | |
| 299 | ✗ | FW_ASSERT(vc.spanningPacket.bytesReceived < headerCap, | |
| 300 | static_cast<FwAssertArgType>(vc.spanningPacket.bytesReceived), | ||
| 301 | static_cast<FwAssertArgType>(headerCap)); | ||
| 302 | ✗ | FW_ASSERT(toHeader <= headerCap, static_cast<FwAssertArgType>(toHeader), | |
| 303 | static_cast<FwAssertArgType>(headerCap)); | ||
| 304 | ✗ | FW_ASSERT(vc.spanningPacket.bytesReceived + toHeader <= headerCap, | |
| 305 | static_cast<FwAssertArgType>(vc.spanningPacket.bytesReceived), | ||
| 306 | static_cast<FwAssertArgType>(toHeader), static_cast<FwAssertArgType>(headerCap)); | ||
| 307 | ✗ | (void)::memcpy(vc.spanningPacket.headerBuf + vc.spanningPacket.bytesReceived, data, toHeader); | |
| 308 | ✗ | vc.spanningPacket.bytesReceived += toHeader; | |
| 309 | |||
| 310 | // We'll work w/ everything past the copied header if we get a clean parse | ||
| 311 | ✗ | data += toHeader; | |
| 312 | ✗ | size -= toHeader; | |
| 313 | ✗ | 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 | ✗ | const FwSizeType packetSize = sizePacket(vc, vc.spanningPacket.headerBuf, vc.spanningPacket.bytesReceived); | |
| 319 | ✗ | if (packetSize == 0) { | |
| 320 | ✗ | 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 | ✗ | if (packetSize < vc.spanningPacket.bytesReceived) { | |
| 326 | ✗ | const FwSizeType surplus = vc.spanningPacket.bytesReceived - packetSize; | |
| 327 | // Prior calls left bytesReceived <= packetSize, so the surplus came from this call | ||
| 328 | ✗ | FW_ASSERT(surplus <= toHeader, static_cast<FwAssertArgType>(surplus), | |
| 329 | static_cast<FwAssertArgType>(toHeader)); | ||
| 330 | ✗ | vc.spanningPacket.bytesReceived = packetSize; | |
| 331 | ✗ | data -= surplus; | |
| 332 | ✗ | size += surplus; | |
| 333 | ✗ | 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 | ✗ | vc.spanningPacket.buffer = this->allocate_out(0, packetSize); | |
| 339 | ✗ | if ((not vc.spanningPacket.buffer.isValid()) || (vc.spanningPacket.buffer.getSize() < packetSize)) { | |
| 340 | ✗ | 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 | ✗ | const FwSizeType remainingBody = packetSize - vc.spanningPacket.bytesReceived; | |
| 344 | ✗ | this->abandonSpanningPacket(vc); | |
| 345 | |||
| 346 | // Seek past the failed packet (header bytes already consumed + remaining body) | ||
| 347 | ✗ | const FwSizeType remainingLength = seekForward + remainingBody; | |
| 348 | ✗ | if (remainingBody > size) { | |
| 349 | ✗ | return 0; | |
| 350 | } else { | ||
| 351 | ✗ | return remainingLength; | |
| 352 | } | ||
| 353 | } | ||
| 354 | |||
| 355 | // Load the header into the dynamic buffer | ||
| 356 | ✗ | 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 | ✗ | 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 | ✗ | (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 | ✗ | const FwSizeType spaceLeft = vc.spanningPacket.buffer.getSize() - vc.spanningPacket.bytesReceived; | |
| 371 | // Copy what we got | ||
| 372 | ✗ | const FwSizeType toBody = FW_MIN(size, spaceLeft); | |
| 373 | ✗ | if (toBody > 0) { | |
| 374 | ✗ | 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 | ✗ | (void)::memcpy(vc.spanningPacket.buffer.getData() + vc.spanningPacket.bytesReceived, data, toBody); | |
| 378 | ✗ | vc.spanningPacket.bytesReceived += toBody; | |
| 379 | ✗ | seekForward += toBody; | |
| 380 | } | ||
| 381 | |||
| 382 | // Check if the spanning packet is now complete | ||
| 383 | ✗ | if (vc.spanningPacket.buffer.getSize() > 0 && | |
| 384 | ✗ | vc.spanningPacket.bytesReceived >= vc.spanningPacket.buffer.getSize()) { | |
| 385 | ✗ | this->dataOut_out(0, vc.spanningPacket.buffer, vc.spanningPacket.context); | |
| 386 | ✗ | this->tlmWrite_PacketsExtracted(++vc.packetsExtracted); | |
| 387 | |||
| 388 | // Ownership of the buffer has transferred downstream; clear local handle before consolidating state reset. | ||
| 389 | ✗ | vc.spanningPacket.buffer = Fw::Buffer(); | |
| 390 | // Buffer won't be returned now since we cleared the handle | ||
| 391 | ✗ | this->abandonSpanningPacket(vc); | |
| 392 | } | ||
| 393 | |||
| 394 | ✗ | return seekForward; | |
| 395 | } | ||
| 396 | |||
| 397 | ✗ | 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 | ✗ | M_PDUHeader mpduHeader; | |
| 400 | ✗ | auto deserializer = data.getDeserializer(); | |
| 401 | ✗ | Fw::SerializeStatus status = deserializer.moveDeserToOffset(AOSHeader::SERIALIZED_SIZE); | |
| 402 | ✗ | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 403 | ✗ | status = deserializer.deserializeTo(mpduHeader); | |
| 404 | ✗ | FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status); | |
| 405 | |||
| 406 | ✗ | U16 firstHeaderPointer = mpduHeader.get_firstHeaderPointer(); | |
| 407 | |||
| 408 | // Calculate data zone boundaries | ||
| 409 | ✗ | const FwSizeType dataZoneStart = AOSHeader::SERIALIZED_SIZE + M_PDUHeader::SERIALIZED_SIZE; | |
| 410 | ✗ | const FwSizeType dataZoneEnd = m_fixedFrameSize - (m_fecfEnabled ? AOSTrailer::SERIALIZED_SIZE : 0); | |
| 411 | ✗ | const FwSizeType dataZoneSize = dataZoneEnd - dataZoneStart; | |
| 412 | ✗ | U8* dataZone = data.getData() + dataZoneStart; | |
| 413 | |||
| 414 | // Handle special First Header Pointer values (Section 4.1.4.2.2.4) | ||
| 415 | ✗ | if (firstHeaderPointer == M_PDUSubfields::FHP_IDLE_DATA_ONLY) { | |
| 416 | // Frame contains only idle data | ||
| 417 | ✗ | this->log_ACTIVITY_LO_IdleFrame(vc.virtualChannelId); | |
| 418 | ✗ | return; | |
| 419 | } | ||
| 420 | // Handle continuation data (data before First Header Pointer) | ||
| 421 | ✗ | else if (firstHeaderPointer == M_PDUSubfields::FHP_NO_PACKET_START) { | |
| 422 | // Entire data zone is continuation of previous packet | ||
| 423 | ✗ | if (vc.spanningPacket.bytesReceived > 0) { | |
| 424 | ✗ | (void)this->appendToSpanningPacket(vc, dataZone, dataZoneSize); | |
| 425 | } | ||
| 426 | // If no spanning packet active, this continuation data cannot be used | ||
| 427 | ✗ | return; | |
| 428 | } | ||
| 429 | |||
| 430 | // Guard against First Header Pointer pointing out of bounds (untrusted input) | ||
| 431 | ✗ | if (firstHeaderPointer >= dataZoneSize) { | |
| 432 | ✗ | this->log_WARNING_HI_InvalidFhp(vc.virtualChannelId, firstHeaderPointer, dataZoneSize); | |
| 433 | ✗ | this->notifyErrorIfConnected(Ccsds::FrameError::AOS_INVALID_LENGTH); | |
| 434 | // Abandon any existing data since this frame (and any continuing packets) are garbage now | ||
| 435 | ✗ | this->abandonSpanningPacket(vc); | |
| 436 | ✗ | return; | |
| 437 | } | ||
| 438 | |||
| 439 | // There is continuation data before the first packet header | ||
| 440 | ✗ | if (firstHeaderPointer > 0 && vc.spanningPacket.bytesReceived > 0) { | |
| 441 | ✗ | (void)this->appendToSpanningPacket(vc, dataZone, static_cast<FwSizeType>(firstHeaderPointer)); | |
| 442 | // We must be done w/ the prior packet since we have a FHP | ||
| 443 | ✗ | this->abandonSpanningPacket(vc); | |
| 444 | } | ||
| 445 | |||
| 446 | // Move to first packet header | ||
| 447 | ✗ | FwSizeType currentOffset = firstHeaderPointer; | |
| 448 | |||
| 449 | // Max Bound is a sequence of 1 byte EPP Idle Packets | ||
| 450 | ✗ | 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 | ✗ | for (FwIndexType iter = 0; iter < maxIters && currentOffset < dataZoneSize; iter++) { | |
| 455 | // Clear out any prior packet data | ||
| 456 | ✗ | this->abandonSpanningPacket(vc); | |
| 457 | |||
| 458 | ✗ | U8* packetStart = dataZone + currentOffset; | |
| 459 | ✗ | FwSizeType remainingBytes = dataZoneSize - currentOffset; | |
| 460 | |||
| 461 | ✗ | FwSizeType packetSize = this->appendToSpanningPacket(vc, packetStart, remainingBytes); | |
| 462 | |||
| 463 | ✗ | if (packetSize == 0) { | |
| 464 | // Break out of loop since we ran out of data | ||
| 465 | ✗ | return; | |
| 466 | } | ||
| 467 | |||
| 468 | ✗ | currentOffset += packetSize; | |
| 469 | } | ||
| 470 | ✗ | } | |
| 471 | |||
| 472 | ✗ | FwSizeType AosDeframer::sizePacket(AosDeframerVc& vc, U8* packetStart, FwSizeType remainingBytes) { | |
| 473 | ✗ | FW_ASSERT(packetStart != nullptr); | |
| 474 | ✗ | FW_ASSERT(remainingBytes > 0, static_cast<FwAssertArgType>(remainingBytes)); | |
| 475 | |||
| 476 | // Determine packet type from PVN (upper 3 bits of first byte) | ||
| 477 | ✗ | U8 pvn = getPacketVersion(packetStart[0]); | |
| 478 | // Default to invalid, override if valid (non-idle) packet | ||
| 479 | ✗ | vc.spanningPacket.context.set_pvn(ComCfg::Pvn::INVALID_UNINITIALIZED); | |
| 480 | |||
| 481 | // Check if this pvn is disabled | ||
| 482 | ✗ | if (~vc.pvnMask & (1 << pvn)) { | |
| 483 | ✗ | this->log_WARNING_HI_DisabledPvn(vc.virtualChannelId, pvn); | |
| 484 | ✗ | return 0; | |
| 485 | } | ||
| 486 | |||
| 487 | ✗ | ComCfg::Pvn pvnEnum = static_cast<ComCfg::Pvn::T>(pvn); | |
| 488 | ✗ | vc.spanningPacket.context.set_pvn(pvnEnum); | |
| 489 | |||
| 490 | // Size the Packet (so we can alloc a buffer) | ||
| 491 | ✗ | switch (pvnEnum) { | |
| 492 | ✗ | case ComCfg::Pvn::SPACE_PACKET_PROTOCOL: | |
| 493 | ✗ | return sizeSppPacket(packetStart, remainingBytes); | |
| 494 | break; | ||
| 495 | ✗ | case ComCfg::Pvn::ENCAPSULATION_PACKET_PROTOCOL: | |
| 496 | ✗ | 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 | ✗ | } | |
| 504 | |||
| 505 | ✗ | FwSizeType AosDeframer::sizeSppPacket(U8* payloadStart, FwSizeType payloadSize) { | |
| 506 | ✗ | FW_ASSERT(payloadStart != nullptr); | |
| 507 | ✗ | SpacePacketHeader header; | |
| 508 | |||
| 509 | ✗ | Fw::Buffer data(payloadStart, payloadSize); | |
| 510 | ✗ | Fw::SerializeStatus status = data.getDeserializer().deserializeTo(header); | |
| 511 | |||
| 512 | ✗ | if (status != Fw::FW_SERIALIZE_OK) { | |
| 513 | ✗ | 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 | ✗ | 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 | ✗ | 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 | ✗ | U16 apid = static_cast<U16>(header.get_packetIdentification() & SpacePacketSubfields::ApidMask); | |
| 531 | |||
| 532 | // Idle means this is the last packet in the frame | ||
| 533 | ✗ | if (apid == static_cast<U16>(ComCfg::Apid::SPP_IDLE_PACKET)) { | |
| 534 | ✗ | return 0; | |
| 535 | } | ||
| 536 | |||
| 537 | ✗ | return totalPacketSize; | |
| 538 | ✗ | } | |
| 539 | |||
| 540 | ✗ | 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 | ✗ | FW_ASSERT(payloadStart != nullptr); | |
| 544 | ✗ | FW_ASSERT(payloadSize > 0, static_cast<FwAssertArgType>(payloadSize)); | |
| 545 | |||
| 546 | // Parse first byte | ||
| 547 | ✗ | U8 firstByte = payloadStart[0]; | |
| 548 | ✗ | U8 protocolId = static_cast<U8>((firstByte & EPPSubfields::protocolIdMask) >> EPPSubfields::protocolIdOffset); | |
| 549 | |||
| 550 | ✗ | FwSizeType totalPacketSize = 0; | |
| 551 | |||
| 552 | // Idle means this is the last packet in the frame | ||
| 553 | ✗ | if (protocolId == static_cast<U8>(EppProtocolId::Idle)) { | |
| 554 | ✗ | return 0; | |
| 555 | } | ||
| 556 | |||
| 557 | // Encapsulation Idle Packet per CCSDS 133.1-B-3 Section 4.1.3.2 | ||
| 558 | ✗ | U8 lengthOfLength = firstByte & EPPSubfields::lengthOfLengthMask; | |
| 559 | |||
| 560 | ✗ | 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 | ✗ | if (lengthOfLength >= EppLengthOfLength::Two) { | |
| 564 | ✗ | 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 | ✗ | if (lengthOfLength == EppLengthOfLength::Four) { | |
| 569 | ✗ | lengthOffset = static_cast<U8>(lengthOffset + 2U); | |
| 570 | // '0d3' on the wire, but means 4 | ||
| 571 | ✗ | lengthOfLength = 4; | |
| 572 | } | ||
| 573 | |||
| 574 | // Bytes to get to length + length of length | ||
| 575 | ✗ | const U8 headerLength = static_cast<U8>(lengthOffset + lengthOfLength); | |
| 576 | |||
| 577 | // Validate and read length field | ||
| 578 | ✗ | if (payloadSize < headerLength) { | |
| 579 | ✗ | return 0; // Incomplete | |
| 580 | } | ||
| 581 | |||
| 582 | // Read length field (big-endian) | ||
| 583 | ✗ | U32 packetDataLength = 0; | |
| 584 | ✗ | for (U8 i = 0; i < lengthOfLength; i++) { | |
| 585 | ✗ | 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 | ✗ | 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 | ✗ | totalPacketSize = static_cast<FwSizeType>(headerLength) + static_cast<FwSizeType>(packetDataLength); | |
| 601 | |||
| 602 | ✗ | return totalPacketSize; | |
| 603 | } | ||
| 604 | |||
| 605 | ✗ | 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 | ✗ | return static_cast<U8>(firstByte >> EPPSubfields::packetVersionOffset); | |
| 609 | } | ||
| 610 | |||
| 611 | } // namespace Ccsds | ||
| 612 | } // namespace Svc | ||
| 613 |