232 lines
7.4 KiB
C++
232 lines
7.4 KiB
C++
|
#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();
|
||
|
}
|