Merge remote-tracking branch 'origin/development' into mueller/refactor-tmtc-stack

This commit is contained in:
Robin Müller 2022-09-01 17:48:40 +02:00
commit 5c20cc804e
12 changed files with 130 additions and 135 deletions

View File

@ -8,6 +8,7 @@
#include "fsfw/ipc/MessageQueueMessage.h"
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/serialize/SerialBufferAdapter.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
#include "fsfw/storagemanager/StorageManagerIF.h"
#include "fsfw/subsystem/SubsystemBase.h"
@ -1257,28 +1258,30 @@ ReturnValue_t DeviceHandlerBase::letChildHandleMessage(CommandMessage* message)
return returnvalue::FAILED;
}
void DeviceHandlerBase::handleDeviceTM(SerializeIF* dataSet, DeviceCommandId_t replyId,
bool forceDirectTm) {
if (dataSet == nullptr) {
return;
}
void DeviceHandlerBase::handleDeviceTm(const uint8_t* rawData, size_t rawDataLen,
DeviceCommandId_t replyId, bool forceDirectTm) {
SerialBufferAdapter bufferWrapper(rawData, rawDataLen);
handleDeviceTm(bufferWrapper, replyId, forceDirectTm);
}
DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId);
void DeviceHandlerBase::handleDeviceTm(const SerializeIF& dataSet, DeviceCommandId_t replyId,
bool forceDirectTm) {
auto iter = deviceReplyMap.find(replyId);
if (iter == deviceReplyMap.end()) {
triggerEvent(DEVICE_UNKNOWN_REPLY, replyId);
return;
}
/* Regular replies to a command */
// Regular replies to a command
if (iter->second.command != deviceCommandMap.end()) {
MessageQueueId_t queueId = iter->second.command->second.sendReplyTo;
if (queueId != NO_COMMANDER) {
/* This may fail, but we'll ignore the fault. */
actionHelper.reportData(queueId, replyId, dataSet);
// This may fail, but we'll ignore the fault.
actionHelper.reportData(queueId, replyId, const_cast<SerializeIF*>(&dataSet));
}
/* This check should make sure we get any TM but don't get anything doubled. */
// This check should make sure we get any TM but don't get anything doubled.
if (wiretappingMode == TM && (requestedRawTraffic != queueId)) {
DeviceTmReportingWrapper wrapper(getObjectId(), replyId, dataSet);
actionHelper.reportData(requestedRawTraffic, replyId, &wrapper);
@ -1289,22 +1292,17 @@ void DeviceHandlerBase::handleDeviceTM(SerializeIF* dataSet, DeviceCommandId_t r
// hiding of sender needed so the service will handle it as
// unexpected Data, no matter what state (progress or completed)
// it is in
actionHelper.reportData(defaultRawReceiver, replyId, dataSet, true);
actionHelper.reportData(defaultRawReceiver, replyId, const_cast<SerializeIF*>(&dataSet),
true);
}
}
/* Unrequested or aperiodic replies */
// Unrequested or aperiodic replies
else {
DeviceTmReportingWrapper wrapper(getObjectId(), replyId, dataSet);
if (wiretappingMode == TM) {
actionHelper.reportData(requestedRawTraffic, replyId, &wrapper);
}
if (forceDirectTm and defaultRawReceiver != MessageQueueIF::NO_QUEUE) {
// sid_t setSid = sid_t(this->getObjectId(), replyId);
// LocalPoolDataSetBase* dataset = getDataSetHandle(setSid);
// if(dataset != nullptr) {
// poolManager.generateHousekeepingPacket(setSid, dataset, true);
// }
// hiding of sender needed so the service will handle it as
// unexpected Data, no matter what state (progress or completed)
// it is in
@ -1325,10 +1323,10 @@ ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, MessageQueue
} else if (iter->second.isExecuting) {
result = COMMAND_ALREADY_SENT;
} else {
iter->second.sendReplyTo = commandedBy;
result = buildCommandFromCommand(actionId, data, size);
}
if (result == returnvalue::OK) {
iter->second.sendReplyTo = commandedBy;
iter->second.isExecuting = true;
cookieInfo.pendingCommand = iter;
cookieInfo.state = COOKIE_WRITE_READY;

View File

@ -1052,9 +1052,25 @@ class DeviceHandlerBase : public DeviceHandlerIF,
bool isAwaitingReply();
void handleDeviceTM(SerializeIF *dataSet, DeviceCommandId_t replyId, bool forceDirectTm = false);
// void handleDeviceTM(uint8_t* data, size_t dataSize, DeviceCommandId_t replyId,
// bool forceDirectTm);
/**
* Wrapper function for @handleDeviceTm which wraps the raw buffer with @SerialBufferAdapter.
* For interpreted data, prefer the other function.
* @param rawData
* @param rawDataLen
* @param replyId
* @param forceDirectTm
*/
void handleDeviceTm(const uint8_t *rawData, size_t rawDataLen, DeviceCommandId_t replyId,
bool forceDirectTm = false);
/**
* Can be used to handle Service 8 data replies. This will also generate the TM wiretapping
* packets accordingly.
* @param dataSet
* @param replyId
* @param forceDirectTm
*/
void handleDeviceTm(const SerializeIF &dataSet, DeviceCommandId_t replyId,
bool forceDirectTm = false);
virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t *msToReachTheMode);

View File

@ -3,10 +3,10 @@
#include "fsfw/serialize/SerializeAdapter.h"
DeviceTmReportingWrapper::DeviceTmReportingWrapper(object_id_t objectId, ActionId_t actionId,
SerializeIF* data)
const SerializeIF& data)
: objectId(objectId), actionId(actionId), data(data) {}
DeviceTmReportingWrapper::~DeviceTmReportingWrapper() {}
DeviceTmReportingWrapper::~DeviceTmReportingWrapper() = default;
ReturnValue_t DeviceTmReportingWrapper::serialize(uint8_t** buffer, size_t* size, size_t maxSize,
Endianness streamEndianness) const {
@ -19,22 +19,14 @@ ReturnValue_t DeviceTmReportingWrapper::serialize(uint8_t** buffer, size_t* size
if (result != returnvalue::OK) {
return result;
}
return data->serialize(buffer, size, maxSize, streamEndianness);
return data.serialize(buffer, size, maxSize, streamEndianness);
}
size_t DeviceTmReportingWrapper::getSerializedSize() const {
return sizeof(objectId) + sizeof(ActionId_t) + data->getSerializedSize();
return sizeof(objectId) + sizeof(ActionId_t) + data.getSerializedSize();
}
ReturnValue_t DeviceTmReportingWrapper::deSerialize(const uint8_t** buffer, size_t* size,
Endianness streamEndianness) {
ReturnValue_t result = SerializeAdapter::deSerialize(&objectId, buffer, size, streamEndianness);
if (result != returnvalue::OK) {
return result;
}
result = SerializeAdapter::deSerialize(&actionId, buffer, size, streamEndianness);
if (result != returnvalue::OK) {
return result;
}
return data->deSerialize(buffer, size, streamEndianness);
return returnvalue::FAILED;
}

View File

@ -7,21 +7,22 @@
class DeviceTmReportingWrapper : public SerializeIF {
public:
DeviceTmReportingWrapper(object_id_t objectId, ActionId_t actionId, SerializeIF* data);
virtual ~DeviceTmReportingWrapper();
DeviceTmReportingWrapper(object_id_t objectId, ActionId_t actionId, const SerializeIF& data);
~DeviceTmReportingWrapper() override;
virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, size_t maxSize,
Endianness streamEndianness) const override;
ReturnValue_t serialize(uint8_t** buffer, size_t* size, size_t maxSize,
Endianness streamEndianness) const override;
virtual size_t getSerializedSize() const override;
virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
Endianness streamEndianness) override;
[[nodiscard]] size_t getSerializedSize() const override;
private:
object_id_t objectId;
ActionId_t actionId;
SerializeIF* data;
const SerializeIF& data;
// Deserialization forbidden
ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
Endianness streamEndianness) override;
};
#endif /* FSFW_DEVICEHANDLERS_DEVICETMREPORTINGWRAPPER_H_ */

View File

@ -161,7 +161,7 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) {
while (true) {
ssize_t retval = recv(connSocket, reinterpret_cast<char*>(receptionBuffer.data()),
receptionBuffer.capacity(), tcpConfig.tcpFlags);
receptionBuffer.size(), tcpConfig.tcpFlags);
if (retval == 0) {
size_t availableReadData = ringBuffer.getAvailableReadData();
if (availableReadData > lastRingBufferSize) {
@ -335,31 +335,27 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) {
}
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) {
if (spacePacketParser == nullptr) {
return returnvalue::FAILED;
}
result =
spacePacketParser->parseSpacePackets(bufPtrPtr, readAmount, startIdx, foundSize, readLen);
SpacePacketParser::FoundPacketInfo info;
if (spacePacketParser == nullptr) {
return returnvalue::FAILED;
}
spacePacketParser->reset();
while (spacePacketParser->getAmountRead() < readAmount) {
result = spacePacketParser->parseSpacePackets(&bufPtr, readAmount, info);
switch (result) {
case (SpacePacketParser::NO_PACKET_FOUND):
case (SpacePacketParser::SPLIT_PACKET): {
break;
}
case (returnvalue::OK): {
result = handleTcReception(receptionBuffer.data() + startIdx, foundSize);
result = handleTcReception(receptionBuffer.data() + info.startIdx, info.sizeFound);
if (result != returnvalue::OK) {
status = result;
}
}
}
ringBuffer.deleteData(foundSize);
ringBuffer.deleteData(info.sizeFound);
lastRingBufferSize = ringBuffer.getAvailableReadData();
std::memset(receptionBuffer.data() + startIdx, 0, foundSize);
}
return status;
}

View File

@ -38,8 +38,9 @@ class Service11TelecommandScheduling final : public PusServiceBase {
static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_11;
static constexpr ReturnValue_t INVALID_TYPE_TIME_WINDOW = returnvalue::makeCode(CLASS_ID, 1);
static constexpr ReturnValue_t TIMESHIFTING_NOT_POSSIBLE = returnvalue::makeCode(CLASS_ID, 2);
static constexpr ReturnValue_t INVALID_RELATIVE_TIME = returnvalue::makeCode(CLASS_ID, 3);
static constexpr ReturnValue_t INVALID_TIME_WINDOW = returnvalue::makeCode(CLASS_ID, 2);
static constexpr ReturnValue_t TIMESHIFTING_NOT_POSSIBLE = returnvalue::makeCode(CLASS_ID, 3);
static constexpr ReturnValue_t INVALID_RELATIVE_TIME = returnvalue::makeCode(CLASS_ID, 4);
static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_11;

View File

@ -564,12 +564,17 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::getMapFilterFr
if (result != returnvalue::OK) {
return result;
}
if(fromTimestamp > toTimestamp) {
return INVALID_TIME_WINDOW;
}
itBegin = telecommandMap.begin();
itEnd = telecommandMap.begin();
while (itBegin->first < fromTimestamp && itBegin != telecommandMap.end()) {
itBegin++;
}
//start looking for end beginning at begin
itEnd = itBegin;
while (itEnd->first <= toTimestamp && itEnd != telecommandMap.end()) {
itEnd++;
}
@ -579,17 +584,6 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::getMapFilterFr
default:
return returnvalue::FAILED;
}
// additional security check, this should never be true
if (itBegin > itEnd) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "11::getMapFilterFromData: itBegin > itEnd\n" << std::endl;
#else
sif::printError("11::getMapFilterFromData: itBegin > itEnd\n");
#endif
return returnvalue::FAILED;
}
// the map range should now be set according to the sent filter.
return returnvalue::OK;
}

View File

@ -6,17 +6,9 @@
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) {
FoundPacketInfo& packetInfo) {
if (buffer == nullptr or nextStartIdx > maxSize) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::warning << "SpacePacketParser::parseSpacePackets: Frame invalid" << std::endl;
#else
@ -26,35 +18,32 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const
}
const uint8_t* bufPtr = *buffer;
auto verifyLengthField = [&](size_t idx) {
uint16_t lengthField = bufPtr[idx + 4] << 8 | bufPtr[idx + 5];
auto verifyLengthField = [&](size_t localIdx) {
uint16_t lengthField = (bufPtr[localIdx + 4] << 8) | bufPtr[localIdx + 5];
size_t packetSize = lengthField + 7;
startIndex = idx;
ReturnValue_t result = returnvalue::OK;
if (lengthField == 0) {
// Skip whole header for now
foundSize = 6;
result = NO_PACKET_FOUND;
} else if (packetSize + idx > maxSize) {
if (packetSize + localIdx + amountRead > maxSize) {
// Don't increment buffer and read length here, user has to decide what to do
foundSize = packetSize;
packetInfo.sizeFound = packetSize;
return SPLIT_PACKET;
} else {
foundSize = packetSize;
packetInfo.sizeFound = packetSize;
}
*buffer += foundSize;
readLen += idx + foundSize;
*buffer += packetInfo.sizeFound;
packetInfo.startIdx = localIdx + amountRead;
nextStartIdx = localIdx + amountRead + packetInfo.sizeFound;
amountRead = nextStartIdx;
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];
while (idx + amountRead < 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) {
if (idx + amountRead >= maxSize - 5) {
return SPLIT_PACKET;
}
return verifyLengthField(idx);
@ -62,10 +51,10 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const
idx++;
}
}
startIndex = 0;
foundSize = maxSize;
*buffer += foundSize;
readLen += foundSize;
nextStartIdx = maxSize;
packetInfo.sizeFound = maxSize;
amountRead = maxSize;
*buffer += maxSize;
return NO_PACKET_FOUND;
}
// Assume that the user verified a valid start of a space packet

View File

@ -17,9 +17,11 @@
*/
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>;
struct FoundPacketInfo {
size_t startIdx = 0;
size_t sizeFound = 0;
};
static constexpr uint8_t INTERFACE_ID = CLASS_ID::SPACE_PACKET_PARSER;
static constexpr ReturnValue_t NO_PACKET_FOUND = MAKE_RETURN_CODE(0x00);
@ -36,44 +38,32 @@ class SpacePacketParser {
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
* Parse a given frame for space packets but also increments the given buffer.
* @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 returnvalue::OK if a packet was found
* @param packetInfo Information about packets found.
* -@c NO_PACKET_FOUND if no packet was found in the given buffer
* -@c SPLIT_PACKET if a packet was found but the detected size exceeds maxSize. packetInfo
* will contain the detected packet size and start index.
* -@c returnvalue::OK if a packet was found. Packet size and start index will be set in
* packetInfo
*/
ReturnValue_t parseSpacePackets(const uint8_t** buffer, const size_t maxSize, size_t& startIndex,
size_t& foundSize, size_t& readLen);
ReturnValue_t parseSpacePackets(const uint8_t** buffer, const size_t maxSize,
FoundPacketInfo& packetInfo);
/**
* 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 returnvalue::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 getAmountRead() {
return amountRead;
}
void reset() {
nextStartIdx = 0;
amountRead = 0;
}
private:
std::vector<uint16_t> validPacketIds;
size_t nextStartIdx = 0;
size_t amountRead = 0;
};
#endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */

View File

@ -18,6 +18,7 @@ add_subdirectory(power)
add_subdirectory(util)
add_subdirectory(container)
add_subdirectory(osal)
add_subdirectory(pus)
add_subdirectory(serialize)
add_subdirectory(datapoollocal)
add_subdirectory(storagemanager)

View File

@ -0,0 +1,3 @@
target_sources(${FSFW_TEST_TGT} PRIVATE
testService11.cpp
)

View File

@ -0,0 +1,14 @@
#include <fsfw/pus/Service11TelecommandScheduling.h>
#include <catch2/catch_test_macros.hpp>
#include "objects/systemObjectList.h"
#include "tmtc/apid.h"
#include "tmtc/pusIds.h"
TEST_CASE("PUS Service 11", "[pus-srvc11]") {
Service11TelecommandScheduling<13> pusService11(objects::PUS_SERVICE_11_TC_SCHEDULER,
apid::DEFAULT_APID, pus::PUS_SERVICE_11, nullptr);
// TODO test something...
}