FpySequencer.cpp

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

#include <Svc/FpySequencer/FpySequencer.hpp>
#include <new>

namespace Svc {

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

FpySequencer ::FpySequencer(const char* const compName)
    : FpySequencerComponentBase(compName),
      m_sequenceBuffer(),
      m_allocatorId(0),
      m_sequenceFilePath("<invalid_seq>"),
      m_sequenceObj(),
      m_computedCRC(0),
      m_sequenceBlockState(),
      m_savedOpCode(0),
      m_savedCmdSeq(0),
      m_goalState(),
      m_sequencesStarted(0),
      m_statementsDispatched(0),
      m_runtime(),
      m_breakpoint(),
      m_tlm() {}

FpySequencer ::~FpySequencer() {}

//! Handler for command RUN
//!
//! Loads, validates and runs a sequence
void FpySequencer::RUN_cmdHandler(FwOpcodeType opCode,               //!< The opcode
                                  U32 cmdSeq,                        //!< The command sequence number
                                  const Fw::CmdStringArg& fileName,  //!< The name of the sequence file
                                  FpySequencer_BlockState block      //!< Return command status when complete or not
) {
    // Empty args and delegate to RUN_ARGS handler
    this->RUN_ARGS_cmdHandler(opCode, cmdSeq, fileName, block, Svc::SeqArgs{0, 0});
}

void FpySequencer ::RUN_ARGS_cmdHandler(
    FwOpcodeType opCode,                 //!< The opcode
    U32 cmdSeq,                          //!< The command sequence number
    const Fw::CmdStringArg& fileName,    //!< The name of the sequence file
    Svc::FpySequencer_BlockState block,  //!< Return command status when complete or not
    Svc::SeqArgs args                    //!< Arguments to pass to the sequencer
) {
    // can only run a seq while in idle
    if (sequencer_getState() != State::IDLE) {
        this->log_WARNING_HI_InvalidCommand(static_cast<I32>(sequencer_getState()));
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR);
        return;
    }

    if (block == FpySequencer_BlockState::BLOCK) {
        // save the opCode and cmdSeq so we can respond later
        this->m_savedOpCode = opCode;
        this->m_savedCmdSeq = cmdSeq;
    }

    // Store args for pushArgsToStack action
    this->sequencer_sendSignal_cmd_RUN(FpySequencer_SequenceExecutionArgs(fileName, block, args));

    // only respond if the user doesn't want us to block further execution
    if (block == FpySequencer_BlockState::NO_BLOCK) {
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
    }
}

//! Handler for command VALIDATE
//!
//! Loads and validates a sequence
void FpySequencer::VALIDATE_cmdHandler(FwOpcodeType opCode,              //!< The opcode
                                       U32 cmdSeq,                       //!< The command sequence number
                                       const Fw::CmdStringArg& fileName  //!< The name of the sequence file
) {
    this->VALIDATE_ARGS_cmdHandler(opCode, cmdSeq, fileName, Svc::SeqArgs{0, 0});
}

//! Handler implementation for command VALIDATE_ARGS
//!
//! Loads and validates a sequence with arguments
void FpySequencer ::VALIDATE_ARGS_cmdHandler(FwOpcodeType opCode,
                                             U32 cmdSeq,
                                             const Fw::CmdStringArg& fileName,
                                             Svc::SeqArgs buffer) {
    // can only validate a seq while in idle
    if (sequencer_getState() != State::IDLE) {
        this->log_WARNING_HI_InvalidCommand(static_cast<I32>(sequencer_getState()));
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR);
        return;
    }

    // validate always blocks until finished, so save opcode/cmdseq
    // so we can respond once done
    this->m_savedOpCode = opCode;
    this->m_savedCmdSeq = cmdSeq;

    // VALIDATE_ARGS receives args via command interface
    // Store args for pushArgsToStack action when RUN_VALIDATED is called
    this->sequencer_sendSignal_cmd_VALIDATE(
        FpySequencer_SequenceExecutionArgs(fileName, FpySequencer_BlockState::BLOCK, buffer));
}

void FpySequencer::RUN_VALIDATED_cmdHandler(
    FwOpcodeType opCode,           //!< The opcode
    U32 cmdSeq,                    //!< The command sequence number
    FpySequencer_BlockState block  //!< Return command status when complete or not
) {
    // can only RUN_VALIDATED if we have validated and are awaiting this exact cmd
    if (sequencer_getState() != State::AWAITING_CMD_RUN_VALIDATED) {
        this->log_WARNING_HI_InvalidCommand(static_cast<I32>(sequencer_getState()));
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR);
        return;
    }

    if (block == FpySequencer_BlockState::BLOCK) {
        // save the opCode and cmdSeq so we can respond later
        this->m_savedOpCode = opCode;
        this->m_savedCmdSeq = cmdSeq;
    }

    this->sequencer_sendSignal_cmd_RUN_VALIDATED(
        FpySequencer_SequenceExecutionArgs(this->m_sequenceFilePath, block, this->m_sequenceArgs));

    // only respond if the user doesn't want us to block further execution
    if (block == FpySequencer_BlockState::NO_BLOCK) {
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
    }
}

//! Handler for command CANCEL
//!
//! Cancels a running or validated sequence
void FpySequencer::CANCEL_cmdHandler(FwOpcodeType opCode,  //!< The opcode
                                     U32 cmdSeq            //!< The command sequence number
) {
    // only state you can't cancel in is IDLE
    if (sequencer_getState() == State::IDLE) {
        this->log_WARNING_HI_InvalidCommand(static_cast<I32>(sequencer_getState()));
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR);
        return;
    }

    this->sequencer_sendSignal_cmd_CANCEL();

    // cancel returns immediately and always succeeds
    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
}

//! Handler for command SET_BREAKPOINT
//!
//! Sets the breakpoint which will pause the execution of the sequencer when
//! reached, until unpaused by the CONTINUE command. Will pause just before
//! dispatching the specified statement. This command is valid in all states. Breakpoint
//! settings are cleared after a sequence ends execution.
void FpySequencer::SET_BREAKPOINT_cmdHandler(FwOpcodeType opCode,  //!< The opcode
                                             U32 cmdSeq,           //!< The command sequence number
                                             U32 stmtIdx,          //!< The statement index to pause execution before.
                                             bool breakOnce  //!< Whether or not to break only once at this breakpoint
) {
    this->sequencer_sendSignal_cmd_SET_BREAKPOINT(FpySequencer_BreakpointArgs(true, breakOnce, stmtIdx));

    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
}

//! Handler for command BREAK
//!
//! Pauses the execution of the sequencer, just before it is about to dispatch the next statement,
//! until unpaused by the CONTINUE command, or stepped by the STEP command. This command is only valid
//! in substates of the RUNNING state that are not RUNNING.PAUSED.
void FpySequencer::BREAK_cmdHandler(FwOpcodeType opCode,  //!< The opcode
                                    U32 cmdSeq            //!< The command sequence number
) {
    if (!this->isRunningState(this->sequencer_getState()) || this->sequencer_getState() == State::RUNNING_PAUSED) {
        // can only break while running, and not paused
        this->log_WARNING_HI_InvalidCommand(static_cast<I32>(sequencer_getState()));
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR);
        return;
    }
    this->sequencer_sendSignal_cmd_BREAK();

    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
}

//! Handler for command CONTINUE
//!
//! Continues the automatic execution of the sequence after it has been paused. If a breakpoint is still
//! set, it may pause again on that breakpoint. This command is only valid in the RUNNING.PAUSED state.
void FpySequencer::CONTINUE_cmdHandler(FwOpcodeType opCode,  //!< The opcode
                                       U32 cmdSeq            //!< The command sequence number
) {
    if (this->sequencer_getState() != State::RUNNING_PAUSED) {
        this->log_WARNING_HI_InvalidCommand(static_cast<I32>(sequencer_getState()));
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR);
        return;
    }

    this->sequencer_sendSignal_cmd_CONTINUE();

    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
}

//! Handler for command CLEAR_BREAKPOINT
//!
//! Clears the breakpoint, but does not continue executing the sequence. This command
//! is valid in all states. This happens automatically when a sequence ends execution.
void FpySequencer::CLEAR_BREAKPOINT_cmdHandler(FwOpcodeType opCode,  //!< The opcode
                                               U32 cmdSeq            //!< The command sequence number
) {
    this->sequencer_sendSignal_cmd_CLEAR_BREAKPOINT();
    this->log_ACTIVITY_HI_BreakpointCleared();

    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
}

//! Handler for command STEP
//!
//! Dispatches and awaits the result of the next directive, or ends the sequence if no more directives remain.
//! Returns to the RUNNING.PAUSED state if the directive executes successfully. This command is only valid in the
//! RUNNING.PAUSED state.
void FpySequencer::STEP_cmdHandler(FwOpcodeType opCode,  //!< The opcode
                                   U32 cmdSeq            //!< The command sequence number
) {
    if (this->sequencer_getState() != State::RUNNING_PAUSED) {
        this->log_WARNING_HI_InvalidCommand(static_cast<I32>(sequencer_getState()));
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR);
        return;
    }

    this->sequencer_sendSignal_cmd_STEP();
    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
}

//! Handler for command DUMP_STACK_TO_FILE
//!
//! Writes the contents of the stack to a file. This command is only valid in the RUNNING.PAUSED state.
void FpySequencer::DUMP_STACK_TO_FILE_cmdHandler(FwOpcodeType opCode,              //!< The opcode
                                                 U32 cmdSeq,                       //!< The command sequence number
                                                 const Fw::CmdStringArg& fileName  //!< The name of the output file
) {
    if (this->sequencer_getState() != State::RUNNING_PAUSED) {
        this->log_WARNING_HI_InvalidCommand(static_cast<I32>(sequencer_getState()));
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR);
        return;
    }
    Os::File sequenceFile;
    Os::File::Status status = sequenceFile.open(fileName.toChar(), Os::File::OPEN_WRITE);

    if (status != Os::File::Status::OP_OK) {
        this->log_WARNING_HI_FileOpenError(this->m_sequenceFilePath, static_cast<I32>(status));
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR);
        return;
    }

    FwSizeType writeSize = static_cast<FwSizeType>(this->m_runtime.stack.size);
    status = sequenceFile.write(this->m_runtime.stack.bytes, writeSize);
    if (status != Os::File::Status::OP_OK || writeSize != this->m_runtime.stack.size) {
        this->log_WARNING_HI_FileWriteError(writeSize, fileName, static_cast<I32>(status));
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::EXECUTION_ERROR);
        return;
    }
    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
    return;
}
//! Handler for input port checkTimers
void FpySequencer::checkTimers_handler(FwIndexType portNum,  //!< The port number
                                       U32 context           //!< The call order
) {
    this->sequencer_sendSignal_checkTimersIn();
}

void FpySequencer::pingIn_handler(FwIndexType portNum, /*!< The port number*/
                                  U32 key              /*!< Value to return to pinger*/
) {
    // send ping response
    this->pingOut_out(0, key);
}

//! Handler for input port cmdResponseIn
void FpySequencer::cmdResponseIn_handler(FwIndexType portNum,             //!< The port number
                                         FwOpcodeType opCode,             //!< Command Op Code
                                         U32 cmdSeq,                      //!< Command Sequence
                                         const Fw::CmdResponse& response  //!< The command response argument
) {
    // if we aren't in the RUNNING state:
    if (!this->isRunningState(sequencer_getState())) {
        // must be a coding error from an outside component (off nom), or due to CANCEL while running a command (nom).
        // because we can't be sure that it wasn't a nominal sequence of events leading to this, don't fail the
        // sequence, just report it
        this->log_WARNING_LO_CmdResponseWhileNotRunningSequence(static_cast<I32>(this->sequencer_getState()), opCode,
                                                                response);
        return;
    }

    // okay, we're running a sequence. now let's use the cmdUid to check if the response was for a cmd
    // from this sequence

    // the cmdSeq arg is confusingly not the cmdSeq in this case, according to the current implementation
    // of the CmdDisp. instead, it is the context that we passed in when we originally sent the cmd out.
    // this context is in turn the cmdUid that we calculated just before sending it. rename the variable for
    // clarity's sake
    U32 cmdUid = cmdSeq;

    // pull the sequence index (modulo 2^16) out of the cmdUid. see the comment in FpySequencer::dispatchCommand
    // for info on the binary format of this cmdUid. as a reminder, this should be equal to the first 16 bits of
    // the m_sequencesStarted variable
    U16 sequenceIndex = static_cast<U16>((cmdUid & 0xFFFF0000) >> 16);
    U16 currentSequenceIndex = static_cast<U16>(this->m_sequencesStarted & 0xFFFF);

    // if it was from a different sequence:
    if (sequenceIndex != currentSequenceIndex) {
        this->log_WARNING_LO_CmdResponseFromOldSequence(opCode, response, sequenceIndex, currentSequenceIndex);
        return;
    }

    // okay, it was from this sequence. now if anything's wrong from this point on we should fail the sequence

    // first, make sure we're actually awaiting a statement response
    if (this->sequencer_getState() != State::RUNNING_AWAITING_STATEMENT_RESPONSE) {
        // okay, crap. something from this sequence responded, and we weren't awaiting anything. end it all
        this->log_WARNING_HI_CmdResponseWhileNotAwaiting(opCode, response);
        this->sequencer_sendSignal_stmtResponse_unexpected();
        return;
    }

    if (this->m_runtime.currentStatementOpcode != Fpy::DirectiveId::CONST_CMD &&
        this->m_runtime.currentStatementOpcode != Fpy::DirectiveId::STACK_CMD) {
        // we were not awaiting a cmd response, we were waiting for a directive
        this->log_WARNING_HI_CmdResponseWhileAwaitingDirective(opCode, response,
                                                               this->m_runtime.currentStatementOpcode);
        this->sequencer_sendSignal_stmtResponse_unexpected();
        return;
    }

    // okay, we were awaiting a cmd response. were we awaiting this opcode?
    if (opCode != this->m_runtime.currentCmdOpcode) {
        // we were not awaiting this opcode. coding error, likely on the part of the responding component or cmd
        // dispatcher
        this->log_WARNING_HI_WrongCmdResponseOpcode(opCode, response, this->m_runtime.currentCmdOpcode);
        this->sequencer_sendSignal_stmtResponse_unexpected();
        return;
    }

    // okay, we were awaiting this opcode. but was it from this exact statement, or a different one with the same opcode
    // in the same file?

    // pull the cmd index (modulo 2^16) out of cmdUid. this should be equal to the first 16 bits of the
    // m_statementsDispatched variable
    U16 cmdIndex = static_cast<U16>(cmdUid & 0xFFFF);
    // check for coding errors. at this point in the function, we have definitely dispatched a stmt
    FW_ASSERT(this->m_statementsDispatched > 0);
    U16 currentCmdIndex = static_cast<U16>((this->m_statementsDispatched) & 0xFFFF);

    if (cmdIndex != currentCmdIndex) {
        // we were not awaiting this exact statement, it was a different one with the same opcode. coding error
        this->log_WARNING_HI_WrongCmdResponseIndex(opCode, response, cmdIndex, currentCmdIndex);
        this->sequencer_sendSignal_stmtResponse_unexpected();
        return;
    }

    // okay, got the right cmd back. we have verified:
    // 1) we are in the RUNNING state
    // 2) the response is from this sequence
    // 3) the response is from the correct opcode
    // 4) the response is from the correct instance of that opcode in the sequence

    // always succeed; the cmd response value is pushed to the stack so the sequence
    // can branch on it if desired
    this->sequencer_sendSignal_stmtResponse_success();

    // push the cmd response to the stack so we can branch off of it
    this->m_runtime.stack.push(static_cast<I32>(response.e));
}

void FpySequencer ::seqCancelIn_handler(FwIndexType portNum) {
    // only state you can't cancel in is IDLE
    if (sequencer_getState() == State::IDLE) {
        this->log_WARNING_HI_InvalidSeqCancelCall(static_cast<I32>(sequencer_getState()));
        return;
    }
    this->sequencer_sendSignal_cmd_CANCEL();
}

//! Handler for input port seqRunIn
void FpySequencer::seqRunIn_handler(FwIndexType portNum, const Fw::StringBase& filename, const Svc::SeqArgs& args) {
    // can only run a seq while in idle
    if (sequencer_getState() != State::IDLE) {
        this->log_WARNING_HI_InvalidSeqRunCall(static_cast<I32>(sequencer_getState()));
        return;
    }

    // seqRunIn is never blocking - store args for pushArgsToStack action
    // Args must be serialized in F' big-endian format by the caller before being sent
    this->sequencer_sendSignal_cmd_RUN(
        FpySequencer_SequenceExecutionArgs(filename, FpySequencer_BlockState::NO_BLOCK, args));
}

//! Handler for input port tlmWrite
void FpySequencer::tlmWrite_handler(FwIndexType portNum,  //!< The port number
                                    U32 context           //!< The call order
) {
    this->tlmWrite_State(static_cast<FwEnumStoreType>(this->sequencer_getState()));
    this->tlmWrite_StatementsDispatched(this->m_statementsDispatched);
    this->tlmWrite_StatementsFailed(this->m_tlm.statementsFailed);
    this->tlmWrite_SequencesCancelled(this->m_tlm.sequencesCancelled);
    this->tlmWrite_SequencesSucceeded(this->m_tlm.sequencesSucceeded);
    this->tlmWrite_SequencesFailed(this->m_tlm.sequencesFailed);
    this->tlmWrite_LastDirectiveError(this->m_tlm.lastDirectiveError);
    this->tlmWrite_DirectiveErrorIndex(this->m_tlm.directiveErrorIndex);
    this->tlmWrite_DirectiveErrorId(this->m_tlm.directiveErrorId);
    this->tlmWrite_SeqPath(this->m_sequenceFilePath);

    this->tlmWrite_BreakpointIndex(this->m_breakpoint.breakpointIndex);
    this->tlmWrite_BreakOnlyOnceOnBreakpoint(this->m_breakpoint.breakOnlyOnceOnBreakpoint);
    this->tlmWrite_BreakBeforeNextLine(this->m_breakpoint.breakBeforeNextLine);
    this->tlmWrite_BreakpointInUse(this->m_breakpoint.breakpointInUse);

    this->updateDebugTelemetryStruct();
    this->tlmWrite_Debug_NextCmdOpcode(this->m_debug.nextCmdOpcode);
    this->tlmWrite_Debug_NextStatementOpcode(this->m_debug.nextStatementOpcode);
    this->tlmWrite_Debug_NextStatementReadSuccess(this->m_debug.nextStatementReadSuccess);
    this->tlmWrite_Debug_NextStatementIndex(this->m_debug.nextStatementIndex);
    this->tlmWrite_Debug_ReachedEndOfFile(this->m_debug.reachedEndOfFile);
    this->tlmWrite_Debug_StackSize(this->m_debug.stackSize);
}

void FpySequencer::updateDebugTelemetryStruct() {
    // only send debug tlm when we are paused
    if (this->sequencer_getState() == State::RUNNING_PAUSED) {
        if (this->m_runtime.nextStatementIndex >= this->m_sequenceObj.get_header().get_statementCount()) {
            // reached end of file, turn on EOF flag and otherwise send some default tlm
            this->m_debug.reachedEndOfFile = true;
            this->m_debug.nextStatementReadSuccess = false;
            this->m_debug.nextStatementOpcode = 0;
            this->m_debug.nextCmdOpcode = 0;
            this->m_debug.nextStatementIndex = this->m_runtime.nextStatementIndex;
            this->m_debug.stackSize = this->m_runtime.stack.size;
            return;
        }

        const Fpy::Statement& nextStmt = this->m_sequenceObj.get_statements()[this->m_runtime.nextStatementIndex];
        DirectiveUnion directiveUnion;
        Fw::Success status = this->deserializeDirective(nextStmt, directiveUnion);

        if (status != Fw::Success::SUCCESS) {
            this->m_debug.reachedEndOfFile = false;
            this->m_debug.nextStatementReadSuccess = false;
            this->m_debug.nextStatementOpcode = nextStmt.get_opCode();
            this->m_debug.nextCmdOpcode = 0;
            this->m_debug.nextStatementIndex = this->m_runtime.nextStatementIndex;
            this->m_debug.stackSize = this->m_runtime.stack.size;
            return;
        }

        if (nextStmt.get_opCode() == Fpy::DirectiveId::CONST_CMD) {
            // send opcode of the cmd to the ground
            this->m_debug.reachedEndOfFile = false;
            this->m_debug.nextStatementReadSuccess = true;
            this->m_debug.nextStatementOpcode = nextStmt.get_opCode();
            this->m_debug.nextCmdOpcode = directiveUnion.constCmd.get_opCode();
            this->m_debug.nextStatementIndex = this->m_runtime.nextStatementIndex;
            this->m_debug.stackSize = this->m_runtime.stack.size;
            return;
        }

        this->m_debug.reachedEndOfFile = false;
        this->m_debug.nextStatementReadSuccess = true;
        this->m_debug.nextStatementOpcode = nextStmt.get_opCode();
        this->m_debug.nextCmdOpcode = 0;
        this->m_debug.nextStatementIndex = this->m_runtime.nextStatementIndex;
        this->m_debug.stackSize = this->m_runtime.stack.size;
        return;
    }
    // send some default tlm when we aren't in debug break
    this->m_debug.reachedEndOfFile = false;
    this->m_debug.nextStatementReadSuccess = false;
    this->m_debug.nextStatementOpcode = 0;
    this->m_debug.nextCmdOpcode = 0;
    this->m_debug.nextStatementIndex = 0;
    this->m_debug.stackSize = 0;
}

void FpySequencer::parametersLoaded() {
    parameterUpdated(PARAMID_STATEMENT_TIMEOUT_SECS);
}

void FpySequencer::parameterUpdated(FwPrmIdType id) {
    Fw::ParamValid valid;
    switch (id) {
        case PARAMID_STATEMENT_TIMEOUT_SECS: {
            this->tlmWrite_PRM_STATEMENT_TIMEOUT_SECS(this->paramGet_STATEMENT_TIMEOUT_SECS(valid));
            break;
        }
        default: {
            FW_ASSERT(0, static_cast<FwAssertArgType>(id));  // coding error, forgot to include in switch statement
        }
    }

    FW_ASSERT(valid != Fw::ParamValid::INVALID && valid != Fw::ParamValid::UNINIT,
              static_cast<FwAssertArgType>(valid.e));
}

bool FpySequencer::isRunningState(State state) {
    // TODO ask Rob if there's a better way to check if we're in a superstate. I don't want to have
    // to update this every time I add a new substate to the RUNNING state.

    return this->sequencer_getState() == State::RUNNING_AWAITING_STATEMENT_RESPONSE ||
           this->sequencer_getState() == State::RUNNING_DISPATCH_STATEMENT ||
           this->sequencer_getState() == State::RUNNING_PAUSED || this->sequencer_getState() == State::RUNNING_SLEEPING;
}

}  // namespace Svc