pus parser implemented

tmtcservices/PusParser.cpp

@@ -13,31 +13,106 @@ ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame,
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 
-	// Size of a pus packet is the value in the packet length field plus 7.
-	uint16_t packetSize = (frame[4] << 8 | frame[5]) + 7;
-	if(packetSize > 0) {
-		indexSizePairFIFO.insert(indexSizePair(0, packetSize));
-		return HasReturnvaluesIF::RETURN_OK;
+	if(indexSizePairFIFO.full()) {
+		sif::error << "PusParser::parsePusPackets: FIFO is full!" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
 	}
-	else {
+
+
+	size_t lengthField = frame[4] << 8 | frame[5];
+
+	if(lengthField == 0) {
 		return NO_PACKET_FOUND;
 	}
 
-	if(frameSize > packetSize) {
-		return readMultiplePackets(frameSize);
+	size_t packetSize = lengthField + 7;
+	if(lengthField > 0) {
+		// 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;
 }
 
-//ReturnValue_t PusParser::readMultiplePackets(size_t frameSize) {
-//	size_t endOfBuffer = frameSize - 1;
-//	size_t endIndex = firstPacketSize;
-//	size_t startIndex = 0;
-//	while (endIndex < endOfBuffer) {
-//		ReturnValue_t result = readNextPacket(&startIndex, &endIndex);
-//		if(result != RETURN_OK) {
-//			return;
-//		}
-//	}
-//}
+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

@@ -23,9 +23,13 @@
  */
 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
@@ -44,10 +48,24 @@ public:
 	 * @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:
-	//! 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>;
+
 	//! 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.
@@ -55,8 +73,10 @@ private:
 
 	bool storeSplitPackets = false;
 
-	ReturnValue_t readMultiplePackets(size_t frameSize);
+	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_ */