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

Update FSFW #25

Merged
muellerr merged 89 commits from mueller/master into master 2021-10-05 20:21:44 +02:00
Conversation 0 Commits 89 Files Changed 28 +473 -106
5 changed files with 473 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 09299802f0 - Show all commits

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_ */