From e0a383f0afb79aed32a90d5e9088b06102ff2ce9 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 17 Oct 2022 16:35:04 +0200
Subject: [PATCH] continue cfdp funnel impl

---
 example/core/GenericFactory.cpp  |  2 +-
 example/utility/CfdpTmFunnel.cpp | 28 ++++++++++++++++++++++++----
 example/utility/CfdpTmFunnel.h   |  8 +++++++-
 example/utility/PusTmFunnel.cpp  |  3 +--
 4 files changed, 33 insertions(+), 8 deletions(-)

diff --git a/example/core/GenericFactory.cpp b/example/core/GenericFactory.cpp
index ef7d9bc..2712ef2 100644
--- a/example/core/GenericFactory.cpp
+++ b/example/core/GenericFactory.cpp
@@ -64,7 +64,7 @@ void ObjectFactory::produceGenericObjects(PusTmFunnel **pusFunnel,
       new CcsdsDistributor(common::COMMON_PUS_APID, objects::CCSDS_DISTRIBUTOR, &tcStore);
   new PusDistributor(common::COMMON_PUS_APID, objects::PUS_DISTRIBUTOR, *ccsdsDistrib);
   *pusFunnel = new PusTmFunnel(objects::PUS_TM_FUNNEL, tmtcBridge, *stamperAndReader);
-  auto *cfdpFunnel = new CfdpTmFunnel(objects::CFDP_TM_FUNNEL, tmtcBridge);
+  auto *cfdpFunnel = new CfdpTmFunnel(objects::CFDP_TM_FUNNEL, tmtcBridge, tmStore);
   new TmFunnel(objects::TM_FUNNEL, **pusFunnel, *cfdpFunnel);
 #endif /* OBSW_ADD_CORE_COMPONENTS == 1 */
 
diff --git a/example/utility/CfdpTmFunnel.cpp b/example/utility/CfdpTmFunnel.cpp
index f2b3ec1..df9e303 100644
--- a/example/utility/CfdpTmFunnel.cpp
+++ b/example/utility/CfdpTmFunnel.cpp
@@ -1,9 +1,11 @@
 #include "CfdpTmFunnel.h"
 
 #include "fsfw/ipc/QueueFactory.h"
+#include "fsfw/tmtcservices/TmTcMessage.h"
 
-CfdpTmFunnel::CfdpTmFunnel(object_id_t objectId, const AcceptsTelemetryIF& downlinkDestination)
-    : SystemObject(objectId) {
+CfdpTmFunnel::CfdpTmFunnel(object_id_t objectId, const AcceptsTelemetryIF& downlinkDestination,
+                           StorageManagerIF& tmStore)
+    : SystemObject(objectId), tmStore(tmStore) {
   msgQueue = QueueFactory::instance()->createMessageQueue(5);
 }
 
@@ -13,6 +15,24 @@ MessageQueueId_t CfdpTmFunnel::getReportReceptionQueue(uint8_t virtualChannel) c
   return msgQueue->getId();
 }
 
-ReturnValue_t CfdpTmFunnel::performOperation(uint8_t opCode) { return returnvalue::OK; }
+ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) {
+  TmTcMessage currentMessage;
+  ReturnValue_t status = msgQueue->receiveMessage(&currentMessage);
+  while (status == returnvalue::OK) {
+    status = handlePacket(currentMessage);
+    if (status != returnvalue::OK) {
+      sif::warning << "TmFunnel packet handling failed" << std::endl;
+      break;
+    }
+    status = msgQueue->receiveMessage(&currentMessage);
+  }
 
-ReturnValue_t CfdpTmFunnel::initialize() { return returnvalue::OK; }
\ No newline at end of file
+  if (status == MessageQueueIF::EMPTY) {
+    return returnvalue::OK;
+  }
+  return status;
+}
+
+ReturnValue_t CfdpTmFunnel::initialize() { return returnvalue::OK; }
+
+ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) { return 0; }
diff --git a/example/utility/CfdpTmFunnel.h b/example/utility/CfdpTmFunnel.h
index bf61c54..28da00e 100644
--- a/example/utility/CfdpTmFunnel.h
+++ b/example/utility/CfdpTmFunnel.h
@@ -2,11 +2,14 @@
 #define FSFW_EXAMPLE_COMMON_CFDPTMFUNNEL_H
 
 #include "fsfw/objectmanager/SystemObject.h"
+#include "fsfw/storagemanager/StorageManagerIF.h"
 #include "fsfw/tmtcservices/AcceptsTelemetryIF.h"
+#include "fsfw/tmtcservices/TmTcMessage.h"
 
 class CfdpTmFunnel : public AcceptsTelemetryIF, public SystemObject {
  public:
-  CfdpTmFunnel(object_id_t objectId, const AcceptsTelemetryIF& downlinkDestination);
+  CfdpTmFunnel(object_id_t objectId, const AcceptsTelemetryIF& downlinkDestination,
+               StorageManagerIF& tmStore);
   [[nodiscard]] const char* getName() const override;
   [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override;
 
@@ -14,6 +17,9 @@ class CfdpTmFunnel : public AcceptsTelemetryIF, public SystemObject {
   ReturnValue_t initialize() override;
 
  private:
+  ReturnValue_t handlePacket(TmTcMessage& msg);
+
   MessageQueueIF* msgQueue;
+  StorageManagerIF& tmStore;
 };
 #endif  // FSFW_EXAMPLE_COMMON_CFDPTMFUNNEL_H
diff --git a/example/utility/PusTmFunnel.cpp b/example/utility/PusTmFunnel.cpp
index 7470045..6e0a14f 100644
--- a/example/utility/PusTmFunnel.cpp
+++ b/example/utility/PusTmFunnel.cpp
@@ -32,9 +32,8 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
 
   if (status == MessageQueueIF::EMPTY) {
     return returnvalue::OK;
-  } else {
-    return status;
   }
+  return status;
 }
 
 ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage *message) {