fsfw/src/fsfw/globalfunctions/DleParser.cpp

#include "DleParser.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 %zu", 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();
}