diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp index 44c75bab..240977f1 100644 --- a/src/fsfw/globalfunctions/DleParser.cpp +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -3,6 +3,8 @@ #include #include +#include +#include DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf, BufPair decodedBuf, UserHandler handler, void* args) @@ -21,143 +23,207 @@ DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPa } } -ReturnValue_t DleParser::passData(uint8_t* data, size_t len) { +ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) { if (data == nullptr or len == 0 or handler == nullptr) { return returnvalue::FAILED; } - 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; + 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); +// } +// } +} + + +ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { + size_t availableData = decodeRingBuf.getAvailableReadData(); + if (availableData > encodedBuf.second) { + ErrorInfo info; + info.len = decodeRingBuf.getAvailableReadData(); + prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); + handler(ctx); + } + ReturnValue_t result = decodeRingBuf.readData(encodedBuf.first, availableData); + if(result != returnvalue::OK) { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); + } + 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; } 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; + // might be lost packet, so we should advance the read pointer + // without skipping the STX + readSize = vectorIdx; + return POSSIBLE_PACKET_LOSS; } - startFound = true; - startFoundInThisPacket = true; - } else if (data[idx] == DleEncoder::ETX_CHAR) { - if (startFoundInThisPacket) { - size_t readLen = 0; + } + // handle ETX char + if (encodedBuf.first[vectorIdx] == DleEncoder::ETX_CHAR) { + if (stxFound) { + // This is propably a packet, so we decode it. size_t decodedLen = 0; - result = decoder.decode(data + startIdx, idx + 1 - startIdx, &readLen, decodedBuf.first, - decodedBuf.second, &decodedLen); + size_t dummy = 0; + readSize = vectorIdx; + 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); - } else if (result == DleEncoder::STREAM_TOO_SHORT) { - ErrorInfo info; - info.res = result; - prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); - handler(ctx); + return returnvalue::OK; } 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; - } + // invalid packet, skip. + readSize = ++vectorIdx; + return POSSIBLE_PACKET_LOSS; } } 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; - } + // might be lost packet, so we should advance the read pointer + readSize = ++vectorIdx; + return POSSIBLE_PACKET_LOSS; } - 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 returnvalue::OK; } + void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -224,6 +290,10 @@ void DleParser::prepareErrorContext(ErrorTypes err, ErrorInfo info) { ctx.error.second = info; } +ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) { + return decodeRingBuf.deleteData(bytesRead); +} + void DleParser::reset() { startFound = false; decodeRingBuf.clear(); diff --git a/src/fsfw/globalfunctions/DleParser.h b/src/fsfw/globalfunctions/DleParser.h index 5ccd55df..b31fbf83 100644 --- a/src/fsfw/globalfunctions/DleParser.h +++ b/src/fsfw/globalfunctions/DleParser.h @@ -18,6 +18,8 @@ */ class DleParser { public: + static constexpr ReturnValue_t NO_PACKET_FOUND = returnvalue::makeCode(1, 1); + static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS = returnvalue::makeCode(1, 2); using BufPair = std::pair; enum class ContextType { PACKET_FOUND, ERROR }; @@ -88,7 +90,11 @@ class DleParser { * @param len * @return */ - ReturnValue_t passData(uint8_t* data, size_t len); + ReturnValue_t passData(const uint8_t* data, size_t len); + + ReturnValue_t parseRingBuf(size_t& bytesRead); + + ReturnValue_t confirmBytesRead(size_t bytesRead); /** * Example found packet handler