CommandDispatcherImpl.cpp

Go to the documentation of this file.

/*
 * CommandDispatcherImpl.cpp
 *
 *  Created on: May 13, 2014
 *      Author: Timothy Canham
 */

#include <Fw/Cmd/CmdPacket.hpp>
#include <Fw/Types/Assert.hpp>
#include <Svc/CmdDispatcher/CommandDispatcherImpl.hpp>
#include <cstdio>
#include <cstring>
#include <limits>

// Check the CMD_DISPATCHER_DISPATCH_TABLE_SIZE and CMD_DISPATCHER_SEQUENCER_TABLE_SIZE for overflow
static_assert(CMD_DISPATCHER_DISPATCH_TABLE_SIZE <= std::numeric_limits<FwOpcodeType>::max(),
              "Opcode table limited to opcode range");
static_assert(CMD_DISPATCHER_SEQUENCER_TABLE_SIZE <= std::numeric_limits<U32>::max(),
              "Sequencer table limited to range of U32");

namespace Svc {
CommandDispatcherImpl::CommandDispatcherImpl(const char* name)
    : CommandDispatcherComponentBase(name),
      m_seq(0),
      m_seqWrapped(false),
      m_executeWhenSequenceTableFull(CmdDispatcherCfg::EXECUTE_WHEN_SEQUENCE_TABLE_FULL_DEFAULT),
      m_numCmdsDispatched(0),
      m_numCmdErrors(0),
      m_numCmdsDropped(0) {}

CommandDispatcherImpl::~CommandDispatcherImpl() {}

void CommandDispatcherImpl::configure(bool executeWhenSequenceTableFull) {
    this->m_executeWhenSequenceTableFull = executeWhenSequenceTableFull;
}

void CommandDispatcherImpl::advanceSequenceNumber() {
    if (this->m_seq == std::numeric_limits<U32>::max()) {
        this->m_seqWrapped = true;
    }
    ++this->m_seq;
}

U32 CommandDispatcherImpl::allocateSequenceNumber() {
    // Before the first wrap, m_seq is monotonic and cannot collide with a tracked key
    if (this->m_seqWrapped) {
        SequenceTrackerEntry trackedCmd;
        const FwSizeType numTrackedCommands = this->m_sequenceTracker.getSize();

        // At most numTrackedCommands keys can collide, so the loop is bounded by the table size
        for (FwSizeType i = 0; i < numTrackedCommands; ++i) {
            if (this->m_sequenceTracker.find(this->m_seq, trackedCmd) != Fw::Success::SUCCESS) {
                break;
            }
            this->advanceSequenceNumber();
        }
    }

    const U32 sequenceNumber = this->m_seq;
    this->advanceSequenceNumber();
    return sequenceNumber;
}

void CommandDispatcherImpl::compCmdReg_handler(FwIndexType portNum, FwOpcodeType opCode) {
    FwIndexType existingPort;
    if (this->m_entryTable.find(opCode, existingPort) == Fw::Success::SUCCESS) {
        // Opcode already present — must be the same port (re-registration)
        FW_ASSERT(existingPort == portNum, static_cast<FwAssertArgType>(opCode));
        this->log_DIAGNOSTIC_OpCodeReregistered(CmdDispatcherCfg::getEventOpcode(opCode), portNum);
    } else {
        const I32 slot = static_cast<I32>(this->m_entryTable.getSize());
        const Fw::Success status = this->m_entryTable.insert(opCode, portNum);
        FW_ASSERT(status == Fw::Success::SUCCESS, static_cast<FwAssertArgType>(opCode));
        this->log_DIAGNOSTIC_OpCodeRegistered(CmdDispatcherCfg::getEventOpcode(opCode), portNum, slot);
    }
}

void CommandDispatcherImpl::compCmdStat_handler(FwIndexType portNum,
                                                FwOpcodeType opCode,
                                                U32 cmdSeq,
                                                const Fw::CmdResponse& response) {
    // check response and log
    if (Fw::CmdResponse::OK == response.e) {
        this->log_COMMAND_OpCodeCompleted(CmdDispatcherCfg::getEventOpcode(opCode));
    } else {
        this->m_numCmdErrors++;
        FW_ASSERT(response.e != Fw::CmdResponse::OK);
        this->log_COMMAND_OpCodeError(CmdDispatcherCfg::getEventOpcode(opCode), response);
    }
    // look for command source
    SequenceTrackerEntry trackedCmd;
    const Fw::Success removeStatus = this->m_sequenceTracker.remove(cmdSeq, trackedCmd);
    if (removeStatus == Fw::Success::SUCCESS) {
        const FwIndexType portToCall = trackedCmd.callerPort;
        const U32 context = trackedCmd.context;
        FW_ASSERT(opCode == trackedCmd.opCode);
        FW_ASSERT(portToCall < this->getNum_seqCmdStatus_OutputPorts());

        // call port to report status
        if (this->isConnected_seqCmdStatus_OutputPort(portToCall)) {
            // NOTE: seqCmdStatus port forwards three arguments: (opCode, cmdSeq, response).
            //       However, the cmdSeq value has no meaning for the calling sequencer.
            //       Instead, the context value is forwarded to allow the caller to utilize it if needed.
            this->seqCmdStatus_out(portToCall, opCode, context, response);
        }
    }
}

void CommandDispatcherImpl::seqCmdBuff_handler(FwIndexType portNum, Fw::ComBuffer& data, U32 context) {
    Fw::CmdPacket cmdPkt;
    Fw::SerializeStatus stat = cmdPkt.deserializeFrom(data);
    const bool portIsConnected = this->isConnected_seqCmdStatus_OutputPort(portNum);

    if (stat != Fw::FW_SERIALIZE_OK) {
        Fw::DeserialStatus serErr(static_cast<Fw::DeserialStatus::t>(stat));
        this->log_WARNING_HI_MalformedCommand(serErr);
        if (portIsConnected) {
            this->seqCmdStatus_out(portNum, cmdPkt.getOpCode(), context, Fw::CmdResponse::VALIDATION_ERROR);
        }
        return;
    }

    // look up opcode in dispatch map
    FwIndexType entryPort;
    Fw::Success findStatus = this->m_entryTable.find(cmdPkt.getOpCode(), entryPort);
    if (findStatus == Fw::Success::SUCCESS and this->isConnected_compCmdSend_OutputPort(entryPort)) {
        Fw::Success pendingInsertStatus = Fw::Success::SUCCESS;
        const U32 sequenceNumber = this->allocateSequenceNumber();

        // register command in command tracker only if response port is connect
        if (portIsConnected) {
            SequenceTrackerEntry pendingCmd;
            pendingCmd.opCode = cmdPkt.getOpCode();
            pendingCmd.context = context;
            pendingCmd.callerPort = portNum;

            pendingInsertStatus = this->m_sequenceTracker.insert(sequenceNumber, pendingCmd);

            // if sequence table is full, reject here unless configured to dispatch untracked
            if (not this->m_executeWhenSequenceTableFull and pendingInsertStatus != Fw::Success::SUCCESS) {
                this->log_WARNING_HI_TooManyCommands(CmdDispatcherCfg::getEventOpcode(cmdPkt.getOpCode()));
                this->seqCmdStatus_out(portNum, cmdPkt.getOpCode(), context, Fw::CmdResponse::EXECUTION_ERROR);
                return;
            }
        }  // end if status port connected
        // pass arguments to argument buffer
        this->compCmdSend_out(entryPort, cmdPkt.getOpCode(), sequenceNumber, cmdPkt.getArgBuffer());
        // log dispatched command
        this->log_COMMAND_OpCodeDispatched(CmdDispatcherCfg::getEventOpcode(cmdPkt.getOpCode()), entryPort);

        // increment command count
        this->m_numCmdsDispatched++;

        // pendingInsertStatus is only non-SUCCESS for a connected caller whose insert failed (see check above)
        if (this->m_executeWhenSequenceTableFull and pendingInsertStatus != Fw::Success::SUCCESS) {
            this->log_WARNING_HI_TooManyCommands(CmdDispatcherCfg::getEventOpcode(cmdPkt.getOpCode()));
            this->seqCmdStatus_out(portNum, cmdPkt.getOpCode(), context, Fw::CmdResponse::DISPATCHED_UNTRACKED);
        }
    } else {
        this->log_WARNING_HI_InvalidCommand(CmdDispatcherCfg::getEventOpcode(cmdPkt.getOpCode()));
        this->m_numCmdErrors++;
        // Fail command back to port, if connected
        if (portIsConnected) {
            this->seqCmdStatus_out(portNum, cmdPkt.getOpCode(), context, Fw::CmdResponse::INVALID_OPCODE);
        }
        // Preserve the existing behavior of consuming a sequence number for an invalid opcode.
        this->advanceSequenceNumber();
    }
}

void CommandDispatcherImpl ::run_handler(FwIndexType portNum, U32 context) {
    this->tlmWrite_CommandsDropped(this->m_numCmdsDropped.load(std::memory_order_relaxed));
    this->tlmWrite_CommandErrors(this->m_numCmdErrors);
    this->tlmWrite_CommandsDispatched(this->m_numCmdsDispatched);
}

void CommandDispatcherImpl::CMD_NO_OP_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) {
    Fw::LogStringArg no_op_string("Hello, World!");
    // Log event for NO_OP here.
    this->log_ACTIVITY_HI_NoOpReceived();
    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
}

void CommandDispatcherImpl::CMD_NO_OP_STRING_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, const Fw::CmdStringArg& arg1) {
    Fw::LogStringArg msg(arg1.toChar());
    // Echo the NO_OP_STRING args here.
    this->log_ACTIVITY_HI_NoOpStringReceived(msg);
    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
}

void CommandDispatcherImpl::CMD_TEST_CMD_1_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, I32 arg1, F32 arg2, U8 arg3) {
    this->log_ACTIVITY_HI_TestCmd1Args(arg1, arg2, arg3);
    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
}

void CommandDispatcherImpl::CMD_CLEAR_TRACKING_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) {
    // Preserve this command's own entry so its OK status still reaches the caller
    SequenceTrackerEntry selfEntry = {};
    const bool selfTracked = (this->m_sequenceTracker.find(cmdSeq, selfEntry) == Fw::Success::SUCCESS);

    // Notify every other caller that its pending status will never arrive
    for (const auto& entry : this->m_sequenceTracker) {
        if (entry.getKey() == cmdSeq) {
            continue;
        }
        const SequenceTrackerEntry& trackedCmd = entry.getValue();
        FW_ASSERT(trackedCmd.callerPort < this->getNum_seqCmdStatus_OutputPorts());
        if (this->isConnected_seqCmdStatus_OutputPort(trackedCmd.callerPort)) {
            this->seqCmdStatus_out(trackedCmd.callerPort, trackedCmd.opCode, trackedCmd.context,
                                   Fw::CmdResponse::CLEARED);
        }
    }

    this->m_sequenceTracker.clear();
    if (selfTracked) {
        const Fw::Success status = this->m_sequenceTracker.insert(cmdSeq, selfEntry);
        FW_ASSERT(status == Fw::Success::SUCCESS);
    }
    this->cmdResponse_out(opCode, cmdSeq, Fw::CmdResponse::OK);
}

void CommandDispatcherImpl::pingIn_handler(FwIndexType portNum, U32 key) {
    // respond to ping
    this->pingOut_out(0, key);
}

void CommandDispatcherImpl::seqCmdBuff_overflowHook(FwIndexType portNum, Fw::ComBuffer& data, U32 context) {
    FwOpcodeType opcode = 0;  // Note: 0 = Reserved opcode
    if (CmdDispatcherCfg::IncludeCommandOpcodesInEvents) {
        Fw::CmdPacket cmdPkt;
        const Fw::SerializeStatus stat = cmdPkt.deserializeFrom(data);
        if (stat == Fw::FW_SERIALIZE_OK) {
            opcode = cmdPkt.getOpCode();
        }
    }

    this->log_WARNING_HI_CommandDroppedQueueOverflow(CmdDispatcherCfg::getEventOpcode(opcode), context);
    // This hook runs on the caller's thread; the counter is atomic so no lock is needed
    this->m_numCmdsDropped.fetch_add(1, std::memory_order_relaxed);
}

}  // namespace Svc