Line data Source code
1 : #include "fsfw/tmtcservices/CommandingServiceBase.h"
2 :
3 : #include "fsfw/ipc/QueueFactory.h"
4 : #include "fsfw/objectmanager/ObjectManager.h"
5 : #include "fsfw/serviceinterface/ServiceInterface.h"
6 : #include "fsfw/tcdistribution/PusDistributorIF.h"
7 : #include "fsfw/tmtcpacket/pus/tc.h"
8 : #include "fsfw/tmtcservices/AcceptsTelemetryIF.h"
9 : #include "fsfw/tmtcservices/TmTcMessage.h"
10 : #include "fsfw/tmtcservices/tcHelpers.h"
11 : #include "fsfw/tmtcservices/tmHelpers.h"
12 :
13 : object_id_t CommandingServiceBase::defaultPacketSource = objects::NO_OBJECT;
14 : object_id_t CommandingServiceBase::defaultPacketDestination = objects::NO_OBJECT;
15 :
16 0 : CommandingServiceBase::CommandingServiceBase(object_id_t setObjectId, uint16_t apid,
17 : const char* name, uint8_t service,
18 : uint8_t numberOfParallelCommands,
19 : uint16_t commandTimeoutSeconds, size_t queueDepth,
20 0 : VerificationReporterIF* verificationReporter)
21 : : SystemObject(setObjectId),
22 0 : apid(apid),
23 0 : service(service),
24 0 : timeoutSeconds(commandTimeoutSeconds),
25 0 : tmStoreHelper(apid),
26 0 : tmHelper(service, tmStoreHelper, tmSendHelper),
27 0 : verificationReporter(verificationReporter),
28 0 : commandMap(numberOfParallelCommands),
29 0 : name(name) {
30 0 : commandQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
31 0 : requestQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
32 0 : }
33 :
34 0 : void CommandingServiceBase::setPacketSource(object_id_t packetSource_) {
35 0 : packetSource = packetSource_;
36 0 : }
37 :
38 0 : void CommandingServiceBase::setPacketDestination(object_id_t packetDestination_) {
39 0 : packetDestination = packetDestination_;
40 0 : }
41 :
42 0 : CommandingServiceBase::~CommandingServiceBase() {
43 0 : QueueFactory::instance()->deleteMessageQueue(commandQueue);
44 0 : QueueFactory::instance()->deleteMessageQueue(requestQueue);
45 0 : }
46 :
47 0 : ReturnValue_t CommandingServiceBase::performOperation(uint8_t opCode) {
48 0 : handleCommandQueue();
49 0 : handleRequestQueue();
50 0 : checkTimeout();
51 0 : doPeriodicOperation();
52 0 : return returnvalue::OK;
53 : }
54 :
55 0 : uint32_t CommandingServiceBase::getIdentifier() const { return service; }
56 :
57 0 : MessageQueueId_t CommandingServiceBase::getRequestQueue() const { return requestQueue->getId(); }
58 :
59 0 : ReturnValue_t CommandingServiceBase::initialize() {
60 0 : ReturnValue_t result = SystemObject::initialize();
61 0 : if (result != returnvalue::OK) {
62 0 : return result;
63 : }
64 :
65 0 : if (packetDestination == objects::NO_OBJECT) {
66 0 : packetDestination = defaultPacketDestination;
67 : }
68 0 : auto* packetForwarding = ObjectManager::instance()->get<AcceptsTelemetryIF>(packetDestination);
69 :
70 0 : if (packetSource == objects::NO_OBJECT) {
71 0 : packetSource = defaultPacketSource;
72 : }
73 0 : auto* distributor = ObjectManager::instance()->get<PusDistributorIF>(packetSource);
74 :
75 0 : if (packetForwarding == nullptr or distributor == nullptr) {
76 : #if FSFW_CPP_OSTREAM_ENABLED == 1
77 : sif::error << "CommandingServiceBase::intialize: Packet source or "
78 : "packet destination invalid!"
79 : << std::endl;
80 : #endif
81 0 : return ObjectManagerIF::CHILD_INIT_FAILED;
82 : }
83 :
84 0 : distributor->registerService(*this);
85 0 : requestQueue->setDefaultDestination(packetForwarding->getReportReceptionQueue());
86 :
87 0 : ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
88 0 : tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
89 :
90 0 : if (ipcStore == nullptr or tcStore == nullptr) {
91 : #if FSFW_CPP_OSTREAM_ENABLED == 1
92 : sif::error << "CommandingServiceBase::intialize: IPC store or TC store "
93 : "not initialized yet!"
94 : << std::endl;
95 : #endif
96 0 : return ObjectManagerIF::CHILD_INIT_FAILED;
97 : }
98 0 : if (tmStoreHelper.getTmStore() == nullptr) {
99 0 : auto* tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
100 0 : if (tmStore == nullptr) {
101 0 : return ObjectManagerIF::CHILD_INIT_FAILED;
102 : }
103 0 : tmStoreHelper.setTmStore(*tmStore);
104 : }
105 : // Generally, all TM packets will pass through a layer where the sequence count is set.
106 : // This avoids duplicate calculation of the CRC16
107 0 : tmStoreHelper.disableCrcCalculation();
108 0 : if (tmTimeStamper == nullptr) {
109 0 : tmTimeStamper = ObjectManager::instance()->get<TimeWriterIF>(objects::TIME_STAMPER);
110 0 : if (tmTimeStamper == nullptr) {
111 0 : return ObjectManagerIF::CHILD_INIT_FAILED;
112 : }
113 : }
114 0 : tmStoreHelper.setTimeStamper(*tmTimeStamper);
115 :
116 0 : if (errReporter == nullptr) {
117 0 : errReporter =
118 0 : ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
119 0 : if (errReporter != nullptr) {
120 0 : tmSendHelper.setInternalErrorReporter(*errReporter);
121 : }
122 : } else {
123 0 : tmSendHelper.setInternalErrorReporter(*errReporter);
124 : }
125 0 : tmSendHelper.setMsgQueue(*requestQueue);
126 :
127 0 : if (verificationReporter == nullptr) {
128 0 : verificationReporter =
129 0 : ObjectManager::instance()->get<VerificationReporterIF>(objects::VERIFICATION_REPORTER);
130 0 : if (verificationReporter == nullptr) {
131 0 : return ObjectManagerIF::CHILD_INIT_FAILED;
132 : }
133 : }
134 0 : return returnvalue::OK;
135 : }
136 :
137 0 : void CommandingServiceBase::handleCommandQueue() {
138 0 : CommandMessage reply;
139 : ReturnValue_t result;
140 : while (true) {
141 0 : result = commandQueue->receiveMessage(&reply);
142 0 : if (result == returnvalue::OK) {
143 0 : handleCommandMessage(&reply);
144 0 : continue;
145 0 : } else if (result == MessageQueueIF::EMPTY) {
146 0 : break;
147 : } else {
148 : #if FSFW_VERBOSE_LEVEL >= 1
149 : #if FSFW_CPP_OSTREAM_ENABLED == 1
150 : sif::warning << "CommandingServiceBase::handleCommandQueue: Receiving message failed"
151 : "with code"
152 : << result << std::endl;
153 : #else
154 : sif::printWarning(
155 : "CommandingServiceBase::handleCommandQueue: Receiving message "
156 : "failed with code %d\n",
157 : result);
158 : #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
159 : #endif /* FSFW_VERBOSE_LEVEL >= 1 */
160 0 : break;
161 : }
162 : }
163 0 : }
164 :
165 0 : void CommandingServiceBase::handleCommandMessage(CommandMessage* reply) {
166 0 : bool isStep = false;
167 0 : CommandMessage nextCommand;
168 0 : CommandMapIter iter = commandMap.find(reply->getSender());
169 :
170 : // handle unrequested reply first
171 0 : if (reply->getSender() == MessageQueueIF::NO_QUEUE or iter == commandMap.end()) {
172 0 : handleUnrequestedReply(reply);
173 0 : return;
174 : }
175 0 : nextCommand.setCommand(CommandMessage::CMD_NONE);
176 :
177 : // Implemented by child class, specifies what to do with reply.
178 0 : ReturnValue_t result = handleReply(reply, iter->second.command, &iter->second.state, &nextCommand,
179 0 : iter->second.objectId, &isStep);
180 :
181 : /* If the child implementation does not implement special handling for
182 : * rejected replies (returnvalue::FAILED or INVALID_REPLY is returned), a
183 : * failure verification will be generated with the reason as the
184 : * return code and the initial command as failure parameter 1 */
185 0 : if ((reply->getCommand() == CommandMessage::REPLY_REJECTED) and
186 0 : (result == returnvalue::FAILED or result == INVALID_REPLY)) {
187 0 : result = reply->getReplyRejectedReason();
188 0 : failureParameter1 = iter->second.command;
189 : }
190 :
191 0 : switch (result) {
192 0 : case EXECUTION_COMPLETE:
193 : case returnvalue::OK:
194 : case NO_STEP_MESSAGE:
195 : // handle result of reply handler implemented by developer.
196 0 : handleReplyHandlerResult(result, iter, &nextCommand, reply, isStep);
197 0 : break;
198 0 : case INVALID_REPLY:
199 : // might be just an unrequested reply at a bad moment
200 0 : handleUnrequestedReply(reply);
201 0 : break;
202 0 : default:
203 0 : if (isStep) {
204 0 : prepareVerificationFailureWithFullInfo(tcverif::PROGRESS_FAILURE, iter->second.tcInfo,
205 : result, true);
206 0 : failParams.step = ++iter->second.step;
207 : } else {
208 0 : prepareVerificationFailureWithFullInfo(tcverif::COMPLETION_FAILURE, iter->second.tcInfo,
209 : result, true);
210 : }
211 0 : verificationReporter->sendFailureReport(failParams);
212 0 : failureParameter1 = 0;
213 0 : failureParameter2 = 0;
214 0 : checkAndExecuteFifo(iter);
215 0 : break;
216 : }
217 0 : }
218 :
219 0 : void CommandingServiceBase::handleReplyHandlerResult(ReturnValue_t result, CommandMapIter iter,
220 : CommandMessage* nextCommand,
221 : CommandMessage* reply, bool& isStep) {
222 0 : iter->second.command = nextCommand->getCommand();
223 :
224 : // In case a new command is to be sent immediately, this is performed here.
225 : // If no new command is sent, only analyse reply result by initializing
226 : // sendResult as RETURN_OK
227 0 : ReturnValue_t sendResult = returnvalue::OK;
228 0 : if (nextCommand->getCommand() != CommandMessage::CMD_NONE) {
229 0 : sendResult = commandQueue->sendMessage(reply->getSender(), nextCommand);
230 : }
231 :
232 0 : if (sendResult == returnvalue::OK) {
233 0 : if (isStep and result != NO_STEP_MESSAGE) {
234 0 : prepareVerificationSuccessWithFullInfo(tcverif::PROGRESS_SUCCESS, iter->second.tcInfo);
235 0 : successParams.step = ++iter->second.step;
236 0 : verificationReporter->sendSuccessReport(successParams);
237 : } else {
238 0 : prepareVerificationSuccessWithFullInfo(tcverif::COMPLETION_SUCCESS, iter->second.tcInfo);
239 0 : verificationReporter->sendSuccessReport(successParams);
240 0 : checkAndExecuteFifo(iter);
241 : }
242 : } else {
243 0 : if (isStep) {
244 0 : prepareVerificationFailureWithFullInfo(tcverif::PROGRESS_FAILURE, iter->second.tcInfo, result,
245 : true);
246 0 : failParams.step = ++iter->second.step;
247 0 : nextCommand->clearCommandMessage();
248 0 : verificationReporter->sendFailureReport(failParams);
249 : } else {
250 0 : prepareVerificationFailureWithFullInfo(tcverif::COMPLETION_FAILURE, iter->second.tcInfo,
251 : result, true);
252 0 : nextCommand->clearCommandMessage();
253 0 : verificationReporter->sendFailureReport(failParams);
254 : }
255 0 : failureParameter1 = 0;
256 0 : failureParameter2 = 0;
257 0 : checkAndExecuteFifo(iter);
258 : }
259 0 : }
260 :
261 0 : void CommandingServiceBase::handleRequestQueue() {
262 0 : TmTcMessage message;
263 : ReturnValue_t result;
264 0 : store_address_t address;
265 : MessageQueueId_t queue;
266 : object_id_t objectId;
267 0 : for (result = requestQueue->receiveMessage(&message); result == returnvalue::OK;
268 0 : result = requestQueue->receiveMessage(&message)) {
269 0 : address = message.getStorageId();
270 0 : result = setUpTcReader(address);
271 0 : if (result != returnvalue::OK) {
272 0 : rejectPacketInvalidTc(result, address);
273 0 : continue;
274 : }
275 0 : if ((tcReader.getSubService() == 0) or
276 0 : (isValidSubservice(tcReader.getSubService()) != returnvalue::OK)) {
277 0 : rejectPacket(tcverif::START_FAILURE, address, INVALID_SUBSERVICE);
278 0 : continue;
279 : }
280 :
281 0 : result = getMessageQueueAndObject(tcReader.getSubService(), tcReader.getUserData(),
282 : tcReader.getUserDataLen(), &queue, &objectId);
283 0 : if (result != returnvalue::OK) {
284 0 : rejectPacket(tcverif::START_FAILURE, address, result);
285 0 : continue;
286 : }
287 :
288 : // Is a command already active for the target object?
289 0 : CommandMapIter iter;
290 0 : iter = commandMap.find(queue);
291 :
292 0 : if (iter != commandMap.end()) {
293 0 : result = iter->second.fifo.insert(address);
294 0 : if (result != returnvalue::OK) {
295 0 : rejectPacket(tcverif::START_FAILURE, address, OBJECT_BUSY);
296 : }
297 : } else {
298 0 : CommandInfo newInfo; // Info will be set by startExecution if neccessary
299 0 : newInfo.objectId = objectId;
300 0 : result = commandMap.insert(queue, newInfo, &iter);
301 0 : if (result != returnvalue::OK) {
302 0 : rejectPacket(tcverif::START_FAILURE, address, BUSY);
303 : } else {
304 0 : startExecution(address, iter);
305 : }
306 0 : }
307 : }
308 0 : }
309 :
310 0 : ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice, const uint8_t* sourceData,
311 : size_t sourceDataLen) {
312 0 : ReturnValue_t result = tmHelper.prepareTmPacket(subservice, sourceData, sourceDataLen);
313 0 : if (result != returnvalue::OK) {
314 0 : return result;
315 : }
316 0 : return tmHelper.storeAndSendTmPacket();
317 : }
318 :
319 0 : ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice, object_id_t objectId,
320 : const uint8_t* data, size_t dataLen) {
321 0 : telemetry::DataWithObjectIdPrefix dataWithObjId(objectId, data, dataLen);
322 0 : ReturnValue_t result = tmHelper.prepareTmPacket(subservice, dataWithObjId);
323 0 : if (result != returnvalue::OK) {
324 0 : return result;
325 : }
326 0 : return tmHelper.storeAndSendTmPacket();
327 0 : }
328 :
329 0 : ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice, SerializeIF& sourceData) {
330 0 : ReturnValue_t result = tmHelper.prepareTmPacket(subservice, sourceData);
331 0 : if (result != returnvalue::OK) {
332 0 : return result;
333 : }
334 0 : return tmHelper.storeAndSendTmPacket();
335 : }
336 :
337 0 : void CommandingServiceBase::startExecution(store_address_t storeId, CommandMapIter& iter) {
338 0 : CommandMessage command;
339 0 : iter->second.subservice = tcReader.getSubService();
340 : ReturnValue_t result =
341 0 : prepareCommand(&command, iter->second.subservice, tcReader.getUserData(),
342 0 : tcReader.getUserDataLen(), &iter->second.state, iter->second.objectId);
343 :
344 0 : ReturnValue_t sendResult = returnvalue::OK;
345 0 : switch (result) {
346 0 : case returnvalue::OK:
347 0 : if (command.getCommand() != CommandMessage::CMD_NONE) {
348 0 : sendResult = commandQueue->sendMessage(iter.value->first, &command);
349 : }
350 0 : if (sendResult == returnvalue::OK) {
351 0 : Clock::getUptime(&iter->second.uptimeOfStart);
352 0 : iter->second.step = 0;
353 0 : iter->second.subservice = tcReader.getSubService();
354 0 : iter->second.command = command.getCommand();
355 0 : iter->second.tcInfo.ackFlags = tcReader.getAcknowledgeFlags();
356 0 : iter->second.tcInfo.tcPacketId = tcReader.getPacketIdRaw();
357 0 : iter->second.tcInfo.tcSequenceControl = tcReader.getPacketSeqCtrlRaw();
358 0 : acceptPacket(tcverif::START_SUCCESS, storeId);
359 : } else {
360 0 : command.clearCommandMessage();
361 0 : rejectPacket(tcverif::START_FAILURE, storeId, sendResult);
362 0 : checkAndExecuteFifo(iter);
363 : }
364 0 : break;
365 0 : case EXECUTION_COMPLETE:
366 0 : if (command.getCommand() != CommandMessage::CMD_NONE) {
367 : // Fire-and-forget command.
368 0 : sendResult = commandQueue->sendMessage(iter.value->first, &command);
369 : }
370 0 : if (sendResult == returnvalue::OK) {
371 0 : verificationReporter->sendSuccessReport(
372 : VerifSuccessParams(tcverif::START_SUCCESS, tcReader));
373 0 : acceptPacket(tcverif::COMPLETION_SUCCESS, storeId);
374 0 : checkAndExecuteFifo(iter);
375 : } else {
376 0 : command.clearCommandMessage();
377 0 : rejectPacket(tcverif::START_FAILURE, storeId, sendResult);
378 0 : checkAndExecuteFifo(iter);
379 : }
380 0 : break;
381 0 : default:
382 0 : rejectPacket(tcverif::START_FAILURE, storeId, result);
383 0 : checkAndExecuteFifo(iter);
384 0 : break;
385 : }
386 0 : }
387 :
388 0 : ReturnValue_t CommandingServiceBase::rejectPacketInvalidTc(ReturnValue_t errorCode,
389 : store_address_t tcStoreId) {
390 0 : failureParameter1 = INVALID_TC;
391 0 : prepareVerificationFailureWithNoTcInfo(tcverif::START_FAILURE, errorCode, true);
392 0 : if (tcStoreId != store_address_t::invalid()) {
393 0 : tcStore->deleteData(tcStoreId);
394 : }
395 0 : return verificationReporter->sendFailureReport(failParams);
396 : }
397 :
398 0 : ReturnValue_t CommandingServiceBase::rejectPacket(uint8_t reportId, store_address_t tcStoreId,
399 : ReturnValue_t errorCode) {
400 : ReturnValue_t result =
401 0 : verificationReporter->sendFailureReport(VerifFailureParams(reportId, tcReader, errorCode));
402 0 : tcStore->deleteData(tcStoreId);
403 0 : return result;
404 : }
405 :
406 0 : ReturnValue_t CommandingServiceBase::acceptPacket(uint8_t reportId, store_address_t tcStoreId) {
407 : ReturnValue_t result =
408 0 : verificationReporter->sendSuccessReport(VerifSuccessParams(reportId, tcReader));
409 0 : tcStore->deleteData(tcStoreId);
410 0 : return result;
411 : }
412 :
413 0 : void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter& iter) {
414 0 : store_address_t address;
415 0 : if (iter->second.fifo.retrieve(&address) != returnvalue::OK) {
416 0 : commandMap.erase(&iter);
417 : } else {
418 0 : ReturnValue_t result = setUpTcReader(address);
419 0 : if (result == returnvalue::OK) {
420 0 : startExecution(address, iter);
421 : } else {
422 : // TODO: Warning?
423 0 : rejectPacket(tcverif::START_FAILURE, address, result);
424 : }
425 : }
426 0 : }
427 :
428 0 : void CommandingServiceBase::handleUnrequestedReply(CommandMessage* reply) {
429 0 : reply->clearCommandMessage();
430 0 : }
431 :
432 0 : inline void CommandingServiceBase::doPeriodicOperation() {}
433 :
434 0 : MessageQueueId_t CommandingServiceBase::getCommandQueue() { return commandQueue->getId(); }
435 :
436 0 : void CommandingServiceBase::checkTimeout() {
437 : uint32_t uptime;
438 0 : Clock::getUptime(&uptime);
439 0 : CommandMapIter iter;
440 0 : for (iter = commandMap.begin(); iter != commandMap.end(); ++iter) {
441 0 : if ((iter->second.uptimeOfStart + (timeoutSeconds * 1000)) < uptime) {
442 0 : prepareVerificationFailureWithFullInfo(tcverif::COMPLETION_FAILURE, iter->second.tcInfo,
443 : TIMEOUT, false);
444 0 : verificationReporter->sendFailureReport(failParams);
445 0 : checkAndExecuteFifo(iter);
446 : }
447 : }
448 0 : }
449 :
450 0 : void CommandingServiceBase::setTaskIF(PeriodicTaskIF* task_) { executingTask = task_; }
451 :
452 0 : void CommandingServiceBase::setCustomTmStore(StorageManagerIF& store) {
453 0 : tmStoreHelper.setTmStore(store);
454 0 : }
455 :
456 0 : ReturnValue_t CommandingServiceBase::setUpTcReader(store_address_t storeId) {
457 0 : return tc::prepareTcReader(*tcStore, storeId, tcReader);
458 : }
459 :
460 0 : void CommandingServiceBase::prepareVerificationFailureWithNoTcInfo(uint8_t reportId,
461 : ReturnValue_t errorCode,
462 : bool setCachedFailParams) {
463 0 : failParams.resetTcFields();
464 0 : failParams.resetFailParams();
465 0 : failParams.reportId = reportId;
466 0 : failParams.errorCode = errorCode;
467 0 : if (setCachedFailParams) {
468 0 : failParams.errorParam1 = failureParameter1;
469 0 : failParams.errorParam2 = failureParameter2;
470 : }
471 0 : }
472 0 : void CommandingServiceBase::prepareVerificationFailureWithFullInfo(uint8_t reportId,
473 : CommandInfo::TcInfo& tcInfo,
474 : ReturnValue_t errorCode,
475 : bool setCachedFailParams) {
476 0 : failParams.reportId = reportId;
477 0 : failParams.tcPacketId = tcInfo.tcPacketId;
478 0 : failParams.tcPsc = tcInfo.tcSequenceControl;
479 0 : failParams.ackFlags = tcInfo.ackFlags;
480 0 : failParams.resetFailParams();
481 0 : failParams.errorCode = errorCode;
482 0 : if (setCachedFailParams) {
483 0 : failParams.errorParam1 = failureParameter1;
484 0 : failParams.errorParam2 = failureParameter2;
485 : }
486 0 : }
487 0 : void CommandingServiceBase::prepareVerificationSuccessWithFullInfo(
488 : uint8_t reportId, CommandingServiceBase::CommandInfo::TcInfo& tcInfo) {
489 0 : successParams.reportId = reportId;
490 0 : successParams.tcPacketId = tcInfo.tcPacketId;
491 0 : successParams.tcPsc = tcInfo.tcSequenceControl;
492 0 : successParams.ackFlags = tcInfo.ackFlags;
493 0 : }
494 :
495 0 : const char* CommandingServiceBase::getName() const { return name; }
|