From 09299802f0cc4456c510f6d6dac3260b96c08777 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 15:01:01 +0200 Subject: [PATCH 01/15] 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 | 229 ++++++++++++++++---- src/fsfw/osal/common/TcpTmTcServer.h | 68 ++++-- src/fsfw/tmtcpacket/SpacePacket.h | 109 +++++----- src/fsfw/tmtcservices/SpacePacketParser.cpp | 77 +++++++ src/fsfw/tmtcservices/SpacePacketParser.h | 96 ++++++++ 5 files changed, 473 insertions(+), 106 deletions(-) create mode 100644 src/fsfw/tmtcservices/SpacePacketParser.cpp create mode 100644 src/fsfw/tmtcservices/SpacePacketParser.h diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 11ab71af8..e320b46ba 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/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 #endif +#include #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): - 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), 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(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,51 +158,82 @@ 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) { + while (true) { + int retval = recv( + connSocket, reinterpret_cast(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) { + 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 { + 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 << "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); @@ -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 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(storeAccessor.data()), storeAccessor.size(), - tcpTmFlags); - if(retval != static_cast(storeAccessor.size())) { - tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::SEND_CALL); + tcpConfig.tcpTmFlags); + if(retval == static_cast(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::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); + } + } +} diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h index c69160807..a0a31655e 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/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 #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, - 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); + /** + * Get a handle to the TCP configuration struct, which can be used to configure TCP + * properties + * @return + */ + TcpConfig& getTcpConfigStruct(); + void setSpacePacketParsingOptions(std::vector 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; - 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 receptionBuffer; - int tcpSockOpt = 0; - int tcpTmFlags = 0; + SimpleRingBuffer ringBuffer; + std::vector 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); }; #endif /* FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_ */ diff --git a/src/fsfw/tmtcpacket/SpacePacket.h b/src/fsfw/tmtcpacket/SpacePacket.h index 49dd5ae52..9eec87a89 100644 --- a/src/fsfw/tmtcpacket/SpacePacket.h +++ b/src/fsfw/tmtcpacket/SpacePacket.h @@ -15,56 +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_ */ diff --git a/src/fsfw/tmtcservices/SpacePacketParser.cpp b/src/fsfw/tmtcservices/SpacePacketParser.cpp new file mode 100644 index 000000000..84f861cfa --- /dev/null +++ b/src/fsfw/tmtcservices/SpacePacketParser.cpp @@ -0,0 +1,77 @@ +#include +#include +#include + +SpacePacketParser::SpacePacketParser(std::vector 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); + } +} diff --git a/src/fsfw/tmtcservices/SpacePacketParser.h b/src/fsfw/tmtcservices/SpacePacketParser.h new file mode 100644 index 000000000..16b53ea4e --- /dev/null +++ b/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 +#include + +/** + * @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; + + 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 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& 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 indexSizePairFIFO; + + std::vector 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_ */ From e536918804255bf035ba78e5ddb9171b01f02761 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 15:09:56 +0200 Subject: [PATCH 02/15] wiretapping in runtime config now --- src/fsfw/osal/common/TcpTmTcServer.cpp | 24 +++++++++++++++--------- src/fsfw/osal/common/TcpTmTcServer.h | 5 ++++- 2 files changed, 19 insertions(+), 10 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index e320b46ba..5be9373eb 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -5,12 +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,11 +21,6 @@ #elif defined(PLATFORM_UNIX) #include #endif -#include - -#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; @@ -202,9 +198,11 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { } ReturnValue_t TcpTmTcServer::handleTcReception(uint8_t* spacePacket, size_t packetSize) { -#if FSFW_TCP_RECV_WIRETAPPING_ENABLED == 1 - arrayprinter::print(receptionBuffer.data(), bytesRead); -#endif + if(wiretappingEnabled) { + sif::info << "Received TC:" << std::endl; + arrayprinter::print(spacePacket, packetSize); + } + if(spacePacket == nullptr or packetSize == 0) { return HasReturnvaluesIF::RETURN_FAILED; } @@ -268,6 +266,10 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent) if(result != HasReturnvaluesIF::RETURN_OK) { return result; } + if(wiretappingEnabled) { + sif::info << "Sending TM:" << std::endl; + arrayprinter::print(storeAccessor.data(), storeAccessor.size()); + } int retval = send(connSocket, reinterpret_cast(storeAccessor.data()), storeAccessor.size(), @@ -346,6 +348,10 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) { return status; } +void TcpTmTcServer::enableWiretapping(bool enable) { + this->wiretappingEnabled = enable; +} + void TcpTmTcServer::handleSocketError(ConstStorageAccessor &accessor) { // Don't delete data accessor.release(); diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h index a0a31655e..d52148483 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/src/fsfw/osal/common/TcpTmTcServer.h @@ -1,7 +1,6 @@ #ifndef FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_ #define FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_ -#include #include "TcpIpBase.h" #include "fsfw/platform.h" @@ -22,6 +21,7 @@ #include class TcpTmTcBridge; +class SpacePacketParser; /** * @brief TCP server implementation @@ -91,6 +91,8 @@ public: ReceptionModes receptionMode = ReceptionModes::SPACE_PACKETS); virtual~ TcpTmTcServer(); + void enableWiretapping(bool enable); + /** * Get a handle to the TCP configuration struct, which can be used to configure TCP * properties @@ -113,6 +115,7 @@ private: //! TMTC bridge is cached. object_id_t tmtcBridgeId = objects::NO_OBJECT; TcpTmTcBridge* tmtcBridge = nullptr; + bool wiretappingEnabled = false; ReceptionModes receptionMode; TcpConfig tcpConfig; From bbea5e33bc66d05589d6143c0e75931acd99de83 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 15:12:23 +0200 Subject: [PATCH 03/15] removed obsolete empty ctor --- src/fsfw/osal/common/TcpTmTcServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 5be9373eb..fb421fc7d 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -29,7 +29,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), validPacketIds() { + ringBuffer(ringBufferSize, true) { } ReturnValue_t TcpTmTcServer::initialize() { From f02852d8d2bf40b2941ccd0816c1ed01d703fea7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 15:13:46 +0200 Subject: [PATCH 04/15] cmake lists file update --- src/fsfw/tmtcservices/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw/tmtcservices/CMakeLists.txt b/src/fsfw/tmtcservices/CMakeLists.txt index c30af214f..96cf99b57 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 From a4d6421510575b9eda7f27c1637963c91881f922 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 15:30:01 +0200 Subject: [PATCH 05/15] documentation and bugfixes --- src/fsfw/tmtcservices/SpacePacketParser.cpp | 2 +- src/fsfw/tmtcservices/SpacePacketParser.h | 100 ++++++++------------ 2 files changed, 42 insertions(+), 60 deletions(-) diff --git a/src/fsfw/tmtcservices/SpacePacketParser.cpp b/src/fsfw/tmtcservices/SpacePacketParser.cpp index 84f861cfa..3d442458d 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.cpp +++ b/src/fsfw/tmtcservices/SpacePacketParser.cpp @@ -44,7 +44,7 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t **buffer, const foundSize = packetSize; } *buffer += foundSize; - readLen += foundSize; + readLen += idx + foundSize; return result; }; diff --git a/src/fsfw/tmtcservices/SpacePacketParser.h b/src/fsfw/tmtcservices/SpacePacketParser.h index 16b53ea4e..82b150106 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.h +++ b/src/fsfw/tmtcservices/SpacePacketParser.h @@ -8,19 +8,11 @@ #include /** - * @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. + * @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 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. + * 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 { @@ -35,62 +27,52 @@ public: /** * @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. + * @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 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 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 + * 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); - - 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& 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 indexSizePairFIFO; - std::vector 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_ */ From 1622e23f1c15ddcf1d4b522c332c1ab156f91fd9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 15:32:58 +0200 Subject: [PATCH 06/15] delay configurable --- src/fsfw/osal/common/TcpTmTcServer.cpp | 2 +- src/fsfw/osal/common/TcpTmTcServer.h | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index fb421fc7d..c39361462 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -187,7 +187,7 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { return; } if(not tcAvailable and not tmSent) { - TaskFactory::delayTask(DEFAULT_LOOP_DELAY_MS); + TaskFactory::delayTask(tcpConfig.tcpLoopDelay); } } else { diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h index d52148483..da0e8bd5f 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/src/fsfw/osal/common/TcpTmTcServer.h @@ -59,9 +59,10 @@ public: int tcpBacklog = 3; /** - * Passed to the select call which is used to ensure non-blocking TC reception + * 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 selectTimeoutMs = DEFAULT_SELECT_TIMEOUT_MS; + uint32_t tcpLoopDelay = DEFAULT_LOOP_DELAY_MS ; /** * Passed to the send call */ From bf02f14772184394c51c500ccc6dc312839de746 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 16:31:53 +0200 Subject: [PATCH 07/15] C++11 adaptions --- src/fsfw/tmtcpacket/SpacePacket.h | 17 ++++++++--------- src/fsfw/tmtcservices/SpacePacketParser.h | 2 +- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/fsfw/tmtcpacket/SpacePacket.h b/src/fsfw/tmtcpacket/SpacePacket.h index 9eec87a89..67746859e 100644 --- a/src/fsfw/tmtcpacket/SpacePacket.h +++ b/src/fsfw/tmtcpacket/SpacePacket.h @@ -32,15 +32,6 @@ public: */ 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. @@ -78,4 +69,12 @@ protected: SpacePacketData localData; }; +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/tmtcservices/SpacePacketParser.h b/src/fsfw/tmtcservices/SpacePacketParser.h index 82b150106..bed3369b5 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.h +++ b/src/fsfw/tmtcservices/SpacePacketParser.h @@ -21,7 +21,7 @@ public: //! is the size of the PUS packet starting at that index. using IndexSizePair = std::pair; - static constexpr uint8_t INTERFACE_ID = CLASS_ID::PUS_PARSER; + 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); From 936bac5abdd6e56e8f3be85b624620948fecf714 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 16:32:43 +0200 Subject: [PATCH 08/15] class id renamed --- src/fsfw/returnvalues/FwClassIds.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw/returnvalues/FwClassIds.h b/src/fsfw/returnvalues/FwClassIds.h index cdbf5657c..337709ed6 100644 --- a/src/fsfw/returnvalues/FwClassIds.h +++ b/src/fsfw/returnvalues/FwClassIds.h @@ -78,6 +78,7 @@ enum: uint8_t { HAL_GPIO, //HGIO MGM_LIS3MDL, //MGMLIS3 MGM_RM3100, //MGMRM3100 + SPACE_PACKET_PARSER, //SPPA FW_CLASS_ID_COUNT // [EXPORT] : [END] }; From 32b5060c626261bd2729210f318bcc127d0117c3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 17:26:02 +0200 Subject: [PATCH 09/15] all windows fixes --- src/fsfw/osal/common/TcpTmTcServer.cpp | 158 +++++++++++++++++++++++-- src/fsfw/osal/common/TcpTmTcServer.h | 17 ++- 2 files changed, 162 insertions(+), 13 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 11ab71af8..b22f4412f 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -41,6 +41,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(objects::TC_STORE); if (tcStore == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -144,11 +155,14 @@ 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(receptionBuffer.data()), receptionBuffer.capacity(), tcpFlags); @@ -159,9 +173,34 @@ void TcpTmTcServer::handleServerOperation(socket_t connSocket) { // Client has finished sending telecommands, send telemetry now handleTmSending(connSocket); } - else { - // Should not happen - tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL); + else if(retval < 0) { + int errorValue = GetLastError(); +#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); } @@ -228,3 +267,106 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket) { } 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); + } + } +} + +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 + } +} diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h index c69160807..2104d8590 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/src/fsfw/osal/common/TcpTmTcServer.h @@ -84,12 +84,19 @@ private: int tcpBacklog = 3; std::vector receptionBuffer; - int tcpSockOpt = 0; - int tcpTmFlags = 0; + SimpleRingBuffer ringBuffer; + std::vector 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_ */ From b1a9c90087bad07c81f9f96594edf7e2fa51d077 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 17:38:35 +0200 Subject: [PATCH 10/15] this should work for both OSes --- src/fsfw/osal/common/TcpTmTcServer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 4348e21e3..519df547d 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -177,7 +177,7 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { ringBuffer.writeData(receptionBuffer.data(), retval); } else if(retval < 0) { - int errorValue = GetLastError(); + int errorValue = getLastSocketError(); #if defined PLATFORM_UNIX int wouldBlockValue = EAGAIN; #elif defined PLATFORM_WIN @@ -387,6 +387,7 @@ void TcpTmTcServer::handleSocketError(ConstStorageAccessor &accessor) { } } +#if defined PLATFORM_WIN void TcpTmTcServer::setSocketNonBlocking(socket_t &connSocket) { u_long iMode = 1; int iResult = ioctlsocket(connSocket, FIONBIO, &iMode); @@ -402,3 +403,4 @@ void TcpTmTcServer::setSocketNonBlocking(socket_t &connSocket) { #endif } } +#endif From cffd77ed325aa24bef3f22bc85dbd573c2db362e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 17:42:29 +0200 Subject: [PATCH 11/15] put functions in namespace --- src/fsfw/tmtcpacket/SpacePacket.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/fsfw/tmtcpacket/SpacePacket.h b/src/fsfw/tmtcpacket/SpacePacket.h index 67746859e..fe8a1044c 100644 --- a/src/fsfw/tmtcpacket/SpacePacket.h +++ b/src/fsfw/tmtcpacket/SpacePacket.h @@ -69,6 +69,8 @@ protected: SpacePacketData localData; }; +namespace spacepacket { + constexpr uint16_t getTcSpacePacketIdFromApid(uint16_t apid) { return (0x18 << 8) | (((apid >> 8) & 0x07) << 8) | (apid & 0x00ff); } @@ -77,4 +79,6 @@ constexpr uint16_t getTmSpacePacketIdFromApid(uint16_t apid) { return (0x08 << 8) | (((apid >> 8) & 0x07) << 8) | (apid & 0x00ff); } +} + #endif /* SPACEPACKET_H_ */ From 0a6a32a13003103ee6462d1c46f814e0c44ab07b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 29 Sep 2021 11:45:20 +0200 Subject: [PATCH 12/15] printout separation --- src/fsfw/osal/common/TcpTmTcServer.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 519df547d..8b34b1a3b 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -210,7 +210,11 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { 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); } @@ -278,7 +282,11 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent) 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, From 6ad7f51297bff44f07543c3875481388460a8179 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 8 Oct 2021 13:24:51 +0200 Subject: [PATCH 13/15] added bind call error string --- src/fsfw/osal/common/tcpipCommon.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/fsfw/osal/common/tcpipCommon.cpp b/src/fsfw/osal/common/tcpipCommon.cpp index 0fdbf867a..2551496df 100644 --- a/src/fsfw/osal/common/tcpipCommon.cpp +++ b/src/fsfw/osal/common/tcpipCommon.cpp @@ -21,6 +21,9 @@ void tcpip::determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std: if(errorSrc == ErrorSources::SETSOCKOPT_CALL) { srcString = "setsockopt call"; } + if(errorSrc == ErrorSources::BIND_CALL) { + srcString = "bind call"; + } else if(errorSrc == ErrorSources::SOCKET_CALL) { srcString = "socket call"; } From 9bcd701a50151491d3fe1b7d1c4d652556266cbd Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 8 Oct 2021 13:51:32 +0200 Subject: [PATCH 14/15] tcp server also parses TCs when client closes connection --- src/fsfw/osal/common/TcpTmTcServer.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 8b34b1a3b..7e6853fc1 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -168,7 +168,10 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { tcpConfig.tcpFlags ); if(retval == 0) { - // Client closed connection + size_t availableReadData = ringBuffer.getAvailableReadData(); + if(availableReadData > lastRingBufferSize) { + handleTcRingBufferData(availableReadData); + } return; } else if(retval > 0) { From a077a1b5877032bedabe931ecdbc1a1048c3e333 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 18 Oct 2021 15:07:00 +0200 Subject: [PATCH 15/15] improved constexpr macros --- src/fsfw/tmtcpacket/SpacePacket.h | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/fsfw/tmtcpacket/SpacePacket.h b/src/fsfw/tmtcpacket/SpacePacket.h index dad8b95ea..677ba0235 100644 --- a/src/fsfw/tmtcpacket/SpacePacket.h +++ b/src/fsfw/tmtcpacket/SpacePacket.h @@ -71,12 +71,20 @@ protected: namespace spacepacket { -constexpr uint16_t getTcSpacePacketIdFromApid(uint16_t apid) { - return (0x18 << 8) | (((apid >> 8) & 0x07) << 8) | (apid & 0x00ff); +constexpr uint16_t getSpacePacketIdFromApid(bool isTc, uint16_t apid, + bool secondaryHeaderFlag = true) { + return (((isTc << 5) & 0x10) | ((secondaryHeaderFlag << 4) & 0x08) | + ((apid >> 8) & 0x07)) << 8 | (apid & 0x00ff); } -constexpr uint16_t getTmSpacePacketIdFromApid(uint16_t apid) { - return (0x08 << 8) | (((apid >> 8) & 0x07) << 8) | (apid & 0x00ff); +constexpr uint16_t getTcSpacketIdFromApid(uint16_t apid, + bool secondaryHeaderFlag = true) { + return getSpacePacketIdFromApid(true, apid, secondaryHeaderFlag); +} + +constexpr uint16_t getTmSpacketIdFromApid(uint16_t apid, + bool secondaryHeaderFlag = true) { + return getSpacePacketIdFromApid(false, apid, secondaryHeaderFlag); } }