restructure repository
This commit is contained in:
7
src/core/subsystem/CMakeLists.txt
Normal file
7
src/core/subsystem/CMakeLists.txt
Normal file
@ -0,0 +1,7 @@
|
||||
target_sources(${LIB_FSFW_NAME}
|
||||
PRIVATE
|
||||
Subsystem.cpp
|
||||
SubsystemBase.cpp
|
||||
)
|
||||
|
||||
add_subdirectory(modes)
|
||||
661
src/core/subsystem/Subsystem.cpp
Normal file
661
src/core/subsystem/Subsystem.cpp
Normal file
@ -0,0 +1,661 @@
|
||||
#include "Subsystem.h"
|
||||
|
||||
#include "../health/HealthMessage.h"
|
||||
#include "../objectmanager/ObjectManager.h"
|
||||
#include "../serialize/SerialArrayListAdapter.h"
|
||||
#include "../serialize/SerialFixedArrayListAdapter.h"
|
||||
#include "../serialize/SerializeElement.h"
|
||||
#include "../serialize/SerialLinkedListAdapter.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), currentTargetTable(),
|
||||
targetSubmode(SUBMODE_NONE), currentSequenceIterator(),
|
||||
modeTables(maxNumberOfTables), modeSequences(maxNumberOfSequences) {}
|
||||
|
||||
Subsystem::~Subsystem() {}
|
||||
|
||||
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) {
|
||||
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;
|
||||
size_t sizeRead;
|
||||
ReturnValue_t result = IPCStore->getData(
|
||||
ModeSequenceMessage::getStoreAddress(message), &pointer,
|
||||
&sizeRead);
|
||||
if (result == RETURN_OK) {
|
||||
Mode_t fallbackId;
|
||||
size_t size = sizeRead;
|
||||
result = SerializeAdapter::deSerialize(&fallbackId, &pointer, &size,
|
||||
SerializeIF::Endianness::BIG);
|
||||
if (result == RETURN_OK) {
|
||||
result = SerialArrayListAdapter<ModeListEntry>::deSerialize(
|
||||
&sequence, &pointer, &size,
|
||||
SerializeIF::Endianness::BIG);
|
||||
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;
|
||||
size_t sizeRead;
|
||||
ReturnValue_t result = IPCStore->getData(
|
||||
ModeSequenceMessage::getStoreAddress(message), &pointer,
|
||||
&sizeRead);
|
||||
if (result == RETURN_OK) {
|
||||
size_t size = sizeRead;
|
||||
result = SerialArrayListAdapter<ModeListEntry>::deSerialize(&table,
|
||||
&pointer, &size, SerializeIF::Endianness::BIG);
|
||||
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;
|
||||
}
|
||||
ReturnValue_t result = deleteSequence(ModeSequenceMessage::getSequenceId(message));
|
||||
replyToCommand(result, 0);
|
||||
}
|
||||
break;
|
||||
case ModeSequenceMessage::DELETE_TABLE:{
|
||||
if (isInTransition) {
|
||||
replyToCommand(IN_TRANSITION, 0);
|
||||
break;
|
||||
}
|
||||
ReturnValue_t 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) {
|
||||
#if FSFW_USE_MODESTORE == 1
|
||||
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) {
|
||||
#if FSFW_USE_MODESTORE == 1
|
||||
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;
|
||||
}
|
||||
|
||||
#if FSFW_USE_MODESTORE == 1
|
||||
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;
|
||||
}
|
||||
|
||||
#if FSFW_USE_MODESTORE == 1
|
||||
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::instance()->get<StorageManagerIF>(objects::IPC_STORE);
|
||||
if (IPCStore == NULL) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
#if FSFW_USE_MODESTORE == 1
|
||||
modeStore = ObjectManager::instance()->get<ModeStoreIF>(objects::MODE_STORE);
|
||||
|
||||
if (modeStore == nullptr) {
|
||||
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->second.fallbackSequence;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
bool Subsystem::isFallbackSequence(Mode_t SequenceId) {
|
||||
for (FixedMap<Mode_t, SequenceInfo>::Iterator iter = modeSequences.begin();
|
||||
iter != modeSequences.end(); iter++) {
|
||||
if (iter->second.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));
|
||||
//keep still and allow arbitrary mode commands to recover
|
||||
isInTransition = false;
|
||||
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;
|
||||
size_t maxSize = 0;
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
maxSize += elements[i]->getSerializedSize();
|
||||
}
|
||||
uint8_t *storeBuffer;
|
||||
store_address_t address;
|
||||
size_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,
|
||||
SerializeIF::Endianness::BIG);
|
||||
}
|
||||
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);
|
||||
}
|
||||
356
src/core/subsystem/SubsystemBase.cpp
Normal file
356
src/core/subsystem/SubsystemBase.cpp
Normal file
@ -0,0 +1,356 @@
|
||||
#include "SubsystemBase.h"
|
||||
|
||||
#include "../serviceinterface/ServiceInterface.h"
|
||||
#include "../objectmanager/ObjectManager.h"
|
||||
#include "../ipc/QueueFactory.h"
|
||||
|
||||
SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent,
|
||||
Mode_t initialMode, uint16_t commandQueueDepth) :
|
||||
SystemObject(setObjectId), mode(initialMode),
|
||||
commandQueue(QueueFactory::instance()->createMessageQueue(
|
||||
commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE)),
|
||||
healthHelper(this, setObjectId), modeHelper(this), parentId(parent) {
|
||||
}
|
||||
|
||||
SubsystemBase::~SubsystemBase() {
|
||||
QueueFactory::instance()->deleteMessageQueue(commandQueue);
|
||||
|
||||
}
|
||||
|
||||
ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) {
|
||||
ChildInfo info;
|
||||
|
||||
HasModesIF *child = ObjectManager::instance()->get<HasModesIF>(objectId);
|
||||
// This is a rather ugly hack to have the changedHealth info for all
|
||||
// children available.
|
||||
HasHealthIF* healthChild = ObjectManager::instance()->get<HasHealthIF>(objectId);
|
||||
if (child == nullptr) {
|
||||
if (healthChild == nullptr) {
|
||||
return CHILD_DOESNT_HAVE_MODES;
|
||||
} else {
|
||||
info.commandQueue = healthChild->getCommandQueue();
|
||||
info.mode = MODE_OFF;
|
||||
}
|
||||
} else {
|
||||
info.commandQueue = child->getCommandQueue();
|
||||
info.mode = -1; //intentional to force an initial command during system startup
|
||||
}
|
||||
|
||||
info.submode = SUBMODE_NONE;
|
||||
info.healthChanged = false;
|
||||
|
||||
auto resultPair = childrenMap.emplace(objectId, info);
|
||||
if (not resultPair.second) {
|
||||
return COULD_NOT_INSERT_CHILD;
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t SubsystemBase::checkStateAgainstTable(
|
||||
HybridIterator<ModeListEntry> tableIter, Submode_t targetSubmode) {
|
||||
|
||||
std::map<object_id_t, ChildInfo>::iterator childIter;
|
||||
|
||||
for (; tableIter.value != NULL; ++tableIter) {
|
||||
object_id_t object = tableIter.value->getObject();
|
||||
|
||||
if ((childIter = childrenMap.find(object)) == childrenMap.end()) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
if (childIter->second.mode != tableIter.value->getMode()) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
Submode_t submodeToCheckAgainst = tableIter.value->getSubmode();
|
||||
if (tableIter.value->inheritSubmode()) {
|
||||
submodeToCheckAgainst = targetSubmode;
|
||||
}
|
||||
|
||||
if (childIter->second.submode != submodeToCheckAgainst) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter,
|
||||
Submode_t targetSubmode) {
|
||||
CommandMessage command;
|
||||
|
||||
std::map<object_id_t, ChildInfo>::iterator iter;
|
||||
|
||||
commandsOutstanding = 0;
|
||||
|
||||
for (; tableIter.value != nullptr; ++tableIter) {
|
||||
object_id_t object = tableIter.value->getObject();
|
||||
if ((iter = childrenMap.find(object)) == childrenMap.end()) {
|
||||
//illegal table entry, should only happen due to misconfigured mode table
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::debug << std::hex << getObjectId() << ": invalid mode table entry"
|
||||
<< std::endl;
|
||||
#endif
|
||||
continue;
|
||||
}
|
||||
|
||||
Submode_t submodeToCommand = tableIter.value->getSubmode();
|
||||
if (tableIter.value->inheritSubmode()) {
|
||||
submodeToCommand = targetSubmode;
|
||||
}
|
||||
|
||||
if (healthHelper.healthTable->hasHealth(object)) {
|
||||
if (healthHelper.healthTable->isFaulty(object)) {
|
||||
ModeMessage::setModeMessage(&command,
|
||||
ModeMessage::CMD_MODE_COMMAND, HasModesIF::MODE_OFF,
|
||||
SUBMODE_NONE);
|
||||
} else {
|
||||
if (modeHelper.isForced()) {
|
||||
ModeMessage::setModeMessage(&command,
|
||||
ModeMessage::CMD_MODE_COMMAND_FORCED,
|
||||
tableIter.value->getMode(), submodeToCommand);
|
||||
} else {
|
||||
if (healthHelper.healthTable->isCommandable(object)) {
|
||||
ModeMessage::setModeMessage(&command,
|
||||
ModeMessage::CMD_MODE_COMMAND,
|
||||
tableIter.value->getMode(), submodeToCommand);
|
||||
} else {
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
ModeMessage::setModeMessage(&command, ModeMessage::CMD_MODE_COMMAND,
|
||||
tableIter.value->getMode(), submodeToCommand);
|
||||
}
|
||||
|
||||
if ((iter->second.mode == ModeMessage::getMode(&command))
|
||||
&& (iter->second.submode == ModeMessage::getSubmode(&command))
|
||||
&& !modeHelper.isForced()) {
|
||||
continue; //don't send redundant mode commands (produces event spam), but still command if mode is forced to reach lower levels
|
||||
}
|
||||
ReturnValue_t result = commandQueue->sendMessage(
|
||||
iter->second.commandQueue, &command);
|
||||
if (result == RETURN_OK) {
|
||||
++commandsOutstanding;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
ReturnValue_t SubsystemBase::updateChildMode(MessageQueueId_t queue,
|
||||
Mode_t mode, Submode_t submode) {
|
||||
std::map<object_id_t, ChildInfo>::iterator iter;
|
||||
|
||||
for (iter = childrenMap.begin(); iter != childrenMap.end(); iter++) {
|
||||
if (iter->second.commandQueue == queue) {
|
||||
iter->second.mode = mode;
|
||||
iter->second.submode = submode;
|
||||
return RETURN_OK;
|
||||
}
|
||||
}
|
||||
return CHILD_NOT_FOUND;
|
||||
}
|
||||
|
||||
ReturnValue_t SubsystemBase::updateChildChangedHealth(MessageQueueId_t queue,
|
||||
bool changedHealth) {
|
||||
for (auto iter = childrenMap.begin(); iter != childrenMap.end(); iter++) {
|
||||
if (iter->second.commandQueue == queue) {
|
||||
iter->second.healthChanged = changedHealth;
|
||||
return RETURN_OK;
|
||||
}
|
||||
}
|
||||
return CHILD_NOT_FOUND;
|
||||
}
|
||||
|
||||
MessageQueueId_t SubsystemBase::getCommandQueue() const {
|
||||
return commandQueue->getId();
|
||||
}
|
||||
|
||||
ReturnValue_t SubsystemBase::initialize() {
|
||||
MessageQueueId_t parentQueue = MessageQueueIF::NO_QUEUE;
|
||||
ReturnValue_t result = SystemObject::initialize();
|
||||
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
|
||||
if (parentId != objects::NO_OBJECT) {
|
||||
SubsystemBase *parent = ObjectManager::instance()->get<SubsystemBase>(parentId);
|
||||
if (parent == nullptr) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
parentQueue = parent->getCommandQueue();
|
||||
|
||||
parent->registerChild(getObjectId());
|
||||
}
|
||||
|
||||
result = healthHelper.initialize(parentQueue);
|
||||
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
|
||||
result = modeHelper.initialize(parentQueue);
|
||||
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t SubsystemBase::performOperation(uint8_t opCode) {
|
||||
|
||||
childrenChangedMode = false;
|
||||
|
||||
checkCommandQueue();
|
||||
|
||||
performChildOperation();
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t SubsystemBase::handleModeReply(CommandMessage* message) {
|
||||
switch (message->getCommand()) {
|
||||
case ModeMessage::REPLY_MODE_INFO:
|
||||
updateChildMode(message->getSender(), ModeMessage::getMode(message),
|
||||
ModeMessage::getSubmode(message));
|
||||
childrenChangedMode = true;
|
||||
return RETURN_OK;
|
||||
case ModeMessage::REPLY_MODE_REPLY:
|
||||
case ModeMessage::REPLY_WRONG_MODE_REPLY:
|
||||
updateChildMode(message->getSender(), ModeMessage::getMode(message),
|
||||
ModeMessage::getSubmode(message));
|
||||
childrenChangedMode = true;
|
||||
commandsOutstanding--;
|
||||
return RETURN_OK;
|
||||
case ModeMessage::REPLY_CANT_REACH_MODE:
|
||||
commandsOutstanding--;
|
||||
{
|
||||
for (auto iter = childrenMap.begin(); iter != childrenMap.end();
|
||||
iter++) {
|
||||
if (iter->second.commandQueue == message->getSender()) {
|
||||
triggerEvent(MODE_CMD_REJECTED, iter->first,
|
||||
message->getParameter());
|
||||
}
|
||||
}
|
||||
}
|
||||
return RETURN_OK;
|
||||
// case ModeMessage::CMD_MODE_COMMAND:
|
||||
// handleCommandedMode(message);
|
||||
// return RETURN_OK;
|
||||
// case ModeMessage::CMD_MODE_ANNOUNCE:
|
||||
// triggerEvent(MODE_INFO, mode, submode);
|
||||
// return RETURN_OK;
|
||||
// case ModeMessage::CMD_MODE_ANNOUNCE_RECURSIVELY:
|
||||
// triggerEvent(MODE_INFO, mode, submode);
|
||||
// commandAllChildren(message);
|
||||
// return RETURN_OK;
|
||||
default:
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t SubsystemBase::checkTable(
|
||||
HybridIterator<ModeListEntry> tableIter) {
|
||||
for (; tableIter.value != NULL; ++tableIter) {
|
||||
if (childrenMap.find(tableIter.value->getObject())
|
||||
== childrenMap.end()) {
|
||||
return TABLE_CONTAINS_INVALID_OBJECT_ID;
|
||||
}
|
||||
}
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
void SubsystemBase::replyToCommand(CommandMessage* message) {
|
||||
commandQueue->reply(message);
|
||||
}
|
||||
|
||||
void SubsystemBase::setMode(Mode_t newMode, Submode_t newSubmode) {
|
||||
modeHelper.modeChanged(newMode, newSubmode);
|
||||
mode = newMode;
|
||||
submode = newSubmode;
|
||||
modeChanged();
|
||||
announceMode(false);
|
||||
}
|
||||
|
||||
void SubsystemBase::setMode(Mode_t newMode) {
|
||||
setMode(newMode, submode);
|
||||
}
|
||||
|
||||
void SubsystemBase::commandAllChildren(CommandMessage* message) {
|
||||
std::map<object_id_t, ChildInfo>::iterator iter;
|
||||
for (iter = childrenMap.begin(); iter != childrenMap.end(); ++iter) {
|
||||
commandQueue->sendMessage(iter->second.commandQueue, message);
|
||||
}
|
||||
}
|
||||
|
||||
void SubsystemBase::getMode(Mode_t* mode, Submode_t* submode) {
|
||||
*mode = this->mode;
|
||||
*submode = this->submode;
|
||||
}
|
||||
|
||||
void SubsystemBase::setToExternalControl() {
|
||||
healthHelper.setHealth(EXTERNAL_CONTROL);
|
||||
}
|
||||
|
||||
void SubsystemBase::announceMode(bool recursive) {
|
||||
triggerEvent(MODE_INFO, mode, submode);
|
||||
if (recursive) {
|
||||
CommandMessage command;
|
||||
ModeMessage::setModeMessage(&command,
|
||||
ModeMessage::CMD_MODE_ANNOUNCE_RECURSIVELY, 0, 0);
|
||||
commandAllChildren(&command);
|
||||
}
|
||||
}
|
||||
|
||||
void SubsystemBase::checkCommandQueue() {
|
||||
ReturnValue_t result;
|
||||
CommandMessage command;
|
||||
|
||||
for (result = commandQueue->receiveMessage(&command); result == RETURN_OK;
|
||||
result = commandQueue->receiveMessage(&command)) {
|
||||
|
||||
result = healthHelper.handleHealthCommand(&command);
|
||||
if (result == RETURN_OK) {
|
||||
continue;
|
||||
}
|
||||
|
||||
result = modeHelper.handleModeCommand(&command);
|
||||
if (result == RETURN_OK) {
|
||||
continue;
|
||||
}
|
||||
|
||||
result = handleModeReply(&command);
|
||||
if (result == RETURN_OK) {
|
||||
continue;
|
||||
}
|
||||
|
||||
result = handleCommandMessage(&command);
|
||||
if (result != RETURN_OK) {
|
||||
CommandMessage reply;
|
||||
reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND,
|
||||
command.getCommand());
|
||||
replyToCommand(&reply);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t SubsystemBase::setHealth(HealthState health) {
|
||||
switch (health) {
|
||||
case HEALTHY:
|
||||
case EXTERNAL_CONTROL:
|
||||
healthHelper.setHealth(health);
|
||||
return RETURN_OK;
|
||||
default:
|
||||
return INVALID_HEALTH_STATE;
|
||||
}
|
||||
}
|
||||
|
||||
HasHealthIF::HealthState SubsystemBase::getHealth() {
|
||||
return healthHelper.getHealth();
|
||||
}
|
||||
|
||||
void SubsystemBase::modeChanged() {
|
||||
}
|
||||
|
||||
5
src/core/subsystem/modes/CMakeLists.txt
Normal file
5
src/core/subsystem/modes/CMakeLists.txt
Normal file
@ -0,0 +1,5 @@
|
||||
target_sources(${LIB_FSFW_NAME}
|
||||
PRIVATE
|
||||
ModeSequenceMessage.cpp
|
||||
ModeStore.cpp
|
||||
)
|
||||
76
src/core/subsystem/modes/ModeSequenceMessage.cpp
Normal file
76
src/core/subsystem/modes/ModeSequenceMessage.cpp
Normal file
@ -0,0 +1,76 @@
|
||||
#include "ModeSequenceMessage.h"
|
||||
|
||||
#include "../../objectmanager/ObjectManager.h"
|
||||
#include "../../storagemanager/StorageManagerIF.h"
|
||||
|
||||
void ModeSequenceMessage::setModeSequenceMessage(CommandMessage* message,
|
||||
Command_t command, Mode_t sequence, store_address_t storeAddress) {
|
||||
message->setCommand(command);
|
||||
message->setParameter(storeAddress.raw);
|
||||
message->setParameter2(sequence);
|
||||
}
|
||||
|
||||
void ModeSequenceMessage::setModeSequenceMessage(CommandMessage* message,
|
||||
Command_t command, Mode_t sequence) {
|
||||
message->setCommand(command);
|
||||
message->setParameter2(sequence);
|
||||
}
|
||||
|
||||
void ModeSequenceMessage::setModeSequenceMessage(CommandMessage* message,
|
||||
Command_t command, store_address_t storeAddress) {
|
||||
message->setCommand(command);
|
||||
message->setParameter(storeAddress.raw);
|
||||
}
|
||||
|
||||
store_address_t ModeSequenceMessage::getStoreAddress(
|
||||
const CommandMessage* message) {
|
||||
store_address_t address;
|
||||
address.raw = message->getParameter();
|
||||
return address;
|
||||
}
|
||||
|
||||
Mode_t ModeSequenceMessage::getSequenceId(const CommandMessage* message) {
|
||||
return message->getParameter2();
|
||||
}
|
||||
|
||||
Mode_t ModeSequenceMessage::getTableId(const CommandMessage* message) {
|
||||
return message->getParameter2();
|
||||
}
|
||||
|
||||
|
||||
uint32_t ModeSequenceMessage::getNumber(const CommandMessage* message) {
|
||||
return message->getParameter2();
|
||||
}
|
||||
|
||||
void ModeSequenceMessage::clear(CommandMessage *message) {
|
||||
switch (message->getCommand()) {
|
||||
case ADD_SEQUENCE:
|
||||
case ADD_TABLE:
|
||||
case SEQUENCE_LIST:
|
||||
case TABLE_LIST:
|
||||
case TABLE:
|
||||
case SEQUENCE: {
|
||||
StorageManagerIF *ipcStore = ObjectManager::instance()->get<StorageManagerIF>(
|
||||
objects::IPC_STORE);
|
||||
if (ipcStore != nullptr){
|
||||
ipcStore->deleteData(ModeSequenceMessage::getStoreAddress(message));
|
||||
}
|
||||
}
|
||||
/* NO BREAK falls through*/
|
||||
case DELETE_SEQUENCE:
|
||||
case DELETE_TABLE:
|
||||
case READ_SEQUENCE:
|
||||
case READ_TABLE:
|
||||
case LIST_SEQUENCES:
|
||||
case LIST_TABLES:
|
||||
case READ_FREE_SEQUENCE_SLOTS:
|
||||
case FREE_SEQUENCE_SLOTS:
|
||||
case READ_FREE_TABLE_SLOTS:
|
||||
case FREE_TABLE_SLOTS:
|
||||
default:
|
||||
message->setCommand(CommandMessage::CMD_NONE);
|
||||
message->setParameter(0);
|
||||
message->setParameter2(0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
128
src/core/subsystem/modes/ModeStore.cpp
Normal file
128
src/core/subsystem/modes/ModeStore.cpp
Normal file
@ -0,0 +1,128 @@
|
||||
#include "ModeStore.h"
|
||||
|
||||
// todo: I think some parts are deprecated. If this is used, the define
|
||||
// USE_MODESTORE could be part of the new FSFWConfig.h file.
|
||||
#if FSFW_USE_MODESTORE == 1
|
||||
|
||||
ModeStore::ModeStore(object_id_t objectId, uint32_t slots) :
|
||||
SystemObject(objectId), store(slots), emptySlot(store.front()) {
|
||||
mutex = MutexFactory::instance()->createMutex();;
|
||||
OSAL::createMutex(objectId + 1, mutex);
|
||||
clear();
|
||||
}
|
||||
|
||||
ModeStore::~ModeStore() {
|
||||
delete mutex;
|
||||
}
|
||||
|
||||
uint32_t ModeStore::getFreeSlots() {
|
||||
OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT);
|
||||
uint32_t count = 0;
|
||||
ArrayList<ModeListEntry, uint32_t>::Iterator iter;
|
||||
for (iter = store.begin(); iter != store.end(); ++iter) {
|
||||
if (iter->getNext() == emptySlot) {
|
||||
++count;
|
||||
}
|
||||
}
|
||||
OSAL::unlockMutex(mutex);
|
||||
return count;
|
||||
}
|
||||
|
||||
ReturnValue_t ModeStore::storeArray(ArrayList<ModeListEntry>* sequence,
|
||||
ModeListEntry** storedFirstEntry) {
|
||||
if (sequence->size == 0) {
|
||||
return CANT_STORE_EMPTY;
|
||||
}
|
||||
OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT);
|
||||
*storedFirstEntry = findEmptySlotNoLock(store.front());
|
||||
|
||||
ModeListEntry* pointer =
|
||||
*storedFirstEntry;
|
||||
pointer->setNext(pointer);
|
||||
|
||||
ArrayList<ModeListEntry>::Iterator iter;
|
||||
for (iter = sequence->begin(); iter != sequence->end(); ++iter) {
|
||||
//SHOULDDO: I need to check this in detail. What is the idea? Why does it not work?
|
||||
pointer = pointer->getNext()->value;
|
||||
if (pointer == NULL) {
|
||||
deleteListNoLock(*storedFirstEntry);
|
||||
OSAL::unlockMutex(mutex);
|
||||
return TOO_MANY_ELEMENTS;
|
||||
}
|
||||
pointer->value->value1 = iter->value1;
|
||||
pointer->value->value2 = iter->value2;
|
||||
pointer->value->value3 = iter->value3;
|
||||
pointer->setNext(findEmptySlotNoLock(pointer + 1));
|
||||
}
|
||||
pointer->setNext(NULL);
|
||||
OSAL::unlockMutex(mutex);
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t ModeStore::deleteList(ModeListEntry* sequence) {
|
||||
ReturnValue_t result = isValidEntry(sequence);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT);
|
||||
deleteListNoLock(sequence);
|
||||
OSAL::unlockMutex(mutex);
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t ModeStore::readList(ModeListEntry* sequence,
|
||||
ArrayList<ModeListEntry>* into) {
|
||||
ReturnValue_t result = isValidEntry(sequence);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT);
|
||||
result = into->insert(*sequence->value);
|
||||
while ((result == HasReturnvaluesIF::RETURN_OK) && (sequence->getNext() != NULL)) {
|
||||
result = into->insert(*sequence->value);
|
||||
sequence = sequence->getNext()->value;
|
||||
}
|
||||
OSAL::unlockMutex(mutex);
|
||||
return result;
|
||||
}
|
||||
|
||||
void ModeStore::clear() {
|
||||
OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT);
|
||||
store.size = store.maxSize();
|
||||
ArrayList<ModeListEntry, uint32_t>::Iterator iter;
|
||||
for (iter = store.begin(); iter != store.end(); ++iter) {
|
||||
iter->setNext(emptySlot);
|
||||
}
|
||||
OSAL::unlockMutex(mutex);
|
||||
}
|
||||
|
||||
ModeListEntry* ModeStore::findEmptySlotNoLock(ModeListEntry* startFrom) {
|
||||
ArrayList<ModeListEntry, uint32_t>::Iterator iter(
|
||||
startFrom);
|
||||
for (; iter != store.end(); ++iter) {
|
||||
if (iter.value->getNext() == emptySlot) {
|
||||
OSAL::unlockMutex(mutex);
|
||||
return iter.value;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void ModeStore::deleteListNoLock(ModeListEntry* sequence) {
|
||||
ModeListEntry* next = sequence;
|
||||
while (next != NULL) {
|
||||
next = sequence->getNext()->value;
|
||||
sequence->setNext(emptySlot);
|
||||
sequence = next;
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t ModeStore::isValidEntry(ModeListEntry* sequence) {
|
||||
if ((sequence < store.front()) || (sequence > store.back())
|
||||
|| sequence->getNext() == emptySlot) {
|
||||
return INVALID_ENTRY;
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user