#include <framework/health/HealthMessage.h> #include <framework/objectmanager/ObjectManagerIF.h> #include <framework/serialize/SerialArrayListAdapter.h> #include <framework/serialize/SerialFixedArrayListAdapter.h> #include <framework/serialize/SerializeElement.h> #include <framework/serialize/SerialLinkedListAdapter.h> #include <framework/subsystem/Subsystem.h> #include <string> Subsystem::Subsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables) : SubsystemBase(setObjectId, parent, 0), isInTransition(false), childrenChangedHealth( false), uptimeStartTable(0), currentTargetTable(), targetMode( 0), targetSubmode(SUBMODE_NONE), initialMode(0), currentSequenceIterator(), modeTables( maxNumberOfTables), modeSequences(maxNumberOfSequences), IPCStore( NULL) #ifdef USE_MODESTORE ,modeStore(NULL) #endif { } Subsystem::~Subsystem() { //Auto-generated destructor stub } ReturnValue_t Subsystem::checkSequence(HybridIterator<ModeListEntry> iter, Mode_t fallbackSequence) { //only check for existence, checking the fallback would lead to a (possibly infinite) recursion. //the fallback sequence will be checked when it is needed. if (!existsModeSequence(fallbackSequence)) { return FALLBACK_SEQUENCE_DOES_NOT_EXIST; } if (iter.value == NULL) { return NO_TARGET_TABLE; } for (; iter.value != NULL; ++iter) { if (!existsModeTable(iter->getTableId())) { return TABLE_DOES_NOT_EXIST; } else { ReturnValue_t result = checkTable(getTable(iter->getTableId())); if (result != RETURN_OK) { return result; } } } return RETURN_OK; } ReturnValue_t Subsystem::checkSequence(Mode_t sequence) { if (!existsModeSequence(sequence)) { return SEQUENCE_DOES_NOT_EXIST; } HybridIterator<ModeListEntry> iter = getSequence(sequence); return checkSequence(iter, getFallbackSequence(sequence)); } bool Subsystem::existsModeSequence(Mode_t id) { return modeSequences.exists(id) == RETURN_OK; } bool Subsystem::existsModeTable(Mode_t id) { return modeTables.exists(id) == RETURN_OK; } HybridIterator<ModeListEntry> Subsystem::getCurrentTable() { return getTable(currentSequenceIterator->getTableId()); } void Subsystem::performChildOperation() { if (isInTransition) { if (commandsOutstanding <= 0) { //all children of the current table were commanded and replied if (currentSequenceIterator.value == NULL) { //we're through with this sequence if (checkStateAgainstTable(currentTargetTable, targetSubmode) == RETURN_OK) { setMode(targetMode, targetSubmode); isInTransition = false; return; } else { transitionFailed(TARGET_TABLE_NOT_REACHED, getSequence(targetMode)->getTableId()); return; } } if (currentSequenceIterator->checkSuccess()) { if (checkStateAgainstTable(getCurrentTable(), targetSubmode) != RETURN_OK) { transitionFailed(TABLE_CHECK_FAILED, currentSequenceIterator->getTableId()); return; } } if (currentSequenceIterator->getWaitSeconds() != 0) { if (uptimeStartTable == 0) { Clock::getUptime(&uptimeStartTable); return; } else { uint32_t uptimeNow; Clock::getUptime(&uptimeNow); if ((uptimeNow - uptimeStartTable) < (currentSequenceIterator->getWaitSeconds() * 1000)) { return; } } } uptimeStartTable = 0; //next Table, but only if there is one if ((++currentSequenceIterator).value != NULL) { //we're through with this sequence executeTable(getCurrentTable(), targetSubmode); } } } else { if (childrenChangedHealth) { triggerEvent(CHILD_CHANGED_HEALTH, 0, 0); childrenChangedHealth = false; startTransition(mode, submode); } else if (childrenChangedMode) { if (checkStateAgainstTable(currentTargetTable, submode) != RETURN_OK) { triggerEvent(CANT_KEEP_MODE, mode, submode); cantKeepMode(); } } } } HybridIterator<ModeListEntry> Subsystem::getSequence(Mode_t id) { SequenceInfo *sequenceInfo = modeSequences.findValue(id); if (sequenceInfo->entries.islinked) { return HybridIterator<ModeListEntry>( sequenceInfo->entries.firstLinkedElement); } else { return HybridIterator<ModeListEntry>( sequenceInfo->entries.array->front(), sequenceInfo->entries.array->back()); } } HybridIterator<ModeListEntry> Subsystem::getTable(Mode_t id) { EntryPointer *entry = modeTables.findValue(id); if (entry->islinked) { return HybridIterator<ModeListEntry>(entry->firstLinkedElement); } else { return HybridIterator<ModeListEntry>(entry->array->front(), entry->array->back()); } } ReturnValue_t Subsystem::handleCommandMessage(CommandMessage* message) { ReturnValue_t result; switch (message->getCommand()) { case HealthMessage::HEALTH_INFO: { HealthState health = HealthMessage::getHealth(message); if (health != EXTERNAL_CONTROL) { //Ignore external control, as it has an effect only if the mode changes, //which is communicated with an additional mode info event. childrenChangedHealth = true; } } break; case ModeSequenceMessage::ADD_SEQUENCE: { FixedArrayList<ModeListEntry, MAX_LENGTH_OF_TABLE_OR_SEQUENCE> sequence; const uint8_t *pointer; uint32_t sizeRead; result = IPCStore->getData( ModeSequenceMessage::getStoreAddress(message), &pointer, &sizeRead); if (result == RETURN_OK) { Mode_t fallbackId; int32_t size = sizeRead; result = SerializeAdapter<Mode_t>::deSerialize(&fallbackId, &pointer, &size, true); if (result == RETURN_OK) { result = SerialArrayListAdapter<ModeListEntry>::deSerialize( &sequence, &pointer, &size, true); if (result == RETURN_OK) { result = addSequence(&sequence, ModeSequenceMessage::getSequenceId(message), fallbackId); } } IPCStore->deleteData(ModeSequenceMessage::getStoreAddress(message)); } replyToCommand(result, 0); } break; case ModeSequenceMessage::ADD_TABLE: { FixedArrayList<ModeListEntry, MAX_LENGTH_OF_TABLE_OR_SEQUENCE> table; const uint8_t *pointer; uint32_t sizeRead; result = IPCStore->getData( ModeSequenceMessage::getStoreAddress(message), &pointer, &sizeRead); if (result == RETURN_OK) { int32_t size = sizeRead; result = SerialArrayListAdapter<ModeListEntry>::deSerialize(&table, &pointer, &size, true); if (result == RETURN_OK) { result = addTable(&table, ModeSequenceMessage::getSequenceId(message)); } IPCStore->deleteData(ModeSequenceMessage::getStoreAddress(message)); } replyToCommand(result, 0); } break; case ModeSequenceMessage::DELETE_SEQUENCE: if (isInTransition) { replyToCommand(IN_TRANSITION, 0); break; } result = deleteSequence(ModeSequenceMessage::getSequenceId(message)); replyToCommand(result, 0); break; case ModeSequenceMessage::DELETE_TABLE: if (isInTransition) { replyToCommand(IN_TRANSITION, 0); break; } result = deleteTable(ModeSequenceMessage::getTableId(message)); replyToCommand(result, 0); break; case ModeSequenceMessage::LIST_SEQUENCES: { SerialFixedArrayListAdapter<Mode_t, MAX_NUMBER_OF_TABLES_OR_SEQUENCES> sequences; FixedMap<Mode_t, SequenceInfo>::Iterator iter; for (iter = modeSequences.begin(); iter != modeSequences.end(); ++iter) { sequences.insert(iter.value->first); } SerializeIF *pointer = &sequences; sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE_LIST, &pointer, 1); } break; case ModeSequenceMessage::LIST_TABLES: { SerialFixedArrayListAdapter<Mode_t, MAX_NUMBER_OF_TABLES_OR_SEQUENCES> tables; FixedMap<Mode_t, EntryPointer>::Iterator iter; for (iter = modeTables.begin(); iter != modeTables.end(); ++iter) { tables.insert(iter.value->first); } SerializeIF *pointer = &tables; sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE_LIST, &pointer, 1); } break; case ModeSequenceMessage::READ_SEQUENCE: { ReturnValue_t result; Mode_t sequence = ModeSequenceMessage::getSequenceId(message); SequenceInfo *sequenceInfo = NULL; result = modeSequences.find(sequence, &sequenceInfo); if (result != RETURN_OK) { replyToCommand(result, 0); } SerializeIF *elements[3]; SerializeElement<Mode_t> sequenceId(sequence); SerializeElement<Mode_t> fallbackSequenceId( getFallbackSequence(sequence)); elements[0] = &sequenceId; elements[1] = &fallbackSequenceId; if (sequenceInfo->entries.islinked) { SerialLinkedListAdapter<ModeListEntry> list( sequenceInfo->entries.firstLinkedElement, true); elements[2] = &list; sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE, elements, 3); } else { SerialArrayListAdapter<ModeListEntry> serializableArray( sequenceInfo->entries.array); elements[2] = &serializableArray; sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE, elements, 3); } } break; case ModeSequenceMessage::READ_TABLE: { ReturnValue_t result; Mode_t table = ModeSequenceMessage::getSequenceId(message); EntryPointer *entry = NULL; result = modeTables.find(table, &entry); if (result != RETURN_OK) { replyToCommand(result, 0); } SerializeIF *elements[2]; SerializeElement<Mode_t> tableId(table); elements[0] = &tableId; if (entry->islinked) { SerialLinkedListAdapter<ModeListEntry> list( entry->firstLinkedElement, true); elements[1] = &list; sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE, elements, 2); } else { SerialArrayListAdapter<ModeListEntry> serializableArray( entry->array); elements[1] = &serializableArray; sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE, elements, 2); } } break; case ModeSequenceMessage::READ_FREE_SEQUENCE_SLOTS: { uint32_t freeSlots = modeSequences.maxSize() - modeSequences.size(); CommandMessage reply; ModeSequenceMessage::setModeSequenceMessage(&reply, ModeSequenceMessage::FREE_SEQUENCE_SLOTS, freeSlots); commandQueue->reply(&reply); } break; case ModeSequenceMessage::READ_FREE_TABLE_SLOTS: { uint32_t free = modeTables.maxSize() - modeTables.size(); CommandMessage reply; ModeSequenceMessage::setModeSequenceMessage(&reply, ModeSequenceMessage::FREE_TABLE_SLOTS, free); commandQueue->reply(&reply); } break; default: return RETURN_FAILED; } return RETURN_OK; } void Subsystem::replyToCommand(ReturnValue_t status, uint32_t parameter) { if (status == RETURN_OK) { CommandMessage reply(CommandMessage::REPLY_COMMAND_OK, 0, 0); commandQueue->reply(&reply); } else { CommandMessage reply(CommandMessage::REPLY_REJECTED, status, 0); commandQueue->reply(&reply); } } ReturnValue_t Subsystem::addSequence(ArrayList<ModeListEntry>* sequence, Mode_t id, Mode_t fallbackSequence, bool inStore, bool preInit) { ReturnValue_t result; //Before initialize() is called, tables must not be checked as the children are not added yet. //Sequences added before are checked by initialize() if (!preInit) { result = checkSequence( HybridIterator<ModeListEntry>(sequence->front(), sequence->back()), fallbackSequence); if (result != RETURN_OK) { return result; } } SequenceInfo info; info.fallbackSequence = fallbackSequence; info.entries.islinked = inStore; info.entries.array = sequence; result = modeSequences.insert(id, info); if (result != RETURN_OK) { return result; } if (inStore) { #ifdef USE_MODESTORE result = modeStore->storeArray(sequence, &(modeSequences.find(id)->entries.firstLinkedElement)); if (result != RETURN_OK) { modeSequences.erase(id); } #else modeSequences.erase(id); return RETURN_FAILED; #endif } return result; } ReturnValue_t Subsystem::addTable(ArrayList<ModeListEntry> *table, Mode_t id, bool inStore, bool preInit) { ReturnValue_t result; //Before initialize() is called, tables must not be checked as the children are not added yet. //Tables added before are checked by initialize() if (!preInit) { result = checkTable( HybridIterator<ModeListEntry>(table->front(), table->back())); if (result != HasReturnvaluesIF::RETURN_OK) { return result; } } EntryPointer pointer; pointer.islinked = inStore; pointer.array = table; result = modeTables.insert(id, pointer); if (result != RETURN_OK) { return result; } if (inStore) { #ifdef USE_MODESTORE result = modeStore->storeArray(table, &(modeTables.find(id)->firstLinkedElement)); if (result != RETURN_OK) { modeTables.erase(id); } #else modeTables.erase(id); return RETURN_FAILED; #endif } return result; } ReturnValue_t Subsystem::deleteSequence(Mode_t id) { if (isFallbackSequence(id)) { return IS_FALLBACK_SEQUENCE; } SequenceInfo *sequenceInfo; ReturnValue_t result; result = modeSequences.find(id, &sequenceInfo); if (result != HasReturnvaluesIF::RETURN_OK) { return result; } if (!sequenceInfo->entries.islinked) { return ACCESS_DENIED; } #ifdef USE_MODESTORE modeStore->deleteList(sequenceInfo->entries.firstLinkedElement); #endif modeSequences.erase(id); return RETURN_OK; } ReturnValue_t Subsystem::deleteTable(Mode_t id) { if (isTableUsed(id)) { return TABLE_IN_USE; } EntryPointer *pointer; ReturnValue_t result; result = modeTables.find(id, &pointer); if (result != HasReturnvaluesIF::RETURN_OK) { return result; } if (!pointer->islinked) { return ACCESS_DENIED; } #ifdef USE_MODESTORE modeStore->deleteList(pointer->firstLinkedElement); #endif modeSequences.erase(id); return RETURN_OK; } ReturnValue_t Subsystem::initialize() { ReturnValue_t result = SubsystemBase::initialize(); if (result != RETURN_OK) { return result; } IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE); if (IPCStore == NULL) { return RETURN_FAILED; } #ifdef USE_MODESTORE modeStore = objectManager->get<ModeStoreIF>(objects::MODE_STORE); if (modeStore == NULL) { return RETURN_FAILED; } #endif if ((modeSequences.maxSize() > MAX_NUMBER_OF_TABLES_OR_SEQUENCES) || (modeTables.maxSize() > MAX_NUMBER_OF_TABLES_OR_SEQUENCES)) { return TABLE_OR_SEQUENCE_LENGTH_INVALID; } mode = initialMode; return RETURN_OK; } MessageQueueId_t Subsystem::getSequenceCommandQueue() const { return SubsystemBase::getCommandQueue(); } ReturnValue_t Subsystem::checkModeCommand(Mode_t mode, Submode_t submode, uint32_t* msToReachTheMode) { //Need to accept all submodes to be able to inherit submodes // if (submode != SUBMODE_NONE) { // return INVALID_SUBMODE; // } if (isInTransition && (mode != getFallbackSequence(targetMode))) { return HasModesIF::IN_TRANSITION; } else { return checkSequence(mode); } } void Subsystem::startTransition(Mode_t sequence, Submode_t submode) { if (modeHelper.isForced()) { triggerEvent(FORCING_MODE, sequence, submode); } else { triggerEvent(CHANGING_MODE, sequence, submode); } targetMode = sequence; targetSubmode = submode; isInTransition = true; commandsOutstanding = 0; currentSequenceIterator = getSequence(sequence); currentTargetTable = getTable(currentSequenceIterator->getTableId()); ++currentSequenceIterator; if (currentSequenceIterator.value != NULL) { executeTable(getCurrentTable(), targetSubmode); } } Mode_t Subsystem::getFallbackSequence(Mode_t sequence) { for (FixedMap<Mode_t, SequenceInfo>::Iterator iter = modeSequences.begin(); iter != modeSequences.end(); ++iter) { if (iter.value->first == sequence) { return iter->fallbackSequence; } } return -1; } bool Subsystem::isFallbackSequence(Mode_t SequenceId) { for (FixedMap<Mode_t, SequenceInfo>::Iterator iter = modeSequences.begin(); iter != modeSequences.end(); iter++) { if (iter->fallbackSequence == SequenceId) { return true; } } return false; } bool Subsystem::isTableUsed(Mode_t tableId) { for (FixedMap<Mode_t, SequenceInfo>::Iterator sequence = modeSequences.begin(); sequence != modeSequences.end(); sequence++) { HybridIterator<ModeListEntry> sequenceIterator = getSequence( sequence.value->first); while (sequenceIterator.value != NULL) { if (sequenceIterator->getTableId() == tableId) { return true; } ++sequenceIterator; } } return false; } void Subsystem::transitionFailed(ReturnValue_t failureCode, uint32_t parameter) { triggerEvent(MODE_TRANSITION_FAILED, failureCode, parameter); if (mode == targetMode) { //already tried going back to the current mode //go into fallback mode, also set current mode to fallback mode, so we come here at the next fail modeHelper.setForced(true); ReturnValue_t result; if ((result = checkSequence(getFallbackSequence(mode))) != RETURN_OK) { triggerEvent(FALLBACK_FAILED, result, getFallbackSequence(mode)); isInTransition = false; //keep still and allow arbitrary mode commands to recover return; } mode = getFallbackSequence(mode); startTransition(mode, submode); } else { //try to go back to the current mode startTransition(mode, submode); } } void Subsystem::sendSerializablesAsCommandMessage(Command_t command, SerializeIF** elements, uint8_t count) { ReturnValue_t result; uint32_t maxSize = 0; for (uint8_t i = 0; i < count; i++) { maxSize += elements[i]->getSerializedSize(); } uint8_t *storeBuffer; store_address_t address; uint32_t size = 0; result = IPCStore->getFreeElement(&address, maxSize, &storeBuffer); if (result != HasReturnvaluesIF::RETURN_OK) { replyToCommand(result, 0); return; } for (uint8_t i = 0; i < count; i++) { elements[i]->serialize(&storeBuffer, &size, maxSize, true); } CommandMessage reply; ModeSequenceMessage::setModeSequenceMessage(&reply, command, address); if (commandQueue->reply(&reply) != RETURN_OK) { IPCStore->deleteData(address); } } ReturnValue_t Subsystem::checkObjectConnections() { ReturnValue_t result = RETURN_OK; for (FixedMap<Mode_t, SequenceInfo>::Iterator iter = modeSequences.begin(); iter != modeSequences.end(); iter++) { result = checkSequence(iter.value->first); if (result != RETURN_OK) { return result; } } return RETURN_OK; } void Subsystem::setInitialMode(Mode_t mode) { initialMode = mode; } void Subsystem::cantKeepMode() { ReturnValue_t result; if ((result = checkSequence(getFallbackSequence(mode))) != RETURN_OK) { triggerEvent(FALLBACK_FAILED, result, getFallbackSequence(mode)); return; } modeHelper.setForced(true); //already set the mode, so that we do not try to go back in our old mode when the transition fails mode = getFallbackSequence(mode); //SHOULDDO: We should store submodes for fallback sequence as well, otherwise we should get rid of submodes completely. startTransition(mode, SUBMODE_NONE); }