From 50a4ec97efc829cc4a3bbcca3d3d7a7bd8a32a23 Mon Sep 17 00:00:00 2001 From: Ulrich Mohr Date: Wed, 5 Jul 2023 23:47:07 +0200 Subject: [PATCH] working on tm --- src/fsfw/action/ActionHelper.cpp | 75 +++++++--- src/fsfw/action/ActionHelper.h | 6 +- src/fsfw/ipc/FwMessageTypes.h | 1 + src/fsfw/objectmanager/frameworkObjects.h | 1 + src/fsfw/tmtc/AcceptsTelemetryIF.h | 4 +- src/fsfw/tmtc/CMakeLists.txt | 1 + src/fsfw/tmtc/TmManager.cpp | 4 +- src/fsfw/tmtc/TmManager.h | 2 +- src/fsfw/tmtc/TmMessage.cpp | 15 ++ src/fsfw/tmtc/TmMessage.h | 20 +++ src/fsfw/tmtc/UdpTmTcBridge.cpp | 168 +++++++++++++++++----- src/fsfw/tmtc/UdpTmTcBridge.h | 39 +++-- 12 files changed, 256 insertions(+), 80 deletions(-) create mode 100644 src/fsfw/tmtc/TmMessage.cpp create mode 100644 src/fsfw/tmtc/TmMessage.h diff --git a/src/fsfw/action/ActionHelper.cpp b/src/fsfw/action/ActionHelper.cpp index cd1196346..e61bc7773 100644 --- a/src/fsfw/action/ActionHelper.cpp +++ b/src/fsfw/action/ActionHelper.cpp @@ -1,8 +1,10 @@ +#include +#include + #include "fsfw/action.h" #include "fsfw/ipc/MessageQueueSenderIF.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" -#include ActionHelper::ActionHelper(HasActionsIF* setOwner, MessageQueueIF* useThisQueue) : owner(setOwner), queueToUse(useThisQueue) {} @@ -11,7 +13,8 @@ ActionHelper::~ActionHelper() = default; ReturnValue_t ActionHelper::handleActionMessage(CommandMessage* command) { if (command->getCommand() == ActionMessage::EXECUTE_ACTION) { - prepareExecution(command->getSender(), ActionMessage::getOffset(command), ActionMessage::getStoreId(command)); + prepareExecution(command->getSender(), ActionMessage::getOffset(command), + ActionMessage::getStoreId(command)); return returnvalue::OK; } else { return CommandMessage::UNKNOWN_COMMAND; @@ -23,6 +26,12 @@ ReturnValue_t ActionHelper::initialize(MessageQueueIF* queueToUse_) { if (ipcStore == nullptr) { return returnvalue::FAILED; } + + tmManager = ObjectManager::instance()->get(objects::TM_MANAGER); + if (tmManager == nullptr) { + return returnvalue::FAILED; + } + if (queueToUse_ != nullptr) { setQueueToUse(queueToUse_); } @@ -57,68 +66,90 @@ void ActionHelper::finish(bool success, MessageQueueId_t reportTo, ActionId_t co void ActionHelper::setQueueToUse(MessageQueueIF* queue) { queueToUse = queue; } -MessageQueueIF const * ActionHelper::getQueue() const { - return queueToUse; -} +MessageQueueIF const* ActionHelper::getQueue() const { return queueToUse; } void ActionHelper::prepareExecution(MessageQueueId_t commandedBy, size_t offset, store_address_t dataAddress) { - const uint8_t* dataPtr = nullptr; - size_t size = 0; - ReturnValue_t result = ipcStore->getData(dataAddress, &dataPtr, &size); + const uint8_t* tcData = nullptr; + size_t tcDataSize = 0; + ReturnValue_t result = ipcStore->getData(dataAddress, &tcData, &tcDataSize); if (result != returnvalue::OK) { CommandMessage reply; ActionMessage::setStepReply(&reply, 0 /*TODO*/, 0, result); - queueToUse->sendMessage(commandedBy, &reply); + // queueToUse->sendMessage(commandedBy, &reply); ipcStore->deleteData(dataAddress); return; } - dataPtr += offset; - size -=offset; + const uint8_t* dataPtr = tcData + offset; + size_t size = tcDataSize - offset; ActionId_t actionId; - result = SerializeAdapter::deSerialize(&actionId, &dataPtr, &size, SerializeIF::Endianness::NETWORK); + result = + SerializeAdapter::deSerialize(&actionId, &dataPtr, &size, SerializeIF::Endianness::NETWORK); if (result != returnvalue::OK) { CommandMessage reply; ActionMessage::setStepReply(&reply, 0 /*TODO*/, 0, result); - queueToUse->sendMessage(commandedBy, &reply); + // queueToUse->sendMessage(commandedBy, &reply); ipcStore->deleteData(dataAddress); return; } auto actionIter = actionMap.find(actionId); - if (actionIter == actionMap.end()){ + if (actionIter == actionMap.end()) { CommandMessage reply; ActionMessage::setStepReply(&reply, actionId, 0, HasActionsIF::INVALID_ACTION_ID); - queueToUse->sendMessage(commandedBy, &reply); + // queueToUse->sendMessage(commandedBy, &reply); ipcStore->deleteData(dataAddress); return; } Action* action = actionIter->second; result = action->deSerialize(&dataPtr, &size, SerializeIF::Endianness::NETWORK); - if ((result != returnvalue::OK) || (size != 0)){ //TODO write unittest for second condition + if ((result != returnvalue::OK) || (size != 0)) { // TODO write unittest for second condition CommandMessage reply; ActionMessage::setStepReply(&reply, actionId, 0, HasActionsIF::INVALID_PARAMETERS); - queueToUse->sendMessage(commandedBy, &reply); + // queueToUse->sendMessage(commandedBy, &reply); ipcStore->deleteData(dataAddress); return; } - //TODO call action->check() + // TODO call action->check() action->commandedBy = commandedBy; result = owner->executeAction(action); - ipcStore->deleteData(dataAddress); + + TmManager::protocolInformation information; + result = tmManager->getProtocolInformation(dataAddress, &information); + if (result != returnvalue::OK) { + // HTF did we get here? + } + + store_address_t replyStoreId; + uint8_t* replyPointer; + result = ipcStore->getFreeElement(&replyStoreId, information.offset + 4, &replyPointer); + + if (result != returnvalue::OK) { + return; + } + + memset(replyPointer, 0xfd, 4); + + CommandMessage message; + + TmMessage::setCommand(&message, replyStoreId, dataAddress); + + queueToUse->sendMessage(information.reportingQueue, &message); + + // ipcStore->deleteData(dataAddress); if (result == HasActionsIF::EXECUTION_FINISHED) { CommandMessage reply; ActionMessage::setCompletionReply(&reply, actionId, true, result); - queueToUse->sendMessage(commandedBy, &reply); + // queueToUse->sendMessage(commandedBy, &reply); } if (result != returnvalue::OK) { CommandMessage reply; ActionMessage::setStepReply(&reply, actionId, 0, result); - queueToUse->sendMessage(commandedBy, &reply); + // queueToUse->sendMessage(commandedBy, &reply); return; } } @@ -205,7 +236,7 @@ ReturnValue_t ActionHelper::reportData(MessageQueueId_t reportTo, ActionId_t rep } void ActionHelper::registerAction(Action* action) { - //TODO error handling + // TODO error handling ActionId_t id = action->getId(); actionMap.emplace(id, action); } diff --git a/src/fsfw/action/ActionHelper.h b/src/fsfw/action/ActionHelper.h index 20323ddff..4eb84e339 100644 --- a/src/fsfw/action/ActionHelper.h +++ b/src/fsfw/action/ActionHelper.h @@ -1,9 +1,12 @@ #ifndef FSFW_ACTION_ACTIONHELPER_H_ #define FSFW_ACTION_ACTIONHELPER_H_ +#include + #include "ActionMessage.h" #include "fsfw/ipc/MessageQueueIF.h" #include "fsfw/serialize/SerializeIF.h" + /** * @brief Action Helper is a helper class which handles action messages * @@ -102,7 +105,7 @@ class ActionHelper { * Needed so templateAction can check if actionHelper was * contructed already to aid in debuggig a nasty coding error. */ - MessageQueueIF const * getQueue() const; + MessageQueueIF const* getQueue() const; void registerAction(Action* action); @@ -116,6 +119,7 @@ class ActionHelper { //! Queue to be used as response sender, has to be set in ctor or with //! setQueueToUse MessageQueueIF* queueToUse; + TmManager* tmManager = nullptr; //! Pointer to an IPC Store, initialized during construction or StorageManagerIF* ipcStore = nullptr; //! Map of all implemented Actions diff --git a/src/fsfw/ipc/FwMessageTypes.h b/src/fsfw/ipc/FwMessageTypes.h index 6bd3dc90d..edbfa302b 100644 --- a/src/fsfw/ipc/FwMessageTypes.h +++ b/src/fsfw/ipc/FwMessageTypes.h @@ -17,6 +17,7 @@ enum FsfwMessageTypes { PARAMETER, FILE_SYSTEM_MESSAGE, HOUSEKEEPING, + TELEMETRY, FW_MESSAGES_COUNT, }; diff --git a/src/fsfw/objectmanager/frameworkObjects.h b/src/fsfw/objectmanager/frameworkObjects.h index 9487147d9..5c951672f 100644 --- a/src/fsfw/objectmanager/frameworkObjects.h +++ b/src/fsfw/objectmanager/frameworkObjects.h @@ -29,6 +29,7 @@ enum framework_objects : object_id_t { EVENT_MANAGER = 0x53030000, INTERNAL_ERROR_REPORTER = 0x53040000, IPC_STORE = 0x534f0300, + TM_MANAGER = 0x534f0400, // IDs for PUS Packet Communication TC_STORE = 0x534f0100, TM_STORE = 0x534f0200, diff --git a/src/fsfw/tmtc/AcceptsTelemetryIF.h b/src/fsfw/tmtc/AcceptsTelemetryIF.h index bb72c4257..2d655d419 100644 --- a/src/fsfw/tmtc/AcceptsTelemetryIF.h +++ b/src/fsfw/tmtc/AcceptsTelemetryIF.h @@ -8,12 +8,12 @@ * @details Any object receiving telemetry shall implement this interface * */ -class AcceptsTelemetryIF { +class AcceptsTelemetryIF2 { public: /** * @brief The virtual destructor as it is mandatory for C++ interfaces. */ - virtual ~AcceptsTelemetryIF() = default; + virtual ~AcceptsTelemetryIF2() = default; // [[nodiscard]] virtual const char* getName() const = 0; // /** diff --git a/src/fsfw/tmtc/CMakeLists.txt b/src/fsfw/tmtc/CMakeLists.txt index ca7543e82..84a2106e2 100644 --- a/src/fsfw/tmtc/CMakeLists.txt +++ b/src/fsfw/tmtc/CMakeLists.txt @@ -2,4 +2,5 @@ target_sources( ${LIB_FSFW_NAME} PRIVATE FsfwProtocolHeader.cpp TmManager.cpp + TmMessage.cpp UdpTmTcBridge.cpp) \ No newline at end of file diff --git a/src/fsfw/tmtc/TmManager.cpp b/src/fsfw/tmtc/TmManager.cpp index ed23afb4f..25e4f7f12 100644 --- a/src/fsfw/tmtc/TmManager.cpp +++ b/src/fsfw/tmtc/TmManager.cpp @@ -2,6 +2,8 @@ #include +TmManager::TmManager(object_id_t setObjectId) : SystemObject(setObjectId) {} + ReturnValue_t TmManager::initialize() { ReturnValue_t result = SystemObject::initialize(); if (result != returnvalue::OK) { @@ -39,7 +41,7 @@ ReturnValue_t TmManager::getProtocolInformation(store_address_t tc, return returnvalue::OK; } -ReturnValue_t TmManager::registerNetworkProtocolInterface(AcceptsTelemetryIF* object, +ReturnValue_t TmManager::registerNetworkProtocolInterface(AcceptsTelemetryIF2* object, neither_type protocol, size_t offset) { if (protocolMap.contains(protocol)) { return returnvalue::FAILED; diff --git a/src/fsfw/tmtc/TmManager.h b/src/fsfw/tmtc/TmManager.h index 77c7de5e2..43173e45c 100644 --- a/src/fsfw/tmtc/TmManager.h +++ b/src/fsfw/tmtc/TmManager.h @@ -41,7 +41,7 @@ class TmManager : public SystemObject { * Offset is where application data should start in a preset zero-copy packet. If no fixed value, * report 0 and allocate and copy yourself */ - ReturnValue_t registerNetworkProtocolInterface(AcceptsTelemetryIF *object, neither_type protocol, + ReturnValue_t registerNetworkProtocolInterface(AcceptsTelemetryIF2 *object, neither_type protocol, size_t offset); protected: diff --git a/src/fsfw/tmtc/TmMessage.cpp b/src/fsfw/tmtc/TmMessage.cpp new file mode 100644 index 000000000..ebc0876f1 --- /dev/null +++ b/src/fsfw/tmtc/TmMessage.cpp @@ -0,0 +1,15 @@ +#include "TmMessage.h" + +void TmMessage::setCommand(CommandMessage* message, store_address_t tm, store_address_t tc) { + message->setCommand(SEND_TM); + message->setParameter(tm.raw); + message->setParameter2(tc.raw); +} + +store_address_t TmMessage::getTm(CommandMessage* message) { + return store_address_t(message->getParameter()); +} + +store_address_t TmMessage::getTc(CommandMessage* message) { + return store_address_t(message->getParameter2()); +} diff --git a/src/fsfw/tmtc/TmMessage.h b/src/fsfw/tmtc/TmMessage.h new file mode 100644 index 000000000..dc6a9ec51 --- /dev/null +++ b/src/fsfw/tmtc/TmMessage.h @@ -0,0 +1,20 @@ +#pragma once + +#include "fsfw/ipc/CommandMessage.h" +#include "fsfw/storagemanager/StorageManagerIF.h" + +class TmMessage { + private: + TmMessage() = delete; + + public: + static const uint8_t MESSAGE_ID = messagetypes::TELEMETRY; + static const Command_t SEND_TM = MAKE_COMMAND_ID(1); + + virtual ~TmMessage() = default; + + static void setCommand(CommandMessage* message, store_address_t tm, store_address_t tc); + + static store_address_t getTm(CommandMessage* message); + static store_address_t getTc(CommandMessage* message); +}; \ No newline at end of file diff --git a/src/fsfw/tmtc/UdpTmTcBridge.cpp b/src/fsfw/tmtc/UdpTmTcBridge.cpp index eacbd264f..e57cf1c98 100644 --- a/src/fsfw/tmtc/UdpTmTcBridge.cpp +++ b/src/fsfw/tmtc/UdpTmTcBridge.cpp @@ -1,20 +1,21 @@ #include "UdpTmTcBridge.h" -#include "FsfwProtocolHeader.h" - #include #include #include #include #include #include -#include +#include #include #include // POSIX.1-2001 does not require the inclusion of , and this header file is not required on Linux. However, some historical (BSD) implementations required this header file, and portable applications are probably wise to include it. +#include "FsfwProtocolHeader.h" +#include "TmMessage.h" + UdpTmTcBridgeNew::UdpTmTcBridgeNew(object_id_t objectId, object_id_t tmStoreId, object_id_t tcStoreId) - : SystemObject(objectId) { + : SystemObject(objectId), defaultDestination(nullptr), defaultDestinationLen(0) { messageQueue = QueueFactory::instance()->createMessageQueue(50, MessageQueueMessage::MAX_MESSAGE_SIZE); } @@ -32,9 +33,18 @@ ReturnValue_t UdpTmTcBridgeNew::initialize() { return returnvalue::FAILED; } + TmManager *tmManager = ObjectManager::instance()->get(objects::TM_MANAGER); + if (tmManager == nullptr) { + return returnvalue::FAILED; + } + + // we do not need any space reserved for the header, as we can send with detached header + // information + tmManager->registerNetworkProtocolInterface(this, IP6, 0); + int retval; - serverSocket = socket(AF_INET, SOCK_DGRAM | SOCK_NONBLOCK, 0); + serverSocket = socket(AF_INET6, SOCK_DGRAM | SOCK_NONBLOCK, 0); if (serverSocket == -1) { // TODO resolve errno #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -43,17 +53,13 @@ ReturnValue_t UdpTmTcBridgeNew::initialize() { return returnvalue::FAILED; } - sockaddr_in serverAddr; - serverAddr.sin_family = AF_INET; - serverAddr.sin_port = htons(6667); + sockaddr_in6 serverAddr; - retval = inet_aton("10.13.90.2", &serverAddr.sin_addr); - if (retval == 0) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "UdpTmTcBridge::initialize: Invalid IP!" << std::endl; -#endif - return returnvalue::FAILED; - } + memset(&serverAddr, 0, sizeof(serverAddr)); + + serverAddr.sin6_family = AF_INET6; + serverAddr.sin6_port = htons(6667); + serverAddr.sin6_addr = IN6ADDR_ANY_INIT; retval = bind(serverSocket, (struct sockaddr *)&serverAddr, sizeof(serverAddr)); if (retval == -1) { @@ -67,55 +73,90 @@ ReturnValue_t UdpTmTcBridgeNew::initialize() { } ReturnValue_t UdpTmTcBridgeNew::performOperation(uint8_t operationCode) { - ssize_t peekLen = recv(serverSocket, NULL, 0, MSG_PEEK | MSG_TRUNC); + switch (operationCode) { + case BOTH: + handleTC(); + handleTM(); + break; + case TM: + handleTM(); + break; + case TC: + handleTC(); + break; + default: + break; + } + return returnvalue::OK; +} + +MessageQueueId_t UdpTmTcBridgeNew::getReportReceptionQueue() const { return messageQueue->getId(); } + +void UdpTmTcBridgeNew::handleTC() { + sockaddr_storage sender; + socklen_t senderlen = sizeof(sender); + + ssize_t peekLen = + recvfrom(serverSocket, NULL, 0, MSG_PEEK | MSG_TRUNC, (struct sockaddr *)&sender, &senderlen); if (peekLen <= 0) { - return returnvalue::OK; + return; } if (peekLen < FsfwProtocolHeader::HEADER_SIZE) { recv(serverSocket, NULL, 0, MSG_TRUNC); - return returnvalue::OK; + return; } + + if (sender.ss_family != AF_INET6) { + // TODO handle v4 if we allow setting a listening address + // for now, we listen as AF_INET6 and this should not happen + return; + } + store_address_t storageId; uint8_t *bufferPointer; - ReturnValue_t result = IPCStore->getFreeElement(&storageId, peekLen, &bufferPointer); + ReturnValue_t result = + IPCStore->getFreeElement(&storageId, peekLen + 1 + sizeof(sockaddr_in6), &bufferPointer); if (result != returnvalue::OK) { - return returnvalue::OK; + return; } - sockaddr_storage sender; - socklen_t senderlen = sizeof(sender); + *bufferPointer = IP6; - ssize_t receivedLen = - recvfrom(serverSocket, bufferPointer, peekLen, 0, (struct sockaddr *)&sender, &senderlen); + uint8_t *applicationData = bufferPointer + 1 + sizeof(sockaddr_in6); + senderlen = sizeof(sockaddr_in6); + + ssize_t receivedLen = recvfrom(serverSocket, applicationData, peekLen, 0, + (struct sockaddr *)(bufferPointer + 1), &senderlen); + + printf("type %i, port %i, senderlen %i, sizeof4 %li, sizeof6 %li\n", sender.ss_family, + ntohs(((sockaddr_in *)&sender)->sin_port), senderlen, sizeof(sockaddr_in), + sizeof(sockaddr_in6)); if (receivedLen == -1) { - return returnvalue::OK; + return; } if (receivedLen != peekLen) { // should not happen, if it does throw away - return returnvalue::OK; + return; } - - size_t bufferLen = receivedLen; - const uint8_t *constBufferPointer = bufferPointer; + const uint8_t *constApplicationData = applicationData; FsfwProtocolHeader header; - result = header.deSerialize(&constBufferPointer, &bufferLen, - SerializeIF::Endianness::NETWORK); + result = header.deSerialize(&constApplicationData, &bufferLen, SerializeIF::Endianness::NETWORK); if (result != returnvalue::OK) { - return returnvalue::OK; + return; } - sif::debug << "Received msg for 0x" << std::hex << header.getObjectId() << std::dec << " interface " - << (int) header.getInterface() << std::endl; + sif::debug << "Received msg for 0x" << std::hex << header.getObjectId() << std::dec + << " interface " << (int)header.getInterface() << std::endl; CommandMessage message; @@ -123,10 +164,10 @@ ReturnValue_t UdpTmTcBridgeNew::performOperation(uint8_t operationCode) { case HasActionsIF::INTERFACE_ID: { HasActionsIF *object = ObjectManager::instance()->get(header.getObjectId()); if (object == nullptr) { - return returnvalue::OK; + return; } - - ActionMessage::setCommand(&message, header.HEADER_SIZE, storageId); + + ActionMessage::setCommand(&message, header.HEADER_SIZE + 1 + sizeof(sockaddr_in6), storageId); result = messageQueue->sendMessage(object->getCommandQueue(), &message); // sif::debug << "UdpTmTcBridge::performOperation: sent " << (int)storageId.raw << std::endl; } break; @@ -137,6 +178,55 @@ ReturnValue_t UdpTmTcBridgeNew::performOperation(uint8_t operationCode) { #endif break; } - - return returnvalue::OK; +} + +void UdpTmTcBridgeNew::handleTM() { + CommandMessage message; + ReturnValue_t result = messageQueue->receiveMessage(&message); + + if (result != returnvalue::OK) { + return; + } + + if (message.getCommand() != TmMessage::SEND_TM) { + return; + } + + store_address_t tc = TmMessage::getTc(&message); + store_address_t tm = TmMessage::getTm(&message); + + const sockaddr *receiver; + socklen_t receiverLen; + + const uint8_t *tcData; + size_t tcDataSize; + result = IPCStore->getData(tc, &tcData, &tcDataSize); + if (result != returnvalue::OK) { + puts("default"); + receiver = defaultDestination; + receiverLen = defaultDestinationLen; + } else { + if (*tcData != IP6) { + // this should not have been routed here + puts("huh?"); + return; + } + puts("from tc"); + receiver = (const sockaddr *)tcData + 1; + receiverLen = sizeof(sockaddr_in6); + } + + const uint8_t *tmData; + size_t tmDataSize; + result = IPCStore->getData(tm, &tmData, &tmDataSize); + if (result != returnvalue::OK) { + // nothing to send + return; + } + + int res = sendto(serverSocket, tmData, tmDataSize, 0, receiver, receiverLen); + + if (res == -1) { + sif::error << "UdpTmTcBridge::handleTM: sendto failed with " << errno << (int) receiver->sa_family << std::endl; + } } diff --git a/src/fsfw/tmtc/UdpTmTcBridge.h b/src/fsfw/tmtc/UdpTmTcBridge.h index 4914a13b4..f214fd7ac 100644 --- a/src/fsfw/tmtc/UdpTmTcBridge.h +++ b/src/fsfw/tmtc/UdpTmTcBridge.h @@ -1,25 +1,36 @@ #pragma once -#include +#include #include #include -#include +#include +#include +#include +class UdpTmTcBridgeNew : public SystemObject, + public ExecutableObjectIF, + public AcceptsTelemetryIF2 { + public: + enum PerformOperationCode { BOTH = 0, TM, TC }; -class UdpTmTcBridgeNew : public SystemObject, public ExecutableObjectIF { - public: + static const size_t MINIMAL_LENGTH = 4 + 2; // ObjectId, interface, function + UdpTmTcBridgeNew(object_id_t objectId, object_id_t tmStoreId, object_id_t tcStoreId); + virtual ~UdpTmTcBridgeNew(); - static const size_t MINIMAL_LENGTH = 4 + 2; // ObjectId, interface, function -UdpTmTcBridgeNew(object_id_t objectId, - object_id_t tmStoreId, object_id_t tcStoreId); -virtual ~UdpTmTcBridgeNew(); + ReturnValue_t initialize() override; -ReturnValue_t initialize() override; + ReturnValue_t performOperation(uint8_t operationCode) override; -ReturnValue_t performOperation(uint8_t operationCode) override; + MessageQueueId_t getReportReceptionQueue() const override; -private: -MessageQueueIF* messageQueue; -int serverSocket; -StorageManagerIF* IPCStore; + private: + MessageQueueIF* messageQueue; + int serverSocket; + StorageManagerIF* IPCStore; + + const sockaddr* const defaultDestination; + const socklen_t defaultDestinationLen; + + void handleTC(); + void handleTM(); }; \ No newline at end of file