TCP refactoring

This refactoring keeps the TCP connection opened until the client closes
it. It also increased the robustness of the TCP reception.

Because TCP is stream based and usually applied to newline separated
data, a special way to handle binary space packets is required.

The new SpacePacketParser class takes care of this by taking TC packet
IDs as as optional start markers to parse for space packets in a given
buffer.

The refactored TCP server uses a ring buffer, a reception buffer and the
new parser to extract space packets from a stream in a safer way.

src/fsfw/osal/common/TcpTmTcServer.cpp

@@ -1,8 +1,11 @@
-#include "fsfw/osal/common/TcpTmTcServer.h"
-#include "fsfw/osal/common/TcpTmTcBridge.h"
-#include "fsfw/osal/common/tcpipHelpers.h"
-
 #include "fsfw/platform.h"
+#include "fsfw/FSFW.h"
+
+#include "TcpTmTcServer.h"
+#include "TcpTmTcBridge.h"
+#include "tcpipHelpers.h"
+
+#include "fsfw/tasks/TaskFactory.h"
 #include "fsfw/container/SharedRingBuffer.h"
 #include "fsfw/ipc/MessageQueueSenderIF.h"
 #include "fsfw/ipc/MutexGuard.h"
@@ -17,6 +20,7 @@
 #elif defined(PLATFORM_UNIX)
 #include <netdb.h>
 #endif
+#include <chrono>
 
 #ifndef FSFW_TCP_RECV_WIRETAPPING_ENABLED
 #define FSFW_TCP_RECV_WIRETAPPING_ENABLED 0
@@ -25,12 +29,11 @@
 const std::string TcpTmTcServer::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_PORT;
 
 TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
-        size_t receptionBufferSize, std::string customTcpServerPort):
+        size_t receptionBufferSize, size_t ringBufferSize, std::string customTcpServerPort,
-        SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge),
+        ReceptionModes receptionMode):
-        tcpPort(customTcpServerPort), receptionBuffer(receptionBufferSize) {
+        SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), receptionMode(receptionMode),
-    if(tcpPort == "") {
+        tcpConfig(customTcpServerPort), receptionBuffer(receptionBufferSize),
-        tcpPort = DEFAULT_SERVER_PORT;
+        ringBuffer(ringBufferSize, true), validPacketIds() {
-    }
 }
 
 ReturnValue_t TcpTmTcServer::initialize() {
@@ -41,6 +44,16 @@ ReturnValue_t TcpTmTcServer::initialize() {
         return result;
     }
 
+    switch(receptionMode) {
+    case(ReceptionModes::SPACE_PACKETS): {
+        spacePacketParser = new SpacePacketParser(validPacketIds);
+        if(spacePacketParser == nullptr) {
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+        tcpConfig.tcpFlags |= MSG_DONTWAIT;
+    }
+    }
+
     tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
     if (tcStore == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -63,7 +76,7 @@ ReturnValue_t TcpTmTcServer::initialize() {
     hints.ai_flags = AI_PASSIVE;
 
     // Listen to all addresses (0.0.0.0) by using AI_PASSIVE in the hint flags
-    retval = getaddrinfo(nullptr, tcpPort.c_str(), &hints, &addrResult);
+    retval = getaddrinfo(nullptr, tcpConfig.tcpPort.c_str(), &hints, &addrResult);
     if (retval != 0) {
         handleError(Protocol::TCP, ErrorSources::GETADDRINFO_CALL);
         return HasReturnvaluesIF::RETURN_FAILED;
@@ -105,7 +118,7 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
 
     // Listen for connection requests permanently for lifetime of program
     while(true) {
-        retval = listen(listenerTcpSocket, tcpBacklog);
+        retval = listen(listenerTcpSocket, tcpConfig.tcpBacklog);
         if(retval == SOCKET_ERROR) {
             handleError(Protocol::TCP, ErrorSources::LISTEN_CALL, 500);
             continue;
@@ -123,11 +136,12 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
         handleServerOperation(connSocket);
 
         // Done, shut down connection and go back to listening for client requests
-        retval = shutdown(connSocket, SHUT_SEND);
+        retval = shutdown(connSocket, SHUT_BOTH);
         if(retval != 0) {
             handleError(Protocol::TCP, ErrorSources::SHUTDOWN_CALL);
         }
         closeSocket(connSocket);
+        connSocket = 0;
     }
     return HasReturnvaluesIF::RETURN_OK;
 }
@@ -144,41 +158,69 @@ ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() {
     return HasReturnvaluesIF::RETURN_OK;
 }
 
-void TcpTmTcServer::handleServerOperation(socket_t connSocket) {
+void TcpTmTcServer::handleServerOperation(socket_t& connSocket) {
-    int retval = 0;
+    while (true) {
-    do {
+        int retval = recv(
-        // Read all telecommands sent by the client
+                connSocket,
-        retval = recv(connSocket,
                 reinterpret_cast<char*>(receptionBuffer.data()),
                 receptionBuffer.capacity(),
-                tcpFlags);
+                tcpConfig.tcpFlags
-        if (retval > 0) {
+        );
-            handleTcReception(retval);
+        if(retval == 0) {
+            // Client closed connection
+            return;
+        }
+        else if(retval > 0) {
+            // The ring buffer was configured for overwrite, so the returnvalue does not need to
+            // be checked for now
+            ringBuffer.writeData(receptionBuffer.data(), retval);
+        }
+        else if(retval < 0)  {
+            if(errno == EAGAIN) {
+                // No data available. Check whether any packets have been read, then send back
+                // telemetry if available
+                bool tcAvailable = false;
+                bool tmSent = false;
+                size_t availableReadData = ringBuffer.getAvailableReadData();
+                if(availableReadData > lastRingBufferSize) {
+                    tcAvailable = true;
+                    handleTcRingBufferData(availableReadData);
+                }
+                ReturnValue_t result = handleTmSending(connSocket, tmSent);
+                if(result == CONN_BROKEN) {
+                    return;
+                }
+                if(not tcAvailable and not tmSent) {
+                    TaskFactory::delayTask(DEFAULT_LOOP_DELAY_MS);
                 }
-        else if(retval == 0) {
-            // Client has finished sending telecommands, send telemetry now
-            handleTmSending(connSocket);
             }
             else {
-            // Should not happen
                 tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL);
             }
-    } while(retval > 0);
+        }
+    }
 }
 
-ReturnValue_t TcpTmTcServer::handleTcReception(size_t bytesRecvd) {
+ReturnValue_t TcpTmTcServer::handleTcReception(uint8_t* spacePacket, size_t packetSize) {
 #if FSFW_TCP_RECV_WIRETAPPING_ENABLED == 1
     arrayprinter::print(receptionBuffer.data(), bytesRead);
 #endif
+    if(spacePacket == nullptr or packetSize == 0) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
     store_address_t storeId;
-    ReturnValue_t result = tcStore->addData(&storeId, receptionBuffer.data(), bytesRecvd);
+    ReturnValue_t result = tcStore->addData(&storeId, spacePacket, packetSize);
     if (result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning<< "TcpTmTcServer::handleServerOperation: Data storage failed." << std::endl;
+        sif::warning << "TcpTmTcServer::handleServerOperation: Data storage with packet size" <<
-        sif::warning << "Packet size: " << bytesRecvd << std::endl;
+                packetSize << " failed" << std::endl;
+#else
+        sif::printWarning("TcpTmTcServer::handleServerOperation: Data storage with packet size %d "
+                "failed\n", packetSize);
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */
+        return result;
     }
 
     TmTcMessage message(storeId);
@@ -187,8 +229,11 @@ ReturnValue_t TcpTmTcServer::handleTcReception(size_t bytesRecvd) {
     if (result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "UdpTcPollingTask::handleSuccessfullTcRead: "
+        sif::warning << "TcpTmTcServer::handleServerOperation: "
                 " Sending message to queue failed" << std::endl;
+#else
+        sif::printWarning("TcpTmTcServer::handleServerOperation: "
+                " Sending message to queue failed\n");
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */
         tcStore->deleteData(storeId);
@@ -196,21 +241,26 @@ ReturnValue_t TcpTmTcServer::handleTcReception(size_t bytesRecvd) {
     return result;
 }
 
-void TcpTmTcServer::setTcpBacklog(uint8_t tcpBacklog) {
-    this->tcpBacklog = tcpBacklog;
-}
-
 std::string TcpTmTcServer::getTcpPort() const {
-    return tcpPort;
+    return tcpConfig.tcpPort;
 }
 
-ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket) {
+void TcpTmTcServer::setSpacePacketParsingOptions(std::vector<uint16_t> validPacketIds) {
+    this->validPacketIds = validPacketIds;
+}
+
+TcpTmTcServer::TcpConfig& TcpTmTcServer::getTcpConfigStruct() {
+    return tcpConfig;
+}
+
+ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent) {
     // Access to the FIFO is mutex protected because it is filled by the bridge
     MutexGuard(tmtcBridge->mutex, tmtcBridge->timeoutType, tmtcBridge->mutexTimeoutMs);
     store_address_t storeId;
     while((not tmtcBridge->tmFifo->empty()) and
             (tmtcBridge->packetSentCounter < tmtcBridge->sentPacketsPerCycle)) {
-        tmtcBridge->tmFifo->retrieve(&storeId);
+        // Send can fail, so only peek from the FIFO
+        tmtcBridge->tmFifo->peek(&storeId);
 
         // Using the store accessor will take care of deleting TM from the store automatically
         ConstStorageAccessor storeAccessor(storeId);
@@ -221,10 +271,101 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket) {
         int retval = send(connSocket,
                 reinterpret_cast<const char*>(storeAccessor.data()),
                 storeAccessor.size(),
-                tcpTmFlags);
+                tcpConfig.tcpTmFlags);
-        if(retval != static_cast<int>(storeAccessor.size())) {
+        if(retval == static_cast<int>(storeAccessor.size())) {
-            tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::SEND_CALL);
+            // Packet sent, clear FIFO entry
+            tmtcBridge->tmFifo->pop();
+            tmSent = true;
+
+        }
+        else if(retval <= 0) {
+            // Assume that the client has closed the connection here for now
+            handleSocketError(storeAccessor);
+            return CONN_BROKEN;
         }
     }
     return HasReturnvaluesIF::RETURN_OK;
 }
+
+ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) {
+    ReturnValue_t status = HasReturnvaluesIF::RETURN_OK;
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    size_t readAmount = availableReadData;
+    lastRingBufferSize = availableReadData;
+    if(readAmount >= ringBuffer.getMaxSize()) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        // Possible configuration error, too much data or/and data coming in too fast,
+        // requiring larger buffers
+        sif::warning << "TcpTmTcServer::handleServerOperation: Ring buffer reached " <<
+                "fill count" << std::endl;
+#else
+        sif::printWarning("TcpTmTcServer::handleServerOperation: Ring buffer reached "
+                "fill count");
+#endif
+#endif
+    }
+    if(readAmount >= receptionBuffer.size()) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        // Possible configuration error, too much data or/and data coming in too fast,
+        // requiring larger buffers
+        sif::warning << "TcpTmTcServer::handleServerOperation: "
+                "Reception buffer too small " << std::endl;
+#else
+        sif::printWarning("TcpTmTcServer::handleServerOperation: Reception buffer too small\n");
+#endif
+#endif
+        readAmount = receptionBuffer.size();
+    }
+    ringBuffer.readData(receptionBuffer.data(), readAmount, true);
+    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;
+            }
+        }
+        }
+        ringBuffer.deleteData(foundSize);
+        lastRingBufferSize = ringBuffer.getAvailableReadData();
+        std::memset(receptionBuffer.data() + startIdx, 0, foundSize);
+    }
+    return status;
+}
+
+void TcpTmTcServer::handleSocketError(ConstStorageAccessor &accessor) {
+    // Don't delete data
+    accessor.release();
+    auto socketError = getLastSocketError();
+    switch(socketError) {
+#if defined PLATFORM_WIN
+    case(WSAECONNRESET): {
+        // See https://docs.microsoft.com/en-us/windows/win32/api/winsock2/nf-winsock2-send
+        // Remote client might have shut down connection
+        return;
+    }
+#else
+    case(EPIPE): {
+        // See https://man7.org/linux/man-pages/man2/send.2.html
+        // Remote client might have shut down connection
+        return;
+    }
+#endif
+    default: {
+        tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::SEND_CALL);
+    }
+    }
+}

src/fsfw/osal/common/TcpTmTcServer.h

@@ -1,11 +1,13 @@
 #ifndef FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_
 #define FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_
 
+#include <fsfw/tmtcservices/SpacePacketParser.h>
 #include "TcpIpBase.h"
 
 #include "fsfw/platform.h"
 #include "fsfw/osal/common/tcpipHelpers.h"
 #include "fsfw/ipc/messageQueueDefinitions.h"
+#include "fsfw/container/SimpleRingBuffer.h"
 #include "fsfw/ipc/MessageQueueIF.h"
 #include "fsfw/objectmanager/frameworkObjects.h"
 #include "fsfw/objectmanager/SystemObject.h"
@@ -42,9 +44,37 @@ class TcpTmTcServer:
         public TcpIpBase,
         public ExecutableObjectIF {
 public:
+    enum class ReceptionModes {
+        SPACE_PACKETS
+    };
+
+    struct TcpConfig {
+    public:
+        TcpConfig(std::string tcpPort): tcpPort(tcpPort) {}
+
+        /**
+         * Passed to the recv call
+         */
+        int tcpFlags = 0;
+        int tcpBacklog = 3;
+
+        /**
+         * Passed to the select call which is used to ensure non-blocking TC reception
+         */
+        //uint32_t selectTimeoutMs = DEFAULT_SELECT_TIMEOUT_MS;
+        /**
+         * Passed to the send call
+         */
+        int tcpTmFlags = 0;
+
+        const std::string tcpPort;
+    };
+
     static const std::string DEFAULT_SERVER_PORT;
 
     static constexpr size_t ETHERNET_MTU_SIZE = 1500;
+    static constexpr size_t RING_BUFFER_SIZE = ETHERNET_MTU_SIZE * 3;
+    static constexpr uint32_t DEFAULT_LOOP_DELAY_MS = 200;
 
     /**
      * TCP Server Constructor
@@ -55,11 +85,19 @@ public:
      * @param customTcpServerPort   The user can specify another port than the default (7301) here.
      */
     TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
-            size_t receptionBufferSize = ETHERNET_MTU_SIZE + 1,
+            size_t receptionBufferSize = RING_BUFFER_SIZE,
-            std::string customTcpServerPort = "");
+            size_t ringBufferSize = RING_BUFFER_SIZE,
+            std::string customTcpServerPort = DEFAULT_SERVER_PORT,
+            ReceptionModes receptionMode = ReceptionModes::SPACE_PACKETS);
     virtual~ TcpTmTcServer();
 
-    void setTcpBacklog(uint8_t tcpBacklog);
+    /**
+     * Get a handle to the TCP configuration struct, which can be used to configure TCP
+     * properties
+     * @return
+     */
+    TcpConfig& getTcpConfigStruct();
+    void setSpacePacketParsingOptions(std::vector<uint16_t> validPacketIds);
 
     ReturnValue_t initialize() override;
     ReturnValue_t performOperation(uint8_t opCode) override;
@@ -71,25 +109,29 @@ protected:
     StorageManagerIF* tcStore = nullptr;
     StorageManagerIF* tmStore = nullptr;
 private:
+    static constexpr ReturnValue_t CONN_BROKEN = HasReturnvaluesIF::makeReturnCode(1, 0);
     //! TMTC bridge is cached.
     object_id_t tmtcBridgeId = objects::NO_OBJECT;
     TcpTmTcBridge* tmtcBridge = nullptr;
 
-    std::string tcpPort;
+    ReceptionModes receptionMode;
-    int tcpFlags = 0;
+    TcpConfig tcpConfig;
-    socket_t listenerTcpSocket = 0;
     struct sockaddr tcpAddress;
+    socket_t listenerTcpSocket = 0;
+
     MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE;
-    int tcpAddrLen = sizeof(tcpAddress);
-    int tcpBacklog = 3;
 
     std::vector<uint8_t> receptionBuffer;
-    int tcpSockOpt = 0;
+    SimpleRingBuffer ringBuffer;
-    int tcpTmFlags = 0;
+    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(size_t bytesRecvd);
+    ReturnValue_t handleTcReception(uint8_t* spacePacket, size_t packetSize);
-    ReturnValue_t handleTmSending(socket_t connSocket);
+    ReturnValue_t handleTmSending(socket_t connSocket, bool& tmSent);
+    ReturnValue_t handleTcRingBufferData(size_t availableReadData);
+    void handleSocketError(ConstStorageAccessor& accessor);
 };
 
 #endif /* FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_ */

src/fsfw/tmtcpacket/SpacePacket.h

@@ -19,7 +19,8 @@ public:
     /**
      * 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 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.
@@ -30,6 +31,16 @@ public:
      * 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.

src/fsfw/tmtcservices/SpacePacketParser.cpp

@@ -0,0 +1,77 @@
+#include <fsfw/serviceinterface/ServiceInterface.h>
+#include <fsfw/tmtcservices/SpacePacketParser.h>
+#include <algorithm>
+
+SpacePacketParser::SpacePacketParser(std::vector<uint16_t> validPacketIds):
+        validPacketIds(validPacketIds) {
+}
+
+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::warning << "SpacePacketParser::parseSpacePackets: Frame invalid" << std::endl;
+#else
+        sif::printWarning("SpacePacketParser::parseSpacePackets: Frame invalid\n");
+#endif
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    const uint8_t* bufPtr = *buffer;
+
+    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;
+    };
+
+    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

@@ -0,0 +1,96 @@
+#ifndef FRAMEWORK_TMTCSERVICES_PUSPARSER_H_
+#define FRAMEWORK_TMTCSERVICES_PUSPARSER_H_
+
+#include "fsfw/container/DynamicFIFO.h"
+#include "fsfw/returnvalues/FwClassIds.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 length field field of the space packets to find
+ * the respective space packet sizes.
+ *
+ * The parser parses a buffer by taking a pointer and the maximum size to scan.
+ * If space 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.
+ * @author   R. Mueller
+ */
+class SpacePacketParser {
+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);
+
+	/**
+	 * @brief   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.
+	 */
+	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* 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();
+
+	/**
+	 * Retrieve the next index and packet size pair from the FIFO.
+	 * This also removes 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.
+	 */
+	//DynamicFIFO<IndexSizePair> indexSizePairFIFO;
+
+	std::vector<uint16_t> validPacketIds;
+
+	//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_ */