eive/fsfw
eive
/
fsfw
forked from fsfw/fsfw
14
1
Fork 0
Code Issues Pull Requests 1 Releases 1 Wiki Activity

try to do this in a simpler way

This commit is contained in:
Robin Müller 2022-10-04 18:38:20 +02:00
parent b9d0ff8fb7
commit cfc00d0260
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
2 changed files with 194 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

304
src/fsfw/globalfunctions/DleParser.cpp
View File

@ -3,6 +3,8 @@
#include <fsfw/serviceinterface/ServiceInterface.h>
#include <cstdio>
#include <fstream>
#include <iostream>
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<const char*>(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();

8
src/fsfw/globalfunctions/DleParser.h
View File

@ -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<uint8_t*, size_t>;
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