F´ Flight Software - C/C++ Documentation  NASA-v2.0.1
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 <stdio.h>
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, Fw::CommandResponse response) {
58  // check response and log
59  if (Fw::COMMAND_OK == response) {
60  this->log_COMMAND_OpCodeCompleted(opCode);
61  } else {
62  this->m_numCmdErrors++;
63  this->tlmWrite_CommandErrors(this->m_numCmdErrors);
64  ErrorResponse evrResp = ERR_UNEXP;
65  switch (response) {
66  case Fw::COMMAND_INVALID_OPCODE:
67  evrResp = ERR_INVALID_OPCODE;
68  break;
69  case Fw::COMMAND_VALIDATION_ERROR:
70  evrResp = ERR_VALIDATION_ERROR;
71  break;
72  case Fw::COMMAND_FORMAT_ERROR:
73  evrResp = ERR_FORMAT_ERROR;
74  break;
75  case Fw::COMMAND_EXECUTION_ERROR:
76  evrResp = ERR_EXECUTION_ERROR;
77  break;
78  case Fw::COMMAND_BUSY:
79  evrResp = ERR_BUSY;
80  break;
81  case Fw::COMMAND_OK:
82  FW_ASSERT(0); // should never get here
83  break;
84  default:
85  evrResp = ERR_UNEXP;
86  break;
87  }
88  this->log_WARNING_HI_OpCodeError(opCode,evrResp);
89  }
90  // look for command source
91  NATIVE_INT_TYPE portToCall = -1;
92  U32 context;
93  for (U32 pending = 0; pending < FW_NUM_ARRAY_ELEMENTS(this->m_sequenceTracker); pending++) {
94  if (
95  (this->m_sequenceTracker[pending].seq == cmdSeq) &&
96  (this->m_sequenceTracker[pending].used)
97  ) {
98  portToCall = this->m_sequenceTracker[pending].callerPort;
99  context = this->m_sequenceTracker[pending].context;
100  FW_ASSERT(opCode == this->m_sequenceTracker[pending].opCode);
101  FW_ASSERT(portToCall < this->getNum_seqCmdStatus_OutputPorts());
102  this->m_sequenceTracker[pending].used = false;
103  break;
104  }
105  }
106 
107  if (portToCall != -1) {
108  // call port to report status
109  if (this->isConnected_seqCmdStatus_OutputPort(portToCall)) {
110  this->seqCmdStatus_out(portToCall,opCode,context,response);
111  }
112  }
113  }
114 
115  void CommandDispatcherImpl::seqCmdBuff_handler(NATIVE_INT_TYPE portNum, Fw::ComBuffer &data, U32 context) {
116 
117  Fw::CmdPacket cmdPkt;
118  Fw::SerializeStatus stat = cmdPkt.deserialize(data);
119 
120  if (stat != Fw::FW_SERIALIZE_OK) {
121  CmdSerError serErr = ERR_UNEXP_STAT;
122  switch (stat) {
123  case Fw::FW_DESERIALIZE_BUFFER_EMPTY:
124  serErr = ERR_BUFFER_TOO_SMALL;
125  break;
126  case Fw::FW_DESERIALIZE_FORMAT_ERROR:
127  serErr = ERR_BUFFER_FORMAT;
128  break;
129  case Fw::FW_DESERIALIZE_SIZE_MISMATCH:
130  serErr = ERR_SIZE_MISMATCH;
131  break;
132  case Fw::FW_DESERIALIZE_TYPE_MISMATCH:
133  serErr = ERR_TYPE_MISMATCH;
134  break;
135  case Fw::FW_SERIALIZE_OK:
136  FW_ASSERT(0); // should never get here
137  break;
138  default:
139  serErr = ERR_UNEXP_STAT;
140  break;
141  }
142  this->log_WARNING_HI_MalformedCommand(serErr);
143  if (this->isConnected_seqCmdStatus_OutputPort(portNum)) {
144  this->seqCmdStatus_out(portNum,cmdPkt.getOpCode(),context,Fw::COMMAND_VALIDATION_ERROR);
145  }
146  return;
147  }
148 
149  // search for opcode in dispatch table
150  U32 entry;
151  bool entryFound = false;
152 
153  for (entry = 0; entry < FW_NUM_ARRAY_ELEMENTS(this->m_entryTable); entry++) {
154  if ((this->m_entryTable[entry].used) and (cmdPkt.getOpCode() == this->m_entryTable[entry].opcode)) {
155  entryFound = true;
156  break;
157  }
158  }
159  if (entryFound and this->isConnected_compCmdSend_OutputPort(this->m_entryTable[entry].port)) {
160  // register command in command tracker only if response port is connect
161  if (this->isConnected_seqCmdStatus_OutputPort(portNum)) {
162  bool pendingFound = false;
163 
164  for (U32 pending = 0; pending < FW_NUM_ARRAY_ELEMENTS(this->m_sequenceTracker); pending++) {
165  if (not this->m_sequenceTracker[pending].used) {
166  pendingFound = true;
167  this->m_sequenceTracker[pending].used = true;
168  this->m_sequenceTracker[pending].opCode = cmdPkt.getOpCode();
169  this->m_sequenceTracker[pending].seq = this->m_seq;
170  this->m_sequenceTracker[pending].context = context;
171  this->m_sequenceTracker[pending].callerPort = portNum;
172  break;
173  }
174  }
175 
176  // if we couldn't find a slot to track the command, quit
177  if (not pendingFound) {
178  this->log_WARNING_HI_TooManyCommands(cmdPkt.getOpCode());
179  if (this->isConnected_seqCmdStatus_OutputPort(portNum)) {
180  this->seqCmdStatus_out(portNum,cmdPkt.getOpCode(),context,Fw::COMMAND_EXECUTION_ERROR);
181  }
182  return;
183  }
184  } // end if status port connected
185  // pass arguments to argument buffer
186  this->compCmdSend_out(this->m_entryTable[entry].port,cmdPkt.getOpCode(),this->m_seq,cmdPkt.getArgBuffer());
187  // log dispatched command
188  this->log_COMMAND_OpCodeDispatched(cmdPkt.getOpCode(),this->m_entryTable[entry].port);
189 
190  // increment command count
191  this->m_numCmdsDispatched++;
192  // write telemetry channel for dispatched commands
193  this->tlmWrite_CommandsDispatched(this->m_numCmdsDispatched);
194  } else {
195  this->log_WARNING_HI_InvalidCommand(cmdPkt.getOpCode());
196  this->m_numCmdErrors++;
197  // Fail command back to port, if connected
198  if (this->isConnected_seqCmdStatus_OutputPort(portNum)) {
199  this->seqCmdStatus_out(portNum,cmdPkt.getOpCode(),context,Fw::COMMAND_INVALID_OPCODE);
200  }
201  this->tlmWrite_CommandErrors(this->m_numCmdErrors);
202  }
203 
204  // increment sequence number
205  this->m_seq++;
206  }
207 
208  void CommandDispatcherImpl::CMD_NO_OP_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) {
209  Fw::LogStringArg no_op_string("Hello, World!");
210  // Log event for NO_OP here.
211  this->log_ACTIVITY_HI_NoOpReceived();
212  this->cmdResponse_out(opCode,cmdSeq,Fw::COMMAND_OK);
213  }
214 
215  void CommandDispatcherImpl::CMD_NO_OP_STRING_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, const Fw::CmdStringArg& arg1) {
216  Fw::LogStringArg msg(arg1.toChar());
217  // Echo the NO_OP_STRING args here.
218  this->log_ACTIVITY_HI_NoOpStringReceived(msg);
219  this->cmdResponse_out(opCode,cmdSeq,Fw::COMMAND_OK);
220  }
221 
222  void CommandDispatcherImpl::CMD_TEST_CMD_1_cmdHandler(FwOpcodeType opCode, U32 cmdSeq, I32 arg1, F32 arg2, U8 arg3) {
223  this->log_ACTIVITY_HI_TestCmd1Args(arg1,arg2,arg3);
224  this->cmdResponse_out(opCode,cmdSeq,Fw::COMMAND_OK);
225  }
226 
227  void CommandDispatcherImpl::CMD_CLEAR_TRACKING_cmdHandler(FwOpcodeType opCode, U32 cmdSeq) {
228  // clear tracking table
229  for (NATIVE_INT_TYPE entry = 0; entry < CMD_DISPATCHER_SEQUENCER_TABLE_SIZE; entry++) {
230  this->m_sequenceTracker[entry].used = false;
231  }
232  this->cmdResponse_out(opCode,cmdSeq,Fw::COMMAND_OK);
233  }
234 
235  void CommandDispatcherImpl::pingIn_handler(NATIVE_INT_TYPE portNum, U32 key) {
236  // respond to ping
237  this->pingOut_out(0,key);
238  }
239 
240 }
Fw::FW_DESERIALIZE_BUFFER_EMPTY
@ FW_DESERIALIZE_BUFFER_EMPTY
Deserialization buffer was empty when trying to read more data.
Definition: Serializable.hpp:18
Fw::SerializeStatus
SerializeStatus
forward declaration for string
Definition: Serializable.hpp:14
Fw::FW_DESERIALIZE_SIZE_MISMATCH
@ FW_DESERIALIZE_SIZE_MISMATCH
Data was left in in the buffer, but not enough to deserialize.
Definition: Serializable.hpp:20
Fw::CmdStringArg::toChar
const char * toChar(void) const
Definition: CmdString.cpp:34
Svc::CommandDispatcherImpl::init
void init(NATIVE_INT_TYPE queueDepth, NATIVE_INT_TYPE instance)
Component initialization routine.
Definition: CommandDispatcherImpl.cpp:27
FW_NUM_ARRAY_ELEMENTS
#define FW_NUM_ARRAY_ELEMENTS(a)
number of elements in an array
Definition: BasicTypes.hpp:103
Fw::LogStringArg
Definition: LogString.hpp:11
U8
uint8_t U8
8-bit unsigned integer
Definition: BasicTypes.hpp:76
CMD_DISPATCHER_SEQUENCER_TABLE_SIZE
@ CMD_DISPATCHER_SEQUENCER_TABLE_SIZE
Definition: CommandDispatcherImplCfg.hpp:15
CmdPacket.hpp
Svc::CommandDispatcherImpl::CommandDispatcherImpl
CommandDispatcherImpl(const char *name)
Command Dispatcher constructor.
Definition: CommandDispatcherImpl.cpp:14
Fw::CmdStringArg
Definition: CmdString.hpp:11
Assert.hpp
Fw::FW_SERIALIZE_OK
@ FW_SERIALIZE_OK
Serialization/Deserialization operation was successful.
Definition: Serializable.hpp:15
Fw::FW_DESERIALIZE_FORMAT_ERROR
@ FW_DESERIALIZE_FORMAT_ERROR
Deserialization data had incorrect values (unexpected data types)
Definition: Serializable.hpp:19
F32
float F32
32-bit floating point
Definition: BasicTypes.hpp:94
FwOpcodeType
#define FwOpcodeType
Type representation for a command opcode.
Definition: FpConfig.hpp:62
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
Fw::CmdPacket::getArgBuffer
CmdArgBuffer & getArgBuffer(void)
Definition: CmdPacket.cpp:59
FW_ASSERT
#define FW_ASSERT(...)
Definition: Assert.hpp:9
Fw::FW_DESERIALIZE_TYPE_MISMATCH
@ FW_DESERIALIZE_TYPE_MISMATCH
Deserialized type ID didn't match.
Definition: Serializable.hpp:21
Svc
Definition: ActiveRateGroupImplCfg.hpp:18
Fw::CmdPacket
Definition: CmdPacket.hpp:16
Svc::CommandDispatcherImpl::~CommandDispatcherImpl
virtual ~CommandDispatcherImpl()
Component destructor.
Definition: CommandDispatcherImpl.cpp:24
Fw::CmdPacket::getOpCode
FwOpcodeType getOpCode(void) const
Definition: CmdPacket.cpp:55
NATIVE_INT_TYPE
int NATIVE_INT_TYPE
native integer type declaration
Definition: BasicTypes.hpp:29
Fw::ComBuffer
Definition: ComBuffer.hpp:21