esbo_ds/fsfw
3
0
Fork 0
forked from fsfw/fsfw
Code Issues Pull Requests Releases Wiki Activity

TCP refactoring

Browse Source 
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/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/container/SharedRingBuffer.h"
#include "fsfw/ipc/MessageQueueSenderIF.h" #include "fsfw/ipc/MessageQueueSenderIF.h"
#include "fsfw/ipc/MutexGuard.h" #include "fsfw/ipc/MutexGuard.h"
@ -17,6 +20,7 @@
#elif defined(PLATFORM_UNIX) #elif defined(PLATFORM_UNIX)
#include <netdb.h> #include <netdb.h>
#endif #endif
#include <chrono>
#ifndef FSFW_TCP_RECV_WIRETAPPING_ENABLED #ifndef FSFW_TCP_RECV_WIRETAPPING_ENABLED
#define FSFW_TCP_RECV_WIRETAPPING_ENABLED 0 #define FSFW_TCP_RECV_WIRETAPPING_ENABLED 0
@ -25,12 +29,11 @@
const std::string TcpTmTcServer::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_PORT; const std::string TcpTmTcServer::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_PORT;
TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge, TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
size_t receptionBufferSize, std::string customTcpServerPort): size_t receptionBufferSize, size_t ringBufferSize, std::string customTcpServerPort,
SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), ReceptionModes receptionMode):
tcpPort(customTcpServerPort), receptionBuffer(receptionBufferSize) { SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), receptionMode(receptionMode),
if(tcpPort == "") { tcpConfig(customTcpServerPort), receptionBuffer(receptionBufferSize),
tcpPort = DEFAULT_SERVER_PORT; ringBuffer(ringBufferSize, true), validPacketIds() {
}
} }
ReturnValue_t TcpTmTcServer::initialize() { ReturnValue_t TcpTmTcServer::initialize() {
@ -41,6 +44,16 @@ ReturnValue_t TcpTmTcServer::initialize() {
return result; 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); tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
if (tcStore == nullptr) { if (tcStore == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
@ -63,7 +76,7 @@ ReturnValue_t TcpTmTcServer::initialize() {
hints.ai_flags = AI_PASSIVE; hints.ai_flags = AI_PASSIVE;
// Listen to all addresses (0.0.0.0) by using AI_PASSIVE in the hint flags // 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) { if (retval != 0) {
handleError(Protocol::TCP, ErrorSources::GETADDRINFO_CALL); handleError(Protocol::TCP, ErrorSources::GETADDRINFO_CALL);
return HasReturnvaluesIF::RETURN_FAILED; return HasReturnvaluesIF::RETURN_FAILED;
@ -105,7 +118,7 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
// Listen for connection requests permanently for lifetime of program // Listen for connection requests permanently for lifetime of program
while(true) { while(true) {
retval = listen(listenerTcpSocket, tcpBacklog); retval = listen(listenerTcpSocket, tcpConfig.tcpBacklog);
if(retval == SOCKET_ERROR) { if(retval == SOCKET_ERROR) {
handleError(Protocol::TCP, ErrorSources::LISTEN_CALL, 500); handleError(Protocol::TCP, ErrorSources::LISTEN_CALL, 500);
continue; continue;
@ -123,11 +136,12 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
handleServerOperation(connSocket); handleServerOperation(connSocket);
// Done, shut down connection and go back to listening for client requests // 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) { if(retval != 0) {
handleError(Protocol::TCP, ErrorSources::SHUTDOWN_CALL); handleError(Protocol::TCP, ErrorSources::SHUTDOWN_CALL);
} }
closeSocket(connSocket); closeSocket(connSocket);
connSocket = 0;
} }
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }
@ -144,51 +158,82 @@ ReturnValue_t TcpTmTcServer::initializeAfterTaskCreation() {
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }
void TcpTmTcServer::handleServerOperation(socket_t connSocket) { void TcpTmTcServer::handleServerOperation(socket_t& connSocket) {
int retval = 0; while (true) {
do { int retval = recv(
// Read all telecommands sent by the client connSocket,
retval = recv(connSocket,
reinterpret_cast<char*>(receptionBuffer.data()), reinterpret_cast<char*>(receptionBuffer.data()),
receptionBuffer.capacity(), receptionBuffer.capacity(),
tcpFlags); tcpConfig.tcpFlags
if (retval > 0) { );
handleTcReception(retval); if(retval == 0) {
// Client closed connection
return;
} }
else if(retval == 0) { else if(retval > 0) {
// Client has finished sending telecommands, send telemetry now // The ring buffer was configured for overwrite, so the returnvalue does not need to
handleTmSending(connSocket); // be checked for now
ringBuffer.writeData(receptionBuffer.data(), retval);
} }
else { else if(retval < 0) {
// Should not happen if(errno == EAGAIN) {
tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL); // 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 #if FSFW_TCP_RECV_WIRETAPPING_ENABLED == 1
arrayprinter::print(receptionBuffer.data(), bytesRead); arrayprinter::print(receptionBuffer.data(), bytesRead);
#endif #endif
if(spacePacket == nullptr or packetSize == 0) {
return HasReturnvaluesIF::RETURN_FAILED;
}
store_address_t storeId; 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 (result != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning<< "TcpTmTcServer::handleServerOperation: Data storage failed." << std::endl; sif::warning << "TcpTmTcServer::handleServerOperation: Data storage with packet size" <<
sif::warning << "Packet size: " << bytesRecvd << std::endl; 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_CPP_OSTREAM_ENABLED == 1 */
#endif /* FSFW_VERBOSE_LEVEL >= 1 */ #endif /* FSFW_VERBOSE_LEVEL >= 1 */
return result;
} }
TmTcMessage message(storeId); TmTcMessage message(storeId);
result = MessageQueueSenderIF::sendMessage(targetTcDestination, &message); result = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
if (result != HasReturnvaluesIF::RETURN_OK) { if (result != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "UdpTcPollingTask::handleSuccessfullTcRead: " sif::warning << "TcpTmTcServer::handleServerOperation: "
" Sending message to queue failed" << std::endl; " 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_CPP_OSTREAM_ENABLED == 1 */
#endif /* FSFW_VERBOSE_LEVEL >= 1 */ #endif /* FSFW_VERBOSE_LEVEL >= 1 */
tcStore->deleteData(storeId); tcStore->deleteData(storeId);
@ -196,21 +241,26 @@ ReturnValue_t TcpTmTcServer::handleTcReception(size_t bytesRecvd) {
return result; return result;
} }
void TcpTmTcServer::setTcpBacklog(uint8_t tcpBacklog) {
this->tcpBacklog = tcpBacklog;
}
std::string TcpTmTcServer::getTcpPort() const { 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 // Access to the FIFO is mutex protected because it is filled by the bridge
MutexGuard(tmtcBridge->mutex, tmtcBridge->timeoutType, tmtcBridge->mutexTimeoutMs); MutexGuard(tmtcBridge->mutex, tmtcBridge->timeoutType, tmtcBridge->mutexTimeoutMs);
store_address_t storeId; store_address_t storeId;
while((not tmtcBridge->tmFifo->empty()) and while((not tmtcBridge->tmFifo->empty()) and
(tmtcBridge->packetSentCounter < tmtcBridge->sentPacketsPerCycle)) { (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 // Using the store accessor will take care of deleting TM from the store automatically
ConstStorageAccessor storeAccessor(storeId); ConstStorageAccessor storeAccessor(storeId);
@ -221,10 +271,101 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket) {
int retval = send(connSocket, int retval = send(connSocket,
reinterpret_cast<const char*>(storeAccessor.data()), reinterpret_cast<const char*>(storeAccessor.data()),
storeAccessor.size(), storeAccessor.size(),
tcpTmFlags); tcpConfig.tcpTmFlags);
if(retval != static_cast<int>(storeAccessor.size())) { if(retval == static_cast<int>(storeAccessor.size())) {
tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::SEND_CALL); // 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; 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_ #ifndef FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_
#define FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_ #define FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_
#include <fsfw/tmtcservices/SpacePacketParser.h>
#include "TcpIpBase.h" #include "TcpIpBase.h"
#include "fsfw/platform.h" #include "fsfw/platform.h"
#include "fsfw/osal/common/tcpipHelpers.h" #include "fsfw/osal/common/tcpipHelpers.h"
#include "fsfw/ipc/messageQueueDefinitions.h" #include "fsfw/ipc/messageQueueDefinitions.h"
#include "fsfw/container/SimpleRingBuffer.h"
#include "fsfw/ipc/MessageQueueIF.h" #include "fsfw/ipc/MessageQueueIF.h"
#include "fsfw/objectmanager/frameworkObjects.h" #include "fsfw/objectmanager/frameworkObjects.h"
#include "fsfw/objectmanager/SystemObject.h" #include "fsfw/objectmanager/SystemObject.h"
@ -42,9 +44,37 @@ class TcpTmTcServer:
public TcpIpBase, public TcpIpBase,
public ExecutableObjectIF { public ExecutableObjectIF {
public: 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 const std::string DEFAULT_SERVER_PORT;
static constexpr size_t ETHERNET_MTU_SIZE = 1500; 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 * TCP Server Constructor
@ -55,11 +85,19 @@ public:
* @param customTcpServerPort The user can specify another port than the default (7301) here. * @param customTcpServerPort The user can specify another port than the default (7301) here.
*/ */
TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge, TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge,
size_t receptionBufferSize = ETHERNET_MTU_SIZE + 1, size_t receptionBufferSize = RING_BUFFER_SIZE,
std::string customTcpServerPort = ""); size_t ringBufferSize = RING_BUFFER_SIZE,
std::string customTcpServerPort = DEFAULT_SERVER_PORT,
ReceptionModes receptionMode = ReceptionModes::SPACE_PACKETS);
virtual~ TcpTmTcServer(); 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 initialize() override;
ReturnValue_t performOperation(uint8_t opCode) override; ReturnValue_t performOperation(uint8_t opCode) override;
@ -71,25 +109,29 @@ protected:
StorageManagerIF* tcStore = nullptr; StorageManagerIF* tcStore = nullptr;
StorageManagerIF* tmStore = nullptr; StorageManagerIF* tmStore = nullptr;
private: private:
static constexpr ReturnValue_t CONN_BROKEN = HasReturnvaluesIF::makeReturnCode(1, 0);
//! TMTC bridge is cached. //! TMTC bridge is cached.
object_id_t tmtcBridgeId = objects::NO_OBJECT; object_id_t tmtcBridgeId = objects::NO_OBJECT;
TcpTmTcBridge* tmtcBridge = nullptr; TcpTmTcBridge* tmtcBridge = nullptr;
std::string tcpPort; ReceptionModes receptionMode;
int tcpFlags = 0; TcpConfig tcpConfig;
socket_t listenerTcpSocket = 0;
struct sockaddr tcpAddress; struct sockaddr tcpAddress;
socket_t listenerTcpSocket = 0;
MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE; MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE;
int tcpAddrLen = sizeof(tcpAddress);
int tcpBacklog = 3;
std::vector<uint8_t> receptionBuffer; std::vector<uint8_t> receptionBuffer;
int tcpSockOpt = 0; SimpleRingBuffer ringBuffer;
int tcpTmFlags = 0; std::vector<uint16_t> validPacketIds;
SpacePacketParser* spacePacketParser = nullptr;
uint8_t lastRingBufferSize = 0;
void handleServerOperation(socket_t connSocket); virtual void handleServerOperation(socket_t& connSocket);
ReturnValue_t handleTcReception(size_t bytesRecvd); ReturnValue_t handleTcReception(uint8_t* spacePacket, size_t packetSize);
ReturnValue_t handleTmSending(socket_t connSocket); 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_ */ #endif /* FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_ */

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

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