bugfix for SP parser

This commit is contained in:
Robin Müller 2022-09-01 08:51:12 +02:00
parent 6c2b5ab39e
commit 28ecd0e5c6
No known key found for this signature in database
GPG Key ID: 71B58F8A3CDFA9AC
3 changed files with 45 additions and 37 deletions

View File

@ -161,7 +161,7 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) {
while (true) { while (true) {
ssize_t retval = recv(connSocket, reinterpret_cast<char*>(receptionBuffer.data()), ssize_t retval = recv(connSocket, reinterpret_cast<char*>(receptionBuffer.data()),
receptionBuffer.capacity(), tcpConfig.tcpFlags); receptionBuffer.size(), tcpConfig.tcpFlags);
if (retval == 0) { if (retval == 0) {
size_t availableReadData = ringBuffer.getAvailableReadData(); size_t availableReadData = ringBuffer.getAvailableReadData();
if (availableReadData > lastRingBufferSize) { if (availableReadData > lastRingBufferSize) {
@ -335,31 +335,28 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) {
} }
ringBuffer.readData(receptionBuffer.data(), readAmount, true); ringBuffer.readData(receptionBuffer.data(), readAmount, true);
const uint8_t* bufPtr = receptionBuffer.data(); const uint8_t* bufPtr = receptionBuffer.data();
const uint8_t** bufPtrPtr = &bufPtr; FoundPacketInfo info;
size_t startIdx = 0; ParsingState parseState;
size_t foundSize = 0; while (parseState.amountRead < readAmount) {
size_t readLen = 0;
while (readLen < readAmount) {
if (spacePacketParser == nullptr) { if (spacePacketParser == nullptr) {
return returnvalue::FAILED; return returnvalue::FAILED;
} }
result = result = spacePacketParser->parseSpacePackets(&bufPtr, readAmount, info, parseState);
spacePacketParser->parseSpacePackets(bufPtrPtr, readAmount, startIdx, foundSize, readLen);
switch (result) { switch (result) {
case (SpacePacketParser::NO_PACKET_FOUND): case (SpacePacketParser::NO_PACKET_FOUND):
case (SpacePacketParser::SPLIT_PACKET): { case (SpacePacketParser::SPLIT_PACKET): {
break; break;
} }
case (returnvalue::OK): { case (returnvalue::OK): {
result = handleTcReception(receptionBuffer.data() + startIdx, foundSize); result = handleTcReception(receptionBuffer.data() + info.startIdx, info.sizeFound);
if (result != returnvalue::OK) { if (result != returnvalue::OK) {
status = result; status = result;
} }
} }
} }
ringBuffer.deleteData(foundSize); ringBuffer.deleteData(info.sizeFound);
lastRingBufferSize = ringBuffer.getAvailableReadData(); lastRingBufferSize = ringBuffer.getAvailableReadData();
std::memset(receptionBuffer.data() + startIdx, 0, foundSize); // std::memset(receptionBuffer.data() + startIdx, 0, foundSize);
} }
return status; return status;
} }

View File

@ -7,16 +7,16 @@ SpacePacketParser::SpacePacketParser(std::vector<uint16_t> validPacketIds)
: validPacketIds(validPacketIds) {} : validPacketIds(validPacketIds) {}
ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t* buffer, const size_t maxSize, ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t* buffer, const size_t maxSize,
size_t& startIndex, size_t& foundSize) { FoundPacketInfo& packetInfo,
ParsingState& parsingState) {
const uint8_t** tempPtr = &buffer; const uint8_t** tempPtr = &buffer;
size_t readLen = 0; return parseSpacePackets(tempPtr, maxSize, packetInfo, parsingState);
return parseSpacePackets(tempPtr, maxSize, startIndex, foundSize, readLen);
} }
ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const size_t maxSize, ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const size_t maxSize,
size_t& startIndex, size_t& foundSize, FoundPacketInfo& packetInfo,
size_t& readLen) { ParsingState& parsingState) {
if (buffer == nullptr or maxSize < 5) { if (buffer == nullptr or parsingState.nextStartIdx > maxSize) {
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "SpacePacketParser::parseSpacePackets: Frame invalid" << std::endl; sif::warning << "SpacePacketParser::parseSpacePackets: Frame invalid" << std::endl;
#else #else
@ -26,35 +26,36 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const
} }
const uint8_t* bufPtr = *buffer; const uint8_t* bufPtr = *buffer;
auto verifyLengthField = [&](size_t idx) { auto verifyLengthField = [&](size_t localIdx) {
uint16_t lengthField = bufPtr[idx + 4] << 8 | bufPtr[idx + 5]; uint16_t lengthField = (bufPtr[localIdx + 4] << 8) | bufPtr[localIdx + 5];
size_t packetSize = lengthField + 7; size_t packetSize = lengthField + 7;
startIndex = idx;
ReturnValue_t result = returnvalue::OK; ReturnValue_t result = returnvalue::OK;
if (lengthField == 0) { if (lengthField == 0) {
// Skip whole header for now // Skip whole header for now
foundSize = 6; packetInfo.sizeFound = 6;
result = NO_PACKET_FOUND; result = NO_PACKET_FOUND;
} else if (packetSize + idx > maxSize) { } else if (packetSize + localIdx + parsingState.amountRead > maxSize) {
// Don't increment buffer and read length here, user has to decide what to do // Don't increment buffer and read length here, user has to decide what to do
foundSize = packetSize; packetInfo.sizeFound = packetSize;
return SPLIT_PACKET; return SPLIT_PACKET;
} else { } else {
foundSize = packetSize; packetInfo.sizeFound = packetSize;
} }
*buffer += foundSize; *buffer += packetInfo.sizeFound;
readLen += idx + foundSize; packetInfo.startIdx = localIdx + parsingState.amountRead;
parsingState.nextStartIdx = localIdx + parsingState.amountRead + packetInfo.sizeFound;
parsingState.amountRead = parsingState.nextStartIdx;
return result; return result;
}; };
size_t idx = 0; size_t idx = 0;
// Space packet ID as start marker // Space packet ID as start marker
if (validPacketIds.size() > 0) { if (validPacketIds.size() > 0) {
while (idx < maxSize - 5) { while (idx + parsingState.amountRead < maxSize - 5) {
uint16_t currentPacketId = bufPtr[idx] << 8 | bufPtr[idx + 1]; uint16_t currentPacketId = (bufPtr[idx] << 8) | bufPtr[idx + 1];
if (std::find(validPacketIds.begin(), validPacketIds.end(), currentPacketId) != if (std::find(validPacketIds.begin(), validPacketIds.end(), currentPacketId) !=
validPacketIds.end()) { validPacketIds.end()) {
if (idx + 5 >= maxSize) { if (idx + parsingState.amountRead >= maxSize - 5) {
return SPLIT_PACKET; return SPLIT_PACKET;
} }
return verifyLengthField(idx); return verifyLengthField(idx);
@ -62,10 +63,10 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const
idx++; idx++;
} }
} }
startIndex = 0; parsingState.nextStartIdx = maxSize;
foundSize = maxSize; packetInfo.sizeFound = maxSize;
*buffer += foundSize; parsingState.amountRead = maxSize;
readLen += foundSize; *buffer += maxSize;
return NO_PACKET_FOUND; return NO_PACKET_FOUND;
} }
// Assume that the user verified a valid start of a space packet // Assume that the user verified a valid start of a space packet

View File

@ -7,6 +7,16 @@
#include "fsfw/container/DynamicFIFO.h" #include "fsfw/container/DynamicFIFO.h"
#include "fsfw/returnvalues/FwClassIds.h" #include "fsfw/returnvalues/FwClassIds.h"
struct FoundPacketInfo {
size_t startIdx = 0;
size_t sizeFound = 0;
};
struct ParsingState {
size_t nextStartIdx = 0;
size_t amountRead = 0;
};
/** /**
* @brief This small helper class scans a given buffer for space packets. * @brief This small helper class scans a given buffer for space packets.
* Can be used if space packets are serialized in a tightly packed frame. * Can be used if space packets are serialized in a tightly packed frame.
@ -53,8 +63,8 @@ class SpacePacketParser {
* will be assigned. * will be assigned.
* -@c returnvalue::OK if a packet was found * -@c returnvalue::OK if a packet was found
*/ */
ReturnValue_t parseSpacePackets(const uint8_t** buffer, const size_t maxSize, size_t& startIndex, ReturnValue_t parseSpacePackets(const uint8_t** buffer, const size_t maxSize,
size_t& foundSize, size_t& readLen); FoundPacketInfo& packetInfo, ParsingState& parsingState);
/** /**
* Parse a given frame for space packets * Parse a given frame for space packets
@ -69,8 +79,8 @@ class SpacePacketParser {
* detected packet * detected packet
* -@c returnvalue::OK if a packet was found * -@c returnvalue::OK if a packet was found
*/ */
ReturnValue_t parseSpacePackets(const uint8_t* buffer, const size_t maxSize, size_t& startIndex, ReturnValue_t parseSpacePackets(const uint8_t* buffer, const size_t maxSize,
size_t& foundSize); FoundPacketInfo& packetInfo, ParsingState& parsingState);
private: private:
std::vector<uint16_t> validPacketIds; std::vector<uint16_t> validPacketIds;