2022-04-13 14:45:36 +02:00
|
|
|
#include "DleParser.h"
|
|
|
|
|
|
|
|
#include <fsfw/serviceinterface/ServiceInterface.h>
|
|
|
|
|
|
|
|
#include <cstdio>
|
2022-10-04 18:38:20 +02:00
|
|
|
#include <fstream>
|
|
|
|
#include <iostream>
|
2022-04-13 14:45:36 +02:00
|
|
|
|
|
|
|
DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf,
|
2022-10-04 20:51:58 +02:00
|
|
|
BufPair decodedBuf)
|
2022-04-13 14:45:36 +02:00
|
|
|
: decodeRingBuf(decodeRingBuf),
|
|
|
|
decoder(decoder),
|
|
|
|
encodedBuf(encodedBuf),
|
2022-10-27 14:01:35 +02:00
|
|
|
decodedBuf(decodedBuf) {}
|
2022-04-13 14:45:36 +02:00
|
|
|
|
2022-10-04 18:38:20 +02:00
|
|
|
ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) {
|
2022-10-04 20:51:58 +02:00
|
|
|
if (data == nullptr or len == 0) {
|
2022-08-24 17:25:45 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-04-13 14:45:36 +02:00
|
|
|
}
|
2022-10-04 20:51:58 +02:00
|
|
|
return decodeRingBuf.writeData(data, len);
|
2022-10-04 18:38:20 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t DleParser::parseRingBuf(size_t& readSize) {
|
2022-10-04 20:51:58 +02:00
|
|
|
ctx.setType(DleParser::ContextType::NONE);
|
2022-10-04 18:38:20 +02:00
|
|
|
size_t availableData = decodeRingBuf.getAvailableReadData();
|
2022-10-27 14:01:35 +02:00
|
|
|
if (availableData == 0) {
|
2022-10-04 23:04:28 +02:00
|
|
|
return NO_PACKET_FOUND;
|
|
|
|
}
|
2022-10-04 18:38:20 +02:00
|
|
|
if (availableData > encodedBuf.second) {
|
|
|
|
ErrorInfo info;
|
|
|
|
info.len = decodeRingBuf.getAvailableReadData();
|
2022-10-04 20:51:58 +02:00
|
|
|
setErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info);
|
|
|
|
return returnvalue::FAILED;
|
2022-10-04 18:38:20 +02:00
|
|
|
}
|
|
|
|
ReturnValue_t result = decodeRingBuf.readData(encodedBuf.first, availableData);
|
2022-10-04 20:51:58 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-10-04 18:38:20 +02:00
|
|
|
ErrorInfo info;
|
|
|
|
info.res = result;
|
2022-10-04 20:51:58 +02:00
|
|
|
setErrorContext(ErrorTypes::RING_BUF_ERROR, info);
|
|
|
|
return result;
|
2022-10-04 18:38:20 +02:00
|
|
|
}
|
|
|
|
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;
|
2022-04-13 14:45:36 +02:00
|
|
|
} else {
|
2022-10-04 18:38:20 +02:00
|
|
|
// might be lost packet, so we should advance the read pointer
|
|
|
|
// without skipping the STX
|
|
|
|
readSize = vectorIdx;
|
2022-10-04 20:51:58 +02:00
|
|
|
ErrorInfo info;
|
|
|
|
setErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info);
|
2022-10-04 18:38:20 +02:00
|
|
|
return POSSIBLE_PACKET_LOSS;
|
2022-04-13 14:45:36 +02:00
|
|
|
}
|
2022-10-04 18:38:20 +02:00
|
|
|
}
|
|
|
|
// handle ETX char
|
|
|
|
if (encodedBuf.first[vectorIdx] == DleEncoder::ETX_CHAR) {
|
|
|
|
if (stxFound) {
|
|
|
|
// This is propably a packet, so we decode it.
|
2022-04-13 14:45:36 +02:00
|
|
|
size_t decodedLen = 0;
|
2022-10-04 18:38:20 +02:00
|
|
|
size_t dummy = 0;
|
2022-10-04 20:51:58 +02:00
|
|
|
|
|
|
|
ReturnValue_t result =
|
|
|
|
decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, &dummy,
|
|
|
|
decodedBuf.first, decodedBuf.second, &decodedLen);
|
2022-08-24 17:25:45 +02:00
|
|
|
if (result == returnvalue::OK) {
|
2022-04-13 14:45:36 +02:00
|
|
|
ctx.setType(ContextType::PACKET_FOUND);
|
|
|
|
ctx.decodedPacket.first = decodedBuf.first;
|
|
|
|
ctx.decodedPacket.second = decodedLen;
|
2022-10-04 21:17:35 +02:00
|
|
|
readSize = ++vectorIdx;
|
2022-10-04 18:38:20 +02:00
|
|
|
return returnvalue::OK;
|
2022-04-13 14:45:36 +02:00
|
|
|
} else {
|
2022-10-04 18:38:20 +02:00
|
|
|
// invalid packet, skip.
|
|
|
|
readSize = ++vectorIdx;
|
2022-10-04 20:51:58 +02:00
|
|
|
ErrorInfo info;
|
|
|
|
info.res = result;
|
|
|
|
setErrorContext(ErrorTypes::DECODE_ERROR, info);
|
2022-10-04 18:38:20 +02:00
|
|
|
return POSSIBLE_PACKET_LOSS;
|
2022-04-13 14:45:36 +02:00
|
|
|
}
|
|
|
|
} else {
|
2022-10-04 18:38:20 +02:00
|
|
|
// might be lost packet, so we should advance the read pointer
|
|
|
|
readSize = ++vectorIdx;
|
2022-10-04 20:51:58 +02:00
|
|
|
ErrorInfo info;
|
|
|
|
info.len = 0;
|
|
|
|
setErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info);
|
2022-10-04 18:38:20 +02:00
|
|
|
return POSSIBLE_PACKET_LOSS;
|
2022-04-13 14:45:36 +02:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2022-10-04 20:51:58 +02:00
|
|
|
return NO_PACKET_FOUND;
|
2022-04-13 14:45:36 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
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
|
|
|
|
}
|
|
|
|
|
2022-10-04 20:51:58 +02:00
|
|
|
void DleParser::defaultErrorHandler() {
|
2022-10-27 14:01:35 +02:00
|
|
|
if (ctx.getType() != DleParser::ContextType::ERROR) {
|
2022-10-04 20:51:58 +02:00
|
|
|
errorPrinter("No error");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
switch (ctx.error.first) {
|
2022-04-13 14:45:36 +02:00
|
|
|
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];
|
2022-10-27 14:01:35 +02:00
|
|
|
snprintf(opt, sizeof(opt), ": Too small for packet with length %zu",
|
|
|
|
ctx.decodedPacket.second);
|
2022-10-04 20:51:58 +02:00
|
|
|
if (ctx.error.first == ErrorTypes::ENCODED_BUF_TOO_SMALL) {
|
2022-04-13 14:45:36 +02:00
|
|
|
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
|
|
|
|
}
|
|
|
|
|
2022-10-04 20:51:58 +02:00
|
|
|
void DleParser::setErrorContext(ErrorTypes err, ErrorInfo info) {
|
2022-04-13 14:45:36 +02:00
|
|
|
ctx.setType(ContextType::ERROR);
|
|
|
|
ctx.error.first = err;
|
|
|
|
ctx.error.second = info;
|
|
|
|
}
|
|
|
|
|
2022-10-04 18:38:20 +02:00
|
|
|
ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) {
|
|
|
|
return decodeRingBuf.deleteData(bytesRead);
|
|
|
|
}
|
|
|
|
|
2022-10-27 14:01:35 +02:00
|
|
|
const DleParser::Context& DleParser::getContext() { return ctx; }
|
2022-10-04 20:51:58 +02:00
|
|
|
|
2022-10-27 14:01:35 +02:00
|
|
|
void DleParser::reset() { decodeRingBuf.clear(); }
|