KSat/fsfw
15
4
Fork 0
forked from fsfw/fsfw
Code Issues Pull Requests 1 Releases Wiki Activity

Merge branch 'mueller/dle-parser' into irini

Browse Source
This commit is contained in:
Robin Müller 2022-04-13 15:00:39 +02:00
commit e0c9bf5871
3 changed files with 368 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/fsfw/globalfunctions/CMakeLists.txt
View File

@ -1,13 +1,13 @@
target_sources(${LIB_FSFW_NAME}
PRIVATE
arrayprinter.cpp
AsciiConverter.cpp
CRC.cpp
DleEncoder.cpp
PeriodicOperationDivider.cpp
timevalOperations.cpp
Type.cpp
bitutility.cpp
target_sources(${LIB_FSFW_NAME} PRIVATE
arrayprinter.cpp
AsciiConverter.cpp
CRC.cpp
DleEncoder.cpp
DleParser.cpp
PeriodicOperationDivider.cpp
timevalOperations.cpp
Type.cpp
bitutility.cpp
)
add_subdirectory(math)

231
src/fsfw/globalfunctions/DleParser.cpp Normal file
View File

@ -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();
}

127
src/fsfw/globalfunctions/DleParser.h Normal file
View File

@ -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_ */