From f442a5889ec4bb213f35668a9320dd4cfedeee94 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Mon, 6 Jul 2020 16:34:11 +0200
Subject: [PATCH] pus parser continued

---
 returnvalues/FwClassIds.h  |  1 +
 tmtcservices/PusParser.cpp | 33 +++++++++++++++++++++++++++++++++
 tmtcservices/PusParser.h   | 11 +++++++++++
 3 files changed, 45 insertions(+)

diff --git a/returnvalues/FwClassIds.h b/returnvalues/FwClassIds.h
index ddee539e..abee534a 100644
--- a/returnvalues/FwClassIds.h
+++ b/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 !
 
 };
diff --git a/tmtcservices/PusParser.cpp b/tmtcservices/PusParser.cpp
index 76934e33..5f326b44 100644
--- a/tmtcservices/PusParser.cpp
+++ b/tmtcservices/PusParser.cpp
@@ -1,4 +1,5 @@
 #include <framework/tmtcservices/PusParser.h>
+#include <framework/serviceinterface/ServiceInterfaceStream.h>
 
 PusParser::PusParser(uint16_t maxExpectedPusPackets,
 		bool storeSplitPackets): indexSizePairFIFO(maxExpectedPusPackets) {
@@ -6,5 +7,37 @@ PusParser::PusParser(uint16_t 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;
+	}
+
+	// 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;
+	}
+	else {
+		return NO_PACKET_FOUND;
+	}
+
+	if(frameSize > packetSize) {
+		return readMultiplePackets(frameSize);
+
+	}
 	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;
+//		}
+//	}
+//}
diff --git a/tmtcservices/PusParser.h b/tmtcservices/PusParser.h
index ef177079..cdc887ed 100644
--- a/tmtcservices/PusParser.h
+++ b/tmtcservices/PusParser.h
@@ -23,6 +23,9 @@
  */
 class PusParser {
 public:
+	static constexpr uint8_t INTERFACE_ID = CLASS_ID::PUS_PARSER;
+	static constexpr ReturnValue_t NO_PACKET_FOUND = MAKE_RETURN_CODE(0x00);
+
 	/**
 	 * Parser constructor.
 	 * @param maxExpectedPusPackets
@@ -34,6 +37,12 @@ public:
 	 */
 	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);
 private:
 	//! The first entry is the index inside the buffer while the second index
@@ -45,6 +54,8 @@ private:
 	fsfw::FIFO<indexSizePair> indexSizePairFIFO;
 
 	bool storeSplitPackets = false;
+
+	ReturnValue_t readMultiplePackets(size_t frameSize);
 };