F´ Flight Software - C/C++ Documentation  NASA-v2.0.1
A framework for building embedded system applications to NASA flight quality standards.
•All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
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