working on tm

src/fsfw/action/ActionHelper.cpp

@@ -1,8 +1,10 @@
+#include <fsfw/serialize/SerializeAdapter.h>
+#include <fsfw/tmtc/TmMessage.h>
+
 #include "fsfw/action.h"
 #include "fsfw/ipc/MessageQueueSenderIF.h"
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
-#include <fsfw/serialize/SerializeAdapter.h>
 
 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<TmManager>(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 {
+MessageQueueIF const* ActionHelper::getQueue() const { return queueToUse; }
-  return queueToUse;
-}
 
 void ActionHelper::prepareExecution(MessageQueueId_t commandedBy, size_t offset,
                                     store_address_t dataAddress) {
-  const uint8_t* dataPtr = nullptr;
+  const uint8_t* tcData = nullptr;
-  size_t size = 0;
+  size_t tcDataSize = 0;
-  ReturnValue_t result = ipcStore->getData(dataAddress, &dataPtr, &size);
+  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;
+  const uint8_t* dataPtr = tcData + offset;
-  size -=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);
 }

src/fsfw/action/ActionHelper.h

@@ -1,9 +1,12 @@
 #ifndef FSFW_ACTION_ACTIONHELPER_H_
 #define FSFW_ACTION_ACTIONHELPER_H_
 
+#include <fsfw/tmtc/TmManager.h>
+
 #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

src/fsfw/ipc/FwMessageTypes.h

@@ -17,6 +17,7 @@ enum FsfwMessageTypes {
   PARAMETER,
   FILE_SYSTEM_MESSAGE,
   HOUSEKEEPING,
+  TELEMETRY,
 
   FW_MESSAGES_COUNT,
 };

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,

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

src/fsfw/tmtc/CMakeLists.txt

@@ -2,4 +2,5 @@ target_sources(
   ${LIB_FSFW_NAME} PRIVATE
   FsfwProtocolHeader.cpp
   TmManager.cpp
+  TmMessage.cpp
   UdpTmTcBridge.cpp)

src/fsfw/tmtc/TmManager.cpp

@@ -2,6 +2,8 @@
 
 #include <fsfw/objectmanager/ObjectManager.h>
 
+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;

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:

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());
+}

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);
+};

src/fsfw/tmtc/UdpTmTcBridge.cpp

@@ -1,20 +1,21 @@
 #include "UdpTmTcBridge.h"
 
-#include "FsfwProtocolHeader.h"
-
 #include <arpa/inet.h>
 #include <errno.h>
 #include <fsfw/action.h>
 #include <fsfw/ipc/QueueFactory.h>
 #include <fsfw/objectmanager.h>
 #include <fsfw/serviceinterface/ServiceInterface.h>
-#include <netinet/in.h>
+#include <fsfw/tmtc/TmManager.h>
 #include <sys/socket.h>
 #include <sys/types.h>  // POSIX.1-2001 does not require the inclusion of <sys/types.h>, 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<TmManager>(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;
+  sockaddr_in6 serverAddr;
-  serverAddr.sin_family = AF_INET;
-  serverAddr.sin_port = htons(6667);
 
-  retval = inet_aton("10.13.90.2", &serverAddr.sin_addr);
+  memset(&serverAddr, 0, sizeof(serverAddr));
-  if (retval == 0) {
+
-#if FSFW_CPP_OSTREAM_ENABLED == 1
+  serverAddr.sin6_family = AF_INET6;
-    sif::error << "UdpTmTcBridge::initialize: Invalid IP!" << std::endl;
+  serverAddr.sin6_port = htons(6667);
-#endif
+  serverAddr.sin6_addr = IN6ADDR_ANY_INIT;
-    return returnvalue::FAILED;
-  }
 
   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;
+  *bufferPointer = IP6;
-  socklen_t senderlen = sizeof(sender);
 
-  ssize_t receivedLen =
+  uint8_t *applicationData = bufferPointer + 1 + sizeof(sockaddr_in6);
-      recvfrom(serverSocket, bufferPointer, peekLen, 0, (struct sockaddr *)&sender, &senderlen);
+  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,
+  result = header.deSerialize(&constApplicationData, &bufferLen, SerializeIF::Endianness::NETWORK);
-                                           SerializeIF::Endianness::NETWORK);
 
   if (result != returnvalue::OK) {
-    return returnvalue::OK;
+    return;
   }
 
-  sif::debug << "Received msg for 0x" << std::hex << header.getObjectId() << std::dec << " interface "
+  sif::debug << "Received msg for 0x" << std::hex << header.getObjectId() << std::dec
-             << (int) header.getInterface() << std::endl;
+             << " 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<HasActionsIF>(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;
+  }
 }

src/fsfw/tmtc/UdpTmTcBridge.h

@@ -1,25 +1,36 @@
 #pragma once
 
-#include <fsfw/tasks/ExecutableObjectIF.h>
+#include <fsfw/ipc/MessageQueueIF.h>
 #include <fsfw/objectmanager/SystemObject.h>
 #include <fsfw/storagemanager/StorageManagerIF.h>
-#include <fsfw/ipc/MessageQueueIF.h>
+#include <fsfw/tasks/ExecutableObjectIF.h>
+#include <fsfw/tmtc/AcceptsTelemetryIF.h>
+#include <netinet/in.h>
 
-
+class UdpTmTcBridgeNew : public SystemObject,
-class UdpTmTcBridgeNew : public SystemObject, public ExecutableObjectIF {
+                         public ExecutableObjectIF,
+                         public AcceptsTelemetryIF2 {
  public:
+  enum PerformOperationCode { BOTH = 0, TM, TC };
 
   static const size_t MINIMAL_LENGTH = 4 + 2;  // ObjectId, interface, function
-UdpTmTcBridgeNew(object_id_t objectId,
+  UdpTmTcBridgeNew(object_id_t objectId, object_id_t tmStoreId, object_id_t tcStoreId);
-                             object_id_t tmStoreId, object_id_t tcStoreId);
+  virtual ~UdpTmTcBridgeNew();
-virtual ~UdpTmTcBridgeNew();
 
-ReturnValue_t initialize() override;
+  ReturnValue_t initialize() override;
 
-ReturnValue_t performOperation(uint8_t operationCode) override;
+  ReturnValue_t performOperation(uint8_t operationCode) override;
 
-private:
+  MessageQueueId_t getReportReceptionQueue() const override;
-MessageQueueIF* messageQueue;
+
-int serverSocket;
+ private:
-StorageManagerIF* IPCStore;
+  MessageQueueIF* messageQueue;
+  int serverSocket;
+  StorageManagerIF* IPCStore;
+
+  const sockaddr* const defaultDestination;
+  const socklen_t defaultDestinationLen;
+
+  void handleTC();
+  void handleTM();
 };