diff --git a/CHANGELOG.md b/CHANGELOG.md index 49f0d2f4f..803384466 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Added +- `DleParser` helper class to parse DLE encoded packets from a byte stream. + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/711 - `UioMapper` is able to resolve symlinks now. PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/709 - Add new `UnsignedByteField` class diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 8ab540a05..60966501a 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -359,6 +359,8 @@ void DeviceHandlerBase::doStateMachine() { if ((switchState == PowerSwitchIF::SWITCH_ON) || (switchState == NO_SWITCH)) { // NOTE: TransitionSourceMode and -SubMode are set by handleCommandedModeTransition childTransitionFailure = CHILD_TIMEOUT; + transitionSourceMode = _MODE_SHUT_DOWN; + transitionSourceSubMode = SUBMODE_NONE; setMode(_MODE_START_UP); callChildStatemachine(); } diff --git a/src/fsfw/globalfunctions/CMakeLists.txt b/src/fsfw/globalfunctions/CMakeLists.txt index cfa02696e..cf8a25d54 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 000000000..cc695babe --- /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 000000000..9802017a5 --- /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; +};