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_ */