eive-obsw/mission/cfdp/CfdpHandler.cpp

#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;
}