FpySequencer.cpp

Go to the documentation of this file.

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

#include <Fw/Prm/ParamValid.hpp>
#include <Svc/FpySequencer/FpySequencer.hpp>
#include <config/CommandDispatcherImplCfg.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(),
      m_totalExpectedArgSize(0),
      m_sequenceBlockState(),
      m_savedOpCode(0),
      m_savedCmdSeq(0),
      m_sequenceArgs(0, 0),
      m_goalState(),
      m_sequencesStarted(0),
      m_statementsDispatched(0),
      m_runtime(),
      m_breakpoint(),
      m_debug(),
      m_tlm() {}

FpySequencer ::~FpySequencer() {}

void FpySequencer::RUN_cmdHandler(FwOpcodeType opCode,               
                                  U32 cmdSeq,                        
                                  const Fw::CmdStringArg& fileName,  
                                  const BlockState& block            
) {
    // 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,               
                                        U32 cmdSeq,                        
                                        const Fw::CmdStringArg& fileName,  
                                        const Svc::BlockState& block,  
                                        const Svc::SeqArgs& args       
) {
    // 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 == 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 == BlockState::NO_BLOCK) {
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
    }
}

void FpySequencer::VALIDATE_cmdHandler(FwOpcodeType opCode,              
                                       U32 cmdSeq,                       
                                       const Fw::CmdStringArg& fileName  
) {
    this->VALIDATE_ARGS_cmdHandler(opCode, cmdSeq, fileName, Svc::SeqArgs{0, 0});
}

void FpySequencer ::VALIDATE_ARGS_cmdHandler(FwOpcodeType opCode,
                                             U32 cmdSeq,
                                             const Fw::CmdStringArg& fileName,
                                             const 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, BlockState::BLOCK, buffer));
}

void FpySequencer::RUN_VALIDATED_cmdHandler(FwOpcodeType opCode,     
                                            U32 cmdSeq,              
                                            const BlockState& block  
) {
    // 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 == 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 == BlockState::NO_BLOCK) {
        this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
    }
}

void FpySequencer::CANCEL_cmdHandler(FwOpcodeType opCode,  
                                     U32 cmdSeq            
) {
    // 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);
}

void FpySequencer::SET_BREAKPOINT_cmdHandler(FwOpcodeType opCode,  
                                             U32 cmdSeq,           
                                             U32 stmtIdx,          
                                             bool breakOnce  
) {
    this->sequencer_sendSignal_cmd_SET_BREAKPOINT(FpySequencer_BreakpointArgs(true, breakOnce, stmtIdx));

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

void FpySequencer::BREAK_cmdHandler(FwOpcodeType opCode,  
                                    U32 cmdSeq            
) {
    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);
}

void FpySequencer::CONTINUE_cmdHandler(FwOpcodeType opCode,  
                                       U32 cmdSeq            
) {
    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);
}

void FpySequencer::CLEAR_BREAKPOINT_cmdHandler(FwOpcodeType opCode,  
                                               U32 cmdSeq            
) {
    this->sequencer_sendSignal_cmd_CLEAR_BREAKPOINT();
    this->log_ACTIVITY_HI_BreakpointCleared();

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

void FpySequencer::STEP_cmdHandler(FwOpcodeType opCode,  
                                   U32 cmdSeq            
) {
    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);
}

void FpySequencer::DUMP_STACK_TO_FILE_cmdHandler(FwOpcodeType opCode,              
                                                 U32 cmdSeq,                       
                                                 const Fw::CmdStringArg& fileName  
) {
    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(fileName, 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;
}
void FpySequencer::checkTimers_handler(FwIndexType portNum,  
                                       U32 context           
) {
    this->sequencer_sendSignal_checkTimersIn();
}

void FpySequencer::pingIn_handler(FwIndexType portNum, 
                                  U32 key              
) {
    // send ping response
    this->pingOut_out(0, key);
}

void FpySequencer::cmdResponseIn_handler(FwIndexType portNum,             
                                         FwOpcodeType opCode,             
                                         U32 cmdSeq,                      
                                         const Fw::CmdResponse& response  
) {
    // 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()),
                                                                CmdDispatcherCfg::getEventOpcode(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(CmdDispatcherCfg::getEventOpcode(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(CmdDispatcherCfg::getEventOpcode(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(CmdDispatcherCfg::getEventOpcode(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(CmdDispatcherCfg::getEventOpcode(opCode), response,
                                                    CmdDispatcherCfg::getEventOpcode(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(CmdDispatcherCfg::getEventOpcode(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
    // the constCmd/stackCmd directive handlers guarantee there is room for this push
    this->m_runtime.stack.push(static_cast<Fw::CmdResponse::SerialType>(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();
}

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, BlockState::NO_BLOCK, args));
}

void FpySequencer::tlmWrite_handler(FwIndexType portNum,  
                                    U32 context           
) {
    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;
        }

        FW_ASSERT(this->m_runtime.nextStatementIndex < Fpy::MAX_SEQUENCE_STATEMENT_COUNT,
                  static_cast<FwAssertArgType>(this->m_runtime.nextStatementIndex));
        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);
    parameterUpdated(PARAMID_SEQ_BASE_DIR);
}

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;
        }
        case PARAMID_SEQ_BASE_DIR: {
            this->tlmWrite_PRM_SEQ_BASE_DIR(this->paramGet_SEQ_BASE_DIR(valid));
            break;
        }
        default: {
            FW_ASSERT(false, static_cast<FwAssertArgType>(id));  // coding error, forgot to include in switch statement
        }
    }

    FW_ASSERT(FW_PARAM_OK(valid), 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 state == State::RUNNING_AWAITING_STATEMENT_RESPONSE || state == State::RUNNING_DISPATCH_STATEMENT ||
           state == State::RUNNING_PAUSED || state == State::RUNNING_SLEEPING;
}

}  // namespace Svc