Merge branch 'mueller/dle-parser' into irini

src/fsfw/globalfunctions/CMakeLists.txt

@@ -1,9 +1,9 @@
-target_sources(${LIB_FSFW_NAME} 
+target_sources(${LIB_FSFW_NAME} PRIVATE
-	PRIVATE 
     arrayprinter.cpp
     AsciiConverter.cpp
     CRC.cpp
     DleEncoder.cpp
+    DleParser.cpp
     PeriodicOperationDivider.cpp
     timevalOperations.cpp
     Type.cpp

src/fsfw/globalfunctions/DleParser.cpp

@@ -0,0 +1,231 @@
+#include "DleParser.h"
+
+#include <fsfw/globalfunctions/DleEncoder.h>
+#include <fsfw/serviceinterface/ServiceInterface.h>
+
+#include <cstdio>
+
+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();
+}

src/fsfw/globalfunctions/DleParser.h

@@ -0,0 +1,127 @@
+#ifndef MISSION_DEVICES_DLEPARSER_H_
+#define MISSION_DEVICES_DLEPARSER_H_
+
+#include <fsfw/container/SimpleRingBuffer.h>
+#include <fsfw/globalfunctions/DleEncoder.h>
+#include <fsfw/returnvalues/HasReturnvaluesIF.h>
+
+#include <cstddef>
+#include <utility>
+
+/**
+ * @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<uint8_t*, size_t>;
+
+  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<ErrorTypes, ErrorInfo>;
+
+  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_ */