added function to determine space packet ID

from APID

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(5, false);
+        spacePacketParser = new SpacePacketParser(validPacketIds);
         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,29 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) {
         readAmount = receptionBuffer.size();
     }
     ringBuffer.readData(receptionBuffer.data(), readAmount, true);
-    uint32_t readPackets = 0;
-    result = spacePacketParser->parseSpacePackets(receptionBuffer.data(), readAmount, readPackets);
-    if(result == SpacePacketParser::NO_PACKET_FOUND) {
-        ringBuffer.deleteData(availableReadData);
-        lastRingBufferSize = ringBuffer.getAvailableReadData();
-    }
-    else if(result == HasReturnvaluesIF::RETURN_OK) {
-        // Space Packets were found. Handle them here
-        auto& fifo = spacePacketParser->fifo();
-        SpacePacketParser::IndexSizePair idxSizePair;
-        while(not fifo.empty()) {
-            fifo.retrieve(&idxSizePair);
-            result = handleTcReception(receptionBuffer.data() + idxSizePair.first,
-                    idxSizePair.second);
-            std::memset(receptionBuffer.data() + idxSizePair.first, 0, idxSizePair.second);
-            ringBuffer.deleteData(idxSizePair.second);
+    const uint8_t* bufPtr = receptionBuffer.data();
+    const uint8_t** bufPtrPtr = &bufPtr;
+    size_t startIdx = 0;
+    size_t foundSize = 0;
+    size_t readLen = 0;
+    while(readLen < readAmount) {
+        result = spacePacketParser->parseSpacePackets(bufPtrPtr, readAmount,
+                startIdx, foundSize, readLen);
+        switch(result) {
+        case(SpacePacketParser::NO_PACKET_FOUND):
+        case(SpacePacketParser::SPLIT_PACKET): {
+            break;
+        }
+        case(HasReturnvaluesIF::RETURN_OK): {
+            result = handleTcReception(receptionBuffer.data() + startIdx, foundSize);
             if(result != HasReturnvaluesIF::RETURN_OK) {
                 status = result;
             }
-            lastRingBufferSize = ringBuffer.getAvailableReadData();
         }
+        }
+        ringBuffer.deleteData(foundSize);
+        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,10 +123,11 @@ private:
 
     std::vector<uint8_t> receptionBuffer;
     SimpleRingBuffer ringBuffer;
+    std::vector<uint16_t> validPacketIds;
     SpacePacketParser* spacePacketParser = nullptr;
     uint8_t lastRingBufferSize = 0;
 
-    void handleServerOperation(socket_t& connSocket);
+    virtual void handleServerOperation(socket_t& connSocket);
     ReturnValue_t handleTcReception(uint8_t* spacePacket, size_t packetSize);
     ReturnValue_t handleTmSending(socket_t connSocket, bool& tmSent);
     ReturnValue_t handleTcRingBufferData(size_t availableReadData);

src/fsfw/tmtcpacket/SpacePacket.h

@@ -15,57 +15,67 @@
  */
 class SpacePacket: public SpacePacketBase {
 public:
-	static const uint16_t PACKET_MAX_SIZE = 1024;
-	/**
-	 * The constructor initializes the packet and sets all header information
-	 * according to the passed parameters.
-	 * @param packetDataLength Sets the packet data length field and therefore specifies
-	 * the size of the packet.
-	 * @param isTelecommand Sets the packet type field to either TC (true) or TM (false).
-	 * @param apid Sets the packet's APID field. The default value describes an idle packet.
-	 * @param sequenceCount ets the packet's Source Sequence Count field.
-	 */
-	SpacePacket(uint16_t packetDataLength, bool isTelecommand = false,
-			uint16_t apid = APID_IDLE_PACKET, uint16_t sequenceCount = 0);
-	/**
-	 * The class's default destructor.
-	 */
-	virtual ~SpacePacket();
-	/**
-	 * With this call, the complete data content (including the CCSDS Primary
-	 * Header) is overwritten with the byte stream given.
-	 * @param p_data		Pointer to data to overwrite the content with
-	 * @param packet_size	Size of the data
-	 * @return	@li	\c true if packet_size is smaller than \c MAX_PACKET_SIZE.
-	 * 			@li \c false else.
-	 */
-	bool addWholeData(const uint8_t* p_data, uint32_t packet_size);
+    static const uint16_t PACKET_MAX_SIZE = 1024;
+    /**
+     * The constructor initializes the packet and sets all header information
+     * according to the passed parameters.
+     * @param packetDataLength Sets the packet data length field and therefore specifies
+     * the size of the packet.
+     * @param isTelecommand Sets the packet type field to either TC (true) or TM (false).
+     * @param apid Sets the packet's APID field. The default value describes an idle packet.
+     * @param sequenceCount ets the packet's Source Sequence Count field.
+     */
+    SpacePacket(uint16_t packetDataLength, bool isTelecommand = false,
+            uint16_t apid = APID_IDLE_PACKET, uint16_t sequenceCount = 0);
+    /**
+     * The class's default destructor.
+     */
+    virtual ~SpacePacket();
+
+    static constexpr uint16_t getTcSpacePacketIdFromApid(uint16_t apid) {
+        uint16_t tcPacketId = (0x18 << 8) | (((apid >> 8) & 0x07) << 8) | (apid & 0x00ff);
+        return tcPacketId;
+    }
+    static constexpr uint16_t getTmSpacePacketIdFromApid(uint16_t apid) {
+        uint16_t tmPacketId = (0x08 << 8) | (((apid >> 8) & 0x07) << 8) | (apid & 0x00ff);
+        return tmPacketId;
+    }
+
+    /**
+     * With this call, the complete data content (including the CCSDS Primary
+     * Header) is overwritten with the byte stream given.
+     * @param p_data		Pointer to data to overwrite the content with
+     * @param packet_size	Size of the data
+     * @return	@li	\c true if packet_size is smaller than \c MAX_PACKET_SIZE.
+     * 			@li \c false else.
+     */
+    bool addWholeData(const uint8_t* p_data, uint32_t packet_size);
 protected:
-	/**
-	 * This structure defines the data structure of a Space Packet as local data.
-	 * There's a buffer which corresponds to the Space Packet Data Field with a
-	 * maximum size of \c PACKET_MAX_SIZE.
-	 */
-	struct PacketStructured {
-		CCSDSPrimaryHeader header;
-		uint8_t buffer[PACKET_MAX_SIZE];
-	};
-	/**
-	 * This union simplifies accessing the full data content of the Space Packet.
-	 * This is achieved by putting the \c PacketStructured struct in a union with
-	 * a plain buffer.
-	 */
-	union SpacePacketData {
-		PacketStructured fields;
-		uint8_t byteStream[PACKET_MAX_SIZE + sizeof(CCSDSPrimaryHeader)];
-	};
-	/**
-	 * This is the data representation of the class.
-	 * It is a struct of CCSDS Primary Header and a data field, which again is
-	 * packed in an union, so the data can be accessed as a byte stream without
-	 * a cast.
-	 */
-	SpacePacketData localData;
+    /**
+     * This structure defines the data structure of a Space Packet as local data.
+     * There's a buffer which corresponds to the Space Packet Data Field with a
+     * maximum size of \c PACKET_MAX_SIZE.
+     */
+    struct PacketStructured {
+        CCSDSPrimaryHeader header;
+        uint8_t buffer[PACKET_MAX_SIZE];
+    };
+    /**
+     * This union simplifies accessing the full data content of the Space Packet.
+     * This is achieved by putting the \c PacketStructured struct in a union with
+     * a plain buffer.
+     */
+    union SpacePacketData {
+        PacketStructured fields;
+        uint8_t byteStream[PACKET_MAX_SIZE + sizeof(CCSDSPrimaryHeader)];
+    };
+    /**
+     * This is the data representation of the class.
+     * It is a struct of CCSDS Primary Header and a data field, which again is
+     * packed in an union, so the data can be accessed as a byte stream without
+     * a cast.
+     */
+    SpacePacketData localData;
 };
 
 #endif /* SPACEPACKET_H_ */

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):
-        indexSizePairFIFO(maxExpectedPusPackets) {
+SpacePacketParser::SpacePacketParser(std::vector<uint16_t> validPacketIds):
+        validPacketIds(validPacketIds) {
 }
 
-ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t *frame,
-		size_t frameSize, uint32_t& foundPackets) {
-	if(frame == nullptr or frameSize < 5) {
+ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t *buffer,
+        const size_t maxSize, size_t& startIndex, size_t& foundSize) {
+    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;
-#else
-		sif::printError("PusParser::parsePusPackets: Frame invalid\n");
+        sif::warning << "SpacePacketParser::parseSpacePackets: Frame invalid" << std::endl;
+#else
+        sif::printWarning("SpacePacketParser::parseSpacePackets: Frame invalid\n");
 #endif
-		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;
+        return HasReturnvaluesIF::RETURN_FAILED;
     }
+    const uint8_t* bufPtr = *buffer;
 
-	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));
-		    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: "
-					<< 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;
-	}
+    auto verifyLengthField = [&](size_t idx) {
+        uint16_t lengthField = bufPtr[idx + 4] << 8 | bufPtr[idx + 5];
+        size_t packetSize = lengthField + 7;
+        startIndex = idx;
+        ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+        if(lengthField == 0) {
+            // Skip whole header for now
+            foundSize = 6;
+            result = NO_PACKET_FOUND;
+        }
+        else if(packetSize + idx > maxSize) {
+            // Don't increment buffer and read length here, user has to decide what to do
+            foundSize = packetSize;
+            return SPLIT_PACKET;
+        }
+        else {
+            foundSize = packetSize;
+        }
+        *buffer += foundSize;
+        readLen += foundSize;
+        return result;
+    };
 
-	ReturnValue_t result = indexSizePairFIFO.insert(IndexSizePair(currentIndex,
-			nextPacketSize));
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		// FIFO full.
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::debug << "PusParser: Issue inserting into start index size "
-		        << "FIFO, it is full!" << std::endl;
-#else
-		sif::printDebug("PusParser: Issue inserting into start index size "
-                "FIFO, it is full!\n");
-#endif
-	}
-	foundPackets += 1;
-	currentIndex += nextPacketSize;
-
-	return result;
+    size_t idx = 0;
+    // Space packet ID as start marker
+    if(validPacketIds.size() > 0) {
+        while(idx < maxSize - 5) {
+            uint16_t currentPacketId = bufPtr[idx] << 8 | bufPtr[idx + 1];
+            if(std::find(validPacketIds.begin(), validPacketIds.end(), currentPacketId) !=
+                    validPacketIds.end()) {
+                if(idx + 5 >= maxSize) {
+                    return SPLIT_PACKET;
+                }
+                return verifyLengthField(idx);
+            }
+            else {
+                idx++;
+            }
+        }
+        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,
-			size_t startIndex, uint32_t& foundPackets);
-	ReturnValue_t readNextPacket(const uint8_t *frame,
-			size_t frameSize, size_t& startIndex, uint32_t& foundPackets);
+	//bool storeSplitPackets = false;
+
+//	ReturnValue_t readMultiplePackets(const uint8_t *frame, size_t frameSize,
+//			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_ */