From 6b931dce7f3de0b3bb529e63ddc8f4f16f0b0932 Mon Sep 17 00:00:00 2001
From: Robin Mueller <muellerr@irs.uni-stuttgart.de>
Date: Mon, 20 Feb 2023 15:02:58 +0100
Subject: [PATCH] important CFDP bugfix

---
 CHANGELOG.md                          |  4 ++++
 bsp_hosted/fsfwconfig/FSFWConfig.h.in |  2 ++
 fsfw                                  |  2 +-
 mission/tmtc/CfdpTmFunnel.cpp         | 16 ++++++++++------
 4 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 891a4d76..71818173 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -17,6 +17,10 @@ change warranting a new major release:
 
 # [unreleased]
 
+## Fixed
+
+- CFDP Funnel bugfix: CCSDS wrapping was buggy and works properly now.
+
 ## Changed
 
 - Add `-Wshadow=local` shadowing warnings and fixed all of them
diff --git a/bsp_hosted/fsfwconfig/FSFWConfig.h.in b/bsp_hosted/fsfwconfig/FSFWConfig.h.in
index db5a9ad8..cf4b941a 100644
--- a/bsp_hosted/fsfwconfig/FSFWConfig.h.in
+++ b/bsp_hosted/fsfwconfig/FSFWConfig.h.in
@@ -50,6 +50,8 @@
 //! If set to 1 the binary needs "cap_sys_nice=eip" privileges to run
 #define FSFW_USE_REALTIME_FOR_LINUX 0
 
+#define FSFW_UDP_SEND_WIRETAPPING_ENABLED 0
+
 namespace fsfwconfig {
 
 //! Default timestamp size. The default timestamp will be an seven byte CDC short timestamp.
diff --git a/fsfw b/fsfw
index c8469ca6..c3279852 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit c8469ca6473f64676e007e2e2f1c733fe6252053
+Subproject commit c32798522283d89028b7c1ecd3bd33b8391e1a39
diff --git a/mission/tmtc/CfdpTmFunnel.cpp b/mission/tmtc/CfdpTmFunnel.cpp
index f19d5360..c057295a 100644
--- a/mission/tmtc/CfdpTmFunnel.cpp
+++ b/mission/tmtc/CfdpTmFunnel.cpp
@@ -53,16 +53,19 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
 #endif
     return result;
   }
-  size_t serSize = 0;
+  size_t packetLen = 0;
+  uint8_t* serPtr = newPacketData;
   result =
-      spacePacketHeader.serializeBe(&newPacketData, &serSize, spacePacketHeader.getFullPacketLen());
+      spacePacketHeader.serializeBe(&serPtr, &packetLen, 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);
+  std::memcpy(serPtr, cfdpPacket, cfdpPacketLen);
+  packetLen += cfdpPacketLen;
+
   // Delete old packet
   tmStore.deleteData(msg.getStorageId());
   msg.setStorageId(newStoreId);
@@ -74,13 +77,14 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
         // Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
         // to bother with send order and where the data is deleted.
         store_address_t storeId;
-        result = tmStore.addData(&storeId, newPacketData, serSize);
+        result = tmStore.addData(&storeId, newPacketData, packetLen);
         if (result == returnvalue::OK) {
           msg.setStorageId(storeId);
         } else {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-          sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy"
-                     << std::endl;
+          sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy or store "
+                        "error" << std::endl;
+          break;
 #endif
         }
       } else {