diff --git a/bsp_hosted/CMakeLists.txt b/bsp_hosted/CMakeLists.txt
index ec2016b4..1300375e 100644
--- a/bsp_hosted/CMakeLists.txt
+++ b/bsp_hosted/CMakeLists.txt
@@ -1,4 +1,4 @@
-target_sources(${OBSW_NAME} PUBLIC scheduling.cpp main.cpp ObjectFactory.cpp)
+target_sources(${OBSW_NAME} PUBLIC scheduling.cpp main.cpp objectFactory.cpp)
 
 add_subdirectory(fsfwconfig)
 add_subdirectory(boardconfig)
diff --git a/bsp_hosted/OBSWConfig.h.in b/bsp_hosted/OBSWConfig.h.in
index 5e6fcc2c..6e525e5a 100644
--- a/bsp_hosted/OBSWConfig.h.in
+++ b/bsp_hosted/OBSWConfig.h.in
@@ -101,7 +101,7 @@
 /** CMake Defines                                                  */
 /*******************************************************************/
 
-#define OBSW_ADD_TMTC_UDP_SERVER            1
+#define OBSW_ADD_TMTC_UDP_SERVER            0
 #define OBSW_ADD_TMTC_TCP_SERVER            1
 
 #cmakedefine EIVE_BUILD_GPSD_GPS_HANDLER
diff --git a/bsp_hosted/ObjectFactory.cpp b/bsp_hosted/objectFactory.cpp
similarity index 97%
rename from bsp_hosted/ObjectFactory.cpp
rename to bsp_hosted/objectFactory.cpp
index 2f43393f..0f785e70 100644
--- a/bsp_hosted/ObjectFactory.cpp
+++ b/bsp_hosted/objectFactory.cpp
@@ -1,4 +1,4 @@
-#include "ObjectFactory.h"
+#include "objectFactory.h"
 
 #include <fsfw/power/DummyPowerSwitcher.h>
 #include <fsfw/tmtcservices/CommandingServiceBase.h>
@@ -61,14 +61,14 @@ void ObjectFactory::produce(void* args) {
   CfdpTmFunnel* cfdpFunnel;
   StorageManagerIF* tmStore;
   StorageManagerIF* ipcStore;
-  PersistentTmStores persistentStores;
+  PersistentTmStores persistentStores{};
   bool enableHkSets = false;
 #if OBSW_ENABLE_PERIODIC_HK == 1
   enableHkSets = true;
 #endif
   auto sdcMan = new DummySdCardManager("/tmp");
   ObjectFactory::produceGenericObjects(nullptr, &pusFunnel, &cfdpFunnel, *sdcMan, &ipcStore,
-                                       &tmStore, persistentStores, 120, enableHkSets);
+                                       &tmStore, persistentStores, 120, enableHkSets, false);
 
   new TmFunnelHandler(objects::LIVE_TM_TASK, *pusFunnel, *cfdpFunnel);
   auto* dummyGpioIF = new DummyGpioIF();
diff --git a/bsp_hosted/ObjectFactory.h b/bsp_hosted/objectFactory.h
similarity index 100%
rename from bsp_hosted/ObjectFactory.h
rename to bsp_hosted/objectFactory.h
diff --git a/bsp_hosted/scheduling.cpp b/bsp_hosted/scheduling.cpp
index e82748f4..95da8ea1 100644
--- a/bsp_hosted/scheduling.cpp
+++ b/bsp_hosted/scheduling.cpp
@@ -13,8 +13,8 @@
 #include <iostream>
 
 #include "OBSWConfig.h"
-#include "ObjectFactory.h"
 #include "mission/scheduling.h"
+#include "objectFactory.h"
 #include "scheduling.h"
 
 #ifdef LINUX
@@ -69,21 +69,25 @@ void scheduling::initTasks() {
   if (result != returnvalue::OK) {
     sif::error << "Adding CFDP distributor failed" << std::endl;
   }
+#if OBSW_ADD_TMTC_UDP_SERVER == 1
   result = tmtcDistributor->addComponent(objects::UDP_TMTC_SERVER);
   if (result != returnvalue::OK) {
     sif::error << "adding UDP server failed" << std::endl;
   }
+#endif
   result = tmtcDistributor->addComponent(objects::TCP_TMTC_SERVER);
   if (result != returnvalue::OK) {
     sif::error << "adding TCP server failed" << std::endl;
   }
 
+#if OBSW_ADD_TMTC_UDP_SERVER == 1
   PeriodicTaskIF* udpPollingTask = factory->createPeriodicTask(
       "UDP_POLLING", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc);
   result = udpPollingTask->addComponent(objects::UDP_TMTC_POLLING_TASK);
   if (result != returnvalue::OK) {
     sif::error << "Add component UDP Polling failed" << std::endl;
   }
+#endif
   PeriodicTaskIF* tcpPollingTask = factory->createPeriodicTask(
       "TCP_POLLING", 70, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc);
   result = tcpPollingTask->addComponent(objects::TCP_TMTC_POLLING_TASK);
@@ -92,7 +96,7 @@ void scheduling::initTasks() {
   }
 
   PeriodicTaskIF* liveTmTask = factory->createPeriodicTask(
-      "LIVE_TM", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.4, nullptr, &RR_SCHEDULING);
+      "LIVE_TM", 55, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, nullptr, &RR_SCHEDULING);
   result = liveTmTask->addComponent(objects::LIVE_TM_TASK);
   if (result != returnvalue::OK) {
     scheduling::printAddObjectError("LIVE_TM", objects::LIVE_TM_TASK);
@@ -179,6 +183,28 @@ void scheduling::initTasks() {
   }
 #endif
 
+  // If those are added at a later stage..
+  /*
+  PeriodicTaskIF* logTmTask = factory->createPeriodicTask(
+      "LOG_PSTORE", 0, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr);
+  result = logTmTask->addComponent(objects::LOG_STORE_AND_TM_TASK);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("LOG_STORE_AND_TM", objects::LOG_STORE_AND_TM_TASK);
+  }
+  PeriodicTaskIF* hkTmTask =
+      factory->createPeriodicTask("HK_PSTORE", 0, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr);
+  result = hkTmTask->addComponent(objects::HK_STORE_AND_TM_TASK);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("HK_STORE_AND_TM", objects::HK_STORE_AND_TM_TASK);
+  }
+  PeriodicTaskIF* cfdpTmTask = factory->createPeriodicTask(
+      "CFDP_PSTORE", 0, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr);
+  result = cfdpTmTask->addComponent(objects::CFDP_STORE_AND_TM_TASK);
+  if (result != returnvalue::OK) {
+    scheduling::printAddObjectError("CFDP_STORE_AND_TM", objects::CFDP_STORE_AND_TM_TASK);
+  }
+   */
+
 #if OBSW_ADD_PLOC_SUPERVISOR == 1
   PeriodicTaskIF* supvHelperTask = factory->createPeriodicTask(
       "PLOC_SUPV_HELPER", 20, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, missedDeadlineFunc);
@@ -218,7 +244,9 @@ void scheduling::initTasks() {
 
   sif::info << "Starting tasks.." << std::endl;
   tmtcDistributor->startTask();
+#if OBSW_ADD_TMTC_UDP_SERVER == 1
   udpPollingTask->startTask();
+#endif
   tcpPollingTask->startTask();
   liveTmTask->startTask();
 
@@ -228,6 +256,12 @@ void scheduling::initTasks() {
   pstTask->startTask();
   thermalTask->startTask();
   dummyTask->startTask();
+
+  // If those are added at a later stage..
+  //logTmTask->startTask();
+  //cfdpTmTask->startTask();
+  //hkTmTask->startTask();
+
 #if OBSW_ADD_PLOC_SUPERVISOR == 1
   supvHelperTask->startTask();
 #endif
diff --git a/common/config/eive/definitions.h b/common/config/eive/definitions.h
index b369f512..d5ebe674 100644
--- a/common/config/eive/definitions.h
+++ b/common/config/eive/definitions.h
@@ -58,14 +58,22 @@ static constexpr uint32_t NOK_STORE_QUEUE_SIZE = 350;
 static constexpr uint32_t HK_STORE_QUEUE_SIZE = 300;
 static constexpr uint32_t CFDP_STORE_QUEUE_SIZE = 300;
 
+static constexpr uint32_t CFDP_MAX_FSM_CALL_COUNT_SRC_HANDLER = 50;
+static constexpr uint32_t CFDP_MAX_FSM_CALL_COUNT_DEST_HANDLER = 300;
+static constexpr uint32_t CFDP_SHORT_DELAY_MS = 100;
+static constexpr uint32_t CFDP_REGULAR_DELAY_MS = 200;
+
 static constexpr uint32_t MAX_PUS_FUNNEL_QUEUE_DEPTH = 100;
-static constexpr uint32_t MAX_CFDP_FUNNEL_QUEUE_DEPTH = 80;
+static constexpr uint32_t MAX_CFDP_FUNNEL_QUEUE_DEPTH = 150;
 static constexpr uint32_t VERIFICATION_SERVICE_QUEUE_DEPTH = 120;
 static constexpr uint32_t HK_SERVICE_QUEUE_DEPTH = 60;
 static constexpr uint32_t ACTION_SERVICE_QUEUE_DEPTH = 60;
 
-static constexpr uint32_t MAX_STORED_CMDS_UDP = 150;
-static constexpr uint32_t MAX_STORED_CMDS_TCP = 180;
+static constexpr uint32_t UDP_MAX_STORED_CMDS = 200;
+static constexpr uint32_t UDP_MSG_QUEUE_DEPTH = UDP_MAX_STORED_CMDS;
+static constexpr uint32_t TCP_MAX_STORED_CMDS = 300;
+static constexpr uint32_t TCP_MSG_QUEUE_DEPTH = TCP_MAX_STORED_CMDS;
+static constexpr uint32_t TCP_MAX_NUMBER_TMS_SENT_PER_CYCLE = TCP_MSG_QUEUE_DEPTH;
 
 namespace spiSched {
 
diff --git a/fsfw b/fsfw
index c1431984..c199cbed 160000
--- a/fsfw
+++ b/fsfw
@@ -1 +1 @@
-Subproject commit c1431984947275f7c2c33c1e671b0e4d17c77f71
+Subproject commit c199cbedaa4b7f6ba0b88fae26412cfa65f36739
diff --git a/mission/cfdp/CfdpHandler.cpp b/mission/cfdp/CfdpHandler.cpp
index e64bd5df..cc400b9a 100644
--- a/mission/cfdp/CfdpHandler.cpp
+++ b/mission/cfdp/CfdpHandler.cpp
@@ -3,10 +3,12 @@
 #include <fsfw/cfdp/CfdpMessage.h>
 #include <fsfw/ipc/CommandMessage.h>
 
+#include "eive/definitions.h"
 #include "fsfw/cfdp/pdu/AckPduReader.h"
 #include "fsfw/cfdp/pdu/PduHeaderReader.h"
 #include "fsfw/globalfunctions/arrayprinter.h"
 #include "fsfw/ipc/QueueFactory.h"
+#include "fsfw/tasks/TaskFactory.h"
 #include "fsfw/tmtcservices/TmTcMessage.h"
 
 using namespace returnvalue;
@@ -46,28 +48,47 @@ ReturnValue_t CfdpHandler::initialize() {
   return SystemObject::initialize();
 }
 
-ReturnValue_t CfdpHandler::performOperation(uint8_t operationCode) {
-  ReturnValue_t status = OK;
-  ReturnValue_t result = handlePduPacketMessages();
-  if (result != OK) {
-    status = result;
+[[noreturn]] ReturnValue_t CfdpHandler::performOperation(uint8_t operationCode) {
+  while (true) {
+    bool shortDelay = false;
+    ReturnValue_t result = handlePduPacketMessages();
+    if (result != OK) {
+    }
+    result = handleCfdpMessages();
+    if (result != OK) {
+    }
+    uint32_t fsmCount = 0;
+    const DestHandler::FsmResult& destResult = destHandler.stateMachine();
+    while (destResult.callStatus == CallStatus::CALL_AGAIN) {
+      if (fsmCount == config::CFDP_MAX_FSM_CALL_COUNT_DEST_HANDLER) {
+        shortDelay = true;
+        break;
+      }
+      destHandler.stateMachine();
+      fsmCount++;
+    }
+    fsmCount = 0;
+    const SourceHandler::FsmResult& srcResult = srcHandler.stateMachine();
+    while (srcResult.callStatus == CallStatus::CALL_AGAIN) {
+      // Limit number of messages.
+      if (fsmCount == config::CFDP_MAX_FSM_CALL_COUNT_SRC_HANDLER) {
+        shortDelay = true;
+        break;
+      }
+      srcHandler.stateMachine();
+      if(srcResult.result == cfdp::TM_STORE_FULL) {
+        sif::warning << "CFDP Source Handler: TM store is full" << std::endl;
+      } else if(srcResult.result == cfdp::TARGET_MSG_QUEUE_FULL) {
+        sif::warning << "CFDP Source Handler: TM queue is full" << std::endl;
+      }
+      fsmCount++;
+    }
+    if (shortDelay) {
+      TaskFactory::delayTask(config::CFDP_SHORT_DELAY_MS);
+      continue;
+    }
+    TaskFactory::delayTask(config::CFDP_REGULAR_DELAY_MS);
   }
-  result = handleCfdpMessages();
-  if (result != OK) {
-    status = result;
-  }
-  const DestHandler::FsmResult& destResult = destHandler.stateMachine();
-  // TODO: Error handling?
-  while (destResult.callStatus == CallStatus::CALL_AGAIN) {
-    destHandler.stateMachine();
-    // TODO: Error handling?
-  }
-  const SourceHandler::FsmResult& srcResult = srcHandler.stateMachine();
-  while (srcResult.callStatus == CallStatus::CALL_AGAIN) {
-    sif::debug << "calling fsm" << std::endl;
-    srcHandler.stateMachine();
-  }
-  return status;
 }
 
 ReturnValue_t CfdpHandler::handlePduPacket(TmTcMessage& msg) {
diff --git a/mission/cfdp/CfdpHandler.h b/mission/cfdp/CfdpHandler.h
index 18cf46b7..d409d6bd 100644
--- a/mission/cfdp/CfdpHandler.h
+++ b/mission/cfdp/CfdpHandler.h
@@ -68,7 +68,7 @@ class CfdpHandler : public SystemObject, public ExecutableObjectIF, public Accep
   [[nodiscard]] MessageQueueId_t getRequestQueue() const override;
 
   ReturnValue_t initialize() override;
-  ReturnValue_t performOperation(uint8_t operationCode) override;
+  [[noreturn]] ReturnValue_t performOperation(uint8_t operationCode) override;
 
  private:
   MessageQueueIF& pduQueue;
diff --git a/mission/genericFactory.cpp b/mission/genericFactory.cpp
index 9d2778f3..ea7a8bcb 100644
--- a/mission/genericFactory.cpp
+++ b/mission/genericFactory.cpp
@@ -99,7 +99,8 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
                                           CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan,
                                           StorageManagerIF** ipcStore, StorageManagerIF** tmStore,
                                           PersistentTmStores& stores,
-                                          uint32_t eventManagerQueueDepth, bool enableHkSets) {
+                                          uint32_t eventManagerQueueDepth, bool enableHkSets,
+                                          bool routeToPersistentStores) {
   // Framework objects
   new EventManager(objects::EVENT_MANAGER, eventManagerQueueDepth);
   auto healthTable = new HealthTable(objects::HEALTH_TABLE);
@@ -118,7 +119,7 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
 
   {
     PoolManager::LocalPoolConfig poolCfg = {{600, 32},  {400, 64},   {400, 128},
-                                            {300, 512}, {300, 1200}, {120, 2048}};
+                                            {350, 512}, {500, 1200}, {100, 2048}};
     *tmStore = new PoolManager(objects::TM_STORE, poolCfg);
   }
 
@@ -128,28 +129,29 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
     *ipcStore = new PoolManager(objects::IPC_STORE, poolCfg);
   }
   PoolManager::LocalPoolConfig poolCfg = {{300, 32},  {400, 64},   {250, 128},
-                                          {150, 512}, {150, 1024}, {150, 2048}};
+                                          {150, 512}, {400, 1200}, {150, 2048}};
   auto* ramToFileStore = new PoolManager(objects::DOWNLINK_RAM_STORE, poolCfg);
 
 #if OBSW_ADD_TCPIP_SERVERS == 1
 #if OBSW_ADD_TMTC_UDP_SERVER == 1
   auto udpBridge =
-      new UdpTmTcBridge(objects::UDP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR, 120);
+      new UdpTmTcBridge(objects::UDP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR, config::UDP_MSG_QUEUE_DEPTH);
   new UdpTcPollingTask(objects::UDP_TMTC_POLLING_TASK, objects::UDP_TMTC_SERVER);
   sif::info << "Created UDP server for TMTC commanding with listener port "
             << udpBridge->getUdpPort() << std::endl;
-  udpBridge->setMaxNumberOfPacketsStored(config::MAX_STORED_CMDS_UDP);
+  udpBridge->setMaxNumberOfPacketsStored(config::UDP_MAX_STORED_CMDS);
 #endif
 #if OBSW_ADD_TMTC_TCP_SERVER == 1
   auto tcpBridge =
-      new TcpTmTcBridge(objects::TCP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR, 120);
+      new TcpTmTcBridge(objects::TCP_TMTC_SERVER, objects::CCSDS_PACKET_DISTRIBUTOR, config::TCP_MSG_QUEUE_DEPTH);
   TcpTmTcServer::TcpConfig cfg(true, true);
   auto tcpServer = new TcpTmTcServer(objects::TCP_TMTC_POLLING_TASK, objects::TCP_TMTC_SERVER, cfg);
   // TCP is stream based. Use packet ID as start marker when parsing for space packets
   tcpServer->setSpacePacketParsingOptions({common::PUS_PACKET_ID, common::CFDP_PACKET_ID});
   sif::info << "Created TCP server for TMTC commanding with listener port "
             << tcpServer->getTcpPort() << std::endl;
-  tcpBridge->setMaxNumberOfPacketsStored(config::MAX_STORED_CMDS_TCP);
+  tcpBridge->setMaxNumberOfPacketsStored(config::TCP_MAX_STORED_CMDS);
+  tcpBridge->setNumberOfSentPacketsPerCycle(config::TCP_MAX_NUMBER_TMS_SENT_PER_CYCLE);
 #endif /* OBSW_USE_TMTC_TCP_BRIDGE == 0 */
 #endif /* OBSW_ADD_TCPIP_BRIDGE == 1 */
 
@@ -224,7 +226,11 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
                                        **ipcStore, config::MAX_CFDP_FUNNEL_QUEUE_DEPTH, sdcMan,
                                        config::CFDP_SEQUENCE_COUNT_FILE,
                                        core::SAVE_CFDP_SEQUENCE_COUNT);
-  *cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, stores.cfdpStore->getReportReceptionQueue(0),
+  std::optional<MessageQueueId_t> fileStoreDest {};
+  if(routeToPersistentStores) {
+    fileStoreDest = stores.cfdpStore->getReportReceptionQueue(0);
+  }
+  *cfdpFunnel = new CfdpTmFunnel(cfdpFunnelCfg, fileStoreDest,
                                  *ramToFileStore, config::EIVE_CFDP_APID);
 
 #if OBSW_ADD_TCPIP_SERVERS == 1
diff --git a/mission/genericFactory.h b/mission/genericFactory.h
index 7845c140..f6c2bf1b 100644
--- a/mission/genericFactory.h
+++ b/mission/genericFactory.h
@@ -46,7 +46,7 @@ void produceGenericObjects(HealthTableIF** healthTable, PusTmFunnel** pusFunnel,
                            CfdpTmFunnel** cfdpFunnel, SdCardMountedIF& sdcMan,
                            StorageManagerIF** ipcStore, StorageManagerIF** tmStore,
                            PersistentTmStores& stores, uint32_t eventManagerQueueDepth,
-                           bool enableHkSets);
+                           bool enableHkSets, bool routeToPersistentStores);
 void createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF& pwrSwitcher,
                                    HeaterHandler*& heaterHandler);
 
diff --git a/mission/tmtc/CfdpTmFunnel.cpp b/mission/tmtc/CfdpTmFunnel.cpp
index 93c294a4..b5408a49 100644
--- a/mission/tmtc/CfdpTmFunnel.cpp
+++ b/mission/tmtc/CfdpTmFunnel.cpp
@@ -4,7 +4,7 @@
 #include "fsfw/tmtcpacket/ccsds/SpacePacketCreator.h"
 #include "fsfw/tmtcservices/TmTcMessage.h"
 
-CfdpTmFunnel::CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, MessageQueueId_t fileStoreDest,
+CfdpTmFunnel::CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, std::optional<MessageQueueId_t> fileStoreDest,
                            StorageManagerIF& ramToFileStore, uint16_t cfdpInCcsdsApid)
     : TmFunnelBase(cfg),
       fileStoreDest(fileStoreDest),
@@ -25,7 +25,9 @@ ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) {
     saveSequenceCount = false;
   }
   status = tmQueue->receiveMessage(&currentMessage);
+  uint32_t handledPackets = 0;
   while (status == returnvalue::OK) {
+    handledPackets++;
     status = handlePacket(currentMessage);
     if (status != returnvalue::OK) {
       sif::warning << "CfdpTmFunnel packet handling failed" << std::endl;
@@ -38,6 +40,10 @@ ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) {
     }
     status = tmQueue->receiveMessage(&currentMessage);
   }
+  if (handledPackets > 0) {
+    // Very useful for profiling and debugging
+    //sif::debug << "CfdpFunnel: Handled " << handledPackets << " packets in one cycle" << std::endl;
+  }
 
   if (status == MessageQueueIF::EMPTY) {
     return returnvalue::OK;
@@ -88,14 +94,16 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
   msg.setStorageId(newStoreId);
   store_address_t origStoreId = newStoreId;
 
-  store_address_t storageId;
-  result = ramToFileStore.addData(&storageId, newPacketData, packetLen);
-  TmTcMessage fileMsg(storageId);
-  if (result != returnvalue::OK) {
-    sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" << std::endl;
-  } else {
-    tmQueue->sendMessage(fileStoreDest, &fileMsg);
-  }
+  if(fileStoreDest.has_value()) {
+    store_address_t storageId;
+    result = ramToFileStore.addData(&storageId, newPacketData, packetLen);
+    TmTcMessage fileMsg(storageId);
+    if (result == returnvalue::OK) {
+      tmQueue->sendMessage(fileStoreDest.value(), &fileMsg);
+    } else if(result == StorageManagerIF::DATA_STORAGE_FULL) {
+      sif::error << "CfdpTmFunnel::handlePacket: RAM to File Store too full to create data copy" << std::endl;
+    }
 
+  }
   return demultiplexLivePackets(origStoreId, newPacketData, packetLen);
 }
diff --git a/mission/tmtc/CfdpTmFunnel.h b/mission/tmtc/CfdpTmFunnel.h
index 7b9efd34..5bb5d95b 100644
--- a/mission/tmtc/CfdpTmFunnel.h
+++ b/mission/tmtc/CfdpTmFunnel.h
@@ -13,7 +13,7 @@
 
 class CfdpTmFunnel : public TmFunnelBase {
  public:
-  CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, MessageQueueId_t fileStoreDest,
+  CfdpTmFunnel(TmFunnelBase::FunnelCfg cfg, std::optional<MessageQueueId_t> fileStoreDest,
                StorageManagerIF& ramToFileStore, uint16_t cfdpInCcsdsApid);
   [[nodiscard]] const char* getName() const override;
   ReturnValue_t performOperation(uint8_t opCode);
@@ -22,7 +22,7 @@ class CfdpTmFunnel : public TmFunnelBase {
  private:
   ReturnValue_t handlePacket(TmTcMessage& msg);
 
-  MessageQueueId_t fileStoreDest;
+  std::optional<MessageQueueId_t> fileStoreDest;
   StorageManagerIF& ramToFileStore;
   uint16_t cfdpInCcsdsApid;
 };
diff --git a/mission/tmtc/PusLiveDemux.cpp b/mission/tmtc/PusLiveDemux.cpp
index 6e9899d4..23d20eff 100644
--- a/mission/tmtc/PusLiveDemux.cpp
+++ b/mission/tmtc/PusLiveDemux.cpp
@@ -9,6 +9,7 @@ ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore,
                                                store_address_t origStoreId, const uint8_t* tmData,
                                                size_t tmSize) {
   ReturnValue_t result = returnvalue::OK;
+  // sif::debug << "tm size: " << tmSize  << " for " << destinations.size() << " destinations"  << std::endl;
   for (unsigned int idx = 0; idx < destinations.size(); idx++) {
     const auto& dest = destinations[idx];
     if ((destinations.size() > 1) and (idx < (destinations.size() - 1))) {
@@ -18,12 +19,22 @@ ReturnValue_t PusLiveDemux::demultiplexPackets(StorageManagerIF& tmStore,
       result = tmStore.addData(&storeId, tmData, tmSize);
       if (result == returnvalue::OK) {
         message.setStorageId(storeId);
-      } else {
+      } else if (result == StorageManagerIF::DATA_STORAGE_FULL) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" << std::endl;
+        uint8_t fillCounts[10];
+        uint8_t written = 0;
+        tmStore.getFillCount(fillCounts, &written);
+        sif::error << "Fill counts: ";
+        for(uint8_t fillIdx = 0; fillIdx < written; fillIdx++) {
+          sif::error << fillCounts[fillIdx];
+          if(fillIdx < written - 1) {
+            sif::error << ", ";
+          }
+        }
+        sif::error << std::endl;
 #endif
-        tmStore.deleteData(origStoreId);
-        break;
+        continue;
       }
     } else {
       message.setStorageId(origStoreId);
diff --git a/tmtc b/tmtc
index 1b0a7aea..649deac8 160000
--- a/tmtc
+++ b/tmtc
@@ -1 +1 @@
-Subproject commit 1b0a7aeabda128cb6ae1092541f100df288d38b1
+Subproject commit 649deac81bbc59d70f0cd1f2e442f434d089bbef