fsfw/devicehandlers/DeviceHandlerBase.cpp

#include "DeviceHandlerBase.h"
#include "AcceptsDeviceResponsesIF.h"
#include "DeviceTmReportingWrapper.h"

#include "../serviceinterface/ServiceInterface.h"
#include "../objectmanager/ObjectManager.h"
#include "../storagemanager/StorageManagerIF.h"
#include "../thermal/ThermalComponentIF.h"
#include "../globalfunctions/CRC.h"
#include "../housekeeping/HousekeepingMessage.h"
#include "../ipc/MessageQueueMessage.h"
#include "../ipc/QueueFactory.h"
#include "../subsystem/SubsystemBase.h"
#include "../datapoollocal/LocalPoolVariable.h"

object_id_t DeviceHandlerBase::powerSwitcherId = objects::NO_OBJECT;
object_id_t DeviceHandlerBase::rawDataReceiverId = objects::NO_OBJECT;
object_id_t DeviceHandlerBase::defaultFdirParentId = objects::NO_OBJECT;

DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId,
		object_id_t deviceCommunication, CookieIF * comCookie,
		FailureIsolationBase* fdirInstance, size_t cmdQueueSize) :
		SystemObject(setObjectId), mode(MODE_OFF), submode(SUBMODE_NONE),
		wiretappingMode(OFF), storedRawData(StorageManagerIF::INVALID_ADDRESS),
		deviceCommunicationId(deviceCommunication), comCookie(comCookie),
		healthHelper(this,setObjectId), modeHelper(this), parameterHelper(this),
		actionHelper(this, nullptr), poolManager(this, nullptr),
		childTransitionFailure(RETURN_OK), fdirInstance(fdirInstance),
		hkSwitcher(this), defaultFDIRUsed(fdirInstance == nullptr),
		switchOffWasReported(false), childTransitionDelay(5000),
		transitionSourceMode(_MODE_POWER_DOWN),
		transitionSourceSubMode(SUBMODE_NONE) {
	commandQueue = QueueFactory::instance()->createMessageQueue(cmdQueueSize,
			MessageQueueMessage::MAX_MESSAGE_SIZE);
	insertInCommandMap(RAW_COMMAND_ID);
	cookieInfo.state = COOKIE_UNUSED;
	cookieInfo.pendingCommand = deviceCommandMap.end();
	if (comCookie == nullptr) {
		printWarningOrError(sif::OutputTypes::OUT_ERROR, "DeviceHandlerBase",
				HasReturnvaluesIF::RETURN_FAILED, "Invalid cookie");
	}
	if (this->fdirInstance == nullptr) {
		this->fdirInstance = new DeviceHandlerFailureIsolation(setObjectId,
				defaultFdirParentId);
	}
}

void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) {
	this->hkDestination = hkDestination;
}

void DeviceHandlerBase::setThermalStateRequestPoolIds(
		lp_id_t thermalStatePoolId, lp_id_t heaterRequestPoolId,
		uint32_t thermalSetId) {
	thermalSet = new DeviceHandlerThermalSet(this, thermalSetId,
			thermalStatePoolId, heaterRequestPoolId);
}


DeviceHandlerBase::~DeviceHandlerBase() {
	delete comCookie;
	if (defaultFDIRUsed) {
		delete fdirInstance;
	}
	QueueFactory::instance()->deleteMessageQueue(commandQueue);
}

ReturnValue_t DeviceHandlerBase::performOperation(uint8_t counter) {
	this->pstStep = counter;
	this->lastStep = this->pstStep;

	if (getComAction() == CommunicationAction::NOTHING) {
	    return HasReturnvaluesIF::RETURN_OK;
	}

	if (getComAction() == CommunicationAction::PERFORM_OPERATION) {
		cookieInfo.state = COOKIE_UNUSED;
		readCommandQueue();
		doStateMachine();
		checkSwitchState();
		decrementDeviceReplyMap();
		fdirInstance->checkForFailures();
		hkSwitcher.performOperation();
		performOperationHook();
		return RETURN_OK;
	}

	if (mode == MODE_OFF) {
		return RETURN_OK;
	}

	switch (getComAction()) {
	case CommunicationAction::SEND_WRITE:
		if (cookieInfo.state == COOKIE_UNUSED) {
		    // if no external command was specified, build internal command.
			buildInternalCommand();
		}
		doSendWrite();
		break;
	case CommunicationAction::GET_WRITE:
		doGetWrite();
		break;
	case CommunicationAction::SEND_READ:
		doSendRead();
		break;
	case CommunicationAction::GET_READ:
		doGetRead();
		// This will be performed after datasets have been updated by the
		// custom device implementation.
		poolManager.performHkOperation();
		break;
	default:
		break;
	}
	return RETURN_OK;
}

ReturnValue_t DeviceHandlerBase::initialize() {
	ReturnValue_t result = SystemObject::initialize();
	if (result != RETURN_OK) {
		return result;
	}

	communicationInterface = objectManager->get<DeviceCommunicationIF>(
			deviceCommunicationId);
	if (communicationInterface == nullptr) {
		printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize",
				ObjectManagerIF::CHILD_INIT_FAILED,
				"Passed communication IF invalid");
		return ObjectManagerIF::CHILD_INIT_FAILED;
	}

	result = communicationInterface->initializeInterface(comCookie);
	if (result != RETURN_OK) {
		printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize",
				ObjectManagerIF::CHILD_INIT_FAILED,
				"ComIF initialization failed");
	    return result;
	}

	IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
	if (IPCStore == nullptr) {
		printWarningOrError(sif::OutputTypes::OUT_ERROR, "initialize",
				ObjectManagerIF::CHILD_INIT_FAILED, "IPC Store not set up");
		return ObjectManagerIF::CHILD_INIT_FAILED;
	}

	if(rawDataReceiverId != objects::NO_OBJECT) {
		AcceptsDeviceResponsesIF *rawReceiver = objectManager->get<
				AcceptsDeviceResponsesIF>(rawDataReceiverId);

		if (rawReceiver == nullptr) {
			printWarningOrError(sif::OutputTypes::OUT_ERROR,
					"initialize", ObjectManagerIF::CHILD_INIT_FAILED,
					"Raw receiver object ID set but no valid object found.");
#if FSFW_CPP_OSTREAM_ENABLED == 1
			sif::error << "Make sure the raw receiver object is set up properly"
					" and implements AcceptsDeviceResponsesIF" << std::endl;
#else
			sif::printError("Make sure the raw receiver object is set up "
					"properly and implements AcceptsDeviceResponsesIF\n");
#endif
			return ObjectManagerIF::CHILD_INIT_FAILED;
		}
		defaultRawReceiver = rawReceiver->getDeviceQueue();
	}

	if(powerSwitcherId != objects::NO_OBJECT) {
		powerSwitcher = objectManager->get<PowerSwitchIF>(powerSwitcherId);
		if (powerSwitcher == nullptr) {
			printWarningOrError(sif::OutputTypes::OUT_ERROR,
					"initialize", ObjectManagerIF::CHILD_INIT_FAILED,
					"Power switcher set but no valid object found.");
#if FSFW_CPP_OSTREAM_ENABLED == 1
			sif::error << "Make sure the power switcher object is set up "
					<< "properly and implements PowerSwitchIF" << std::endl;
#else
			sif::printError("Make sure the power switcher object is set up "
					"properly and implements PowerSwitchIF\n");
#endif
			return ObjectManagerIF::CHILD_INIT_FAILED;
		}
	}

	result = healthHelper.initialize();
	if (result != RETURN_OK) {
	    return result;
	}

	result = modeHelper.initialize();
	if (result != RETURN_OK) {
		return result;
	}
	result = actionHelper.initialize(commandQueue);
	if (result != RETURN_OK) {
		return result;
	}
	result = fdirInstance->initialize();
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}

	result = parameterHelper.initialize();
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}

	result = hkSwitcher.initialize();
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}

	result = poolManager.initialize(commandQueue);
	if (result != HasReturnvaluesIF::RETURN_OK) {
		return result;
	}

	fillCommandAndReplyMap();

	if(thermalSet != nullptr) {
		//Set temperature target state to NON_OP.
		result = thermalSet->read();
		if(result == HasReturnvaluesIF::RETURN_OK) {
			thermalSet->heaterRequest.value =
					ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL;
			thermalSet->heaterRequest.setValid(true);
			thermalSet->commit();
		}

	}

	return RETURN_OK;
}

void DeviceHandlerBase::decrementDeviceReplyMap() {
	for (std::map<DeviceCommandId_t, DeviceReplyInfo>::iterator iter =
			deviceReplyMap.begin(); iter != deviceReplyMap.end(); iter++) {
		if (iter->second.delayCycles != 0) {
			iter->second.delayCycles--;
			if (iter->second.delayCycles == 0) {
				if (iter->second.periodic) {
					iter->second.delayCycles = iter->second.maxDelayCycles;
				}
				replyToReply(iter, TIMEOUT);
				missedReply(iter->first);
			}
		}
	}
}

void DeviceHandlerBase::readCommandQueue() {

	if (dontCheckQueue()) {
		return;
	}

	CommandMessage command;
	ReturnValue_t result = commandQueue->receiveMessage(&command);
	if (result != RETURN_OK) {
		return;
	}

	result = healthHelper.handleHealthCommand(&command);
	if (result == RETURN_OK) {
	    return;
	}

	result = modeHelper.handleModeCommand(&command);
	if (result == RETURN_OK) {
		return;
	}

	result = actionHelper.handleActionMessage(&command);
	if (result == RETURN_OK) {
		return;
	}

	result = parameterHelper.handleParameterMessage(&command);
	if (result == RETURN_OK) {
		return;
	}

	result = poolManager.handleHousekeepingMessage(&command);
	if (result == RETURN_OK) {
		return;
	}

	result = handleDeviceHandlerMessage(&command);
	if (result == RETURN_OK) {
		return;
	}

	result = letChildHandleMessage(&command);
	if (result == RETURN_OK) {
		return;
	}

	replyReturnvalueToCommand(CommandMessage::UNKNOWN_COMMAND);

}

void DeviceHandlerBase::doStateMachine() {
	switch (mode) {
	case _MODE_START_UP:
	case _MODE_SHUT_DOWN:
	case _MODE_TO_NORMAL:
	case _MODE_TO_ON:
	case _MODE_TO_RAW: {
		Mode_t currentMode = mode;
		callChildStatemachine();
		//Only do timeout if child did not change anything
		if (mode != currentMode) {
			break;
		}
		uint32_t currentUptime;
		Clock::getUptime(&currentUptime);
		if (currentUptime - timeoutStart >= childTransitionDelay) {
			triggerEvent(MODE_TRANSITION_FAILED, childTransitionFailure, 0);
			setMode(transitionSourceMode, transitionSourceSubMode);
			break;
		}
	}
		break;
	case _MODE_POWER_DOWN:
		commandSwitch(PowerSwitchIF::SWITCH_OFF);
		setMode(_MODE_WAIT_OFF);
		break;
	case _MODE_POWER_ON:
		commandSwitch(PowerSwitchIF::SWITCH_ON);
		setMode(_MODE_WAIT_ON);
		break;
	case _MODE_WAIT_ON: {
		uint32_t currentUptime;
		Clock::getUptime(&currentUptime);
		if (powerSwitcher != nullptr and currentUptime - timeoutStart >=
				powerSwitcher->getSwitchDelayMs()) {
			triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT,
					0);
			setMode(_MODE_POWER_DOWN);
			callChildStatemachine();
			break;
		}
		ReturnValue_t switchState = getStateOfSwitches();
		if ((switchState == PowerSwitchIF::SWITCH_ON)
				|| (switchState == NO_SWITCH)) {
			//NOTE: TransitionSourceMode and -SubMode are set by handleCommandedModeTransition
			childTransitionFailure = CHILD_TIMEOUT;
			setMode(_MODE_START_UP);
			callChildStatemachine();
		}
	}
		break;
	case _MODE_WAIT_OFF: {
		uint32_t currentUptime;
		Clock::getUptime(&currentUptime);

		if(powerSwitcher == nullptr) {
		    setMode(MODE_OFF);
		    break;
		}

		if (currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) {
			triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT,
					0);
			setMode(MODE_ERROR_ON);
			break;
		}
		ReturnValue_t switchState = getStateOfSwitches();
		if ((switchState == PowerSwitchIF::SWITCH_OFF)
				|| (switchState == NO_SWITCH)) {
			setMode(_MODE_SWITCH_IS_OFF);
		}
	}
		break;
	case MODE_OFF:
		doOffActivity();
		break;
	case MODE_ON:
		doOnActivity();
		break;
	case MODE_RAW:
	case MODE_NORMAL:
	case MODE_ERROR_ON:
		break;
	case _MODE_SWITCH_IS_OFF:
		setMode(MODE_OFF, SUBMODE_NONE);
		break;
	default:
		triggerEvent(OBJECT_IN_INVALID_MODE, mode, submode);
		setMode(_MODE_POWER_DOWN, 0);
		break;
	}
}

ReturnValue_t DeviceHandlerBase::isModeCombinationValid(Mode_t mode,
		Submode_t submode) {
	switch (mode) {
	case MODE_OFF:
	case MODE_ON:
	case MODE_NORMAL:
	case MODE_RAW:
		if (submode == SUBMODE_NONE) {
			return RETURN_OK;
		} else {
			return INVALID_SUBMODE;
		}
	default:
		return HasModesIF::INVALID_MODE;
	}
}

ReturnValue_t DeviceHandlerBase::insertInCommandAndReplyMap(
		DeviceCommandId_t deviceCommand, uint16_t maxDelayCycles,
		LocalPoolDataSetBase* replyDataSet, size_t replyLen, bool periodic,
		bool hasDifferentReplyId, DeviceCommandId_t replyId) {
	//No need to check, as we may try to insert multiple times.
	insertInCommandMap(deviceCommand);
	if (hasDifferentReplyId) {
		return insertInReplyMap(replyId, maxDelayCycles,
		        replyDataSet, replyLen, periodic);
	} else {
		return insertInReplyMap(deviceCommand, maxDelayCycles,
		        replyDataSet, replyLen, periodic);
	}
}

ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId,
		uint16_t maxDelayCycles, LocalPoolDataSetBase* dataSet,
		size_t replyLen, bool periodic) {
	DeviceReplyInfo info;
	info.maxDelayCycles = maxDelayCycles;
	info.periodic = periodic;
	info.delayCycles = 0;
	info.replyLen = replyLen;
	info.dataSet = dataSet;
	info.command = deviceCommandMap.end();
	auto resultPair = deviceReplyMap.emplace(replyId, info);
	if (resultPair.second) {
		return RETURN_OK;
	} else {
		return RETURN_FAILED;
	}
}

ReturnValue_t DeviceHandlerBase::insertInCommandMap(
		DeviceCommandId_t deviceCommand) {
	DeviceCommandInfo info;
	info.expectedReplies = 0;
	info.isExecuting = false;
	info.sendReplyTo = NO_COMMANDER;
	auto resultPair = deviceCommandMap.emplace(deviceCommand, info);
	if (resultPair.second) {
		return RETURN_OK;
	} else {
		return RETURN_FAILED;
	}
}

ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceReply,
		uint16_t delayCycles, uint16_t maxDelayCycles, bool periodic) {
	auto replyIter = deviceReplyMap.find(deviceReply);
	if (replyIter == deviceReplyMap.end()) {
		triggerEvent(INVALID_DEVICE_COMMAND, deviceReply);
		return RETURN_FAILED;
	} else {
		DeviceReplyInfo *info = &(replyIter->second);
		if (maxDelayCycles != 0) {
			info->maxDelayCycles = maxDelayCycles;
		}
		info->delayCycles = delayCycles;
		info->periodic = periodic;
		return RETURN_OK;
	}
}


ReturnValue_t DeviceHandlerBase::setReplyDataset(DeviceCommandId_t replyId,
        LocalPoolDataSetBase *dataSet) {
    auto replyIter = deviceReplyMap.find(replyId);
    if(replyIter == deviceReplyMap.end()) {
        return HasReturnvaluesIF::RETURN_FAILED;
    }
    replyIter->second.dataSet = dataSet;
    return HasReturnvaluesIF::RETURN_OK;
}

void DeviceHandlerBase::callChildStatemachine() {
	if (mode == _MODE_START_UP) {
		doStartUp();
	} else if (mode == _MODE_SHUT_DOWN) {
		doShutDown();
	} else if (mode & TRANSITION_MODE_CHILD_ACTION_MASK) {
		doTransition(transitionSourceMode, transitionSourceSubMode);
	}
}

void DeviceHandlerBase::setTransition(Mode_t modeTo, Submode_t submodeTo) {
	triggerEvent(CHANGING_MODE, modeTo, submodeTo);
	childTransitionDelay = getTransitionDelayMs(mode, modeTo);
	transitionSourceMode = mode;
	transitionSourceSubMode = submode;
	childTransitionFailure = CHILD_TIMEOUT;

	// transitionTargetMode is set by setMode
	setMode((modeTo | TRANSITION_MODE_CHILD_ACTION_MASK), submodeTo);
}

void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) {
	changeHK(mode, submode, false);
	submode = newSubmode;
	mode = newMode;
	modeChanged();
	setNormalDatapoolEntriesInvalid();
	if (!isTransitionalMode()) {
		modeHelper.modeChanged(newMode, newSubmode);
		announceMode(false);
	}
	Clock::getUptime(&timeoutStart);

	if (mode == MODE_OFF and thermalSet != nullptr) {
		ReturnValue_t result = thermalSet->read();
		if(result == HasReturnvaluesIF::RETURN_OK) {
			if (thermalSet->heaterRequest.value !=
					ThermalComponentIF::STATE_REQUEST_IGNORE) {
				thermalSet->heaterRequest.value = ThermalComponentIF::
						STATE_REQUEST_NON_OPERATIONAL;
			}
			thermalSet->heaterRequest.commit(PoolVariableIF::VALID);
		}

	}
	changeHK(mode, submode, true);
}

void DeviceHandlerBase::setMode(Mode_t newMode) {
	setMode(newMode, submode);
}

void DeviceHandlerBase::replyReturnvalueToCommand(ReturnValue_t status,
		uint32_t parameter) {
	//This is actually the reply protocol for raw and misc DH commands.
	if (status == RETURN_OK) {
		CommandMessage reply(CommandMessage::REPLY_COMMAND_OK, 0, parameter);
		commandQueue->reply(&reply);
	} else {
		CommandMessage reply(CommandMessage::REPLY_REJECTED, status, parameter);
		commandQueue->reply(&reply);
	}
}

void DeviceHandlerBase::replyToCommand(ReturnValue_t status,
		uint32_t parameter) {
	// Check if we reply to a raw command.
	if (cookieInfo.pendingCommand->first == RAW_COMMAND_ID) {
		if (status == NO_REPLY_EXPECTED) {
			status = RETURN_OK;
		}
		replyReturnvalueToCommand(status, parameter);
		// Always delete data from a raw command.
		IPCStore->deleteData(storedRawData);
		return;
	}
	// Check if we were externally commanded.
	if (cookieInfo.pendingCommand->second.sendReplyTo != NO_COMMANDER) {
		MessageQueueId_t queueId = cookieInfo.pendingCommand->second.sendReplyTo;
		if (status == NO_REPLY_EXPECTED) {
			actionHelper.finish(queueId, cookieInfo.pendingCommand->first,
					RETURN_OK);
		} else {
			actionHelper.step(1, queueId, cookieInfo.pendingCommand->first,
					status);
		}
	}
}

void DeviceHandlerBase::replyToReply(DeviceReplyMap::iterator iter,
		ReturnValue_t status) {