diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index b9d97e90..c1cb4bde 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -822,6 +822,16 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator uint8_t enabledReplies = 0; + auto enableThreeReplies = [&](DeviceCommandId_t replyId) { + enabledReplies = 3; + result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, replyId); + if (result != returnvalue::OK) { + sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " + << mpsoc::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl; + return result; + } + break; + }; switch (command->first) { case mpsoc::TC_MEM_WRITE: case mpsoc::TC_FLASHDELETE: @@ -838,26 +848,16 @@ ReturnValue_t PlocMPSoCHandler::enableReplyInReplyMap(DeviceCommandMap::iterator case mpsoc::TC_MODE_SNAPSHOT: enabledReplies = 2; break; + case mpsoc::TC_GET_HK_REPORT: { + enableThreeReplies(mpsoc::TM_GET_HK_REPORT); + break; + } case mpsoc::TC_MEM_READ: { - enabledReplies = 3; - result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, - mpsoc::TM_MEMORY_READ_REPORT); - if (result != returnvalue::OK) { - sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " - << mpsoc::TM_MEMORY_READ_REPORT << " not in replyMap" << std::endl; - return result; - } + enableThreeReplies(mpsoc::TM_MEMORY_READ_REPORT); break; } case mpsoc::TC_CAM_CMD_SEND: { - enabledReplies = 3; - result = DeviceHandlerBase::enableReplyInReplyMap(command, enabledReplies, true, - mpsoc::TM_CAM_CMD_RPT); - if (result != returnvalue::OK) { - sif::debug << "PlocMPSoCHandler::enableReplyInReplyMap: Reply with id " - << mpsoc::TM_CAM_CMD_RPT << " not in replyMap" << std::endl; - return result; - } + enableThreeReplies(mpsoc::TM_CAM_CMD_RPT); break; } case mpsoc::OBSW_RESET_SEQ_COUNT: @@ -1065,6 +1065,13 @@ void PlocMPSoCHandler::disableAllReplies() { DeviceCommandId_t commandId = getPendingCommand(); + auto disableCommandWithReply = [](DeviceCommandId_t replyId) { + iter = deviceReplyMap.find(replyId); + info = &(iter->second); + info->delayCycles = 0; + info->active = false; + info->command = deviceCommandMap.end(); + }; /* If the command expects a telemetry packet the appropriate tm reply will be disabled here */ switch (commandId) { case TC_MEM_WRITE: @@ -1082,19 +1089,15 @@ void PlocMPSoCHandler::disableAllReplies() { case TC_MODE_SNAPSHOT: break; case TC_MEM_READ: { - iter = deviceReplyMap.find(TM_MEMORY_READ_REPORT); - info = &(iter->second); - info->delayCycles = 0; - info->active = false; - info->command = deviceCommandMap.end(); + disableCommandWithReply(TM_MEMORY_READ_REPORT); + break; + } + case TC_GET_HK_REPORT: { + disableCommandWithReply(TM_GET_HK_REPORT); break; } case TC_CAM_CMD_SEND: { - iter = deviceReplyMap.find(TM_CAM_CMD_RPT); - info = &(iter->second); - info->delayCycles = 0; - info->active = false; - info->command = deviceCommandMap.end(); + disableCommandWithReply(TM_CAM_CMD_RPT); break; } default: {