diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp index 240977f1..b992debd 100644 --- a/src/fsfw/globalfunctions/DleParser.cpp +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -7,176 +7,35 @@ #include DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf, - BufPair decodedBuf, UserHandler handler, void* args) + BufPair decodedBuf) : 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 - } + decodedBuf(decodedBuf) { } ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) { - if (data == nullptr or len == 0 or handler == nullptr) { + if (data == nullptr or len == 0) { return returnvalue::FAILED; } - return decodeRingBuf.writeData(data , len); -// std::string filename = std::string("/mnt/sd0/scex/transfer") + std::to_string(COUNTER++); -// std::ofstream of(filename); -// of.write(reinterpret_cast(data), len); -// size_t copyIntoRingBufFromHere = 0; -// size_t copyAmount = len; -// size_t startIdx = 0; -// ReturnValue_t result = returnvalue::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 == returnvalue::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 != returnvalue::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 == returnvalue::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 != returnvalue::OK) { -// ErrorInfo info; -// info.res = result; -// prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); -// handler(ctx); -// } -// } + return decodeRingBuf.writeData(data, len); } - ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { + ctx.setType(DleParser::ContextType::NONE); size_t availableData = decodeRingBuf.getAvailableReadData(); if (availableData > encodedBuf.second) { ErrorInfo info; info.len = decodeRingBuf.getAvailableReadData(); - prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); - handler(ctx); + setErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); + return returnvalue::FAILED; } ReturnValue_t result = decodeRingBuf.readData(encodedBuf.first, availableData); - if(result != returnvalue::OK) { + if (result != returnvalue::OK) { ErrorInfo info; info.res = result; - prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); + setErrorContext(ErrorTypes::RING_BUF_ERROR, info); + return result; } bool stxFound = false; size_t stxIdx = 0; @@ -190,6 +49,8 @@ ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { // might be lost packet, so we should advance the read pointer // without skipping the STX readSize = vectorIdx; + ErrorInfo info; + setErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info); return POSSIBLE_PACKET_LOSS; } } @@ -199,31 +60,37 @@ ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { // This is propably a packet, so we decode it. size_t decodedLen = 0; size_t dummy = 0; - readSize = vectorIdx; - ReturnValue_t result = decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, - &dummy, decodedBuf.first, decodedBuf.second, &decodedLen); + + ReturnValue_t result = + decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, &dummy, + decodedBuf.first, decodedBuf.second, &decodedLen); if (result == returnvalue::OK) { ctx.setType(ContextType::PACKET_FOUND); ctx.decodedPacket.first = decodedBuf.first; ctx.decodedPacket.second = decodedLen; - this->handler(ctx); + readSize = vectorIdx + 1; return returnvalue::OK; } else { // invalid packet, skip. readSize = ++vectorIdx; + ErrorInfo info; + info.res = result; + setErrorContext(ErrorTypes::DECODE_ERROR, info); return POSSIBLE_PACKET_LOSS; } } else { // might be lost packet, so we should advance the read pointer readSize = ++vectorIdx; + ErrorInfo info; + info.len = 0; + setErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info); return POSSIBLE_PACKET_LOSS; } } } - return returnvalue::OK; + return NO_PACKET_FOUND; } - void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -235,8 +102,12 @@ void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* arg #endif } -void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) { - switch (err) { +void DleParser::defaultErrorHandler() { + if(ctx.getType() != DleParser::ContextType::ERROR) { + errorPrinter("No error"); + return; + } + switch (ctx.error.first) { case (ErrorTypes::NONE): { errorPrinter("No error"); break; @@ -252,8 +123,8 @@ void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) { 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) { + snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.decodedPacket.second); + if (ctx.error.first == ErrorTypes::ENCODED_BUF_TOO_SMALL) { errorPrinter("Encoded buf too small", opt); } else { errorPrinter("Decoding buf too small", opt); @@ -284,7 +155,7 @@ void DleParser::errorPrinter(const char* str, const char* opt) { #endif } -void DleParser::prepareErrorContext(ErrorTypes err, ErrorInfo info) { +void DleParser::setErrorContext(ErrorTypes err, ErrorInfo info) { ctx.setType(ContextType::ERROR); ctx.error.first = err; ctx.error.second = info; @@ -294,7 +165,10 @@ ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) { return decodeRingBuf.deleteData(bytesRead); } +const DleParser::Context& DleParser::getContext() { + return ctx; +} + void DleParser::reset() { - startFound = false; decodeRingBuf.clear(); } diff --git a/src/fsfw/globalfunctions/DleParser.h b/src/fsfw/globalfunctions/DleParser.h index b31fbf83..9802017a 100644 --- a/src/fsfw/globalfunctions/DleParser.h +++ b/src/fsfw/globalfunctions/DleParser.h @@ -22,7 +22,7 @@ class DleParser { static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS = returnvalue::makeCode(1, 2); using BufPair = std::pair; - enum class ContextType { PACKET_FOUND, ERROR }; + enum class ContextType { NONE, PACKET_FOUND, ERROR }; enum class ErrorTypes { NONE, @@ -43,7 +43,7 @@ class DleParser { struct Context { public: - Context(void* args) : userArgs(args) { setType(ContextType::PACKET_FOUND); } + Context() { setType(ContextType::PACKET_FOUND); } void setType(ContextType type) { this->type = type; @@ -60,14 +60,11 @@ class DleParser { 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 @@ -81,7 +78,7 @@ class DleParser { * @param args Arbitrary user argument */ DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf, - BufPair decodedBuf, UserHandler handler, void* args); + BufPair decodedBuf); /** * This function allows to pass new data into the parser. It then scans for DLE packets @@ -96,6 +93,7 @@ class DleParser { ReturnValue_t confirmBytesRead(size_t bytesRead); + const Context& getContext(); /** * Example found packet handler * function call @@ -110,11 +108,11 @@ class DleParser { * - 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); + void defaultErrorHandler(); static void errorPrinter(const char* str, const char* opt = nullptr); - void prepareErrorContext(ErrorTypes err, ErrorInfo ctx); + void setErrorContext(ErrorTypes err, ErrorInfo ctx); /** * Resets the parser by resetting the internal states and clearing the decoding ring buffer */ @@ -125,7 +123,5 @@ class DleParser { DleEncoder& decoder; BufPair encodedBuf; BufPair decodedBuf; - UserHandler handler = nullptr; Context ctx; - bool startFound = false; }; diff --git a/src/fsfw/pus/Service5EventReporting.cpp b/src/fsfw/pus/Service5EventReporting.cpp index c1affa6f..b4b9b03d 100644 --- a/src/fsfw/pus/Service5EventReporting.cpp +++ b/src/fsfw/pus/Service5EventReporting.cpp @@ -15,8 +15,8 @@ Service5EventReporting::Service5EventReporting(PsbParams params, size_t maxNumbe maxNumberReportsPerCycle(maxNumberReportsPerCycle) { auto mqArgs = MqArgs(getObjectId(), static_cast(this)); psbParams.name = "PUS 5 Event Reporting"; - eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth, - MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + eventQueue = QueueFactory::instance()->createMessageQueue( + messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } Service5EventReporting::~Service5EventReporting() {