From a51aae30a3802d753b140147f7bf485528b2addf Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Mon, 6 Jul 2020 23:40:57 +0200 Subject: [PATCH] PusParser init --- tmtcservices/PusParser.cpp | 115 +++++++++++++++++++++++++++++++++++++ tmtcservices/PusParser.h | 82 ++++++++++++++++++++++++++ 2 files changed, 197 insertions(+) create mode 100644 tmtcservices/PusParser.cpp create mode 100644 tmtcservices/PusParser.h diff --git a/tmtcservices/PusParser.cpp b/tmtcservices/PusParser.cpp new file mode 100644 index 000000000..175c28934 --- /dev/null +++ b/tmtcservices/PusParser.cpp @@ -0,0 +1,115 @@ +#include +#include + +PusParser::PusParser(uint16_t maxExpectedPusPackets, + bool storeSplitPackets): indexSizePairFIFO(maxExpectedPusPackets) { +} + +ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame, + size_t frameSize) { + if(frame == nullptr) { + sif::error << "PusParser::parsePusPackets: Frame pointers in invalid!" + << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + + if(indexSizePairFIFO.full()) { + sif::error << "PusParser::parsePusPackets: FIFO is full!" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + + size_t lengthField = frame[4] << 8 | frame[5]; + + if(lengthField == 0) { + return NO_PACKET_FOUND; + } + + size_t packetSize = lengthField + 7; + // 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 + { + sif::debug << "TcSerialPollingTask::readNextPacket: Next packet " + "larger than remaining frame," << std::endl; + sif::debug << "Throwing away packet. Detected packet size: " + << packetSize << std::endl; + } + 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; +} + +fsfw::FIFO* 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; + 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 + { + sif::debug << "TcSerialPollingTask::readNextPacket: Next packet " + "larger than remaining frame," << std::endl; + sif::debug << "Throwing away packet. Detected packet size: " + << nextPacketSize << std::endl; + } + return SPLIT_PACKET; + } + + ReturnValue_t result = indexSizePairFIFO.insert(indexSizePair(currentIndex, + nextPacketSize)); + if (result != HasReturnvaluesIF::RETURN_OK) { + // FIFO full. + sif::debug << "PusParser: Issue inserting into start index size " + "FIFO, it is full!" << std::endl; + } + currentIndex += nextPacketSize; + + return result; +} diff --git a/tmtcservices/PusParser.h b/tmtcservices/PusParser.h new file mode 100644 index 000000000..82aff57e1 --- /dev/null +++ b/tmtcservices/PusParser.h @@ -0,0 +1,82 @@ +#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. + */ +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 + */ + fsfw::FIFO* 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. + fsfw::FIFO 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_ */