eive-obsw/bsp_q7s/devices/PlocUpdater.cpp

#include "PlocUpdater.h"

#include <filesystem>
#include <fstream>
#include <string>

#include "fsfw/ipc/QueueFactory.h"

PlocUpdater::PlocUpdater(object_id_t objectId)
    : SystemObject(objectId), commandActionHelper(this), actionHelper(this, nullptr) {
  commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE);
}

PlocUpdater::~PlocUpdater() {}

ReturnValue_t PlocUpdater::initialize() {
#if BOARD_TE0720 == 0
  sdcMan = SdCardManager::instance();
#endif
  ReturnValue_t result = SystemObject::initialize();
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  result = commandActionHelper.initialize();
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
  result = actionHelper.initialize(commandQueue);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }

  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t PlocUpdater::performOperation(uint8_t operationCode) {
  readCommandQueue();
  doStateMachine();
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t PlocUpdater::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                         const uint8_t* data, size_t size) {
  ReturnValue_t result = RETURN_FAILED;

  if (state != State::IDLE) {
    return IS_BUSY;
  }

  if (size > MAX_PLOC_UPDATE_PATH) {
    return NAME_TOO_LONG;
  }

  switch (actionId) {
    case UPDATE_A_UBOOT:
      image = Image::A;
      partition = Partition::UBOOT;
      break;
    case UPDATE_A_BITSTREAM:
      image = Image::A;
      partition = Partition::BITSTREAM;
      break;
    case UPDATE_A_LINUX:
      image = Image::A;
      partition = Partition::LINUX_OS;
      break;
    case UPDATE_A_APP_SW:
      image = Image::A;
      partition = Partition::APP_SW;
      break;
    case UPDATE_B_UBOOT:
      image = Image::B;
      partition = Partition::UBOOT;
      break;
    case UPDATE_B_BITSTREAM:
      image = Image::B;
      partition = Partition::BITSTREAM;
      break;
    case UPDATE_B_LINUX:
      image = Image::B;
      partition = Partition::LINUX_OS;
      break;
    case UPDATE_B_APP_SW:
      image = Image::B;
      partition = Partition::APP_SW;
      break;
    default:
      return INVALID_ACTION_ID;
  }

  result = getImageLocation(data, size);

  if (result != RETURN_OK) {
    return result;
  }

  state = State::UPDATE_AVAILABLE;

  return EXECUTION_FINISHED;
}

MessageQueueId_t PlocUpdater::getCommandQueue() const { return commandQueue->getId(); }

MessageQueueIF* PlocUpdater::getCommandQueuePtr() { return commandQueue; }

void PlocUpdater::readCommandQueue() {
  CommandMessage message;
  ReturnValue_t result;

  for (result = commandQueue->receiveMessage(&message); result == HasReturnvaluesIF::RETURN_OK;
       result = commandQueue->receiveMessage(&message)) {
    if (result != RETURN_OK) {
      continue;
    }
    result = actionHelper.handleActionMessage(&message);
    if (result == HasReturnvaluesIF::RETURN_OK) {
      continue;
    }

    result = commandActionHelper.handleReply(&message);
    if (result == HasReturnvaluesIF::RETURN_OK) {
      continue;
    }

    sif::debug << "PlocUpdater::readCommandQueue: Received message with invalid format"
               << std::endl;
  }
}

void PlocUpdater::doStateMachine() {
  switch (state) {
    case State::IDLE:
      break;
    case State::UPDATE_AVAILABLE:
      commandUpdateAvailable();
      break;
    case State::UPDATE_TRANSFER:
      commandUpdatePacket();
      break;
    case State::UPDATE_VERIFY:
      commandUpdateVerify();
      break;
    case State::COMMAND_EXECUTING:
      break;
    default:
      sif::debug << "PlocUpdater::doStateMachine: Invalid state" << std::endl;
      break;
  }
}

ReturnValue_t PlocUpdater::checkNameLength(size_t size) {
  if (size > MAX_PLOC_UPDATE_PATH) {
    return NAME_TOO_LONG;
  }
  return RETURN_OK;
}

ReturnValue_t PlocUpdater::getImageLocation(const uint8_t* data, size_t size) {
  ReturnValue_t result = checkNameLength(size);
  if (result != RETURN_OK) {
    return result;
  }

#if BOARD_TE0720 == 0
  // Check if file is stored on SD card and if associated SD card is mounted
  if (std::string(reinterpret_cast<const char*>(data), SD_PREFIX_LENGTH) ==
      std::string(SdCardManager::SD_0_MOUNT_POINT)) {
    if (!sdcMan->isSdCardMounted(sd::SLOT_0)) {
      sif::warning << "PlocUpdater::getImageLocation: SD card 0 not mounted" << std::endl;
      return SD_NOT_MOUNTED;
    }
  } else if (std::string(reinterpret_cast<const char*>(data), SD_PREFIX_LENGTH) ==
             std::string(SdCardManager::SD_1_MOUNT_POINT)) {
    if (!sdcMan->isSdCardMounted(sd::SLOT_0)) {
      sif::warning << "PlocUpdater::getImageLocation: SD card 1 not mounted" << std::endl;
      return SD_NOT_MOUNTED;
    }
  } else {
    // update image not stored on SD card
  }
#endif /* BOARD_TE0720 == 0 */

  updateFile = std::string(reinterpret_cast<const char*>(data), size);

  // Check if file exists
  if (not std::filesystem::exists(updateFile)) {
    return FILE_NOT_EXISTS;
  }
  return RETURN_OK;
}

void PlocUpdater::stepSuccessfulReceived(ActionId_t actionId, uint8_t step) {}

void PlocUpdater::stepFailedReceived(ActionId_t actionId, uint8_t step, ReturnValue_t returnCode) {}

void PlocUpdater::dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) {}

void PlocUpdater::completionSuccessfulReceived(ActionId_t actionId) {
  switch (pendingCommand) {
    case (PLOC_SPV::UPDATE_AVAILABLE):
      state = State::UPDATE_TRANSFER;
      break;
    case (PLOC_SPV::UPDATE_IMAGE_DATA):
      if (remainingPackets == 0) {
        packetsSent = 0;  // Reset packets sent variable for next update sequence
        state = State::UPDATE_VERIFY;
      } else {
        state = State::UPDATE_TRANSFER;
      }
      break;
    case (PLOC_SPV::UPDATE_VERIFY):
      triggerEvent(UPDATE_FINISHED);
      state = State::IDLE;
      pendingCommand = PLOC_SPV::NONE;
      break;
    default:
      sif::debug << "PlocUpdater::completionSuccessfulReceived: Invalid pending command"
                 << std::endl;
      state = State::IDLE;
      break;
  }
}

void PlocUpdater::completionFailedReceived(ActionId_t actionId, ReturnValue_t returnCode) {
  switch (pendingCommand) {
    case (PLOC_SPV::UPDATE_AVAILABLE): {
      triggerEvent(UPDATE_AVAILABLE_FAILED);
      break;
    }
    case (PLOC_SPV::UPDATE_IMAGE_DATA): {
      triggerEvent(UPDATE_TRANSFER_FAILED, packetsSent);
      break;
    }
    case (PLOC_SPV::UPDATE_VERIFY): {
      triggerEvent(UPDATE_VERIFY_FAILED);
      break;
    }
    default:
      sif::debug << "PlocUpdater::completionFailedReceived: Invalid pending command " << std::endl;
      break;
  }
  state = State::IDLE;
}

void PlocUpdater::commandUpdateAvailable() {
  ReturnValue_t result = RETURN_OK;

  if (not std::filesystem::exists(updateFile)) {
    triggerEvent(UPDATE_FILE_NOT_EXISTS, static_cast<uint8_t>(state));
    state = State::IDLE;
    return;
  }

  std::ifstream file(updateFile, std::ifstream::binary);
  file.seekg(0, file.end);
  imageSize = static_cast<size_t>(file.tellg());
  file.close();

  numOfUpdatePackets = imageSize / MAX_SP_DATA;
  if (imageSize % MAX_SP_DATA) {
    numOfUpdatePackets++;
  }

  remainingPackets = numOfUpdatePackets;
  packetsSent = 0;

  calcImageCrc();

  PLOC_SPV::UpdateInfo packet(PLOC_SPV::APID_UPDATE_AVAILABLE, static_cast<uint8_t>(image),
                              static_cast<uint8_t>(partition), imageSize, imageCrc,
                              numOfUpdatePackets);

  result = commandActionHelper.commandAction(objects::PLOC_SUPERVISOR_HANDLER,
                                             PLOC_SPV::UPDATE_AVAILABLE, packet.getWholeData(),
                                             packet.getFullSize());
  if (result != RETURN_OK) {
    sif::warning << "PlocUpdater::commandUpdateAvailable: Failed to send update available"
                 << " packet to supervisor handler" << std::endl;
    triggerEvent(ACTION_COMMANDING_FAILED, result, PLOC_SPV::UPDATE_AVAILABLE);
    state = State::IDLE;
    pendingCommand = PLOC_SPV::NONE;
    return;
  }

  pendingCommand = PLOC_SPV::UPDATE_AVAILABLE;
  state = State::COMMAND_EXECUTING;
  return;
}

void PlocUpdater::commandUpdatePacket() {
  ReturnValue_t result = RETURN_OK;
  uint16_t payloadLength = 0;

  if (not std::filesystem::exists(updateFile)) {
    triggerEvent(UPDATE_FILE_NOT_EXISTS, static_cast<uint8_t>(state), packetsSent);
    state = State::IDLE;
    return;
  }

  std::ifstream file(updateFile, std::ifstream::binary);
  file.seekg(packetsSent * MAX_SP_DATA, file.beg);

  if (remainingPackets == 1) {
    payloadLength = imageSize - static_cast<uint16_t>(file.tellg());
  } else {
    payloadLength = MAX_SP_DATA;
  }

  PLOC_SPV::UpdatePacket packet(payloadLength);
  file.read(reinterpret_cast<char*>(packet.getDataFieldPointer()), payloadLength);
  file.close();
  // sequence count of first packet is 1
  packet.setPacketSequenceCount((packetsSent + 1) & PLOC_SPV::SEQUENCE_COUNT_MASK);
  if (numOfUpdatePackets > 1) {
    adjustSequenceFlags(packet);
  }
  packet.makeCrc();

  result = commandActionHelper.commandAction(objects::PLOC_SUPERVISOR_HANDLER,
                                             PLOC_SPV::UPDATE_IMAGE_DATA, packet.getWholeData(),
                                             packet.getFullSize());

  if (result != RETURN_OK) {
    sif::warning << "PlocUpdater::commandUpdateAvailable: Failed to send update"
                 << " packet to supervisor handler" << std::endl;
    triggerEvent(ACTION_COMMANDING_FAILED, result, PLOC_SPV::UPDATE_IMAGE_DATA);
    state = State::IDLE;
    pendingCommand = PLOC_SPV::NONE;
    return;
  }

  remainingPackets--;
  packetsSent++;

  pendingCommand = PLOC_SPV::UPDATE_IMAGE_DATA;
  state = State::COMMAND_EXECUTING;
}

void PlocUpdater::commandUpdateVerify() {
  ReturnValue_t result = RETURN_OK;

  PLOC_SPV::UpdateInfo packet(PLOC_SPV::APID_UPDATE_VERIFY, static_cast<uint8_t>(image),
                              static_cast<uint8_t>(partition), imageSize, imageCrc,
                              numOfUpdatePackets);

  result =
      commandActionHelper.commandAction(objects::PLOC_SUPERVISOR_HANDLER, PLOC_SPV::UPDATE_VERIFY,
                                        packet.getWholeData(), packet.getFullSize());
  if (result != RETURN_OK) {
    sif::warning << "PlocUpdater::commandUpdateAvailable: Failed to send update available"
                 << " packet to supervisor handler" << std::endl;
    triggerEvent(ACTION_COMMANDING_FAILED, result, PLOC_SPV::UPDATE_VERIFY);
    state = State::IDLE;
    pendingCommand = PLOC_SPV::NONE;
    return;
  }
  state = State::COMMAND_EXECUTING;
  pendingCommand = PLOC_SPV::UPDATE_VERIFY;
  return;
}

void PlocUpdater::calcImageCrc() {
  std::ifstream file(updateFile, std::ifstream::binary);
  file.seekg(0, file.end);
  uint32_t count;
  uint32_t bit;
  uint32_t remainder = INITIAL_REMAINDER_32;
  char input;
  for (count = 0; count < imageSize; count++) {
    file.seekg(count, file.beg);
    file.read(&input, 1);
    remainder ^= (input << 16);
    for (bit = 8; bit > 0; --bit) {
      if (remainder & TOPBIT_32) {
        remainder = (remainder << 1) ^ POLYNOMIAL_32;
      } else {
        remainder = (remainder << 1);
      }
    }
  }
  file.close();
  imageCrc = (remainder ^ FINAL_XOR_VALUE_32);
}

void PlocUpdater::adjustSequenceFlags(PLOC_SPV::UpdatePacket& packet) {
  if (packetsSent == 0) {
    packet.setSequenceFlags(static_cast<uint8_t>(PLOC_SPV::SequenceFlags::FIRST_PKT));
  } else if (remainingPackets == 1) {
    packet.setSequenceFlags(static_cast<uint8_t>(PLOC_SPV::SequenceFlags::LAST_PKT));
  } else {
    packet.setSequenceFlags(static_cast<uint8_t>(PLOC_SPV::SequenceFlags::CONTINUED_PKT));
  }
}