diff --git a/src/fsfw/tcdistribution/CcsdsDistributor.cpp b/src/fsfw/tcdistribution/CcsdsDistributor.cpp index f168e20aa..2e09e71f0 100644 --- a/src/fsfw/tcdistribution/CcsdsDistributor.cpp +++ b/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) { - // Do not call accessor release method here to ensure the old packet gets deleted. - return handleCcsdsHeaderRemoval(accessorPair.second); - } - } else { + } else if (iter == receiverMap.end()) { // 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; } diff --git a/src/fsfw/tcdistribution/CcsdsDistributor.h b/src/fsfw/tcdistribution/CcsdsDistributor.h index c2ada02da..0444a8687 100644 --- a/src/fsfw/tcdistribution/CcsdsDistributor.h +++ b/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; /** diff --git a/src/fsfw/tcdistribution/TcDistributorBase.cpp b/src/fsfw/tcdistribution/TcDistributorBase.cpp index 33c779eb8..e673d628b 100644 --- a/src/fsfw/tcdistribution/TcDistributorBase.cpp +++ b/src/fsfw/tcdistribution/TcDistributorBase.cpp @@ -40,8 +40,7 @@ ReturnValue_t TcDistributorBase::handlePacket() { if (result != HasReturnvaluesIF::RETURN_OK) { return result; } - result = tcQueue->sendMessage(destId, ¤tMessage); - return callbackAfterSending(result); + return callbackAfterSending(tcQueue->sendMessage(destId, ¤tMessage)); } ReturnValue_t TcDistributorBase::callbackAfterSending(ReturnValue_t queueStatus) { diff --git a/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.cpp b/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.cpp index ee27cc9b4..a946650b5 100644 --- a/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.cpp +++ b/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.cpp @@ -40,7 +40,13 @@ ReturnValue_t SpacePacketCreator::serialize(uint8_t **buffer, size_t *size, size return SerializeAdapter::serialize(¶ms.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 diff --git a/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.h b/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.h index 5869560d9..59234a1b2 100644 --- a/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.h +++ b/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 diff --git a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp b/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp index 2e786e6e2..b40724ba0 100644 --- a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp +++ b/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() + - 1); + spCreator.setCcsdsLenFromTotalDataFieldLen(ecss::PusTcDataFieldHeader::MIN_SIZE + + pusParams.dataWrapper.getLength() + + sizeof(ecss::PusChecksumT)); } size_t PusTcCreator::getSerializedSize() const { return spCreator.getFullPacketLen(); } diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp index 64dbbfeb0..eefcbec7f 100644 --- a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp +++ b/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() + - sizeof(ecss::PusChecksumT) - 1; + size_t headerLen = + 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); } diff --git a/unittests/cfdp/testTlvsLvs.cpp b/unittests/cfdp/testTlvsLvs.cpp index c251fd157..98dec980a 100644 --- a/unittests/cfdp/testTlvsLvs.cpp +++ b/unittests/cfdp/testTlvsLvs.cpp @@ -16,15 +16,15 @@ TEST_CASE("CFDP TLV LV", "[CfdpTlvLv]") { using namespace cfdp; - int result = HasReturnvaluesIF::RETURN_OK; - std::array rawBuf; + ReturnValue_t result; + std::array 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 tlvRawBuf; + std::array 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 tlvRawBuf; + std::array 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 lvRawBuf; + std::array lvRawBuf{}; serPtr = lvRawBuf.data(); result = sourceId.serialize(&serPtr, &deserSize, lvRawBuf.size(), SerializeIF::Endianness::NETWORK); diff --git a/unittests/tcdistributor/testCcsdsDistributor.cpp b/unittests/tcdistributor/testCcsdsDistributor.cpp index 8aa248fa9..30e969457 100644 --- a/unittests/tcdistributor/testCcsdsDistributor.cpp +++ b/unittests/tcdistributor/testCcsdsDistributor.cpp @@ -28,15 +28,25 @@ TEST_CASE("CCSDS Distributor", "[ccsds-distrib]") { SpacePacketCreator spCreator(spParams); std::array 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 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 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); + } } \ No newline at end of file diff --git a/unittests/tmtcpacket/testCcsdsCreator.cpp b/unittests/tmtcpacket/testCcsdsCreator.cpp index 465e64759..b4a089d0d 100644 --- a/unittests/tmtcpacket/testCcsdsCreator.cpp +++ b/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(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(std::pow(2, 11)) - 1); base.setSeqCount(static_cast(std::pow(2, 14)) - 1); base.setSeqFlags(ccsds::SequenceFlags::UNSEGMENTED); - base.setDataLen(static_cast(std::pow(2, 16)) - 1); + base.setDataLenField(static_cast(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);