F´ Flight Software - C/C++ Documentation  NASA-v1.6.0
A framework for building embedded system applications to NASA flight quality standards.
CommandDispatcherImpl.cpp
Go to the documentation of this file.
1 /*
2  * CommandDispatcherImpl.cpp
3  *
4  * Created on: May 13, 2014
5  * Author: Timothy Canham
6  */
7 
8 #include <Svc/CmdDispatcher/CommandDispatcherImpl.hpp>
9 #include <Fw/Cmd/CmdPacket.hpp>
10 #include <Fw/Types/Assert.hpp>
11 #include <cstdio>
12 
13 namespace Svc {
14  CommandDispatcherImpl::CommandDispatcherImpl(const char* name) :
15  CommandDispatcherComponentBase(name),
16  m_seq(0),
17  m_numCmdsDispatched(0),
18  m_numCmdErrors(0)
19  {
20  memset(this->m_entryTable,0,sizeof(this->m_entryTable));
21  memset(this->m_sequenceTracker,0,sizeof(this->m_sequenceTracker));
22  }
23 
24  CommandDispatcherImpl::~CommandDispatcherImpl() {
25  }
26 
27  void CommandDispatcherImpl::init(
28  NATIVE_INT_TYPE queueDepth,
29  NATIVE_INT_TYPE instance
30  ) {
31  CommandDispatcherComponentBase::init(queueDepth);
32  }
33 
34  void CommandDispatcherImpl::compCmdReg_handler(NATIVE_INT_TYPE portNum, FwOpcodeType opCode) {
35  // search for an empty slot
36  bool slotFound = false;
37  for (U32 slot = 0; slot < FW_NUM_ARRAY_ELEMENTS(this->m_entryTable); slot++) {
38  if ((not this->m_entryTable[slot].used) and (not slotFound)) {
39  this->m_entryTable[slot].opcode = opCode;
40  this->m_entryTable[slot].port = portNum;
41  this->m_entryTable[slot].used = true;
42  this->log_DIAGNOSTIC_OpCodeRegistered(opCode,portNum,slot);
43  slotFound = true;
44  } else if ((this->m_entryTable[slot].used) &&
45  (this->m_entryTable[slot].opcode == opCode) &&
46  (this->m_entryTable[slot].port == portNum) &&
47  (not slotFound)) {
48  slotFound = true;
49  this->log_DIAGNOSTIC_OpCodeReregistered(opCode,portNum);
50  } else if (this->m_entryTable[slot].used) { // make sure no duplicates
51  FW_ASSERT(this->m_entryTable[slot].opcode != opCode, opCode);
52  }
53  }
54  FW_ASSERT(slotFound,opCode);
55  }
56 
57  void CommandDispatcherImpl::compCmdStat_handler(NATIVE_INT_TYPE portNum, FwOpcodeType opCode, U32 cmdSeq, const Fw::CmdResponse &response) {
58  // check response and log
59  if (Fw::CmdResponse::OK == response.e) {
60  this->log_COMMAND_OpCodeCompleted(opCode);
61  } else {
62  this->m_numCmdErrors++;
63  this->tlmWrite_CommandErrors(this->m_numCmdErrors);
64  FW_ASSERT(response.e != Fw::CmdResponse::OK);
65  this->log_COMMAND_OpCodeError(opCode,response);
66  }
67  // look for command source
68  NATIVE_INT_TYPE portToCall = -1;
69  U32 context;
70  for (U32 pending = 0; pending < FW_NUM_ARRAY_ELEMENTS(this->m_sequenceTracker); pending++) {
71  if (
72  (this->m_sequenceTracker[pending].seq == cmdSeq) &&
73  (this->m_sequenceTracker[pending].used)
74  ) {
75  portToCall = this->m_sequenceTracker[pending].callerPort;
76  context = this->m_sequenceTracker[pending].context;
77  FW_ASSERT(opCode == this->m_sequenceTracker[pending].opCode);
78  FW_ASSERT(portToCall < this->getNum_seqCmdStatus_OutputPorts());
79  this->m_sequenceTracker[pending].used = false;
80  break;
81  }
82  }
83 
84  if (portToCall != -1) {
85  // call port to report status
86  if (this->isConnected_seqCmdStatus_OutputPort(portToCall)) {
87  this->seqCmdStatus_out(portToCall,opCode,context,response);
88  }
89  }
90  }
91 
92  void CommandDispatcherImpl::seqCmdBuff_handler(NATIVE_INT_TYPE portNum, Fw::ComBuffer &data, U32 context) {
93 
94  Fw::CmdPacket cmdPkt;
95  Fw::SerializeStatus stat = cmdPkt.deserialize(data);
96 
97  if (stat != Fw::FW_SERIALIZE_OK) {
98  Fw::DeserialStatus serErr(static_cast<Fw::DeserialStatus::t>(stat));
99  this->log_WARNING_HI_MalformedCommand(serErr);
100  if (this->isConnected_seqCmdStatus_OutputPort(portNum)) {
101  this->seqCmdStatus_out(portNum,cmdPkt.getOpCode(),context,Fw::CmdResponse::VALIDATION_ERROR);
102  }
103  return;
104  }
105 
106  // search for opcode in dispatch table
107  U32 entry;
108  bool entryFound = false;
109 
110  for (entry = 0; entry < FW_NUM_ARRAY_ELEMENTS(this->m_entryTable); entry++) {
111  if ((this->m_entryTable[entry].used) and (cmdPkt.getOpCode() == this->m_entryTable[entry].opcode)) {
112  entryFound = true;
113  break;
114  }
115  }
116  if (entryFound and this->isConnected_compCmdSend_OutputPort(this->m_entryTable[entry].port)) {
117  // register command in command tracker only if response port is connect
118  if (this->isConnected_seqCmdStatus_OutputPort(portNum)) {
119  bool pendingFound = false;
120 
121  for (U32 pending = 0; pending < FW_NUM_ARRAY_ELEMENTS(this->m_sequenceTracker); pending++) {
122  if (not this->m_sequenceTracker[pending].used) {
123  pendingFound = true;
124  this->m_sequenceTracker[pending].used = true;
125  this->m_sequenceTracker[pending].opCode = cmdPkt.getOpCode();
126  this->m_sequenceTracker[pending].seq = this->m_seq;
127  this->m_sequenceTracker[pending].context = context;
128  this->m_sequenceTracker[pending].callerPort = portNum;
129  break;
130  }
131  }
132 
133  // if we couldn't find a slot to track the command, quit
134  if (not pendingFound) {
135  this->log_WARNING_HI_TooManyCommands(cmdPkt.getOpCode());
136  if (this->isConnected_seqCmdStatus_OutputPort(portNum)) {
137  this->seqCmdStatus_out(portNum,cmdPkt.getOpCode(),context,Fw::CmdResponse::EXECUTION_ERROR);
138  }
139  return;
140  }
141  } // end if status port connected
142  // pass arguments to argument buffer
143  this->compCmdSend_out(this->m_entryTable[entry].port,cmdPkt.getOpCode(),this->m_seq,cmdPkt.getArgBuffer());
144  // log dispatched command
145  this->log_COMMAND_OpCodeDispatched(cmdPkt.getOpCode(),this->m_entryTable[entry].port);
146 
147  // increment command count
148  this->m_numCmdsDispatched++;
149  // write telemetry channel for dispatched commands
150  this->tlmWrite_CommandsDispatched(this->m_numCmdsDispatched);
151  } else {
152  this->log_WARNING_HI_InvalidCommand(cmdPkt.getOpCode());
153  this->m_numCmdErrors++;
154  // Fail command back to port, if connected
155  if (this->isConnected_seqCmdStatus_OutputPort(portNum)) {
156  this->seqCmdStatus_out(portNum,cmdPkt.getOpCode(),context,Fw::CmdResponse::INVALID_OPCODE);
157  }
158  this->tlmWrite_CommandErrors(this->m_numCmdErrors);
159  }
160 
161  // increment sequence number
162  this->m_seq++;
163  }
164 
165  void CommandDispatcherImpl::CMD_NO_OP_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) {
166  Fw::LogStringArg no_op_string("Hello, World!");
167  // Log event for NO_OP here.
168  this->log_ACTIVITY_HI_NoOpReceived();
169  this->cmdResponse_out(opCode,cmdSeq,Fw::CmdResponse::OK);
170  }
171 
172  void CommandDispatcherImpl::CMD_NO_OP_STRING_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, const Fw::CmdStringArg& arg1) {
173  Fw::LogStringArg msg(arg1.toChar());
174  // Echo the NO_OP_STRING args here.
175  this->log_ACTIVITY_HI_NoOpStringReceived(msg);
176  this->cmdResponse_out(opCode,cmdSeq,Fw::CmdResponse::OK);
177  }
178 
179  void CommandDispatcherImpl::CMD_TEST_CMD_1_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, I32 arg1, F32 arg2, U8 arg3) {
180  this->log_ACTIVITY_HI_TestCmd1Args(arg1,arg2,arg3);
181  this->cmdResponse_out(opCode,cmdSeq,Fw::CmdResponse::OK);
182  }
183 
184  void CommandDispatcherImpl::CMD_CLEAR_TRACKING_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) {
185  // clear tracking table
186  for (NATIVE_INT_TYPE entry = 0; entry < CMD_DISPATCHER_SEQUENCER_TABLE_SIZE; entry++) {
187  this->m_sequenceTracker[entry].used = false;
188  }
189  this->cmdResponse_out(opCode,cmdSeq,Fw::CmdResponse::OK);
190  }
191 
192  void CommandDispatcherImpl::pingIn_handler(NATIVE_INT_TYPE portNum, U32 key) {
193  // respond to ping
194  this->pingOut_out(0,key);
195  }
196 
197 }
Fw::SerializeStatus
SerializeStatus
forward declaration for string
Definition: Serializable.hpp:14
Svc::CommandDispatcherImpl::init
void init(NATIVE_INT_TYPE queueDepth, NATIVE_INT_TYPE instance)
Component initialization routine.
Definition: CommandDispatcherImpl.cpp:27
Fw::LogStringArg
Definition: LogString.hpp:11
U8
uint8_t U8
8-bit unsigned integer
Definition: BasicTypes.hpp:73
CmdPacket.hpp
Svc::CommandDispatcherImpl::CommandDispatcherImpl
CommandDispatcherImpl(const char *name)
Command Dispatcher constructor.
Definition: CommandDispatcherImpl.cpp:14
F32
float F32
32-bit floating point
Definition: BasicTypes.hpp:91
Fw::CmdStringArg
Definition: CmdString.hpp:11
FW_NUM_ARRAY_ELEMENTS
#define FW_NUM_ARRAY_ELEMENTS(a)
number of elements in an array
Definition: BasicTypes.hpp:100
NATIVE_INT_TYPE
int NATIVE_INT_TYPE
native integer type declaration
Definition: BasicTypes.hpp:27
CMD_DISPATCHER_SEQUENCER_TABLE_SIZE
@ CMD_DISPATCHER_SEQUENCER_TABLE_SIZE
Definition: CommandDispatcherImplCfg.hpp:15
Fw::CmdPacket::getArgBuffer
CmdArgBuffer & getArgBuffer()
Definition: CmdPacket.cpp:59
OK
@ OK
Definition: StandardTypes.hpp:15
CommandDispatcherImpl.hpp
Component responsible for dispatching incoming commands to registered components.
Fw::CmdPacket::deserialize
SerializeStatus deserialize(SerializeBufferBase &buffer)
deserialize to contents
Definition: CmdPacket.cpp:29
FwOpcodeType
#define FwOpcodeType
Type representation for a command opcode.
Definition: FpConfig.hpp:62
FW_ASSERT
#define FW_ASSERT(...)
Definition: Assert.hpp:9
Svc
Definition: ActiveRateGroupCfg.hpp:18
Fw::CmdPacket
Definition: CmdPacket.hpp:16
Svc::CommandDispatcherImpl::~CommandDispatcherImpl
virtual ~CommandDispatcherImpl()
Component destructor.
Definition: CommandDispatcherImpl.cpp:24
Fw::ComBuffer
Definition: ComBuffer.hpp:21
Fw::CmdPacket::getOpCode
FwOpcodeType getOpCode() const
Definition: CmdPacket.cpp:55
Fw::CmdStringArg::toChar
const char * toChar() const
Definition: CmdString.cpp:48
Fw::FW_SERIALIZE_OK
@ FW_SERIALIZE_OK
Serialization/Deserialization operation was successful.
Definition: Serializable.hpp:15