Init Commit
This commit is contained in:
@@ -0,0 +1,253 @@
|
||||
#include "CfdpHandler.h"
|
||||
|
||||
#include <fsfw/cfdp/CfdpMessage.h>
|
||||
#include <fsfw/ipc/CommandMessage.h>
|
||||
|
||||
#include "eive/definitions.h"
|
||||
#include "fsfw/cfdp/pdu/AckPduReader.h"
|
||||
#include "fsfw/cfdp/pdu/PduHeaderReader.h"
|
||||
#include "fsfw/globalfunctions/arrayprinter.h"
|
||||
#include "fsfw/ipc/QueueFactory.h"
|
||||
#include "fsfw/tasks/TaskFactory.h"
|
||||
#include "fsfw/tmtcservices/TmTcMessage.h"
|
||||
#include "mission/sysDefs.h"
|
||||
|
||||
using namespace returnvalue;
|
||||
using namespace cfdp;
|
||||
|
||||
CfdpHandler::CfdpHandler(const FsfwHandlerParams& fsfwHandlerParams, const CfdpHandlerCfg& cfdpCfg,
|
||||
const std::atomic_bool& throttleSignal)
|
||||
: SystemObject(fsfwHandlerParams.objectId),
|
||||
pduQueue(fsfwHandlerParams.tmtcQueue),
|
||||
cfdpRequestQueue(fsfwHandlerParams.cfdpQueue),
|
||||
localCfg(cfdpCfg.id, cfdpCfg.indicCfg, cfdpCfg.faultHandler),
|
||||
remoteCfgProvider(cfdpCfg.remoteCfgProvider),
|
||||
fsfwParams(fsfwHandlerParams.packetDest, &fsfwHandlerParams.tmtcQueue, this,
|
||||
fsfwHandlerParams.tcStore, fsfwHandlerParams.tmStore),
|
||||
destHandler(DestHandlerParams(localCfg, cfdpCfg.userHandler, cfdpCfg.remoteCfgProvider,
|
||||
cfdpCfg.packetInfoList, cfdpCfg.lostSegmentsList),
|
||||
this->fsfwParams),
|
||||
srcHandler(SourceHandlerParams(localCfg, cfdpCfg.userHandler, seqCntProvider),
|
||||
this->fsfwParams),
|
||||
ipcStore(fsfwHandlerParams.ipcStore),
|
||||
throttleSignal(throttleSignal) {}
|
||||
|
||||
[[nodiscard]] const char* CfdpHandler::getName() const { return "CFDP Handler"; }
|
||||
|
||||
[[nodiscard]] uint32_t CfdpHandler::getIdentifier() const {
|
||||
return destHandler.getDestHandlerParams().cfg.localId.getValue();
|
||||
}
|
||||
|
||||
[[nodiscard]] MessageQueueId_t CfdpHandler::getRequestQueue() const { return pduQueue.getId(); }
|
||||
|
||||
ReturnValue_t CfdpHandler::initialize() {
|
||||
ReturnValue_t result = destHandler.initialize();
|
||||
if (result != OK) {
|
||||
return result;
|
||||
}
|
||||
tcStore = destHandler.getTcStore();
|
||||
tmStore = destHandler.getTmStore();
|
||||
|
||||
return SystemObject::initialize();
|
||||
}
|
||||
|
||||
[[noreturn]] ReturnValue_t CfdpHandler::performOperation(uint8_t operationCode) {
|
||||
while (true) {
|
||||
bool shortDelay = false;
|
||||
ReturnValue_t result = handlePduPacketMessages();
|
||||
if (result != OK) {
|
||||
}
|
||||
result = handleCfdpMessages();
|
||||
if (result != OK) {
|
||||
}
|
||||
uint32_t fsmCount = 1;
|
||||
const DestHandler::FsmResult& destResult = destHandler.stateMachine();
|
||||
while (destResult.callStatus == CallStatus::CALL_AGAIN) {
|
||||
if (fsmCount == config::CFDP_MAX_FSM_CALL_COUNT_DEST_HANDLER) {
|
||||
shortDelay = true;
|
||||
break;
|
||||
}
|
||||
destHandler.stateMachine();
|
||||
fsmCount++;
|
||||
}
|
||||
fsmCount = 1;
|
||||
|
||||
throttlePeriodOngoing = throttleSignal;
|
||||
|
||||
// CFDP can be throttled by the slowest live TM handler to handle back pressure in a sensible
|
||||
// way without requiring huge amounts of memory for large files.
|
||||
if (!throttlePeriodOngoing) {
|
||||
const SourceHandler::FsmResult& srcResult = srcHandler.stateMachine();
|
||||
if (srcResult.packetsSent > 0) {
|
||||
signals::CFDP_MSG_COUNTER.fetch_add(srcResult.packetsSent, std::memory_order_relaxed);
|
||||
}
|
||||
while (srcResult.callStatus == CallStatus::CALL_AGAIN) {
|
||||
// Limit number of messages.
|
||||
if (fsmCount == config::CFDP_MAX_FSM_CALL_COUNT_SRC_HANDLER) {
|
||||
shortDelay = true;
|
||||
break;
|
||||
}
|
||||
srcHandler.stateMachine();
|
||||
if (srcResult.packetsSent > 0) {
|
||||
signals::CFDP_MSG_COUNTER.fetch_add(srcResult.packetsSent, std::memory_order_relaxed);
|
||||
}
|
||||
if (srcResult.result == cfdp::TM_STORE_FULL) {
|
||||
sif::warning << "CFDP Source Handler: TM store is full" << std::endl;
|
||||
} else if (srcResult.result == cfdp::TARGET_MSG_QUEUE_FULL) {
|
||||
sif::warning << "CFDP Source Handler: TM queue is full" << std::endl;
|
||||
}
|
||||
fsmCount++;
|
||||
}
|
||||
}
|
||||
if (shortDelay) {
|
||||
TaskFactory::delayTask(config::CFDP_SHORT_DELAY_MS);
|
||||
continue;
|
||||
}
|
||||
TaskFactory::delayTask(config::CFDP_REGULAR_DELAY_MS);
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t CfdpHandler::handlePduPacket(TmTcMessage& msg) {
|
||||
auto accessorPair = tcStore->getData(msg.getStorageId());
|
||||
if (accessorPair.first != OK) {
|
||||
return accessorPair.first;
|
||||
}
|
||||
PduHeaderReader reader(accessorPair.second.data(), accessorPair.second.size());
|
||||
ReturnValue_t result = reader.parseData();
|
||||
if (result != returnvalue::OK) {
|
||||
return INVALID_PDU_FORMAT;
|
||||
}
|
||||
// The CFDP distributor should have taken care of ensuring the destination ID is correct
|
||||
PduType type = reader.getPduType();
|
||||
// Only the destination handler can process these PDUs
|
||||
if (type == PduType::FILE_DATA) {
|
||||
// Disable auto-deletion of packet
|
||||
accessorPair.second.release();
|
||||
PacketInfo info(type, msg.getStorageId());
|
||||
result = destHandler.passPacket(info);
|
||||
} else {
|
||||
// Route depending on PDU type and directive type if applicable. It retrieves directive type
|
||||
// from the raw stream for better performance (with sanity and directive code check).
|
||||
// The routing is based on section 4.5 of the CFDP standard which specifies the PDU forwarding
|
||||
// procedure.
|
||||
|
||||
// PDU header only. Invalid supplied data. A directive packet should have a valid data field
|
||||
// with at least one byte being the directive code
|
||||
const uint8_t* pduDataField = reader.getPduDataField();
|
||||
if (pduDataField == nullptr) {
|
||||
return INVALID_PDU_FORMAT;
|
||||
}
|
||||
if (not FileDirectiveReader::checkFileDirective(pduDataField[0])) {
|
||||
sif::error << "CfdpHandler: Invalid PDU directive field " << static_cast<int>(pduDataField[0])
|
||||
<< std::endl;
|
||||
return INVALID_DIRECTIVE_FIELD;
|
||||
}
|
||||
auto directive = static_cast<FileDirective>(pduDataField[0]);
|
||||
|
||||
auto passToDestHandler = [&]() {
|
||||
accessorPair.second.release();
|
||||
PacketInfo info(type, msg.getStorageId(), directive);
|
||||
return destHandler.passPacket(info);
|
||||
};
|
||||
auto passToSourceHandler = [&]() {
|
||||
accessorPair.second.release();
|
||||
PacketInfo info(type, msg.getStorageId(), directive);
|
||||
// Implement this function.
|
||||
// result = srcHandler.passPacket(info);
|
||||
};
|
||||
if (directive == FileDirective::METADATA or directive == FileDirective::EOF_DIRECTIVE or
|
||||
directive == FileDirective::PROMPT) {
|
||||
// Section b) of 4.5.3: These PDUs should always be targeted towards the file receiver a.k.a.
|
||||
// the destination handler
|
||||
return passToDestHandler();
|
||||
} else if (directive == FileDirective::FINISH or directive == FileDirective::NAK or
|
||||
directive == FileDirective::KEEP_ALIVE) {
|
||||
// Section c) of 4.5.3: These PDUs should always be targeted towards the file sender a.k.a.
|
||||
// the source handler
|
||||
passToSourceHandler();
|
||||
} else if (directive == FileDirective::ACK) {
|
||||
// Section a): Recipient depends of the type of PDU that is being acknowledged. We can simply
|
||||
// extract the PDU type from the raw stream. If it is an EOF PDU, this packet is passed to
|
||||
// the source handler, for a Finished PDU, it is passed to the destination handler.
|
||||
FileDirective ackedDirective;
|
||||
if (not AckPduReader::checkAckedDirectiveField(pduDataField[1], ackedDirective)) {
|
||||
return INVALID_ACK_DIRECTIVE_FIELDS;
|
||||
}
|
||||
if (ackedDirective == FileDirective::EOF_DIRECTIVE) {
|
||||
passToSourceHandler();
|
||||
} else if (ackedDirective == FileDirective::FINISH) {
|
||||
return passToDestHandler();
|
||||
}
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t CfdpHandler::handleCfdpRequest(CommandMessage& msg) {
|
||||
if (msg.getCommand() == CfdpMessage::PUT_REQUEST) {
|
||||
sif::info << "Received CFDP put request" << std::endl;
|
||||
if (srcHandler.getState() != CfdpState::IDLE) {
|
||||
if (putRequestQueue.full()) {
|
||||
// TODO: Trigger event and discard request. Queue is full, too many requests.
|
||||
return FAILED;
|
||||
}
|
||||
putRequestQueue.push(CfdpMessage::getStoreId(&msg));
|
||||
} else {
|
||||
PutRequest putRequest;
|
||||
auto accessorPair = ipcStore.getData(CfdpMessage::getStoreId(&msg));
|
||||
const uint8_t* dataPtr = accessorPair.second.data();
|
||||
size_t dataSize = accessorPair.second.size();
|
||||
ReturnValue_t result =
|
||||
putRequest.deSerialize(&dataPtr, &dataSize, SerializeIF::Endianness::MACHINE);
|
||||
if (result != OK) {
|
||||
return result;
|
||||
}
|
||||
RemoteEntityCfg* remoteCfg;
|
||||
remoteCfgProvider.getRemoteCfg(putRequest.getDestId(), &remoteCfg);
|
||||
if (remoteCfg == nullptr) {
|
||||
sif::error << "CfdpHandler: No remote configuration found for destination ID "
|
||||
<< putRequest.getDestId() << std::endl;
|
||||
// TODO: Trigger event
|
||||
return FAILED;
|
||||
}
|
||||
sif::info << "Starting file copy operation for source file "
|
||||
<< putRequest.getSourceName().getString() << " and dest file "
|
||||
<< putRequest.getDestName().getString() << std::endl;
|
||||
return srcHandler.transactionStart(putRequest, *remoteCfg);
|
||||
}
|
||||
}
|
||||
return OK;
|
||||
}
|
||||
|
||||
ReturnValue_t CfdpHandler::handlePduPacketMessages() {
|
||||
ReturnValue_t status;
|
||||
ReturnValue_t result = OK;
|
||||
TmTcMessage pduMsg;
|
||||
for (status = pduQueue.receiveMessage(&pduMsg); status == returnvalue::OK;
|
||||
status = pduQueue.receiveMessage(&pduMsg)) {
|
||||
result = handlePduPacket(pduMsg);
|
||||
if (result != OK) {
|
||||
// TODO: Maybe add printout with context specific information?
|
||||
status = result;
|
||||
}
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
ReturnValue_t CfdpHandler::handleCfdpMessages() {
|
||||
ReturnValue_t status;
|
||||
ReturnValue_t result;
|
||||
CommandMessage cfdpMsg;
|
||||
for (status = cfdpRequestQueue.receiveMessage(&cfdpMsg); status == returnvalue::OK;
|
||||
status = cfdpRequestQueue.receiveMessage(&cfdpMsg)) {
|
||||
result = handleCfdpRequest(cfdpMsg);
|
||||
if (result != OK) {
|
||||
sif::warning << "Handling CFDP request failed with code 0x" << std::setw(4) << std::hex
|
||||
<< result << std::dec << std::endl;
|
||||
triggerEvent(cfdp::events::HANDLING_CFDP_REQUEST_FAILED, 0, result);
|
||||
// TODO: Maybe add printout with context specific information?
|
||||
status = result;
|
||||
}
|
||||
}
|
||||
return status;
|
||||
}
|
||||
Reference in New Issue
Block a user