sequence counter isn ow persistent, msg type count support

mission/tmtc/CfdpTmFunnel.cpp

@@ -15,8 +15,16 @@ const char* CfdpTmFunnel::getName() const { return "CFDP TM Funnel"; }
 
 ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) {
   TmTcMessage currentMessage;
+  ReturnValue_t status;
   unsigned int count = 0;
-  ReturnValue_t status = tmQueue->receiveMessage(&currentMessage);
+  if (saveSequenceCount) {
+    status = saveSequenceCountToFile();
+    if (status != returnvalue::OK) {
+      sif::error << "CfdpTmFunnel: Storing sequence count to file has failed" << std::endl;
+    }
+    saveSequenceCount = false;
+  }
+  status = tmQueue->receiveMessage(&currentMessage);
   while (status == returnvalue::OK) {
     status = handlePacket(currentMessage);
     if (status != returnvalue::OK) {

mission/tmtc/CfdpTmFunnel.h

@@ -3,6 +3,7 @@
 
 #include <mission/tmtc/TmFunnelBase.h>
 
+#include <atomic>
 #include <vector>
 
 #include "fsfw/objectmanager/SystemObject.h"
@@ -23,7 +24,6 @@ class CfdpTmFunnel : public TmFunnelBase {
 
   MessageQueueId_t fileStoreDest;
   StorageManagerIF& ramToFileStore;
-  uint16_t sourceSequenceCount = 0;
   uint16_t cfdpInCcsdsApid;
 };
 #endif  // FSFW_EXAMPLE_COMMON_CFDPTMFUNNEL_H

mission/tmtc/PusTmFunnel.cpp

@@ -1,5 +1,7 @@
 #include "PusTmFunnel.h"
 
+#include <limits>
+
 #include "eive/definitions.h"
 #include "eive/objects.h"
 #include "fsfw/ipc/CommandMessage.h"
@@ -11,8 +13,8 @@
 #include "tmtc/pusIds.h"
 
 PusTmFunnel::PusTmFunnel(TmFunnelBase::FunnelCfg cfg, StorageManagerIF &ramToFileStore,
-                         TimeReaderIF &timeReader, SdCardMountedIF &sdcMan)
-    : TmFunnelBase(cfg), ramToFileStore(ramToFileStore), timeReader(timeReader), sdcMan(sdcMan) {}
+                         TimeReaderIF &timeReader)
+    : TmFunnelBase(cfg), ramToFileStore(ramToFileStore), timeReader(timeReader) {}
 
 PusTmFunnel::~PusTmFunnel() = default;
 
@@ -21,6 +23,13 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
   ReturnValue_t result;
   TmTcMessage currentMessage;
   unsigned int count = 0;
+  if (saveSequenceCount) {
+    result = saveSequenceCountToFile();
+    if (result != returnvalue::OK) {
+      sif::error << "PusTmFunnel: Storing sequence count to file has failed" << std::endl;
+    }
+    saveSequenceCount = false;
+  }
   result = tmQueue->receiveMessage(&currentMessage);
   while (result == returnvalue::OK) {
     result = handleTmPacket(currentMessage);
@@ -61,6 +70,17 @@ ReturnValue_t PusTmFunnel::handleTmPacket(TmTcMessage &message) {
   packet.setSequenceCount(sourceSequenceCount++);
   sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT;
   packet.updateErrorControl();
+  uint8_t service = packet.getMessageTypeCounter();
+  auto mapIter = msgCounterMap.find(service);
+  if (mapIter == msgCounterMap.end()) {
+    msgCounterMap.emplace(service, 0);
+  }
+  if (mapIter->second == std::numeric_limits<uint16_t>::max()) {
+    mapIter->second = 0;
+  } else {
+    mapIter->second++;
+  }
+  packet.setMessageCount(mapIter->second);
 
   // Send to persistent TM store if the packet matches some filter.
   MessageQueueId_t destination;

mission/tmtc/PusTmFunnel.h

@@ -9,6 +9,8 @@
 #include <mission/tmtc/PusTmRouteByFilterHelper.h>
 #include <mission/tmtc/TmFunnelBase.h>
 
+#include <atomic>
+#include <map>
 #include <vector>
 
 #include "PersistentTmStore.h"
@@ -25,7 +27,7 @@
 class PusTmFunnel : public TmFunnelBase {
  public:
   PusTmFunnel(TmFunnelBase::FunnelCfg cfg, StorageManagerIF &ramToFileStore,
-              TimeReaderIF &timeReader, SdCardMountedIF &sdcMan);
+              TimeReaderIF &timeReader);
   [[nodiscard]] const char *getName() const override;
   ~PusTmFunnel() override;
 
@@ -36,11 +38,10 @@ class PusTmFunnel : public TmFunnelBase {
   // Update TV stamp every 5 minutes
   static constexpr dur_millis_t TV_UPDATE_INTERVAL_SECS = 60 * 5;
 
-  uint16_t sourceSequenceCount = 0;
+  std::map<uint8_t, uint16_t> msgCounterMap;
   StorageManagerIF &ramToFileStore;
   TimeReaderIF &timeReader;
   bool storesInitialized = false;
-  SdCardMountedIF &sdcMan;
   PusTmRouteByFilterHelper persistentTmMap;
 
   ReturnValue_t handleTmPacket(TmTcMessage &message);

mission/tmtc/TmFunnelBase.cpp

@@ -2,6 +2,10 @@
 
 #include <fsfw/tmtcservices/TmTcMessage.h>
 
+#include <atomic>
+#include <filesystem>
+#include <fstream>
+
 #include "fsfw/ipc/QueueFactory.h"
 
 TmFunnelBase::TmFunnelBase(FunnelCfg cfg)
@@ -10,7 +14,10 @@ TmFunnelBase::TmFunnelBase(FunnelCfg cfg)
       tmStore(cfg.tmStore),
       ipcStore(cfg.ipcStore),
       tmQueue(QueueFactory::instance()->createMessageQueue(cfg.tmMsgDepth)),
-      liveDemux(*tmQueue) {}
+      liveDemux(*tmQueue),
+      sdcMan(cfg.sdcMan),
+      sequenceCounterFilename(cfg.sequenceCounterFilename),
+      saveSequenceCount(cfg.saveSequenceCount) {}
 
 ReturnValue_t TmFunnelBase::demultiplexLivePackets(store_address_t origStoreId,
                                                    const uint8_t *tmData, size_t tmSize) {
@@ -27,3 +34,38 @@ void TmFunnelBase::addLiveDestination(const char *name,
                                       const AcceptsTelemetryIF &downlinkDestination, uint8_t vcid) {
   liveDemux.addDestination(name, downlinkDestination, vcid);
 }
+
+ReturnValue_t TmFunnelBase::initialize() {
+  using namespace std::filesystem;
+  // The filesystem should always be available at the start.. Let's assume it always is, otherwise
+  // we just live with a regular 0 initialization. It simplifies a lot.
+  std::error_code e;
+  path filePath =
+      path(sdcMan.getCurrentMountPrefix() / std::string("conf") / sequenceCounterFilename);
+  if (exists(filePath, e)) {
+    std::ifstream ifile(filePath);
+    if (ifile.bad()) {
+      sif::error << "TmFunnelBase::initialize: Faulty file open for sequence counter initialization"
+                 << std::endl;
+      return returnvalue::OK;
+    }
+    if (not(ifile >> sourceSequenceCount)) {
+      // Initialize to 0 in any case if reading a number failed.
+      sourceSequenceCount = 0;
+    }
+  }
+  return returnvalue::OK;
+}
+
+ReturnValue_t TmFunnelBase::saveSequenceCountToFile() {
+  using namespace std::filesystem;
+  std::error_code e;
+  path filePath =
+      path(sdcMan.getCurrentMountPrefix() / std::string("conf") / sequenceCounterFilename);
+  std::ofstream ofile(filePath);
+  if (ofile.bad()) {
+    return returnvalue::FAILED;
+  }
+  ofile << sourceSequenceCount << "\n";
+  return returnvalue::OK;
+}

mission/tmtc/TmFunnelBase.h

@@ -6,25 +6,34 @@
 #include <fsfw/tmstorage/TmStoreFrontendSimpleIF.h>
 #include <fsfw/tmtcservices/AcceptsTelemetryIF.h>
 #include <fsfw/tmtcservices/TmTcMessage.h>
+#include <mission/memory/SdCardMountedIF.h>
 #include <mission/tmtc/PusLiveDemux.h>
 
+#include <atomic>
 #include <vector>
 
 class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject {
  public:
   struct FunnelCfg {
     FunnelCfg(object_id_t objId, const char* name, StorageManagerIF& tmStore,
-              StorageManagerIF& ipcStore, uint32_t tmMsgDepth)
+              StorageManagerIF& ipcStore, uint32_t tmMsgDepth, SdCardMountedIF& sdcMan,
+              const char* sequenceCounterFilename, std::atomic_bool& saveSequenceCount)
         : objectId(objId),
           name(name),
           tmStore(tmStore),
           ipcStore(ipcStore),
-          tmMsgDepth(tmMsgDepth) {}
+          tmMsgDepth(tmMsgDepth),
+          sdcMan(sdcMan),
+          sequenceCounterFilename(sequenceCounterFilename),
+          saveSequenceCount(saveSequenceCount) {}
     object_id_t objectId;
     const char* name;
     StorageManagerIF& tmStore;
     StorageManagerIF& ipcStore;
     uint32_t tmMsgDepth;
+    SdCardMountedIF& sdcMan;
+    const char* sequenceCounterFilename;
+    std::atomic_bool& saveSequenceCount;
   };
   explicit TmFunnelBase(FunnelCfg cfg);
   [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override;
@@ -32,6 +41,9 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject {
                           uint8_t vcid = 0);
   ReturnValue_t demultiplexLivePackets(store_address_t origStoreId, const uint8_t* tmData,
                                        size_t tmSize);
+  ReturnValue_t initialize() override;
+
+  ReturnValue_t saveSequenceCountToFile();
 
   ~TmFunnelBase() override;
 
@@ -41,6 +53,10 @@ class TmFunnelBase : public AcceptsTelemetryIF, public SystemObject {
   StorageManagerIF& ipcStore;
   MessageQueueIF* tmQueue = nullptr;
   PusLiveDemux liveDemux;
+  SdCardMountedIF& sdcMan;
+  const char* sequenceCounterFilename;
+  std::atomic_bool& saveSequenceCount;
+  uint16_t sourceSequenceCount = 0;
 };
 
 #endif /* MISSION_TMTC_TMFUNNELBASE_H_ */