fprime/Svc/SeqDispatcher/SeqDispatcher.cpp

// ======================================================================
// \title  SeqDispatcher.cpp
// \author zimri.leisher
// \brief  cpp file for SeqDispatcher component implementation class
// ======================================================================

#include <Svc/SeqDispatcher/SeqDispatcher.hpp>

namespace Svc {

// ----------------------------------------------------------------------
// Construction, initialization, and destruction
// ----------------------------------------------------------------------

SeqDispatcher ::SeqDispatcher(const char* const compName) : SeqDispatcherComponentBase(compName) {}

SeqDispatcher ::~SeqDispatcher() {}

FwIndexType SeqDispatcher::getNextAvailableSequencerIdx() {
    for (FwIndexType i = 0; i < SeqDispatcherSequencerPorts; i++) {
        if (this->isConnected_seqRunOut_OutputPort(i) &&
            this->m_entryTable[i].state == SeqDispatcher_CmdSequencerState::AVAILABLE) {
            return i;
        }
    }
    return -1;
}

void SeqDispatcher::runSequence(FwIndexType sequencerIdx, const Fw::ConstStringBase& fileName, Fw::Wait block) {
    // this function is only designed for internal usage
    // we can guarantee it cannot be called with input that would fail
    FW_ASSERT(sequencerIdx >= 0 && sequencerIdx < SeqDispatcherSequencerPorts,
              static_cast<FwAssertArgType>(sequencerIdx));
    FW_ASSERT(this->isConnected_seqRunOut_OutputPort(sequencerIdx));
    FW_ASSERT(this->m_entryTable[sequencerIdx].state == SeqDispatcher_CmdSequencerState::AVAILABLE,
              static_cast<FwAssertArgType>(this->m_entryTable[sequencerIdx].state));

    if (block == Fw::Wait::NO_WAIT) {
        this->m_entryTable[sequencerIdx].state = SeqDispatcher_CmdSequencerState::RUNNING_SEQUENCE_NO_BLOCK;
    } else {
        this->m_entryTable[sequencerIdx].state = SeqDispatcher_CmdSequencerState::RUNNING_SEQUENCE_BLOCK;
    }

    this->m_sequencersAvailable--;
    this->tlmWrite_sequencersAvailable(this->m_sequencersAvailable);
    this->m_entryTable[sequencerIdx].sequenceRunning = fileName;

    this->m_dispatchedCount++;
    this->tlmWrite_dispatchedCount(this->m_dispatchedCount);
    this->seqRunOut_out(sequencerIdx, this->m_entryTable[sequencerIdx].sequenceRunning);
}

void SeqDispatcher::seqStartIn_handler(FwIndexType portNum,            //!< The port number
                                       const Fw::StringBase& fileName  //!< The sequence file name
) {
    FW_ASSERT(portNum >= 0 && portNum < SeqDispatcherSequencerPorts, static_cast<FwAssertArgType>(portNum));
    if (this->m_entryTable[portNum].state == SeqDispatcher_CmdSequencerState::RUNNING_SEQUENCE_BLOCK ||
        this->m_entryTable[portNum].state == SeqDispatcher_CmdSequencerState::RUNNING_SEQUENCE_NO_BLOCK) {
        // we were aware of this sequencer running a sequence
        if (this->m_entryTable[portNum].sequenceRunning != fileName) {
            // uh oh. entry table is wrong
            // let's just update it to be correct. nothing we can do about
            // it except raise a warning and update our state
            this->log_WARNING_HI_ConflictingSequenceStarted(static_cast<U16>(portNum), fileName,
                                                            this->m_entryTable[portNum].sequenceRunning);
            this->m_entryTable[portNum].sequenceRunning = fileName;
        }
    } else {
        // we were not aware that this sequencer was running. ground must have
        // directly commanded that specific sequencer

        // warn because this may be unintentional
        this->log_WARNING_LO_UnexpectedSequenceStarted(static_cast<U16>(portNum), fileName);

        // update the state
        this->m_entryTable[portNum].state = SeqDispatcher_CmdSequencerState::RUNNING_SEQUENCE_NO_BLOCK;
        this->m_entryTable[portNum].sequenceRunning = fileName;
        this->m_sequencersAvailable--;
        this->tlmWrite_sequencersAvailable(this->m_sequencersAvailable);
    }
}

void SeqDispatcher::seqDoneIn_handler(FwIndexType portNum,             //!< The port number
                                      FwOpcodeType opCode,             //!< Command Op Code
                                      U32 cmdSeq,                      //!< Command Sequence
                                      const Fw::CmdResponse& response  //!< The command response argument
) {
    FW_ASSERT(portNum >= 0 && portNum < SeqDispatcherSequencerPorts, static_cast<FwAssertArgType>(portNum));
    if (this->m_entryTable[portNum].state != SeqDispatcher_CmdSequencerState::RUNNING_SEQUENCE_BLOCK &&
        this->m_entryTable[portNum].state != SeqDispatcher_CmdSequencerState::RUNNING_SEQUENCE_NO_BLOCK) {
        // this sequencer was not running a sequence that we were aware of.

        // we should have caught this in seqStartIn and updated the state
        // accordingly, but somehow we didn't? very sad and shouldn't happen

        // anyways, don't have to do anything cuz now that this seq we didn't know
        // about is done, the sequencer is available again (which is its current
        // state in our internal entry table already)
        this->log_WARNING_LO_UnknownSequenceFinished(static_cast<U16>(portNum));
    } else {
        // ok, a sequence has finished that we knew about
        if (this->m_entryTable[portNum].state == SeqDispatcher_CmdSequencerState::RUNNING_SEQUENCE_BLOCK) {
            // we need to give a cmd response cuz some other sequence is being blocked
            // by this
            this->cmdResponse_out(this->m_entryTable[portNum].opCode, this->m_entryTable[portNum].cmdSeq, response);

            if (response == Fw::CmdResponse::EXECUTION_ERROR) {
                // dispatched sequence errored
                this->m_errorCount++;
                this->tlmWrite_errorCount(this->m_errorCount);
            }
        }
    }

    // all command responses mean the sequence is no longer running
    // so component should be available
    this->m_entryTable[portNum].state = SeqDispatcher_CmdSequencerState::AVAILABLE;
    this->m_entryTable[portNum].sequenceRunning = "<no seq>";
    this->m_sequencersAvailable++;
    this->tlmWrite_sequencersAvailable(this->m_sequencersAvailable);
}

//! Handler for input port seqRunIn
void SeqDispatcher::seqRunIn_handler(FwIndexType portNum, const Fw::StringBase& fileName) {
    FwIndexType idx = this->getNextAvailableSequencerIdx();
    // no available sequencers
    if (idx == -1) {
        this->log_WARNING_HI_NoAvailableSequencers();
        return;
    }

    this->runSequence(idx, fileName, Fw::Wait::NO_WAIT);
}
// ----------------------------------------------------------------------
// Command handler implementations
// ----------------------------------------------------------------------

void SeqDispatcher ::RUN_cmdHandler(const FwOpcodeType opCode,
                                    const U32 cmdSeq,
                                    const Fw::CmdStringArg& fileName,
                                    Fw::Wait block) {
    FwIndexType idx = this->getNextAvailableSequencerIdx();
    // no available sequencers
    if (idx == -1) {
        this->log_WARNING_HI_NoAvailableSequencers();
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR);
        return;
    }

    this->runSequence(idx, fileName, block);

    if (block == Fw::Wait::NO_WAIT) {
        // return instantly
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
    } else {
        // otherwise don't return a response yet. just save the opCode and cmdSeq
        // so we can return a response later
        this->m_entryTable[idx].opCode = opCode;
        this->m_entryTable[idx].cmdSeq = cmdSeq;
    }
}

void SeqDispatcher::LOG_STATUS_cmdHandler(const FwOpcodeType opCode, /*!< The opcode*/
                                          const U32 cmdSeq) {        /*!< The command sequence number*/
    for (FwIndexType idx = 0; idx < SeqDispatcherSequencerPorts; idx++) {
        this->log_ACTIVITY_LO_LogSequencerStatus(static_cast<U16>(idx), this->m_entryTable[idx].state,
                                                 Fw::LogStringArg(this->m_entryTable[idx].sequenceRunning));
    }
    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
}
}  // namespace Svc