GCC Code Coverage Report

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		✗	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		✗	configure(ComCfg::AosMaxFrameFixedSize, true);
22		✗	}
23
24		✗	AosFramer ::~AosFramer() {}
25
26		✗	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		✗	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		✗	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		✗	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		✗	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		✗	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		✗	this->m_fecf = frameErrorControlField;
52		✗	this->m_spacecraftId = spacecraftId;
53
54			// For each vc, init the buffer objects
55		✗	for (U8 ind = 0; ind < sizeof(this->m_vcs) / sizeof(AosVc); ind++) {
56		✗	AosVc& currentVc = this->m_vcs[ind];
57
58			// Write the index for mapping a vc struct onto an output port
59		✗	currentVc.vc_struct_index = ind;
60		✗	currentVc.virtualChannelId = vcId;
61		✗	currentVc.frame.buffer = {currentVc.frame.backer, fixedFrameSize};
62			// Set the bitmask of PVNs to use for idle packets
63		✗	currentVc.idle_packet_types = idlePvns;
64			}
65		✗	}
66
67			// ----------------------------------------------------------------------
68			// Handler implementations for typed input ports
69			// ----------------------------------------------------------------------
70
71		✗	void AosFramer ::dataIn_handler(FwIndexType portNum, Fw::Buffer& data, const ComCfg::FrameContext& context) {
72			// Get the VC Struct for this buffer
73		✗	const U8 vcId = context.get_vcId();
74		✗	AosVc& currentVc = this->get_vc_struct(context);
75		✗	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		✗	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		✗	if (!currentVc.past_first_fresh_packet) {
85			// setup our headers if we have a packet + context ready to go
86		✗	setup_header(context);
87		✗	setup_m_pdu_header(context);
88
89			// We now will only work with fresh packets for the rest of this frame
90		✗	currentVc.past_first_fresh_packet = true;
91			}
92
93			// Pack this packet
94		✗	pack_pad_send(data, context);
95		✗	}
96
97		✗	void AosFramer::compute_and_inject_fecf(AosVc& currentVc) {
98			// -------------------------------------------------
99			// Trailer (CRC)
100			// -------------------------------------------------
101		✗	AOSTrailer trailer;
102
103		✗	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		✗	U16 crc = Ccsds::Utils::CRC16::compute(currentVc.frame.buffer.getData(), static_cast<U32>(fecfStart));
106			// Set the Frame Error Control Field (FECF)
107		✗	trailer.set_fecf(crc);
108			// Move the serializer pointer to the end of the location where the trailer will be
109			// serialized
110		✗	auto frameSerializer = currentVc.frame.buffer.getSerializer();
111		✗	Fw::SerializeStatus status = frameSerializer.moveSerToOffset(fecfStart);
112		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
113
114		✗	status = frameSerializer.serializeFrom(trailer);
115		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
116		✗	}
117
118		✗	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		✗	if (this->isConnected_comStatusOut_OutputPort(portNum)) {
122			// Forward the comStatus upstream
123		✗	this->comStatusOut_out(portNum, condition);
124			}
125		✗	}
126
127		✗	void AosFramer ::dataReturnIn_handler(FwIndexType portNum,
128			Fw::Buffer& frameBuffer,
129			const ComCfg::FrameContext& context) {
130			// Get the VC Struct for this buffer
131		✗	AosVc& currentVc = this->get_vc_struct(context);
132
133			// Assert that the returned buffer is the member, and set ownership state
134		✗	FW_ASSERT(buffer_belongs(frameBuffer, currentVc.frame.backer, sizeof(currentVc.frame.backer)));
135		✗	currentVc.frame.state = BufferOwnershipState::OWNED;
136
137			// If we have an outstanding packet from the prior frame, pack it
138		✗	if (currentVc.outstanding.packet.isValid()) {
139		✗	this->pack_pad_send(currentVc.outstanding.packet, currentVc.outstanding.context, currentVc.outstanding.offset);
140			}
141		✗	}
142
143		✗	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		✗	const U8 ind = 0;
147		✗	AosVc& currentVc = this->m_vcs[ind];
148
149			// Ensure configure was called
150		✗	FW_ASSERT(currentVc.vc_struct_index == ind);
151		✗	FW_ASSERT(currentVc.virtualChannelId == context.get_vcId());
152		✗	return currentVc;
153			}
154
155		✗	void AosFramer ::setup_header(const ComCfg::FrameContext& context) {
156			// Get the VC Struct for this vc
157		✗	AosVc& currentVc = this->get_vc_struct(context);
158
159			// -----------------------------------------------
160			// Header
161			// -----------------------------------------------
162		✗	AOSHeader header;
163
164			// GVCID (Global Virtual Channel ID) (Standard 4.1.2.2 and 4.1.2.3)
165		✗	U16 globalVcId = static_cast<U16>((context.get_vcId() << AOSHeaderSubfields::virtualChannelIdOffset) |
166		✗	((m_spacecraftId & 0x00FF) << AOSHeaderSubfields::spacecraftIdLsbOffset) |
167		✗	((Tfvn::AOS & 0x3) << AOSHeaderSubfields::frameVersionOffset));
168
169			// Virtual Channel Frame Count (4.1.2.4)
170		✗	U32 frameCountAndSignaling = static_cast<U32>((currentVc.virtualFrameCount & 0x00FFFFFFU)
171			<< static_cast<FwSizeType>(AOSHeaderSubfields::vcFrameCountOffset));
172
173			// Virtual Channel Frame Count Cycle Use Flag (4.1.2.5.3)
174		✗	frameCountAndSignaling |= static_cast<U32>(1 << AOSHeaderSubfields::cycleCountFlagOffset);
175
176			// Spacecraft ID MSB (4.1.2.5.4)
177		✗	frameCountAndSignaling |=
178		✗	static_cast<U32>((m_spacecraftId & 0x0300) >> (8 - AOSHeaderSubfields::spacecraftIdMsbOffset));
179
180			// Virtual Channel Frame Cycle Count (4.1.2.5.5)
181		✗	frameCountAndSignaling |= static_cast<U32>((currentVc.virtualFrameCount & 0x0F000000) >> 24);
182
183		✗	header.set_globalVcId(globalVcId);
184		✗	header.set_frameCountAndSignaling(frameCountAndSignaling);
185
186			// Perform the modulo at serialization time makes vc cycle count easier to make optional
187		✗	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		✗	auto frameSerializer = currentVc.frame.buffer.getSerializer();
197
198		✗	status = frameSerializer.moveSerToOffset(0);
199		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
200
201		✗	status = frameSerializer.serializeFrom(header);
202		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
203		✗	}
204
205		✗	void AosFramer ::setup_m_pdu_header(const ComCfg::FrameContext& context, bool no_fresh) {
206			// Get the VC Struct for this vc
207		✗	AosVc& currentVc = this->get_vc_struct(context);
208
209		✗	M_PDUHeader muxedPdu;
210		✗	if (no_fresh) {
211			// All Ones since no packets start in this frame (no fresh packets)
212		✗	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		✗	muxedPdu.set_firstHeaderPointer(currentVc.current_payload_offset);
217			}
218
219		✗	auto frameSerializer = currentVc.frame.buffer.getSerializer();
220		✗	Fw::SerializeStatus status = frameSerializer.moveSerToOffset(AOSHeader::SERIALIZED_SIZE);
221		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
222
223		✗	status = frameSerializer.serializeFrom(muxedPdu);
224		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
225		✗	}
226
227		✗	FwSizeType AosFramer::get_min_size() {
228		✗	return AOSHeader::SERIALIZED_SIZE + M_PDUHeader::SERIALIZED_SIZE + (this->m_fecf ? AOSTrailer::SERIALIZED_SIZE : 0);
229			}
230
231		✗	bool AosFramer::buffer_belongs(const Fw::Buffer& buffer, U8 const* start, FwSizeType size) {
232		✗	return (buffer.getData() >= start && buffer.getData() < start + size);
233			}
234
235		✗	void AosFramer::check_and_send_vc(AosFramer::AosVc& currentVc) {
236		✗	const FwSizeType maxPayload = currentVc.frame.buffer.getSize() - get_min_size();
237
238			// Check if we've filled up the M_PDU payload
239		✗	if (currentVc.current_payload_offset == maxPayload) {
240			// Write the headers if we haven't already (continuing packet only)
241		✗	if (!currentVc.past_first_fresh_packet) {
242			// Setup the AOS header right before we send with the context cached in outstanding
243		✗	setup_header(currentVc.outstanding.context);
244
245			// Setup the M_PDU header w/ all zero (only continuing packets)
246		✗	setup_m_pdu_header(currentVc.outstanding.context, true);
247			}
248
249			// Compute Trailer
250		✗	if (this->m_fecf) {
251		✗	compute_and_inject_fecf(currentVc);
252			}
253
254			// Ensure we aren't double sending
255		✗	FW_ASSERT(currentVc.frame.state == BufferOwnershipState::OWNED);
256		✗	currentVc.frame.state = BufferOwnershipState::NOT_OWNED;
257
258			// Clean up our per frame vc values
259		✗	currentVc.current_payload_offset = 0;
260		✗	currentVc.past_first_fresh_packet = false;
261
262			// Write the completed frame
263		✗	this->dataOut_out(0, currentVc.frame.buffer, currentVc.outstanding.context);
264		✗	} else if (this->isConnected_comStatusOut_OutputPort(currentVc.vc_struct_index)) {
265			// We got more room, so ask for more bytes
266		✗	Fw::Success condition = Fw::Success::SUCCESS;
267		✗	this->comStatusOut_out(currentVc.vc_struct_index, condition);
268		✗	}
269		✗	}
270
271		✗	void AosFramer ::pack_pad_send(Fw::Buffer& data, const ComCfg::FrameContext& context, FwSizeType dataOffset) {
272			// Pack this packet into the M_PDU
273		✗	pack_packet(data, context, dataOffset);
274
275			// Get the VC Struct for this buffer
276		✗	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		✗	if (context.get_sendNow()) {
281			// Compute some sizes to check if we've got space left to pad
282		✗	const FwSizeType min_size = get_min_size();
283		✗	FW_ASSERT(currentVc.frame.buffer.getSize() >= min_size,
284			static_cast<FwAssertArgType>(currentVc.frame.buffer.getSize()),
285			static_cast<FwAssertArgType>(min_size));
286		✗	const FwSizeType maxPayload = currentVc.frame.buffer.getSize() - min_size;
287
288		✗	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		✗	fill_with_idle_packet(currentVc, context);
291			}
292			}
293
294			// Send the frame if we've filled it
295		✗	check_and_send_vc(currentVc);
296		✗	}
297
298		✗	void AosFramer ::pack_packet(Fw::Buffer& data, const ComCfg::FrameContext& context, FwSizeType dataOffset) {
299			// Ensure the packet is starting within the frame
300		✗	const FwSizeType min_size = get_min_size();
301
302			// Get the VC Struct for this buffer
303		✗	AosVc& currentVc = this->get_vc_struct(context);
304
305		✗	const FwSizeType maxPayload = currentVc.frame.buffer.getSize() - min_size;
306		✗	const FwSizeType bytesAvailable = maxPayload - currentVc.current_payload_offset;
307		✗	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		✗	auto frameSerializer = currentVc.frame.buffer.getSerializer();
316		✗	status = frameSerializer.moveSerToOffset(START_OF_PAYLOAD + currentVc.current_payload_offset);
317		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
318
319		✗	const U8* dataStart = data.getData() + dataOffset;
320			// min of (remaining bytes in buffer and available bytes in frame)
321		✗	FwSizeType dataSize = data.getSize() - dataOffset;
322
323			// Determine if we can write the whole packet or not
324		✗	if (dataSize <= bytesAvailable) {
325			// Whole packet fits, no need to store to outstanding
326		✗	currentVc.outstanding.offset = 0;
327			} else {
328			// We'll only write a subset (clamp our write to the available size)
329		✗	dataSize = bytesAvailable;
330
331			// We'll pick up serialization from here later
332		✗	currentVc.outstanding.offset = dataOffset + dataSize;
333		✗	currentVc.outstanding.packet = data;
334			}
335
336		✗	status = frameSerializer.serializeFrom(dataStart, dataSize, Fw::Serialization::OMIT_LENGTH);
337		✗	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		✗	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		✗	currentVc.outstanding.context = context;
344
345			// Return the buffer if no bytes are outstanding
346		✗	if (currentVc.outstanding.offset == 0) {
347			// Return the buffer if this isn't the SPP Idle buff
348		✗	const bool isSppIdleBuffer = buffer_belongs(data, currentVc.spp_idle.backer, sizeof(currentVc.spp_idle.backer));
349		✗	if (!isSppIdleBuffer) {
350		✗	this->dataReturnOut_out(0, data,
351			context); // return ownership of the original data buffer
352			}
353
354		✗	currentVc.outstanding.packet = {};
355		✗	currentVc.outstanding.offset = 0;
356			}
357		✗	}
358
359		✗	void AosFramer ::serialize_idle_spp_packet(Fw::SerialBufferBase& serializer, U16 length) {
360			// APID to use for this Idle Packet
361		✗	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		✗	const U16 lengthToken = static_cast<U16>(length - SpacePacketHeader::SERIALIZED_SIZE - 1);
365
366		✗	SpacePacketHeader header;
367		✗	header.set_packetIdentification(idleApid);
368		✗	header.set_packetSequenceControl(
369			0x3 << SpacePacketSubfields::SeqFlagsOffset); // Sequence Flags = 0b11 (unsegmented) & unused Seq count
370		✗	header.set_packetDataLength(lengthToken);
371			// Serialize header into frame
372		✗	Fw::SerializeStatus status = serializer.serializeFrom(header);
373		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
374
375			// Fill with idle pattern
376		✗	for (U16 i = static_cast<U16>(SpacePacketHeader::SERIALIZED_SIZE); i < length; i++) {
377		✗	status = serializer.serializeFrom(SPP_IDLE_DATA_PATTERN); // Idle data
378		✗	FW_ASSERT(status == Fw::FW_SERIALIZE_OK, status);
379			}
380		✗	}
381
382		✗	void AosFramer ::fill_with_idle_packet(AosVc& vc, const ComCfg::FrameContext& context) {
383			// Bytes that aren't actual PDUs
384		✗	const U16 overhead = START_OF_PAYLOAD + (m_fecf ? AOSTrailer::SERIALIZED_SIZE : 0);
385			// Bytes for all the PDUs in this VC Frame
386		✗	FW_ASSERT(vc.frame.buffer.getSize() > 0, static_cast<FwAssertArgType>(vc.frame.buffer.getSize()));
387		✗	FW_ASSERT(vc.frame.buffer.getSize() < std::numeric_limits<U16>::max(),
388			static_cast<FwAssertArgType>(vc.frame.buffer.getSize()));
389		✗	const U16 pduSize = static_cast<U16>(vc.frame.buffer.getSize() - overhead);
390
391			// How many bytes are left over
392		✗	const U16 idlePacketSize = static_cast<U16>(pduSize - vc.current_payload_offset);
393
394			// Grab a serializer @ the current offset
395		✗	auto frameSerializer = vc.frame.buffer.getSerializer();
396		✗	Fw::SerializeStatus status = frameSerializer.moveSerToOffset(START_OF_PAYLOAD + vc.current_payload_offset);
397		✗	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		✗	if (!vc.past_first_fresh_packet) {
402			// setup our headers if we have a packet + context ready to go
403		✗	setup_header(context);
404		✗	setup_m_pdu_header(context);
405
406			// This idle packet is the fresh packet
407		✗	vc.past_first_fresh_packet = true;
408			}
409
410			// EPP-only idle fill is not yet supported; configure() requires the SPP bit
411		✗	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		✗	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		✗	FW_ASSERT(!vc.outstanding.packet.isValid());
421
422			// Setup the PDU to point at our idle buffer
423		✗	vc.outstanding.packet = {vc.spp_idle.backer, MIN_SPP_LENGTH};
424
425			// Write the SPP Idle to the outstanding packet
426		✗	auto pduSerializer = vc.outstanding.packet.getSerializer();
427		✗	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		✗	ComCfg::FrameContext filtered_context = context;
432		✗	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		✗	pack_packet(vc.outstanding.packet, filtered_context);
437		✗	} else {
438			// Serialize an idle packet right into the frame
439		✗	serialize_idle_spp_packet(frameSerializer, idlePacketSize);
440
441			// Increment the offset since we serialized directly into the frame
442		✗	vc.current_payload_offset = static_cast<U16>(vc.current_payload_offset + idlePacketSize);
443			}
444		✗	}
445
446			} // namespace Ccsds
447			} // namespace Svc
448