diff --git a/src/fsfw/cfdp/CMakeLists.txt b/src/fsfw/cfdp/CMakeLists.txt index 2d58e190..9bd54fc8 100644 --- a/src/fsfw/cfdp/CMakeLists.txt +++ b/src/fsfw/cfdp/CMakeLists.txt @@ -1,4 +1,4 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE CfdpMessage.cpp CfdpRouter.cpp) +target_sources(${LIB_FSFW_NAME} PRIVATE CfdpMessage.cpp CfdpDistributor.cpp) # CfdpDistributor.cpp CfdpHandler.cpp add_subdirectory(pdu) diff --git a/src/fsfw/cfdp/CfdpDistributor.cpp b/src/fsfw/cfdp/CfdpDistributor.cpp index fa221822..14e0180c 100644 --- a/src/fsfw/cfdp/CfdpDistributor.cpp +++ b/src/fsfw/cfdp/CfdpDistributor.cpp @@ -1,144 +1,53 @@ -//#include "CfdpRouter.h" -//#include "fsfw/tcdistribution/CcsdsDistributorIF.h" -//#include "fsfw/tmtcpacket/cfdp/CfdpPacketStored.h" -// -//#ifndef FSFW_CFDP_DISTRIBUTOR_DEBUGGING -//#define FSFW_CFDP_DISTRIBUTOR_DEBUGGING 1 -//#endif -// -// CfdpHandler::CfdpHandler(uint16_t setApid, object_id_t setObjectId, -// object_id_t setPacketSource) -// : TcDistributorBase(setObjectId), -// apid(setApid), -// checker(setApid), -// tcStatus(RETURN_FAILED), -// packetSource(setPacketSource) {} -// -// CfdpHandler::~CfdpHandler() = default; -// -// ReturnValue_t CfdpHandler::selectDestination(MessageQueueId_t& destId) { -// //#if FSFW_CFDP_DISTRIBUTOR_DEBUGGING == 1 -// // store_address_t storeId = this->currentMessage.getStorageId(); -// //#if FSFW_CPP_OSTREAM_ENABLED == 1 -// // sif::debug << "CFDPDistributor::handlePacket received: " << storeId.poolIndex << ", " -// // << storeId.packetIndex << std::endl; -// //#else -// // sif::printDebug("CFDPDistributor::handlePacket received: %d, %d\n", storeId.poolIndex, -// // storeId.packetIndex); -// //#endif -// //#endif -// // auto queueMapIt = this->queueMap.end(); -// // if (this->currentPacket == nullptr) { -// // return queueMapIt; -// // } -// // this->currentPacket->setStoreAddress(this->currentMessage.getStorageId()); -// // if (currentPacket->getFullData() != nullptr) { -// // tcStatus = checker.checkPacket(*currentPacket, currentPacket->getFullPacketLen()); -// // if (tcStatus != HasReturnvaluesIF::RETURN_OK) { -// //#if FSFW_VERBOSE_LEVEL >= 1 -// //#if FSFW_CPP_OSTREAM_ENABLED == 1 -// // sif::debug << "CFDPDistributor::handlePacket: Packet format invalid, code " -// // << static_cast(tcStatus) << std::endl; -// //#else -// // sif::printDebug("CFDPDistributor::handlePacket: Packet format invalid, code %d\n", -// // static_cast(tcStatus)); -// //#endif -// //#endif -// // } -// // queueMapIt = this->queueMap.find(0); -// // } else { -// // tcStatus = PACKET_LOST; -// // } -// // -// // if (queueMapIt == this->queueMap.end()) { -// // tcStatus = DESTINATION_NOT_FOUND; -// //#if FSFW_VERBOSE_LEVEL >= 1 -// //#if FSFW_CPP_OSTREAM_ENABLED == 1 -// // sif::debug << "CFDPDistributor::handlePacket: Destination not found" << std::endl; -// //#else -// // sif::printDebug("CFDPDistributor::handlePacket: Destination not found\n"); -// //#endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */ -// //#endif -// // } -// // -// // if (tcStatus != RETURN_OK) { -// // return this->queueMap.end(); -// // } else { -// // return queueMapIt; -// // } -// return HasReturnvaluesIF::RETURN_OK; -//} -// -// ReturnValue_t CfdpHandler::registerHandler(AcceptsTelecommandsIF* handler) { -// // uint16_t handlerId = -// // handler->getIdentifier(); // should be 0, because CfdpHandler does not set a set a -// // service-ID -// //#if FSFW_CFDP_DISTRIBUTOR_DEBUGGING == 1 -// //#if FSFW_CPP_OSTREAM_ENABLED == 1 -// // sif::info << "CFDPDistributor::registerHandler: Handler ID: " << static_cast(handlerId) -// // << std::endl; -// //#else -// // sif::printInfo("CFDPDistributor::registerHandler: Handler ID: %d\n", -// // static_cast(handlerId)); -// //#endif -// //#endif -// // MessageQueueId_t queue = handler->getRequestQueue(); -// // auto returnPair = queueMap.emplace(handlerId, queue); -// // if (not returnPair.second) { -// //#if FSFW_VERBOSE_LEVEL >= 1 -// //#if FSFW_CPP_OSTREAM_ENABLED == 1 -// // sif::error << "CFDPDistributor::registerHandler: Service ID already" -// // " exists in map" -// // << std::endl; -// //#else -// // sif::printError("CFDPDistributor::registerHandler: Service ID already exists in map\n"); -// //#endif -// //#endif -// // return SERVICE_ID_ALREADY_EXISTS; -// // } -// return HasReturnvaluesIF::RETURN_OK; -//} -// -// MessageQueueId_t CfdpHandler::getRequestQueue() const { return tcQueue->getId(); } -// -//// ReturnValue_t CFDPDistributor::callbackAfterSending(ReturnValue_t queueStatus) { -//// if (queueStatus != RETURN_OK) { -//// tcStatus = queueStatus; -//// } -//// if (tcStatus != RETURN_OK) { -//// this->verifyChannel.sendFailureReport(tc_verification::ACCEPTANCE_FAILURE, -//// currentPacket, tcStatus); -//// // A failed packet is deleted immediately after reporting, -//// // otherwise it will block memory. -//// currentPacket->deletePacket(); -//// return RETURN_FAILED; -//// } else { -//// this->verifyChannel.sendSuccessReport(tc_verification::ACCEPTANCE_SUCCESS, -//// currentPacket); -//// return RETURN_OK; -//// } -//// } -// -// uint32_t CfdpHandler::getIdentifier() const { return this->apid; } -// -// ReturnValue_t CfdpHandler::initialize() { -// // currentPacket = new CfdpPacketStored(); -// // if (currentPacket == nullptr) { -// // // Should not happen, memory allocation failed! -// // return ObjectManagerIF::CHILD_INIT_FAILED; -// // } -// // -// // auto* ccsdsDistributor = ObjectManager::instance()->get(packetSource); -// // if (ccsdsDistributor == nullptr) { -// //#if FSFW_CPP_OSTREAM_ENABLED == 1 -// // sif::error << "CFDPDistributor::initialize: Packet source invalid" << std::endl; -// // sif::error << " Make sure it exists and implements CCSDSDistributorIF!" << std::endl; -// //#else -// // sif::printError("CFDPDistributor::initialize: Packet source invalid\n"); -// // sif::printError("Make sure it exists and implements CcsdsDistributorIF\n"); -// //#endif -// // return RETURN_FAILED; -// // } -// // return ccsdsDistributor->registerApplication(this); -// return HasReturnvaluesIF::RETURN_OK; -//} +#include "CfdpDistributor.h" + +#include "fsfw/tcdistribution/definitions.h" + +CfdpDistributor::CfdpDistributor(CfdpRouterCfg cfg) : TcDistributorBase(cfg.objectId), cfg(cfg) {} + +ReturnValue_t CfdpDistributor::registerTcDestination(const cfdp::EntityId& address, + AcceptsTelecommandsIF& tcDest) { + for (const auto& dest : tcDestinations) { + if (dest.id == address) { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + tcDestinations.emplace_back(address, tcDest.getName(), tcDest.getRequestQueue()); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t CfdpDistributor::selectDestination(MessageQueueId_t& destId) { + auto accessorPair = cfg.tcStore.getData(currentMessage.getStorageId()); + if (accessorPair.first != HasReturnvaluesIF::RETURN_OK) { + return accessorPair.first; + } + ReturnValue_t result = pduReader.setData(accessorPair.second.data(), accessorPair.second.size()); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = pduReader.parseData(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + cfdp::EntityId foundId; + pduReader.getDestId(foundId); + bool destFound = false; + for (const auto& dest : tcDestinations) { + if (dest.id == foundId) { + destId = dest.queueId; + destFound = true; + } + } + if (not destFound) { + // TODO: Warning and event? + return HasReturnvaluesIF::RETURN_FAILED; + } + // Packet was forwarded successfully, so do not delete it. + accessorPair.second.release(); + return HasReturnvaluesIF::RETURN_OK; +} + +const char* CfdpDistributor::getName() const { return "CFDP Router"; } + +uint32_t CfdpDistributor::getIdentifier() const { return 0; } + +MessageQueueId_t CfdpDistributor::getRequestQueue() const { return tcQueue->getId(); } diff --git a/src/fsfw/cfdp/CfdpRouter.h b/src/fsfw/cfdp/CfdpDistributor.h similarity index 70% rename from src/fsfw/cfdp/CfdpRouter.h rename to src/fsfw/cfdp/CfdpDistributor.h index 178f0177..cfb31f2b 100644 --- a/src/fsfw/cfdp/CfdpRouter.h +++ b/src/fsfw/cfdp/CfdpDistributor.h @@ -4,7 +4,7 @@ #include #include -#include "CfdpRouterIF.h" +#include "fsfw/cfdp/pdu/HeaderReader.h" #include "fsfw/returnvalues/HasReturnvaluesIF.h" #include "fsfw/tcdistribution/CfdpPacketChecker.h" #include "fsfw/tcdistribution/TcDistributorBase.h" @@ -13,22 +13,23 @@ #include "fsfw/tmtcservices/VerificationReporter.h" struct CfdpRouterCfg { - CfdpRouterCfg(object_id_t objectId, MessageQueueIF& tmQueue) - : objectId(objectId), tmQueue(tmQueue) {} + CfdpRouterCfg(object_id_t objectId, MessageQueueIF& tmQueue, StorageManagerIF& tcStore, + StorageManagerIF& tmStore) + : objectId(objectId), tmQueue(tmQueue), tcStore(tcStore), tmStore(tmStore) {} object_id_t objectId; MessageQueueIF& tmQueue; + StorageManagerIF& tcStore; + StorageManagerIF& tmStore; }; + /** * This will be the primary component to perform PDU forwading procedures. This includes forwarding * CFDP TC packets to registered source or destination handlers, and forwarding all telemetry * generated by them to registered TM sinks. * @ingroup tc_distribution */ -class CfdpRouter : public TcDistributorBase, - public CfdpRouterIF, - public AcceptsTelecommandsIF, - public AcceptsTelemetryIF { +class CfdpDistributor : public TcDistributorBase, public AcceptsTelecommandsIF { public: /** * The ctor passes @c set_apid to the checker class and calls the @@ -38,17 +39,12 @@ class CfdpRouter : public TcDistributorBase, * @param setPacketSource Object ID of the source of TC packets. * Must implement CcsdsDistributorIF. */ - explicit CfdpRouter(CfdpRouterCfg cfg); + explicit CfdpDistributor(CfdpRouterCfg cfg); - ReturnValue_t performOperation(uint8_t opCode) override; - ReturnValue_t performTmHandling(); [[nodiscard]] const char* getName() const override; [[nodiscard]] uint32_t getIdentifier() const override; [[nodiscard]] MessageQueueId_t getRequestQueue() const override; - MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) override; - ReturnValue_t registerTmSink(cfdp::EntityId address, AcceptsTelemetryIF& tmDest) override; - ReturnValue_t registerTcDestination(cfdp::EntityId address, - AcceptsTelecommandsIF& tcDest) override; + ReturnValue_t registerTcDestination(const cfdp::EntityId& address, AcceptsTelecommandsIF& tcDest); protected: struct EntityInfo { @@ -58,13 +54,13 @@ class CfdpRouter : public TcDistributorBase, const char* name; MessageQueueId_t queueId; }; + HeaderReader pduReader; ReturnValue_t lastTcError = HasReturnvaluesIF::RETURN_OK; ReturnValue_t lastTmError = HasReturnvaluesIF::RETURN_OK; // I don't think a regular OBSW will have more than 1 or 2 of these destinations, so I think // it is okay to accept the overhead here - std::vector tmDestinations; std::vector tcDestinations; - MessageQueueIF& tmQueue; + CfdpRouterCfg cfg; ReturnValue_t selectDestination(MessageQueueId_t& destId) override; diff --git a/src/fsfw/cfdp/CfdpRouter.cpp b/src/fsfw/cfdp/CfdpRouter.cpp deleted file mode 100644 index dfe910e6..00000000 --- a/src/fsfw/cfdp/CfdpRouter.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include "CfdpRouter.h" - -#include "fsfw/tcdistribution/definitions.h" - -CfdpRouter::CfdpRouter(CfdpRouterCfg cfg) : TcDistributorBase(cfg.objectId), tmQueue(cfg.tmQueue) {} - -ReturnValue_t CfdpRouter::registerTmSink(cfdp::EntityId address, AcceptsTelemetryIF& tmDest) { - for (const auto& dest : tmDestinations) { - if (dest.id == address) { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - tmDestinations.emplace_back(address, tmDest.getName(), tmDest.getReportReceptionQueue()); - return HasReturnvaluesIF::RETURN_OK; -} -ReturnValue_t CfdpRouter::registerTcDestination(cfdp::EntityId address, - AcceptsTelecommandsIF& tcDest) { - for (const auto& dest : tcDestinations) { - if (dest.id == address) { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - tcDestinations.emplace_back(address, tcDest.getName(), tcDest.getRequestQueue()); - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t CfdpRouter::selectDestination(MessageQueueId_t& destId) { return 0; } - -const char* CfdpRouter::getName() const { return "CFDP Router"; } - -uint32_t CfdpRouter::getIdentifier() const { return 0; } - -MessageQueueId_t CfdpRouter::getRequestQueue() const { return tcQueue->getId(); } - -MessageQueueId_t CfdpRouter::getReportReceptionQueue(uint8_t virtualChannel) { - return tmQueue.getId(); -} - -ReturnValue_t CfdpRouter::performOperation(uint8_t opCode) { - lastTcError = TcDistributorBase::performOperation(opCode); - lastTmError = performTmHandling(); - if (lastTcError != HasReturnvaluesIF::RETURN_OK or lastTmError != HasReturnvaluesIF::RETURN_OK) { - return HasReturnvaluesIF::RETURN_FAILED; - } - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t CfdpRouter::performTmHandling() { - ReturnValue_t status; - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; - for (status = tmQueue.receiveMessage(¤tMessage); status == RETURN_OK; - status = tmQueue.receiveMessage(¤tMessage)) { - ReturnValue_t packetResult = handlePacket(); - if (packetResult != HasReturnvaluesIF::RETURN_OK) { - result = packetResult; - triggerEvent(tmtcdistrib::HANDLE_PACKET_FAILED, packetResult, ccsds::PacketType::TM); - } - } - if (status == MessageQueueIF::EMPTY) { - return result; - } - return result; -} diff --git a/src/fsfw/cfdp/CfdpRouterIF.h b/src/fsfw/cfdp/CfdpRouterIF.h deleted file mode 100644 index 0c630407..00000000 --- a/src/fsfw/cfdp/CfdpRouterIF.h +++ /dev/null @@ -1,32 +0,0 @@ -#ifndef FSFW_TCDISTRIBUTION_CFDPDISTRIBUTORIF_H_ -#define FSFW_TCDISTRIBUTION_CFDPDISTRIBUTORIF_H_ - -#include "fsfw/cfdp/definitions.h" -#include "fsfw/cfdp/pdu/PduHeaderIF.h" -#include "fsfw/ipc/MessageQueueSenderIF.h" -#include "fsfw/tmtcservices/AcceptsTelecommandsIF.h" -#include "fsfw/tmtcservices/AcceptsTelemetryIF.h" - -/** - * This interface allows CFDP Services to register themselves at a CFDP Distributor. - * @ingroup tc_distribution - */ -class CfdpRouterIF { - public: - /** - * The empty virtual destructor. - */ - virtual ~CfdpRouterIF() = default; - - virtual ReturnValue_t registerTmSink(cfdp::EntityId address, AcceptsTelemetryIF& tmDest) = 0; - /** - * With this method, Handlers can register themselves at the CFDP Distributor. - * @param handler A pointer to the registering Handler. - * @return - @c RETURN_OK on success, - * - @c RETURN_FAILED on failure. - */ - virtual ReturnValue_t registerTcDestination(cfdp::EntityId address, - AcceptsTelecommandsIF& tcDest) = 0; -}; - -#endif /* FSFW_TCDISTRIBUTION_CFDPDISTRIBUTORIF_H_ */ diff --git a/src/fsfw/cfdp/pdu/HeaderReader.h b/src/fsfw/cfdp/pdu/HeaderReader.h index 1543a71b..67a0470a 100644 --- a/src/fsfw/cfdp/pdu/HeaderReader.h +++ b/src/fsfw/cfdp/pdu/HeaderReader.h @@ -25,6 +25,7 @@ struct PduHeaderFixedStruct { */ class HeaderReader : public RedirectableDataPointerIF, public PduHeaderIF { public: + HeaderReader() = default; /** * Initialize a PDU header from raw data. This is a zero-copy implementation and #parseData * needs to be called to ensure the data is valid