GCC Code Coverage Report

Line	Branch	Exec	Source
1			// ======================================================================
2			// \title TmFramer.cpp
3			// \brief cpp file for CCSDS TM Transfer Frame downlink framer implementation class
4			// ======================================================================
5
6			#include "fprime-samd/Svc/TmFramer/TmFramer.hpp"
7			#include "Drv/ByteStreamDriverModel/ByteStreamStatusEnumAc.hpp"
8			#include "Fw/Com/ComBuffer.hpp"
9			#include "Fw/Types/Assert.hpp"
10			#include "Svc/Ccsds/Utils/CRC16.hpp"
11			#include "samd-config/FramerConfig.hpp"
12
13			namespace Samd21 {
14
15			// Out-of-class definition required pre-C++17 for any ODR-use of a static constexpr member
16			constexpr U8 TmFramer::IDLE_DATA_PATTERN;
17
18			static_assert(ComCfg::TmFrameFixedSize > Svc::Ccsds::TMHeader::SERIALIZED_SIZE + Svc::Ccsds::TMTrailer::SERIALIZED_SIZE,
19			"ComCfg::TmFrameFixedSize too small to hold TMHeader + TMTrailer");
20
21			static constexpr FwSizeType TM_DATA_FIELD_CAPACITY =
22			ComCfg::TmFrameFixedSize - (Svc::Ccsds::TMHeader::SERIALIZED_SIZE + Svc::Ccsds::TMTrailer::SERIALIZED_SIZE);
23
24			static constexpr FwSizeType IDLE_SPACE_PACKET_MIN_SIZE = Svc::Ccsds::SpacePacketHeader::SERIALIZED_SIZE + 1;
25			static constexpr FwSizeType MAX_SINGLE_SPACE_PACKET_SIZE =
26			Svc::Ccsds::SpacePacketHeader::SERIALIZED_SIZE + FW_COM_BUFFER_MAX_SIZE;
27			static_assert(TM_DATA_FIELD_CAPACITY < MAX_SINGLE_SPACE_PACKET_SIZE ||
28			(TM_DATA_FIELD_CAPACITY - MAX_SINGLE_SPACE_PACKET_SIZE) == 0 ||
29			(TM_DATA_FIELD_CAPACITY - MAX_SINGLE_SPACE_PACKET_SIZE) >= IDLE_SPACE_PACKET_MIN_SIZE,
30			"FW_COM_BUFFER_MAX_SIZE vs ComCfg::TmFrameFixedSize can leave an unfillable "
31			"single-packet remainder on an empty TM frame buffer");
32
33			// ----------------------------------------------------------------------
34			// Component construction and destruction
35			// ----------------------------------------------------------------------
36
37		✗	TmFramer ::TmFramer(const char* const compName)
38			: TmFramerComponentBase(compName),
39		✗	m_driverConnected(false),
40		✗	m_activeBufferIdx(0),
41		✗	m_droppedPackets(0),
42		✗	m_apidOverflowCount(0),
43		✗	m_numApidsTracked(0),
44		✗	m_masterFrameCount(0),
45		✗	m_virtualFrameCount(0) {
46		✗	for (FwIndexType i = 0; i < 2; i++) {
47		✗	m_buffers[i].dataFieldSize = 0;
48		✗	m_buffers[i].state = IDLE;
49			}
50		✗	}
51
52		✗	TmFramer ::~TmFramer() {}
53
54			// ----------------------------------------------------------------------
55			// Handler implementations for typed input ports
56			// ----------------------------------------------------------------------
57
58			// Unlike stock Svc::Ccsds::TmFramer (one buffer per frame), this component accumulates
59			// multiple Space Packets per frame
60
61		✗	void TmFramer ::comPacketQueueIn_handler(FwIndexType portNum, Fw::ComBuffer& data, U32 context) {
62		✗	TxBuffer* activeBuf = &m_buffers[m_activeBufferIdx];
63
64			// Space Packet header (6B) + payload, no idle padding included yet.
65		✗	FwSizeType spaceNeeded = Svc::Ccsds::SpacePacketHeader::SERIALIZED_SIZE + data.getSize();
66
67			// If both buffers are in flight (backpressure), drop this packet
68		✗	if (activeBuf->state == TRANSMITTING) {
69		✗	this->m_droppedPackets++;
70		✗	return;
71			}
72
73			// Flush first if adding this Space Packet would overflow the data field, OR would fit but
74			// leave a remainder too small for closeFrame()'s idle pad (the dead zone described above).
75		✗	FwSizeType afterAppend = activeBuf->dataFieldSize + spaceNeeded;
76		✗	bool wouldOverflow = afterAppend > TM_DATA_FIELD_CAPACITY;
77		✗	bool wouldLeaveDeadZone = false;
78		✗	if (!wouldOverflow) {
79		✗	FwSizeType remainderAfterAppend = TM_DATA_FIELD_CAPACITY - afterAppend;
80		✗	wouldLeaveDeadZone = remainderAfterAppend > 0 && remainderAfterAppend < IDLE_SPACE_PACKET_MIN_SIZE;
81			}
82
83		✗	if (wouldOverflow || wouldLeaveDeadZone) {
84		✗	flushActiveBuffer();
85
86			// flushActiveBuffer() swapped the active buffer -- re-acquire it
87		✗	activeBuf = &m_buffers[m_activeBufferIdx];
88
89			// If both buffers are in flight (backpressure), drop this packet
90		✗	if (activeBuf->state == TRANSMITTING) {
91		✗	this->m_droppedPackets++;
92		✗	return;
93			}
94
95		✗	FW_ASSERT(activeBuf->state == IDLE);
96		✗	FW_ASSERT(activeBuf->dataFieldSize == 0);
97			}
98
99			// A single ComBuffer + Space Packet header must never exceed the entire data field
100			// capacity -- otherwise it could never fit in any frame regardless of flushing.
101		✗	FW_ASSERT(spaceNeeded <= TM_DATA_FIELD_CAPACITY, static_cast<FwAssertArgType>(spaceNeeded));
102
103		✗	if (activeBuf->state == IDLE) {
104		✗	activeBuf->state = ACTIVE;
105			}
106
107			// Data field starts after the (not-yet-written) TMHeader; append at that offset.
108		✗	U8* dataFieldStart = activeBuf->data + Svc::Ccsds::TMHeader::SERIALIZED_SIZE;
109		✗	FwSizeType written = appendSpacePacket(dataFieldStart + activeBuf->dataFieldSize,
110		✗	TM_DATA_FIELD_CAPACITY - activeBuf->dataFieldSize, data);
111		✗	activeBuf->dataFieldSize += written;
112		✗	FW_ASSERT(activeBuf->dataFieldSize <= TM_DATA_FIELD_CAPACITY,
113			static_cast<FwAssertArgType>(activeBuf->dataFieldSize));
114
115			// dataFieldSize must end up either exactly full or with room for a valid idle pad.
116		✗	FwSizeType finalRemainder = TM_DATA_FIELD_CAPACITY - activeBuf->dataFieldSize;
117		✗	FW_ASSERT(finalRemainder == 0 || finalRemainder >= IDLE_SPACE_PACKET_MIN_SIZE,
118			static_cast<FwAssertArgType>(finalRemainder));
119			}
120
121		✗	ComCfg::Apid TmFramer ::apidForComBuffer(const Fw::ComBuffer& data) const {
122			// Every ComBuffer from Fw::TlmPacket/Fw::LogPacket leads with a FwPacketDescriptorType
123			// (Fw::ComPacket::serializeBase()) that is bit-identical to the ComCfg::Apid value we
124			// want. Peek it via a temporary ExternalSerializeBuffer over the same memory so `data`'s
125			// own cursor is untouched -- setBuffLen() marks its bytes readable without copying.
126		✗	Fw::ExternalSerializeBuffer peek(const_cast<U8*>(data.getBuffAddr()), data.getSize());
127		✗	Fw::SerializeStatus status = peek.setBuffLen(data.getSize());
128		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
129		✗	FwPacketDescriptorType descriptor = 0;
130		✗	status = peek.deserializeTo(descriptor);
131		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
132		✗	return static_cast<ComCfg::Apid>(descriptor);
133		✗	}
134
135		✗	U16 TmFramer ::nextApidSequenceCount(ComCfg::Apid apid) {
136		✗	FwIndexType slotIdx = -1;
137		✗	for (FwIndexType i = 0; i < this->m_numApidsTracked; i++) {
138		✗	if (this->m_apidSequenceCounts[i].apid == apid) {
139		✗	slotIdx = i;
140		✗	break;
141			}
142			}
143
144		✗	if (slotIdx == -1) {
145			// First time seeing this APID -- claim the next free slot if one is available. If
146			// every downlink packet source is accounted for in Samd21::FramerConfig::MAX_TRACKED_APIDS,
147			// This path must stay non-fatal because it can be reached while handling a
148			// fatal/assert packet, and a crash here would prevent that packet from ever being sent.
149		✗	if (this->m_numApidsTracked >= Samd21::FramerConfig::MAX_TRACKED_APIDS) {
150		✗	this->m_apidOverflowCount++;
151		✗	return 0;
152			}
153		✗	slotIdx = this->m_numApidsTracked;
154		✗	this->m_apidSequenceCounts[slotIdx].apid = apid;
155		✗	this->m_apidSequenceCounts[slotIdx].count = 0;
156		✗	this->m_numApidsTracked++;
157			}
158
159		✗	U16 count = this->m_apidSequenceCounts[slotIdx].count;
160			// Sequence count is 14 bits, mod-16384 per APID (CCSDS 133.0-B-2 4.1.3.4).
161		✗	this->m_apidSequenceCounts[slotIdx].count =
162		✗	static_cast<U16>((count + 1) & Svc::Ccsds::SpacePacketSubfields::SeqCountMask);
163		✗	return count;
164			}
165
166		✗	FwSizeType TmFramer ::appendSpacePacket(U8* dest, FwSizeType destCapacity, Fw::ComBuffer& data) {
167		✗	FW_ASSERT(destCapacity >= Svc::Ccsds::SpacePacketHeader::SERIALIZED_SIZE + data.getSize());
168
169		✗	Fw::Buffer bufWrapper(dest, destCapacity);
170		✗	Fw::ExternalSerializeBuffer serializer(bufWrapper.getData(), bufWrapper.getSize());
171
172			// Space Packet primary header (CCSDS 133.0-B-2): Packet Type = 0 (TM), Secondary Header
173			// Flag = 0, Sequence Flags = 0b11 (unsegmented) for every packet this framer builds.
174		✗	ComCfg::Apid apid = apidForComBuffer(data);
175
176		✗	Svc::Ccsds::SpacePacketHeader header;
177		✗	U16 packetIdentification = static_cast<U16>(apid) & Svc::Ccsds::SpacePacketSubfields::ApidMask;
178		✗	header.set_packetIdentification(packetIdentification);
179
180		✗	U16 packetSequenceControl = static_cast<U16>(0x3 << Svc::Ccsds::SpacePacketSubfields::SeqFlagsOffset);
181		✗	packetSequenceControl |= (nextApidSequenceCount(apid) & Svc::Ccsds::SpacePacketSubfields::SeqCountMask);
182		✗	header.set_packetSequenceControl(packetSequenceControl);
183
184			// Packet Data Length = size of user data field minus one (CCSDS 133.0-B-2 4.1.3.5.3)
185		✗	FW_ASSERT(data.getSize() > 0, static_cast<FwAssertArgType>(data.getSize()));
186		✗	header.set_packetDataLength(static_cast<U16>(data.getSize() - 1));
187
188		✗	Fw::SerializeStatus status = serializer.serializeFrom(header);
189		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
190
191		✗	status = serializer.serializeFrom(data.getBuffAddr(), data.getSize(), Fw::Serialization::OMIT_LENGTH);
192		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status, data.getSize());
193
194		✗	return serializer.getSize();
195		✗	}
196
197		✗	void TmFramer ::drvConnected_handler(FwIndexType portNum) {
198		✗	this->m_driverConnected = true;
199		✗	}
200
201		✗	void TmFramer ::drvReturnIn_handler(FwIndexType portNum, Fw::Buffer& fwBuffer, const Drv::ByteStreamStatus& status) {
202		✗	for (FwIndexType i = 0; i < 2; i++) {
203		✗	if (m_buffers[i].state == TRANSMITTING && fwBuffer.getData() == m_buffers[i].data) {
204		✗	m_buffers[i].state = IDLE;
205		✗	m_buffers[i].dataFieldSize = 0;
206		✗	return;
207			}
208			}
209		✗	FW_ASSERT(false); // Buffer returned that we didn't send
210			}
211
212		✗	void TmFramer ::closeFrame(U8* frameData, FwSizeType dataFieldUsed) {
213		✗	U8* dataField = frameData + Svc::Ccsds::TMHeader::SERIALIZED_SIZE;
214
215			// Pad the remainder of the data field with a CCSDS idle Space Packet
216			// (CCSDS 132.0-B-3 4.2.2.5).
217		✗	FwSizeType remaining = TM_DATA_FIELD_CAPACITY - dataFieldUsed;
218		✗	if (remaining > 0) {
219		✗	FW_ASSERT(remaining >= Svc::Ccsds::SpacePacketHeader::SERIALIZED_SIZE + 1,
220			static_cast<FwAssertArgType>(remaining));
221
222		✗	Fw::Buffer idleWrapper(dataField + dataFieldUsed, remaining);
223		✗	Fw::ExternalSerializeBuffer idleSerializer(idleWrapper.getData(), idleWrapper.getSize());
224
225		✗	Svc::Ccsds::SpacePacketHeader idleHeader;
226		✗	idleHeader.set_packetIdentification(static_cast<U16>(ComCfg::Apid::SPP_IDLE_PACKET) &
227			Svc::Ccsds::SpacePacketSubfields::ApidMask);
228		✗	idleHeader.set_packetSequenceControl(static_cast<U16>(0x3 << Svc::Ccsds::SpacePacketSubfields::SeqFlagsOffset));
229		✗	FwSizeType idleDataLen = remaining - Svc::Ccsds::SpacePacketHeader::SERIALIZED_SIZE;
230		✗	idleHeader.set_packetDataLength(static_cast<U16>(idleDataLen - 1));
231
232		✗	Fw::SerializeStatus status = idleSerializer.serializeFrom(idleHeader);
233		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
234		✗	for (FwSizeType i = 0; i < idleDataLen; i++) {
235		✗	status = idleSerializer.serializeFrom(IDLE_DATA_PATTERN);
236		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
237			}
238		✗	}
239
240			// -----------------------------------------------
241			// Header (TM Transfer Frame Primary Header, CCSDS 132.0-B-3 4.1.2)
242			// -----------------------------------------------
243		✗	Svc::Ccsds::TMHeader header;
244		✗	U16 globalVcId = static_cast<U16>(
245			1 << Svc::Ccsds::TMSubfields::virtualChannelIdOffset); // VCID=1, this project's downlink virtual channel
246		✗	globalVcId |= static_cast<U16>(ComCfg::SpacecraftId << Svc::Ccsds::TMSubfields::spacecraftIdOffset);
247		✗	globalVcId |= 0x0; // Operational Control Field: Flag = 0 (no OCF present)
248
249		✗	U16 dataFieldStatus = static_cast<U16>(0x3 << Svc::Ccsds::TMSubfields::segLengthOffset); // Seg Length Id = 0b11
250			// First Header Pointer = 0: the first Space Packet in every frame starts at data-field
251			// offset 0 (this framer never spans a packet across frames).
252
253		✗	header.set_globalVcId(globalVcId);
254		✗	header.set_masterFrameCount(this->m_masterFrameCount);
255		✗	header.set_virtualFrameCount(this->m_virtualFrameCount);
256		✗	header.set_dataFieldStatus(dataFieldStatus);
257
258			// Single Master Channel / Virtual Channel in this deployment, so both counters
259			// track together.
260		✗	this->m_masterFrameCount++; // U8 wraps mod-256
261		✗	this->m_virtualFrameCount++; // U8 wraps mod-256
262
263		✗	Fw::Buffer headerWrapper(frameData, Svc::Ccsds::TMHeader::SERIALIZED_SIZE);
264		✗	Fw::ExternalSerializeBuffer headerSerializer(headerWrapper.getData(), headerWrapper.getSize());
265		✗	Fw::SerializeStatus status = headerSerializer.serializeFrom(header);
266		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
267
268			// -----------------------------------------------
269			// Trailer (FECF, CRC-16/CCITT-FALSE per CCSDS 132.0-B-3 4.1.6)
270			// -----------------------------------------------
271			U16 crc =
272		✗	Svc::Ccsds::Utils::CRC16::compute(frameData, ComCfg::TmFrameFixedSize - Svc::Ccsds::TMTrailer::SERIALIZED_SIZE);
273		✗	Svc::Ccsds::TMTrailer trailer;
274		✗	trailer.set_fecf(crc);
275
276		✗	Fw::Buffer trailerWrapper(frameData + ComCfg::TmFrameFixedSize - Svc::Ccsds::TMTrailer::SERIALIZED_SIZE,
277		✗	Svc::Ccsds::TMTrailer::SERIALIZED_SIZE);
278		✗	Fw::ExternalSerializeBuffer trailerSerializer(trailerWrapper.getData(), trailerWrapper.getSize());
279		✗	status = trailerSerializer.serializeFrom(trailer);
280		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
281		✗	}
282
283		✗	void TmFramer ::flushActiveBuffer() {
284		✗	FW_ASSERT(this->m_driverConnected);
285
286		✗	TxBuffer& activeBuf = m_buffers[m_activeBufferIdx];
287
288			// Nothing to flush if the buffer hasn't been touched since its last return.
289		✗	if (activeBuf.state == IDLE) {
290		✗	return;
291			}
292
293		✗	FW_ASSERT(activeBuf.state == ACTIVE);
294		✗	FW_ASSERT(activeBuf.dataFieldSize > 0);
295
296		✗	closeFrame(activeBuf.data, activeBuf.dataFieldSize);
297
298		✗	activeBuf.state = TRANSMITTING;
299		✗	m_activeBufferIdx = (m_activeBufferIdx + 1) % 2;
300
301		✗	Fw::Buffer txBuffer(activeBuf.data, ComCfg::TmFrameFixedSize);
302		✗	drvSendOut_out(0, txBuffer);
303		✗	}
304
305		✗	void TmFramer ::schedIn_handler(FwIndexType portNum, U32 context) {
306		✗	if (this->m_driverConnected) {
307		✗	FwIndexType nextBufferIdx = (m_activeBufferIdx + 1) % 2;
308		✗	TxBuffer& nextBuf = m_buffers[nextBufferIdx];
309
310		✗	this->tlmWrite_DroppedPackets(this->m_droppedPackets);
311		✗	this->tlmWrite_ApidOverflowCount(this->m_apidOverflowCount);
312
313		✗	if (nextBuf.state == TRANSMITTING) {
314			// Can't flush because both buffers would be in flight
315		✗	return;
316			}
317
318		✗	flushActiveBuffer();
319			}
320			}
321
322			} // namespace Samd21
323