fsfw/fsfw
fsfw
/
fsfw
10
3
Fork 6
Code Issues 31 Pull Requests 4 Releases 10 Wiki Activity

Keep Open TCP Implementation #496

Merged
gaisser merged 23 commits from eive/fsfw:mueller/tcp-keep-open-pr into development 2021-10-18 15:13:09 +02:00
Conversation 1 Commits 23 Files Changed 8 +531 -114
8 changed files with 531 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,13 +1,17 @@
#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/tmtcservices/SpacePacketParser.h"
#include "fsfw/tasks/TaskFactory.h"
#include "fsfw/globalfunctions/arrayprinter.h"
#include "fsfw/container/SharedRingBuffer.h"
#include "fsfw/ipc/MessageQueueSenderIF.h"
#include "fsfw/ipc/MutexGuard.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/tmtcservices/TmTcMessage.h"
@ -18,19 +22,14 @@
#include <netdb.h>
#endif
#ifndef FSFW_TCP_RECV_WIRETAPPING_ENABLED
#define FSFW_TCP_RECV_WIRETAPPING_ENABLED 0
#endif
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) {
}
ReturnValue_t TcpTmTcServer::initialize() {
@ -41,6 +40,17 @@ ReturnValue_t TcpTmTcServer::initialize() {
return result;
}
switch(receptionMode) {
case(ReceptionModes::SPACE_PACKETS): {
spacePacketParser = new SpacePacketParser(validPacketIds);
if(spacePacketParser == nullptr) {
return HasReturnvaluesIF::RETURN_FAILED;
}
#if defined PLATFORM_UNIX
tcpConfig.tcpFlags |= MSG_DONTWAIT;
#endif
}
}
tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
if (tcStore == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
@ -63,7 +73,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 +115,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 +133,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 +155,101 @@ 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) {
#if defined PLATFORM_WIN
setSocketNonBlocking(connSocket);
#endif
while (true) {
int retval = recv(
connSocket,
reinterpret_cast<char*>(receptionBuffer.data()),
receptionBuffer.capacity(),
tcpFlags);
if (retval > 0) {
handleTcReception(retval);
tcpConfig.tcpFlags
);
if(retval == 0) {
size_t availableReadData = ringBuffer.getAvailableReadData();
if(availableReadData > lastRingBufferSize) {
handleTcRingBufferData(availableReadData);
}
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) {
int errorValue = getLastSocketError();
#if defined PLATFORM_UNIX
int wouldBlockValue = EAGAIN;
#elif defined PLATFORM_WIN
int wouldBlockValue = WSAEWOULDBLOCK;
#endif
if(errorValue == wouldBlockValue) {
// 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(tcpConfig.tcpLoopDelay);
}
}
else {
tcpip::handleError(tcpip::Protocol::TCP, tcpip::ErrorSources::RECV_CALL, 300);
}
}
} while(retval > 0);
}
}
ReturnValue_t TcpTmTcServer::handleTcReception(size_t bytesRecvd) {
#if FSFW_TCP_RECV_WIRETAPPING_ENABLED == 1
arrayprinter::print(receptionBuffer.data(), bytesRead);
ReturnValue_t TcpTmTcServer::handleTcReception(uint8_t* spacePacket, size_t packetSize) {
if(wiretappingEnabled) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::info << "Received TC:" << std::endl;
#else
sif::printInfo("Received TC:\n");
#endif
arrayprinter::print(spacePacket, packetSize);
}
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 +257,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);
@ -218,13 +284,134 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket) {
if(result != HasReturnvaluesIF::RETURN_OK) {
return result;
}
if(wiretappingEnabled) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::info << "Sending TM:" << std::endl;
#else
sif::printInfo("Sending TM:\n");
#endif
arrayprinter::print(storeAccessor.data(), storeAccessor.size());
}
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::enableWiretapping(bool enable) {
this->wiretappingEnabled = enable;
}
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);
}
}
}
#if defined PLATFORM_WIN
void TcpTmTcServer::setSocketNonBlocking(socket_t &connSocket) {
u_long iMode = 1;
int iResult = ioctlsocket(connSocket, FIONBIO, &iMode);
if(iResult != NO_ERROR) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "TcpTmTcServer::handleServerOperation: Setting socket"
" non-blocking failed with error " << iResult;
#else
sif::printWarning("TcpTmTcServer::handleServerOperation: Setting socket"
" non-blocking failed with error %d\n", iResult);
#endif
#endif
}
}
#endif

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

@ -6,6 +6,7 @@
#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"
@ -20,6 +21,7 @@
#include <vector>
class TcpTmTcBridge;
class SpacePacketParser;
/**
* @brief TCP server implementation
@ -42,9 +44,38 @@ 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;
/**
* If no telecommands packets are being received and no telemetry is being sent,
* the TCP server will delay periodically by this amount to decrease the CPU load
*/
uint32_t tcpLoopDelay = DEFAULT_LOOP_DELAY_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 +86,21 @@ 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);
void enableWiretapping(bool enable);
/**
* 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 +112,33 @@ 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;
bool wiretappingEnabled = false;
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);
#if defined PLATFORM_WIN
void setSocketNonBlocking(socket_t& connSocket);
#endif
};
#endif /* FSFW_OSAL_COMMON_TCP_TMTC_SERVER_H_ */

3
src/fsfw/osal/common/tcpipCommon.cpp
View File

@ -21,6 +21,9 @@ void tcpip::determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std:
if(errorSrc == ErrorSources::SETSOCKOPT_CALL) {
srcString = "setsockopt call";
}
if(errorSrc == ErrorSources::BIND_CALL) {
srcString = "bind call";
}
else if(errorSrc == ErrorSources::SOCKET_CALL) {
srcString = "socket call";
}

1
src/fsfw/returnvalues/FwClassIds.h
View File

@ -80,6 +80,7 @@ enum: uint8_t {
FIXED_SLOT_TASK_IF, //FTIF
MGM_LIS3MDL, //MGMLIS3
MGM_RM3100, //MGMRM3100
SPACE_PACKET_PARSER, //SPPA
FW_CLASS_ID_COUNT // [EXPORT] : [END]
};

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

@ -15,57 +15,78 @@
*/
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();
/**
* 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;
};
namespace spacepacket {
constexpr uint16_t getSpacePacketIdFromApid(bool isTc, uint16_t apid,
bool secondaryHeaderFlag = true) {
return (((isTc << 5) & 0x10) | ((secondaryHeaderFlag << 4) & 0x08) |
((apid >> 8) & 0x07)) << 8 | (apid & 0x00ff);
}
constexpr uint16_t getTcSpacketIdFromApid(uint16_t apid,
bool secondaryHeaderFlag = true) {
return getSpacePacketIdFromApid(true, apid, secondaryHeaderFlag);
}
constexpr uint16_t getTmSpacketIdFromApid(uint16_t apid,
bool secondaryHeaderFlag = true) {
return getSpacePacketIdFromApid(false, apid, secondaryHeaderFlag);
}
}
#endif /* SPACEPACKET_H_ */

1
src/fsfw/tmtcservices/CMakeLists.txt
View File

@ -6,4 +6,5 @@ target_sources(${LIB_FSFW_NAME}
TmTcBridge.cpp
TmTcMessage.cpp
VerificationReporter.cpp
SpacePacketParser.cpp
)

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 += idx + foundSize;
gaisser marked this conversation as resolved
gaisser commented 2021-10-11 14:59:24 +02:00
Review
Copy Link

What does readLen mean? If you add += idx you might have more bytes than the buffer size.

What does readLen mean? If you add += idx you might have more bytes than the buffer size.
muellerr commented 2021-10-11 16:58:21 +02:00
Review
Copy Link

Total read length in buffer. If the packet does not start with the start marker, idx will be incremented until a start marker is detected. It then needs to be added on top of the found size

Total read length in buffer. If the packet does not start with the start marker, idx will be incremented until a start marker is detected. It then needs to be added on top of the found size
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);
}
}

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

@ -0,0 +1,78 @@
#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 space packets.
* Can be used if space packets are serialized in a tightly packed frame.
* @details
* The parser uses the length field field and the 16-bit TC packet ID of the space packets to find
* find space packets in a given data stream
* @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::SPACE_PACKET_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 validPacketIds This vector contains the allowed 16-bit TC packet ID start markers
* The parser will search for these stark markers to detect the start of a space packet.
* It is also possible to pass an empty vector here, but this is not recommended.
* If an empty vector is passed, the parser will assume that the start of the given stream
* contains the start of a new space packet.
*/
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
* @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 RETURN_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& readLen);
/**
* 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 RETURN_OK if a packet was found
*/
ReturnValue_t parseSpacePackets(const uint8_t* buffer, const size_t maxSize,
size_t& startIndex, size_t& foundSize);
private:
std::vector<uint16_t> validPacketIds;
};
#endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */