From 0d106fe4d6da18eb16bec56a0848f2f403412e09 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 17 Oct 2022 16:58:43 +0200
Subject: [PATCH] completed CFDP TM funnel impl

---
 example/utility/CfdpTmFunnel.cpp | 32 ++++++++++++++++++++++++++------
 example/utility/PusTmFunnel.cpp  |  3 +++
 2 files changed, 29 insertions(+), 6 deletions(-)

diff --git a/example/utility/CfdpTmFunnel.cpp b/example/utility/CfdpTmFunnel.cpp
index 9568112..4a75971 100644
--- a/example/utility/CfdpTmFunnel.cpp
+++ b/example/utility/CfdpTmFunnel.cpp
@@ -37,18 +37,38 @@ ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) {
 ReturnValue_t CfdpTmFunnel::initialize() { return returnvalue::OK; }
 
 ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
-  uint8_t* packetData = nullptr;
-  size_t size = 0;
-  ReturnValue_t result = tmStore.modifyData(msg.getStorageId(), &packetData, &size);
+  const uint8_t* cfdpPacket = nullptr;
+  size_t cfdpPacketLen = 0;
+  ReturnValue_t result = tmStore.getData(msg.getStorageId(), &cfdpPacket, &cfdpPacketLen);
   if (result != returnvalue::OK) {
     return result;
   }
-  auto spacePacket =
+  auto spacePacketHeader =
       SpacePacketCreator(ccsds::PacketType::TM, false, cfdpInCcsdsApid,
                          ccsds::SequenceFlags::UNSEGMENTED, sourceSequenceCount++, 0);
   sourceSequenceCount = sourceSequenceCount & ccsds::LIMIT_SEQUENCE_COUNT;
-  spacePacket.setCcsdsLenFromTotalDataFieldLen(size);
-  // TODO: Get new slot from TM store, serialize space packet header first, and then the CFDP packet
+  spacePacketHeader.setCcsdsLenFromTotalDataFieldLen(cfdpPacketLen);
+  uint8_t* newPacketData = nullptr;
+  store_address_t newStoreId{};
+  result =
+      tmStore.getFreeElement(&newStoreId, spacePacketHeader.getFullPacketLen(), &newPacketData);
+  if (result != returnvalue::OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::warning << "CfdpTmFunnel::handlePacket: Error getting TM store element of size "
+                 << spacePacketHeader.getFullPacketLen() << std::endl;
+#endif
+    return result;
+  }
+  size_t serSize = 0;
+  result =
+      spacePacketHeader.serializeBe(&newPacketData, &serSize, spacePacketHeader.getFullPacketLen());
+  if (result != returnvalue::OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::error << "CfdpTmFunnel::handlePacket: Error serializing packet" << std::endl;
+#endif
+    return result;
+  }
+  std::memcpy(newPacketData, cfdpPacket, cfdpPacketLen);
   result = msgQueue->sendToDefault(&msg);
   if (result != returnvalue::OK) {
     tmStore.deleteData(msg.getStorageId());
diff --git a/example/utility/PusTmFunnel.cpp b/example/utility/PusTmFunnel.cpp
index da590b7..7164d23 100644
--- a/example/utility/PusTmFunnel.cpp
+++ b/example/utility/PusTmFunnel.cpp
@@ -46,6 +46,9 @@ ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage &message) {
   PusTmZeroCopyWriter packet(timeReader, packetData, size);
   result = packet.parseDataWithoutCrcCheck();
   if (result != returnvalue::OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::warning << "PusTmFunnel::handlePacket: Error parsing received PUS packet" << std::endl;
+#endif
     return result;
   }
   packet.setSequenceCount(sourceSequenceCount++);