Merge pull request 'Bugfix for Space Packet Parser' (#673) from eive/fsfw:mueller/fixes-tmtc-server-sp-parser into development

Reviewed-on: fsfw/fsfw#673
This commit is contained in:
Ulrich Mohr 2022-09-01 14:12:16 +02:00
commit eb8e236cd4
3 changed files with 52 additions and 77 deletions

View File

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

View File

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

View File

@ -17,9 +17,11 @@
*/
class SpacePacketParser {
public:
//! The first entry is the index inside the buffer while the second index
//! is the size of the PUS packet starting at that index.
using IndexSizePair = std::pair<size_t, size_t>;
struct FoundPacketInfo {
size_t startIdx = 0;
size_t sizeFound = 0;
};
static constexpr uint8_t INTERFACE_ID = CLASS_ID::SPACE_PACKET_PARSER;
static constexpr ReturnValue_t NO_PACKET_FOUND = MAKE_RETURN_CODE(0x00);
@ -36,44 +38,32 @@ class SpacePacketParser {
SpacePacketParser(std::vector<uint16_t> validPacketIds);
/**
* Parse a given frame for space packets but also increment the given buffer and assign the
* total number of bytes read so far
* Parse a given frame for space packets but also increments the given buffer.
* @param buffer Parser will look for space packets in this buffer
* @param maxSize Maximum size of the buffer
* @param startIndex Start index of a found space packet
* @param foundSize Found size of the space packet
* @param readLen Length read so far. This value is incremented by the number of parsed
* bytes which also includes the size of a found packet
* -@c NO_PACKET_FOUND if no packet was found in the given buffer or the length field is
* invalid. foundSize will be set to the size of the space packet header. buffer and
* readLen will be incremented accordingly.
* -@c SPLIT_PACKET if a packet was found but the detected size exceeds maxSize. foundSize
* will be set to the detected packet size and startIndex will be set to the start of the
* detected packet. buffer and read length will not be incremented but the found length
* will be assigned.
* -@c returnvalue::OK if a packet was found
* @param packetInfo Information about packets found.
* -@c NO_PACKET_FOUND if no packet was found in the given buffer
* -@c SPLIT_PACKET if a packet was found but the detected size exceeds maxSize. packetInfo
* will contain the detected packet size and start index.
* -@c returnvalue::OK if a packet was found. Packet size and start index will be set in
* packetInfo
*/
ReturnValue_t parseSpacePackets(const uint8_t** buffer, const size_t maxSize, size_t& startIndex,
size_t& foundSize, size_t& readLen);
ReturnValue_t parseSpacePackets(const uint8_t** buffer, const size_t maxSize,
FoundPacketInfo& packetInfo);
/**
* Parse a given frame for space packets
* @param buffer Parser will look for space packets in this buffer
* @param maxSize Maximum size of the buffer
* @param startIndex Start index of a found space packet
* @param foundSize Found size of the space packet
* -@c NO_PACKET_FOUND if no packet was found in the given buffer or the length field is
* invalid. foundSize will be set to the size of the space packet header
* -@c SPLIT_PACKET if a packet was found but the detected size exceeds maxSize. foundSize
* will be set to the detected packet size and startIndex will be set to the start of the
* detected packet
* -@c returnvalue::OK if a packet was found
*/
ReturnValue_t parseSpacePackets(const uint8_t* buffer, const size_t maxSize, size_t& startIndex,
size_t& foundSize);
size_t getAmountRead() {
return amountRead;
}
void reset() {
nextStartIdx = 0;
amountRead = 0;
}
private:
std::vector<uint16_t> validPacketIds;
size_t nextStartIdx = 0;
size_t amountRead = 0;
};
#endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */