From 9483c2809d2a8c573b56dafac2e820b016021178 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 11:26:53 +0100 Subject: [PATCH] DLE parser --- src/fsfw/globalfunctions/CMakeLists.txt | 1 + src/fsfw/globalfunctions/DleParser.cpp | 173 ++++++++++++++++++++++++ src/fsfw/globalfunctions/DleParser.h | 127 +++++++++++++++++ 3 files changed, 301 insertions(+) create mode 100644 src/fsfw/globalfunctions/DleParser.cpp create mode 100644 src/fsfw/globalfunctions/DleParser.h diff --git a/src/fsfw/globalfunctions/CMakeLists.txt b/src/fsfw/globalfunctions/CMakeLists.txt index cfa02696..cf8a25d5 100644 --- a/src/fsfw/globalfunctions/CMakeLists.txt +++ b/src/fsfw/globalfunctions/CMakeLists.txt @@ -4,6 +4,7 @@ target_sources( AsciiConverter.cpp CRC.cpp DleEncoder.cpp + DleParser.cpp PeriodicOperationDivider.cpp timevalOperations.cpp Type.cpp diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp new file mode 100644 index 00000000..cc695bab --- /dev/null +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -0,0 +1,173 @@ +#include "DleParser.h" + +#include + +#include +#include +#include + +DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf, + BufPair decodedBuf) + : decodeRingBuf(decodeRingBuf), + decoder(decoder), + encodedBuf(encodedBuf), + decodedBuf(decodedBuf) {} + +ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) { + if (data == nullptr or len == 0) { + return returnvalue::FAILED; + } + return decodeRingBuf.writeData(data, len); +} + +ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { + ctx.setType(DleParser::ContextType::NONE); + size_t availableData = decodeRingBuf.getAvailableReadData(); + if (availableData == 0) { + return NO_PACKET_FOUND; + } + if (availableData > encodedBuf.second) { + ErrorInfo info; + info.len = decodeRingBuf.getAvailableReadData(); + setErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); + return returnvalue::FAILED; + } + ReturnValue_t result = decodeRingBuf.readData(encodedBuf.first, availableData); + if (result != returnvalue::OK) { + ErrorInfo info; + info.res = result; + setErrorContext(ErrorTypes::RING_BUF_ERROR, info); + return result; + } + bool stxFound = false; + size_t stxIdx = 0; + for (size_t vectorIdx = 0; vectorIdx < availableData; vectorIdx++) { + // handle STX char + if (encodedBuf.first[vectorIdx] == DleEncoder::STX_CHAR) { + if (not stxFound) { + stxFound = true; + stxIdx = vectorIdx; + } else { + // might be lost packet, so we should advance the read pointer + // without skipping the STX + readSize = vectorIdx; + ErrorInfo info; + setErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info); + return POSSIBLE_PACKET_LOSS; + } + } + // handle ETX char + if (encodedBuf.first[vectorIdx] == DleEncoder::ETX_CHAR) { + if (stxFound) { + // This is propably a packet, so we decode it. + size_t decodedLen = 0; + size_t dummy = 0; + + ReturnValue_t result = + decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, &dummy, + decodedBuf.first, decodedBuf.second, &decodedLen); + if (result == returnvalue::OK) { + ctx.setType(ContextType::PACKET_FOUND); + ctx.decodedPacket.first = decodedBuf.first; + ctx.decodedPacket.second = decodedLen; + readSize = ++vectorIdx; + return returnvalue::OK; + } else { + // invalid packet, skip. + readSize = ++vectorIdx; + ErrorInfo info; + info.res = result; + setErrorContext(ErrorTypes::DECODE_ERROR, info); + return POSSIBLE_PACKET_LOSS; + } + } else { + // might be lost packet, so we should advance the read pointer + readSize = ++vectorIdx; + ErrorInfo info; + info.len = 0; + setErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info); + return POSSIBLE_PACKET_LOSS; + } + } + } + return NO_PACKET_FOUND; +} + +void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args) { +#if FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "DleParserBase::handleFoundPacket: Detected DLE packet with " << len << " bytes" + << std::endl; +#else + sif::printInfo("DleParserBase::handleFoundPacket: Detected DLE packet with %d bytes\n", len); +#endif +#endif +} + +void DleParser::defaultErrorHandler() { + if (ctx.getType() != DleParser::ContextType::ERROR) { + errorPrinter("No error"); + return; + } + switch (ctx.error.first) { + case (ErrorTypes::NONE): { + errorPrinter("No error"); + break; + } + case (ErrorTypes::DECODE_ERROR): { + errorPrinter("Decode Error"); + break; + } + case (ErrorTypes::RING_BUF_ERROR): { + errorPrinter("Ring Buffer Error"); + break; + } + case (ErrorTypes::ENCODED_BUF_TOO_SMALL): + case (ErrorTypes::DECODING_BUF_TOO_SMALL): { + char opt[64]; + snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", + ctx.decodedPacket.second); + if (ctx.error.first == ErrorTypes::ENCODED_BUF_TOO_SMALL) { + errorPrinter("Encoded buf too small", opt); + } else { + errorPrinter("Decoding buf too small", opt); + } + break; + } + case (ErrorTypes::CONSECUTIVE_STX_CHARS): { + errorPrinter("Consecutive STX chars detected"); + break; + } + case (ErrorTypes::CONSECUTIVE_ETX_CHARS): { + errorPrinter("Consecutive ETX chars detected"); + break; + } + } +} + +void DleParser::errorPrinter(const char* str, const char* opt) { + if (opt == nullptr) { + opt = ""; + } +#if FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "DleParserBase::handleParseError: " << str << opt << std::endl; +#else + sif::printInfo("DleParserBase::handleParseError: %s%s\n", str, opt); +#endif +#endif +} + +void DleParser::setErrorContext(ErrorTypes err, ErrorInfo info) { + ctx.setType(ContextType::ERROR); + ctx.error.first = err; + ctx.error.second = info; +} + +ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) { + return decodeRingBuf.deleteData(bytesRead); +} + +const DleParser::Context& DleParser::getContext() { return ctx; } + +void DleParser::reset() { decodeRingBuf.clear(); } diff --git a/src/fsfw/globalfunctions/DleParser.h b/src/fsfw/globalfunctions/DleParser.h new file mode 100644 index 00000000..9802017a --- /dev/null +++ b/src/fsfw/globalfunctions/DleParser.h @@ -0,0 +1,127 @@ +#pragma once + +#include +#include +#include + +#include +#include + +/** + * @brief This base helper class can be used to extract DLE encoded packets from a data stream + * @details + * The core API of the parser takes received packets which can contains DLE packets. The parser + * can deal with DLE packets split across multiple packets. It does so by using a dedicated + * decoding ring buffer. The user can process received packets and detect errors by + * overriding two provided virtual methods. This also allows detecting multiple DLE packets + * inside one passed packet. + */ +class DleParser { + public: + static constexpr ReturnValue_t NO_PACKET_FOUND = returnvalue::makeCode(1, 1); + static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS = returnvalue::makeCode(1, 2); + using BufPair = std::pair; + + enum class ContextType { NONE, PACKET_FOUND, ERROR }; + + enum class ErrorTypes { + NONE, + ENCODED_BUF_TOO_SMALL, + DECODING_BUF_TOO_SMALL, + DECODE_ERROR, + RING_BUF_ERROR, + CONSECUTIVE_STX_CHARS, + CONSECUTIVE_ETX_CHARS + }; + + union ErrorInfo { + size_t len; + ReturnValue_t res; + }; + + using ErrorPair = std::pair; + + struct Context { + public: + Context() { setType(ContextType::PACKET_FOUND); } + + void setType(ContextType type) { + this->type = type; + if (type == ContextType::PACKET_FOUND) { + error.first = ErrorTypes::NONE; + error.second.len = 0; + } else { + decodedPacket.first = nullptr; + decodedPacket.second = 0; + } + } + + ContextType getType() const { return type; } + + BufPair decodedPacket = {}; + ErrorPair error; + + private: + ContextType type; + }; + + /** + * Base class constructor + * @param decodeRingBuf Ring buffer used to store multiple packets to allow detecting DLE packets + * split across multiple packets + * @param decoder Decoder instance + * @param encodedBuf Buffer used to store encoded packets. It has to be large enough to hold + * the largest expected encoded DLE packet size + * @param decodedBuf Buffer used to store decoded packets. It has to be large enough to hold the + * largest expected decoded DLE packet size + * @param handler Function which will be called on a found packet + * @param args Arbitrary user argument + */ + DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf, + BufPair decodedBuf); + + /** + * This function allows to pass new data into the parser. It then scans for DLE packets + * automatically and inserts (part of) the packet into a ring buffer if necessary. + * @param data + * @param len + * @return + */ + ReturnValue_t passData(const uint8_t* data, size_t len); + + ReturnValue_t parseRingBuf(size_t& bytesRead); + + ReturnValue_t confirmBytesRead(size_t bytesRead); + + const Context& getContext(); + /** + * Example found packet handler + * function call + * @param packet Decoded packet + * @param len Length of detected packet + */ + void defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args); + /** + * Will be called if an error occured in the #passData call + * @param err + * @param ctx Context information depending on the error type + * - For buffer length errors, will be set to the detected packet length which is too large + * - For decode or ring buffer errors, will be set to the result returned from the failed call + */ + void defaultErrorHandler(); + + static void errorPrinter(const char* str, const char* opt = nullptr); + + void setErrorContext(ErrorTypes err, ErrorInfo ctx); + /** + * Resets the parser by resetting the internal states and clearing the decoding ring buffer + */ + void reset(); + + private: + SimpleRingBuffer& decodeRingBuf; + DleEncoder& decoder; + BufPair encodedBuf; + BufPair decodedBuf; + Context ctx; +};