fix: do not mix an epoch offset into the FreeRTOS monotonic clock (#74)

`Clock::getClockMonotonic` on FreeRTOS returned an epoch offset added on top of the uptime:

```cpp
*time = Timekeeper::instance()->getMonotonicClockOffset() + getUptime();
```

`monotonicClockOffset` is a `timeval` member with no initializer, and `Timekeeper`'s constructor
only initialises `offset`. The singleton is heap allocated, so the member holds indeterminate
bytes until `Timekeeper::setOffset` latches it, which happens on the first `Clock::setClock`.

Until that point `getClockMonotonic` returns garbage plus uptime, and at the latch it jumps by
whatever the difference happens to be. Any `Countdown` or `Stopwatch` armed before the latch
expires wrongly, in both directions:

- forward jump: `getCurrentTime() - startTime >= timeout`
- backward jump: the `getCurrentTime() < startTime` guard in `Countdown::hasTimedOut`

On our OBC this is not a race but the normal case. The CoreController sets the clock from its
own task, so everything constructed during object creation and early boot is on the wrong side
of the latch.

## Why the offset can go

The monotonicity came from the uptime alone. The offset only made the value look epoch based,
and nothing depends on that:

- `Countdown` uses `getCurrentTime() - startTime`
- `Stopwatch` uses `endTime - startTime`
- `PeriodicHelper::performPeriodicHkGeneration` uses `now - setSpec.lastGenerated`

Those are the only three callers in the framework. All take differences.

The Linux and host implementations already return `CLOCK_MONOTONIC_RAW` with no offset, so
FreeRTOS was the only OSAL where `getClockMonotonic` meant something different. The interface
doc in `Clock.h` also already describes the intended contract: "less suited when the absolute
time is required", with `CLOCK_MONOTONIC_RAW` named as the reference implementation.

## Changes

- `osal/freertos/Clock.cpp`: `getClockMonotonic` returns `getUptime()`
- `osal/freertos/Timekeeper.{h,cpp}`: drop `monotonicClockOffset`, `monotonicClockInitialized`,
  `getMonotonicClockOffset` and the never defined `setMonotonicClockOffset`. `setOffset` is now
  a one liner
- `timemanager/Clock.h`: drop the `monotonicClockInitialized` and `monotonicClockOffset` statics,
  which were declared but never defined or used

`Clock::getClock` is unchanged and still returns offset plus uptime, so the wall clock is
unaffected.

## Compatibility

`getClockMonotonic` on FreeRTOS now counts from scheduler start instead of the epoch. Code that
compares a monotonic timestamp against a wall clock value would break, but that would already be
broken on Linux, and no such code exists.

---------

Co-authored-by: Tobias Baumgartl <tobias.baumgartl@ksat-stuttgart.de>
Reviewed-on: #74

CHANGELOG.md

@@ -26,6 +26,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Added
 
+- functions to configure pus routing
 - FreeRTOS monotonic clock which is not subjected to time jumps of the system clock
 - add CFDP subsystem ID
   https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/742
@@ -35,6 +36,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 ## Changed
 
+- send HK one-parameter-report back to sender instead of default hk queue
 - Complete overhaul of HK subsystem. Replaced local data pool manager by periodic HK
   helper. The shared pool and the periodic HK generation are now distinct concepts.
 - The local HK manager was replaced by a periodic HK helper which has reduced responsibilities.
@@ -68,7 +70,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
   configurable.
 - Switched to vendored versions for both the Embedded Template Library (ETL) and the
   Catch2 unittesting library.
-
+- Increased maximum number of mode tables from 70 to 100
+- Exposed health table mutex via getter function
 ## Added
 
 - `EventManager`: Add function to print all listeners.

misc/defaultcfg/fsfwconfig/FSFWConfig.h

@@ -71,6 +71,7 @@ static constexpr size_t FSFW_EVENTMGMR_RANGEMATCHERS   = 120;
 static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 6;
 
 static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 124;
+static constexpr size_t FSFW_PRINT_BUFFER_AMOUNT = 32;
 
 static constexpr size_t FSFW_MAX_TM_PACKET_SIZE = 2048;
 

scripts/auto-formatter.sh

@@ -18,12 +18,12 @@ else
   echo "No ${cmake_fmt} tool found, not formatting CMake files"
 fi
 
-cpp_format="clang-format"
+cpp_format="clang-format-19"
 file_selectors="-iname *.h -o -iname *.cpp -o -iname *.c -o -iname *.tpp"
 if command -v ${cpp_format} &> /dev/null; then
   for dir in ${folder_list[@]}; do
     echo "Auto-formatting ${dir} recursively"
-    find ${dir} ${file_selectors} | xargs clang-format --style=file -i
+    find ${dir} ${file_selectors} | xargs ${cpp_format} --style=file -i
   done
 else
   echo "No ${cpp_format} tool found, not formatting C++/C files"

src/fsfw/cfdp/handler/DestHandler.cpp

@@ -9,7 +9,9 @@
 #include "fsfw/cfdp/pdu/FileDataReader.h"
 #include "fsfw/cfdp/pdu/FinishedPduCreator.h"
 #include "fsfw/cfdp/pdu/HeaderReader.h"
+#include "fsfw/cfdp/pdu/KeepAlivePduCreator.h"
 #include "fsfw/objectmanager.h"
+#include "fsfw/returnvalues/returnvalue.h"
 #include "fsfw/tmtcservices/TmTcMessage.h"
 
 using namespace returnvalue;
@@ -450,6 +452,19 @@ ReturnValue_t cfdp::DestHandler::noticeOfCompletion() {
   return OK;
 }
 
+ReturnValue_t cfdp::DestHandler::sendKeepAlivePdu() {
+  Fss progress(transactionParams.progress);
+  KeepAlivePduCreator keepAlivePdu(transactionParams.pduConf, progress);
+  size_t serLen = 0;
+  ReturnValue_t result =
+      keepAlivePdu.serialize(pduBuf.data(), serLen, keepAlivePdu.getSerializedSize());
+  if (result != OK) {
+    return result;
+  }
+  return pduSender.sendPdu(PduType::FILE_DIRECTIVE, FileDirective::KEEP_ALIVE, pduBuf.data(),
+                           serLen);
+}
+
 ReturnValue_t cfdp::DestHandler::sendFinishedPdu() {
   FinishedInfo info(transactionParams.conditionCode, transactionParams.deliveryCode,
                     transactionParams.deliveryStatus);
@@ -496,6 +511,8 @@ const cfdp::TransactionId& cfdp::DestHandler::getTransactionId() const {
   return transactionParams.transactionId;
 }
 
+uint64_t cfdp::DestHandler::getProgress() const { return transactionParams.progress; }
+
 void cfdp::DestHandler::checkAndHandleError(ReturnValue_t result, uint8_t& errorIdx) {
   if (result != OK and errorIdx < 3) {
     fsmRes.errorCodes[errorIdx] = result;
@@ -509,4 +526,4 @@ void cfdp::DestHandler::setEventReporter(EventReportingProxyIF& reporter) {
 
 const cfdp::DestHandlerParams& cfdp::DestHandler::getDestHandlerParams() const {
   return destParams;
-}
+}

src/fsfw/cfdp/handler/DestHandler.h

@@ -101,6 +101,8 @@ class DestHandler {
 
   [[nodiscard]] CfdpState getCfdpState() const;
   [[nodiscard]] TransactionStep getTransactionStep() const;
+  [[nodiscard]] uint64_t getProgress() const;
+  ReturnValue_t sendKeepAlivePdu();
   [[nodiscard]] const TransactionId& getTransactionId() const;
   [[nodiscard]] const DestHandlerParams& getDestHandlerParams() const;
 

src/fsfw/cfdp/pdu/KeepAlivePduCreator.h

@@ -14,6 +14,7 @@ class KeepAlivePduCreator : public FileDirectiveCreator {
 
   ReturnValue_t serialize(uint8_t** buffer, size_t* size, size_t maxSize,
                           Endianness streamEndianness) const override;
+  using FileDirectiveCreator::serialize;
 
  private:
   cfdp::Fss& progress;

src/fsfw/devicehandlers/AssemblyBase.cpp

@@ -71,7 +71,8 @@ bool AssemblyBase::handleChildrenChangedHealth() {
   if (iter == childrenMap.end()) {
     return false;
   }
-  HealthState healthState = healthHelper.healthTable->getHealth(iter->first);
+  HealthState healthState =
+      healthHelper.healthTable->getHealth(convertToDeviceObjectId(iter->first));
   if (healthState == HasHealthIF::NEEDS_RECOVERY) {
     triggerEvent(TRYING_RECOVERY, iter->first, 0);
     recoveryState = RECOVERY_STARTED;
@@ -91,10 +92,14 @@ bool AssemblyBase::handleChildrenChangedHealth() {
 void AssemblyBase::handleChildrenTransition() {
   if (commandsOutstanding <= 0) {
     switch (internalState) {
-      case STATE_NEED_SECOND_STEP:
+      case STATE_NEED_SECOND_STEP: {
         internalState = STATE_SECOND_STEP;
-        commandChildren(targetMode, targetSubmode);
+        ReturnValue_t result = commandChildren(targetMode, targetSubmode);
+        if (result == NEED_SECOND_STEP) {
+          internalState = STATE_NEED_SECOND_STEP;
+        }
         return;
+      }
       case STATE_OVERWRITE_HEALTH: {
         internalState = STATE_SINGLE_STEP;
         ReturnValue_t result = commandChildren(mode, submode);
@@ -170,7 +175,7 @@ ReturnValue_t AssemblyBase::checkChildrenStateOff() {
 
 ReturnValue_t AssemblyBase::checkChildOff(uint32_t objectId) {
   ChildInfo childInfo = childrenMap.find(objectId)->second;
-  if (healthHelper.healthTable->isCommandable(objectId)) {
+  if (healthHelper.healthTable->isCommandable(convertToDeviceObjectId(objectId))) {
     if (childInfo.submode != SUBMODE_NONE) {
       return returnvalue::FAILED;
     } else {
@@ -227,7 +232,7 @@ bool AssemblyBase::checkAndHandleRecovery() {
     case RECOVERY_STARTED:
       // The recovery was already start in #handleChildrenChangedHealth and we just need
       // to wait for an off time period.
-      // TODO: make time period configurable
+      // The timeout can be defined by #setRecoveryWaitTimer
       recoveryState = RECOVERY_WAIT;
       recoveryOffTimer.resetTimer();
       return true;
@@ -235,14 +240,14 @@ bool AssemblyBase::checkAndHandleRecovery() {
       if (recoveryOffTimer.isBusy()) {
         return true;
       }
-      triggerEvent(RECOVERY_STEP, 0);
+      triggerEvent(RECOVERY_WAITING, recoveringDevice->first);
       sendHealthCommand(recoveringDevice->second.commandQueue, HEALTHY);
       internalState = STATE_NONE;
       recoveryState = RECOVERY_ONGOING;
       // Don't check state!
       return true;
     case RECOVERY_ONGOING:
-      triggerEvent(RECOVERY_STEP, 1);
+      triggerEvent(RECOVERY_RESTARTING, recoveringDevice->first);
       recoveryState = RECOVERY_ONGOING_2;
       recoveringDevice->second.healthChanged = false;
       // Device should be healthy again, so restart a transition.
@@ -250,7 +255,7 @@ bool AssemblyBase::checkAndHandleRecovery() {
       doStartTransition(targetMode, targetSubmode);
       return true;
     case RECOVERY_ONGOING_2:
-      triggerEvent(RECOVERY_DONE);
+      triggerEvent(RECOVERY_DONE, recoveringDevice->first);
       // Now we're through, but not sure if it was successful.
       recoveryState = RECOVERY_IDLE;
       return false;
@@ -264,7 +269,14 @@ void AssemblyBase::overwriteDeviceHealth(object_id_t objectId, HasHealthIF::Heal
   triggerEvent(OVERWRITING_HEALTH, objectId, oldHealth);
   internalState = STATE_OVERWRITE_HEALTH;
   modeHelper.setForced(true);
-  sendHealthCommand(childrenMap[objectId].commandQueue, EXTERNAL_CONTROL);
+  if (childrenMap.find(objectId) != childrenMap.end()) {
+    sendHealthCommand(childrenMap.at(objectId).commandQueue, EXTERNAL_CONTROL);
+  } else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::debug << std::hex << SystemObject::getObjectId() << ": invalid mode table entry"
+               << std::endl;
+#endif
+  }
 }
 
 void AssemblyBase::triggerModeHelperEvents(Mode_t mode, Submode_t submode) {
@@ -274,3 +286,7 @@ void AssemblyBase::triggerModeHelperEvents(Mode_t mode, Submode_t submode) {
     triggerEvent(CHANGING_MODE, mode, submode);
   }
 }
+
+void AssemblyBase::setRecoveryWaitTimer(uint32_t timeoutMS) {
+  recoveryOffTimer.setTimeout(timeoutMS);
+}

src/fsfw/devicehandlers/AssemblyBase.h

@@ -206,6 +206,8 @@ class AssemblyBase : public SubsystemBase {
   void overwriteDeviceHealth(object_id_t objectId, HasHealthIF::HealthState oldHealth);
 
   void triggerModeHelperEvents(Mode_t mode, Submode_t submode);
+
+  void setRecoveryWaitTimer(uint32_t timeoutMS);
 };
 
 #endif /* FSFW_DEVICEHANDLERS_ASSEMBLYBASE_H_ */

src/fsfw/devicehandlers/DeviceHandlerBase.cpp

@@ -1268,7 +1268,7 @@ ReturnValue_t DeviceHandlerBase::letChildHandleMessage(CommandMessage* message)
 
 void DeviceHandlerBase::handleDeviceTm(const uint8_t* rawData, size_t rawDataLen,
                                        DeviceCommandId_t replyId, bool forceDirectTm) {
-  SerialBufferAdapter bufferWrapper(rawData, rawDataLen);
+  SerialBufferAdapter<uint32_t> bufferWrapper(rawData, rawDataLen);
   handleDeviceTm(bufferWrapper, replyId, forceDirectTm);
 }
 

src/fsfw/devicehandlers/FreshDeviceHandlerBase.cpp

@@ -190,6 +190,4 @@ ReturnValue_t FreshDeviceHandlerBase::getParameter(uint8_t domainId, uint8_t uni
   return INVALID_DOMAIN_ID;
 }
 
-datapool::SharedPool* FreshDeviceHandlerBase::getOptionalSharedPool() { return nullptr; }
-
-ModeHelper& FreshDeviceHandlerBase::getModeHelper() { return this->modeHelper; }
+datapool::SharedPool* FreshDeviceHandlerBase::getOptionalSharedPool() { return nullptr; }

src/fsfw/devicehandlers/FreshDeviceHandlerBase.h

@@ -97,11 +97,6 @@ class FreshDeviceHandlerBase : public SystemObject,
   ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) override;
   ModeTreeChildIF& getModeTreeChildIF() override;
 
-  /**
-   * @brief Return an interface to the ModeHelper.
-   */
-  ModeHelper& getModeHelper();
-
  protected:
   ActionHelper actionHelper;
   ModeHelper modeHelper;

src/fsfw/events/Event.h

@@ -22,7 +22,7 @@ constexpr EventId_t getEventId(Event event) { return (event & 0xFFFF); }
 
 constexpr EventSeverity_t getSeverity(Event event) { return ((event >> 16) & 0xFF); }
 
-template<uint8_t subsystemId, UniqueEventId_t uniqueEventId, EventSeverity_t eventSeverity>
+template <uint8_t subsystemId, UniqueEventId_t uniqueEventId, EventSeverity_t eventSeverity>
 constexpr Event makeEvent() {
   static_assert(uniqueEventId < 100, "The unique event ID must be smaller than 100!");
   return (eventSeverity << 16) + (subsystemId * 100) + uniqueEventId;

src/fsfw/globalfunctions/CMakeLists.txt

@@ -2,6 +2,7 @@ target_sources(
   ${LIB_FSFW_NAME}
   PRIVATE arrayprinter.cpp
           AsciiConverter.cpp
+          CobsEncoder.cpp
           CRC.cpp
           DleEncoder.cpp
           DleParser.cpp

src/fsfw/globalfunctions/CobsEncoder.cpp

@@ -0,0 +1,78 @@
+#include "fsfw/globalfunctions/CobsEncoder.h"
+
+#include <cstddef>
+#include <cstring>
+
+ReturnValue_t CobsEncoder::encode(const uint8_t* sourceStream, size_t sourceLen,
+                                  uint8_t* destStream, size_t maxDestLen, size_t* encodedLen) {
+  *encodedLen = 0;
+
+  if (maxDestLen < worstCaseEncodedLen(sourceLen)) return INSUFFICIENT_SPACE;
+
+  size_t codeIdx = 0;
+  size_t outIdx = 1;
+  uint8_t code = 1;
+
+  for (size_t i = 0; i < sourceLen; ++i) {
+    const auto isZero = sourceStream[i] == 0x00;
+
+    if (not isZero) {
+      destStream[outIdx++] = sourceStream[i];
+      code += 1;
+    }
+
+    if (isZero or code == CobsEncoder::MAX_BLOCK_LEN + 1) {  // + delimiter
+      destStream[codeIdx] = code;
+      codeIdx = outIdx++;
+      code = 1;
+    }
+  }
+
+  destStream[codeIdx] = code;
+  destStream[outIdx++] = 0x00;
+
+  *encodedLen = outIdx;
+  return returnvalue::OK;
+}
+
+ReturnValue_t CobsEncoder::decode(const uint8_t* sourceStream, size_t sourceLen, size_t* readLen,
+                                  uint8_t* destStream, size_t maxDestLen, size_t* decodedLen) {
+  *readLen = 0;
+  *decodedLen = 0;
+
+  if (sourceLen == 0) return NO_DATA_AVAILABLE;
+
+  // COBS, so 0 byte never occures. Used as delimiters.
+  const auto* delimiter = static_cast<const uint8_t*>(std::memchr(sourceStream, 0x00, sourceLen));
+  if (delimiter == nullptr) return STREAM_TOO_SHORT;
+
+  const size_t frameLen = delimiter - sourceStream;
+  const size_t consumedLen = frameLen + 1;  // + delimiter
+
+  size_t inIdx = 0;
+  size_t outIdx = 0;
+
+  while (inIdx < frameLen) {
+    const size_t blockLen = sourceStream[inIdx++] - 1;  // COBS, so for all bytes > 0
+    const auto isFullBlock = blockLen == MAX_BLOCK_LEN;
+
+    // COBS data block is bogus => bad data (set readLen != 0)
+    if (inIdx + blockLen > frameLen) return *readLen = consumedLen, DECODING_ERROR;
+    // Data block won't fit into provided buffer
+    if (maxDestLen < outIdx + blockLen) return INSUFFICIENT_SPACE;
+
+    std::memcpy(destStream + outIdx, sourceStream + inIdx, blockLen);
+    inIdx += blockLen;
+    outIdx += blockLen;
+
+    // COBS: != 0xFF, then meant to be zero
+    if (not isFullBlock and inIdx < frameLen) {
+      if (maxDestLen < outIdx + 1) return INSUFFICIENT_SPACE;
+      destStream[outIdx++] = 0x00;
+    }
+  }
+
+  *readLen = consumedLen;
+  *decodedLen = outIdx;
+  return returnvalue::OK;
+}

src/fsfw/globalfunctions/CobsEncoder.h

@@ -0,0 +1,113 @@
+#ifndef FSFW_GLOBALFUNCTIONS_COBSENCODER_H_
+#define FSFW_GLOBALFUNCTIONS_COBSENCODER_H_
+
+#include <cstddef>
+#include <cstdint>
+
+#include "fsfw/returnvalues/returnvalue.h"
+
+/**
+ * @brief   This COBS Encoder (Consistent Overhead Byte Stuffing) can be used to encode and
+ *          decode arbitrary data.
+ *
+ * @details
+ * Protocol information: https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing
+ *
+ * A COBS frame contains no zero bytes but is terminated by one, so frames can be picked out of a
+ * byte stream by looking for that delimiter.
+ */
+class CobsEncoder {
+ public:
+  CobsEncoder() = delete;
+  virtual ~CobsEncoder() = delete;
+
+  static constexpr uint8_t INTERFACE_ID = CLASS_ID::COBS_ENCODER;
+
+  /** The source stream holds no frame delimiter yet, so the frame may still be arriving.
+   * Nothing was consumed and the caller should retry once more data has been received. */
+  static constexpr ReturnValue_t STREAM_TOO_SHORT = MAKE_RETURN_CODE(1);
+  /** The frame is delimited but its block structure is malformed, so it cannot be recovered.
+   * `readLen` skips past the whole frame so decoding can resynchronise on the next one. */
+  static constexpr ReturnValue_t DECODING_ERROR = MAKE_RETURN_CODE(2);
+  /** The input stream is empty. */
+  static constexpr ReturnValue_t NO_DATA_AVAILABLE = MAKE_RETURN_CODE(3);
+  /** The output buffer is not large enough to fit the input data. */
+  static constexpr ReturnValue_t INSUFFICIENT_SPACE = MAKE_RETURN_CODE(4);
+
+  /** Longest run of data bytes a single code byte can describe. */
+  static constexpr size_t MAX_BLOCK_LEN = 254;
+
+  /**
+   * Upper bound on the encoded size of `sourceLen` bytes, including the frame delimiter.
+   * Exact when the input holds no zero bytes and at most one byte per full block too large
+   * otherwise.
+   * Use this during compiletime to create a correctly sized output buffer.
+   * @param sourceLen Max length of buffer to encode
+   * @return Use as follows: `uint8_t destBuffer[worstCaseEncodedLen(srcBufferSize)];`.
+   */
+  static constexpr size_t worstCaseEncodedLen(size_t sourceLen) {
+    // Derivation:
+    // We know: Frame length = (data bytes) + (code bytes) + (delimiter)
+    // Let n be the source length, z the number of zero bytes in it and s the number of times a data
+    // block is over 254 and splits.
+    // We have (data bytes) = n - z, since only non-zero bytes get copied over.
+    // (code bytes) = 1 + z + s, since starting code, blocks end at zero and 254 splits.
+    // So, Frame length = (data bytes) + (code bytes) + (delimiter)
+    //                  = (n - z) + (1 + z + s) + 1 = n + s + 2
+    // s depends on the data, so we derive the upper bound, since we know these splits happen every
+    // 254 bytes (or less since maybe there are enough zeros spread out to not need a 254 split):
+    // s <= (n - z) / 254 <= n / 254. (z >= 0)
+    // Thus, n + s + 2 <= n + (n / 254) + 2
+    // Note: We do integer division (floor), since a 254 code only appears every FULL 254.
+    return sourceLen + sourceLen / MAX_BLOCK_LEN + 2;
+  }
+
+  /**
+   * Encodes the given data stream into COBS format
+   * @param sourceStream Start of the source buffer
+   * @param sourceLen Length of the source buffer
+   * @param destStream Destination buffer
+   * @param maxDestLen Maximum length of the destination buffer
+   * @param encodedLen Out pointer which is written with the actual amount of data written to the
+   * destination buffer.
+   * @return
+   *  - returnvalue::OK for successful encoding operation
+   *  - INSUFFICIENT_SPACE if `maxDestLen` is below `worstCaseEncodedLen(sourceLen)`.
+   *    Note: Technically smaller sizes would fit but `worstCaseEncodedLen` is easy to compute.
+   */
+  static ReturnValue_t encode(const uint8_t* sourceStream, size_t sourceLen, uint8_t* destStream,
+                              size_t maxDestLen, size_t* encodedLen);
+
+  /**
+   * Converts an encoded stream back from COBS format.
+   *
+   * This function only ever decodes a single COBS frame.
+   * To drain a stream, advance it by `readLen` and call again until `STREAM_TOO_SHORT` reports that
+   * no complete frame is left.
+   * Because `readLen` is also set for a corrupt frame, a damaged frame never blocks the intact ones
+   * queued behind it.
+   *
+   * An empty frame, meaning a delimiter with no data in front of it, decodes successfully with a
+   * `decodedLen` of zero.
+   * Callers that treat runs of delimiters as idle filler should ignore those.
+   * @param sourceStream Start of the source buffer
+   * @param sourceStreamLen Length of the source buffer
+   * @param readLen Out pointer which is written with the amount of data that was actually read from
+   *        the source buffer. Set on success and on DECODING_ERROR, both times covering the whole
+   *        frame including its delimiter. Left at zero otherwise.
+   * @param destStream Destination buffer
+   * @param maxDestStreamlen Maximum length of the destination buffer
+   * @param decodedLen Out pointer which is written with the actual amount of data written to the
+   *        destination buffer.
+   * @return
+   *  - returnvalue::OK for successful decode operation
+   *  - STREAM_TOO_SHORT if the source stream holds no complete frame yet
+   *  - DECODING_ERROR if the frame is delimited but malformed. Skip `readLen` bytes to resync
+   *  - INSUFFICIENT_SPACE if the destination buffer cannot hold the decoded frame
+   *  - NO_DATA_AVAILABLE if the source stream is empty
+   */
+  static ReturnValue_t decode(const uint8_t* sourceStream, size_t sourceStreamLen, size_t* readLen,
+                              uint8_t* destStream, size_t maxDestStreamlen, size_t* decodedLen);
+};
+
+#endif /* FSFW_GLOBALFUNCTIONS_COBSENCODER_H_ */

src/fsfw/health/HasHealthIF.h

@@ -27,13 +27,19 @@ class HasHealthIF {
   static const Event CHILD_PROBLEMS = MAKE_EVENT(8, severity::LOW);
   //! Assembly overwrites health information of children to keep satellite alive.
   static const Event OVERWRITING_HEALTH = MAKE_EVENT(9, severity::LOW);
-  //! Someone starts a recovery of a component (typically power-cycle). No parameters.
+  //! Someone starts a recovery of a component (typically power-cycle).
+  //! P1: Object Id of the recovering device.
   static const Event TRYING_RECOVERY = MAKE_EVENT(10, severity::MEDIUM);
-  //! Recovery is ongoing. Comes twice during recovery.
-  //! P1: 0 for the first, 1 for the second event. P2: 0
-  static const Event RECOVERY_STEP = MAKE_EVENT(11, severity::MEDIUM);
   //! Recovery was completed. Not necessarily successful. No parameters.
+  //! P1: Object Id of the recovering device.
   static const Event RECOVERY_DONE = MAKE_EVENT(12, severity::MEDIUM);
+  //! Recovery is ongoing. The recovering device is currently OFF, waiting for restart.
+  //! P1: Object Id of the recovering device.
+  static const Event RECOVERY_WAITING = MAKE_EVENT(13, severity::MEDIUM);
+  //! Recovery is ongoing. Restarting the recovering device.
+  //! P1: Object Id of the recovering device.
+  static const Event RECOVERY_RESTARTING = MAKE_EVENT(14, severity::MEDIUM);
+
   virtual ~HasHealthIF() {}
 
   virtual MessageQueueId_t getCommandQueue() const = 0;

src/fsfw/health/HealthTable.cpp

@@ -17,11 +17,11 @@ void HealthTable::setMutexTimeout(MutexIF::TimeoutType timeoutType, uint32_t tim
 HealthTable::~HealthTable() { MutexFactory::instance()->deleteMutex(mutex); }
 
 ReturnValue_t HealthTable::registerObject(object_id_t object,
-                                          HasHealthIF::HealthState initilialState) {
+                                          HasHealthIF::HealthState initialState) {
   if (healthMap.count(object) != 0) {
     return returnvalue::FAILED;
   }
-  healthMap.emplace(object, initilialState);
+  healthMap.emplace(object, initialState);
   return returnvalue::OK;
 }
 
@@ -112,3 +112,5 @@ ReturnValue_t HealthTable::iterate(HealthEntry* value, bool reset) {
   mapIterator++;
   return result;
 }
+
+MutexIF* HealthTable::getMutex() { return mutex; }

src/fsfw/health/HealthTable.h

@@ -18,7 +18,7 @@ class HealthTable : public HealthTableIF, public SystemObject {
 
   /** HealthTableIF overrides */
   virtual ReturnValue_t registerObject(
-      object_id_t object, HasHealthIF::HealthState initilialState = HasHealthIF::HEALTHY) override;
+      object_id_t object, HasHealthIF::HealthState initialState = HasHealthIF::HEALTHY) override;
   ReturnValue_t removeObject(object_id_t object) override;
   virtual size_t getPrintSize() override;
   virtual void printAll(uint8_t* pointer, size_t maxSize) override;
@@ -28,6 +28,8 @@ class HealthTable : public HealthTableIF, public SystemObject {
   virtual void setHealth(object_id_t object, HasHealthIF::HealthState newState) override;
   virtual HasHealthIF::HealthState getHealth(object_id_t) override;
 
+  MutexIF* getMutex();
+
  protected:
   using HealthMap = std::map<object_id_t, HasHealthIF::HealthState>;
   using HealthEntry = std::pair<object_id_t, HasHealthIF::HealthState>;

src/fsfw/health/HealthTableIF.h

@@ -12,7 +12,7 @@ class HealthTableIF : public ManagesHealthIF {
   virtual ~HealthTableIF() {}
 
   virtual ReturnValue_t registerObject(
-      object_id_t object, HasHealthIF::HealthState initilialState = HasHealthIF::HEALTHY) = 0;
+      object_id_t object, HasHealthIF::HealthState initialState = HasHealthIF::HEALTHY) = 0;
 
   virtual ReturnValue_t removeObject(object_id_t objectId) = 0;
 

src/fsfw/housekeeping/PeriodicHkHelper.cpp

@@ -9,7 +9,6 @@
 #include "fsfw/housekeeping/HousekeepingSnapshot.h"
 #include "fsfw/ipc/QueueFactory.h"
 #include "fsfw/objectmanager/ObjectManager.h"
-#include "fsfw/timemanager/CCSDSTime.h"
 
 using namespace hk;
 
@@ -84,6 +83,7 @@ ReturnValue_t PeriodicHelper::performHkOperation() {
 
 ReturnValue_t PeriodicHelper::handleHousekeepingMessage(CommandMessage* message) {
   Command_t command = message->getCommand();
+  MessageQueueId_t sender = message->getSender();
   dp::sid_t sid = HousekeepingMessage::getStructureId(message);
   ReturnValue_t result = returnvalue::OK;
   switch (command) {
@@ -113,7 +113,7 @@ ReturnValue_t PeriodicHelper::handleHousekeepingMessage(CommandMessage* message)
     }
 
     case (HousekeepingMessage::GENERATE_ONE_PARAMETER_REPORT): {
-      return generateHousekeepingPacket(HousekeepingMessage::getStructureId(message));
+      return generateHousekeepingPacket(HousekeepingMessage::getStructureId(message), sender);
     }
 
     default:

src/fsfw/ipc/MessageQueueBase.cpp

@@ -45,7 +45,7 @@ bool MessageQueueBase::isDefaultDestinationSet() const { return (defaultDest !=
 
 ReturnValue_t MessageQueueBase::sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
                                             bool ignoreFault) {
-  return sendMessageFrom(sendTo, message, this->getId(), false);
+  return sendMessageFrom(sendTo, message, this->getId(), ignoreFault);
 }
 
 ReturnValue_t MessageQueueBase::sendToDefaultFrom(MessageQueueMessageIF* message,

src/fsfw/ipc/MutexGuard.h

@@ -47,7 +47,10 @@ class MutexGuard {
   ReturnValue_t getLockResult() const { return result; }
 
   ~MutexGuard() {
-    if (internalMutex != nullptr) {
+    // Only unlock what was actually locked. Unlocking after a failed take gives away a mutex held
+    // by another task, which trips configASSERT(pxTCB == pxCurrentTCB) in FreeRTOS'
+    // xTaskPriorityDisinherit and halts the system.
+    if (internalMutex != nullptr and result == returnvalue::OK) {
       internalMutex->unlockMutex();
     }
   }

src/fsfw/modes/ModeHelper.cpp

@@ -35,12 +35,8 @@ ReturnValue_t ModeHelper::handleModeCommand(CommandMessage* command) {
       commandedMode = mode;
       commandedSubmode = submode;
 
-      // if (((parentQueueId != MessageQueueIF::NO_QUEUE) &&
-      //     (theOneWhoCommandedAMode != parentQueueId))) {
-      //   owner->setToExternalControl();
-      // }
-
-      if(theOneWhoCommandedAMode != parentQueueId and theOneWhoCommandedAMode != powerswitchQueueId) {
+      if ((parentQueueId != MessageQueueIF::NO_QUEUE) &&
+          (theOneWhoCommandedAMode != parentQueueId)) {
         owner->setToExternalControl();
       }
 
@@ -110,7 +106,3 @@ bool ModeHelper::isTimedOut() { return countdown.hasTimedOut(); }
 bool ModeHelper::isForced() { return forced; }
 
 void ModeHelper::setForced(bool forced) { this->forced = forced; }
-
-void ModeHelper::setPowerSwitchQueueId(MessageQueueId_t queueId) {
-  powerswitchQueueId = queueId;
-}

src/fsfw/modes/ModeHelper.h

@@ -21,15 +21,10 @@ class ModeHelper {
 
   /**
    * @param parentQueue the Queue id of the parent object.
-   * Set to MessageQueueIF::NO_QUEUE if no parent present
+   * Set to 0 if no parent present
    */
   void setParentQueue(MessageQueueId_t parentQueueId);
 
-  /**
-   * Set to MessageQueue::NO_QUEUE if no powerswitch is commanding the obejct.
-   */
-  void setPowerSwitchQueueId(MessageQueueId_t queueId);
-
   ReturnValue_t initialize(MessageQueueId_t parentQueueId);
 
   ReturnValue_t initialize(void);
@@ -47,7 +42,6 @@ class ModeHelper {
  protected:
   HasModesIF *owner;
   MessageQueueId_t parentQueueId = MessageQueueIF::NO_QUEUE;
-  MessageQueueId_t powerswitchQueueId = MessageQueueIF::NO_QUEUE;
 
   Countdown countdown;
 

src/fsfw/objectmanager/frameworkObjects.h

@@ -36,6 +36,8 @@ enum framework_objects : object_id_t {
   TIME_STAMPER = 0x53500010,
   VERIFICATION_REPORTER = 0x53500020,
 
+  SIF_PRINT_TASK = 0x53600000,
+
   FSFW_OBJECTS_END = 0x53ffffff,
   NO_OBJECT = 0xFFFFFFFF
 };

src/fsfw/osal/common/UdpTcPollingTask.cpp

@@ -41,7 +41,7 @@ UdpTcPollingTask::UdpTcPollingTask(object_id_t objectId, object_id_t tmtcUdpBrid
 
 [[noreturn]] ReturnValue_t UdpTcPollingTask::performOperation(uint8_t opCode) {
   /* Sender Address is cached here. */
-  struct sockaddr senderAddress {};
+  struct sockaddr senderAddress{};
   socklen_t senderAddressSize = sizeof(senderAddress);
 
   /* Poll for new UDP datagrams in permanent loop. */

src/fsfw/osal/freertos/Clock.cpp

@@ -5,8 +5,8 @@
 
 #include "FreeRTOS.h"
 #include "fsfw/globalfunctions/timevalOperations.h"
-#include "fsfw/serviceinterface/ServiceInterfacePrinter.h"
 #include "fsfw/osal/freertos/Timekeeper.h"
+#include "fsfw/serviceinterface/ServiceInterfacePrinter.h"
 #include "task.h"
 
 // TODO sanitize input?
@@ -48,7 +48,8 @@ ReturnValue_t Clock::getClock(timeval* time) {
 }
 
 ReturnValue_t Clock::getClockMonotonic(timeval* time) {
-	*time = Timekeeper::instance()->getMonotonicClockOffset() + getUptime();
+  // Retrieves the FreeRTOS tick count, which is hopefully monotonic
+  *time = getUptime();
   return returnvalue::OK;
 }
 

src/fsfw/osal/freertos/Timekeeper.cpp

@@ -17,13 +17,7 @@ Timekeeper* Timekeeper::instance() {
   return myinstance;
 }
 
-void Timekeeper::setOffset(const timeval& offset) {
-	if (not monotonicClockInitialized) {
-		this->monotonicClockOffset = offset;
-		monotonicClockInitialized = true;
-	}
-	this->offset = offset;
-}
+void Timekeeper::setOffset(const timeval& offset) { this->offset = offset; }
 
 timeval Timekeeper::ticksToTimeval(TickType_t ticks) {
   timeval uptime;
@@ -39,7 +33,3 @@ timeval Timekeeper::ticksToTimeval(TickType_t ticks) {
 }
 
 TickType_t Timekeeper::getTicks() { return xTaskGetTickCount(); }
-
-const timeval Timekeeper::getMonotonicClockOffset() const {
-	return monotonicClockOffset;
-}

src/fsfw/osal/freertos/Timekeeper.h

@@ -18,14 +18,9 @@ class Timekeeper {
   Timekeeper();
 
   timeval offset;
-  // Set when offset is initialized the first time
-  timeval monotonicClockOffset;
-  bool monotonicClockInitialized = false;
 
   static Timekeeper* myinstance;
 
-  void setMonotonicClockOffset(const timeval& monotonicClockOffset);
-
  public:
   static Timekeeper* instance();
   virtual ~Timekeeper();
@@ -39,7 +34,6 @@ class Timekeeper {
 
   const timeval& getOffset() const;
   void setOffset(const timeval& offset);
-  const timeval getMonotonicClockOffset() const;
 };
 
 #endif /* FRAMEWORK_OSAL_FREERTOS_TIMEKEEPER_H_ */

src/fsfw/osal/host/Clock.cpp

@@ -173,7 +173,7 @@ ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) {
 }
 
 ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from, timeval* to) {
-  struct tm time_tm {};
+  struct tm time_tm{};
 
   time_tm.tm_year = from->year - 1900;
   time_tm.tm_mon = from->month - 1;

src/fsfw/osal/host/TaskFactory.cpp

@@ -52,6 +52,6 @@ void TaskFactory::printMissedDeadline() {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
   sif::warning << "TaskFactory::printMissedDeadline: " << name << std::endl;
 #else
-  sif::printWarning("TaskFactory::printMissedDeadline: %s\n", name);
+  sif::printWarning("TaskFactory::printMissedDeadline: %s\n", name.c_str());
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 }

src/fsfw/pus/Service8FunctionManagement.h

@@ -1,6 +1,8 @@
 #ifndef FSFW_PUS_SERVICE8FUNCTIONMANAGEMENT_H_
 #define FSFW_PUS_SERVICE8FUNCTIONMANAGEMENT_H_
 
+#include <cstdint>
+
 #include "fsfw/action/ActionMessage.h"
 #include "fsfw/tmtcservices/CommandingServiceBase.h"
 
@@ -35,6 +37,13 @@ class Service8FunctionManagement : public CommandingServiceBase {
                              uint16_t commandTimeoutSeconds = 60);
   ~Service8FunctionManagement() override;
 
+  enum class Subservice : uint8_t {
+    //!< [EXPORT] : [COMMAND] Functional commanding
+    COMMAND_DIRECT_COMMANDING = 128,
+    //!< [EXPORT] : [REPLY] Data reply
+    REPLY_DIRECT_COMMANDING_DATA = 130,
+  };
+
  protected:
   /* CSB abstract functions implementation . See CSB documentation. */
   ReturnValue_t isValidSubservice(uint8_t subservice) override;
@@ -48,13 +57,6 @@ class Service8FunctionManagement : public CommandingServiceBase {
                             bool* isStep) override;
 
  private:
-  enum class Subservice {
-    //!< [EXPORT] : [COMMAND] Functional commanding
-    COMMAND_DIRECT_COMMANDING = 128,
-    //!< [EXPORT] : [REPLY] Data reply
-    REPLY_DIRECT_COMMANDING_DATA = 130,
-  };
-
   ReturnValue_t checkInterfaceAndAcquireMessageQueue(MessageQueueId_t* messageQueueToSet,
                                                      object_id_t* objectId);
   ReturnValue_t prepareDirectCommand(CommandMessage* message, const uint8_t* tcData,

src/fsfw/pus/servicepackets/Service8Packets.h

@@ -1,11 +1,14 @@
 #ifndef FSFW_PUS_SERVICEPACKETS_SERVICE8PACKETS_H_
 #define FSFW_PUS_SERVICEPACKETS_SERVICE8PACKETS_H_
 
+#include <cstdint>
+
 #include "../../action/ActionMessage.h"
 #include "../../objectmanager/SystemObjectIF.h"
+#include "../../returnvalues/returnvalue.h"
 #include "../../serialize/SerialBufferAdapter.h"
-#include "../../serialize/SerialFixedArrayListAdapter.h"
 #include "../../serialize/SerialLinkedListAdapter.h"
+#include "../../serialize/SerializeAdapter.h"
 #include "../../serialize/SerializeElement.h"
 
 /**
@@ -22,14 +25,41 @@ class DirectCommand
     parametersSize = size;
   }
 
+  DirectCommand() : parametersSize(0), parameterBuffer(nullptr) {}
+
   ActionId_t getActionId() const { return actionId; }
+  void setActionId(ActionId_t actionId) { this->actionId = actionId; }
 
   object_id_t getObjectId() const { return objectId; }
+  void setObjectId(object_id_t objectId) { this->objectId = objectId; }
 
   const uint8_t* getParameters() { return parameterBuffer; }
+  // The given pointer is not deallocated and must outlive the DirectCommand!
+  void setParameters(const uint8_t* parameters, uint32_t parametersSize) {
+    this->parameterBuffer = parameters;
+    this->parametersSize = parametersSize;
+  }
 
   uint32_t getParametersSize() const { return parametersSize; }
 
+  // ^SerializeIF
+  virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, size_t maxSize,
+                                  Endianness streamEndianness) const override {
+    auto const oldSize = *size;
+    auto result = SerializeAdapter::serialize(&objectId, buffer, size, maxSize, streamEndianness);
+    if (result != returnvalue::OK) return result;
+    result = SerializeAdapter::serialize(&actionId, buffer, size, maxSize - ((*size) - oldSize),
+                                         streamEndianness);
+    if (result != returnvalue::OK) return result;
+
+    auto remainingSize = maxSize - ((*size) - oldSize);
+    if (remainingSize < parametersSize) return returnvalue::FAILED;
+    memcpy(*buffer, parameterBuffer, parametersSize);
+    *size += parametersSize;
+
+    return returnvalue::OK;
+  }
+
  private:
   DirectCommand(const DirectCommand& command);
   object_id_t objectId = 0;

src/fsfw/returnvalues/FwClassIds.h

@@ -85,6 +85,7 @@ enum : uint8_t {
   MGM_LIS3MDL,                    // MGMLIS3
   MGM_RM3100,                     // MGMRM3100
   SPACE_PACKET_PARSER,            // SPPA
+  COBS_ENCODER,                   // COBS
   FW_CLASS_ID_COUNT               // [EXPORT] : [END]
 
 };

src/fsfw/serviceinterface/CMakeLists.txt

@@ -1,4 +1,4 @@
 target_sources(
   ${LIB_FSFW_NAME}
   PRIVATE ServiceInterfaceStream.cpp ServiceInterfaceBuffer.cpp
-          ServiceInterfacePrinter.cpp)
+          ServiceInterfacePrinter.cpp ServiceInterfacePrinterTask.cpp)

src/fsfw/serviceinterface/ServiceInterfacePrinter.cpp

@@ -1,9 +1,12 @@
 #include "fsfw/serviceinterface/ServiceInterfacePrinter.h"
 
+#include <algorithm>
 #include <cstdarg>
-#include <cstdint>
+#include <cstring>
 
 #include "fsfw/FSFW.h"
+#include "fsfw/ipc/MutexFactory.h"
+#include "fsfw/ipc/MutexGuard.h"
 #include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
 #include "fsfw/timemanager/Clock.h"
 
@@ -14,11 +17,32 @@ static bool consoleInitialized = false;
 
 #if FSFW_DISABLE_PRINTOUT == 0
 
-static bool addCrAtEnd = false;
+namespace {
 
-uint8_t printBuffer[fsfwconfig::FSFW_PRINT_BUFFER_SIZE];
+// Formatted messages are staged in a ring buffer and drained by the print task
+// in bounded chunks, so producers never block on the slow debug UART.
+constexpr size_t RING_SIZE =
+    fsfwconfig::FSFW_PRINT_BUFFER_SIZE * fsfwconfig::FSFW_PRINT_BUFFER_AMOUNT;
 
-void fsfwPrint(sif::PrintLevel printType, const char *fmt, va_list arg) {
+constexpr size_t MAX_BYTES_PER_CYCLE = 512;
+
+// The print ring buffer
+char ring[RING_SIZE];
+MutexIF* ringMutex = nullptr;
+
+size_t readIdx = 0;    // Start of data to print out
+size_t bytesUsed = 0;  // Pending data amount to print out
+uint32_t droppedMessages = 0;
+uint32_t droppedMessagesTotal = 0;
+
+bool addCrAtEnd = false;
+bool replaceLastCharWithNewline = false;
+
+// False until the print task runs.
+// Prints go directly to stdout before that.
+bool taskRunning = false;
+
+void fsfwPrint(sif::PrintLevel printType, const char* fmt, va_list arg) {
 #if defined(WIN32) && FSFW_COLORED_OUTPUT == 1
   if (not consoleInitialized) {
     HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE);
@@ -30,100 +54,156 @@ void fsfwPrint(sif::PrintLevel printType, const char *fmt, va_list arg) {
   consoleInitialized = true;
 #endif
 
-  size_t len = 0;
-  char *bufferPosition = reinterpret_cast<char *>(printBuffer);
-
-  /* Check logger level */
   if (printType == sif::PrintLevel::NONE or printType > printLevel) {
     return;
   }
 
-  /* Log message to terminal */
-
+  static const char* const labels[] = {"", "ERROR  ", "WARNING", "INFO   ", "DEBUG  "};
 #if FSFW_COLORED_OUTPUT == 1
-  if (printType == sif::PrintLevel::INFO_LEVEL) {
-    len += sprintf(bufferPosition, sif::ANSI_COLOR_GREEN);
-  } else if (printType == sif::PrintLevel::DEBUG_LEVEL) {
-    len += sprintf(bufferPosition, sif::ANSI_COLOR_CYAN);
-  } else if (printType == sif::PrintLevel::WARNING_LEVEL) {
-    len += sprintf(bufferPosition, sif::ANSI_COLOR_YELLOW);
-  } else if (printType == sif::PrintLevel::ERROR_LEVEL) {
-    len += sprintf(bufferPosition, sif::ANSI_COLOR_RED);
-  }
-#endif
-
-  if (printType == sif::PrintLevel::INFO_LEVEL) {
-    len += sprintf(bufferPosition + len, "INFO   ");
-  }
-  if (printType == sif::PrintLevel::DEBUG_LEVEL) {
-    len += sprintf(bufferPosition + len, "DEBUG  ");
-  }
-  if (printType == sif::PrintLevel::WARNING_LEVEL) {
-    len += sprintf(bufferPosition + len, "WARNING");
-  }
-
-  if (printType == sif::PrintLevel::ERROR_LEVEL) {
-    len += sprintf(bufferPosition + len, "ERROR  ");
-  }
-
-#if FSFW_COLORED_OUTPUT == 1
-  len += sprintf(bufferPosition + len, sif::ANSI_COLOR_RESET);
+  static const char* const colors[] = {"", sif::ANSI_COLOR_RED, sif::ANSI_COLOR_YELLOW,
+                                       sif::ANSI_COLOR_GREEN, sif::ANSI_COLOR_CYAN};
+  const char* color = colors[printType];
+  const char* reset = sif::ANSI_COLOR_RESET;
+#else
+  const char* color = "";
+  const char* reset = "";
 #endif
 
   Clock::TimeOfDay_t now;
   Clock::getDateAndTime(&now);
-  /*
-   * Log current time to terminal if desired.
-   */
-  len += sprintf(bufferPosition + len, " | %02lu:%02lu:%02lu.%03lu | ", (unsigned long)now.hour,
-                 (unsigned long)now.minute, (unsigned long)now.second,
-                 (unsigned long)now.usecond / 1000);
 
-  len += vsnprintf(bufferPosition + len, sizeof(printBuffer) - len, fmt, arg);
+  char buf[fsfwconfig::FSFW_PRINT_BUFFER_SIZE + 2];  // slack for the CR/newline
 
-  if (addCrAtEnd) {
-    len += sprintf(bufferPosition + len, "\r");
+  // Need to clamp, since snprintf returns WOULD be written length (not actual; could be higher than
+  // really)
+  const auto prefixLen =
+      std::min(static_cast<size_t>(snprintf(
+                   buf, fsfwconfig::FSFW_PRINT_BUFFER_SIZE, "%s%s%s | %02u:%02u:%02u.%03u | ",
+                   color, labels[printType], reset, static_cast<unsigned>(now.hour),
+                   static_cast<unsigned>(now.minute), static_cast<unsigned>(now.second),
+                   static_cast<unsigned>(now.usecond / 1000))),
+               fsfwconfig::FSFW_PRINT_BUFFER_SIZE - 1);
+
+  // Same here
+  const auto msgLen =
+      std::min(static_cast<size_t>(vsnprintf(
+                   buf + prefixLen, fsfwconfig::FSFW_PRINT_BUFFER_SIZE - prefixLen, fmt, arg)),
+               fsfwconfig::FSFW_PRINT_BUFFER_SIZE - 1 - prefixLen);
+
+  size_t textLen = prefixLen + msgLen;
+  if (addCrAtEnd and buf[textLen - 1] == '\n') {
+    buf[textLen++] = '\r';
+  }
+  if (replaceLastCharWithNewline and buf[textLen - 1] != '\n' and buf[textLen - 1] != '\r') {
+    buf[textLen++] = '\n';
   }
 
-  printf("%s", printBuffer);
+  if (not taskRunning) {
+    fwrite(buf, 1, textLen, stdout);
+    return;
+  }
+
+  MutexGuard guard(ringMutex, MutexIF::TimeoutType::BLOCKING);
+
+  if (textLen > RING_SIZE - bytesUsed) {
+    ++droppedMessages;
+    return;
+  }
+
+  const size_t writeIdx = (readIdx + bytesUsed) % RING_SIZE;
+  const size_t firstPart = std::min(textLen, RING_SIZE - writeIdx);
+
+  std::memcpy(ring + writeIdx, buf, firstPart);
+  std::memcpy(ring, buf + firstPart, textLen - firstPart);
+
+  bytesUsed += textLen;
 }
 
-void sif::printInfo(const char *fmt, ...) {
+}  // namespace
+
+void sif::setToAddCrAtEnd(const bool addCrAtEnd_) { addCrAtEnd = addCrAtEnd_; }
+
+void sif::setReplaceLastCharWithNewline(const bool replace) {
+  replaceLastCharWithNewline = replace;
+}
+
+void sif::printInfo(const char* fmt, ...) {
   va_list args;
   va_start(args, fmt);
   fsfwPrint(sif::PrintLevel::INFO_LEVEL, fmt, args);
   va_end(args);
 }
 
-void sif::printWarning(const char *fmt, ...) {
+void sif::printWarning(const char* fmt, ...) {
   va_list args;
   va_start(args, fmt);
   fsfwPrint(sif::PrintLevel::WARNING_LEVEL, fmt, args);
   va_end(args);
 }
 
-void sif::printDebug(const char *fmt, ...) {
+void sif::printDebug(const char* fmt, ...) {
   va_list args;
   va_start(args, fmt);
   fsfwPrint(sif::PrintLevel::DEBUG_LEVEL, fmt, args);
   va_end(args);
 }
 
-void sif::setToAddCrAtEnd(bool addCrAtEnd_) { addCrAtEnd = addCrAtEnd_; }
-
-void sif::printError(const char *fmt, ...) {
+void sif::printError(const char* fmt, ...) {
   va_list args;
   va_start(args, fmt);
   fsfwPrint(sif::PrintLevel::ERROR_LEVEL, fmt, args);
   va_end(args);
 }
 
+void sif::printCallback() {
+  taskRunning = true;
+
+  size_t chunk;
+  {
+    MutexGuard guard(ringMutex, MutexIF::TimeoutType::BLOCKING);
+    chunk = std::min(bytesUsed, MAX_BYTES_PER_CYCLE);
+  }
+
+  if (chunk > 0) {
+    // Safe without the mutex: producers only touch the ring beyond
+    // readIdx + bytesUsed and only the print task advances readIdx.
+    const size_t firstPart = std::min(chunk, RING_SIZE - readIdx);
+    fwrite(ring + readIdx, 1, firstPart, stdout);
+    fwrite(ring, 1, chunk - firstPart, stdout);
+    fflush(stdout);
+  }
+
+  uint32_t dropped;
+  {
+    MutexGuard guard(ringMutex, MutexIF::TimeoutType::BLOCKING);
+    readIdx = (readIdx + chunk) % RING_SIZE;
+    bytesUsed -= chunk;
+    dropped = droppedMessages;
+    droppedMessages = 0;
+  }
+
+  droppedMessagesTotal += dropped;
+
+  if (dropped != 0) {
+    sif::printError("ServiceInterfacePrinter: Dropped %lu messages\n",
+                    static_cast<unsigned long>(dropped));
+  }
+}
+
+void sif::init() { ringMutex = MutexFactory::instance()->createMutex(); }
+
+uint32_t sif::getDroppedMessagesCount() { return droppedMessagesTotal; }
+
 #else
 
-void sif::printInfo(const char *fmt, ...) {}
-void sif::printWarning(const char *fmt, ...) {}
-void sif::printDebug(const char *fmt, ...) {}
-void sif::printError(const char *fmt, ...) {}
+void sif::printInfo(const char* fmt, ...) {}
+void sif::printWarning(const char* fmt, ...) {}
+void sif::printDebug(const char* fmt, ...) {}
+void sif::printError(const char* fmt, ...) {}
+
+void sif::printCallback() {}
+void sif::init() {}
+uint32_t sif::getDroppedMessagesCount() { return 0; }
 
 #endif /* FSFW_DISABLE_PRINTOUT == 0 */
 

src/fsfw/serviceinterface/ServiceInterfacePrinter.h

@@ -39,6 +39,11 @@ PrintLevel getPrintLevel();
 
 void setToAddCrAtEnd(bool addCrAtEnd_);
 
+/**
+ * Replaces the last char of a print buffer with a newline
+ */
+void setReplaceLastCharWithNewline(bool replace);
+
 /**
  * These functions can be used like the C stdio printf and forward the
  * supplied formatted string arguments to a printf function.
@@ -51,6 +56,21 @@ void printWarning(const char* fmt, ...);
 void printDebug(const char* fmt, ...);
 void printError(const char* fmt, ...);
 
+/**
+ * This function is to be called periodically by a dedicated print task.
+ */
+void printCallback();
+
+/**
+ * Initializes the global state for the print task.
+ */
+void init();
+
+/**
+ * Gets the total estimated number of dropped messages
+ */
+uint32_t getDroppedMessagesCount();
+
 }  // namespace sif
 
 #endif /* FSFW_SERVICEINTERFACE_SERVICEINTERFACEPRINTER */

src/fsfw/serviceinterface/ServiceInterfacePrinterTask.cpp

@@ -0,0 +1,14 @@
+#include "ServiceInterfacePrinterTask.h"
+
+#include "ServiceInterfacePrinter.h"
+#include "fsfw/objectmanager/SystemObject.h"
+
+ServiceInterfacePrinterTask::ServiceInterfacePrinterTask(object_id_t objectId)
+    : SystemObject(objectId) {
+  sif::init();
+}
+
+ReturnValue_t ServiceInterfacePrinterTask::performOperation(uint8_t operationCode) {
+  sif::printCallback();
+  return returnvalue::OK;
+}

src/fsfw/serviceinterface/ServiceInterfacePrinterTask.h

@@ -0,0 +1,10 @@
+#pragma once
+
+#include "fsfw/objectmanager/SystemObject.h"
+#include "fsfw/tasks/ExecutableObjectIF.h"
+
+class ServiceInterfacePrinterTask : public ExecutableObjectIF, public SystemObject {
+ public:
+  explicit ServiceInterfacePrinterTask(object_id_t objectId);
+  ReturnValue_t performOperation(uint8_t operationCode) override;
+};

src/fsfw/subsystem/Subsystem.h

@@ -99,7 +99,7 @@ class Subsystem : public SubsystemBase, public HasModeSequenceIF {
     EntryPointer entries;
   };
 
-  static const uint8_t MAX_NUMBER_OF_TABLES_OR_SEQUENCES = 70;
+  static const uint8_t MAX_NUMBER_OF_TABLES_OR_SEQUENCES = 100;
 
   static const uint8_t MAX_LENGTH_OF_TABLE_OR_SEQUENCE = 20;
 

src/fsfw/subsystem/SubsystemBase.cpp

@@ -64,11 +64,6 @@ void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter, Submod
 
   for (; tableIter.value != nullptr; ++tableIter) {
     object_id_t object = tableIter.value->getObject();
-
-    // As default, the objectId in the commandTable is the same as the one in the childrenMap.
-    // The user has to specify otherwise if required.
-    object = commandObjectIdToChildrenMapObjectId(object);
-
     if ((iter = childrenMap.find(object)) == childrenMap.end()) {
       // illegal table entry, should only happen due to misconfigured mode table
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -83,9 +78,8 @@ void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter, Submod
       submodeToCommand = targetSubmode;
     }
 
-    if (healthHelper.healthTable->hasHealth(object)) {
-
-      switch (healthHelper.healthTable->getHealth(object)) {
+    if (healthHelper.healthTable->hasHealth(convertToDeviceObjectId(object))) {
+      switch (healthHelper.healthTable->getHealth(convertToDeviceObjectId(object))) {
         case NEEDS_RECOVERY:
         case FAULTY:
         case PERMANENT_FAULTY:
@@ -124,11 +118,7 @@ void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter, Submod
       continue;  // don't send redundant mode commands (produces event spam), but still command if
                  // mode is forced to reach lower levels
     }
-    
-    // Get the messageQueueId if the receiver specified by the commandTable.
-    // This the same MessageQueueId as stored in the childrenMap if the commanded object is part of the childrenMap.
-    MessageQueueId_t commandedReceiver = ObjectManager::instance()->get<HasHealthIF>(tableIter->getObject())->getCommandQueue();
-    ReturnValue_t result = commandQueue->sendMessage(commandedReceiver, &command);
+    ReturnValue_t result = commandQueue->sendMessage(iter->second.commandQueue, &command);
     if (result == returnvalue::OK) {
       ++commandsOutstanding;
     }
@@ -363,6 +353,4 @@ ReturnValue_t SubsystemBase::registerChild(object_id_t childObjectId, MessageQue
   return returnvalue::OK;
 }
 
-object_id_t SubsystemBase::commandObjectIdToChildrenMapObjectId(object_id_t commandId) {
-  return commandId;
-}
+object_id_t SubsystemBase::convertToDeviceObjectId(object_id_t id) { return id; }

src/fsfw/subsystem/SubsystemBase.h

@@ -113,8 +113,8 @@ class SubsystemBase : public SystemObject,
    * We need to know the target Submode, as children are able to inherit the submode
    * Still, we have a default for all child implementations which do not use submode inheritance
    */
-  void executeTable(HybridIterator<ModeListEntry> tableIter,
-                    Submode_t targetSubmode = SUBMODE_NONE);
+  virtual void executeTable(HybridIterator<ModeListEntry> tableIter,
+                            Submode_t targetSubmode = SUBMODE_NONE);
 
   ReturnValue_t updateChildMode(MessageQueueId_t queue, Mode_t mode, Submode_t submode);
   ReturnValue_t updateChildModeByObjId(object_id_t objectId, Mode_t mode, Submode_t submode);
@@ -155,13 +155,10 @@ class SubsystemBase : public SystemObject,
   virtual void modeChanged();
 
   /**
- * @brief This converts the objectId of the object we want to send a mode command to into the 
- * objectId of the corresponding object in the childrenMap for the current mode command. 
- * As default implementation, this is the same objectId, and this functions returns it's input value
- * 
- * It is up to the user to specify otherwise.
- */
-  virtual object_id_t commandObjectIdToChildrenMapObjectId(object_id_t commandId);
+   * @brief Provides an adaptation point for the user to change an objectId into
+   * a different objectId.
+   */
+  virtual object_id_t convertToDeviceObjectId(object_id_t id);
 };
 
 #endif /* FSFW_SUBSYSTEM_SUBSYSTEMBASE_H_ */

src/fsfw/tcdistribution/PusDistributor.cpp

@@ -18,6 +18,10 @@ PusDistributor::PusDistributor(uint16_t setApid, object_id_t setObjectId, Storag
 
 PusDistributor::~PusDistributor() = default;
 
+void PusDistributor::setVerificationReporter(object_id_t verificationReporter_) {
+  verificationReporter = verificationReporter_;
+}
+
 ReturnValue_t PusDistributor::selectDestination(MessageQueueId_t& destId) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1 && PUS_DISTRIBUTOR_DEBUGGING == 1
   store_address_t storeId = currentMessage.getStorageId();
@@ -131,8 +135,7 @@ ReturnValue_t PusDistributor::initialize() {
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
   if (verifyChannel == nullptr) {
-    verifyChannel =
-        ObjectManager::instance()->get<VerificationReporterIF>(objects::VERIFICATION_REPORTER);
+    verifyChannel = ObjectManager::instance()->get<VerificationReporterIF>(verificationReporter);
     if (verifyChannel == nullptr) {
       return ObjectManagerIF::CHILD_INIT_FAILED;
     }

src/fsfw/tcdistribution/PusDistributor.h

@@ -43,6 +43,10 @@ class PusDistributor : public TcDistributorBase,
   [[nodiscard]] MessageQueueId_t getRequestQueue() const override;
   ReturnValue_t initialize() override;
   [[nodiscard]] uint32_t getIdentifier() const override;
+  /**
+   * @brief Can be used to set the verification reporter if another than the default should be used
+   */
+  void setVerificationReporter(object_id_t verificationReporter_);
 
  protected:
   struct ServiceInfo {
@@ -75,6 +79,8 @@ class PusDistributor : public TcDistributorBase,
    */
   ReturnValue_t tcStatus;
 
+  object_id_t verificationReporter = objects::VERIFICATION_REPORTER;
+
   /**
    * This method reads the packet service, checks if such a service is
    * registered and forwards the packet to the destination.

src/fsfw/timemanager/Clock.h

@@ -192,8 +192,6 @@ class Clock {
   static MutexIF *timeMutex;
   static uint16_t leapSeconds;
   static bool leapSecondsSet;
-  static bool monotonicClockInitialized;
-  static timeval monotonicClockOffset;
 };
 
 #endif /* FSFW_TIMEMANAGER_CLOCK_H_ */

src/fsfw/tmstorage/TmStoreMessage.cpp

@@ -114,6 +114,11 @@ void TmStoreMessage::setDownlinkContentTimeMessage(CommandMessage* cmd, store_ad
   cmd->setParameter2(storeId.raw);
 }
 
+void TmStoreMessage::setStopDownlinkContentMessage(CommandMessage* cmd, store_address_t storeId) {
+  cmd->setCommand(STOP_DOWNLINK_STORE_CONTENT);
+  cmd->setParameter2(storeId.raw);
+}
+
 uint32_t TmStoreMessage::getAddressLow(CommandMessage* cmd) { return cmd->getParameter(); }
 uint32_t TmStoreMessage::getAddressHigh(CommandMessage* cmd) { return cmd->getParameter2(); }
 

src/fsfw/tmstorage/TmStoreMessage.h

@@ -21,6 +21,7 @@ class TmStoreMessage {
   static void setStoreCatalogueReportMessage(CommandMessage* cmd, object_id_t objectId,
                                              store_address_t storeId);
   static void setDownlinkContentTimeMessage(CommandMessage* cmd, store_address_t storeId);
+  static void setStopDownlinkContentMessage(CommandMessage* cmd, store_address_t storeId);
   static void setIndexReportMessage(CommandMessage* cmd, store_address_t storeId);
   static ReturnValue_t setDeleteBlocksMessage(CommandMessage* cmd, uint32_t addressLow,
                                               uint32_t addressHigh);
@@ -54,6 +55,7 @@ class TmStoreMessage {
   static const Command_t DOWNLINK_STORE_CONTENT_BLOCKS = MAKE_COMMAND_ID(12);
   static const Command_t REPORT_INDEX_REQUEST = MAKE_COMMAND_ID(13);
   static const Command_t INDEX_REPORT = MAKE_COMMAND_ID(14);
+  static const Command_t STOP_DOWNLINK_STORE_CONTENT = MAKE_COMMAND_ID(15);
 
  private:
   TmStoreMessage();

src/fsfw/tmtcservices/CommandingServiceBase.cpp

@@ -128,7 +128,7 @@ ReturnValue_t CommandingServiceBase::initialize() {
 
   if (verificationReporter == nullptr) {
     verificationReporter =
-        ObjectManager::instance()->get<VerificationReporterIF>(objects::VERIFICATION_REPORTER);
+        ObjectManager::instance()->get<VerificationReporterIF>(verificationReporterId);
     if (verificationReporter == nullptr) {
       return ObjectManagerIF::CHILD_INIT_FAILED;
     }
@@ -136,6 +136,10 @@ ReturnValue_t CommandingServiceBase::initialize() {
   return returnvalue::OK;
 }
 
+void CommandingServiceBase::setVerificationReporter(object_id_t verificationReporterId_) {
+  verificationReporterId = verificationReporterId_;
+}
+
 void CommandingServiceBase::handleCommandQueue() {
   CommandMessage reply;
   ReturnValue_t result;

src/fsfw/tmtcservices/CommandingServiceBase.h

@@ -126,6 +126,8 @@ class CommandingServiceBase : public SystemObject,
 
   ReturnValue_t initialize() override;
 
+  void setVerificationReporter(object_id_t verificationReporterId_);
+
   /**
    * Implementation of ExecutableObjectIF function
    *
@@ -262,6 +264,8 @@ class CommandingServiceBase : public SystemObject,
 
   const uint16_t timeoutSeconds;
 
+  object_id_t verificationReporterId = objects::VERIFICATION_REPORTER;
+
   PusTcReader tcReader;
   TmStoreHelper tmStoreHelper;
   TmSendHelper tmSendHelper;

src/fsfw/tmtcservices/PusServiceBase.cpp

@@ -111,7 +111,7 @@ ReturnValue_t PusServiceBase::initialize() {
   }
 
   if (psbParams.pusDistributor == nullptr) {
-    psbParams.pusDistributor = ObjectManager::instance()->get<PusDistributorIF>(PUS_DISTRIBUTOR);
+    psbParams.pusDistributor = ObjectManager::instance()->get<PusDistributorIF>(pusDistributor);
     if (psbParams.pusDistributor != nullptr) {
       registerService(*psbParams.pusDistributor);
     }
@@ -126,7 +126,7 @@ ReturnValue_t PusServiceBase::initialize() {
 
   if (psbParams.verifReporter == nullptr) {
     psbParams.verifReporter =
-        ObjectManager::instance()->get<VerificationReporterIF>(objects::VERIFICATION_REPORTER);
+        ObjectManager::instance()->get<VerificationReporterIF>(verificationReporter);
     if (psbParams.verifReporter == nullptr) {
       return ObjectManagerIF::CHILD_INIT_FAILED;
     }
@@ -134,6 +134,14 @@ ReturnValue_t PusServiceBase::initialize() {
   return returnvalue::OK;
 }
 
+void PusServiceBase::setPusDistributor(object_id_t pusDistributor_) {
+  pusDistributor = pusDistributor_;
+}
+
+void PusServiceBase::setVerificationReporter(object_id_t verificationReporter_) {
+  verificationReporter = verificationReporter_;
+}
+
 void PusServiceBase::setTcPool(StorageManagerIF& tcPool) { psbParams.tcPool = &tcPool; }
 
 void PusServiceBase::setErrorReporter(InternalErrorReporterIF& errReporter_) {

src/fsfw/tmtcservices/PusServiceBase.h

@@ -201,6 +201,9 @@ class PusServiceBase : public ExecutableObjectIF,
   void setTaskIF(PeriodicTaskIF* taskHandle) override;
   [[nodiscard]] const char* getName() const override;
 
+  void setPusDistributor(object_id_t pusDistributor_);
+  void setVerificationReporter(object_id_t verificationReporter_);
+
  protected:
   /**
    * @brief   Handle to the underlying task
@@ -228,6 +231,9 @@ class PusServiceBase : public ExecutableObjectIF,
   static object_id_t PACKET_DESTINATION;
   static object_id_t PUS_DISTRIBUTOR;
 
+  object_id_t pusDistributor = PUS_DISTRIBUTOR;
+  object_id_t verificationReporter = objects::VERIFICATION_REPORTER;
+
  private:
   void handleRequestQueue();
 };

src/fsfw_hal/CMakeLists.txt

@@ -1,7 +1,7 @@
 add_subdirectory(common)
-add_subdirectory(host)
 
 if(UNIX)
+  add_subdirectory(host)
   add_subdirectory(linux)
 endif()
 

src/fsfw_hal/host/CMakeLists.txt

@@ -1 +1 @@
-target_sources(${LIB_FSFW_NAME} PUBLIC HostFilesystem.cpp)
+target_sources(${LIB_FSFW_NAME} PRIVATE HostFilesystem.cpp)

src/fsfw_hal/linux/CommandExecutor.h

@@ -116,7 +116,7 @@ class CommandExecutor {
   int currentFd = 0;
   bool printOutput = true;
   std::vector<char> readVec;
-  struct pollfd waiter {};
+  struct pollfd waiter{};
   SimpleRingBuffer* ringBuffer = nullptr;
   DynamicFIFO<uint16_t>* sizesFifo = nullptr;
 

unittests/globalfunctions/CMakeLists.txt

@@ -1,3 +1,4 @@
 target_sources(
-  ${FSFW_TEST_TGT} PRIVATE testDleEncoder.cpp testOpDivider.cpp testBitutil.cpp
-                           testCRC.cpp testTimevalOperations.cpp)
+  ${FSFW_TEST_TGT}
+  PRIVATE testCobsEncoder.cpp testDleEncoder.cpp testOpDivider.cpp
+          testBitutil.cpp testCRC.cpp testTimevalOperations.cpp)

unittests/globalfunctions/testCobsEncoder.cpp

@@ -0,0 +1,336 @@
+#include <algorithm>
+#include <array>
+#include <cstdint>
+#include <vector>
+
+#include "catch2/catch_test_macros.hpp"
+#include "fsfw/globalfunctions/CobsEncoder.h"
+#include "fsfw/returnvalues/returnvalue.h"
+
+// no zero bytes
+constexpr std::array<uint8_t, 3> ti0 = {1, 2, 43};
+constexpr std::array<uint8_t, 5> to0 = {0x04, 0x01, 0x02, 0x2b, 0x00};
+
+// single zero byte
+constexpr std::array<uint8_t, 4> ti1 = {1, 2, 0, 3};
+constexpr std::array<uint8_t, 6> to1 = {3, 1, 2, 2, 3, 0};
+
+// multiple zero bytes in sequence
+constexpr std::array<uint8_t, 6> ti2 = {1, 2, 0, 0, 0, 3};
+constexpr std::array<uint8_t, 8> to2 = {3, 1, 2, 1, 1, 2, 3, 0};
+
+// stuffing bytes required because of the length without a single zero byte
+// clang-format off
+constexpr std::array<uint8_t, 257> ti3 = {
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  // #257
+  0xaa,
+};
+constexpr std::array<uint8_t, 260> to3 = {
+  0xff,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+  1, 2, 3, 4,5 , 6, 7, 8, 9, 10, 11, 12, 13, 14,
+  // #255 -> stuffing
+  0x04,
+  15, 16,
+  0xaa, 0
+};
+// clang-format on
+
+// real packet data
+
+// PLOC startup packets (usually in one or two messages, but 3 separate COBS encoded packets)
+// clang-format off
+constexpr std::array<uint8_t, 36> ploc_startup_1_i = {
+  0x08,0x42,0xc0,0x00,0x00,0x1d,0x20,0x05,0x02,0x00,0x00,0x00,0x00,0x40,0x61,0x0d,0x02,0x8e,
+  0xb4,0xef,0x2e,0xe1,0x44,0x00,0x1f,0x03,0x00,0x00,0x52,0x18,0x00,0x00,0x00,0x00,0x1b,0xe2,
+};
+constexpr std::array<uint8_t, 36> ploc_startup_2_i = {
+  0x08,0x42,0xc0,0x01,0x00,0x1d,0x20,0x05,0x02,0x00,0x01,0x00,0x00,0x40,0x61,0x0d,0x02,0x8e,
+  0xb4,0xef,0x30,0x74,0x44,0x00,0x1f,0x07,0x00,0x00,0x50,0x03,0x00,0x00,0x00,0x00,0x98,0xe3,
+};
+constexpr std::array<uint8_t, 36> ploc_startup_3_i = {
+  0x08,0x42,0xc0,0x02,0x00,0x1d,0x20,0x05,0x01,0x00,0x02,0x00,0x00,0x40,0x61,0x0d,0x02,0x8e,
+  0xb5,0xb8,0x30,0x73,0x44,0x00,0x1f,0x07,0x00,0x00,0x3b,0x04,0x00,0x00,0x00,0x00,0x22,0x68,
+};
+constexpr std::array<uint8_t, 38> ploc_startup_1_o = {
+  0x04,0x08,0x42,0xc0,0x01,0x05,0x1d,0x20,0x05,0x02,
+  0x01,0x01,0x01,0x0b,0x40,0x61,0x0d,0x02,0x8e,0xb4,
+  0xef,0x2e,0xe1,0x44,0x03,0x1f,0x03,0x01,0x03,0x52,
+  0x18,0x01,0x01,0x01,0x03,0x1b,0xe2, 0x00,
+};
+constexpr std::array<uint8_t, 38> ploc_startup_2_o = {
+  0x05, 0x08,
+  0x42,0xc0,0x01,0x05,0x1d,0x20,0x05,0x02,0x02,0x01,
+  0x01,0x0b,0x40,0x61,0x0d,0x02,0x8e,0xb4,0xef,0x30,
+  0x74,0x44,0x03,0x1f,0x07,0x01,0x03,0x50,0x03,0x01,
+  0x01,0x01,0x03,0x98,0xe3,0x00
+};
+constexpr std::array<uint8_t, 38> ploc_startup_3_o = {
+  0x05,0x08,0x42,0xc0,0x02,0x05,0x1d,0x20,0x05,0x01,
+  0x02,0x02,0x01,0x0b,0x40,0x61,0x0d,0x02,0x8e,0xb5,
+  0xb8,0x30,0x73,0x44,0x03,0x1f,0x07,0x01,0x03,0x3b,
+  0x04,0x01,0x01,0x01,0x03,0x22,0x68,0x00,
+};
+// clang-format on
+
+// PLOC Ping (sent from OBC)
+// clang-format off
+constexpr std::array<uint8_t, 13> ploc_ping_i = {
+  // as received by PLOC
+  0x18,0x42,0xc0,0x00,0x00,0x06,0x2f,0x11,0x01,0x00,
+  0x00,0xd5,0xc8,
+};
+constexpr std::array<uint8_t, 15> ploc_ping_o = {
+  // as sent from OBC
+  0x04,0x18,0x42,0xc0,0x01,0x05,0x06,0x2f,0x11,0x01,0x01,0x03,0xd5,0xc8,0x00,
+};
+// clang-format on
+// Corresponding PLOC Pong (sent from PLOC)
+// clang-format off
+constexpr std::array<uint8_t, 26> ploc_pong_i = {
+  // as received by OBC
+  0x08,0x42,0xc0,0x05,0x00,0x13,0x20,0x01,0x07,0x00,
+  0x01,0x00,0x00,0x40,0x61,0x0d,0x02,0x98,0xbd,0xab,
+  0x18,0x42,0xc0,0x00,0x13,0xd5,
+};
+constexpr std::array<uint8_t, 28> ploc_pong_o = {
+  // as sent by PLOC
+  0x05,0x08,0x42,0xc0,0x05,0x05,0x13,0x20,0x01,0x07,
+  0x02,0x01,0x01,0x0b,0x40,0x61,0x0d,0x02,0x98,0xbd,
+  0xab,0x18,0x42,0xc0,0x03,0x13,0xd5,0x00,
+};
+// clang-format on
+
+#define FOR_EACH_TEST_ARRAY(macro)           \
+  macro(ti0, to0);                           \
+  macro(ti1, to1);                           \
+  macro(ti2, to2);                           \
+  macro(ti3, to3);                           \
+  macro(ploc_startup_1_i, ploc_startup_1_o); \
+  macro(ploc_startup_2_i, ploc_startup_2_o); \
+  macro(ploc_startup_3_i, ploc_startup_3_o); \
+  macro(ploc_ping_i, ploc_ping_o);           \
+  macro(ploc_pong_i, ploc_pong_o);
+
+template <typename FI, typename FE, typename SI>
+void check_iter_equal(FI first_begin, FE first_end, SI second_begin) {
+  auto first_iter = first_begin;
+  auto second_iter = second_begin;
+  auto i = 0;
+  while (first_iter < first_end) {
+    CHECK(*first_iter == *second_iter);
+    ++first_iter;
+    ++second_iter;
+    ++i;
+  }
+
+  // safety check
+  CHECK(std::equal(first_begin, first_end, second_begin));
+}
+
+#define ENCODE_TEST(input, output)                                                  \
+  TEST_CASE("COBS encode " #input, "[cobs]") {                                      \
+    std::vector<uint8_t> outstream;                                                 \
+    outstream.resize(output.size());                                                \
+    size_t encodedLen = 0;                                                          \
+    auto result = CobsEncoder::encode(input.data(), input.size(), outstream.data(), \
+                                      outstream.size(), &encodedLen);               \
+    if (result == returnvalue::OK) {                                                \
+      CHECK(encodedLen == output.size());                                           \
+      check_iter_equal(outstream.begin(), outstream.end(), output.begin());         \
+    } else {                                                                        \
+      CHECK(result == returnvalue::OK);                                             \
+    }                                                                               \
+  }
+
+FOR_EACH_TEST_ARRAY(ENCODE_TEST)
+#undef ENCODE_TEST
+
+#define DECODE_TEST(input, output)                                                             \
+  TEST_CASE("COBS decode " #output, "[cobs]") {                                                \
+    std::vector<uint8_t> instream;                                                             \
+    instream.resize(input.size());                                                             \
+    size_t decodedLen = 0;                                                                     \
+    size_t readLen = 0;                                                                        \
+    auto result = CobsEncoder::decode(output.data(), output.size(), &readLen, instream.data(), \
+                                      instream.size(), &decodedLen);                           \
+    if (result == returnvalue::OK) {                                                           \
+      CHECK(readLen == output.size());                                                         \
+      CHECK(decodedLen == input.size());                                                       \
+      check_iter_equal(instream.begin(), instream.end(), input.begin());                       \
+    } else {                                                                                   \
+      CHECK(result == returnvalue::OK);                                                        \
+    }                                                                                          \
+  }
+
+FOR_EACH_TEST_ARRAY(DECODE_TEST)
+#undef DECODE_TEST
+
+// payloads far past any block count round-trip and the worst case bound is tight enough to hold
+// them
+TEST_CASE("Large payload", "[cobs]") {
+  // no zero bytes at all, so this needs the maximum number of blocks the input length allows
+  std::vector<uint8_t> input(64 * 1024, 1);
+  std::vector<uint8_t> encoded(CobsEncoder::worstCaseEncodedLen(input.size()));
+  size_t encodedLen = 0;
+
+  auto result =
+      CobsEncoder::encode(input.data(), input.size(), encoded.data(), encoded.size(), &encodedLen);
+  REQUIRE(result == returnvalue::OK);
+  CHECK(encodedLen == encoded.size());
+
+  std::vector<uint8_t> decoded(input.size());
+  size_t decodedLen = 0;
+  size_t readLen = 0;
+  result = CobsEncoder::decode(encoded.data(), encodedLen, &readLen, decoded.data(), decoded.size(),
+                               &decodedLen);
+  REQUIRE(result == returnvalue::OK);
+  CHECK(readLen == encodedLen);
+  REQUIRE(decodedLen == input.size());
+  CHECK(std::equal(decoded.begin(), decoded.end(), input.begin()));
+}
+
+// check that too small target buffers are not blindly used
+TEST_CASE("Insufficient space", "[cobs]") {
+  std::vector<uint8_t> output{0, 2};
+  size_t encodedLen = 0;
+
+  auto result =
+      CobsEncoder::encode(ti1.data(), ti1.size(), output.data(), output.size(), &encodedLen);
+  CHECK(result == CobsEncoder::INSUFFICIENT_SPACE);
+
+  // even equal size should not work
+  output.resize(ti1.size());
+  result = CobsEncoder::encode(ti1.data(), ti1.size(), output.data(), output.size(), &encodedLen);
+  CHECK(result == CobsEncoder::INSUFFICIENT_SPACE);
+
+  // check reverse direction for decoding
+  output.resize(2);
+  size_t readLen;
+  size_t decodedLen;
+  result = CobsEncoder::decode(to1.data(), to1.size(), &readLen, output.data(), output.size(),
+                               &decodedLen);
+  CHECK(result == CobsEncoder::INSUFFICIENT_SPACE);
+}
+
+TEST_CASE("Malformed COBS data", "[cobs]") {
+  // decode a cut-off packet
+  std::vector<uint8_t> input{std::begin(to2), std::end(to2)};
+  input.resize(to2.size() / 2);
+
+  std::vector<uint8_t> output;
+  output.resize(ti2.size() * 2);
+  size_t encodedLen;
+  size_t readLen;
+
+  auto result = CobsEncoder::decode(input.data(), input.size(), &readLen, output.data(),
+                                    output.size(), &encodedLen);
+  CHECK(result == CobsEncoder::STREAM_TOO_SHORT);
+
+  // inject a zero byte somewhere in the middle of a list of nonzero bytes
+  input = {10, 1, 2, 3, 4, 0, 6, 7, 8, 9, 0};
+  result = CobsEncoder::decode(input.data(), input.size(), &readLen, output.data(), output.size(),
+                               &encodedLen);
+  CHECK(result == CobsEncoder::DECODING_ERROR);
+  // the whole broken frame is skipped, up to and including its delimiter
+  CHECK(readLen == 6);
+}
+
+// a corrupt frame must not hold back the intact frames queued behind it
+TEST_CASE("Resynchronisation after a corrupt frame", "[cobs]") {
+  // a block claiming far more bytes than the frame holds, terminated like a real frame
+  std::vector<uint8_t> stream{0x40, 0x11, 0x22, 0x00};
+  const size_t corruptLen = stream.size();
+  stream.insert(stream.end(), to1.begin(), to1.end());
+
+  std::vector<uint8_t> output(ti1.size());
+  size_t readLen = 0;
+  size_t decodedLen = 0;
+
+  auto result = CobsEncoder::decode(stream.data(), stream.size(), &readLen, output.data(),
+                                    output.size(), &decodedLen);
+  REQUIRE(result == CobsEncoder::DECODING_ERROR);
+  REQUIRE(readLen == corruptLen);
+
+  // the next frame decodes from where the corrupt one ended
+  result = CobsEncoder::decode(stream.data() + readLen, stream.size() - readLen, &readLen,
+                               output.data(), output.size(), &decodedLen);
+  REQUIRE(result == returnvalue::OK);
+  CHECK(readLen == to1.size());
+  REQUIRE(decodedLen == ti1.size());
+  CHECK(std::equal(output.begin(), output.end(), ti1.begin()));
+}
+
+// without a delimiter the frame may simply still be arriving, so nothing may be consumed
+TEST_CASE("Incomplete frame is not consumed", "[cobs]") {
+  std::vector<uint8_t> output(16);
+  size_t readLen = 0xdead;
+  size_t decodedLen = 0xdead;
+
+  // a self-consistent frame that is merely missing its delimiter
+  std::vector<uint8_t> input{3, 1, 2, 2, 3};
+  auto result = CobsEncoder::decode(input.data(), input.size(), &readLen, output.data(),
+                                    output.size(), &decodedLen);
+  CHECK(result == CobsEncoder::STREAM_TOO_SHORT);
+  CHECK(readLen == 0);
+  CHECK(decodedLen == 0);
+
+  // once the delimiter arrives, the very same bytes decode
+  input.push_back(0);
+  result = CobsEncoder::decode(input.data(), input.size(), &readLen, output.data(), output.size(),
+                               &decodedLen);
+  REQUIRE(result == returnvalue::OK);
+  CHECK(readLen == input.size());
+  CHECK(decodedLen == ti1.size());
+}
+
+// an empty payload survives a round trip and is reported as a zero length frame
+TEST_CASE("Empty frame", "[cobs]") {
+  std::vector<uint8_t> encoded(CobsEncoder::worstCaseEncodedLen(0));
+  size_t encodedLen = 0;
+
+  auto result = CobsEncoder::encode(nullptr, 0, encoded.data(), encoded.size(), &encodedLen);
+  REQUIRE(result == returnvalue::OK);
+  REQUIRE(encodedLen == 2);
+  CHECK(encoded[0] == 0x01);
+  CHECK(encoded[1] == 0x00);
+
+  std::vector<uint8_t> output(4);
+  size_t readLen = 0;
+  size_t decodedLen = 0;
+  result = CobsEncoder::decode(encoded.data(), encodedLen, &readLen, output.data(), output.size(),
+                               &decodedLen);
+  CHECK(result == returnvalue::OK);
+  CHECK(readLen == 2);
+  CHECK(decodedLen == 0);
+}

unittests/tmtcpacket/testPusTmReader.cpp

@@ -38,7 +38,7 @@ TEST_CASE("PUS TM Reader", "[pus-tm-reader]") {
       readerPtr->setTimeReader(&timeStamperAndReader);
       deleteReader = true;
     }
-    REQUIRE(not *readerPtr);
+    REQUIRE(not*readerPtr);
     REQUIRE(readerPtr->isNull());
     REQUIRE(readerPtr->parseDataWithCrcCheck() == returnvalue::OK);
     REQUIRE(not readerPtr->isNull());