diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 11ab71af8..7e6853fc1 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -1,13 +1,17 @@ -#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/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" @@ -18,19 +22,14 @@ #include #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() { @@ -41,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(objects::TC_STORE); if (tcStore == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -63,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; @@ -105,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; @@ -123,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; } @@ -144,51 +155,101 @@ 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); - if (retval > 0) { - handleTcReception(retval); + tcpConfig.tcpFlags + ); + if(retval == 0) { + size_t availableReadData = ringBuffer.getAvailableReadData(); + if(availableReadData > lastRingBufferSize) { + handleTcRingBufferData(availableReadData); + } + 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); @@ -196,21 +257,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); @@ -218,13 +284,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(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::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 c69160807..64726a30a 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 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 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 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_ */ 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"; } diff --git a/src/fsfw/returnvalues/FwClassIds.h b/src/fsfw/returnvalues/FwClassIds.h index 4a3a578a8..9fa0c9aea 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/tmtcpacket/SpacePacket.h b/src/fsfw/tmtcpacket/SpacePacket.h index 2957576f0..677ba0235 100644 --- a/src/fsfw/tmtcpacket/SpacePacket.h +++ b/src/fsfw/tmtcpacket/SpacePacket.h @@ -15,57 +15,78 @@ */ 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 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 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); +} + +} + #endif /* SPACEPACKET_H_ */ 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 diff --git a/src/fsfw/tmtcservices/SpacePacketParser.cpp b/src/fsfw/tmtcservices/SpacePacketParser.cpp new file mode 100644 index 000000000..3d442458d --- /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 += 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 000000000..bed3369b5 --- /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 +#include + +/** + * @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; + + 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 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 validPacketIds; +}; + +#endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */