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

refactored space packet parser

Browse Source
This commit is contained in:
Robin Müller 2021-09-28 14:14:59 +02:00
parent 5fd7a8c9b7
commit 80ccaede02
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
4 changed files with 109 additions and 167 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
src/fsfw/osal/common/TcpTmTcServer.cpp
View File

@ -33,7 +33,7 @@ TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
ReceptionModes receptionMode):
SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), receptionMode(receptionMode),
tcpConfig(customTcpServerPort), receptionBuffer(receptionBufferSize),
ringBuffer(ringBufferSize, true) {
ringBuffer(ringBufferSize, true), validPacketIds() {
}
ReturnValue_t TcpTmTcServer::initialize() {
@ -46,8 +46,7 @@ ReturnValue_t TcpTmTcServer::initialize() {
switch(receptionMode) {
case(ReceptionModes::SPACE_PACKETS): {
// For now, hardcode a maximum of 5 store packets here and no split packets are allowed
spacePacketParser = new SpacePacketParser(5, false);
spacePacketParser = new SpacePacketParser(validPacketIds);
if(spacePacketParser == nullptr) {
return HasReturnvaluesIF::RETURN_FAILED;
}
@ -246,7 +245,8 @@ std::string TcpTmTcServer::getTcpPort() const {
return tcpConfig.tcpPort;
}
void TcpTmTcServer::setSpacePacketParsingOptions(uint8_t parserFifoSize) {
void TcpTmTcServer::setSpacePacketParsingOptions(std::vector<uint16_t> validPacketIds) {
this->validPacketIds = validPacketIds;
}
TcpTmTcServer::TcpConfig& TcpTmTcServer::getTcpConfigStruct() {
@ -319,27 +319,32 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) {
readAmount = receptionBuffer.size();
}
ringBuffer.readData(receptionBuffer.data(), readAmount, true);
uint32_t readPackets = 0;
result = spacePacketParser->parseSpacePackets(receptionBuffer.data(), readAmount, readPackets);
if(result == SpacePacketParser::NO_PACKET_FOUND) {
ringBuffer.deleteData(availableReadData);
lastRingBufferSize = ringBuffer.getAvailableReadData();
}
else if(result == HasReturnvaluesIF::RETURN_OK) {
// Space Packets were found. Handle them here
auto& fifo = spacePacketParser->fifo();
SpacePacketParser::IndexSizePair idxSizePair;
while(not fifo.empty()) {
fifo.retrieve(&idxSizePair);
result = handleTcReception(receptionBuffer.data() + idxSizePair.first,
idxSizePair.second);
std::memset(receptionBuffer.data() + idxSizePair.first, 0, idxSizePair.second);
ringBuffer.deleteData(idxSizePair.second);
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) {
result = spacePacketParser->parseSpacePackets(bufPtrPtr, readAmount,
startIdx, foundSize, readLen);
if(result == SpacePacketParser::NO_PACKET_FOUND) {
ringBuffer.deleteData(foundSize);
lastRingBufferSize = ringBuffer.getAvailableReadData();
}
else if(result == SpacePacketParser::SPLIT_PACKET) {
// might be half of a packet? Skip it for now
ringBuffer.deleteData(foundSize);
lastRingBufferSize = ringBuffer.getAvailableReadData();
}
else if(result == HasReturnvaluesIF::RETURN_OK) {
result = handleTcReception(receptionBuffer.data() + startIdx, foundSize);
ringBuffer.deleteData(foundSize);
if(result != HasReturnvaluesIF::RETURN_OK) {
status = result;
}
lastRingBufferSize = ringBuffer.getAvailableReadData();
}
std::memset(receptionBuffer.data() + startIdx, 0, foundSize);
}
return status;
}

3
src/fsfw/osal/common/TcpTmTcServer.h
View File

@ -97,7 +97,7 @@ public:
* @return
*/
TcpConfig& getTcpConfigStruct();
void setSpacePacketParsingOptions(uint8_t parserFifoSize);
void setSpacePacketParsingOptions(std::vector<uint16_t> validPacketIds);
ReturnValue_t initialize() override;
ReturnValue_t performOperation(uint8_t opCode) override;
@ -123,6 +123,7 @@ private:
std::vector<uint8_t> receptionBuffer;
SimpleRingBuffer ringBuffer;
std::vector<uint16_t> validPacketIds;
SpacePacketParser* spacePacketParser = nullptr;
uint8_t lastRingBufferSize = 0;

200
src/fsfw/tmtcservices/SpacePacketParser.cpp
View File

@ -1,147 +1,77 @@
#include <fsfw/serviceinterface/ServiceInterface.h>
#include <fsfw/tmtcservices/SpacePacketParser.h>
#include <algorithm>
SpacePacketParser::SpacePacketParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets):
indexSizePairFIFO(maxExpectedPusPackets) {
SpacePacketParser::SpacePacketParser(std::vector<uint16_t> validPacketIds):
validPacketIds(validPacketIds) {
}
ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t *frame,
size_t frameSize, uint32_t& foundPackets) {
if(frame == nullptr or frameSize < 5) {
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) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PusParser::parsePusPackets: Frame invalid" << std::endl;
sif::warning << "SpacePacketParser::parseSpacePackets: Frame invalid" << std::endl;
#else
sif::printError("PusParser::parsePusPackets: Frame invalid\n");
sif::printWarning("SpacePacketParser::parseSpacePackets: Frame invalid\n");
#endif
return HasReturnvaluesIF::RETURN_FAILED;
}
if(indexSizePairFIFO.full()) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PusParser::parsePusPackets: FIFO is full" << std::endl;
#else
sif::printError("PusParser::parsePusPackets: FIFO is full\n");
#endif
return HasReturnvaluesIF::RETURN_FAILED;
}
size_t lengthField = frame[4] << 8 | frame[5];
if(lengthField == 0) {
return NO_PACKET_FOUND;
}
// Size of a space packet is the value in the packet length field plus 7
size_t packetSize = lengthField + 7;
if(packetSize > frameSize) {
if(storeSplitPackets) {
indexSizePairFIFO.insert(IndexSizePair(0, frameSize));
foundPackets = 1;
}
else {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "TcSerialPollingTask::readNextPacket: Next packet "
<< "larger than remaining frame," << std::endl;
sif::warning << "Throwing away packet. Detected packet size: "
<< packetSize << std::endl;
#else
sif::printWarning("TcSerialPollingTask::readNextPacket: Next packet "
"larger than remaining frame.\n");
sif::printWarning("Throwing away packet. Detected packet size: %lu",
static_cast<unsigned long>(packetSize));
#endif
#endif /* FSFW_VERBOSE_LEVEL >= 1 */
}
return SPLIT_PACKET;
}
else {
indexSizePairFIFO.insert(IndexSizePair(0, packetSize));
if(packetSize >= frameSize) {
foundPackets = 1;
return HasReturnvaluesIF::RETURN_OK;
}
}
// packet size is smaller than frame size, parse for more packets.
return readMultiplePackets(frame, frameSize, packetSize, foundPackets);
}
ReturnValue_t SpacePacketParser::readMultiplePackets(const uint8_t *frame,
size_t frameSize, size_t startIndex, uint32_t& foundPackets) {
while (startIndex < frameSize) {
ReturnValue_t result = readNextPacket(frame, frameSize, startIndex, foundPackets);
if(result != HasReturnvaluesIF::RETURN_OK) {
return result;
}
}
return HasReturnvaluesIF::RETURN_OK;
}
DynamicFIFO<SpacePacketParser::IndexSizePair>& SpacePacketParser::fifo(){
return indexSizePairFIFO;
}
SpacePacketParser::IndexSizePair SpacePacketParser::getNextFifoPair() {
IndexSizePair nextIndexSizePair;
indexSizePairFIFO.retrieve(&nextIndexSizePair);
return nextIndexSizePair;
}
ReturnValue_t SpacePacketParser::readNextPacket(const uint8_t *frame,
size_t frameSize, size_t& currentIndex, uint32_t& foundPackets) {
if(currentIndex + 5 > frameSize) {
currentIndex = frameSize;
return HasReturnvaluesIF::RETURN_OK;
return HasReturnvaluesIF::RETURN_FAILED;
}
const uint8_t* bufPtr = *buffer;
uint16_t lengthField = frame[currentIndex + 4] << 8 |
frame[currentIndex + 5];
if(lengthField == 0) {
// It is assumed that no packet follows.
currentIndex = frameSize;
return HasReturnvaluesIF::RETURN_OK;
}
size_t nextPacketSize = lengthField + 7;
size_t remainingSize = frameSize - currentIndex;
if(nextPacketSize > remainingSize)
{
if(storeSplitPackets) {
indexSizePairFIFO.insert(IndexSizePair(currentIndex, remainingSize));
foundPackets += 1;
}
else {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "TcSerialPollingTask::readNextPacket: Next packet "
<< "larger than remaining frame." << std::endl;
sif::warning << "Throwing away packet. Detected packet size: "
<< nextPacketSize << std::endl;
#else
sif::printWarning("TcSerialPollingTask::readNextPacket: Next packet "
"larger than remaining frame.\n");
sif::printWarning("Throwing away packet. Detected packet size: %lu\n",
static_cast<unsigned long>(nextPacketSize));
#endif
#endif
}
return SPLIT_PACKET;
}
auto verifyLengthField = [&](size_t idx) {
uint16_t lengthField = bufPtr[idx + 4] << 8 | bufPtr[idx + 5];
size_t packetSize = lengthField + 7;
startIndex = idx;
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
if(lengthField == 0) {
// Skip whole header for now
foundSize = 6;
result = NO_PACKET_FOUND;
}
else if(packetSize + idx > maxSize) {
// Don't increment buffer and read length here, user has to decide what to do
foundSize = packetSize;
return SPLIT_PACKET;
}
else {
foundSize = packetSize;
}
*buffer += foundSize;
readLen += foundSize;
return result;
};
ReturnValue_t result = indexSizePairFIFO.insert(IndexSizePair(currentIndex,
nextPacketSize));
if (result != HasReturnvaluesIF::RETURN_OK) {
// FIFO full.
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::debug << "PusParser: Issue inserting into start index size "
<< "FIFO, it is full!" << std::endl;
#else
sif::printDebug("PusParser: Issue inserting into start index size "
"FIFO, it is full!\n");
#endif
}
foundPackets += 1;
currentIndex += nextPacketSize;
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];
if(std::find(validPacketIds.begin(), validPacketIds.end(), currentPacketId) !=
validPacketIds.end()) {
if(idx + 5 >= maxSize) {
return SPLIT_PACKET;
}
return verifyLengthField(idx);
}
else {
idx++;
}
}
startIndex = 0;
foundSize = maxSize;
*buffer += foundSize;
readLen += foundSize;
return NO_PACKET_FOUND;
}
// Assume that the user verified a valid start of a space packet
else {
return verifyLengthField(idx);
}
}

26
src/fsfw/tmtcservices/SpacePacketParser.h
View File

@ -42,26 +42,30 @@ public:
* Specifies whether split packets are also stored inside the FIFO,
* with the size being the remaining frame size.
*/
SpacePacketParser(uint16_t maxExpectedPusPackets, bool storeSplitPackets);
SpacePacketParser(std::vector<uint16_t> validPacketIds);
/**
* Parse a given frame for PUS packets
* @param frame
* @param frameSize
* @param foundPackets The number of found packets will be stored here
* @return
* -@c NO_PACKET_FOUND if no packet was found
* -@c SPLIT_PACKET if splitting is enabled and a split packet was found
* -@c RETURN_OK if a packet was found. The index and sizes are stored in the internal FIFO
*/
ReturnValue_t parseSpacePackets(const uint8_t* frame, size_t frameSize, uint32_t& foundPackets);
ReturnValue_t parseSpacePackets(const uint8_t* buffer, const size_t maxSize,
size_t& startIndex, size_t& foundSize);
ReturnValue_t parseSpacePackets(const uint8_t **buffer, const size_t maxSize,
size_t& startIndex, size_t& foundSize, size_t& readLen);
/**
* Accessor function to get a reference to the internal FIFO which
* stores pairs of index and packet sizes. This FIFO is filled
* by the #parsePusPackets function.
* @return
*/
DynamicFIFO<IndexSizePair>& fifo();
//DynamicFIFO<IndexSizePair>& fifo();
/**
* Retrieve the next index and packet size pair from the FIFO.
@ -69,7 +73,7 @@ public:
* is empty, an empty pair will be returned.
* @return
*/
IndexSizePair getNextFifoPair();
//IndexSizePair getNextFifoPair();
private:
/**
@ -77,14 +81,16 @@ private:
* inside the receive buffer. The maximum number of entries is defined
* by the first constructor argument.
*/
DynamicFIFO<IndexSizePair> indexSizePairFIFO;
//DynamicFIFO<IndexSizePair> indexSizePairFIFO;
bool storeSplitPackets = false;
std::vector<uint16_t> validPacketIds;
ReturnValue_t readMultiplePackets(const uint8_t *frame, size_t frameSize,
size_t startIndex, uint32_t& foundPackets);
ReturnValue_t readNextPacket(const uint8_t *frame,
size_t frameSize, size_t& startIndex, uint32_t& foundPackets);
//bool storeSplitPackets = false;
// ReturnValue_t readMultiplePackets(const uint8_t *frame, size_t frameSize,
// size_t startIndex, uint32_t& foundPackets);
// ReturnValue_t readNextPacket(const uint8_t *frame,
// size_t frameSize, size_t& startIndex, uint32_t& foundPackets);
};
#endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */