From 5431dfc9bdea0ecc342edc5123bab7062f7f4626 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 14 Dec 2022 13:51:24 +0100
Subject: [PATCH] basic packet routing

---
 common/config/eive/objects.h        |  3 ++-
 fsfw                                |  2 +-
 mission/tmtc/Service15TmStorage.cpp | 18 ++++++++++++++++++
 3 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/common/config/eive/objects.h b/common/config/eive/objects.h
index fa5ebecc..6c2d87a1 100644
--- a/common/config/eive/objects.h
+++ b/common/config/eive/objects.h
@@ -149,7 +149,8 @@ enum commonObjects : uint32_t {
   MISC_TM_STORE = 0x73020001,
   OK_TM_STORE = 0x73020002,
   NOT_OK_TM_STORE = 0x73020003,
-  HK_TM_STORE = 0x73020004
+  HK_TM_STORE = 0x73020004,
+  CFDP_TM_STORE = 0x73030000
 };
 }
 
diff --git a/fsfw b/fsfw
index 75fc7a05..2aa4af69 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit 75fc7a056db2a4aee8c20c225fd3c2b79fce8ce3
+Subproject commit 2aa4af69742d932f09ec2a1d3d29b648295035d1
diff --git a/mission/tmtc/Service15TmStorage.cpp b/mission/tmtc/Service15TmStorage.cpp
index 5bdec8e1..c4c9bc43 100644
--- a/mission/tmtc/Service15TmStorage.cpp
+++ b/mission/tmtc/Service15TmStorage.cpp
@@ -1,5 +1,10 @@
 #include "Service15TmStorage.h"
 
+#include <fsfw/objectmanager/ObjectManager.h>
+#include <fsfw/tmstorage/TmStoreFrontendSimpleIF.h>
+
+#include "eive/objects.h"
+
 using namespace returnvalue;
 
 Service15TmStorage::Service15TmStorage(object_id_t objectId, uint16_t apid,
@@ -24,6 +29,19 @@ ReturnValue_t Service15TmStorage::getMessageQueueAndObject(uint8_t subservice,
                                                            const uint8_t *tcData, size_t tcDataLen,
                                                            MessageQueueId_t *id,
                                                            object_id_t *objectId) {
+  object_id_t targetObjectId;
+  SerializeAdapter::deSerialize(&targetObjectId, &tcData, &tcDataLen,
+                                SerializeIF::Endianness::NETWORK);
+  if (targetObjectId == objects::CFDP_TM_STORE) {
+    *objectId = objects::CFDP_TM_FUNNEL;
+  } else {
+    *objectId = objects::PUS_TM_FUNNEL;
+  }
+  auto *frontendIF = ObjectManager::instance()->get<TmStoreFrontendSimpleIF>(*objectId);
+  if (frontendIF == nullptr) {
+    return FAILED;
+  }
+  *id = frontendIF->getCommandQueue();
   return OK;
 }