diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 86589d21..484ce47c 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -225,9 +225,9 @@ void PlocMPSoCHandler::doShutDown() { } ReturnValue_t PlocMPSoCHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { - if (not normalCmdPending) { + if (not commandIsExecuting(mpsoc::TC_GET_HK_REPORT)) { *id = mpsoc::TC_GET_HK_REPORT; - normalCmdPending = true; + // normalCmdPending = true; return buildCommandFromCommand(*id, nullptr, 0); } return NOTHING_TO_SEND; @@ -424,6 +424,7 @@ ReturnValue_t PlocMPSoCHandler::scanForReply(const uint8_t* start, size_t remain triggerEvent(MPSOC_HANDLER_SEQUENCE_COUNT_MISMATCH, sequenceCount, recvSeqCnt); sequenceCount = recvSeqCnt; } + sif::debug << "sequence count: " << sequenceCount.get() << std::endl; // This sequence count ping pong does not make any sense but it is how the MPSoC expects it. sequenceCount++; return result; @@ -804,19 +805,22 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { uint16_t apid = (*(data) << 8 | *(data + 1)) & APID_MASK; - auto cmdDoneHandler = [&]() { + auto cmdDoneHandler = [&](bool success) { if (normalCmdPending) { normalCmdPending = false; } auto commandIter = deviceCommandMap.find(getPendingCommand()); if (commandIter != deviceCommandMap.end()) { commandIter->second.isExecuting = false; + if (commandIter->second.sendReplyTo != MessageQueueIF::NO_QUEUE) { + actionHelper.finish(success, commandIter->second.sendReplyTo, getPendingCommand(), result); + } } disableAllReplies(); }; switch (apid) { case (mpsoc::apid::EXE_SUCCESS): { - cmdDoneHandler(); + cmdDoneHandler(true); break; } case (mpsoc::apid::EXE_FAILURE): { @@ -833,7 +837,7 @@ ReturnValue_t PlocMPSoCHandler::handleExecutionReport(const uint8_t* data) { printStatus(data); sendFailureReport(mpsoc::EXE_REPORT, MPSoCReturnValuesIF::RECEIVED_EXE_FAILURE); result = IGNORE_REPLY_DATA; - cmdDoneHandler(); + cmdDoneHandler(false); break; } default: {