2022-07-29 12:03:14 +02:00
|
|
|
#include "fsfw/tcdistribution/CcsdsDistributor.h"
|
2020-10-01 13:23:06 +02:00
|
|
|
|
2022-08-01 11:00:36 +02:00
|
|
|
#include "definitions.h"
|
2022-08-01 17:16:37 +02:00
|
|
|
#include "fsfw/FSFW.h"
|
2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
2022-07-20 11:43:16 +02:00
|
|
|
#include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-07-27 17:13:42 +02:00
|
|
|
#define CCSDS_DISTRIBUTOR_DEBUGGING 0
|
2021-04-07 12:09:06 +02:00
|
|
|
|
2022-07-29 12:03:14 +02:00
|
|
|
CcsdsDistributor::CcsdsDistributor(uint16_t setDefaultApid, object_id_t setObjectId,
|
2022-08-01 17:16:37 +02:00
|
|
|
StorageManagerIF* tcStore, MessageQueueIF* queue,
|
2022-07-27 17:00:43 +02:00
|
|
|
CcsdsPacketCheckIF* packetChecker)
|
2022-08-01 17:16:37 +02:00
|
|
|
: TcDistributorBase(setObjectId, queue),
|
2022-08-01 14:23:52 +02:00
|
|
|
defaultApid(setDefaultApid),
|
2022-08-01 17:16:37 +02:00
|
|
|
tcStore(tcStore),
|
2022-08-01 14:23:52 +02:00
|
|
|
packetChecker(packetChecker) {}
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-08-03 11:36:56 +02:00
|
|
|
CcsdsDistributor::~CcsdsDistributor() {
|
|
|
|
if (ownedPacketChecker) {
|
|
|
|
delete packetChecker;
|
|
|
|
}
|
|
|
|
}
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-08-01 11:00:36 +02:00
|
|
|
ReturnValue_t CcsdsDistributor::selectDestination(MessageQueueId_t& destId) {
|
2021-04-07 12:09:06 +02:00
|
|
|
#if CCSDS_DISTRIBUTOR_DEBUGGING == 1
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::debug << "CCSDSDistributor::selectDestination received: "
|
|
|
|
<< this->currentMessage.getStorageId().poolIndex << ", "
|
|
|
|
<< this->currentMessage.getStorageId().packetIndex << std::endl;
|
2021-04-07 12:09:06 +02:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printDebug("CCSDSDistributor::selectDestination received: %d, %d\n",
|
|
|
|
currentMessage.getStorageId().poolIndex,
|
|
|
|
currentMessage.getStorageId().packetIndex);
|
2021-04-07 12:09:06 +02:00
|
|
|
#endif
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-08-01 17:16:37 +02:00
|
|
|
auto accessorPair = tcStore->getData(currentMessage.getStorageId());
|
|
|
|
ReturnValue_t result = accessorPair.first;
|
2022-08-22 16:35:53 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2021-04-07 12:09:06 +02:00
|
|
|
#if FSFW_VERBOSE_LEVEL >= 1
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "CCSDSDistributor::selectDestination: Getting data from"
|
|
|
|
" store failed!"
|
|
|
|
<< std::endl;
|
2021-04-07 12:09:06 +02:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printError(
|
|
|
|
"CCSDSDistributor::selectDestination: Getting data from"
|
|
|
|
" store failed!\n");
|
2021-04-07 12:09:06 +02:00
|
|
|
#endif
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-08-01 11:00:36 +02:00
|
|
|
return result;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2022-08-03 11:33:10 +02:00
|
|
|
// 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
|
2022-08-01 17:16:37 +02:00
|
|
|
return SerializeIF::STREAM_TOO_SHORT;
|
|
|
|
}
|
|
|
|
SpacePacketReader currentPacket(accessorPair.second.data(), accessorPair.second.size());
|
|
|
|
result = packetChecker->checkPacket(currentPacket, accessorPair.second.size());
|
2022-08-22 16:35:53 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-08-01 11:00:36 +02:00
|
|
|
handlePacketCheckFailure(result);
|
|
|
|
return result;
|
2022-07-27 17:00:43 +02:00
|
|
|
}
|
2021-04-07 12:09:06 +02:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1 && CCSDS_DISTRIBUTOR_DEBUGGING == 1
|
2022-07-27 17:00:43 +02:00
|
|
|
sif::info << "CCSDSDistributor::selectDestination has packet with APID 0x" << std::hex
|
|
|
|
<< currentPacket.getApid() << std::dec << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-08-01 14:23:52 +02:00
|
|
|
auto iter = receiverMap.find(currentPacket.getApid());
|
|
|
|
if (iter != receiverMap.end()) {
|
|
|
|
destId = iter->second.destId;
|
2022-08-03 11:33:10 +02:00
|
|
|
} else if (iter == receiverMap.end()) {
|
2022-02-02 10:29:30 +01:00
|
|
|
// The APID was not found. Forward packet to main SW-APID anyway to
|
2022-08-01 17:16:37 +02:00
|
|
|
// create acceptance failure report.
|
2022-08-01 14:23:52 +02:00
|
|
|
iter = receiverMap.find(defaultApid);
|
2022-08-01 11:00:36 +02:00
|
|
|
if (iter != receiverMap.end()) {
|
|
|
|
destId = iter->second.destId;
|
|
|
|
} else {
|
|
|
|
return DESTINATION_NOT_FOUND;
|
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2022-08-03 11:33:10 +02:00
|
|
|
if (iter->second.removeHeader) {
|
|
|
|
// Do not call accessor release method here to ensure the old packet gets deleted.
|
|
|
|
return handleCcsdsHeaderRemoval(accessorPair.second);
|
|
|
|
}
|
2022-08-01 17:16:37 +02:00
|
|
|
accessorPair.second.release();
|
2022-08-22 16:35:53 +02:00
|
|
|
return returnvalue::OK;
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-08-01 11:00:36 +02:00
|
|
|
void CcsdsDistributor::handlePacketCheckFailure(ReturnValue_t result) {
|
2022-08-01 14:23:52 +02:00
|
|
|
#if FSFW_VERBOSE_LEVEL >= 1
|
2022-08-01 11:00:36 +02:00
|
|
|
const char* reason = "Unknown reason";
|
2022-08-03 15:12:29 +02:00
|
|
|
if (result == tmtcdistrib::INVALID_CCSDS_VERSION) {
|
2022-08-01 11:00:36 +02:00
|
|
|
reason = "Invalid CCSDS version";
|
2022-08-08 12:31:19 +02:00
|
|
|
} else if (result == tmtcdistrib::INCOMPLETE_PACKET) {
|
2022-08-01 11:00:36 +02:00
|
|
|
reason = "Size missmatch between CCSDS data length and packet length";
|
2022-08-03 15:12:29 +02:00
|
|
|
} else if (result == tmtcdistrib::INVALID_APID) {
|
2022-08-01 11:00:36 +02:00
|
|
|
reason = "No valid handler APID found";
|
2022-08-03 15:12:29 +02:00
|
|
|
} else if (result == tmtcdistrib::INVALID_PACKET_TYPE) {
|
2022-08-01 11:00:36 +02:00
|
|
|
reason = "Invalid Packet Type TM detected";
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2022-08-01 11:00:36 +02:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
|
|
sif::warning << "CCSDS packet check failed: " << reason << std::endl;
|
|
|
|
#else
|
|
|
|
sif::printWarning("CCSDS packet check failed: %s\n", reason);
|
|
|
|
#endif
|
|
|
|
#endif
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-08-01 11:00:36 +02:00
|
|
|
MessageQueueId_t CcsdsDistributor::getRequestQueue() const { return tcQueue->getId(); }
|
|
|
|
|
|
|
|
ReturnValue_t CcsdsDistributor::registerApplication(DestInfo info) {
|
2022-08-22 16:35:53 +02:00
|
|
|
ReturnValue_t returnValue = returnvalue::OK;
|
2022-08-01 11:00:36 +02:00
|
|
|
auto insertPair = receiverMap.emplace(info.apid, info);
|
2022-02-02 10:29:30 +01:00
|
|
|
if (not insertPair.second) {
|
2022-08-22 16:35:53 +02:00
|
|
|
returnValue = returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
return returnValue;
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-08-01 11:00:36 +02:00
|
|
|
uint32_t CcsdsDistributor::getIdentifier() const { return 0; }
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-07-29 12:03:14 +02:00
|
|
|
ReturnValue_t CcsdsDistributor::initialize() {
|
2022-08-01 17:16:37 +02:00
|
|
|
ReturnValue_t result = TcDistributorBase::initialize();
|
2022-08-22 16:35:53 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-08-01 17:16:37 +02:00
|
|
|
return result;
|
|
|
|
}
|
2022-08-03 11:36:56 +02:00
|
|
|
if (packetChecker == nullptr) {
|
|
|
|
ownedPacketChecker = true;
|
|
|
|
packetChecker = new CcsdsPacketChecker(ccsds::PacketType::TC);
|
|
|
|
}
|
2022-08-01 17:16:37 +02:00
|
|
|
if (tcStore == nullptr) {
|
|
|
|
tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
|
|
|
|
if (tcStore == nullptr) {
|
2021-04-07 12:09:06 +02:00
|
|
|
#if FSFW_VERBOSE_LEVEL >= 1
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-08-01 17:16:37 +02:00
|
|
|
sif::error << "CCSDSDistributor::initialize: Could not initialize"
|
|
|
|
" TC store!"
|
|
|
|
<< std::endl;
|
2021-04-07 12:09:06 +02:00
|
|
|
#else
|
2022-08-01 17:16:37 +02:00
|
|
|
sif::printError(
|
|
|
|
"CCSDSDistributor::initialize: Could not initialize"
|
|
|
|
" TC store!\n");
|
2021-04-07 12:09:06 +02:00
|
|
|
#endif
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-08-01 17:16:37 +02:00
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2022-08-01 17:16:37 +02:00
|
|
|
return result;
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-07-29 12:03:14 +02:00
|
|
|
ReturnValue_t CcsdsDistributor::callbackAfterSending(ReturnValue_t queueStatus) {
|
2022-08-22 16:35:53 +02:00
|
|
|
if (queueStatus != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
tcStore->deleteData(currentMessage.getStorageId());
|
|
|
|
}
|
2022-08-22 16:35:53 +02:00
|
|
|
return returnvalue::OK;
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
2022-08-01 11:00:36 +02:00
|
|
|
|
|
|
|
void CcsdsDistributor::print() {
|
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
|
|
sif::debug << "Distributor content is: " << std::endl << "ID\t| Message Queue ID" << std::endl;
|
|
|
|
sif::debug << std::setfill('0') << std::setw(8) << std::hex;
|
|
|
|
for (const auto& iter : receiverMap) {
|
|
|
|
sif::debug << iter.first << "\t| 0x" << iter.second.destId
|
|
|
|
<< ", Header Removed: " << std::boolalpha << iter.second.removeHeader << std::endl;
|
|
|
|
}
|
|
|
|
sif::debug << std::setfill(' ') << std::dec;
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
const char* CcsdsDistributor::getName() const { return "CCSDS Distributor"; }
|
2022-08-01 14:23:52 +02:00
|
|
|
|
2022-08-01 17:16:37 +02:00
|
|
|
ReturnValue_t CcsdsDistributor::handleCcsdsHeaderRemoval(ConstStorageAccessor& accessor) {
|
|
|
|
store_address_t newStoreId;
|
|
|
|
ReturnValue_t result = tcStore->addData(&newStoreId, accessor.data() + ccsds::HEADER_LEN,
|
|
|
|
accessor.size() - ccsds::HEADER_LEN);
|
2022-08-22 16:35:53 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-08-01 14:23:52 +02:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-08-01 17:16:37 +02:00
|
|
|
sif::error << __func__ << ": TC store full" << std::endl;
|
2022-08-01 14:23:52 +02:00
|
|
|
#else
|
2022-08-01 17:16:37 +02:00
|
|
|
sif::printError("%s: TC store full\n", __func__);
|
2022-08-01 14:23:52 +02:00
|
|
|
#endif
|
2022-08-01 17:16:37 +02:00
|
|
|
return result;
|
2022-08-01 14:23:52 +02:00
|
|
|
}
|
2022-08-01 17:16:37 +02:00
|
|
|
currentMessage.setStorageId(newStoreId);
|
|
|
|
// The const accessor will delete the old data automatically
|
2022-08-22 16:35:53 +02:00
|
|
|
return returnvalue::OK;
|
2022-08-01 14:23:52 +02:00
|
|
|
}
|