diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5ff66a6d..6acd9770 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -13,6 +13,10 @@ option(FSFW_ADD_INTERNAL_TESTS "Add internal unit tests" ON)
 option(FSFW_ADD_UNITTESTS "Add regular unittests. Requires Catch2" OFF)
 option(FSFW_ADD_HAL "Add Hardware Abstraction Layer" ON)
 
+if(FSFW_ADD_UNITTESTS)
+    option(FSFW_CUSTOM_UNITTEST_RUNNER "Add FSFW custom main runner" ON)
+endif()
+
 # Optional sources
 option(FSFW_ADD_PUS "Compile with PUS sources" ON)
 option(FSFW_ADD_MONITORING "Compile with monitoring components" ON)
@@ -186,4 +190,4 @@ target_compile_options(${LIB_FSFW_NAME} PRIVATE
 
 target_link_libraries(${LIB_FSFW_NAME} PRIVATE
     ${FSFW_ADDITIONAL_LINK_LIBS}
-)
\ No newline at end of file
+)
diff --git a/src/fsfw/FSFW.h.in b/src/fsfw/FSFW.h.in
index 4da49b16..76c535ed 100644
--- a/src/fsfw/FSFW.h.in
+++ b/src/fsfw/FSFW.h.in
@@ -20,6 +20,14 @@
 #define FSFW_TCP_RECV_WIRETAPPING_ENABLED   0
 #endif
 
+#ifndef FSFW_USE_PUS_C_TELEMETRY
+#define FSFW_USE_PUS_C_TELEMETRY            1
+#endif
+
+#ifndef FSFW_USE_PUS_C_TELECOMMANDS
+#define FSFW_USE_PUS_C_TELECOMMANDS         1
+#endif
+
 // Can be used for low-level debugging of the SPI bus
 #ifndef FSFW_HAL_SPI_WIRETAPPING
 #define FSFW_HAL_SPI_WIRETAPPING            0
diff --git a/src/fsfw/container/SimpleRingBuffer.h b/src/fsfw/container/SimpleRingBuffer.h
index 6f31c5fb..dc0aeba8 100644
--- a/src/fsfw/container/SimpleRingBuffer.h
+++ b/src/fsfw/container/SimpleRingBuffer.h
@@ -5,8 +5,7 @@
 #include <cstddef>
 
 /**
- * @brief     Circular buffer implementation, useful for buffering
- *          into data streams.
+ * @brief     Circular buffer implementation, useful for buffering into data streams.
  * @details
  * Note that the deleteData() has to be called to increment the read pointer.
  * This class allocated dynamically, so
@@ -20,8 +19,8 @@ public:
      * @param size
      * @param overwriteOld If the ring buffer is overflowing at a write
      * operation, the oldest data will be overwritten.
-     * @param maxExcessBytes These additional bytes will be allocated in addtion
-     * to the specified size to accomodate contiguous write operations
+     * @param maxExcessBytes These additional bytes will be allocated in addition
+     * to the specified size to accommodate continuous write operations
      * with getFreeElement.
      *
      */
@@ -32,10 +31,10 @@ public:
      * @param buffer
      * @param size
      * @param overwriteOld
-     * If the ring buffer is overflowing at a write operartion, the oldest data
+     * If the ring buffer is overflowing at a write operation, the oldest data
      * will be overwritten.
      * @param maxExcessBytes
-     * If the buffer can accomodate additional bytes for contigous write
+     * If the buffer can accommodate additional bytes for contiguous write
      * operations with getFreeElement, this is the maximum allowed additional
      * size
      */
@@ -48,7 +47,7 @@ public:
      * Write to circular buffer and increment write pointer by amount.
      * @param data
      * @param amount
-     * @return -@c RETURN_OK if write operation was successfull
+     * @return -@c RETURN_OK if write operation was successful
      * -@c RETURN_FAILED if
      */
     ReturnValue_t writeData(const uint8_t* data, size_t amount);
@@ -108,7 +107,7 @@ public:
      * Delete data by incrementing read pointer.
      * @param amount
      * @param deleteRemaining
-     * If the amount specified is larger than the remaing size to read and this
+     * If the amount specified is larger than the remaining size to read and this
      * is set to true, the remaining amount will be deleted as well
      * @param trueAmount [out]
      * If deleteRemaining was set to true, the amount deleted will be assigned
diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp
index c819f9e7..8b34b1a3 100644
--- a/src/fsfw/osal/common/TcpTmTcServer.cpp
+++ b/src/fsfw/osal/common/TcpTmTcServer.cpp
@@ -5,11 +5,13 @@
 #include "TcpTmTcBridge.h"
 #include "tcpipHelpers.h"
 
+#include "fsfw/tmtcservices/SpacePacketParser.h"
+#include "fsfw/tasks/TaskFactory.h"
+#include "fsfw/globalfunctions/arrayprinter.h"
 #include "fsfw/container/SharedRingBuffer.h"
 #include "fsfw/ipc/MessageQueueSenderIF.h"
 #include "fsfw/ipc/MutexGuard.h"
 #include "fsfw/objectmanager/ObjectManager.h"
-
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/tmtcservices/TmTcMessage.h"
 
@@ -20,19 +22,14 @@
 #include <netdb.h>
 #endif
 
-#ifndef FSFW_TCP_RECV_WIRETAPPING_ENABLED
-#define FSFW_TCP_RECV_WIRETAPPING_ENABLED 0
-#endif
-
 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):
-        SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge),
-        tcpPort(customTcpServerPort), receptionBuffer(receptionBufferSize) {
-    if(tcpPort == "") {
-        tcpPort = DEFAULT_SERVER_PORT;
-    }
+        size_t receptionBufferSize, size_t ringBufferSize, std::string customTcpServerPort,
+        ReceptionModes receptionMode):
+        SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), receptionMode(receptionMode),
+        tcpConfig(customTcpServerPort), receptionBuffer(receptionBufferSize),
+        ringBuffer(ringBufferSize, true) {
 }
 
 ReturnValue_t TcpTmTcServer::initialize() {
@@ -43,6 +40,17 @@ ReturnValue_t TcpTmTcServer::initialize() {
         return result;
     }
 
+    switch(receptionMode) {
+    case(ReceptionModes::SPACE_PACKETS): {
+        spacePacketParser = new SpacePacketParser(validPacketIds);
+        if(spacePacketParser == nullptr) {
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+#if defined PLATFORM_UNIX
+        tcpConfig.tcpFlags |= MSG_DONTWAIT;
+#endif
+    }
+    }
     tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
     if (tcStore == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -65,7 +73,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;
@@ -107,7 +115,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;
@@ -125,11 +133,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;
 }
@@ -146,51 +155,98 @@ ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() {
     return HasReturnvaluesIF::RETURN_OK;
 }
 
-void TcpTmTcServer::handleServerOperation(socket_t connSocket) {
-    int retval = 0;
-    do {
-        // Read all telecommands sent by the client
-        retval = recv(connSocket,
+void TcpTmTcServer::handleServerOperation(socket_t& connSocket) {
+#if defined PLATFORM_WIN
+    setSocketNonBlocking(connSocket);
+#endif
+
+    while (true) {
+        int retval = recv(
+                connSocket,
                 reinterpret_cast<char*>(receptionBuffer.data()),
                 receptionBuffer.capacity(),
-                tcpFlags);
-        if (retval > 0) {
-            handleTcReception(retval);
+                tcpConfig.tcpFlags
+        );
+        if(retval == 0) {
+            // Client closed connection
+            return;
         }
-        else if(retval == 0) {
-            // Client has finished sending telecommands, send telemetry now
-            handleTmSending(connSocket);
+        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 {
-            // Should not happen
-            tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL);
+        else if(retval < 0)  {
+            int errorValue = getLastSocketError();
+#if defined PLATFORM_UNIX
+            int wouldBlockValue = EAGAIN;
+#elif defined PLATFORM_WIN
+            int wouldBlockValue = WSAEWOULDBLOCK;
+#endif
+            if(errorValue == wouldBlockValue) {
+                // 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(tcpConfig.tcpLoopDelay);
+                }
+            }
+            else {
+                tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL, 300);
+            }
         }
-    } while(retval > 0);
+    }
 }
 
-ReturnValue_t TcpTmTcServer::handleTcReception(size_t bytesRecvd) {
-#if FSFW_TCP_RECV_WIRETAPPING_ENABLED == 1
-    arrayprinter::print(receptionBuffer.data(), bytesRead);
+ReturnValue_t TcpTmTcServer::handleTcReception(uint8_t* spacePacket, size_t packetSize) {
+    if(wiretappingEnabled) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::info << "Received TC:" << std::endl;
+#else
+        sif::printInfo("Received TC:\n");
 #endif
+        arrayprinter::print(spacePacket, packetSize);
+    }
+
+    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 << "Packet size: " << bytesRecvd << std::endl;
+        sif::warning << "TcpTmTcServer::handleServerOperation: Data storage with packet size" <<
+                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);
 
-    result  = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
+    result = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
     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);
@@ -198,21 +254,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);
@@ -220,13 +281,134 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket) {
         if(result != HasReturnvaluesIF::RETURN_OK) {
             return result;
         }
+        if(wiretappingEnabled) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::info << "Sending TM:" << std::endl;
+#else
+            sif::printInfo("Sending TM:\n");
+#endif
+            arrayprinter::print(storeAccessor.data(), storeAccessor.size());
+        }
         int retval = send(connSocket,
                 reinterpret_cast<const char*>(storeAccessor.data()),
                 storeAccessor.size(),
-                tcpTmFlags);
-        if(retval != static_cast<int>(storeAccessor.size())) {
-            tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::SEND_CALL);
+                tcpConfig.tcpTmFlags);
+        if(retval == static_cast<int>(storeAccessor.size())) {
+            // 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::enableWiretapping(bool enable) {
+    this->wiretappingEnabled = enable;
+}
+
+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);
+    }
+    }
+}
+
+#if defined PLATFORM_WIN
+void TcpTmTcServer::setSocketNonBlocking(socket_t &connSocket) {
+    u_long iMode = 1;
+    int iResult = ioctlsocket(connSocket, FIONBIO, &iMode);
+    if(iResult != NO_ERROR) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "TcpTmTcServer::handleServerOperation: Setting socket"
+                " non-blocking failed with error " << iResult;
+#else
+        sif::printWarning("TcpTmTcServer::handleServerOperation: Setting socket"
+                " non-blocking failed with error %d\n", iResult);
+#endif
+#endif
+    }
+}
+#endif
diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h
index c6916080..64726a30 100644
--- a/src/fsfw/osal/common/TcpTmTcServer.h
+++ b/src/fsfw/osal/common/TcpTmTcServer.h
@@ -6,6 +6,7 @@
 #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"
@@ -20,6 +21,7 @@
 #include <vector>
 
 class TcpTmTcBridge;
+class SpacePacketParser;
 
 /**
  * @brief   TCP server implementation
@@ -42,9 +44,38 @@ 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;
+
+        /**
+         * If no telecommands packets are being received and no telemetry is being sent,
+         * the TCP server will delay periodically by this amount to decrease the CPU load
+         */
+        uint32_t tcpLoopDelay = DEFAULT_LOOP_DELAY_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 +86,21 @@ 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,
-            std::string customTcpServerPort = "");
+            size_t receptionBufferSize = RING_BUFFER_SIZE,
+            size_t ringBufferSize = RING_BUFFER_SIZE,
+            std::string customTcpServerPort = DEFAULT_SERVER_PORT,
+            ReceptionModes receptionMode = ReceptionModes::SPACE_PACKETS);
     virtual~ TcpTmTcServer();
 
-    void setTcpBacklog(uint8_t tcpBacklog);
+    void enableWiretapping(bool enable);
+
+    /**
+     * 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 +112,33 @@ 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;
+    bool wiretappingEnabled = false;
 
-    std::string tcpPort;
-    int tcpFlags = 0;
-    socket_t listenerTcpSocket = 0;
+    ReceptionModes receptionMode;
+    TcpConfig tcpConfig;
     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;
-    int tcpTmFlags = 0;
+    SimpleRingBuffer ringBuffer;
+    std::vector<uint16_t> validPacketIds;
+    SpacePacketParser* spacePacketParser = nullptr;
+    uint8_t lastRingBufferSize = 0;
 
-    void handleServerOperation(socket_t connSocket);
-    ReturnValue_t handleTcReception(size_t bytesRecvd);
-    ReturnValue_t handleTmSending(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);
+    void handleSocketError(ConstStorageAccessor& accessor);
+#if defined PLATFORM_WIN
+    void setSocketNonBlocking(socket_t& connSocket);
+#endif
 };
 
 #endif /* FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_ */
diff --git a/src/fsfw/osal/linux/CommandExecutor.cpp b/src/fsfw/osal/linux/CommandExecutor.cpp
index 5d131472..8216e591 100644
--- a/src/fsfw/osal/linux/CommandExecutor.cpp
+++ b/src/fsfw/osal/linux/CommandExecutor.cpp
@@ -62,8 +62,13 @@ ReturnValue_t CommandExecutor::close() {
 
 void CommandExecutor::printLastError(std::string funcName) const {
     if(lastError != 0) {
-        sif::error << funcName << " pclose failed with code " <<
-                lastError << ": " << strerror(lastError) << std::endl;
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << funcName << " pclose failed with code " << lastError << ": " <<
+                strerror(lastError) << std::endl;
+#else
+        sif::printError("%s pclose failed with code %d: %s\n",
+                funcName, lastError, strerror(lastError));
+#endif
     }
 }
 
@@ -98,15 +103,23 @@ ReturnValue_t CommandExecutor::check(bool& bytesRead) {
             ssize_t readBytes = read(currentFd, readVec.data(), readVec.size());
             if(readBytes == 0) {
                 // Should not happen
-                sif::warning << "CommandExecutor::check: "
-                        "No bytes read after poll event.." << std::endl;
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+                sif::warning << "CommandExecutor::check: No bytes read "
+                        "after poll event.." << std::endl;
+#else
+                sif::printWarning("CommandExecutor::check: No bytes read after poll event..\n");
+#endif
                 break;
             }
             else if(readBytes > 0) {
                 bytesRead = true;
                 if(printOutput) {
                     // It is assumed the command output is line terminated
+#if FSFW_CPP_OSTREAM_ENABLED == 1
                     sif::info << currentCmd << " | " << readVec.data();
+#else
+                    sif::printInfo("%s | %s", currentCmd, readVec.data());
+#endif
                 }
                 if(ringBuffer != nullptr) {
                     ringBuffer->writeData(reinterpret_cast<const uint8_t*>(
@@ -121,12 +134,20 @@ ReturnValue_t CommandExecutor::check(bool& bytesRead) {
             }
             else {
                 // Should also not happen
+#if FSFW_CPP_OSTREAM_ENABLED == 1
                 sif::warning << "CommandExecutor::check: Error " << errno << ": " <<
                         strerror(errno) << std::endl;
+#else
+                sif::printWarning("CommandExecutor::check: Error %d: %s\n", errno, strerror(errno));
+#endif
             }
         }
         else if(waiter.revents & POLLERR) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
             sif::warning << "CommandExecuter::check: Poll error" << std::endl;
+#else
+            sif::printWarning("CommandExecuter::check: Poll error\n");
+#endif
             return COMMAND_ERROR;
         }
         else if(waiter.revents & POLLHUP) {
@@ -165,7 +186,11 @@ ReturnValue_t CommandExecutor::executeBlocking() {
     while(fgets(readVec.data(), readVec.size(), currentCmdFile) != nullptr) {
         std::string output(readVec.data());
         if(printOutput) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
             sif::info << currentCmd << " | " << output;
+#else
+            sif::printInfo("%s | %s", currentCmd, output);
+#endif
         }
         if(ringBuffer != nullptr) {
             ringBuffer->writeData(reinterpret_cast<const uint8_t*>(output.data()), output.size());
diff --git a/src/fsfw/returnvalues/FwClassIds.h b/src/fsfw/returnvalues/FwClassIds.h
index 4a3a578a..9fa0c9ae 100644
--- a/src/fsfw/returnvalues/FwClassIds.h
+++ b/src/fsfw/returnvalues/FwClassIds.h
@@ -80,6 +80,7 @@ enum: uint8_t {
     FIXED_SLOT_TASK_IF, //FTIF
     MGM_LIS3MDL, //MGMLIS3
     MGM_RM3100, //MGMRM3100
+    SPACE_PACKET_PARSER, //SPPA
     FW_CLASS_ID_COUNT // [EXPORT] : [END]
 
 };
diff --git a/src/fsfw/tcdistribution/PUSDistributor.cpp b/src/fsfw/tcdistribution/PUSDistributor.cpp
index eec02429..1a5f713d 100644
--- a/src/fsfw/tcdistribution/PUSDistributor.cpp
+++ b/src/fsfw/tcdistribution/PUSDistributor.cpp
@@ -29,12 +29,31 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
         tcStatus = checker.checkPacket(currentPacket);
         if(tcStatus != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_VERBOSE_LEVEL >= 1
+            std::string keyword;
+            if(tcStatus == TcPacketCheck::INCORRECT_CHECKSUM) {
+                keyword = "checksum";
+            }
+            else if(tcStatus == TcPacketCheck::INCORRECT_PRIMARY_HEADER) {
+                keyword = "incorrect primary header";
+            }
+            else if(tcStatus == TcPacketCheck::ILLEGAL_APID) {
+                keyword = "illegal APID";
+            }
+            else if(tcStatus == TcPacketCheck::INCORRECT_SECONDARY_HEADER) {
+                keyword = "incorrect secondary header";
+            }
+            else if(tcStatus == TcPacketCheck::INCOMPLETE_PACKET) {
+                keyword = "incomplete packet";
+            }
+            else {
+                keyword = "unnamed error";
+            }
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::debug << "PUSDistributor::handlePacket: Packet format invalid, code " <<
-                    static_cast<int>(tcStatus) << std::endl;
+            sif::warning << "PUSDistributor::handlePacket: Packet format invalid, "
+                    << keyword << " error" << std::endl;
 #else
-            sif::printDebug("PUSDistributor::handlePacket: Packet format invalid, code %d\n",
-                    static_cast<int>(tcStatus));
+            sif::printWarning("PUSDistributor::handlePacket: Packet format invalid, "
+                    "%s error\n", keyword);
 #endif
 #endif
         }
diff --git a/src/fsfw/tmtcpacket/SpacePacket.h b/src/fsfw/tmtcpacket/SpacePacket.h
index 49dd5ae5..fe8a1044 100644
--- a/src/fsfw/tmtcpacket/SpacePacket.h
+++ b/src/fsfw/tmtcpacket/SpacePacket.h
@@ -15,56 +15,70 @@
  */
 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();
+
+    /**
+     * 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;
 };
 
+namespace spacepacket {
+
+constexpr uint16_t getTcSpacePacketIdFromApid(uint16_t apid) {
+    return (0x18 << 8) | (((apid >> 8) & 0x07) << 8) | (apid & 0x00ff);
+}
+
+constexpr uint16_t getTmSpacePacketIdFromApid(uint16_t apid) {
+    return (0x08 << 8) | (((apid >> 8) & 0x07) << 8) | (apid & 0x00ff);
+}
+
+}
+
 #endif /* SPACEPACKET_H_ */
diff --git a/src/fsfw/tmtcpacket/SpacePacketBase.cpp b/src/fsfw/tmtcpacket/SpacePacketBase.cpp
index cffdec9a..16883d7f 100644
--- a/src/fsfw/tmtcpacket/SpacePacketBase.cpp
+++ b/src/fsfw/tmtcpacket/SpacePacketBase.cpp
@@ -4,7 +4,7 @@
 #include <cstring>
 
 SpacePacketBase::SpacePacketBase(const uint8_t* setAddress) {
-	this->data = reinterpret_cast<SpacePacketPointer*>(const_cast<uint8_t*>(setAddress));
+    this->data = reinterpret_cast<SpacePacketPointer*>(const_cast<uint8_t*>(setAddress));
 }
 
 SpacePacketBase::~SpacePacketBase() {
@@ -12,11 +12,11 @@ SpacePacketBase::~SpacePacketBase() {
 
 //CCSDS Methods:
 uint8_t SpacePacketBase::getPacketVersionNumber( void ) {
-	return (this->data->header.packet_id_h & 0b11100000) >> 5;
+    return (this->data->header.packet_id_h & 0b11100000) >> 5;
 }
 
 ReturnValue_t SpacePacketBase::initSpacePacketHeader(bool isTelecommand,
-		bool hasSecondaryHeader, uint16_t apid, uint16_t sequenceCount) {
+        bool hasSecondaryHeader, uint16_t apid, uint16_t sequenceCount) {
     if(data == nullptr) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -28,41 +28,42 @@ ReturnValue_t SpacePacketBase::initSpacePacketHeader(bool isTelecommand,
 #endif
         return HasReturnvaluesIF::RETURN_FAILED;
     }
-	//reset header to zero:
-	memset(data, 0, sizeof(this->data->header) );
-	//Set TC/TM bit.
-	data->header.packet_id_h = ((isTelecommand? 1 : 0)) << 4;
-	//Set secondaryHeader bit
-	data->header.packet_id_h |= ((hasSecondaryHeader? 1 : 0)) << 3;
-	this->setAPID( apid );
-	//Always initialize as standalone packets.
-	data->header.sequence_control_h = 0b11000000;
-	setPacketSequenceCount(sequenceCount);
-	return HasReturnvaluesIF::RETURN_OK;
+    //reset header to zero:
+    memset(data, 0, sizeof(this->data->header) );
+    //Set TC/TM bit.
+    data->header.packet_id_h = ((isTelecommand? 1 : 0)) << 4;
+    //Set secondaryHeader bit
+    data->header.packet_id_h |= ((hasSecondaryHeader? 1 : 0)) << 3;
+    this->setAPID( apid );
+    //Always initialize as standalone packets.
+    data->header.sequence_control_h = 0b11000000;
+    setPacketSequenceCount(sequenceCount);
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 bool SpacePacketBase::isTelecommand( void ) {
-	return (this->data->header.packet_id_h & 0b00010000) >> 4;
+    return (this->data->header.packet_id_h & 0b00010000) >> 4;
 }
 
 bool SpacePacketBase::hasSecondaryHeader( void ) {
-	return (this->data->header.packet_id_h & 0b00001000) >> 3;
+    return (this->data->header.packet_id_h & 0b00001000) >> 3;
 }
 
 uint16_t SpacePacketBase::getPacketId() {
-	return ( (this->data->header.packet_id_h) << 8 ) +
-			this->data->header.packet_id_l;
+    return ( (this->data->header.packet_id_h) << 8 ) +
+            this->data->header.packet_id_l;
 }
 
 uint16_t SpacePacketBase::getAPID( void ) const {
-	return ( (this->data->header.packet_id_h & 0b00000111) << 8 ) +
-			this->data->header.packet_id_l;
+    return ( (this->data->header.packet_id_h & 0b00000111) << 8 ) +
+            this->data->header.packet_id_l;
 }
 
 void SpacePacketBase::setAPID( uint16_t new_apid ) {
-	//Use first three bits of new APID, but keep rest of packet id as it was (see specification).
-	this->data->header.packet_id_h = (this->data->header.packet_id_h & 0b11111000) | ( ( new_apid & 0x0700 ) >> 8 );
-	this->data->header.packet_id_l = ( new_apid & 0x00FF );
+    // Use first three bits of new APID, but keep rest of packet id as it was (see specification).
+    this->data->header.packet_id_h = (this->data->header.packet_id_h & 0b11111000) |
+            ( ( new_apid & 0x0700 ) >> 8 );
+    this->data->header.packet_id_l = ( new_apid & 0x00FF );
 }
 
 void SpacePacketBase::setSequenceFlags( uint8_t sequenceflags ) {
@@ -71,51 +72,52 @@ void SpacePacketBase::setSequenceFlags( uint8_t sequenceflags ) {
 }
 
 uint16_t SpacePacketBase::getPacketSequenceControl( void ) {
-	return ( (this->data->header.sequence_control_h) << 8 )
-		+ this->data->header.sequence_control_l;
+    return ( (this->data->header.sequence_control_h) << 8 )
+            + this->data->header.sequence_control_l;
 }
 
 uint8_t SpacePacketBase::getSequenceFlags( void ) {
-	return (this->data->header.sequence_control_h & 0b11000000) >> 6 ;
+    return (this->data->header.sequence_control_h & 0b11000000) >> 6 ;
 }
 
 uint16_t SpacePacketBase::getPacketSequenceCount( void ) const {
-	return ( (this->data->header.sequence_control_h & 0b00111111) << 8 )
-		+ this->data->header.sequence_control_l;
+    return ( (this->data->header.sequence_control_h & 0b00111111) << 8 )
+            + this->data->header.sequence_control_l;
 }
 
 void SpacePacketBase::setPacketSequenceCount( uint16_t new_count) {
-	this->data->header.sequence_control_h = ( this->data->header.sequence_control_h & 0b11000000 ) | ( ( (new_count%LIMIT_SEQUENCE_COUNT) & 0x3F00 ) >> 8 );
-	this->data->header.sequence_control_l = ( (new_count%LIMIT_SEQUENCE_COUNT) & 0x00FF );
+    this->data->header.sequence_control_h = ( this->data->header.sequence_control_h & 0b11000000 ) |
+            ( ( (new_count%LIMIT_SEQUENCE_COUNT) & 0x3F00 ) >> 8 );
+    this->data->header.sequence_control_l = ( (new_count%LIMIT_SEQUENCE_COUNT) & 0x00FF );
 }
 
 uint16_t SpacePacketBase::getPacketDataLength() const {
-	return ( (this->data->header.packet_length_h) << 8 )
-		+ this->data->header.packet_length_l;
+    return ( (this->data->header.packet_length_h) << 8 )
+            + this->data->header.packet_length_l;
 }
 
 void SpacePacketBase::setPacketDataLength( uint16_t new_length) {
-	this->data->header.packet_length_h = ( ( new_length & 0xFF00 ) >> 8 );
-	this->data->header.packet_length_l = ( new_length & 0x00FF );
+    this->data->header.packet_length_h = ( ( new_length & 0xFF00 ) >> 8 );
+    this->data->header.packet_length_l = ( new_length & 0x00FF );
 }
 
 size_t SpacePacketBase::getFullSize() {
-	//+1 is done because size in packet data length field is: size of data field -1
-	return this->getPacketDataLength() + sizeof(this->data->header) + 1;
+    // +1 is done because size in packet data length field is: size of data field -1
+    return this->getPacketDataLength() + sizeof(this->data->header) + 1;
 }
 
 uint8_t* SpacePacketBase::getWholeData() {
-	return (uint8_t*)this->data;
+    return (uint8_t*)this->data;
 }
 
 void SpacePacketBase::setData( const uint8_t* p_Data ) {
-	this->data = (SpacePacketPointer*)p_Data;
+    this->data = (SpacePacketPointer*)p_Data;
 }
 
 uint32_t SpacePacketBase::getApidAndSequenceCount() const {
-	return (getAPID() << 16) + getPacketSequenceCount();
+    return (getAPID() << 16) + getPacketSequenceCount();
 }
 
 uint8_t* SpacePacketBase::getPacketData() {
-	return &(data->packet_data);
+    return &(data->packet_data);
 }
diff --git a/src/fsfw/tmtcpacket/SpacePacketBase.h b/src/fsfw/tmtcpacket/SpacePacketBase.h
index fae64745..1ebc484f 100644
--- a/src/fsfw/tmtcpacket/SpacePacketBase.h
+++ b/src/fsfw/tmtcpacket/SpacePacketBase.h
@@ -22,8 +22,8 @@
  * @ingroup tmtcpackets
  */
 struct SpacePacketPointer {
-	CCSDSPrimaryHeader header;
-	uint8_t packet_data;
+    CCSDSPrimaryHeader header;
+    uint8_t packet_data;
 };
 
 /**
@@ -39,150 +39,151 @@ struct SpacePacketPointer {
  */
 class SpacePacketBase {
 protected:
-	/**
-	 * A pointer to a structure which defines the data structure of
-	 * the packet header.
-	 * To be hardware-safe, all elements are of byte size.
-	 */
-	SpacePacketPointer* data;
+    /**
+     * A pointer to a structure which defines the data structure of
+     * the packet header.
+     * To be hardware-safe, all elements are of byte size.
+     */
+    SpacePacketPointer* data;
 public:
-	static const uint16_t LIMIT_APID = 2048; //2^1
-	static const uint16_t LIMIT_SEQUENCE_COUNT = 16384; // 2^14
-	static const uint16_t APID_IDLE_PACKET = 0x7FF;
-	static const uint8_t TELECOMMAND_PACKET = 1;
-	static const uint8_t TELEMETRY_PACKET = 0;
-	/**
-	 * This definition defines the CRC size in byte.
-	 */
-	static const uint8_t CRC_SIZE = 2;
-	/**
-	 * This is the minimum size of a SpacePacket.
-	 */
-	static const uint16_t MINIMUM_SIZE = sizeof(CCSDSPrimaryHeader) + CRC_SIZE;
-	/**
-	 * This is the default constructor.
-	 * It sets its internal data pointer to the address passed.
-	 * @param set_address	The position where the packet data lies.
-	 */
-	SpacePacketBase( const uint8_t* set_address );
-	/**
-	 * No data is allocated, so the destructor is empty.
-	 */
-	virtual ~SpacePacketBase();
+    static const uint16_t LIMIT_APID = 2048; //2^1
+    static const uint16_t LIMIT_SEQUENCE_COUNT = 16384; // 2^14
+    static const uint16_t APID_IDLE_PACKET = 0x7FF;
+    static const uint8_t TELECOMMAND_PACKET = 1;
+    static const uint8_t TELEMETRY_PACKET = 0;
+    /**
+     * This definition defines the CRC size in byte.
+     */
+    static const uint8_t CRC_SIZE = 2;
+    /**
+     * This is the minimum size of a SpacePacket.
+     */
+    static const uint16_t MINIMUM_SIZE = sizeof(CCSDSPrimaryHeader) + CRC_SIZE;
+    /**
+     * This is the default constructor.
+     * It sets its internal data pointer to the address passed.
+     * @param set_address	The position where the packet data lies.
+     */
+    SpacePacketBase( const uint8_t* set_address );
+    /**
+     * No data is allocated, so the destructor is empty.
+     */
+    virtual ~SpacePacketBase();
 
-	//CCSDS Methods:
-	/**
-	 * Getter for the packet version number field.
-	 * @return Returns the highest three bit of the packet in one byte.
-	 */
-	uint8_t getPacketVersionNumber( void );
-	/**
-	 * This method checks the type field in the header.
-	 * This bit specifies, if the command is interpreted as Telecommand of
-	 * as Telemetry. For a Telecommand, the bit is set.
-	 * @return Returns true if the bit is set and false if not.
-	 */
-	bool isTelecommand( void );
+    //CCSDS Methods
 
-	ReturnValue_t initSpacePacketHeader(bool isTelecommand, bool hasSecondaryHeader,
-	        uint16_t apid, uint16_t sequenceCount = 0);
-	/**
-	 * The CCSDS header provides a secondary header flag (the fifth-highest bit),
-	 *  which is checked with this method.
-	 * @return	Returns true if the bit is set and false if not.
-	 */
-	bool hasSecondaryHeader( void );
-	/**
-	 * Returns the complete first two bytes of the packet, which together form
-	 * the CCSDS packet id.
-	 * @return	The CCSDS packet id.
-	 */
-	uint16_t getPacketId( void );
-	/**
-	 * Returns the APID of a packet, which are the lowest 11 bit of the packet
-	 * id.
-	 * @return The CCSDS APID.
-	 */
-	uint16_t getAPID( void ) const;
-	/**
-	 * Sets the APID of a packet, which are the lowest 11 bit of the packet
-	 * id.
-	 * @param 	The APID to set. The highest five bits of the parameter are
-	 * 			ignored.
-	 */
-	void setAPID( uint16_t setAPID );
+    /**
+     * Getter for the packet version number field.
+     * @return Returns the highest three bit of the packet in one byte.
+     */
+    uint8_t getPacketVersionNumber( void );
+    /**
+     * This method checks the type field in the header.
+     * This bit specifies, if the command is interpreted as Telecommand of
+     * as Telemetry. For a Telecommand, the bit is set.
+     * @return Returns true if the bit is set and false if not.
+     */
+    bool isTelecommand( void );
 
-	/**
+    ReturnValue_t initSpacePacketHeader(bool isTelecommand, bool hasSecondaryHeader,
+            uint16_t apid, uint16_t sequenceCount = 0);
+    /**
+     * The CCSDS header provides a secondary header flag (the fifth-highest bit),
+     *  which is checked with this method.
+     * @return	Returns true if the bit is set and false if not.
+     */
+    bool hasSecondaryHeader( void );
+    /**
+     * Returns the complete first two bytes of the packet, which together form
+     * the CCSDS packet id.
+     * @return	The CCSDS packet id.
+     */
+    uint16_t getPacketId( void );
+    /**
+     * Returns the APID of a packet, which are the lowest 11 bit of the packet
+     * id.
+     * @return The CCSDS APID.
+     */
+    uint16_t getAPID( void ) const;
+    /**
+     * Sets the APID of a packet, which are the lowest 11 bit of the packet
+     * id.
+     * @param 	The APID to set. The highest five bits of the parameter are
+     * 			ignored.
+     */
+    void setAPID( uint16_t setAPID );
+
+    /**
      * Sets the sequence flags of a packet, which are bit 17 and 18 in the space packet header.
      * @param   The sequence flags to set
      */
     void setSequenceFlags( uint8_t sequenceflags );
 
-	/**
-	 * Returns the CCSDS packet sequence control field, which are the third and
-	 * the fourth byte of the CCSDS primary header.
-	 * @return The CCSDS packet sequence control field.
-	 */
-	uint16_t getPacketSequenceControl( void );
-	/**
-	 * Returns the SequenceFlags, which are the highest two bit of the packet
-	 * sequence control field.
-	 * @return	The CCSDS sequence flags.
-	 */
-	uint8_t getSequenceFlags( void );
-	/**
-	 * Returns the packet sequence count, which are the lowest 14 bit of the
-	 * packet sequence control field.
-	 * @return The CCSDS sequence count.
-	 */
-	uint16_t getPacketSequenceCount( void ) const;
-	/**
-	 * Sets the packet sequence count, which are the lowest 14 bit of the
-	 * packet sequence control field.
-	 * setCount is modulo-divided by \c LIMIT_SEQUENCE_COUNT to avoid overflows.
-	 * @param setCount	The value to set the count to.
-	 */
-	void setPacketSequenceCount( uint16_t setCount );
-	/**
-	 * Returns the packet data length, which is the fifth and sixth byte of the
-	 * CCSDS Primary Header. The packet data length is the size of every kind
-	 * of data \b after the CCSDS Primary Header \b -1.
-	 * @return
-	 * The CCSDS packet data length. uint16_t is sufficient,
-	 * because this is limit in CCSDS standard
-	 */
-	uint16_t getPacketDataLength(void) const;
-	/**
-	 * Sets the packet data length, which is the fifth and sixth byte of the
-	 * CCSDS Primary Header.
-	 * @param setLength The value of the length to set. It must fit the true
-	 * 					CCSDS packet data length . The packet data length is
-	 * 					the size of every kind of data \b after the CCSDS
-	 * 					Primary Header \b -1.
-	 */
-	void setPacketDataLength( uint16_t setLength );
+    /**
+     * Returns the CCSDS packet sequence control field, which are the third and
+     * the fourth byte of the CCSDS primary header.
+     * @return The CCSDS packet sequence control field.
+     */
+    uint16_t getPacketSequenceControl( void );
+    /**
+     * Returns the SequenceFlags, which are the highest two bit of the packet
+     * sequence control field.
+     * @return	The CCSDS sequence flags.
+     */
+    uint8_t getSequenceFlags( void );
+    /**
+     * Returns the packet sequence count, which are the lowest 14 bit of the
+     * packet sequence control field.
+     * @return The CCSDS sequence count.
+     */
+    uint16_t getPacketSequenceCount( void ) const;
+    /**
+     * Sets the packet sequence count, which are the lowest 14 bit of the
+     * packet sequence control field.
+     * setCount is modulo-divided by \c LIMIT_SEQUENCE_COUNT to avoid overflows.
+     * @param setCount	The value to set the count to.
+     */
+    void setPacketSequenceCount( uint16_t setCount );
+    /**
+     * Returns the packet data length, which is the fifth and sixth byte of the
+     * CCSDS Primary Header. The packet data length is the size of every kind
+     * of data \b after the CCSDS Primary Header \b -1.
+     * @return
+     * The CCSDS packet data length. uint16_t is sufficient,
+     * because this is limit in CCSDS standard
+     */
+    uint16_t getPacketDataLength(void) const;
+    /**
+     * Sets the packet data length, which is the fifth and sixth byte of the
+     * CCSDS Primary Header.
+     * @param setLength The value of the length to set. It must fit the true
+     * 					CCSDS packet data length . The packet data length is
+     * 					the size of every kind of data \b after the CCSDS
+     * 					Primary Header \b -1.
+     */
+    void setPacketDataLength( uint16_t setLength );
 
-	//Helper methods:
-	/**
-	 * This method returns a raw uint8_t pointer to the packet.
-	 * @return	A \c uint8_t pointer to the first byte of the CCSDS primary header.
-	 */
-	virtual uint8_t* getWholeData( void );
+    // Helper methods
+    /**
+     * This method returns a raw uint8_t pointer to the packet.
+     * @return	A \c uint8_t pointer to the first byte of the CCSDS primary header.
+     */
+    virtual uint8_t* getWholeData( void );
 
-	uint8_t* getPacketData();
-	/**
-	 * With this method, the packet data pointer can be redirected to another
-	 * location.
-	 * @param p_Data A pointer to another raw Space Packet.
-	 */
-	virtual void setData( const uint8_t* p_Data );
-	/**
-	 * This method returns the full raw packet size.
-	 * @return	The full size of the packet in bytes.
-	 */
-	size_t getFullSize();
+    uint8_t* getPacketData();
+    /**
+     * With this method, the packet data pointer can be redirected to another
+     * location.
+     * @param p_Data A pointer to another raw Space Packet.
+     */
+    virtual void setData( const uint8_t* p_Data );
+    /**
+     * This method returns the full raw packet size.
+     * @return	The full size of the packet in bytes.
+     */
+    size_t getFullSize();
 
-	uint32_t getApidAndSequenceCount() const;
+    uint32_t getApidAndSequenceCount() const;
 
 };
 
diff --git a/src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp b/src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp
index 4a6e4d21..7f75407d 100644
--- a/src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp
+++ b/src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp
@@ -77,9 +77,9 @@ TmPacketStoredPusC::TmPacketStoredPusC(uint16_t apid, uint8_t service,
                     "%d too large\n", sizeToReserve);
             break;
         }
-#endif
 #endif
         }
+#endif
         TmPacketStoredBase::checkAndReportLostTm();
         return;
     }
diff --git a/src/fsfw/tmtcservices/CMakeLists.txt b/src/fsfw/tmtcservices/CMakeLists.txt
index c30af214..96cf99b5 100644
--- a/src/fsfw/tmtcservices/CMakeLists.txt
+++ b/src/fsfw/tmtcservices/CMakeLists.txt
@@ -6,4 +6,5 @@ target_sources(${LIB_FSFW_NAME}
 		TmTcBridge.cpp
 		TmTcMessage.cpp
 		VerificationReporter.cpp
+		SpacePacketParser.cpp
 )
\ No newline at end of file
diff --git a/src/fsfw/tmtcservices/SpacePacketParser.cpp b/src/fsfw/tmtcservices/SpacePacketParser.cpp
new file mode 100644
index 00000000..3d442458
--- /dev/null
+++ b/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 += idx + 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);
+    }
+}
diff --git a/src/fsfw/tmtcservices/SpacePacketParser.h b/src/fsfw/tmtcservices/SpacePacketParser.h
new file mode 100644
index 00000000..bed3369b
--- /dev/null
+++ b/src/fsfw/tmtcservices/SpacePacketParser.h
@@ -0,0 +1,78 @@
+#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 space packets.
+ * 			Can be used if space packets are serialized in a tightly packed frame.
+ * @details
+ * The parser uses the length field field and the 16-bit TC packet ID of the space packets to find
+ * find space packets in a given data stream
+ * @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::SPACE_PACKET_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 validPacketIds    This vector contains the allowed 16-bit TC packet ID start markers
+	 * The parser will search for these stark markers to detect the start of a space packet.
+	 * It is also possible to pass an empty vector here, but this is not recommended.
+	 * If an empty vector is passed, the parser will assume that the start of the given stream
+	 * contains the start of a new space packet.
+	 */
+	SpacePacketParser(std::vector<uint16_t> validPacketIds);
+
+    /**
+     * Parse a given frame for space packets but also increment the given buffer and assign the
+     * total number of bytes read so far
+     * @param buffer        Parser will look for space packets in this buffer
+     * @param maxSize       Maximum size of the buffer
+     * @param startIndex    Start index of a found space packet
+     * @param foundSize     Found size of the space packet
+     * @param readLen       Length read so far. This value is incremented by the number of parsed
+     *                      bytes which also includes the size of a found packet
+     *  -@c NO_PACKET_FOUND if no packet was found in the given buffer or the length field is
+     *      invalid. foundSize will be set to the size of the space packet header. buffer and
+     *      readLen will be incremented accordingly.
+     *  -@c SPLIT_PACKET if a packet was found but the detected size exceeds maxSize. foundSize
+     *      will be set to the detected packet size and startIndex will be set to the start of the
+     *      detected packet. buffer and read length will not be incremented but the found length
+     *      will be assigned.
+     *  -@c RETURN_OK if a packet was found
+     */
+    ReturnValue_t parseSpacePackets(const uint8_t **buffer, const size_t maxSize,
+            size_t& startIndex, size_t& foundSize, size_t& readLen);
+
+	/**
+	 * Parse a given frame for space packets
+	 * @param buffer        Parser will look for space packets in this buffer
+	 * @param maxSize       Maximum size of the buffer
+	 * @param startIndex    Start index of a found space packet
+	 * @param foundSize     Found size of the space packet
+	 *  -@c NO_PACKET_FOUND if no packet was found in the given buffer or the length field is
+	 *      invalid. foundSize will be set to the size of the space packet header
+	 *  -@c SPLIT_PACKET if a packet was found but the detected size exceeds maxSize. foundSize
+	 *      will be set to the detected packet size and startIndex will be set to the start of the
+	 *      detected packet
+	 *  -@c RETURN_OK if a packet was found
+	 */
+	ReturnValue_t parseSpacePackets(const uint8_t* buffer, const size_t maxSize,
+	        size_t& startIndex, size_t& foundSize);
+private:
+	std::vector<uint16_t> validPacketIds;
+};
+
+#endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */
diff --git a/tests/user/testcfg/CMakeLists.txt b/tests/user/testcfg/CMakeLists.txt
index dbf0256f..46808ec4 100644
--- a/tests/user/testcfg/CMakeLists.txt
+++ b/tests/user/testcfg/CMakeLists.txt
@@ -1,11 +1,9 @@
-target_sources(${TARGET_NAME} 
-	PRIVATE
-		ipc/MissionMessageTypes.cpp
-		pollingsequence/PollingSequenceFactory.cpp
+target_sources(${TARGET_NAME} PRIVATE
+    ipc/MissionMessageTypes.cpp
+    pollingsequence/PollingSequenceFactory.cpp
 )
 
 # Add include paths for the executable
-target_include_directories(${TARGET_NAME} 
-	PUBLIC 
-		${CMAKE_CURRENT_SOURCE_DIR}
-)
+target_include_directories(${TARGET_NAME} PUBLIC
+    ${CMAKE_CURRENT_SOURCE_DIR}
+)
\ No newline at end of file