From ba6cbde28afaf55ff7689db1a18b13793c051d6a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 27 Sep 2021 11:05:26 +0200 Subject: [PATCH 01/24] Increased TM stack robustness 1. More nullptr check 2. returnvalue for inititalize function which can fail --- src/fsfw/tmtcpacket/SpacePacket.h | 3 +- src/fsfw/tmtcpacket/SpacePacketBase.cpp | 102 ++++--- src/fsfw/tmtcpacket/SpacePacketBase.h | 276 +++++++++--------- src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.cpp | 8 +- src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.h | 2 +- .../tmtcpacket/pus/tm/TmPacketStoredPusC.cpp | 44 ++- 6 files changed, 248 insertions(+), 187 deletions(-) diff --git a/src/fsfw/tmtcpacket/SpacePacket.h b/src/fsfw/tmtcpacket/SpacePacket.h index 49dd5ae5..2957576f 100644 --- a/src/fsfw/tmtcpacket/SpacePacket.h +++ b/src/fsfw/tmtcpacket/SpacePacket.h @@ -19,7 +19,8 @@ public: /** * The constructor initializes the packet and sets all header information * according to the passed parameters. - * @param packetDataLength Sets the packet data length field and therefore specifies the size of the packet. + * @param packetDataLength Sets the packet data length field and therefore specifies + * the size of the packet. * @param isTelecommand Sets the packet type field to either TC (true) or TM (false). * @param apid Sets the packet's APID field. The default value describes an idle packet. * @param sequenceCount ets the packet's Source Sequence Count field. diff --git a/src/fsfw/tmtcpacket/SpacePacketBase.cpp b/src/fsfw/tmtcpacket/SpacePacketBase.cpp index e9a0b836..16883d7f 100644 --- a/src/fsfw/tmtcpacket/SpacePacketBase.cpp +++ b/src/fsfw/tmtcpacket/SpacePacketBase.cpp @@ -3,8 +3,8 @@ #include -SpacePacketBase::SpacePacketBase( const uint8_t* set_address ) { - this->data = (SpacePacketPointer*) set_address; +SpacePacketBase::SpacePacketBase(const uint8_t* setAddress) { + this->data = reinterpret_cast(const_cast(setAddress)); } SpacePacketBase::~SpacePacketBase() { @@ -12,94 +12,112 @@ 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; } -void SpacePacketBase::initSpacePacketHeader(bool isTelecommand, - bool hasSecondaryHeader, uint16_t apid, uint16_t sequenceCount) { - //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); - +ReturnValue_t SpacePacketBase::initSpacePacketHeader(bool isTelecommand, + bool hasSecondaryHeader, uint16_t apid, uint16_t sequenceCount) { + if(data == nullptr) { +#if FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "SpacePacketBase::initSpacePacketHeader: Data pointer is invalid" + << std::endl; +#else + sif::printWarning("SpacePacketBase::initSpacePacketHeader: Data pointer is invalid!\n"); +#endif +#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; } 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 ) { + this->data->header.sequence_control_h &= 0x3F; + this->data->header.sequence_control_h |= sequenceflags << 6; } 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 13cb3130..1ebc484f 100644 --- a/src/fsfw/tmtcpacket/SpacePacketBase.h +++ b/src/fsfw/tmtcpacket/SpacePacketBase.h @@ -2,6 +2,8 @@ #define FSFW_TMTCPACKET_SPACEPACKETBASE_H_ #include "ccsds_header.h" +#include "fsfw/returnvalues/HasReturnvaluesIF.h" + #include /** @@ -20,8 +22,8 @@ * @ingroup tmtcpackets */ struct SpacePacketPointer { - CCSDSPrimaryHeader header; - uint8_t packet_data; + CCSDSPrimaryHeader header; + uint8_t packet_data; }; /** @@ -37,143 +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 - void 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 ); - /** - * 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 ); + /** + * 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 ); - //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 ); + 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 ); - 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(); + /** + * 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 ); - uint32_t getApidAndSequenceCount() const; + /** + * 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 ); + + 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; }; diff --git a/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.cpp b/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.cpp index ea25f5d2..2c6e1d97 100644 --- a/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.cpp +++ b/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.cpp @@ -53,11 +53,14 @@ uint8_t* TmPacketPusC::getPacketTimeRaw() const{ } -void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service, +ReturnValue_t TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice, uint16_t packetSubcounter, uint16_t destinationId, uint8_t timeRefField) { //Set primary header: - initSpacePacketHeader(false, true, apid); + ReturnValue_t result = initSpacePacketHeader(false, true, apid); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } //Set data Field Header: //First, set to zero. memset(&tmData->dataField, 0, sizeof(tmData->dataField)); @@ -76,6 +79,7 @@ void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service, timeStamper->addTimeStamp(tmData->dataField.time, sizeof(tmData->dataField.time)); } + return HasReturnvaluesIF::RETURN_OK; } void TmPacketPusC::setSourceDataSize(uint16_t size) { diff --git a/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.h b/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.h index fe373c6f..3a9be132 100644 --- a/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.h +++ b/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.h @@ -100,7 +100,7 @@ protected: * @param subservice PUS Subservice * @param packetSubcounter Additional subcounter used. */ - void initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice, + ReturnValue_t initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice, uint16_t packetSubcounter, uint16_t destinationId = 0, uint8_t timeRefField = 0); /** diff --git a/src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp b/src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp index add4f4b9..4a6e4d21 100644 --- a/src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp +++ b/src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp @@ -43,27 +43,55 @@ TmPacketStoredPusC::TmPacketStoredPusC(uint16_t apid, uint8_t service, return; } size_t sourceDataSize = 0; - if (content != NULL) { + if (content != nullptr) { sourceDataSize += content->getSerializedSize(); } - if (header != NULL) { + if (header != nullptr) { sourceDataSize += header->getSerializedSize(); } - uint8_t *p_data = NULL; - ReturnValue_t returnValue = store->getFreeElement(&storeAddress, - (getPacketMinimumSize() + sourceDataSize), &p_data); + uint8_t *pData = nullptr; + size_t sizeToReserve = getPacketMinimumSize() + sourceDataSize; + ReturnValue_t returnValue = store->getFreeElement(&storeAddress, sizeToReserve, &pData); if (returnValue != store->RETURN_OK) { +#if FSFW_VERBOSE_LEVEL >= 1 + switch(returnValue) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + case(StorageManagerIF::DATA_STORAGE_FULL): { + sif::warning << "TmPacketStoredPusC::TmPacketStoredPusC: Store full for packet with " + "size " << sizeToReserve << std::endl; + break; + } + case(StorageManagerIF::DATA_TOO_LARGE): { + sif::warning << "TmPacketStoredPusC::TmPacketStoredPusC: Data with size " << + sizeToReserve << " too large" << std::endl; + break; + } +#else + case(StorageManagerIF::DATA_STORAGE_FULL): { + sif::printWarning("TmPacketStoredPusC::TmPacketStoredPusC: Store full for packet with " + "size %d\n", sizeToReserve); + break; + } + case(StorageManagerIF::DATA_TOO_LARGE): { + sif::printWarning("TmPacketStoredPusC::TmPacketStoredPusC: Data with size " + "%d too large\n", sizeToReserve); + break; + } +#endif +#endif + } TmPacketStoredBase::checkAndReportLostTm(); + return; } - setData(p_data); + setData(pData); initializeTmPacket(apid, service, subservice, packetSubcounter, destinationId, timeRefField); uint8_t *putDataHere = getSourceData(); size_t size = 0; - if (header != NULL) { + if (header != nullptr) { header->serialize(&putDataHere, &size, sourceDataSize, SerializeIF::Endianness::BIG); } - if (content != NULL) { + if (content != nullptr) { content->serialize(&putDataHere, &size, sourceDataSize, SerializeIF::Endianness::BIG); } From df0adfb33c376adb386a2b1df46ff79848a563e1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 27 Sep 2021 15:35:09 +0200 Subject: [PATCH 02/24] added new tcp code --- src/fsfw/osal/common/TcpTmTcServer.cpp | 77 ++++++++++++++++++++------ 1 file changed, 61 insertions(+), 16 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index c819f9e7..06cace2d 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -5,6 +5,7 @@ #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" @@ -19,6 +20,7 @@ #elif defined(PLATFORM_UNIX) #include #endif +#include #ifndef FSFW_TCP_RECV_WIRETAPPING_ENABLED #define FSFW_TCP_RECV_WIRETAPPING_ENABLED 0 @@ -28,8 +30,8 @@ const std::string TcpTmTcServer::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_POR TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge, size_t receptionBufferSize, std::string customTcpServerPort): - SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), - tcpPort(customTcpServerPort), receptionBuffer(receptionBufferSize) { + SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), + tcpPort(customTcpServerPort), receptionBuffer(receptionBufferSize) { if(tcpPort == "") { tcpPort = DEFAULT_SERVER_PORT; } @@ -148,24 +150,67 @@ ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() { void TcpTmTcServer::handleServerOperation(socket_t connSocket) { int retval = 0; - do { - // Read all telecommands sent by the client - retval = recv(connSocket, - reinterpret_cast(receptionBuffer.data()), - receptionBuffer.capacity(), - tcpFlags); - if (retval > 0) { - handleTcReception(retval); + using namespace std::chrono_literals; + + // Receive until the peer shuts down the connection, use select to do this + fd_set rfds; + fd_set efds; + + FD_ZERO(&rfds); + FD_SET(connSocket, &rfds); + + FD_ZERO(&efds); + FD_SET(connSocket, &efds); + + timeval tv; + tv.tv_sec = 1; + tv.tv_usec = 0; + + int nfds = connSocket + 1; + + // do { + // // Read all telecommands sent by the client + // retval = recv( + // connSocket, + // reinterpret_cast(receptionBuffer.data()), + // receptionBuffer.capacity(), + // tcpFlags + // ); + // if (retval > 0) { + // handleTcReception(retval); + // } + // else if(retval == 0) { + // // Client has finished sending telecommands, send telemetry now + // handleTmSending(connSocket); + // } + // else { + // // Should not happen + // tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL); + // } + // } while(retval > 0); + while (true) { + uint32_t index = 0; + int retval = select(nfds, &rfds, nullptr, &efds, &tv); + if(retval < 0) { + // client might have shut down connection? } - else if(retval == 0) { - // Client has finished sending telecommands, send telemetry now - handleTmSending(connSocket); + else if(retval > 0) { + if(FD_ISSET(connSocket, &rfds)) { + // data available + //int result = receiveData(); + //if(result == 0) { + // break; + //} + } + if(FD_ISSET(connSocket, &efds)) { + //spdlog::error("{}: Exception detected on receive FD", tcpip::SERVER_PR); + } } else { - // Should not happen - tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL); + // no data available + TaskFactory::delayTask(500); } - } while(retval > 0); + } } ReturnValue_t TcpTmTcServer::handleTcReception(size_t bytesRecvd) { From 45c0074bd705673448f5978375df792a83dafaf0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 27 Sep 2021 15:50:23 +0200 Subject: [PATCH 03/24] printout improvements --- src/fsfw/osal/common/TcpTmTcServer.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 06cace2d..ee1b0f23 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -222,20 +222,27 @@ ReturnValue_t TcpTmTcServer::handleTcReception(size_t bytesRecvd) { if (result != HasReturnvaluesIF::RETURN_OK) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning<< "TcpTmTcServer::handleServerOperation: Data storage failed." << std::endl; - sif::warning << "Packet size: " << bytesRecvd << std::endl; + sif::warning << "TcpTmTcServer::handleServerOperation: Data storage with packet size" << + bytesRecvd << " failed" << std::endl; +#else + sif::printWarning("TcpTmTcServer::handleServerOperation: Data storage with packet size %d " + "failed\n", bytesRecvd); #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); From 62a6e5da0bc18f5913ed794accfca6c96abdd59b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 27 Sep 2021 16:11:48 +0200 Subject: [PATCH 04/24] added PUS again --- src/fsfw/osal/common/TcpTmTcServer.cpp | 25 +++-- src/fsfw/osal/common/TcpTmTcServer.h | 3 + src/fsfw/tmtcservices/PusParser.cpp | 141 +++++++++++++++++++++++++ src/fsfw/tmtcservices/PusParser.h | 83 +++++++++++++++ 4 files changed, 244 insertions(+), 8 deletions(-) create mode 100644 src/fsfw/tmtcservices/PusParser.cpp create mode 100644 src/fsfw/tmtcservices/PusParser.h diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index ee1b0f23..1de0816d 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -30,8 +30,8 @@ const std::string TcpTmTcServer::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_POR TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge, size_t receptionBufferSize, std::string customTcpServerPort): - SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), - tcpPort(customTcpServerPort), receptionBuffer(receptionBufferSize) { + SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), + tcpPort(customTcpServerPort), receptionBuffer(receptionBufferSize) { if(tcpPort == "") { tcpPort = DEFAULT_SERVER_PORT; } @@ -127,7 +127,7 @@ 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); } @@ -163,8 +163,8 @@ void TcpTmTcServer::handleServerOperation(socket_t connSocket) { FD_SET(connSocket, &efds); timeval tv; - tv.tv_sec = 1; - tv.tv_usec = 0; + tv.tv_sec = selectTimeoutMs / 1000; + tv.tv_usec = (selectTimeoutMs % 1000) * 1000; int nfds = connSocket + 1; @@ -192,11 +192,19 @@ void TcpTmTcServer::handleServerOperation(socket_t connSocket) { uint32_t index = 0; int retval = select(nfds, &rfds, nullptr, &efds, &tv); if(retval < 0) { - // client might have shut down connection? + // client might have shut down connection + return; } else if(retval > 0) { if(FD_ISSET(connSocket, &rfds)) { // data available + int retval = recv( + connSocket, + reinterpret_cast(receptionBuffer.data()), + receptionBuffer.capacity(), + tcpFlags + ); + handleTcReception(retval); //int result = receiveData(); //if(result == 0) { // break; @@ -207,8 +215,9 @@ void TcpTmTcServer::handleServerOperation(socket_t connSocket) { } } else { - // no data available - TaskFactory::delayTask(500); + // no data available. Send back telemetry now + handleTmSending(connSocket); + //TaskFactory::delayTask(500); } } } diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h index c6916080..a33a0427 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/src/fsfw/osal/common/TcpTmTcServer.h @@ -45,6 +45,7 @@ public: static const std::string DEFAULT_SERVER_PORT; static constexpr size_t ETHERNET_MTU_SIZE = 1500; + static constexpr uint32_t DEFAULT_SELECT_TIMEOUT_MS = 200; /** * TCP Server Constructor @@ -59,6 +60,7 @@ public: std::string customTcpServerPort = ""); virtual~ TcpTmTcServer(); + void setSelectTimeout(uint32_t timeout); void setTcpBacklog(uint8_t tcpBacklog); ReturnValue_t initialize() override; @@ -77,6 +79,7 @@ private: std::string tcpPort; int tcpFlags = 0; + uint32_t selectTimeoutMs = DEFAULT_SELECT_TIMEOUT_MS; socket_t listenerTcpSocket = 0; struct sockaddr tcpAddress; MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE; diff --git a/src/fsfw/tmtcservices/PusParser.cpp b/src/fsfw/tmtcservices/PusParser.cpp new file mode 100644 index 00000000..42044da3 --- /dev/null +++ b/src/fsfw/tmtcservices/PusParser.cpp @@ -0,0 +1,141 @@ +#include "PusParser.h" +#include + +PusParser::PusParser(uint16_t maxExpectedPusPackets, + bool storeSplitPackets): indexSizePairFIFO(maxExpectedPusPackets) { +} + +ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame, + size_t frameSize) { + if(frame == nullptr or frameSize < 5) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "PusParser::parsePusPackets: Frame invalid!" << std::endl; +#else + sif::printError("PusParser::parsePusPackets: Frame invalid!\n"); +#endif + return HasReturnvaluesIF::RETURN_FAILED; + } + + if(indexSizePairFIFO.full()) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "PusParser::parsePusPackets: FIFO is full!" << std::endl; +#else + sif::printError("PusParser::parsePusPackets: FIFO is full!\n"); +#endif + return HasReturnvaluesIF::RETURN_FAILED; + } + + size_t lengthField = frame[4] << 8 | frame[5]; + + if(lengthField == 0) { + return NO_PACKET_FOUND; + } + + size_t packetSize = lengthField + 7; + // sif::debug << frameSize << std::endl; + // Size of a pus packet is the value in the packet length field plus 7. + if(packetSize > frameSize) { + if(storeSplitPackets) { + indexSizePairFIFO.insert(indexSizePair(0, frameSize)); + } + else { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::debug << "TcSerialPollingTask::readNextPacket: Next packet " + << "larger than remaining frame," << std::endl; + sif::debug << "Throwing away packet. Detected packet size: " + << packetSize << std::endl; +#else + sif::printDebug("TcSerialPollingTask::readNextPacket: Next packet " + "larger than remaining frame.\n"); + sif::printDebug("Throwing away packet. Detected packet size: %lu", + static_cast(packetSize)); +#endif + } + return SPLIT_PACKET; + } + else { + indexSizePairFIFO.insert(indexSizePair(0, packetSize)); + if(packetSize == frameSize) { + return HasReturnvaluesIF::RETURN_OK; + } + } + + // packet size is smaller than frame size, parse for more packets. + return readMultiplePackets(frame, frameSize, packetSize); +} + +ReturnValue_t PusParser::readMultiplePackets(const uint8_t *frame, + size_t frameSize, size_t startIndex) { + while (startIndex < frameSize) { + ReturnValue_t result = readNextPacket(frame, frameSize, startIndex); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + } + return HasReturnvaluesIF::RETURN_OK; +} + +DynamicFIFO* PusParser::fifo(){ + return &indexSizePairFIFO; +} + +PusParser::indexSizePair PusParser::getNextFifoPair() { + indexSizePair nextIndexSizePair; + indexSizePairFIFO.retrieve(&nextIndexSizePair); + return nextIndexSizePair; +} + +ReturnValue_t PusParser::readNextPacket(const uint8_t *frame, + size_t frameSize, size_t& currentIndex) { + // sif::debug << startIndex << std::endl; + if(currentIndex + 5 > frameSize) { + currentIndex = frameSize; + return HasReturnvaluesIF::RETURN_OK; + } + + uint16_t lengthField = frame[currentIndex + 4] << 8 | + frame[currentIndex + 5]; + if(lengthField == 0) { + // It is assumed that no packet follows. + currentIndex = frameSize; + return HasReturnvaluesIF::RETURN_OK; + } + size_t nextPacketSize = lengthField + 7; + size_t remainingSize = frameSize - currentIndex; + if(nextPacketSize > remainingSize) + { + if(storeSplitPackets) { + indexSizePairFIFO.insert(indexSizePair(currentIndex, remainingSize)); + } + else { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::debug << "TcSerialPollingTask::readNextPacket: Next packet " + << "larger than remaining frame." << std::endl; + sif::debug << "Throwing away packet. Detected packet size: " + << nextPacketSize << std::endl; +#else + sif::printDebug("TcSerialPollingTask::readNextPacket: Next packet " + "larger than remaining frame.\n"); + sif::printDebug("Throwing away packet. Detected packet size: %lu\n", + static_cast(nextPacketSize)); +#endif + } + return SPLIT_PACKET; + } + + ReturnValue_t result = indexSizePairFIFO.insert(indexSizePair(currentIndex, + nextPacketSize)); + if (result != HasReturnvaluesIF::RETURN_OK) { + // FIFO full. +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::debug << "PusParser: Issue inserting into start index size " + << "FIFO, it is full!" << std::endl; +#else + sif::printDebug("PusParser: Issue inserting into start index size " + "FIFO, it is full!\n"); +#endif + } + currentIndex += nextPacketSize; + + return result; +} diff --git a/src/fsfw/tmtcservices/PusParser.h b/src/fsfw/tmtcservices/PusParser.h new file mode 100644 index 00000000..0efbdcc5 --- /dev/null +++ b/src/fsfw/tmtcservices/PusParser.h @@ -0,0 +1,83 @@ +#ifndef FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ +#define FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ + +#include +#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 payload length field of PUS packets to find + * the respective PUS packet sizes. + * + * The parser parses a buffer by taking a pointer and the maximum size to scan. + * If PUS 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 PusParser { +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); + /** + * 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. + */ + PusParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets); + + /** + * Parse a given frame for PUS packets + * @param frame + * @param frameSize + * @return -@c NO_PACKET_FOUND if no packet was found. + */ + ReturnValue_t parsePusPackets(const uint8_t* frame, size_t frameSize); + + /** + * Accessor function to get a reference to the internal FIFO which + * stores pairs of indexi 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 removed 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; + + bool storeSplitPackets = false; + + ReturnValue_t readMultiplePackets(const uint8_t *frame, size_t frameSize, + size_t startIndex); + ReturnValue_t readNextPacket(const uint8_t *frame, + size_t frameSize, size_t& startIndex); +}; + +#endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */ From 68fe94d5944a5462cd0ba05d96ded321c1f52e20 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 27 Sep 2021 16:15:51 +0200 Subject: [PATCH 05/24] PusParser integration --- src/fsfw/returnvalues/FwClassIds.h | 1 + src/fsfw/tmtcservices/PusParser.cpp | 26 +++++++++++++++----------- src/fsfw/tmtcservices/PusParser.h | 7 +++++-- 3 files changed, 21 insertions(+), 13 deletions(-) diff --git a/src/fsfw/returnvalues/FwClassIds.h b/src/fsfw/returnvalues/FwClassIds.h index 4a3a578a..8422baa5 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 + PUS_PARSER, //PUSP FW_CLASS_ID_COUNT // [EXPORT] : [END] }; diff --git a/src/fsfw/tmtcservices/PusParser.cpp b/src/fsfw/tmtcservices/PusParser.cpp index 42044da3..5b63710c 100644 --- a/src/fsfw/tmtcservices/PusParser.cpp +++ b/src/fsfw/tmtcservices/PusParser.cpp @@ -1,8 +1,8 @@ #include "PusParser.h" #include -PusParser::PusParser(uint16_t maxExpectedPusPackets, - bool storeSplitPackets): indexSizePairFIFO(maxExpectedPusPackets) { +PusParser::PusParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets): + indexSizePairFIFO(maxExpectedPusPackets) { } ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame, @@ -39,17 +39,19 @@ ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame, indexSizePairFIFO.insert(indexSizePair(0, frameSize)); } else { +#if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::debug << "TcSerialPollingTask::readNextPacket: Next packet " + sif::warning << "TcSerialPollingTask::readNextPacket: Next packet " << "larger than remaining frame," << std::endl; - sif::debug << "Throwing away packet. Detected packet size: " + sif::warning << "Throwing away packet. Detected packet size: " << packetSize << std::endl; #else - sif::printDebug("TcSerialPollingTask::readNextPacket: Next packet " + sif::printWarning("TcSerialPollingTask::readNextPacket: Next packet " "larger than remaining frame.\n"); - sif::printDebug("Throwing away packet. Detected packet size: %lu", + sif::printWarning("Throwing away packet. Detected packet size: %lu", static_cast(packetSize)); -#endif +#endif +#endif /* FSFW_VERBOSE_LEVEL >= 1 */ } return SPLIT_PACKET; } @@ -108,16 +110,18 @@ ReturnValue_t PusParser::readNextPacket(const uint8_t *frame, indexSizePairFIFO.insert(indexSizePair(currentIndex, remainingSize)); } else { +#if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::debug << "TcSerialPollingTask::readNextPacket: Next packet " + sif::warning << "TcSerialPollingTask::readNextPacket: Next packet " << "larger than remaining frame." << std::endl; - sif::debug << "Throwing away packet. Detected packet size: " + sif::warning << "Throwing away packet. Detected packet size: " << nextPacketSize << std::endl; #else - sif::printDebug("TcSerialPollingTask::readNextPacket: Next packet " + sif::printWarning("TcSerialPollingTask::readNextPacket: Next packet " "larger than remaining frame.\n"); - sif::printDebug("Throwing away packet. Detected packet size: %lu\n", + sif::printWarning("Throwing away packet. Detected packet size: %lu\n", static_cast(nextPacketSize)); +#endif #endif } return SPLIT_PACKET; diff --git a/src/fsfw/tmtcservices/PusParser.h b/src/fsfw/tmtcservices/PusParser.h index 0efbdcc5..851f1a5d 100644 --- a/src/fsfw/tmtcservices/PusParser.h +++ b/src/fsfw/tmtcservices/PusParser.h @@ -1,7 +1,9 @@ #ifndef FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ #define FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ -#include +#include "fsfw/container/DynamicFIFO.h" +#include "fsfw/returnvalues/FwClassIds.h" + #include #include @@ -66,7 +68,8 @@ public: indexSizePair getNextFifoPair(); private: - /** A FIFO is used to store information about multiple PUS packets + /** + * 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. */ From d6b31679223f734c84eccb0fb23ea6dec2828b01 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 27 Sep 2021 18:05:45 +0200 Subject: [PATCH 06/24] tcp keep open implementation done --- src/fsfw/container/SimpleRingBuffer.h | 15 +- src/fsfw/osal/common/TcpTmTcServer.cpp | 170 ++++++++++++++---- src/fsfw/osal/common/TcpTmTcServer.h | 68 +++++-- src/fsfw/tmtcservices/CMakeLists.txt | 1 + .../{PusParser.cpp => SpacePacketParser.cpp} | 26 +-- .../{PusParser.h => SpacePacketParser.h} | 32 ++-- 6 files changed, 227 insertions(+), 85 deletions(-) rename src/fsfw/tmtcservices/{PusParser.cpp => SpacePacketParser.cpp} (80%) rename src/fsfw/tmtcservices/{PusParser.h => SpacePacketParser.h} (70%) 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 /** - * @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 1de0816d..fbbf83e4 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -29,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) { } ReturnValue_t TcpTmTcServer::initialize() { @@ -45,6 +44,16 @@ ReturnValue_t TcpTmTcServer::initialize() { return result; } + switch(receptionMode) { + case(ReceptionModes::SPACE_PACKETS): { + // For now, hardcode a maximum of 5 store packets here and no split packets are allowed + spacePacketParser = new SpacePacketParser(5, false); + if(spacePacketParser == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + } + tcStore = ObjectManager::instance()->get(objects::TC_STORE); if (tcStore == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -67,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; @@ -109,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; @@ -149,7 +158,7 @@ ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() { } void TcpTmTcServer::handleServerOperation(socket_t connSocket) { - int retval = 0; + //int retval = 0; using namespace std::chrono_literals; // Receive until the peer shuts down the connection, use select to do this @@ -163,8 +172,8 @@ void TcpTmTcServer::handleServerOperation(socket_t connSocket) { FD_SET(connSocket, &efds); timeval tv; - tv.tv_sec = selectTimeoutMs / 1000; - tv.tv_usec = (selectTimeoutMs % 1000) * 1000; + tv.tv_sec = 0;//tcpConfig.selectTimeoutMs / 1000; + tv.tv_usec = 0;//(tcpConfig.selectTimeoutMs % 1000) * 1000; int nfds = connSocket + 1; @@ -189,7 +198,6 @@ void TcpTmTcServer::handleServerOperation(socket_t connSocket) { // } // } while(retval > 0); while (true) { - uint32_t index = 0; int retval = select(nfds, &rfds, nullptr, &efds, &tv); if(retval < 0) { // client might have shut down connection @@ -202,40 +210,57 @@ void TcpTmTcServer::handleServerOperation(socket_t connSocket) { connSocket, reinterpret_cast(receptionBuffer.data()), receptionBuffer.capacity(), - tcpFlags + tcpConfig.tcpFlags ); - handleTcReception(retval); - //int result = receiveData(); - //if(result == 0) { - // break; - //} + ringBuffer.writeData(receptionBuffer.data(), retval); } if(FD_ISSET(connSocket, &efds)) { - //spdlog::error("{}: Exception detected on receive FD", tcpip::SERVER_PR); +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "TcpTmTcServer::handleServerOperation: " + "Exception detected" << std::endl; +#else + sif::printWarning("TcpTmTcServer::handleServerOperation: Exception detected\n"); +#endif } } else { - // no data available. Send back telemetry now - handleTmSending(connSocket); - //TaskFactory::delayTask(500); + // no data available. Check whether any packets have been read, then send back + // telemetry now + bool tcAvailable = false; + bool tmSent = false; + size_t availableReadData = ringBuffer.getAvailableReadData(); + if(availableReadData > lastRingBufferSize) { + tcAvailable = true; + handleRingBufferData(availableReadData); + } + ReturnValue_t result = handleTmSending(connSocket, tmSent); + if(result == CONN_BROKEN) { + return; + } + if(not tcAvailable and not tmSent) { + TaskFactory::delayTask(DEFAULT_LOOP_DELAY_MS); + } } } } -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 with packet size" << - bytesRecvd << " failed" << std::endl; + packetSize << " failed" << std::endl; #else sif::printWarning("TcpTmTcServer::handleServerOperation: Data storage with packet size %d " - "failed\n", bytesRecvd); + "failed\n", packetSize); #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ #endif /* FSFW_VERBOSE_LEVEL >= 1 */ return result; @@ -259,21 +284,25 @@ 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(uint8_t parserFifoSize) { +} + +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); @@ -284,10 +313,81 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket) { int retval = send(connSocket, reinterpret_cast(storeAccessor.data()), storeAccessor.size(), - tcpTmFlags); + tcpConfig.tcpTmFlags); if(retval != static_cast(storeAccessor.size())) { - tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::SEND_CALL); + // Assume that the client has closed the connection here for now + handleSocketError(storeAccessor); + return CONN_BROKEN; + } + else { + // Packet sent, clear FIFO entry + tmtcBridge->tmFifo->pop(); + tmSent = true; } } return HasReturnvaluesIF::RETURN_OK; } + +ReturnValue_t TcpTmTcServer::handleRingBufferData(size_t availableReadData) { + ReturnValue_t status = HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + lastRingBufferSize = availableReadData; + if(availableReadData == 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 + } + ringBuffer.readData(receptionBuffer.data(), availableReadData, true); + result = spacePacketParser->parsePusPackets(receptionBuffer.data(), + receptionBuffer.size()); + if(result == SpacePacketParser::NO_PACKET_FOUND) { + ringBuffer.deleteData(availableReadData); + lastRingBufferSize = ringBuffer.getAvailableReadData(); + } + else if(result == HasReturnvaluesIF::RETURN_OK) { + // Space Packets were found. Handle them here + auto fifo = spacePacketParser->fifo(); + SpacePacketParser::IndexSizePair idxSizePair; + while(not fifo.empty()) { + fifo.retrieve(&idxSizePair); + result = handleTcReception(receptionBuffer.data() + idxSizePair.first, + idxSizePair.second); + if(result != HasReturnvaluesIF::RETURN_OK) { + status = result; + } + } + } + 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 a33a0427..64785a46 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,10 +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 uint32_t DEFAULT_SELECT_TIMEOUT_MS = 200; + static constexpr size_t RING_BUFFER_SIZE = ETHERNET_MTU_SIZE * 3; + static constexpr uint32_t DEFAULT_LOOP_DELAY_MS = 200; /** * TCP Server Constructor @@ -56,12 +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 setSelectTimeout(uint32_t timeout); - 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(uint8_t parserFifoSize); ReturnValue_t initialize() override; ReturnValue_t performOperation(uint8_t opCode) override; @@ -73,26 +109,28 @@ 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; - uint32_t selectTimeoutMs = DEFAULT_SELECT_TIMEOUT_MS; - 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; + SpacePacketParser* spacePacketParser = nullptr; + uint8_t lastRingBufferSize = 0; void handleServerOperation(socket_t connSocket); - ReturnValue_t handleTcReception(size_t bytesRecvd); - ReturnValue_t handleTmSending(socket_t connSocket); + ReturnValue_t handleTcReception(uint8_t* spacePacket, size_t packetSize); + ReturnValue_t handleTmSending(socket_t connSocket, bool& tmSent); + ReturnValue_t handleRingBufferData(size_t availableReadData); + void handleSocketError(ConstStorageAccessor& accessor); }; #endif /* FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_ */ 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/PusParser.cpp b/src/fsfw/tmtcservices/SpacePacketParser.cpp similarity index 80% rename from src/fsfw/tmtcservices/PusParser.cpp rename to src/fsfw/tmtcservices/SpacePacketParser.cpp index 5b63710c..7e510900 100644 --- a/src/fsfw/tmtcservices/PusParser.cpp +++ b/src/fsfw/tmtcservices/SpacePacketParser.cpp @@ -1,11 +1,11 @@ -#include "PusParser.h" #include +#include -PusParser::PusParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets): +SpacePacketParser::SpacePacketParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets): indexSizePairFIFO(maxExpectedPusPackets) { } -ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame, +ReturnValue_t SpacePacketParser::parsePusPackets(const uint8_t *frame, size_t frameSize) { if(frame == nullptr or frameSize < 5) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -36,7 +36,7 @@ ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame, // Size of a pus packet is the value in the packet length field plus 7. if(packetSize > frameSize) { if(storeSplitPackets) { - indexSizePairFIFO.insert(indexSizePair(0, frameSize)); + indexSizePairFIFO.insert(IndexSizePair(0, frameSize)); } else { #if FSFW_VERBOSE_LEVEL >= 1 @@ -56,7 +56,7 @@ ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame, return SPLIT_PACKET; } else { - indexSizePairFIFO.insert(indexSizePair(0, packetSize)); + indexSizePairFIFO.insert(IndexSizePair(0, packetSize)); if(packetSize == frameSize) { return HasReturnvaluesIF::RETURN_OK; } @@ -66,7 +66,7 @@ ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame, return readMultiplePackets(frame, frameSize, packetSize); } -ReturnValue_t PusParser::readMultiplePackets(const uint8_t *frame, +ReturnValue_t SpacePacketParser::readMultiplePackets(const uint8_t *frame, size_t frameSize, size_t startIndex) { while (startIndex < frameSize) { ReturnValue_t result = readNextPacket(frame, frameSize, startIndex); @@ -77,17 +77,17 @@ ReturnValue_t PusParser::readMultiplePackets(const uint8_t *frame, return HasReturnvaluesIF::RETURN_OK; } -DynamicFIFO* PusParser::fifo(){ - return &indexSizePairFIFO; +DynamicFIFO& SpacePacketParser::fifo(){ + return indexSizePairFIFO; } -PusParser::indexSizePair PusParser::getNextFifoPair() { - indexSizePair nextIndexSizePair; +SpacePacketParser::IndexSizePair SpacePacketParser::getNextFifoPair() { + IndexSizePair nextIndexSizePair; indexSizePairFIFO.retrieve(&nextIndexSizePair); return nextIndexSizePair; } -ReturnValue_t PusParser::readNextPacket(const uint8_t *frame, +ReturnValue_t SpacePacketParser::readNextPacket(const uint8_t *frame, size_t frameSize, size_t& currentIndex) { // sif::debug << startIndex << std::endl; if(currentIndex + 5 > frameSize) { @@ -107,7 +107,7 @@ ReturnValue_t PusParser::readNextPacket(const uint8_t *frame, if(nextPacketSize > remainingSize) { if(storeSplitPackets) { - indexSizePairFIFO.insert(indexSizePair(currentIndex, remainingSize)); + indexSizePairFIFO.insert(IndexSizePair(currentIndex, remainingSize)); } else { #if FSFW_VERBOSE_LEVEL >= 1 @@ -127,7 +127,7 @@ ReturnValue_t PusParser::readNextPacket(const uint8_t *frame, return SPLIT_PACKET; } - ReturnValue_t result = indexSizePairFIFO.insert(indexSizePair(currentIndex, + ReturnValue_t result = indexSizePairFIFO.insert(IndexSizePair(currentIndex, nextPacketSize)); if (result != HasReturnvaluesIF::RETURN_OK) { // FIFO full. diff --git a/src/fsfw/tmtcservices/PusParser.h b/src/fsfw/tmtcservices/SpacePacketParser.h similarity index 70% rename from src/fsfw/tmtcservices/PusParser.h rename to src/fsfw/tmtcservices/SpacePacketParser.h index 851f1a5d..d0d7bc4d 100644 --- a/src/fsfw/tmtcservices/PusParser.h +++ b/src/fsfw/tmtcservices/SpacePacketParser.h @@ -11,11 +11,11 @@ * @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 payload length field of PUS packets to find - * the respective PUS packet sizes. + * 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 PUS packets are found, they are stored in a FIFO which stores pairs + * 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 @@ -23,17 +23,18 @@ * store that split packet or throw away the packet. * @author R. Mueller */ -class PusParser { +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; + 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); + /** - * Parser constructor. + * @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) @@ -41,31 +42,34 @@ public: * Specifies whether split packets are also stored inside the FIFO, * with the size being the remaining frame size. */ - PusParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets); + SpacePacketParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets); /** * Parse a given frame for PUS packets * @param frame * @param frameSize - * @return -@c NO_PACKET_FOUND if no packet was found. + * @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 parsePusPackets(const uint8_t* frame, size_t frameSize); /** * Accessor function to get a reference to the internal FIFO which - * stores pairs of indexi and packet sizes. This FIFO is filled - * by the parsePusPackets() function. + * stores pairs of index and packet sizes. This FIFO is filled + * by the #parsePusPackets function. * @return */ - DynamicFIFO* fifo(); + DynamicFIFO& fifo(); /** * Retrieve the next index and packet size pair from the FIFO. - * This also removed it from the FIFO. Please note that if 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(); + IndexSizePair getNextFifoPair(); private: /** @@ -73,7 +77,7 @@ private: * inside the receive buffer. The maximum number of entries is defined * by the first constructor argument. */ - DynamicFIFO indexSizePairFIFO; + DynamicFIFO indexSizePairFIFO; bool storeSplitPackets = false; From 71036bf6b168ecd9c4bd524cd1ec820211df29f8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 27 Sep 2021 18:35:50 +0200 Subject: [PATCH 07/24] something wrong with the socket.. --- src/fsfw/osal/common/TcpTmTcServer.cpp | 17 ++++++++++++++++- src/fsfw/osal/common/TcpTmTcServer.h | 2 +- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index fbbf83e4..9c5a8304 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -135,6 +135,7 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) { handleServerOperation(connSocket); + sif::debug << "Shutting down" << std::endl; // Done, shut down connection and go back to listening for client requests retval = shutdown(connSocket, SHUT_BOTH); if(retval != 0) { @@ -157,7 +158,7 @@ ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() { return HasReturnvaluesIF::RETURN_OK; } -void TcpTmTcServer::handleServerOperation(socket_t connSocket) { +void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { //int retval = 0; using namespace std::chrono_literals; @@ -198,12 +199,23 @@ void TcpTmTcServer::handleServerOperation(socket_t connSocket) { // } // } while(retval > 0); while (true) { + sif::debug << "polling fd.." << std::endl; int retval = select(nfds, &rfds, nullptr, &efds, &tv); + // data available + int test = recv( + connSocket, + reinterpret_cast(receptionBuffer.data()), + receptionBuffer.capacity(), + tcpConfig.tcpFlags + ); + sif::debug << "Received " << retval << " bytes" << std::endl; if(retval < 0) { // client might have shut down connection return; } + else if(retval > 0) { + sif::debug << "some descriptor set.." << std::endl; if(FD_ISSET(connSocket, &rfds)) { // data available int retval = recv( @@ -212,6 +224,7 @@ void TcpTmTcServer::handleServerOperation(socket_t connSocket) { receptionBuffer.capacity(), tcpConfig.tcpFlags ); + sif::debug << "Received " << retval << " bytes" << std::endl; ringBuffer.writeData(receptionBuffer.data(), retval); } if(FD_ISSET(connSocket, &efds)) { @@ -229,7 +242,9 @@ void TcpTmTcServer::handleServerOperation(socket_t connSocket) { bool tcAvailable = false; bool tmSent = false; size_t availableReadData = ringBuffer.getAvailableReadData(); + //sif::debug << "ring buffer data: " << availableReadData << std::endl; if(availableReadData > lastRingBufferSize) { + sif::debug << "ring buffer size changed" << std::endl; tcAvailable = true; handleRingBufferData(availableReadData); } diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h index 64785a46..ff7757d0 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/src/fsfw/osal/common/TcpTmTcServer.h @@ -126,7 +126,7 @@ private: SpacePacketParser* spacePacketParser = nullptr; uint8_t lastRingBufferSize = 0; - void handleServerOperation(socket_t connSocket); + void handleServerOperation(socket_t& connSocket); ReturnValue_t handleTcReception(uint8_t* spacePacket, size_t packetSize); ReturnValue_t handleTmSending(socket_t connSocket, bool& tmSent); ReturnValue_t handleRingBufferData(size_t availableReadData); From 9b7da4d9e6b4570d1a2677c567365fbeab05598e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 27 Sep 2021 20:00:01 +0200 Subject: [PATCH 08/24] fixing last bugs --- src/fsfw/osal/common/TcpTmTcServer.cpp | 157 ++++++++++++++----------- 1 file changed, 89 insertions(+), 68 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 9c5a8304..03145e20 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -31,9 +31,9 @@ const std::string TcpTmTcServer::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_POR TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge, size_t receptionBufferSize, size_t ringBufferSize, std::string customTcpServerPort, ReceptionModes receptionMode): - SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), receptionMode(receptionMode), - tcpConfig(customTcpServerPort), receptionBuffer(receptionBufferSize), - ringBuffer(ringBufferSize, true) { + SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), receptionMode(receptionMode), + tcpConfig(customTcpServerPort), receptionBuffer(receptionBufferSize), + ringBuffer(ringBufferSize, true) { } ReturnValue_t TcpTmTcServer::initialize() { @@ -127,6 +127,7 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) { //connSocket = accept(listenerTcpSocket, &clientSockAddr, &connectorSockAddrLen); connSocket = accept(listenerTcpSocket, nullptr, nullptr); + sif::debug << "accepted new conn socket " << connSocket << std::endl; if(connSocket == INVALID_SOCKET) { handleError(Protocol::TCP, ErrorSources::ACCEPT_CALL, 500); closeSocket(connSocket); @@ -142,6 +143,7 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) { handleError(Protocol::TCP, ErrorSources::SHUTDOWN_CALL); } closeSocket(connSocket); + connSocket = 0; } return HasReturnvaluesIF::RETURN_OK; } @@ -160,24 +162,24 @@ ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() { void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { //int retval = 0; - using namespace std::chrono_literals; - - // Receive until the peer shuts down the connection, use select to do this - fd_set rfds; - fd_set efds; - - FD_ZERO(&rfds); - FD_SET(connSocket, &rfds); - - FD_ZERO(&efds); - FD_SET(connSocket, &efds); - - timeval tv; - tv.tv_sec = 0;//tcpConfig.selectTimeoutMs / 1000; - tv.tv_usec = 0;//(tcpConfig.selectTimeoutMs % 1000) * 1000; - - int nfds = connSocket + 1; - + // using namespace std::chrono_literals; + // + // // Receive until the peer shuts down the connection, use select to do this + // fd_set rfds; + // fd_set efds; + // + // FD_ZERO(&rfds); + // FD_SET(connSocket, &rfds); + // + // FD_ZERO(&efds); + // FD_SET(connSocket, &efds); + // + // timeval tv = {}; + // tv.tv_sec = 0;//tcpConfig.selectTimeoutMs / 1000; + // tv.tv_usec = 0;// DEFAULT_LOOP_DELAY_MS * 1000;//(tcpConfig.selectTimeoutMs % 1000) * 1000; + // + // int nfds = connSocket + 1; + //setsockopt(connSocket, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv); // do { // // Read all telecommands sent by the client // retval = recv( @@ -198,64 +200,73 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { // tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL); // } // } while(retval > 0); + // data available + // int test = recv( + // connSocket, + // reinterpret_cast(receptionBuffer.data()), + // receptionBuffer.capacity(), + // tcpConfig.tcpFlags + // ); + // sif::debug << "Received " << test << " bytes" << std::endl; while (true) { - sif::debug << "polling fd.." << std::endl; - int retval = select(nfds, &rfds, nullptr, &efds, &tv); - // data available - int test = recv( + int retval = recv( connSocket, reinterpret_cast(receptionBuffer.data()), receptionBuffer.capacity(), - tcpConfig.tcpFlags + MSG_DONTWAIT//tcpConfig.tcpFlags ); - sif::debug << "Received " << retval << " bytes" << std::endl; - if(retval < 0) { - // client might have shut down connection + + if(retval == 0) { + // Client closed connection return; } - else if(retval > 0) { - sif::debug << "some descriptor set.." << std::endl; - if(FD_ISSET(connSocket, &rfds)) { - // data available - int retval = recv( - connSocket, - reinterpret_cast(receptionBuffer.data()), - receptionBuffer.capacity(), - tcpConfig.tcpFlags - ); - sif::debug << "Received " << retval << " bytes" << std::endl; - ringBuffer.writeData(receptionBuffer.data(), retval); + sif::debug << "Received " << retval << " bytes" << std::endl; + ringBuffer.writeData(receptionBuffer.data(), retval); + sif::debug << "select retval: " << retval << std::endl; + } + else if(retval < 0) { + if(errno == EAGAIN) { + // no data available. Check whether any packets have been read, then send back + // telemetry now + bool tcAvailable = false; + bool tmSent = false; + size_t availableReadData = ringBuffer.getAvailableReadData(); + //sif::debug << "ring buffer data: " << availableReadData << std::endl; + //sif::debug << "last buf size: " << lastRingBufferSize << std::endl; + if(availableReadData > lastRingBufferSize) { + sif::debug << "ring buffer size changed" << std::endl; + tcAvailable = true; + handleRingBufferData(availableReadData); + } + ReturnValue_t result = handleTmSending(connSocket, tmSent); + if(result == CONN_BROKEN) { + return; + } + if(not tcAvailable and not tmSent) { + TaskFactory::delayTask(DEFAULT_LOOP_DELAY_MS); + } } - if(FD_ISSET(connSocket, &efds)) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "TcpTmTcServer::handleServerOperation: " - "Exception detected" << std::endl; -#else - sif::printWarning("TcpTmTcServer::handleServerOperation: Exception detected\n"); -#endif - } - } - else { - // no data available. Check whether any packets have been read, then send back - // telemetry now - bool tcAvailable = false; - bool tmSent = false; - size_t availableReadData = ringBuffer.getAvailableReadData(); - //sif::debug << "ring buffer data: " << availableReadData << std::endl; - if(availableReadData > lastRingBufferSize) { - sif::debug << "ring buffer size changed" << std::endl; - tcAvailable = true; - handleRingBufferData(availableReadData); - } - ReturnValue_t result = handleTmSending(connSocket, tmSent); - if(result == CONN_BROKEN) { - return; - } - if(not tcAvailable and not tmSent) { - TaskFactory::delayTask(DEFAULT_LOOP_DELAY_MS); + if(errno == ETIMEDOUT) { + + retval = 0; } } + // data available + // int retval = recv( + // connSocket, + // reinterpret_cast(receptionBuffer.data()), + // receptionBuffer.capacity(), + // tcpConfig.tcpFlags + // ); + //sif::debug << "recv retval: " << retval << std::endl; + // data available + // int test = recv( + // connSocket, + // reinterpret_cast(receptionBuffer.data()), + // receptionBuffer.capacity(), + // tcpConfig.tcpFlags + // ); } } @@ -317,12 +328,14 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent) while((not tmtcBridge->tmFifo->empty()) and (tmtcBridge->packetSentCounter < tmtcBridge->sentPacketsPerCycle)) { // Send can fail, so only peek from the FIFO + sif::debug << "sending TM" << std::endl; tmtcBridge->tmFifo->peek(&storeId); // Using the store accessor will take care of deleting TM from the store automatically ConstStorageAccessor storeAccessor(storeId); ReturnValue_t result = tmStore->getData(storeId, storeAccessor); if(result != HasReturnvaluesIF::RETURN_OK) { + sif::debug << "oh no" << std::endl; return result; } int retval = send(connSocket, @@ -331,12 +344,14 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent) tcpConfig.tcpTmFlags); if(retval != static_cast(storeAccessor.size())) { // Assume that the client has closed the connection here for now + sif::debug << "conn broken?" << std::endl; handleSocketError(storeAccessor); return CONN_BROKEN; } else { // Packet sent, clear FIFO entry tmtcBridge->tmFifo->pop(); + sif::debug << "fifo size: " << tmtcBridge->tmFifo->size() << std::endl; tmSent = true; } } @@ -366,6 +381,7 @@ ReturnValue_t TcpTmTcServer::handleRingBufferData(size_t availableReadData) { if(result == SpacePacketParser::NO_PACKET_FOUND) { ringBuffer.deleteData(availableReadData); lastRingBufferSize = ringBuffer.getAvailableReadData(); + sif::debug << lastRingBufferSize << std::endl; } else if(result == HasReturnvaluesIF::RETURN_OK) { // Space Packets were found. Handle them here @@ -373,13 +389,18 @@ ReturnValue_t TcpTmTcServer::handleRingBufferData(size_t availableReadData) { SpacePacketParser::IndexSizePair idxSizePair; while(not fifo.empty()) { fifo.retrieve(&idxSizePair); + sif::debug << "handle tc" << std::endl; result = handleTcReception(receptionBuffer.data() + idxSizePair.first, idxSizePair.second); + ringBuffer.deleteData(idxSizePair.second); if(result != HasReturnvaluesIF::RETURN_OK) { status = result; } + lastRingBufferSize = ringBuffer.getAvailableReadData(); + sif::debug << lastRingBufferSize << std::endl; } } + std::memset(receptionBuffer.data(), 0, receptionBuffer.size()); return status; } From 254aac51ec1234b44eefafd7425b62e54f348958 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 10:57:21 +0200 Subject: [PATCH 09/24] cleaning up --- src/fsfw/osal/common/TcpTmTcServer.cpp | 136 ++++++-------------- src/fsfw/tcdistribution/PUSDistributor.cpp | 27 +++- src/fsfw/tmtcservices/SpacePacketParser.cpp | 30 +++-- src/fsfw/tmtcservices/SpacePacketParser.h | 6 +- 4 files changed, 80 insertions(+), 119 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 03145e20..2e65429f 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -31,9 +31,9 @@ const std::string TcpTmTcServer::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_POR TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge, size_t receptionBufferSize, size_t ringBufferSize, std::string customTcpServerPort, ReceptionModes receptionMode): - SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), receptionMode(receptionMode), - tcpConfig(customTcpServerPort), receptionBuffer(receptionBufferSize), - ringBuffer(ringBufferSize, true) { + SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), receptionMode(receptionMode), + tcpConfig(customTcpServerPort), receptionBuffer(receptionBufferSize), + ringBuffer(ringBufferSize, true) { } ReturnValue_t TcpTmTcServer::initialize() { @@ -51,6 +51,7 @@ ReturnValue_t TcpTmTcServer::initialize() { if(spacePacketParser == nullptr) { return HasReturnvaluesIF::RETURN_FAILED; } + tcpConfig.tcpFlags |= MSG_DONTWAIT; } } @@ -127,7 +128,6 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) { //connSocket = accept(listenerTcpSocket, &clientSockAddr, &connectorSockAddrLen); connSocket = accept(listenerTcpSocket, nullptr, nullptr); - sif::debug << "accepted new conn socket " << connSocket << std::endl; if(connSocket == INVALID_SOCKET) { handleError(Protocol::TCP, ErrorSources::ACCEPT_CALL, 500); closeSocket(connSocket); @@ -136,7 +136,6 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) { handleServerOperation(connSocket); - sif::debug << "Shutting down" << std::endl; // Done, shut down connection and go back to listening for client requests retval = shutdown(connSocket, SHUT_BOTH); if(retval != 0) { @@ -161,81 +160,30 @@ ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() { } void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { - //int retval = 0; - // using namespace std::chrono_literals; - // - // // Receive until the peer shuts down the connection, use select to do this - // fd_set rfds; - // fd_set efds; - // - // FD_ZERO(&rfds); - // FD_SET(connSocket, &rfds); - // - // FD_ZERO(&efds); - // FD_SET(connSocket, &efds); - // - // timeval tv = {}; - // tv.tv_sec = 0;//tcpConfig.selectTimeoutMs / 1000; - // tv.tv_usec = 0;// DEFAULT_LOOP_DELAY_MS * 1000;//(tcpConfig.selectTimeoutMs % 1000) * 1000; - // - // int nfds = connSocket + 1; - //setsockopt(connSocket, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv); - // do { - // // Read all telecommands sent by the client - // retval = recv( - // connSocket, - // reinterpret_cast(receptionBuffer.data()), - // receptionBuffer.capacity(), - // tcpFlags - // ); - // if (retval > 0) { - // handleTcReception(retval); - // } - // else if(retval == 0) { - // // Client has finished sending telecommands, send telemetry now - // handleTmSending(connSocket); - // } - // else { - // // Should not happen - // tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL); - // } - // } while(retval > 0); - // data available - // int test = recv( - // connSocket, - // reinterpret_cast(receptionBuffer.data()), - // receptionBuffer.capacity(), - // tcpConfig.tcpFlags - // ); - // sif::debug << "Received " << test << " bytes" << std::endl; while (true) { int retval = recv( connSocket, reinterpret_cast(receptionBuffer.data()), receptionBuffer.capacity(), - MSG_DONTWAIT//tcpConfig.tcpFlags + tcpConfig.tcpFlags ); - if(retval == 0) { // Client closed connection return; } else if(retval > 0) { - sif::debug << "Received " << retval << " bytes" << std::endl; + // The ring buffer was configured for overwrite, so the returnvalue does not need to + // be checked for now ringBuffer.writeData(receptionBuffer.data(), retval); - sif::debug << "select retval: " << retval << std::endl; } else if(retval < 0) { if(errno == EAGAIN) { - // no data available. Check whether any packets have been read, then send back - // telemetry now + // 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(); - //sif::debug << "ring buffer data: " << availableReadData << std::endl; - //sif::debug << "last buf size: " << lastRingBufferSize << std::endl; if(availableReadData > lastRingBufferSize) { - sif::debug << "ring buffer size changed" << std::endl; tcAvailable = true; handleRingBufferData(availableReadData); } @@ -247,26 +195,10 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { TaskFactory::delayTask(DEFAULT_LOOP_DELAY_MS); } } - if(errno == ETIMEDOUT) { - - retval = 0; + else { + tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL); } } - // data available - // int retval = recv( - // connSocket, - // reinterpret_cast(receptionBuffer.data()), - // receptionBuffer.capacity(), - // tcpConfig.tcpFlags - // ); - //sif::debug << "recv retval: " << retval << std::endl; - // data available - // int test = recv( - // connSocket, - // reinterpret_cast(receptionBuffer.data()), - // receptionBuffer.capacity(), - // tcpConfig.tcpFlags - // ); } } @@ -328,31 +260,28 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent) while((not tmtcBridge->tmFifo->empty()) and (tmtcBridge->packetSentCounter < tmtcBridge->sentPacketsPerCycle)) { // Send can fail, so only peek from the FIFO - sif::debug << "sending TM" << std::endl; tmtcBridge->tmFifo->peek(&storeId); // Using the store accessor will take care of deleting TM from the store automatically ConstStorageAccessor storeAccessor(storeId); ReturnValue_t result = tmStore->getData(storeId, storeAccessor); if(result != HasReturnvaluesIF::RETURN_OK) { - sif::debug << "oh no" << std::endl; return result; } int retval = send(connSocket, reinterpret_cast(storeAccessor.data()), storeAccessor.size(), tcpConfig.tcpTmFlags); - if(retval != static_cast(storeAccessor.size())) { - // Assume that the client has closed the connection here for now - sif::debug << "conn broken?" << std::endl; - handleSocketError(storeAccessor); - return CONN_BROKEN; - } - else { + if(retval == static_cast(storeAccessor.size())) { // Packet sent, clear FIFO entry tmtcBridge->tmFifo->pop(); - sif::debug << "fifo size: " << tmtcBridge->tmFifo->size() << std::endl; 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; @@ -361,8 +290,9 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent) ReturnValue_t TcpTmTcServer::handleRingBufferData(size_t availableReadData) { ReturnValue_t status = HasReturnvaluesIF::RETURN_OK; ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + size_t readAmount = availableReadData; lastRingBufferSize = availableReadData; - if(availableReadData == ringBuffer.getMaxSize()) { + 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, @@ -375,32 +305,42 @@ ReturnValue_t TcpTmTcServer::handleRingBufferData(size_t availableReadData) { #endif #endif } - ringBuffer.readData(receptionBuffer.data(), availableReadData, true); - result = spacePacketParser->parsePusPackets(receptionBuffer.data(), - receptionBuffer.size()); + 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); + uint32_t readPackets = 0; + result = spacePacketParser->parseSpacePackets(receptionBuffer.data(), readAmount, readPackets); if(result == SpacePacketParser::NO_PACKET_FOUND) { ringBuffer.deleteData(availableReadData); lastRingBufferSize = ringBuffer.getAvailableReadData(); - sif::debug << lastRingBufferSize << std::endl; } else if(result == HasReturnvaluesIF::RETURN_OK) { // Space Packets were found. Handle them here - auto fifo = spacePacketParser->fifo(); + auto& fifo = spacePacketParser->fifo(); SpacePacketParser::IndexSizePair idxSizePair; while(not fifo.empty()) { fifo.retrieve(&idxSizePair); - sif::debug << "handle tc" << std::endl; result = handleTcReception(receptionBuffer.data() + idxSizePair.first, idxSizePair.second); + std::memset(receptionBuffer.data() + idxSizePair.first, 0, idxSizePair.second); ringBuffer.deleteData(idxSizePair.second); if(result != HasReturnvaluesIF::RETURN_OK) { status = result; } lastRingBufferSize = ringBuffer.getAvailableReadData(); - sif::debug << lastRingBufferSize << std::endl; } } - std::memset(receptionBuffer.data(), 0, receptionBuffer.size()); return status; } 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(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(tcStatus)); + sif::printWarning("PUSDistributor::handlePacket: Packet format invalid, " + "%s error\n", keyword); #endif #endif } diff --git a/src/fsfw/tmtcservices/SpacePacketParser.cpp b/src/fsfw/tmtcservices/SpacePacketParser.cpp index 7e510900..e48fe525 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.cpp +++ b/src/fsfw/tmtcservices/SpacePacketParser.cpp @@ -5,22 +5,22 @@ SpacePacketParser::SpacePacketParser(uint16_t maxExpectedPusPackets, bool storeS indexSizePairFIFO(maxExpectedPusPackets) { } -ReturnValue_t SpacePacketParser::parsePusPackets(const uint8_t *frame, - size_t frameSize) { +ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t *frame, + size_t frameSize, uint32_t& foundPackets) { if(frame == nullptr or frameSize < 5) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PusParser::parsePusPackets: Frame invalid!" << std::endl; + sif::error << "PusParser::parsePusPackets: Frame invalid" << std::endl; #else - sif::printError("PusParser::parsePusPackets: Frame invalid!\n"); + sif::printError("PusParser::parsePusPackets: Frame invalid\n"); #endif return HasReturnvaluesIF::RETURN_FAILED; } if(indexSizePairFIFO.full()) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PusParser::parsePusPackets: FIFO is full!" << std::endl; + sif::error << "PusParser::parsePusPackets: FIFO is full" << std::endl; #else - sif::printError("PusParser::parsePusPackets: FIFO is full!\n"); + sif::printError("PusParser::parsePusPackets: FIFO is full\n"); #endif return HasReturnvaluesIF::RETURN_FAILED; } @@ -31,12 +31,12 @@ ReturnValue_t SpacePacketParser::parsePusPackets(const uint8_t *frame, return NO_PACKET_FOUND; } + // Size of a space packet is the value in the packet length field plus 7 size_t packetSize = lengthField + 7; - // sif::debug << frameSize << std::endl; - // Size of a pus packet is the value in the packet length field plus 7. if(packetSize > frameSize) { if(storeSplitPackets) { indexSizePairFIFO.insert(IndexSizePair(0, frameSize)); + foundPackets = 1; } else { #if FSFW_VERBOSE_LEVEL >= 1 @@ -57,19 +57,20 @@ ReturnValue_t SpacePacketParser::parsePusPackets(const uint8_t *frame, } else { indexSizePairFIFO.insert(IndexSizePair(0, packetSize)); - if(packetSize == frameSize) { + if(packetSize >= frameSize) { + foundPackets = 1; return HasReturnvaluesIF::RETURN_OK; } } // packet size is smaller than frame size, parse for more packets. - return readMultiplePackets(frame, frameSize, packetSize); + return readMultiplePackets(frame, frameSize, packetSize, foundPackets); } ReturnValue_t SpacePacketParser::readMultiplePackets(const uint8_t *frame, - size_t frameSize, size_t startIndex) { + size_t frameSize, size_t startIndex, uint32_t& foundPackets) { while (startIndex < frameSize) { - ReturnValue_t result = readNextPacket(frame, frameSize, startIndex); + ReturnValue_t result = readNextPacket(frame, frameSize, startIndex, foundPackets); if(result != HasReturnvaluesIF::RETURN_OK) { return result; } @@ -88,8 +89,7 @@ SpacePacketParser::IndexSizePair SpacePacketParser::getNextFifoPair() { } ReturnValue_t SpacePacketParser::readNextPacket(const uint8_t *frame, - size_t frameSize, size_t& currentIndex) { - // sif::debug << startIndex << std::endl; + size_t frameSize, size_t& currentIndex, uint32_t& foundPackets) { if(currentIndex + 5 > frameSize) { currentIndex = frameSize; return HasReturnvaluesIF::RETURN_OK; @@ -108,6 +108,7 @@ ReturnValue_t SpacePacketParser::readNextPacket(const uint8_t *frame, { if(storeSplitPackets) { indexSizePairFIFO.insert(IndexSizePair(currentIndex, remainingSize)); + foundPackets += 1; } else { #if FSFW_VERBOSE_LEVEL >= 1 @@ -139,6 +140,7 @@ ReturnValue_t SpacePacketParser::readNextPacket(const uint8_t *frame, "FIFO, it is full!\n"); #endif } + foundPackets += 1; currentIndex += nextPacketSize; return result; diff --git a/src/fsfw/tmtcservices/SpacePacketParser.h b/src/fsfw/tmtcservices/SpacePacketParser.h index d0d7bc4d..5cd093dd 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.h +++ b/src/fsfw/tmtcservices/SpacePacketParser.h @@ -53,7 +53,7 @@ public: * -@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 parsePusPackets(const uint8_t* frame, size_t frameSize); + ReturnValue_t parseSpacePackets(const uint8_t* frame, size_t frameSize, uint32_t& foundPackets); /** * Accessor function to get a reference to the internal FIFO which @@ -82,9 +82,9 @@ private: bool storeSplitPackets = false; ReturnValue_t readMultiplePackets(const uint8_t *frame, size_t frameSize, - size_t startIndex); + size_t startIndex, uint32_t& foundPackets); ReturnValue_t readNextPacket(const uint8_t *frame, - size_t frameSize, size_t& startIndex); + size_t frameSize, size_t& startIndex, uint32_t& foundPackets); }; #endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */ From 5fd7a8c9b76bae2d7d019e0175b481083e7ba460 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 11:03:17 +0200 Subject: [PATCH 10/24] smaller tweaks --- src/fsfw/osal/common/TcpTmTcServer.cpp | 4 ++-- src/fsfw/osal/common/TcpTmTcServer.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 2e65429f..6c8ec781 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -185,7 +185,7 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { size_t availableReadData = ringBuffer.getAvailableReadData(); if(availableReadData > lastRingBufferSize) { tcAvailable = true; - handleRingBufferData(availableReadData); + handleTcRingBufferData(availableReadData); } ReturnValue_t result = handleTmSending(connSocket, tmSent); if(result == CONN_BROKEN) { @@ -287,7 +287,7 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent) return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t TcpTmTcServer::handleRingBufferData(size_t availableReadData) { +ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) { ReturnValue_t status = HasReturnvaluesIF::RETURN_OK; ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; size_t readAmount = availableReadData; diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h index ff7757d0..4a5b3e64 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/src/fsfw/osal/common/TcpTmTcServer.h @@ -129,7 +129,7 @@ private: void handleServerOperation(socket_t& connSocket); ReturnValue_t handleTcReception(uint8_t* spacePacket, size_t packetSize); ReturnValue_t handleTmSending(socket_t connSocket, bool& tmSent); - ReturnValue_t handleRingBufferData(size_t availableReadData); + ReturnValue_t handleTcRingBufferData(size_t availableReadData); void handleSocketError(ConstStorageAccessor& accessor); }; From 80ccaede025480a3e3d803c2ba220786a0d42ff2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 14:14:59 +0200 Subject: [PATCH 11/24] refactored space packet parser --- src/fsfw/osal/common/TcpTmTcServer.cpp | 45 +++-- src/fsfw/osal/common/TcpTmTcServer.h | 3 +- src/fsfw/tmtcservices/SpacePacketParser.cpp | 202 +++++++------------- src/fsfw/tmtcservices/SpacePacketParser.h | 26 ++- 4 files changed, 109 insertions(+), 167 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 6c8ec781..b2a5b007 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -33,7 +33,7 @@ TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge, ReceptionModes receptionMode): SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), receptionMode(receptionMode), tcpConfig(customTcpServerPort), receptionBuffer(receptionBufferSize), - ringBuffer(ringBufferSize, true) { + ringBuffer(ringBufferSize, true), validPacketIds() { } ReturnValue_t TcpTmTcServer::initialize() { @@ -46,8 +46,7 @@ ReturnValue_t TcpTmTcServer::initialize() { switch(receptionMode) { case(ReceptionModes::SPACE_PACKETS): { - // For now, hardcode a maximum of 5 store packets here and no split packets are allowed - spacePacketParser = new SpacePacketParser(5, false); + spacePacketParser = new SpacePacketParser(validPacketIds); if(spacePacketParser == nullptr) { return HasReturnvaluesIF::RETURN_FAILED; } @@ -246,7 +245,8 @@ std::string TcpTmTcServer::getTcpPort() const { return tcpConfig.tcpPort; } -void TcpTmTcServer::setSpacePacketParsingOptions(uint8_t parserFifoSize) { +void TcpTmTcServer::setSpacePacketParsingOptions(std::vector validPacketIds) { + this->validPacketIds = validPacketIds; } TcpTmTcServer::TcpConfig& TcpTmTcServer::getTcpConfigStruct() { @@ -319,27 +319,32 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) { readAmount = receptionBuffer.size(); } ringBuffer.readData(receptionBuffer.data(), readAmount, true); - uint32_t readPackets = 0; - result = spacePacketParser->parseSpacePackets(receptionBuffer.data(), readAmount, readPackets); - if(result == SpacePacketParser::NO_PACKET_FOUND) { - ringBuffer.deleteData(availableReadData); - lastRingBufferSize = ringBuffer.getAvailableReadData(); - } - else if(result == HasReturnvaluesIF::RETURN_OK) { - // Space Packets were found. Handle them here - auto& fifo = spacePacketParser->fifo(); - SpacePacketParser::IndexSizePair idxSizePair; - while(not fifo.empty()) { - fifo.retrieve(&idxSizePair); - result = handleTcReception(receptionBuffer.data() + idxSizePair.first, - idxSizePair.second); - std::memset(receptionBuffer.data() + idxSizePair.first, 0, idxSizePair.second); - ringBuffer.deleteData(idxSizePair.second); + const uint8_t* bufPtr = receptionBuffer.data(); + const uint8_t** bufPtrPtr = &bufPtr; + size_t startIdx = 0; + size_t foundSize = 0; + size_t readLen = 0; + while(readLen <= readAmount) { + result = spacePacketParser->parseSpacePackets(bufPtrPtr, readAmount, + startIdx, foundSize, readLen); + if(result == SpacePacketParser::NO_PACKET_FOUND) { + ringBuffer.deleteData(foundSize); + lastRingBufferSize = ringBuffer.getAvailableReadData(); + } + else if(result == SpacePacketParser::SPLIT_PACKET) { + // might be half of a packet? Skip it for now + ringBuffer.deleteData(foundSize); + lastRingBufferSize = ringBuffer.getAvailableReadData(); + } + else if(result == HasReturnvaluesIF::RETURN_OK) { + result = handleTcReception(receptionBuffer.data() + startIdx, foundSize); + ringBuffer.deleteData(foundSize); if(result != HasReturnvaluesIF::RETURN_OK) { status = result; } lastRingBufferSize = ringBuffer.getAvailableReadData(); } + std::memset(receptionBuffer.data() + startIdx, 0, foundSize); } return status; } diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h index 4a5b3e64..559b2c8c 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/src/fsfw/osal/common/TcpTmTcServer.h @@ -97,7 +97,7 @@ public: * @return */ TcpConfig& getTcpConfigStruct(); - void setSpacePacketParsingOptions(uint8_t parserFifoSize); + void setSpacePacketParsingOptions(std::vector validPacketIds); ReturnValue_t initialize() override; ReturnValue_t performOperation(uint8_t opCode) override; @@ -123,6 +123,7 @@ private: std::vector receptionBuffer; SimpleRingBuffer ringBuffer; + std::vector validPacketIds; SpacePacketParser* spacePacketParser = nullptr; uint8_t lastRingBufferSize = 0; diff --git a/src/fsfw/tmtcservices/SpacePacketParser.cpp b/src/fsfw/tmtcservices/SpacePacketParser.cpp index e48fe525..84f861cf 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.cpp +++ b/src/fsfw/tmtcservices/SpacePacketParser.cpp @@ -1,147 +1,77 @@ #include #include +#include -SpacePacketParser::SpacePacketParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets): - indexSizePairFIFO(maxExpectedPusPackets) { +SpacePacketParser::SpacePacketParser(std::vector validPacketIds): + validPacketIds(validPacketIds) { } -ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t *frame, - size_t frameSize, uint32_t& foundPackets) { - if(frame == nullptr or frameSize < 5) { +ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t *buffer, + const size_t maxSize, size_t& startIndex, size_t& foundSize) { + const uint8_t** tempPtr = &buffer; + size_t readLen = 0; + return parseSpacePackets(tempPtr, maxSize, startIndex, foundSize, readLen); +} + +ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t **buffer, const size_t maxSize, + size_t &startIndex, size_t &foundSize, size_t& readLen) { + if(buffer == nullptr or maxSize < 5) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PusParser::parsePusPackets: Frame invalid" << std::endl; -#else - sif::printError("PusParser::parsePusPackets: Frame invalid\n"); + sif::warning << "SpacePacketParser::parseSpacePackets: Frame invalid" << std::endl; +#else + sif::printWarning("SpacePacketParser::parseSpacePackets: Frame invalid\n"); #endif - return HasReturnvaluesIF::RETURN_FAILED; - } - - if(indexSizePairFIFO.full()) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PusParser::parsePusPackets: FIFO is full" << std::endl; -#else - sif::printError("PusParser::parsePusPackets: FIFO is full\n"); -#endif - return HasReturnvaluesIF::RETURN_FAILED; - } - - size_t lengthField = frame[4] << 8 | frame[5]; - - if(lengthField == 0) { - return NO_PACKET_FOUND; - } - - // Size of a space packet is the value in the packet length field plus 7 - size_t packetSize = lengthField + 7; - if(packetSize > frameSize) { - if(storeSplitPackets) { - indexSizePairFIFO.insert(IndexSizePair(0, frameSize)); - foundPackets = 1; - } - else { -#if FSFW_VERBOSE_LEVEL >= 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "TcSerialPollingTask::readNextPacket: Next packet " - << "larger than remaining frame," << std::endl; - sif::warning << "Throwing away packet. Detected packet size: " - << packetSize << std::endl; -#else - sif::printWarning("TcSerialPollingTask::readNextPacket: Next packet " - "larger than remaining frame.\n"); - sif::printWarning("Throwing away packet. Detected packet size: %lu", - static_cast(packetSize)); -#endif -#endif /* FSFW_VERBOSE_LEVEL >= 1 */ - } - return SPLIT_PACKET; - } - else { - indexSizePairFIFO.insert(IndexSizePair(0, packetSize)); - if(packetSize >= frameSize) { - foundPackets = 1; - return HasReturnvaluesIF::RETURN_OK; - } - } - - // packet size is smaller than frame size, parse for more packets. - return readMultiplePackets(frame, frameSize, packetSize, foundPackets); -} - -ReturnValue_t SpacePacketParser::readMultiplePackets(const uint8_t *frame, - size_t frameSize, size_t startIndex, uint32_t& foundPackets) { - while (startIndex < frameSize) { - ReturnValue_t result = readNextPacket(frame, frameSize, startIndex, foundPackets); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - } - return HasReturnvaluesIF::RETURN_OK; -} - -DynamicFIFO& SpacePacketParser::fifo(){ - return indexSizePairFIFO; -} - -SpacePacketParser::IndexSizePair SpacePacketParser::getNextFifoPair() { - IndexSizePair nextIndexSizePair; - indexSizePairFIFO.retrieve(&nextIndexSizePair); - return nextIndexSizePair; -} - -ReturnValue_t SpacePacketParser::readNextPacket(const uint8_t *frame, - size_t frameSize, size_t& currentIndex, uint32_t& foundPackets) { - if(currentIndex + 5 > frameSize) { - currentIndex = frameSize; - return HasReturnvaluesIF::RETURN_OK; + return HasReturnvaluesIF::RETURN_FAILED; } + const uint8_t* bufPtr = *buffer; - uint16_t lengthField = frame[currentIndex + 4] << 8 | - frame[currentIndex + 5]; - if(lengthField == 0) { - // It is assumed that no packet follows. - currentIndex = frameSize; - return HasReturnvaluesIF::RETURN_OK; - } - size_t nextPacketSize = lengthField + 7; - size_t remainingSize = frameSize - currentIndex; - if(nextPacketSize > remainingSize) - { - if(storeSplitPackets) { - indexSizePairFIFO.insert(IndexSizePair(currentIndex, remainingSize)); - foundPackets += 1; - } - else { -#if FSFW_VERBOSE_LEVEL >= 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "TcSerialPollingTask::readNextPacket: Next packet " - << "larger than remaining frame." << std::endl; - sif::warning << "Throwing away packet. Detected packet size: " - << nextPacketSize << std::endl; -#else - sif::printWarning("TcSerialPollingTask::readNextPacket: Next packet " - "larger than remaining frame.\n"); - sif::printWarning("Throwing away packet. Detected packet size: %lu\n", - static_cast(nextPacketSize)); -#endif -#endif - } - return SPLIT_PACKET; - } + auto verifyLengthField = [&](size_t idx) { + uint16_t lengthField = bufPtr[idx + 4] << 8 | bufPtr[idx + 5]; + size_t packetSize = lengthField + 7; + startIndex = idx; + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + if(lengthField == 0) { + // Skip whole header for now + foundSize = 6; + result = NO_PACKET_FOUND; + } + else if(packetSize + idx > maxSize) { + // Don't increment buffer and read length here, user has to decide what to do + foundSize = packetSize; + return SPLIT_PACKET; + } + else { + foundSize = packetSize; + } + *buffer += foundSize; + readLen += foundSize; + return result; + }; - ReturnValue_t result = indexSizePairFIFO.insert(IndexSizePair(currentIndex, - nextPacketSize)); - if (result != HasReturnvaluesIF::RETURN_OK) { - // FIFO full. -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::debug << "PusParser: Issue inserting into start index size " - << "FIFO, it is full!" << std::endl; -#else - sif::printDebug("PusParser: Issue inserting into start index size " - "FIFO, it is full!\n"); -#endif - } - foundPackets += 1; - currentIndex += nextPacketSize; - - return result; + size_t idx = 0; + // Space packet ID as start marker + if(validPacketIds.size() > 0) { + while(idx < maxSize - 5) { + uint16_t currentPacketId = bufPtr[idx] << 8 | bufPtr[idx + 1]; + if(std::find(validPacketIds.begin(), validPacketIds.end(), currentPacketId) != + validPacketIds.end()) { + if(idx + 5 >= maxSize) { + return SPLIT_PACKET; + } + return verifyLengthField(idx); + } + else { + idx++; + } + } + startIndex = 0; + foundSize = maxSize; + *buffer += foundSize; + readLen += foundSize; + return NO_PACKET_FOUND; + } + // Assume that the user verified a valid start of a space packet + else { + return verifyLengthField(idx); + } } diff --git a/src/fsfw/tmtcservices/SpacePacketParser.h b/src/fsfw/tmtcservices/SpacePacketParser.h index 5cd093dd..16b53ea4 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.h +++ b/src/fsfw/tmtcservices/SpacePacketParser.h @@ -42,26 +42,30 @@ public: * Specifies whether split packets are also stored inside the FIFO, * with the size being the remaining frame size. */ - SpacePacketParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets); + SpacePacketParser(std::vector validPacketIds); /** * Parse a given frame for PUS packets * @param frame * @param frameSize + * @param foundPackets The number of found packets will be stored here * @return * -@c NO_PACKET_FOUND if no packet was found * -@c SPLIT_PACKET if splitting is enabled and a split packet was found * -@c RETURN_OK if a packet was found. The index and sizes are stored in the internal FIFO */ - ReturnValue_t parseSpacePackets(const uint8_t* frame, size_t frameSize, uint32_t& foundPackets); + ReturnValue_t parseSpacePackets(const uint8_t* buffer, const size_t maxSize, + size_t& startIndex, size_t& foundSize); + ReturnValue_t parseSpacePackets(const uint8_t **buffer, const size_t maxSize, + size_t& startIndex, size_t& foundSize, size_t& readLen); /** * Accessor function to get a reference to the internal FIFO which * stores pairs of index and packet sizes. This FIFO is filled * by the #parsePusPackets function. * @return */ - DynamicFIFO& fifo(); + //DynamicFIFO& fifo(); /** * Retrieve the next index and packet size pair from the FIFO. @@ -69,7 +73,7 @@ public: * is empty, an empty pair will be returned. * @return */ - IndexSizePair getNextFifoPair(); + //IndexSizePair getNextFifoPair(); private: /** @@ -77,14 +81,16 @@ private: * inside the receive buffer. The maximum number of entries is defined * by the first constructor argument. */ - DynamicFIFO indexSizePairFIFO; + //DynamicFIFO indexSizePairFIFO; - bool storeSplitPackets = false; + std::vector validPacketIds; - ReturnValue_t readMultiplePackets(const uint8_t *frame, size_t frameSize, - size_t startIndex, uint32_t& foundPackets); - ReturnValue_t readNextPacket(const uint8_t *frame, - size_t frameSize, size_t& startIndex, uint32_t& foundPackets); + //bool storeSplitPackets = false; + +// ReturnValue_t readMultiplePackets(const uint8_t *frame, size_t frameSize, +// size_t startIndex, uint32_t& foundPackets); +// ReturnValue_t readNextPacket(const uint8_t *frame, +// size_t frameSize, size_t& startIndex, uint32_t& foundPackets); }; #endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */ From c7ce568a302538ef98e308add1bcae632563c48e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 14:50:50 +0200 Subject: [PATCH 12/24] added function to determine space packet ID from APID --- src/fsfw/osal/common/TcpTmTcServer.cpp | 21 ++--- src/fsfw/osal/common/TcpTmTcServer.h | 2 +- src/fsfw/tmtcpacket/SpacePacket.h | 110 ++++++++++++++----------- 3 files changed, 70 insertions(+), 63 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index b2a5b007..e320b46b 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -324,26 +324,23 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) { size_t startIdx = 0; size_t foundSize = 0; size_t readLen = 0; - while(readLen <= readAmount) { + while(readLen < readAmount) { result = spacePacketParser->parseSpacePackets(bufPtrPtr, readAmount, startIdx, foundSize, readLen); - if(result == SpacePacketParser::NO_PACKET_FOUND) { - ringBuffer.deleteData(foundSize); - lastRingBufferSize = ringBuffer.getAvailableReadData(); + switch(result) { + case(SpacePacketParser::NO_PACKET_FOUND): + case(SpacePacketParser::SPLIT_PACKET): { + break; } - else if(result == SpacePacketParser::SPLIT_PACKET) { - // might be half of a packet? Skip it for now - ringBuffer.deleteData(foundSize); - lastRingBufferSize = ringBuffer.getAvailableReadData(); - } - else if(result == HasReturnvaluesIF::RETURN_OK) { + case(HasReturnvaluesIF::RETURN_OK): { result = handleTcReception(receptionBuffer.data() + startIdx, foundSize); - ringBuffer.deleteData(foundSize); if(result != HasReturnvaluesIF::RETURN_OK) { status = result; } - lastRingBufferSize = ringBuffer.getAvailableReadData(); } + } + ringBuffer.deleteData(foundSize); + lastRingBufferSize = ringBuffer.getAvailableReadData(); std::memset(receptionBuffer.data() + startIdx, 0, foundSize); } return status; diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h index 559b2c8c..a0a31655 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/src/fsfw/osal/common/TcpTmTcServer.h @@ -127,7 +127,7 @@ private: SpacePacketParser* spacePacketParser = nullptr; uint8_t lastRingBufferSize = 0; - void handleServerOperation(socket_t& connSocket); + virtual void handleServerOperation(socket_t& connSocket); ReturnValue_t handleTcReception(uint8_t* spacePacket, size_t packetSize); ReturnValue_t handleTmSending(socket_t connSocket, bool& tmSent); ReturnValue_t handleTcRingBufferData(size_t availableReadData); diff --git a/src/fsfw/tmtcpacket/SpacePacket.h b/src/fsfw/tmtcpacket/SpacePacket.h index 2957576f..9eec87a8 100644 --- a/src/fsfw/tmtcpacket/SpacePacket.h +++ b/src/fsfw/tmtcpacket/SpacePacket.h @@ -15,57 +15,67 @@ */ class SpacePacket: public SpacePacketBase { public: - static const uint16_t PACKET_MAX_SIZE = 1024; - /** - * The constructor initializes the packet and sets all header information - * according to the passed parameters. - * @param packetDataLength Sets the packet data length field and therefore specifies - * the size of the packet. - * @param isTelecommand Sets the packet type field to either TC (true) or TM (false). - * @param apid Sets the packet's APID field. The default value describes an idle packet. - * @param sequenceCount ets the packet's Source Sequence Count field. - */ - SpacePacket(uint16_t packetDataLength, bool isTelecommand = false, - uint16_t apid = APID_IDLE_PACKET, uint16_t sequenceCount = 0); - /** - * The class's default destructor. - */ - virtual ~SpacePacket(); - /** - * With this call, the complete data content (including the CCSDS Primary - * Header) is overwritten with the byte stream given. - * @param p_data Pointer to data to overwrite the content with - * @param packet_size Size of the data - * @return @li \c true if packet_size is smaller than \c MAX_PACKET_SIZE. - * @li \c false else. - */ - bool addWholeData(const uint8_t* p_data, uint32_t packet_size); + static const uint16_t PACKET_MAX_SIZE = 1024; + /** + * The constructor initializes the packet and sets all header information + * according to the passed parameters. + * @param packetDataLength Sets the packet data length field and therefore specifies + * the size of the packet. + * @param isTelecommand Sets the packet type field to either TC (true) or TM (false). + * @param apid Sets the packet's APID field. The default value describes an idle packet. + * @param sequenceCount ets the packet's Source Sequence Count field. + */ + SpacePacket(uint16_t packetDataLength, bool isTelecommand = false, + uint16_t apid = APID_IDLE_PACKET, uint16_t sequenceCount = 0); + /** + * The class's default destructor. + */ + virtual ~SpacePacket(); + + static constexpr uint16_t getTcSpacePacketIdFromApid(uint16_t apid) { + uint16_t tcPacketId = (0x18 << 8) | (((apid >> 8) & 0x07) << 8) | (apid & 0x00ff); + return tcPacketId; + } + static constexpr uint16_t getTmSpacePacketIdFromApid(uint16_t apid) { + uint16_t tmPacketId = (0x08 << 8) | (((apid >> 8) & 0x07) << 8) | (apid & 0x00ff); + return tmPacketId; + } + + /** + * With this call, the complete data content (including the CCSDS Primary + * Header) is overwritten with the byte stream given. + * @param p_data Pointer to data to overwrite the content with + * @param packet_size Size of the data + * @return @li \c true if packet_size is smaller than \c MAX_PACKET_SIZE. + * @li \c false else. + */ + bool addWholeData(const uint8_t* p_data, uint32_t packet_size); protected: - /** - * This structure defines the data structure of a Space Packet as local data. - * There's a buffer which corresponds to the Space Packet Data Field with a - * maximum size of \c PACKET_MAX_SIZE. - */ - struct PacketStructured { - CCSDSPrimaryHeader header; - uint8_t buffer[PACKET_MAX_SIZE]; - }; - /** - * This union simplifies accessing the full data content of the Space Packet. - * This is achieved by putting the \c PacketStructured struct in a union with - * a plain buffer. - */ - union SpacePacketData { - PacketStructured fields; - uint8_t byteStream[PACKET_MAX_SIZE + sizeof(CCSDSPrimaryHeader)]; - }; - /** - * This is the data representation of the class. - * It is a struct of CCSDS Primary Header and a data field, which again is - * packed in an union, so the data can be accessed as a byte stream without - * a cast. - */ - SpacePacketData localData; + /** + * This structure defines the data structure of a Space Packet as local data. + * There's a buffer which corresponds to the Space Packet Data Field with a + * maximum size of \c PACKET_MAX_SIZE. + */ + struct PacketStructured { + CCSDSPrimaryHeader header; + uint8_t buffer[PACKET_MAX_SIZE]; + }; + /** + * This union simplifies accessing the full data content of the Space Packet. + * This is achieved by putting the \c PacketStructured struct in a union with + * a plain buffer. + */ + union SpacePacketData { + PacketStructured fields; + uint8_t byteStream[PACKET_MAX_SIZE + sizeof(CCSDSPrimaryHeader)]; + }; + /** + * This is the data representation of the class. + * It is a struct of CCSDS Primary Header and a data field, which again is + * packed in an union, so the data can be accessed as a byte stream without + * a cast. + */ + SpacePacketData localData; }; #endif /* SPACEPACKET_H_ */ From 5a045d03a542a9201aaacba7c7e96fc040d0bba6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 15:10:50 +0200 Subject: [PATCH 13/24] 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 e320b46b..5be9373e 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 a0a31655..d5214848 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 f2020b24923b77390ff5234e579a5aa9fc492f4e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 15:12:23 +0200 Subject: [PATCH 14/24] 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 5be9373e..fb421fc7 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 304d7e8e32a45d4223ba833a7ca490ea810de2c0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 15:30:31 +0200 Subject: [PATCH 15/24] space packet parser cleaned up, documentation --- 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 84f861cf..3d442458 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 16b53ea4..82b15010 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 be8623a4f85d4445b218873bddd6bcf0ed24fb5f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 15:32:58 +0200 Subject: [PATCH 16/24] 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 fb421fc7..c3936146 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 d5214848..da0e8bd5 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 d4bdf314f74e75f3260042682718299d0655d494 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 16:31:53 +0200 Subject: [PATCH 17/24] 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 9eec87a8..67746859 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 82b15010..bed3369b 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 6881c6b66a5a3d11756485f8f845928aea9a705e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 16:32:43 +0200 Subject: [PATCH 18/24] class id renamed --- src/fsfw/returnvalues/FwClassIds.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/returnvalues/FwClassIds.h b/src/fsfw/returnvalues/FwClassIds.h index 8422baa5..9fa0c9ae 100644 --- a/src/fsfw/returnvalues/FwClassIds.h +++ b/src/fsfw/returnvalues/FwClassIds.h @@ -80,7 +80,7 @@ enum: uint8_t { FIXED_SLOT_TASK_IF, //FTIF MGM_LIS3MDL, //MGMLIS3 MGM_RM3100, //MGMRM3100 - PUS_PARSER, //PUSP + SPACE_PACKET_PARSER, //SPPA FW_CLASS_ID_COUNT // [EXPORT] : [END] }; From 01e380c85897706b1be3a17ef5692dc82dc1ac2d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 17:36:27 +0200 Subject: [PATCH 19/24] windows fixes --- src/fsfw/osal/common/TcpTmTcServer.cpp | 33 +++++++++++++++++++++++--- src/fsfw/osal/common/TcpTmTcServer.h | 3 +++ 2 files changed, 33 insertions(+), 3 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index c3936146..4348e21e 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -46,10 +46,11 @@ ReturnValue_t TcpTmTcServer::initialize() { 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 @@ -155,6 +156,10 @@ ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() { } void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { +#if defined PLATFORM_WIN + setSocketNonBlocking(connSocket); +#endif + while (true) { int retval = recv( connSocket, @@ -172,7 +177,13 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { ringBuffer.writeData(receptionBuffer.data(), retval); } else if(retval < 0) { - if(errno == EAGAIN) { + 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; @@ -191,7 +202,7 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { } } else { - tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL); + tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL, 300); } } } @@ -375,3 +386,19 @@ void TcpTmTcServer::handleSocketError(ConstStorageAccessor &accessor) { } } } + +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 da0e8bd5..64726a30 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/src/fsfw/osal/common/TcpTmTcServer.h @@ -136,6 +136,9 @@ private: 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 69922e77c501ddaef84481c96e25801dc1889c11 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 17:38:35 +0200 Subject: [PATCH 20/24] 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 4348e21e..519df547 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 5ee315f8ca3b0bb27d5a368948ecd115c6a055c9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Sep 2021 17:42:29 +0200 Subject: [PATCH 21/24] 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 67746859..fe8a1044 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 354e158cc196d7deaf7de24ebcc96f5f6f5f20eb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 29 Sep 2021 09:30:50 +0200 Subject: [PATCH 22/24] format fixes --- tests/user/testcfg/CMakeLists.txt | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) 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 From 04cb8e82f1aaae89e20985539c74c1e36bb85ae3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 29 Sep 2021 10:52:21 +0200 Subject: [PATCH 23/24] improvements and fixes --- CMakeLists.txt | 2 +- src/fsfw/FSFW.h.in | 8 +++++ src/fsfw/osal/common/TcpTmTcServer.cpp | 8 +++++ src/fsfw/osal/linux/CommandExecutor.cpp | 33 ++++++++++++++++--- .../tmtcpacket/pus/tm/TmPacketStoredPusC.cpp | 2 +- 5 files changed, 47 insertions(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5ff66a6d..584dd943 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -186,4 +186,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/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 519df547..8b34b1a3 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, 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( @@ -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(output.data()), output.size()); 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; } From 3d0ce1998114c5d7a43034233ee03a800c8821b0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 29 Sep 2021 10:58:01 +0200 Subject: [PATCH 24/24] additional options for c ustom main --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 584dd943..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)