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

TCP refactoring 

This refactoring keeps the TCP connection opened until the client closes
it. It also increased the robustness of the TCP reception.

Because TCP is stream based and usually applied to newline separated
data, a special way to handle binary space packets is required.

The new SpacePacketParser class takes care of this by taking TC packet
IDs as as optional start markers to parse for space packets in a given
buffer.

The refactored TCP server uses a ring buffer, a reception buffer and the
new parser to extract space packets from a stream in a safer way.
This commit is contained in:
Robin Müller 2021-09-28 15:01:01 +02:00
parent 6e88f8f400
commit 09299802f0
No known key found for this signature in database
GPG Key ID: 11D4952C8CCEF814
5 changed files with 473 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,8 +1,11 @@
#include "fsfw/osal/common/TcpTmTcServer.h"
#include "fsfw/osal/common/TcpTmTcBridge.h"
#include "fsfw/osal/common/tcpipHelpers.h"
#include "fsfw/platform.h"
#include "fsfw/FSFW.h"
#include "TcpTmTcServer.h"
#include "TcpTmTcBridge.h"
#include "tcpipHelpers.h"
#include "fsfw/tasks/TaskFactory.h"
#include "fsfw/container/SharedRingBuffer.h"
#include "fsfw/ipc/MessageQueueSenderIF.h"
#include "fsfw/ipc/MutexGuard.h"
@ -17,6 +20,7 @@
#elif defined(PLATFORM_UNIX)
#include <netdb.h>
#endif
#include <chrono>
#ifndef FSFW_TCP_RECV_WIRETAPPING_ENABLED
#define FSFW_TCP_RECV_WIRETAPPING_ENABLED 0
@ -25,12 +29,11 @@
const std::string TcpTmTcServer::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_PORT;
TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
size_t receptionBufferSize, std::string customTcpServerPort):
SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge),
tcpPort(customTcpServerPort), receptionBuffer(receptionBufferSize) {
if(tcpPort == "") {
tcpPort = DEFAULT_SERVER_PORT;
}
size_t receptionBufferSize, size_t ringBufferSize, std::string customTcpServerPort,
ReceptionModes receptionMode):
SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), receptionMode(receptionMode),
tcpConfig(customTcpServerPort), receptionBuffer(receptionBufferSize),
ringBuffer(ringBufferSize, true), validPacketIds() {
}
ReturnValue_t TcpTmTcServer::initialize() {
@ -41,6 +44,16 @@ ReturnValue_t TcpTmTcServer::initialize() {
return result;
}
switch(receptionMode) {
case(ReceptionModes::SPACE_PACKETS): {
spacePacketParser = new SpacePacketParser(validPacketIds);
if(spacePacketParser == nullptr) {
return HasReturnvaluesIF::RETURN_FAILED;
}
tcpConfig.tcpFlags |= MSG_DONTWAIT;
}
}
tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
if (tcStore == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
@ -63,7 +76,7 @@ ReturnValue_t TcpTmTcServer::initialize() {
hints.ai_flags = AI_PASSIVE;
// Listen to all addresses (0.0.0.0) by using AI_PASSIVE in the hint flags
retval = getaddrinfo(nullptr, tcpPort.c_str(), &hints, &addrResult);
retval = getaddrinfo(nullptr, tcpConfig.tcpPort.c_str(), &hints, &addrResult);
if (retval != 0) {
handleError(Protocol::TCP, ErrorSources::GETADDRINFO_CALL);
return HasReturnvaluesIF::RETURN_FAILED;
@ -105,7 +118,7 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
// Listen for connection requests permanently for lifetime of program
while(true) {
retval = listen(listenerTcpSocket, tcpBacklog);
retval = listen(listenerTcpSocket, tcpConfig.tcpBacklog);
if(retval == SOCKET_ERROR) {
handleError(Protocol::TCP, ErrorSources::LISTEN_CALL, 500);
continue;
@ -123,11 +136,12 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
handleServerOperation(connSocket);
// Done, shut down connection and go back to listening for client requests
retval = shutdown(connSocket, SHUT_SEND);
retval = shutdown(connSocket, SHUT_BOTH);
if(retval != 0) {
handleError(Protocol::TCP, ErrorSources::SHUTDOWN_CALL);
}
closeSocket(connSocket);
connSocket = 0;
}
return HasReturnvaluesIF::RETURN_OK;
}
@ -144,51 +158,82 @@ ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() {
return HasReturnvaluesIF::RETURN_OK;
}
void TcpTmTcServer::handleServerOperation(socket_t connSocket) {
int retval = 0;
do {
// Read all telecommands sent by the client
retval = recv(connSocket,
void TcpTmTcServer::handleServerOperation(socket_t& connSocket) {
while (true) {
int retval = recv(
connSocket,
reinterpret_cast<char*>(receptionBuffer.data()),
receptionBuffer.capacity(),
tcpFlags);
if (retval > 0) {
handleTcReception(retval);
tcpConfig.tcpFlags
);
if(retval == 0) {
// Client closed connection
return;
}
else if(retval == 0) {
// Client has finished sending telecommands, send telemetry now
handleTmSending(connSocket);
else if(retval > 0) {
// The ring buffer was configured for overwrite, so the returnvalue does not need to
// be checked for now
ringBuffer.writeData(receptionBuffer.data(), retval);
}
else {
// Should not happen
tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL);
else if(retval < 0) {
if(errno == EAGAIN) {
// No data available. Check whether any packets have been read, then send back
// telemetry if available
bool tcAvailable = false;
bool tmSent = false;
size_t availableReadData = ringBuffer.getAvailableReadData();
if(availableReadData > lastRingBufferSize) {
tcAvailable = true;
handleTcRingBufferData(availableReadData);
}
ReturnValue_t result = handleTmSending(connSocket, tmSent);
if(result == CONN_BROKEN) {
return;
}
if(not tcAvailable and not tmSent) {
TaskFactory::delayTask(DEFAULT_LOOP_DELAY_MS);
}
}
else {
tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL);
}
}
} while(retval > 0);
}
}
ReturnValue_t TcpTmTcServer::handleTcReception(size_t bytesRecvd) {
ReturnValue_t TcpTmTcServer::handleTcReception(uint8_t* spacePacket, size_t packetSize) {
#if FSFW_TCP_RECV_WIRETAPPING_ENABLED == 1
arrayprinter::print(receptionBuffer.data(), bytesRead);
#endif
if(spacePacket == nullptr or packetSize == 0) {
return HasReturnvaluesIF::RETURN_FAILED;
}
store_address_t storeId;
ReturnValue_t result = tcStore->addData(&storeId, receptionBuffer.data(), bytesRecvd);
ReturnValue_t result = tcStore->addData(&storeId, spacePacket, packetSize);
if (result != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning<< "TcpTmTcServer::handleServerOperation: Data storage failed." << std::endl;
sif::warning << "Packet size: " << bytesRecvd << std::endl;
sif::warning << "TcpTmTcServer::handleServerOperation: Data storage with packet size" <<
packetSize << " failed" << std::endl;
#else
sif::printWarning("TcpTmTcServer::handleServerOperation: Data storage with packet size %d "
"failed\n", packetSize);
#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
#endif /* FSFW_VERBOSE_LEVEL >= 1 */
return result;
}
TmTcMessage message(storeId);
result = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
result = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
if (result != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "UdpTcPollingTask::handleSuccessfullTcRead: "
sif::warning << "TcpTmTcServer::handleServerOperation: "
" Sending message to queue failed" << std::endl;
#else
sif::printWarning("TcpTmTcServer::handleServerOperation: "
" Sending message to queue failed\n");
#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
#endif /* FSFW_VERBOSE_LEVEL >= 1 */
tcStore->deleteData(storeId);
@ -196,21 +241,26 @@ ReturnValue_t TcpTmTcServer::handleTcReception(size_t bytesRecvd) {
return result;
}
void TcpTmTcServer::setTcpBacklog(uint8_t tcpBacklog) {
this->tcpBacklog = tcpBacklog;
}
std::string TcpTmTcServer::getTcpPort() const {
return tcpPort;
return tcpConfig.tcpPort;
}
ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket) {
void TcpTmTcServer::setSpacePacketParsingOptions(std::vector<uint16_t> validPacketIds) {
this->validPacketIds = validPacketIds;
}
TcpTmTcServer::TcpConfig& TcpTmTcServer::getTcpConfigStruct() {
return tcpConfig;
}
ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent) {
// Access to the FIFO is mutex protected because it is filled by the bridge
MutexGuard(tmtcBridge->mutex, tmtcBridge->timeoutType, tmtcBridge->mutexTimeoutMs);
store_address_t storeId;
while((not tmtcBridge->tmFifo->empty()) and
(tmtcBridge->packetSentCounter < tmtcBridge->sentPacketsPerCycle)) {
tmtcBridge->tmFifo->retrieve(&storeId);
// Send can fail, so only peek from the FIFO
tmtcBridge->tmFifo->peek(&storeId);
// Using the store accessor will take care of deleting TM from the store automatically
ConstStorageAccessor storeAccessor(storeId);
@ -221,10 +271,101 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket) {
int retval = send(connSocket,
reinterpret_cast<const char*>(storeAccessor.data()),
storeAccessor.size(),
tcpTmFlags);
if(retval != static_cast<int>(storeAccessor.size())) {
tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::SEND_CALL);
tcpConfig.tcpTmFlags);
if(retval == static_cast<int>(storeAccessor.size())) {
// Packet sent, clear FIFO entry
tmtcBridge->tmFifo->pop();
tmSent = true;
}
else if(retval <= 0) {
// Assume that the client has closed the connection here for now
handleSocketError(storeAccessor);
return CONN_BROKEN;
}
}
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) {
ReturnValue_t status = HasReturnvaluesIF::RETURN_OK;
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
size_t readAmount = availableReadData;
lastRingBufferSize = availableReadData;
if(readAmount >= ringBuffer.getMaxSize()) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
// Possible configuration error, too much data or/and data coming in too fast,
// requiring larger buffers
sif::warning << "TcpTmTcServer::handleServerOperation: Ring buffer reached " <<
"fill count" << std::endl;
#else
sif::printWarning("TcpTmTcServer::handleServerOperation: Ring buffer reached "
"fill count");
#endif
#endif
}
if(readAmount >= receptionBuffer.size()) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
// Possible configuration error, too much data or/and data coming in too fast,
// requiring larger buffers
sif::warning << "TcpTmTcServer::handleServerOperation: "
"Reception buffer too small " << std::endl;
#else
sif::printWarning("TcpTmTcServer::handleServerOperation: Reception buffer too small\n");
#endif
#endif
readAmount = receptionBuffer.size();
}
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) {
result = spacePacketParser->parseSpacePackets(bufPtrPtr, readAmount,
startIdx, foundSize, readLen);
switch(result) {
case(SpacePacketParser::NO_PACKET_FOUND):
case(SpacePacketParser::SPLIT_PACKET): {
break;
}
case(HasReturnvaluesIF::RETURN_OK): {
result = handleTcReception(receptionBuffer.data() + startIdx, foundSize);
if(result != HasReturnvaluesIF::RETURN_OK) {
status = result;
}
}
}
ringBuffer.deleteData(foundSize);
lastRingBufferSize = ringBuffer.getAvailableReadData();
std::memset(receptionBuffer.data() + startIdx, 0, foundSize);
}
return status;
}
void TcpTmTcServer::handleSocketError(ConstStorageAccessor &accessor) {
// Don't delete data
accessor.release();
auto socketError = getLastSocketError();
switch(socketError) {
#if defined PLATFORM_WIN
case(WSAECONNRESET): {
// See https://docs.microsoft.com/en-us/windows/win32/api/winsock2/nf-winsock2-send
// Remote client might have shut down connection
return;
}
#else
case(EPIPE): {
// See https://man7.org/linux/man-pages/man2/send.2.html
// Remote client might have shut down connection
return;
}
#endif
default: {
tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::SEND_CALL);
}
}
}

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

@ -1,11 +1,13 @@
#ifndef FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_
#define FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_
#include <fsfw/tmtcservices/SpacePacketParser.h>
#include "TcpIpBase.h"
#include "fsfw/platform.h"
#include "fsfw/osal/common/tcpipHelpers.h"
#include "fsfw/ipc/messageQueueDefinitions.h"
#include "fsfw/container/SimpleRingBuffer.h"
#include "fsfw/ipc/MessageQueueIF.h"
#include "fsfw/objectmanager/frameworkObjects.h"
#include "fsfw/objectmanager/SystemObject.h"
@ -42,9 +44,37 @@ class TcpTmTcServer:
public TcpIpBase,
public ExecutableObjectIF {
public:
enum class ReceptionModes {
SPACE_PACKETS
};
struct TcpConfig {
public:
TcpConfig(std::string tcpPort): tcpPort(tcpPort) {}
/**
* Passed to the recv call
*/
int tcpFlags = 0;
int tcpBacklog = 3;
/**
* Passed to the select call which is used to ensure non-blocking TC reception
*/
//uint32_t selectTimeoutMs = DEFAULT_SELECT_TIMEOUT_MS;
/**
* Passed to the send call
*/
int tcpTmFlags = 0;
const std::string tcpPort;
};
static const std::string DEFAULT_SERVER_PORT;
static constexpr size_t ETHERNET_MTU_SIZE = 1500;
static constexpr size_t RING_BUFFER_SIZE = ETHERNET_MTU_SIZE * 3;
static constexpr uint32_t DEFAULT_LOOP_DELAY_MS = 200;
/**
* TCP Server Constructor
@ -55,11 +85,19 @@ public:
* @param customTcpServerPort The user can specify another port than the default (7301) here.
*/
TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
size_t receptionBufferSize = ETHERNET_MTU_SIZE + 1,
std::string customTcpServerPort = "");
size_t receptionBufferSize = RING_BUFFER_SIZE,
size_t ringBufferSize = RING_BUFFER_SIZE,
std::string customTcpServerPort = DEFAULT_SERVER_PORT,
ReceptionModes receptionMode = ReceptionModes::SPACE_PACKETS);
virtual~ TcpTmTcServer();
void setTcpBacklog(uint8_t tcpBacklog);
/**
* Get a handle to the TCP configuration struct, which can be used to configure TCP
* properties
* @return
*/
TcpConfig& getTcpConfigStruct();
void setSpacePacketParsingOptions(std::vector<uint16_t> validPacketIds);
ReturnValue_t initialize() override;
ReturnValue_t performOperation(uint8_t opCode) override;
@ -71,25 +109,29 @@ protected:
StorageManagerIF* tcStore = nullptr;
StorageManagerIF* tmStore = nullptr;
private:
static constexpr ReturnValue_t CONN_BROKEN = HasReturnvaluesIF::makeReturnCode(1, 0);
//! TMTC bridge is cached.
object_id_t tmtcBridgeId = objects::NO_OBJECT;
TcpTmTcBridge* tmtcBridge = nullptr;
std::string tcpPort;
int tcpFlags = 0;
socket_t listenerTcpSocket = 0;
ReceptionModes receptionMode;
TcpConfig tcpConfig;
struct sockaddr tcpAddress;
socket_t listenerTcpSocket = 0;
MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE;
int tcpAddrLen = sizeof(tcpAddress);
int tcpBacklog = 3;
std::vector<uint8_t> receptionBuffer;
int tcpSockOpt = 0;
int tcpTmFlags = 0;
SimpleRingBuffer ringBuffer;
std::vector<uint16_t> validPacketIds;
SpacePacketParser* spacePacketParser = nullptr;
uint8_t lastRingBufferSize = 0;
void handleServerOperation(socket_t connSocket);
ReturnValue_t handleTcReception(size_t bytesRecvd);
ReturnValue_t handleTmSending(socket_t connSocket);
virtual void handleServerOperation(socket_t& connSocket);
ReturnValue_t handleTcReception(uint8_t* spacePacket, size_t packetSize);
ReturnValue_t handleTmSending(socket_t connSocket, bool& tmSent);
ReturnValue_t handleTcRingBufferData(size_t availableReadData);
void handleSocketError(ConstStorageAccessor& accessor);
};
#endif /* FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_ */

109
src/fsfw/tmtcpacket/SpacePacket.h
View File

@ -15,56 +15,67 @@
*/
class SpacePacket: public SpacePacketBase {
public:
static const uint16_t PACKET_MAX_SIZE = 1024;
/**
* The constructor initializes the packet and sets all header information
* according to the passed parameters.
* @param packetDataLength Sets the packet data length field and therefore specifies the size of the packet.
* @param isTelecommand Sets the packet type field to either TC (true) or TM (false).
* @param apid Sets the packet's APID field. The default value describes an idle packet.
* @param sequenceCount ets the packet's Source Sequence Count field.
*/
SpacePacket(uint16_t packetDataLength, bool isTelecommand = false,
uint16_t apid = APID_IDLE_PACKET, uint16_t sequenceCount = 0);
/**
* The class's default destructor.
*/
virtual ~SpacePacket();
/**
* With this call, the complete data content (including the CCSDS Primary
* Header) is overwritten with the byte stream given.
* @param p_data Pointer to data to overwrite the content with
* @param packet_size Size of the data
* @return @li \c true if packet_size is smaller than \c MAX_PACKET_SIZE.
* @li \c false else.
*/
bool addWholeData(const uint8_t* p_data, uint32_t packet_size);
static const uint16_t PACKET_MAX_SIZE = 1024;
/**
* The constructor initializes the packet and sets all header information
* according to the passed parameters.
* @param packetDataLength Sets the packet data length field and therefore specifies
* the size of the packet.
* @param isTelecommand Sets the packet type field to either TC (true) or TM (false).
* @param apid Sets the packet's APID field. The default value describes an idle packet.
* @param sequenceCount ets the packet's Source Sequence Count field.
*/
SpacePacket(uint16_t packetDataLength, bool isTelecommand = false,
uint16_t apid = APID_IDLE_PACKET, uint16_t sequenceCount = 0);
/**
* The class's default destructor.
*/
virtual ~SpacePacket();
static constexpr uint16_t getTcSpacePacketIdFromApid(uint16_t apid) {
uint16_t tcPacketId = (0x18 << 8) | (((apid >> 8) & 0x07) << 8) | (apid & 0x00ff);
return tcPacketId;
}
static constexpr uint16_t getTmSpacePacketIdFromApid(uint16_t apid) {
uint16_t tmPacketId = (0x08 << 8) | (((apid >> 8) & 0x07) << 8) | (apid & 0x00ff);
return tmPacketId;
}
/**
* With this call, the complete data content (including the CCSDS Primary
* Header) is overwritten with the byte stream given.
* @param p_data Pointer to data to overwrite the content with
* @param packet_size Size of the data
* @return @li \c true if packet_size is smaller than \c MAX_PACKET_SIZE.
* @li \c false else.
*/
bool addWholeData(const uint8_t* p_data, uint32_t packet_size);
protected:
/**
* This structure defines the data structure of a Space Packet as local data.
* There's a buffer which corresponds to the Space Packet Data Field with a
* maximum size of \c PACKET_MAX_SIZE.
*/
struct PacketStructured {
CCSDSPrimaryHeader header;
uint8_t buffer[PACKET_MAX_SIZE];
};
/**
* This union simplifies accessing the full data content of the Space Packet.
* This is achieved by putting the \c PacketStructured struct in a union with
* a plain buffer.
*/
union SpacePacketData {
PacketStructured fields;
uint8_t byteStream[PACKET_MAX_SIZE + sizeof(CCSDSPrimaryHeader)];
};
/**
* This is the data representation of the class.
* It is a struct of CCSDS Primary Header and a data field, which again is
* packed in an union, so the data can be accessed as a byte stream without
* a cast.
*/
SpacePacketData localData;
/**
* This structure defines the data structure of a Space Packet as local data.
* There's a buffer which corresponds to the Space Packet Data Field with a
* maximum size of \c PACKET_MAX_SIZE.
*/
struct PacketStructured {
CCSDSPrimaryHeader header;
uint8_t buffer[PACKET_MAX_SIZE];
};
/**
* This union simplifies accessing the full data content of the Space Packet.
* This is achieved by putting the \c PacketStructured struct in a union with
* a plain buffer.
*/
union SpacePacketData {
PacketStructured fields;
uint8_t byteStream[PACKET_MAX_SIZE + sizeof(CCSDSPrimaryHeader)];
};
/**
* This is the data representation of the class.
* It is a struct of CCSDS Primary Header and a data field, which again is
* packed in an union, so the data can be accessed as a byte stream without
* a cast.
*/
SpacePacketData localData;
};
#endif /* SPACEPACKET_H_ */

77
src/fsfw/tmtcservices/SpacePacketParser.cpp Normal file
View File

@ -0,0 +1,77 @@
#include <fsfw/serviceinterface/ServiceInterface.h>
#include <fsfw/tmtcservices/SpacePacketParser.h>
#include <algorithm>
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) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "SpacePacketParser::parseSpacePackets: Frame invalid" << std::endl;
#else
sif::printWarning("SpacePacketParser::parseSpacePackets: Frame invalid\n");
#endif
return HasReturnvaluesIF::RETURN_FAILED;
}
const uint8_t* bufPtr = *buffer;
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;
};
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);
}
}

96
src/fsfw/tmtcservices/SpacePacketParser.h Normal file
View File

@ -0,0 +1,96 @@
#ifndef FRAMEWORK_TMTCSERVICES_PUSPARSER_H_
#define FRAMEWORK_TMTCSERVICES_PUSPARSER_H_
#include "fsfw/container/DynamicFIFO.h"
#include "fsfw/returnvalues/FwClassIds.h"
#include <utility>
#include <cstdint>
/**
* @brief This small helper class scans a given buffer for PUS packets.
* Can be used if PUS packets are serialized in a tightly packed frame.
* @details
* The parser uses the length field field of the space packets to find
* the respective space packet sizes.
*
* The parser parses a buffer by taking a pointer and the maximum size to scan.
* If space packets are found, they are stored in a FIFO which stores pairs
* consisting of the index in the buffer and the respective packet sizes.
*
* If the parser detects split packets (which means that the size of the
* next packet is larger than the remaining size to scan), it can either
* store that split packet or throw away the packet.
* @author R. Mueller
*/
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>;
static constexpr uint8_t INTERFACE_ID = CLASS_ID::PUS_PARSER;
static constexpr ReturnValue_t NO_PACKET_FOUND = MAKE_RETURN_CODE(0x00);
static constexpr ReturnValue_t SPLIT_PACKET = MAKE_RETURN_CODE(0x01);
/**
* @brief Parser constructor.
* @param maxExpectedPusPackets
* Maximum expected number of PUS packets. A good estimate is to divide
* the frame size by the minimum size of a PUS packet (12 bytes)
* @param storeSplitPackets
* Specifies whether split packets are also stored inside the FIFO,
* with the size being the remaining frame size.
*/
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* 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();
/**
* Retrieve the next index and packet size pair from the FIFO.
* This also removes it from the FIFO. Please note that if the FIFO
* is empty, an empty pair will be returned.
* @return
*/
//IndexSizePair getNextFifoPair();
private:
/**
* A FIFO is used to store information about multiple PUS packets
* inside the receive buffer. The maximum number of entries is defined
* by the first constructor argument.
*/
//DynamicFIFO<IndexSizePair> indexSizePairFIFO;
std::vector<uint16_t> validPacketIds;
//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_ */