diff --git a/src/fsfw/globalfunctions/CMakeLists.txt b/src/fsfw/globalfunctions/CMakeLists.txt index 5ccd3c4c..acd1edbe 100644 --- a/src/fsfw/globalfunctions/CMakeLists.txt +++ b/src/fsfw/globalfunctions/CMakeLists.txt @@ -1,13 +1,13 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - arrayprinter.cpp - AsciiConverter.cpp - CRC.cpp - DleEncoder.cpp - PeriodicOperationDivider.cpp - timevalOperations.cpp - Type.cpp - bitutility.cpp +target_sources(${LIB_FSFW_NAME} PRIVATE + arrayprinter.cpp + AsciiConverter.cpp + CRC.cpp + DleEncoder.cpp + DleParser.cpp + PeriodicOperationDivider.cpp + timevalOperations.cpp + Type.cpp + bitutility.cpp ) add_subdirectory(math) diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp new file mode 100644 index 00000000..b68dbde1 --- /dev/null +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -0,0 +1,231 @@ +#include "DleParser.h" + +#include +#include + +#include + +DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf, + BufPair decodedBuf, UserHandler handler, void* args) + : decodeRingBuf(decodeRingBuf), + decoder(decoder), + encodedBuf(encodedBuf), + decodedBuf(decodedBuf), + handler(handler), + ctx(args) { + if (handler == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "DleParser::DleParser: Invalid user handler" << std::endl; +#else + sif::printError("DleParser::DleParser: Invalid user handler\n"); +#endif + } +} + +ReturnValue_t DleParser::passData(uint8_t* data, size_t len) { + if (data == nullptr or len == 0 or handler == nullptr) { + return RETURN_FAILED; + } + size_t copyIntoRingBufFromHere = 0; + size_t copyAmount = len; + size_t startIdx = 0; + ReturnValue_t result = RETURN_OK; + bool startFoundInThisPacket = false; + for (size_t idx = 0; idx < len; idx++) { + if (data[idx] == DleEncoder::STX_CHAR) { + if (not startFound and not startFoundInThisPacket) { + startIdx = idx; + copyIntoRingBufFromHere = idx; + copyAmount = len - idx; + } else { + // Maybe print warning, should not happen + decodeRingBuf.clear(); + ErrorInfo info; + info.len = idx; + prepareErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info); + handler(ctx); + copyIntoRingBufFromHere = idx; + copyAmount = len - idx; + } + startFound = true; + startFoundInThisPacket = true; + } else if (data[idx] == DleEncoder::ETX_CHAR) { + if (startFoundInThisPacket) { + size_t readLen = 0; + size_t decodedLen = 0; + result = decoder.decode(data + startIdx, idx + 1 - startIdx, &readLen, decodedBuf.first, + decodedBuf.second, &decodedLen); + if (result == HasReturnvaluesIF::RETURN_OK) { + ctx.setType(ContextType::PACKET_FOUND); + ctx.decodedPacket.first = decodedBuf.first; + ctx.decodedPacket.second = decodedLen; + this->handler(ctx); + } else if (result == DleEncoder::STREAM_TOO_SHORT) { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); + handler(ctx); + } else { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); + handler(ctx); + } + decodeRingBuf.clear(); + if ((idx + 1) < len) { + copyIntoRingBufFromHere = idx + 1; + copyAmount = len - idx - 1; + } else { + copyAmount = 0; + } + } else if (startFound) { + // ETX found but STX was found in another mini packet. Reconstruct the full packet + // to decode it + result = decodeRingBuf.writeData(data, idx + 1); + if (result != HasReturnvaluesIF::RETURN_OK) { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); + handler(ctx); + } + size_t fullEncodedLen = decodeRingBuf.getAvailableReadData(); + if (fullEncodedLen > encodedBuf.second) { + ErrorInfo info; + info.len = fullEncodedLen; + prepareErrorContext(ErrorTypes::ENCODED_BUF_TOO_SMALL, info); + handler(ctx); + decodeRingBuf.clear(); + } else { + size_t decodedLen = 0; + size_t readLen = 0; + decodeRingBuf.readData(encodedBuf.first, fullEncodedLen, true); + result = decoder.decode(encodedBuf.first, fullEncodedLen, &readLen, decodedBuf.first, + decodedBuf.second, &decodedLen); + if (result == HasReturnvaluesIF::RETURN_OK) { + if (this->handler != nullptr) { + ctx.setType(ContextType::PACKET_FOUND); + ctx.decodedPacket.first = decodedBuf.first; + ctx.decodedPacket.second = decodedLen; + this->handler(ctx); + } + } else if (result == DleEncoder::STREAM_TOO_SHORT) { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); + handler(ctx); + } else { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::DECODE_ERROR, info); + handler(ctx); + } + decodeRingBuf.clear(); + startFound = false; + startFoundInThisPacket = false; + if ((idx + 1) < len) { + copyIntoRingBufFromHere = idx + 1; + copyAmount = len - idx - 1; + } else { + copyAmount = 0; + } + } + } else { + // End data without preceeding STX + ErrorInfo info; + info.len = idx + 1; + prepareErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info); + handler(ctx); + decodeRingBuf.clear(); + if ((idx + 1) < len) { + copyIntoRingBufFromHere = idx + 1; + copyAmount = len - idx - 1; + } else { + copyAmount = 0; + } + } + startFoundInThisPacket = false; + startFound = false; + } + } + if (copyAmount > 0) { + result = decodeRingBuf.writeData(data + copyIntoRingBufFromHere, copyAmount); + if (result != HasReturnvaluesIF::RETURN_OK) { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); + handler(ctx); + } + } + return RETURN_OK; +} + +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(ErrorTypes err, ErrorInfo ctx) { + switch (err) { + 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 %d", ctx.len); + if (err == 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::prepareErrorContext(ErrorTypes err, ErrorInfo info) { + ctx.setType(ContextType::ERROR); + ctx.error.first = err; + ctx.error.second = info; +} + +void DleParser::reset() { + startFound = false; + decodeRingBuf.clear(); +} diff --git a/src/fsfw/globalfunctions/DleParser.h b/src/fsfw/globalfunctions/DleParser.h new file mode 100644 index 00000000..32fe38cb --- /dev/null +++ b/src/fsfw/globalfunctions/DleParser.h @@ -0,0 +1,127 @@ +#ifndef MISSION_DEVICES_DLEPARSER_H_ +#define MISSION_DEVICES_DLEPARSER_H_ + +#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 HasReturnvaluesIF { + public: + using BufPair = std::pair; + + enum class ContextType { 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(void* args) : userArgs(args) { setType(ContextType::PACKET_FOUND); } + + void setType(ContextType 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; + void* userArgs; + + private: + ContextType type; + }; + + using UserHandler = void (*)(const Context& ctx); + + /** + * 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, UserHandler handler, void* args); + + /** + * 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(uint8_t* data, size_t len); + + /** + * 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 + */ + static void defaultErrorHandler(ErrorTypes err, ErrorInfo ctx); + + static void errorPrinter(const char* str, const char* opt = nullptr); + + void prepareErrorContext(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; + UserHandler handler = nullptr; + Context ctx; + bool startFound = false; +}; + +#endif /* MISSION_DEVICES_DLEPARSER_H_ */