eive-obsw/bsp_q7s/devices/ploc/PlocMPSoCHelper.cpp

#include "PlocMPSoCHelper.h"
#include "mission/utility/Timestamp.h"
#include "bsp_q7s/memory/FileSystemHelper.h"
#include <fstream>
#include <filesystem>

PlocMPSoCHelper::PlocMPSoCHelper(object_id_t objectId) : SystemObject(objectId){

}

PlocMPSoCHelper::~PlocMPSoCHelper() {
}

ReturnValue_t PlocMPSoCHelper::initialize() {
    sdcMan = SdCardManager::instance();
    if (sdcMan == nullptr) {
        sif::warning << "PlocMPSoCHelper::initialize: Invalid SD Card Manager" << std::endl;
        return RETURN_FAILED;
    }
    return RETURN_OK;
}

ReturnValue_t PlocMPSoCHelper::performOperation(uint8_t operationCode) {
    ReturnValue_t result = RETURN_OK;
    semaphore.acquire();
    while(true) {
        switch(internalState) {
        case InternalState::IDLE: {
            semaphore.acquire();
            break;
        }
        case InternalState::FLASH_WRITE: {
            result = performFlashWrite();
            if (result == RETURN_OK){
                triggerEvent(MPSOC_FLASH_WRITE_SUCCESSFUL);
            }
            else {
                triggerEvent(MPSOC_FLASH_WRITE_FAILED);
            }
            internalState = InternalState::IDLE;
            break;
        }
        default:
            sif::debug << "PlocMPSoCHelper::performOperation: Invalid state" << std::endl;
            break;
        }
    }
}

ReturnValue_t PlocMPSoCHelper::setComIF(DeviceCommunicationIF* communicationInterface_) {
    uartComIF = dynamic_cast<UartComIF*>(communicationInterface_);
    if (uartComIF == nullptr) {
        sif::warning << "PlocMPSoCHelper::initialize: Invalid uart com if" << std::endl;
        return RETURN_FAILED;
    }
    return RETURN_OK;
}

void PlocMPSoCHelper::setComCookie(CookieIF* comCookie_) {
    comCookie = comCookie_;
}

void PlocMPSoCHelper::setSequenceCount(SourceSequenceCounter* sequenceCount_) {
    sequenceCount = sequenceCount_;
}

ReturnValue_t PlocMPSoCHelper::startFlashWrite(std::string file) {
    ReturnValue_t result = FilesystemHelper::checkPath(file);
    if (result != RETURN_OK) {
        return result;
    }
    result = FilesystemHelper::fileExists(file);
    if (result != RETURN_OK) {
        return result;
    }
    flashWrite.file = file;
    internalState = InternalState::FLASH_WRITE;
    semaphore.release();
    terminate = false;
    return RETURN_OK;
}

void PlocMPSoCHelper::stopProcess() {
    terminate = true;
}

ReturnValue_t PlocMPSoCHelper::performFlashWrite() {
    ReturnValue_t result = RETURN_OK;
    uint8_t tempData[mpsoc::MAX_DATA_SIZE];
    std::ifstream file(flashWrite.file, std::ifstream::binary);
    // Set position of next character to end of file input stream
    file.seekg(0, file.end);
    // tellg returns position of character in input stream
    size_t remainingSize = file.tellg();
    size_t dataLength = 0;
    while (remainingSize > 0) {
        if (terminate) {
            return RETURN_OK;
        }
        if (remainingSize > mpsoc::MAX_DATA_SIZE) {
            dataLength = mpsoc::MAX_DATA_SIZE;
        }
        else {
            dataLength = remainingSize;
        }
        file.read(reinterpret_cast<char*>(tempData), dataLength);
        sequenceCount++;
        mpsoc::FlashWrite tc(*sequenceCount);
        tc.createPacket(tempData, dataLength);
        result = sendCommand(&tc);
        if (result != RETURN_OK) {
            return result;
        }
        result = handleAck();
        if (result != RETURN_OK) {
            return result;
        }
        result = handleExe();
        if (result != RETURN_OK) {
            return result;
        }
    }
    return result;
}

ReturnValue_t PlocMPSoCHelper::sendCommand(mpsoc::TcBase* tc) {
    ReturnValue_t result = RETURN_OK;
    result = uartComIF->sendMessage(comCookie, tc->getWholeData(), tc->getFullSize());
    if (result != RETURN_OK) {
        sif::warning << "PlocMPSoCHelper::sendCommand: Failed to send command" << std::endl;
        triggerEvent(SENDING_COMMAND_FAILED, result, static_cast<uint32_t>(internalState));
        return result;
    }
    return result;
}

ReturnValue_t PlocMPSoCHelper::handleAck() {
    ReturnValue_t result = RETURN_OK;
    mpsoc::TmPacket tmPacket;
    result = handleTmReception(&tmPacket, mpsoc::SIZE_ACK_REPORT);
    if (result != RETURN_OK) {
        return result;
    }
    uint16_t apid = tmPacket.getAPID();
    if (apid != mpsoc::apid::ACK_SUCCESS) {
        handleAckApidFailure(apid);
        return RETURN_FAILED;
    }
    return RETURN_OK;
}

void PlocMPSoCHelper::handleAckApidFailure(uint16_t apid) {
    if (apid == mpsoc::apid::ACK_FAILURE) {
        triggerEvent(ACK_FAILURE_REPORT, static_cast<uint32_t>(internalState));
        sif::warning << "PlocMPSoCHelper::handleAckApidFailure: Received acknowledgement failure "
                << "report" << std::endl;
    }
    else {
        triggerEvent(ACK_INVALID_APID, apid, static_cast<uint32_t>(internalState));
        sif::warning << "PlocMPSoCHelper::handleAckApidFailure: Expected acknowledgement report "
                << "but received space packet with apid " << std::hex << apid << std::endl;
    }
}

ReturnValue_t PlocMPSoCHelper::handleExe() {
    ReturnValue_t result = RETURN_OK;
    mpsoc::TmPacket tmPacket;
    result = handleTmReception(&tmPacket, mpsoc::SIZE_EXE_REPORT);
    if (result != RETURN_OK) {
        return result;
    }
    uint16_t apid = tmPacket.getAPID();
    if (apid != mpsoc::apid::EXE_SUCCESS) {
        handleExeApidFailure(apid);
        return RETURN_FAILED;
    }
    return RETURN_OK;
}

void PlocMPSoCHelper::handleExeApidFailure(uint16_t apid) {
    if (apid == mpsoc::apid::EXE_FAILURE) {
        triggerEvent(EXE_FAILURE_REPORT, static_cast<uint32_t>(internalState));
        sif::warning << "PlocMPSoCHelper::handleExeApidFailure: Received execution failure "
                << "report" << std::endl;
    }
    else {
        triggerEvent(EXE_INVALID_APID, apid, static_cast<uint32_t>(internalState));
        sif::warning << "PlocMPSoCHelper::handleAckApidFailure: Expected execution report "
                << "but received space packet with apid " << std::hex << apid << std::endl;
    }
}

ReturnValue_t PlocMPSoCHelper::handleTmReception(mpsoc::TmPacket* tmPacket, size_t remainingBytes) {
    ReturnValue_t result = RETURN_OK;
    size_t readBytes = 0;
    for(int retries = 0; retries < RETRIES; retries++) {
        result = receive(tmPacket->getWholeData(), &readBytes, remainingBytes);
        if (result != RETURN_OK) {
            return result;
        }
        remainingBytes = remainingBytes - readBytes;
        if (remainingBytes == 0) {
            break;
        }
    }
    if (remainingBytes != 0) {
        sif::warning << "PlocMPSoCHelper::handleTmReception: Failed to receive reply" << std::endl;
        triggerEvent(MISSING_EXE, remainingBytes, static_cast<uint32_t>(internalState));
        return RETURN_FAILED;
    }
    result = tmPacket->checkCrc();
    if (result != RETURN_OK) {
        sif::warning << "PlocMPSoCHelper::handleTmReception: CRC check failed" << std::endl;
        return result;
    }
    uint16_t recvSeqCnt = tmPacket->getPacketSequenceCount();
    if (recvSeqCnt != *sequenceCount) {
        triggerEvent(MPSOC_HELPER_SEQ_CNT_MISMATCH, *sequenceCount, recvSeqCnt);
        *sequenceCount = recvSeqCnt;
        return result;
    }
    return result;
}

ReturnValue_t PlocMPSoCHelper::receive(uint8_t* data, size_t* readBytes, size_t requestBytes) {
    ReturnValue_t result = RETURN_OK;
    result = uartComIF->requestReceiveMessage(comCookie, requestBytes);
    if (result != RETURN_OK) {
        sif::warning << "PlocMPSoCHelper::receive: Failed to request reply" << std::endl;
        triggerEvent(MPSOC_HELPER_REQUESTING_REPLY_FAILED, result,
                static_cast<uint32_t>(static_cast<uint32_t>(internalState)));
        return RETURN_FAILED;
    }
    result = uartComIF->readReceivedMessage(comCookie, &data, readBytes);
    if (result != RETURN_OK) {
        sif::warning << "PlocMPSoCHelper::receive: Failed to read received message" << std::endl;
        triggerEvent(MPSOC_HELPER_READING_REPLY_FAILED, result,
                static_cast<uint32_t>(internalState));
        return RETURN_FAILED;
    }
    return result;
}