From 64a7fde30171bfd4711fb01b349184f9777817cf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 15:16:54 +0200 Subject: [PATCH 01/10] this is annoying --- src/fsfw/pus/Service11TelecommandScheduling.h | 5 +++-- src/fsfw/pus/Service11TelecommandScheduling.tpp | 14 +++----------- 2 files changed, 6 insertions(+), 13 deletions(-) diff --git a/src/fsfw/pus/Service11TelecommandScheduling.h b/src/fsfw/pus/Service11TelecommandScheduling.h index 2a14401b..57ba4982 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.h +++ b/src/fsfw/pus/Service11TelecommandScheduling.h @@ -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; diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp index 7f60f7e8..3cf24b11 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.tpp +++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp @@ -571,6 +571,9 @@ inline ReturnValue_t Service11TelecommandScheduling::getMapFilterFr if (result != returnvalue::OK) { return result; } + if(fromTimestamp > toTimestamp) { + return INVALID_TIME_WINDOW; + } itBegin = telecommandMap.begin(); itEnd = telecommandMap.begin(); @@ -586,17 +589,6 @@ inline ReturnValue_t Service11TelecommandScheduling::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; } From ab719a3e59f25571fba34e28c14d4a8a613394e9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 23:38:55 +0200 Subject: [PATCH 02/10] alternative solution --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 37 +++++++++---------- src/fsfw/devicehandlers/DeviceHandlerBase.h | 6 +-- 2 files changed, 20 insertions(+), 23 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 525a3dcc..56d99b71 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -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,30 +1258,32 @@ 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(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, &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); + DeviceTmReportingWrapper wrapper(getObjectId(), replyId, &dataSet); actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); } @@ -1289,22 +1292,16 @@ 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, &dataSet, true); } } - /* Unrequested or aperiodic replies */ + // Unrequested or aperiodic replies else { - DeviceTmReportingWrapper wrapper(getObjectId(), replyId, dataSet); + 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 diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index a20eae0c..f98d70f3 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -1052,9 +1052,9 @@ 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); + void handleDeviceTm(const uint8_t *rawData, size_t rawDataLen, DeviceCommandId_t replyId, + bool forceDirectTm = false); + void handleDeviceTm(SerializeIF &dataSet, DeviceCommandId_t replyId, bool forceDirectTm = false); virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode); From 158007fa7f9cc0b8cf63acecba4d3ca8b5284557 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 Aug 2022 00:02:25 +0200 Subject: [PATCH 03/10] const correct API --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 11 ++++++----- src/fsfw/devicehandlers/DeviceHandlerBase.h | 3 ++- .../DeviceTmReportingWrapper.cpp | 18 +++++------------- .../devicehandlers/DeviceTmReportingWrapper.h | 19 ++++++++++--------- 4 files changed, 23 insertions(+), 28 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 56d99b71..b0d14380 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1264,7 +1264,7 @@ void DeviceHandlerBase::handleDeviceTm(const uint8_t* rawData, size_t rawDataLen handleDeviceTm(bufferWrapper, replyId, forceDirectTm); } -void DeviceHandlerBase::handleDeviceTm(SerializeIF& dataSet, DeviceCommandId_t replyId, +void DeviceHandlerBase::handleDeviceTm(const SerializeIF& dataSet, DeviceCommandId_t replyId, bool forceDirectTm) { auto iter = deviceReplyMap.find(replyId); if (iter == deviceReplyMap.end()) { @@ -1278,12 +1278,12 @@ void DeviceHandlerBase::handleDeviceTm(SerializeIF& dataSet, DeviceCommandId_t r if (queueId != NO_COMMANDER) { // This may fail, but we'll ignore the fault. - actionHelper.reportData(queueId, replyId, &dataSet); + actionHelper.reportData(queueId, replyId, const_cast(&dataSet)); } // 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); + DeviceTmReportingWrapper wrapper(getObjectId(), replyId, dataSet); actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); } @@ -1292,12 +1292,13 @@ 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(&dataSet), + true); } } // Unrequested or aperiodic replies else { - DeviceTmReportingWrapper wrapper(getObjectId(), replyId, &dataSet); + DeviceTmReportingWrapper wrapper(getObjectId(), replyId, dataSet); if (wiretappingMode == TM) { actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); } diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index f98d70f3..ff1c7dc1 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -1054,7 +1054,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, void handleDeviceTm(const uint8_t *rawData, size_t rawDataLen, DeviceCommandId_t replyId, bool forceDirectTm = false); - void handleDeviceTm(SerializeIF &dataSet, DeviceCommandId_t replyId, bool forceDirectTm = false); + void handleDeviceTm(const SerializeIF &dataSet, DeviceCommandId_t replyId, + bool forceDirectTm = false); virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode); diff --git a/src/fsfw/devicehandlers/DeviceTmReportingWrapper.cpp b/src/fsfw/devicehandlers/DeviceTmReportingWrapper.cpp index dc987e6d..8fc0d368 100644 --- a/src/fsfw/devicehandlers/DeviceTmReportingWrapper.cpp +++ b/src/fsfw/devicehandlers/DeviceTmReportingWrapper.cpp @@ -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; } diff --git a/src/fsfw/devicehandlers/DeviceTmReportingWrapper.h b/src/fsfw/devicehandlers/DeviceTmReportingWrapper.h index 71c64453..ae385f4c 100644 --- a/src/fsfw/devicehandlers/DeviceTmReportingWrapper.h +++ b/src/fsfw/devicehandlers/DeviceTmReportingWrapper.h @@ -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_ */ From 7345c18b0439e27169a02e4a1ce98f6837175635 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 Aug 2022 16:26:18 +0200 Subject: [PATCH 04/10] small fix which allows sending action reply immediately --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 525a3dcc..c4ce7781 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1320,18 +1320,22 @@ ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, MessageQueue return result; } DeviceCommandMap::iterator iter = deviceCommandMap.find(actionId); + MessageQueueId_t prevRecipient = MessageQueueIF::NO_QUEUE; if (iter == deviceCommandMap.end()) { result = COMMAND_NOT_SUPPORTED; } else if (iter->second.isExecuting) { result = COMMAND_ALREADY_SENT; } else { + prevRecipient = iter->second.sendReplyTo; + 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; + } else { + iter->second.sendReplyTo = prevRecipient; } return result; } From 28ecd0e5c6304a91bae9840159d200ed8361e68a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Sep 2022 08:51:12 +0200 Subject: [PATCH 05/10] bugfix for SP parser --- src/fsfw/osal/common/TcpTmTcServer.cpp | 19 ++++----- src/fsfw/tmtcservices/SpacePacketParser.cpp | 45 +++++++++++---------- src/fsfw/tmtcservices/SpacePacketParser.h | 18 +++++++-- 3 files changed, 45 insertions(+), 37 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index f9cb923e..f1174734 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -161,7 +161,7 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { while (true) { ssize_t retval = recv(connSocket, reinterpret_cast(receptionBuffer.data()), - receptionBuffer.capacity(), tcpConfig.tcpFlags); + receptionBuffer.size(), tcpConfig.tcpFlags); if (retval == 0) { size_t availableReadData = ringBuffer.getAvailableReadData(); if (availableReadData > lastRingBufferSize) { @@ -335,31 +335,28 @@ 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) { + FoundPacketInfo info; + ParsingState parseState; + while (parseState.amountRead < readAmount) { if (spacePacketParser == nullptr) { return returnvalue::FAILED; } - result = - spacePacketParser->parseSpacePackets(bufPtrPtr, readAmount, startIdx, foundSize, readLen); + result = spacePacketParser->parseSpacePackets(&bufPtr, readAmount, info, parseState); 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); + // std::memset(receptionBuffer.data() + startIdx, 0, foundSize); } return status; } diff --git a/src/fsfw/tmtcservices/SpacePacketParser.cpp b/src/fsfw/tmtcservices/SpacePacketParser.cpp index b8364138..16ee5dac 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.cpp +++ b/src/fsfw/tmtcservices/SpacePacketParser.cpp @@ -7,16 +7,16 @@ SpacePacketParser::SpacePacketParser(std::vector validPacketIds) : validPacketIds(validPacketIds) {} ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t* buffer, const size_t maxSize, - size_t& startIndex, size_t& foundSize) { + FoundPacketInfo& packetInfo, + ParsingState& parsingState) { const uint8_t** tempPtr = &buffer; - size_t readLen = 0; - return parseSpacePackets(tempPtr, maxSize, startIndex, foundSize, readLen); + return parseSpacePackets(tempPtr, maxSize, packetInfo, parsingState); } 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, + ParsingState& parsingState) { + if (buffer == nullptr or parsingState.nextStartIdx > maxSize) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "SpacePacketParser::parseSpacePackets: Frame invalid" << std::endl; #else @@ -26,35 +26,36 @@ 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; + packetInfo.sizeFound = 6; result = NO_PACKET_FOUND; - } else if (packetSize + idx > maxSize) { + } else if (packetSize + localIdx + parsingState.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 + parsingState.amountRead; + parsingState.nextStartIdx = localIdx + parsingState.amountRead + packetInfo.sizeFound; + parsingState.amountRead = parsingState.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 + parsingState.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 + parsingState.amountRead >= maxSize - 5) { return SPLIT_PACKET; } return verifyLengthField(idx); @@ -62,10 +63,10 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const idx++; } } - startIndex = 0; - foundSize = maxSize; - *buffer += foundSize; - readLen += foundSize; + parsingState.nextStartIdx = maxSize; + packetInfo.sizeFound = maxSize; + parsingState.amountRead = maxSize; + *buffer += maxSize; return NO_PACKET_FOUND; } // Assume that the user verified a valid start of a space packet diff --git a/src/fsfw/tmtcservices/SpacePacketParser.h b/src/fsfw/tmtcservices/SpacePacketParser.h index 93f90afe..394083b2 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.h +++ b/src/fsfw/tmtcservices/SpacePacketParser.h @@ -7,6 +7,16 @@ #include "fsfw/container/DynamicFIFO.h" #include "fsfw/returnvalues/FwClassIds.h" +struct FoundPacketInfo { + size_t startIdx = 0; + size_t sizeFound = 0; +}; + +struct ParsingState { + size_t nextStartIdx = 0; + size_t amountRead = 0; +}; + /** * @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. @@ -53,8 +63,8 @@ class SpacePacketParser { * will be assigned. * -@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& readLen); + ReturnValue_t parseSpacePackets(const uint8_t** buffer, const size_t maxSize, + FoundPacketInfo& packetInfo, ParsingState& parsingState); /** * Parse a given frame for space packets @@ -69,8 +79,8 @@ class SpacePacketParser { * 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); + ReturnValue_t parseSpacePackets(const uint8_t* buffer, const size_t maxSize, + FoundPacketInfo& packetInfo, ParsingState& parsingState); private: std::vector validPacketIds; From 7ae82a5cb4287e70ff13ef8da3f9264096c1879e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Sep 2022 08:58:18 +0200 Subject: [PATCH 06/10] do not cache previous recipient --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index c4ce7781..efb535a5 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1320,13 +1320,11 @@ ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, MessageQueue return result; } DeviceCommandMap::iterator iter = deviceCommandMap.find(actionId); - MessageQueueId_t prevRecipient = MessageQueueIF::NO_QUEUE; if (iter == deviceCommandMap.end()) { result = COMMAND_NOT_SUPPORTED; } else if (iter->second.isExecuting) { result = COMMAND_ALREADY_SENT; } else { - prevRecipient = iter->second.sendReplyTo; iter->second.sendReplyTo = commandedBy; result = buildCommandFromCommand(actionId, data, size); } @@ -1334,8 +1332,6 @@ ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, MessageQueue iter->second.isExecuting = true; cookieInfo.pendingCommand = iter; cookieInfo.state = COOKIE_WRITE_READY; - } else { - iter->second.sendReplyTo = prevRecipient; } return result; } From 7d3223d766bcd2b0a693885f9db899d8bd1a841a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Sep 2022 10:44:57 +0200 Subject: [PATCH 07/10] add comment blocks --- src/fsfw/devicehandlers/DeviceHandlerBase.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index ff1c7dc1..792e70ad 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -1052,8 +1052,23 @@ class DeviceHandlerBase : public DeviceHandlerIF, bool isAwaitingReply(); + /** + * 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 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); From c7f300671fd708cbea51393d24857597f964c24b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Sep 2022 10:45:10 +0200 Subject: [PATCH 08/10] update header --- src/fsfw/devicehandlers/DeviceHandlerBase.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 792e70ad..700e960d 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -1063,7 +1063,7 @@ class DeviceHandlerBase : public DeviceHandlerIF, void handleDeviceTm(const uint8_t *rawData, size_t rawDataLen, DeviceCommandId_t replyId, bool forceDirectTm = false); /** - * Can be used to handle Service 8 replies. This will also generate the TM wiretapping + * Can be used to handle Service 8 data replies. This will also generate the TM wiretapping * packets accordingly. * @param dataSet * @param replyId From 01651f0521d851402426fd6dca4b449e103b3d4d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Sep 2022 10:51:09 +0200 Subject: [PATCH 09/10] more simplfications --- src/fsfw/osal/common/TcpTmTcServer.cpp | 15 +++-- src/fsfw/tmtcservices/SpacePacketParser.cpp | 32 ++++------ src/fsfw/tmtcservices/SpacePacketParser.h | 66 +++++++-------------- 3 files changed, 40 insertions(+), 73 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index f1174734..dff959ba 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -335,13 +335,13 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) { } ringBuffer.readData(receptionBuffer.data(), readAmount, true); const uint8_t* bufPtr = receptionBuffer.data(); - FoundPacketInfo info; - ParsingState parseState; - while (parseState.amountRead < readAmount) { - if (spacePacketParser == nullptr) { - return returnvalue::FAILED; - } - result = spacePacketParser->parseSpacePackets(&bufPtr, readAmount, info, parseState); + 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): { @@ -356,7 +356,6 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) { } ringBuffer.deleteData(info.sizeFound); lastRingBufferSize = ringBuffer.getAvailableReadData(); - // std::memset(receptionBuffer.data() + startIdx, 0, foundSize); } return status; } diff --git a/src/fsfw/tmtcservices/SpacePacketParser.cpp b/src/fsfw/tmtcservices/SpacePacketParser.cpp index 16ee5dac..75db7a4e 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.cpp +++ b/src/fsfw/tmtcservices/SpacePacketParser.cpp @@ -6,17 +6,9 @@ SpacePacketParser::SpacePacketParser(std::vector validPacketIds) : validPacketIds(validPacketIds) {} -ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t* buffer, const size_t maxSize, - FoundPacketInfo& packetInfo, - ParsingState& parsingState) { - const uint8_t** tempPtr = &buffer; - return parseSpacePackets(tempPtr, maxSize, packetInfo, parsingState); -} - ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const size_t maxSize, - FoundPacketInfo& packetInfo, - ParsingState& parsingState) { - if (buffer == nullptr or parsingState.nextStartIdx > maxSize) { + FoundPacketInfo& packetInfo) { + if (buffer == nullptr or nextStartIdx > maxSize) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "SpacePacketParser::parseSpacePackets: Frame invalid" << std::endl; #else @@ -30,11 +22,7 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const uint16_t lengthField = (bufPtr[localIdx + 4] << 8) | bufPtr[localIdx + 5]; size_t packetSize = lengthField + 7; ReturnValue_t result = returnvalue::OK; - if (lengthField == 0) { - // Skip whole header for now - packetInfo.sizeFound = 6; - result = NO_PACKET_FOUND; - } else if (packetSize + localIdx + parsingState.amountRead > maxSize) { + if (packetSize + localIdx + amountRead > maxSize) { // Don't increment buffer and read length here, user has to decide what to do packetInfo.sizeFound = packetSize; return SPLIT_PACKET; @@ -42,20 +30,20 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const packetInfo.sizeFound = packetSize; } *buffer += packetInfo.sizeFound; - packetInfo.startIdx = localIdx + parsingState.amountRead; - parsingState.nextStartIdx = localIdx + parsingState.amountRead + packetInfo.sizeFound; - parsingState.amountRead = parsingState.nextStartIdx; + 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 + parsingState.amountRead < maxSize - 5) { + 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 + parsingState.amountRead >= maxSize - 5) { + if (idx + amountRead >= maxSize - 5) { return SPLIT_PACKET; } return verifyLengthField(idx); @@ -63,9 +51,9 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const idx++; } } - parsingState.nextStartIdx = maxSize; + nextStartIdx = maxSize; packetInfo.sizeFound = maxSize; - parsingState.amountRead = maxSize; + amountRead = maxSize; *buffer += maxSize; return NO_PACKET_FOUND; } diff --git a/src/fsfw/tmtcservices/SpacePacketParser.h b/src/fsfw/tmtcservices/SpacePacketParser.h index 394083b2..3a1b4d16 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.h +++ b/src/fsfw/tmtcservices/SpacePacketParser.h @@ -7,16 +7,6 @@ #include "fsfw/container/DynamicFIFO.h" #include "fsfw/returnvalues/FwClassIds.h" -struct FoundPacketInfo { - size_t startIdx = 0; - size_t sizeFound = 0; -}; - -struct ParsingState { - size_t nextStartIdx = 0; - size_t amountRead = 0; -}; - /** * @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. @@ -27,9 +17,11 @@ struct ParsingState { */ 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; + + 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); @@ -46,44 +38,32 @@ class SpacePacketParser { SpacePacketParser(std::vector 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, - FoundPacketInfo& packetInfo, ParsingState& parsingState); + 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, - FoundPacketInfo& packetInfo, ParsingState& parsingState); + size_t getAmountRead() { + return amountRead; + } + + void reset() { + nextStartIdx = 0; + amountRead = 0; + } private: std::vector validPacketIds; + size_t nextStartIdx = 0; + size_t amountRead = 0; }; #endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */ From 2b01e86f9c7b56d036b7bab0404a10db90bd0d76 Mon Sep 17 00:00:00 2001 From: Ulrich Mohr Date: Thu, 1 Sep 2022 11:56:07 +0200 Subject: [PATCH 10/10] one small additional precaution --- src/fsfw/pus/Service11TelecommandScheduling.tpp | 4 +++- unittests/CMakeLists.txt | 1 + unittests/pus/CMakeLists.txt | 3 +++ unittests/pus/testService11.cpp | 14 ++++++++++++++ 4 files changed, 21 insertions(+), 1 deletion(-) create mode 100644 unittests/pus/CMakeLists.txt create mode 100644 unittests/pus/testService11.cpp diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp index 3cf24b11..c82c1703 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.tpp +++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp @@ -575,11 +575,13 @@ inline ReturnValue_t Service11TelecommandScheduling::getMapFilterFr 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++; } diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 8c7f2463..d7c35975 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -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) diff --git a/unittests/pus/CMakeLists.txt b/unittests/pus/CMakeLists.txt new file mode 100644 index 00000000..ecd7fb49 --- /dev/null +++ b/unittests/pus/CMakeLists.txt @@ -0,0 +1,3 @@ +target_sources(${FSFW_TEST_TGT} PRIVATE + testService11.cpp +) diff --git a/unittests/pus/testService11.cpp b/unittests/pus/testService11.cpp new file mode 100644 index 00000000..72708f09 --- /dev/null +++ b/unittests/pus/testService11.cpp @@ -0,0 +1,14 @@ +#include + +#include + +#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... +} \ No newline at end of file