returnvalues/FwClassIds.h

@@ -64,6 +64,7 @@ enum {
 	LOCAL_POOL_OWNER_IF, //LPIF 58
 	POOL_VARIABLE_IF, //PVA 59
 	HOUSEKEEPING_MANAGER, //HKM 60
+	PUS_PARSER, //PUSP 61
 	FW_CLASS_ID_COUNT //is actually count + 1 !
 
 };

tmtcservices/PusParser.cpp

@@ -0,0 +1,115 @@
+#include <framework/tmtcservices/PusParser.h>
+#include <framework/serviceinterface/ServiceInterfaceStream.h>
+
+PusParser::PusParser(uint16_t maxExpectedPusPackets,
+		bool storeSplitPackets): indexSizePairFIFO(maxExpectedPusPackets) {
+}
+
+ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame,
+		size_t frameSize) {
+	if(frame == nullptr) {
+		sif::error << "PusParser::parsePusPackets: Frame pointers in invalid!"
+				<< std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	if(indexSizePairFIFO.full()) {
+		sif::error << "PusParser::parsePusPackets: FIFO is full!" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	size_t lengthField = frame[4] << 8 | frame[5];
+
+	if(lengthField == 0) {
+		return NO_PACKET_FOUND;
+	}
+
+	size_t packetSize = lengthField + 7;
+	// Size of a pus packet is the value in the packet length field plus 7.
+	if(packetSize > frameSize)
+	{
+		if(storeSplitPackets)
+		{
+			indexSizePairFIFO.insert(indexSizePair(0, frameSize));
+		}
+		else
+		{
+			sif::debug << "TcSerialPollingTask::readNextPacket: Next packet "
+					"larger than remaining frame," << std::endl;
+			sif::debug << "Throwing away packet. Detected packet size: "
+					<< packetSize << std::endl;
+		}
+		return SPLIT_PACKET;
+	}
+	else
+	{
+		indexSizePairFIFO.insert(indexSizePair(0, packetSize));
+		if(packetSize == frameSize) {
+			return HasReturnvaluesIF::RETURN_OK;
+		}
+	}
+
+	// packet size is smaller than frame size, parse for more packets.
+	return readMultiplePackets(frame, frameSize, packetSize);
+}
+
+ReturnValue_t PusParser::readMultiplePackets(const uint8_t *frame,
+		size_t frameSize, size_t startIndex) {
+	while (startIndex < frameSize) {
+		ReturnValue_t result = readNextPacket(frame, frameSize, startIndex);
+		if(result != HasReturnvaluesIF::RETURN_OK) {
+			return result;
+		}
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+fsfw::FIFO<PusParser::indexSizePair>* PusParser::fifo(){
+	return &indexSizePairFIFO;
+}
+
+PusParser::indexSizePair PusParser::getNextFifoPair() {
+	indexSizePair nextIndexSizePair;
+	indexSizePairFIFO.retrieve(&nextIndexSizePair);
+	return nextIndexSizePair;
+}
+
+ReturnValue_t PusParser::readNextPacket(const uint8_t *frame,
+		size_t frameSize, size_t& currentIndex) {
+	// sif::debug << startIndex << std::endl;
+	uint16_t lengthField = frame[currentIndex + 4] << 8 |
+			frame[currentIndex + 5];
+	if(lengthField == 0) {
+		// It is assumed that no packet follows.
+		currentIndex = frameSize;
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	size_t nextPacketSize = lengthField + 7;
+	size_t remainingSize = frameSize - currentIndex;
+	if(nextPacketSize > remainingSize)
+	{
+		if(storeSplitPackets)
+		{
+			indexSizePairFIFO.insert(indexSizePair(currentIndex, remainingSize));
+		}
+		else
+		{
+			sif::debug << "TcSerialPollingTask::readNextPacket: Next packet "
+					"larger than remaining frame," << std::endl;
+			sif::debug << "Throwing away packet. Detected packet size: "
+					<< nextPacketSize << std::endl;
+		}
+		return SPLIT_PACKET;
+	}
+
+	ReturnValue_t result = indexSizePairFIFO.insert(indexSizePair(currentIndex,
+			nextPacketSize));
+	if (result != HasReturnvaluesIF::RETURN_OK) {
+		// FIFO full.
+		sif::debug << "PusParser: Issue inserting into start index size "
+		        "FIFO, it is full!" << std::endl;
+	}
+	currentIndex += nextPacketSize;
+
+	return result;
+}

tmtcservices/PusParser.h

@@ -0,0 +1,82 @@
+#ifndef FRAMEWORK_TMTCSERVICES_PUSPARSER_H_
+#define FRAMEWORK_TMTCSERVICES_PUSPARSER_H_
+
+#include <framework/container/FIFO.h>
+
+#include <utility>
+#include <cstdint>
+
+/**
+ * @brief	This small helper class scans a given buffer for PUS packets.
+ * 			Can be used if PUS packets are serialized in a tightly packed frame.
+ * @details
+ * The parser uses the payload length field of PUS packets to find
+ * the respective PUS packet sizes.
+ *
+ * The parser parses a buffer by taking a pointer and the maximum size to scan.
+ * If PUS packets are found, they are stored in a FIFO which stores pairs
+ * consisting of the index in the buffer and the respective packet sizes.
+ *
+ * If the parser detects split packets (which means that the size of the
+ * next packet is larger than the remaining size to scan), it can either
+ * store that split packet or throw away the packet.
+ */
+class PusParser {
+public:
+	//! The first entry is the index inside the buffer while the second index
+	//! is the size of the PUS packet starting at that index.
+	using indexSizePair = std::pair<size_t, size_t>;
+
+	static constexpr uint8_t INTERFACE_ID = CLASS_ID::PUS_PARSER;
+	static constexpr ReturnValue_t NO_PACKET_FOUND = MAKE_RETURN_CODE(0x00);
+	static constexpr ReturnValue_t SPLIT_PACKET = MAKE_RETURN_CODE(0x01);
+	/**
+	 * Parser constructor.
+	 * @param maxExpectedPusPackets
+	 * Maximum expected number of PUS packets. A good estimate is to divide
+	 * the frame size by the minimum size of a PUS packet (12 bytes)
+	 * @param storeSplitPackets
+	 * Specifies whether split packets are also stored inside the FIFO,
+	 * with the size being the remaining frame size.
+	 */
+	PusParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets);
+
+	/**
+	 * Parse a given frame for PUS packets
+	 * @param frame
+	 * @param frameSize
+	 * @return -@c NO_PACKET_FOUND if no packet was found.
+	 */
+	ReturnValue_t parsePusPackets(const uint8_t* frame, size_t frameSize);
+
+	/**
+	 * Accessor function to get a reference to the internal FIFO which
+	 * stores pairs of indexi and packet sizes. This FIFO is filled
+	 * by the parsePusPackets() function.
+	 * @return
+	 */
+	fsfw::FIFO<indexSizePair>* fifo();
+
+	/**
+	 * Retrieve the next index and packet size pair from the FIFO.
+	 * This also removed it from the FIFO. Please note that if the FIFO
+	 * is empty, an empty pair will be returned.
+	 * @return
+	 */
+	indexSizePair getNextFifoPair();
+private:
+
+	//! A FIFO is used to store information about multiple PUS packets
+	//! inside the receive buffer. The maximum number of entries is defined
+	//! by the first constructor argument.
+	fsfw::FIFO<indexSizePair> indexSizePairFIFO;
+
+	bool storeSplitPackets = false;
+
+	ReturnValue_t readMultiplePackets(const uint8_t *frame, size_t frameSize,
+			size_t startIndex);
+	ReturnValue_t readNextPacket(const uint8_t *frame,
+			size_t frameSize, size_t& startIndex);
+};
+
+#endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */