From 98e1beafdf671027d89536ce6627b53d0dbeef18 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Wed, 13 Sep 2023 13:09:07 +0200
Subject: [PATCH] implemented missing queue handling

---
 mission/com/LiveTmTask.cpp | 34 ++++++++++++++++++++++++++++++++--
 mission/com/LiveTmTask.h   |  2 ++
 mission/sysDefs.h          |  4 ++--
 3 files changed, 36 insertions(+), 4 deletions(-)

diff --git a/mission/com/LiveTmTask.cpp b/mission/com/LiveTmTask.cpp
index 14706705..e9dcc84b 100644
--- a/mission/com/LiveTmTask.cpp
+++ b/mission/com/LiveTmTask.cpp
@@ -116,14 +116,44 @@ void LiveTmTask::readCommandQueue(void) {
   }
 }
 
-ReturnValue_t LiveTmTask::handleRegularTmQueue() { return returnvalue::OK; }
+ReturnValue_t LiveTmTask::handleRegularTmQueue() { return handleGenericTmQueue(*regularTmQueue); }
 
-ReturnValue_t LiveTmTask::handleCfdpTmQueue() { return returnvalue::OK; }
+ReturnValue_t LiveTmTask::handleCfdpTmQueue() { return handleGenericTmQueue(*cfdpTmQueue); }
+
+ReturnValue_t LiveTmTask::handleGenericTmQueue(MessageQueueIF& queue) {
+  TmTcMessage message;
+  ReturnValue_t result = queue.receiveMessage(&message);
+  if (result == MessageQueueIF::EMPTY) {
+    return result;
+  }
+  store_address_t storeId = message.getStorageId();
+  const uint8_t* data = nullptr;
+  size_t size = 0;
+  result = tmStore->getData(storeId, &data, &size);
+  if (result != returnvalue::OK) {
+    sif::warning << "VirtualChannel::performOperation: Failed to read data from TM store"
+                 << std::endl;
+    tmStore->deleteData(storeId);
+    return result;
+  }
+
+  if (ptmeLocked) {
+    result = channel.write(data, size);
+  }
+  // Try delete in any case, ignore failures (which should not happen), it is more important to
+  // propagate write errors.
+  tmStore->deleteData(storeId);
+  return result;
+}
 
 ModeTreeChildIF& LiveTmTask::getModeTreeChildIF() { return *this; }
 
 ReturnValue_t LiveTmTask::initialize() {
   modeHelper.initialize();
+  tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
+  if (tmStore == nullptr) {
+    return ObjectManagerIF::CHILD_INIT_FAILED;
+  }
   return returnvalue::OK;
 }
 
diff --git a/mission/com/LiveTmTask.h b/mission/com/LiveTmTask.h
index d1173ccc..a271b327 100644
--- a/mission/com/LiveTmTask.h
+++ b/mission/com/LiveTmTask.h
@@ -31,6 +31,7 @@ class LiveTmTask : public SystemObject,
   MessageQueueIF* requestQueue;
   MessageQueueIF* cfdpTmQueue;
   MessageQueueIF* regularTmQueue;
+  StorageManagerIF* tmStore = nullptr;
   ModeHelper modeHelper;
   Mode_t mode = HasModesIF::MODE_OFF;
   Countdown tmFunnelCd = Countdown(100);
@@ -44,6 +45,7 @@ class LiveTmTask : public SystemObject,
 
   ReturnValue_t handleRegularTmQueue();
   ReturnValue_t handleCfdpTmQueue();
+  ReturnValue_t handleGenericTmQueue(MessageQueueIF& queue);
 
   MessageQueueId_t getCommandQueue() const override;
 
diff --git a/mission/sysDefs.h b/mission/sysDefs.h
index e1259c5c..b7cbd4e0 100644
--- a/mission/sysDefs.h
+++ b/mission/sysDefs.h
@@ -1,8 +1,6 @@
 #ifndef MISSION_SYSDEFS_H_
 #define MISSION_SYSDEFS_H_
 
-#include "eive/eventSubsystemIds.h"
-
 #include <fsfw/action/ActionMessage.h>
 #include <fsfw/action/HasActionsIF.h>
 #include <fsfw/modes/ModeMessage.h>
@@ -11,6 +9,8 @@
 #include <atomic>
 #include <cstring>
 
+#include "eive/eventSubsystemIds.h"
+
 namespace signals {
 
 extern std::atomic_bool CFDP_CHANNEL_THROTTLE_SIGNAL;