From 8ba75fc3c2fad46fc0e37fe17a627a99cfa3f09a Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Mon, 6 Jul 2020 19:36:21 +0200 Subject: [PATCH] pus parser implemented --- tmtcservices/PusParser.cpp | 113 ++++++++++++++++++++++++++++++------- tmtcservices/PusParser.h | 32 +++++++++-- 2 files changed, 120 insertions(+), 25 deletions(-) diff --git a/tmtcservices/PusParser.cpp b/tmtcservices/PusParser.cpp index 5f326b44..21d44efa 100644 --- a/tmtcservices/PusParser.cpp +++ b/tmtcservices/PusParser.cpp @@ -13,31 +13,106 @@ ReturnValue_t PusParser::parsePusPackets(const uint8_t *frame, return HasReturnvaluesIF::RETURN_FAILED; } - // Size of a pus packet is the value in the packet length field plus 7. - uint16_t packetSize = (frame[4] << 8 | frame[5]) + 7; - if(packetSize > 0) { - indexSizePairFIFO.insert(indexSizePair(0, packetSize)); - return HasReturnvaluesIF::RETURN_OK; + if(indexSizePairFIFO.full()) { + sif::error << "PusParser::parsePusPackets: FIFO is full!" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; } - else { + + + size_t lengthField = frame[4] << 8 | frame[5]; + + if(lengthField == 0) { return NO_PACKET_FOUND; } - if(frameSize > packetSize) { - return readMultiplePackets(frameSize); + size_t packetSize = lengthField + 7; + if(lengthField > 0) { + // 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; } -//ReturnValue_t PusParser::readMultiplePackets(size_t frameSize) { -// size_t endOfBuffer = frameSize - 1; -// size_t endIndex = firstPacketSize; -// size_t startIndex = 0; -// while (endIndex < endOfBuffer) { -// ReturnValue_t result = readNextPacket(&startIndex, &endIndex); -// if(result != RETURN_OK) { -// return; -// } -// } -//} +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 index cdc887ed..82aff57e 100644 --- a/tmtcservices/PusParser.h +++ b/tmtcservices/PusParser.h @@ -23,9 +23,13 @@ */ 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 @@ -44,10 +48,24 @@ public: * @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: - //! 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; + //! 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. @@ -55,8 +73,10 @@ private: bool storeSplitPackets = false; - ReturnValue_t readMultiplePackets(size_t frameSize); + 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_ */