refactored space packet parser

src/fsfw/osal/common/TcpTmTcServer.cpp

@@ -33,7 +33,7 @@ TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
         ReceptionModes receptionMode):
         SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), receptionMode(receptionMode),
         tcpConfig(customTcpServerPort), receptionBuffer(receptionBufferSize),
-        ringBuffer(ringBufferSize, true) {
+        ringBuffer(ringBufferSize, true), validPacketIds() {
 }
 
 ReturnValue_t TcpTmTcServer::initialize() {
@@ -46,8 +46,7 @@ ReturnValue_t TcpTmTcServer::initialize() {
 
     switch(receptionMode) {
     case(ReceptionModes::SPACE_PACKETS): {
-        // For now, hardcode a maximum of 5 store packets here and no split packets are allowed
+        spacePacketParser = new SpacePacketParser(validPacketIds);
-        spacePacketParser = new SpacePacketParser(5, false);
         if(spacePacketParser == nullptr) {
             return HasReturnvaluesIF::RETURN_FAILED;
         }
@@ -246,7 +245,8 @@ std::string TcpTmTcServer::getTcpPort() const {
     return tcpConfig.tcpPort;
 }
 
-void TcpTmTcServer::setSpacePacketParsingOptions(uint8_t parserFifoSize) {
+void TcpTmTcServer::setSpacePacketParsingOptions(std::vector<uint16_t> validPacketIds) {
+    this->validPacketIds = validPacketIds;
 }
 
 TcpTmTcServer::TcpConfig& TcpTmTcServer::getTcpConfigStruct() {
@@ -319,27 +319,32 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) {
         readAmount = receptionBuffer.size();
     }
     ringBuffer.readData(receptionBuffer.data(), readAmount, true);
-    uint32_t readPackets = 0;
+    const uint8_t* bufPtr = receptionBuffer.data();
-    result = spacePacketParser->parseSpacePackets(receptionBuffer.data(), readAmount, readPackets);
+    const uint8_t** bufPtrPtr = &bufPtr;
-    if(result == SpacePacketParser::NO_PACKET_FOUND) {
+    size_t startIdx = 0;
-        ringBuffer.deleteData(availableReadData);
+    size_t foundSize = 0;
-        lastRingBufferSize = ringBuffer.getAvailableReadData();
+    size_t readLen = 0;
-    }
+    while(readLen <= readAmount) {
-    else if(result == HasReturnvaluesIF::RETURN_OK) {
+        result = spacePacketParser->parseSpacePackets(bufPtrPtr, readAmount,
-        // Space Packets were found. Handle them here
+                startIdx, foundSize, readLen);
-        auto& fifo = spacePacketParser->fifo();
+        if(result == SpacePacketParser::NO_PACKET_FOUND) {
-        SpacePacketParser::IndexSizePair idxSizePair;
+            ringBuffer.deleteData(foundSize);
-        while(not fifo.empty()) {
+            lastRingBufferSize = ringBuffer.getAvailableReadData();
-            fifo.retrieve(&idxSizePair);
+        }
-            result = handleTcReception(receptionBuffer.data() + idxSizePair.first,
+        else if(result == SpacePacketParser::SPLIT_PACKET) {
-                    idxSizePair.second);
+            // might be half of a packet? Skip it for now
-            std::memset(receptionBuffer.data() + idxSizePair.first, 0, idxSizePair.second);
+            ringBuffer.deleteData(foundSize);
-            ringBuffer.deleteData(idxSizePair.second);
+            lastRingBufferSize = ringBuffer.getAvailableReadData();
+        }
+        else if(result == HasReturnvaluesIF::RETURN_OK) {
+            result = handleTcReception(receptionBuffer.data() + startIdx, foundSize);
+            ringBuffer.deleteData(foundSize);
             if(result != HasReturnvaluesIF::RETURN_OK) {
                 status = result;
             }
             lastRingBufferSize = ringBuffer.getAvailableReadData();
         }
+        std::memset(receptionBuffer.data() + startIdx, 0, foundSize);
     }
     return status;
 }

src/fsfw/osal/common/TcpTmTcServer.h

@@ -97,7 +97,7 @@ public:
      * @return
      */
     TcpConfig& getTcpConfigStruct();
-    void setSpacePacketParsingOptions(uint8_t parserFifoSize);
+    void setSpacePacketParsingOptions(std::vector<uint16_t> validPacketIds);
 
     ReturnValue_t initialize() override;
     ReturnValue_t performOperation(uint8_t opCode) override;
@@ -123,6 +123,7 @@ private:
 
     std::vector<uint8_t> receptionBuffer;
     SimpleRingBuffer ringBuffer;
+    std::vector<uint16_t> validPacketIds;
     SpacePacketParser* spacePacketParser = nullptr;
     uint8_t lastRingBufferSize = 0;
 

src/fsfw/tmtcservices/SpacePacketParser.cpp

@@ -1,147 +1,77 @@
 #include <fsfw/serviceinterface/ServiceInterface.h>
 #include <fsfw/tmtcservices/SpacePacketParser.h>
+#include <algorithm>
 
-SpacePacketParser::SpacePacketParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets):
+SpacePacketParser::SpacePacketParser(std::vector<uint16_t> validPacketIds):
-        indexSizePairFIFO(maxExpectedPusPackets) {
+        validPacketIds(validPacketIds) {
 }
 
-ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t *frame,
+ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t *buffer,
-		size_t frameSize, uint32_t& foundPackets) {
+        const size_t maxSize, size_t& startIndex, size_t& foundSize) {
-	if(frame == nullptr or frameSize < 5) {
+    const uint8_t** tempPtr = &buffer;
+    size_t readLen = 0;
+    return parseSpacePackets(tempPtr, maxSize, startIndex, foundSize, readLen);
+}
+
+ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t **buffer, const size_t maxSize,
+        size_t &startIndex, size_t &foundSize, size_t& readLen) {
+    if(buffer == nullptr or maxSize < 5) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PusParser::parsePusPackets: Frame invalid" << std::endl;
+        sif::warning << "SpacePacketParser::parseSpacePackets: Frame invalid" << std::endl;
-#else
+#else
-		sif::printError("PusParser::parsePusPackets: Frame invalid\n");
+        sif::printWarning("SpacePacketParser::parseSpacePackets: Frame invalid\n");
 #endif
-		return HasReturnvaluesIF::RETURN_FAILED;
+        return HasReturnvaluesIF::RETURN_FAILED;
-	}
-
-	if(indexSizePairFIFO.full()) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PusParser::parsePusPackets: FIFO is full" << std::endl;
-#else
-		sif::printError("PusParser::parsePusPackets: FIFO is full\n");
-#endif
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-
-	size_t lengthField = frame[4] << 8 | frame[5];
-
-	if(lengthField == 0) {
-		return NO_PACKET_FOUND;
-	}
-
-    // Size of a space packet is the value in the packet length field plus 7
-	size_t packetSize = lengthField + 7;
-	if(packetSize > frameSize) {
-		if(storeSplitPackets) {
-			indexSizePairFIFO.insert(IndexSizePair(0, frameSize));
-			foundPackets = 1;
-		}
-		else {
-#if FSFW_VERBOSE_LEVEL >= 1
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::warning << "TcSerialPollingTask::readNextPacket: Next packet "
-					<< "larger than remaining frame," << std::endl;
-			sif::warning << "Throwing away packet. Detected packet size: "
-					<< packetSize << std::endl;
-#else
-			sif::printWarning("TcSerialPollingTask::readNextPacket: Next packet "
-                    "larger than remaining frame.\n");
-            sif::printWarning("Throwing away packet. Detected packet size: %lu",
-                    static_cast<unsigned long>(packetSize));
-#endif
-#endif /* FSFW_VERBOSE_LEVEL >= 1 */
-		}
-		return SPLIT_PACKET;
-	}
-	else {
-		indexSizePairFIFO.insert(IndexSizePair(0, packetSize));
-		if(packetSize >= frameSize) {
-		    foundPackets = 1;
-			return HasReturnvaluesIF::RETURN_OK;
-		}
-	}
-
-	// packet size is smaller than frame size, parse for more packets.
-	return readMultiplePackets(frame, frameSize, packetSize, foundPackets);
-}
-
-ReturnValue_t SpacePacketParser::readMultiplePackets(const uint8_t *frame,
-		size_t frameSize, size_t startIndex, uint32_t& foundPackets) {
-	while (startIndex < frameSize) {
-		ReturnValue_t result = readNextPacket(frame, frameSize, startIndex, foundPackets);
-		if(result != HasReturnvaluesIF::RETURN_OK) {
-			return result;
-		}
-	}
-	return HasReturnvaluesIF::RETURN_OK;
-}
-
-DynamicFIFO<SpacePacketParser::IndexSizePair>& SpacePacketParser::fifo(){
-	return indexSizePairFIFO;
-}
-
-SpacePacketParser::IndexSizePair SpacePacketParser::getNextFifoPair() {
-	IndexSizePair nextIndexSizePair;
-	indexSizePairFIFO.retrieve(&nextIndexSizePair);
-	return nextIndexSizePair;
-}
-
-ReturnValue_t SpacePacketParser::readNextPacket(const uint8_t *frame,
-		size_t frameSize, size_t& currentIndex, uint32_t& foundPackets) {
-    if(currentIndex + 5 > frameSize) {
-        currentIndex = frameSize;
-        return HasReturnvaluesIF::RETURN_OK;
     }
+    const uint8_t* bufPtr = *buffer;
 
-	uint16_t lengthField = frame[currentIndex + 4] << 8 |
+    auto verifyLengthField = [&](size_t idx) {
-			frame[currentIndex + 5];
+        uint16_t lengthField = bufPtr[idx + 4] << 8 | bufPtr[idx + 5];
-	if(lengthField == 0) {
+        size_t packetSize = lengthField + 7;
-		// It is assumed that no packet follows.
+        startIndex = idx;
-		currentIndex = frameSize;
+        ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
-		return HasReturnvaluesIF::RETURN_OK;
+        if(lengthField == 0) {
-	}
+            // Skip whole header for now
-	size_t nextPacketSize = lengthField + 7;
+            foundSize = 6;
-	size_t remainingSize = frameSize - currentIndex;
+            result = NO_PACKET_FOUND;
-	if(nextPacketSize > remainingSize)
+        }
-	{
+        else if(packetSize + idx > maxSize) {
-		if(storeSplitPackets) {
+            // Don't increment buffer and read length here, user has to decide what to do
-			indexSizePairFIFO.insert(IndexSizePair(currentIndex, remainingSize));
+            foundSize = packetSize;
-		    foundPackets += 1;
+            return SPLIT_PACKET;
-		}
+        }
-		else {
+        else {
-#if FSFW_VERBOSE_LEVEL >= 1
+            foundSize = packetSize;
-#if FSFW_CPP_OSTREAM_ENABLED == 1
+        }
-			sif::warning << "TcSerialPollingTask::readNextPacket: Next packet "
+        *buffer += foundSize;
-					<< "larger than remaining frame." << std::endl;
+        readLen += foundSize;
-			sif::warning << "Throwing away packet. Detected packet size: "
+        return result;
-					<< nextPacketSize << std::endl;
+    };
-#else
-			sif::printWarning("TcSerialPollingTask::readNextPacket: Next packet "
-                    "larger than remaining frame.\n");
-			sif::printWarning("Throwing away packet. Detected packet size: %lu\n",
-			        static_cast<unsigned long>(nextPacketSize));
-#endif
-#endif
-		}
-		return SPLIT_PACKET;
-	}
 
-	ReturnValue_t result = indexSizePairFIFO.insert(IndexSizePair(currentIndex,
+    size_t idx = 0;
-			nextPacketSize));
+    // Space packet ID as start marker
-	if (result != HasReturnvaluesIF::RETURN_OK) {
+    if(validPacketIds.size() > 0) {
-		// FIFO full.
+        while(idx < maxSize - 5) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
+            uint16_t currentPacketId = bufPtr[idx] << 8 | bufPtr[idx + 1];
-		sif::debug << "PusParser: Issue inserting into start index size "
+            if(std::find(validPacketIds.begin(), validPacketIds.end(), currentPacketId) !=
-		        << "FIFO, it is full!" << std::endl;
+                    validPacketIds.end()) {
-#else
+                if(idx + 5 >= maxSize) {
-		sif::printDebug("PusParser: Issue inserting into start index size "
+                    return SPLIT_PACKET;
-                "FIFO, it is full!\n");
+                }
-#endif
+                return verifyLengthField(idx);
-	}
+            }
-	foundPackets += 1;
+            else {
-	currentIndex += nextPacketSize;
+                idx++;
-
+            }
-	return result;
+        }
+        startIndex = 0;
+        foundSize = maxSize;
+        *buffer += foundSize;
+        readLen += foundSize;
+        return NO_PACKET_FOUND;
+    }
+    // Assume that the user verified a valid start of a space packet
+    else {
+        return verifyLengthField(idx);
+    }
 }

src/fsfw/tmtcservices/SpacePacketParser.h

@@ -42,26 +42,30 @@ public:
 	 * Specifies whether split packets are also stored inside the FIFO,
 	 * with the size being the remaining frame size.
 	 */
-	SpacePacketParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets);
+	SpacePacketParser(std::vector<uint16_t> validPacketIds);
 
 	/**
 	 * Parse a given frame for PUS packets
 	 * @param frame
 	 * @param frameSize
+	 * @param foundPackets The number of found packets will be stored here
 	 * @return
 	 *  -@c NO_PACKET_FOUND if no packet was found
 	 *  -@c SPLIT_PACKET if splitting is enabled and a split packet was found
 	 *  -@c RETURN_OK if a packet was found. The index and sizes are stored in the internal FIFO
 	 */
-	ReturnValue_t parseSpacePackets(const uint8_t* frame, size_t frameSize, uint32_t& foundPackets);
+	ReturnValue_t parseSpacePackets(const uint8_t* buffer, const size_t maxSize,
+	        size_t& startIndex, size_t& foundSize);
 
+	ReturnValue_t parseSpacePackets(const uint8_t **buffer, const size_t maxSize,
+	        size_t& startIndex, size_t& foundSize, size_t& readLen);
 	/**
 	 * Accessor function to get a reference to the internal FIFO which
 	 * stores pairs of index and packet sizes. This FIFO is filled
 	 * by the #parsePusPackets function.
 	 * @return
 	 */
-	DynamicFIFO<IndexSizePair>& fifo();
+	//DynamicFIFO<IndexSizePair>& fifo();
 
 	/**
 	 * Retrieve the next index and packet size pair from the FIFO.
@@ -69,7 +73,7 @@ public:
 	 * is empty, an empty pair will be returned.
 	 * @return
 	 */
-	IndexSizePair getNextFifoPair();
+	//IndexSizePair getNextFifoPair();
 
 private:
 	/**
@@ -77,14 +81,16 @@ private:
 	 * inside the receive buffer. The maximum number of entries is defined
 	 * by the first constructor argument.
 	 */
-	DynamicFIFO<IndexSizePair> indexSizePairFIFO;
+	//DynamicFIFO<IndexSizePair> indexSizePairFIFO;
 
-	bool storeSplitPackets = false;
+	std::vector<uint16_t> validPacketIds;
 
-	ReturnValue_t readMultiplePackets(const uint8_t *frame, size_t frameSize,
+	//bool storeSplitPackets = false;
-			size_t startIndex, uint32_t& foundPackets);
+
-	ReturnValue_t readNextPacket(const uint8_t *frame,
+//	ReturnValue_t readMultiplePackets(const uint8_t *frame, size_t frameSize,
-			size_t frameSize, size_t& startIndex, uint32_t& foundPackets);
+//			size_t startIndex, uint32_t& foundPackets);
+//	ReturnValue_t readNextPacket(const uint8_t *frame,
+//			size_t frameSize, size_t& startIndex, uint32_t& foundPackets);
 };
 
 #endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */