src/fsfw/tcdistribution/CcsdsDistributor.cpp

@@ -46,7 +46,16 @@ ReturnValue_t CcsdsDistributor::selectDestination(MessageQueueId_t& destId) {
 #endif
     return result;
   }
-  if (accessorPair.second.size() < ccsds::HEADER_LEN) {
+  // Minimum length of a space packet
+  if (accessorPair.second.size() < ccsds::HEADER_LEN + 1) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::error << __func__ << ": SP with length" << accessorPair.second.size() << " too short"
+               << std::endl;
+#else
+    sif::printError("%s: SP with length %d too short\n", __func__, accessorPair.second.size());
+#endif
+#endif
     return SerializeIF::STREAM_TOO_SHORT;
   }
   SpacePacketReader currentPacket(accessorPair.second.data(), accessorPair.second.size());
@@ -62,11 +71,7 @@ ReturnValue_t CcsdsDistributor::selectDestination(MessageQueueId_t& destId) {
   auto iter = receiverMap.find(currentPacket.getApid());
   if (iter != receiverMap.end()) {
     destId = iter->second.destId;
-    if (iter->second.removeHeader) {
+  } else if (iter == receiverMap.end()) {
-      // Do not call accessor release method here to ensure the old packet gets deleted.
-      return handleCcsdsHeaderRemoval(accessorPair.second);
-    }
-  } else {
     // The APID was not found. Forward packet to main SW-APID anyway to
     // create acceptance failure report.
     iter = receiverMap.find(defaultApid);
@@ -76,6 +81,10 @@ ReturnValue_t CcsdsDistributor::selectDestination(MessageQueueId_t& destId) {
       return DESTINATION_NOT_FOUND;
     }
   }
+  if (iter->second.removeHeader) {
+    // Do not call accessor release method here to ensure the old packet gets deleted.
+    return handleCcsdsHeaderRemoval(accessorPair.second);
+  }
   accessorPair.second.release();
   return HasReturnvaluesIF::RETURN_OK;
 }

src/fsfw/tcdistribution/CcsdsDistributor.h

@@ -53,7 +53,9 @@ class CcsdsDistributor : public TcDistributorBase,
    * registered and forwards the packet to the according message queue.
    * If the packet is not found, it returns the queue to @c defaultApid,
    * where a Acceptance Failure message should be generated.
-   * @return Iterator to map entry of found APID or iterator to default APID.
+   * @return
+   *  - @c RETURN_OK if a valid desintation was found, error code otherwise
+   *  - @c SerializeIF::STREAM_TOO_SHORT: Packet too short to be a space packet
    */
   ReturnValue_t selectDestination(MessageQueueId_t& destId) override;
   /**

src/fsfw/tcdistribution/TcDistributorBase.cpp

@@ -40,8 +40,7 @@ ReturnValue_t TcDistributorBase::handlePacket() {
   if (result != HasReturnvaluesIF::RETURN_OK) {
     return result;
   }
-  result = tcQueue->sendMessage(destId, &currentMessage);
+  return callbackAfterSending(tcQueue->sendMessage(destId, &currentMessage));
-  return callbackAfterSending(result);
 }
 
 ReturnValue_t TcDistributorBase::callbackAfterSending(ReturnValue_t queueStatus) {

src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.cpp

@@ -40,7 +40,13 @@ ReturnValue_t SpacePacketCreator::serialize(uint8_t **buffer, size_t *size, size
   return SerializeAdapter::serialize(&params.dataLen, buffer, size, maxSize, streamEndianness);
 }
 
-size_t SpacePacketCreator::getSerializedSize() const { return 6; }
+void SpacePacketCreator::setCcsdsLenFromTotalDataFieldLen(size_t actualLength) {
+  if (actualLength == 0) {
+    return;
+  }
+  setDataLenField(actualLength - 1);
+}
+size_t SpacePacketCreator::getSerializedSize() const { return ccsds::HEADER_LEN; }
 
 ReturnValue_t SpacePacketCreator::deSerialize(const uint8_t **buffer, size_t *size,
                                               SerializeIF::Endianness streamEndianness) {
@@ -64,7 +70,7 @@ void SpacePacketCreator::setSeqCount(uint16_t seqCount) {
 void SpacePacketCreator::setSeqFlags(ccsds::SequenceFlags flags) {
   params.packetSeqCtrl.seqFlags = flags;
 }
-void SpacePacketCreator::setDataLen(uint16_t dataLen_) { params.dataLen = dataLen_; }
+void SpacePacketCreator::setDataLenField(uint16_t dataLen_) { params.dataLen = dataLen_; }
 void SpacePacketCreator::checkFieldValidity() {
   valid = true;
   if (params.packetId.apid > ccsds::LIMIT_APID or

src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.h

@@ -38,24 +38,51 @@ class SpacePacketCreator : public SpacePacketIF, public SerializeIF {
   [[nodiscard]] uint16_t getPacketDataLen() const override;
 
   SpacePacketParams &getParams();
+  /**
+   * Sets the CCSDS data length field from the actual data field length. The field will contain
+   * the actual length minus one. This means that the minimum allowed size is one, as is also
+   * specified in 4.1.4.1.2 of the standard. Values of 0 will be ignored.
+   * @param dataFieldLen
+   */
+  void setCcsdsLenFromTotalDataFieldLen(size_t dataFieldLen);
   void setParams(SpacePacketParams params);
   void setSecHeaderFlag();
   void setPacketType(ccsds::PacketType type);
   void setApid(uint16_t apid);
   void setSeqCount(uint16_t seqCount);
   void setSeqFlags(ccsds::SequenceFlags flags);
-  void setDataLen(uint16_t dataLen);
+  void setDataLenField(uint16_t dataLen);
 
+  /**
+   * Please note that this method will only serialize the header part of the space packet.
+   * @param buffer
+   * @param size
+   * @param maxSize
+   * @param streamEndianness
+   * @return
+   */
   ReturnValue_t serialize(uint8_t **buffer, size_t *size, size_t maxSize,
                           Endianness streamEndianness) const override;
 
+  /**
+   * This will always return 6 or ccsds::HEADER_LEN
+   * @return
+   */
   [[nodiscard]] size_t getSerializedSize() const override;
-  ReturnValue_t deSerialize(const uint8_t **buffer, size_t *size,
-                            Endianness streamEndianness) override;
 
  private:
   void checkFieldValidity();
   bool valid{};
   SpacePacketParams params{};
+
+  /**
+   * Forbidden to call and always return HasReturnvaluesIF::RETURN_FAILED
+   * @param buffer
+   * @param size
+   * @param streamEndianness
+   * @return
+   */
+  ReturnValue_t deSerialize(const uint8_t **buffer, size_t *size,
+                            Endianness streamEndianness) override;
 };
 #endif  // FSFW_TMTCPACKET_SPACEPACKETCREATOR_H

src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp

@@ -58,8 +58,9 @@ ReturnValue_t PusTcCreator::serialize(uint8_t **buffer, size_t *size, size_t max
 }
 
 void PusTcCreator::updateSpLengthField() {
-  spCreator.setDataLen(ecss::PusTcDataFieldHeader::MIN_SIZE + pusParams.dataWrapper.getLength() +
+  spCreator.setCcsdsLenFromTotalDataFieldLen(ecss::PusTcDataFieldHeader::MIN_SIZE +
-                       1);
+                                             pusParams.dataWrapper.getLength() +
+                                             sizeof(ecss::PusChecksumT));
 }
 
 size_t PusTcCreator::getSerializedSize() const { return spCreator.getFullPacketLen(); }

src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp

@@ -111,12 +111,12 @@ TimeStamperIF* PusTmCreator::getTimestamper() const { return pusParams.secHeader
 SpacePacketParams& PusTmCreator::getSpParams() { return spCreator.getParams(); }
 
 void PusTmCreator::updateSpLengthField() {
-  size_t headerLen = PusTmIF::MIN_SEC_HEADER_LEN + pusParams.dataWrapper.getLength() +
+  size_t headerLen =
-                     sizeof(ecss::PusChecksumT) - 1;
+      PusTmIF::MIN_SEC_HEADER_LEN + pusParams.dataWrapper.getLength() + sizeof(ecss::PusChecksumT);
   if (pusParams.secHeader.timeStamper != nullptr) {
     headerLen += pusParams.secHeader.timeStamper->getSerializedSize();
   }
-  spCreator.setDataLen(headerLen);
+  spCreator.setCcsdsLenFromTotalDataFieldLen(headerLen);
 }
 
 void PusTmCreator::setApid(uint16_t apid) { spCreator.setApid(apid); }

unittests/cfdp/testTlvsLvs.cpp

@@ -16,15 +16,15 @@
 
 TEST_CASE("CFDP TLV LV", "[CfdpTlvLv]") {
   using namespace cfdp;
-  int result = HasReturnvaluesIF::RETURN_OK;
+  ReturnValue_t result;
-  std::array<uint8_t, 255> rawBuf;
+  std::array<uint8_t, 255> rawBuf{};
   uint8_t* serPtr = rawBuf.data();
   const uint8_t* deserPtr = rawBuf.data();
   size_t deserSize = 0;
   cfdp::EntityId sourceId = EntityId(cfdp::WidthInBytes::TWO_BYTES, 0x0ff0);
 
   SECTION("TLV Serialization") {
-    std::array<uint8_t, 8> tlvRawBuf;
+    std::array<uint8_t, 8> tlvRawBuf{};
     serPtr = tlvRawBuf.data();
     result =
         sourceId.serialize(&serPtr, &deserSize, tlvRawBuf.size(), SerializeIF::Endianness::NETWORK);
@@ -88,7 +88,7 @@ TEST_CASE("CFDP TLV LV", "[CfdpTlvLv]") {
 
   SECTION("TLV Deserialization") {
     // Serialization was tested before, generate raw data now
-    std::array<uint8_t, 8> tlvRawBuf;
+    std::array<uint8_t, 8> tlvRawBuf{};
     serPtr = tlvRawBuf.data();
     result =
         sourceId.serialize(&serPtr, &deserSize, tlvRawBuf.size(), SerializeIF::Endianness::NETWORK);
@@ -136,7 +136,7 @@ TEST_CASE("CFDP TLV LV", "[CfdpTlvLv]") {
   }
 
   SECTION("LV Serialization") {
-    std::array<uint8_t, 8> lvRawBuf;
+    std::array<uint8_t, 8> lvRawBuf{};
     serPtr = lvRawBuf.data();
     result =
         sourceId.serialize(&serPtr, &deserSize, lvRawBuf.size(), SerializeIF::Endianness::NETWORK);

unittests/tcdistributor/testCcsdsDistributor.cpp

@@ -28,15 +28,25 @@ TEST_CASE("CCSDS Distributor", "[ccsds-distrib]") {
   SpacePacketCreator spCreator(spParams);
   std::array<uint8_t, 32> buf{};
 
-  auto createSpacePacket = [&](uint16_t apid, TmTcMessage& msg) {
+  auto createSpacePacket = [&](uint16_t apid, TmTcMessage& msg, uint8_t* dataField = nullptr,
+                               size_t dataFieldLen = 1) {
     store_address_t storeId{};
     spCreator.setApid(tcAcceptorApid);
+    spCreator.setCcsdsLenFromTotalDataFieldLen(dataFieldLen);
     uint8_t* dataPtr;
-    REQUIRE(pool.getFreeElement(&storeId, spCreator.getSerializedSize(), &dataPtr) == result::OK);
+    REQUIRE(pool.getFreeElement(&storeId, spCreator.getSerializedSize() + dataFieldLen, &dataPtr) ==
+            result::OK);
     size_t serLen = 0;
     REQUIRE(spCreator.SerializeIF::serializeBe(dataPtr, serLen, ccsds::HEADER_LEN) == result::OK);
     REQUIRE(spCreator.SerializeIF::serializeBe(buf.data(), serLen, ccsds::HEADER_LEN) ==
             result::OK);
+    if (dataField == nullptr) {
+      dataPtr[ccsds::HEADER_LEN] = 0;
+      buf[ccsds::HEADER_LEN] = 0;
+    } else {
+      std::memcpy(dataPtr + ccsds::HEADER_LEN, dataField, dataFieldLen);
+      std::memcpy(buf.data() + ccsds::HEADER_LEN, dataField, dataFieldLen);
+    }
     msg.setStorageId(storeId);
   };
 
@@ -57,7 +67,7 @@ TEST_CASE("CCSDS Distributor", "[ccsds-distrib]") {
     store_address_t storeId = message.getStorageId();
     queue.addReceivedMessage(message);
     REQUIRE(ccsdsDistrib.performOperation(0) == result::OK);
-    CHECK(checkerMock.checkedPacketLen == 6);
+    CHECK(checkerMock.checkedPacketLen == 7);
     CHECK(checkerMock.checkCallCount == 1);
     CHECK(queue.wasMessageSent());
     CHECK(queue.numberOfSentMessages() == 1);
@@ -68,7 +78,7 @@ TEST_CASE("CCSDS Distributor", "[ccsds-distrib]") {
     CHECK(sentMsg.getStorageId() == storeId);
     auto accessor = pool.getData(storeId);
     CHECK(accessor.first == result::OK);
-    CHECK(accessor.second.size() == ccsds::HEADER_LEN);
+    CHECK(accessor.second.size() == ccsds::HEADER_LEN + 1);
     for (size_t i = 0; i < ccsds::HEADER_LEN; i++) {
       CHECK(accessor.second.data()[i] == buf[i]);
     }
@@ -92,7 +102,7 @@ TEST_CASE("CCSDS Distributor", "[ccsds-distrib]") {
     message.setStorageId(storeId);
     queue.addReceivedMessage(message);
     REQUIRE(ccsdsDistrib.performOperation(0) == result::OK);
-    CHECK(checkerMock.checkedPacketLen == 6);
+    CHECK(checkerMock.checkedPacketLen == 7);
     CHECK(checkerMock.checkCallCount == 1);
     CHECK(queue.wasMessageSent());
     CHECK(queue.numberOfSentMessages() == 1);
@@ -103,11 +113,56 @@ TEST_CASE("CCSDS Distributor", "[ccsds-distrib]") {
     CHECK(sentMsg.getStorageId() == storeId);
     auto accessor = pool.getData(storeId);
     CHECK(accessor.first == result::OK);
-    CHECK(accessor.second.size() == ccsds::HEADER_LEN);
+    CHECK(accessor.second.size() == ccsds::HEADER_LEN + 1);
     for (size_t i = 0; i < ccsds::HEADER_LEN; i++) {
       CHECK(accessor.second.data()[i] == buf[i]);
     }
   }
 
-  SECTION("Remove CCSDS header") {}
+  SECTION("Remove CCSDS header") {
+    uint16_t tgtApid = 0;
+    MessageQueueId_t tgtQueueId = MessageQueueIF::NO_QUEUE;
+    SECTION("Default destination") {
+      CcsdsDistributor::DestInfo info(defReceiverMock, true);
+      tgtApid = defaultApid;
+      tgtQueueId = defaultQueueId;
+      REQUIRE(ccsdsDistrib.registerApplication(info) == result::OK);
+    }
+    SECTION("Specific destination") {
+      CcsdsDistributor::DestInfo info(tcAcceptorMock, true);
+      tgtApid = tcAcceptorApid;
+      tgtQueueId = tcAcceptorQueueId;
+      REQUIRE(ccsdsDistrib.registerApplication(info) == result::OK);
+    }
+    TmTcMessage message;
+    std::array<uint8_t, 5> dataField = {0, 1, 2, 3, 4};
+    createSpacePacket(tgtApid, message, dataField.data(), 5);
+    store_address_t storeId = message.getStorageId();
+    message.setStorageId(storeId);
+    queue.addReceivedMessage(message);
+    REQUIRE(ccsdsDistrib.performOperation(0) == result::OK);
+    CHECK(checkerMock.checkedPacketLen == 11);
+    CHECK(checkerMock.checkCallCount == 1);
+    // Data was deleted from old slot to re-store without the header
+    CHECK(not pool.hasDataAtId(storeId));
+    TmTcMessage sentMsg;
+    CHECK(queue.getNextSentMessage(tgtQueueId, sentMsg) == result::OK);
+    CHECK(sentMsg.getStorageId() != storeId);
+    auto accessor = pool.getData(sentMsg.getStorageId());
+    CHECK(accessor.first == result::OK);
+    CHECK(accessor.second.size() == 5);
+    // Verify correctness of data field
+    for (size_t i = 0; i < 5; i++) {
+      CHECK(accessor.second.data()[i] == i);
+    }
+  }
+
+  SECTION("Invalid Space Packet, Too Short") {
+    store_address_t storeId{};
+    std::array<uint8_t, 4> data = {1, 2, 3, 4};
+    pool.addData(&storeId, data.data(), data.size());
+    TmTcMessage message(storeId);
+    queue.addReceivedMessage(message);
+    REQUIRE(ccsdsDistrib.performOperation(0) == SerializeIF::STREAM_TOO_SHORT);
+  }
 }

unittests/tmtcpacket/testCcsdsCreator.cpp

@@ -39,7 +39,8 @@ TEST_CASE("CCSDS Creator", "[ccsds-creator]") {
   SECTION("Deserialization Fails") {
     serLen = 6;
     const uint8_t* readOnlyPtr = buf.data();
-    REQUIRE(base.deSerialize(&readOnlyPtr, &serLen, SerializeIF::Endianness::BIG) ==
+    SerializeIF& ser = dynamic_cast<SerializeIF&>(base);
+    REQUIRE(ser.deSerialize(&readOnlyPtr, &serLen, SerializeIF::Endianness::BIG) ==
             HasReturnvaluesIF::RETURN_FAILED);
   }
 
@@ -64,7 +65,7 @@ TEST_CASE("CCSDS Creator", "[ccsds-creator]") {
     base.setApid(static_cast<int>(std::pow(2, 11)) - 1);
     base.setSeqCount(static_cast<int>(std::pow(2, 14)) - 1);
     base.setSeqFlags(ccsds::SequenceFlags::UNSEGMENTED);
-    base.setDataLen(static_cast<int>(std::pow(2, 16)) - 1);
+    base.setDataLenField(static_cast<int>(std::pow(2, 16)) - 1);
     REQUIRE(base.isValid());
     REQUIRE(base.serializeBe(&bufPtr, &serLen, buf.size()) == HasReturnvaluesIF::RETURN_OK);
     CHECK(buf[0] == 0x1F);
@@ -75,6 +76,15 @@ TEST_CASE("CCSDS Creator", "[ccsds-creator]") {
     CHECK(buf[5] == 0xFF);
   }
 
+  SECTION("Setting data length 0 is ignored") {
+    SpacePacketCreator creator = SpacePacketCreator(
+        ccsds::PacketType::TC, true, 0xFFFF, ccsds::SequenceFlags::FIRST_SEGMENT, 0x34, 0x22);
+    creator.setCcsdsLenFromTotalDataFieldLen(0);
+    REQUIRE(creator.getPacketDataLen() == 0x22);
+    creator.setCcsdsLenFromTotalDataFieldLen(1);
+    REQUIRE(creator.getPacketDataLen() == 0x00);
+  }
+
   SECTION("Invalid APID") {
     SpacePacketCreator creator = SpacePacketCreator(
         ccsds::PacketType::TC, true, 0xFFFF, ccsds::SequenceFlags::FIRST_SEGMENT, 0x34, 0x16);