diff --git a/THANKYOU b/THANKYOU index 0cdbb458..9df00689 100644 --- a/THANKYOU +++ b/THANKYOU @@ -1,4 +1,5 @@ -Class Ziemke, together with Bastian Bätz, started work on the code which would later become the Flight Software Framework in late 2009 as a student at the Institut für Raumfahrtsysteme at Universität Stuttgart. +Besides Bastian Bätz and Ulrich Mohr, who were the main developers for Flying Laptop's Onboard Software, the following PhD Students contributed to the project: -"A dream would come true if the software framework I developed would be released in the public domain so that it can be used by other projects and to be used for teaching students how to program on-board software in the course of the studies at the IRS" - - Claas in his Diploma Thesis +Rouven Witt, who developed the FDIR concept and kept morale high as the team's Spaßbeauftragter. +Marek Dittmar, who started work on the ACS code and later tried to keep the development in time. +Nico Bucher, who performed software tests and as such was invaluable during the development. diff --git a/action/ActionHelper.cpp b/action/ActionHelper.cpp new file mode 100644 index 00000000..c85f8d01 --- /dev/null +++ b/action/ActionHelper.cpp @@ -0,0 +1,92 @@ + +#include +#include +#include +ActionHelper::ActionHelper(HasActionsIF* setOwner, MessageQueue* useThisQueue) : + owner(setOwner), queueToUse(useThisQueue), ipcStore( + NULL) { +} + +ActionHelper::~ActionHelper() { +} + +ReturnValue_t ActionHelper::handleActionMessage(CommandMessage* command) { + if (command->getCommand() == ActionMessage::EXECUTE_ACTION) { + ActionId_t currentAction = ActionMessage::getActionId(command); + prepareExecution(command->getSender(), currentAction, + ActionMessage::getStoreId(command)); + return HasReturnvaluesIF::RETURN_OK; + } else { + return CommandMessage::UNKNOW_COMMAND; + } +} + +ReturnValue_t ActionHelper::initialize() { + ipcStore = objectManager->get(objects::IPC_STORE); + if (ipcStore == NULL) { + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; +} + +void ActionHelper::step(uint8_t step, MessageQueueId_t reportTo, ActionId_t commandId, ReturnValue_t result) { + CommandMessage reply; + ActionMessage::setStepReply(&reply, commandId, step + STEP_OFFSET, result); + queueToUse->sendMessage(reportTo, &reply); +} + +void ActionHelper::finish(MessageQueueId_t reportTo, ActionId_t commandId, ReturnValue_t result) { + CommandMessage reply; + ActionMessage::setCompletionReply(&reply, commandId, result); + queueToUse->sendMessage(reportTo, &reply); +} + +void ActionHelper::prepareExecution(MessageQueueId_t commandedBy, ActionId_t actionId, + store_address_t dataAddress) { + const uint8_t* dataPtr = NULL; + uint32_t size = 0; + ReturnValue_t result = ipcStore->getData(dataAddress, &dataPtr, &size); + if (result != HasReturnvaluesIF::RETURN_OK) { + CommandMessage reply; + ActionMessage::setStepReply(&reply, actionId, 0, result); + queueToUse->sendMessage(commandedBy, &reply); + return; + } + result = owner->executeAction(actionId, commandedBy, dataPtr, size); + ipcStore->deleteData(dataAddress); + if (result != HasReturnvaluesIF::RETURN_OK) { + CommandMessage reply; + ActionMessage::setStepReply(&reply, actionId, 0, result); + queueToUse->sendMessage(commandedBy, &reply); + return; + } +} + +void ActionHelper::reportData(MessageQueueId_t reportTo, ActionId_t replyId, SerializeIF* data) { + CommandMessage reply; + store_address_t storeAddress; + uint8_t *dataPtr; + uint32_t maxSize = data->getSerializedSize(); + if (maxSize == 0) { + return; + } + uint32_t size = 0; + ReturnValue_t result = ipcStore->getFreeElement(&storeAddress, maxSize, + &dataPtr); + if (result != HasReturnvaluesIF::RETURN_OK) { + //TODO event? + return; + } + result = data->serialize(&dataPtr, &size, maxSize, true); + if (result != HasReturnvaluesIF::RETURN_OK) { + ipcStore->deleteData(storeAddress); + //TODO event? + return; + } + ActionMessage::setDataReply(&reply, replyId, storeAddress); + if (queueToUse->sendMessage(reportTo, &reply) != HasReturnvaluesIF::RETURN_OK){ + ipcStore->deleteData(storeAddress); + } + //We don't neeed the objectId, as we receive REQUESTED data before the completion success message. + //True aperiodic replies need to be reported with dedicated DH message. +} diff --git a/action/ActionHelper.h b/action/ActionHelper.h new file mode 100644 index 00000000..c6965414 --- /dev/null +++ b/action/ActionHelper.h @@ -0,0 +1,28 @@ + +#ifndef ACTIONHELPER_H_ +#define ACTIONHELPER_H_ + +#include +#include + +class HasActionsIF; +//TODO: Change MessageQueueId usage. +class ActionHelper { +public: + ActionHelper(HasActionsIF* setOwner, MessageQueue* useThisQueue); + virtual ~ActionHelper(); + ReturnValue_t handleActionMessage(CommandMessage* command); + ReturnValue_t initialize(); + void step(uint8_t step, MessageQueueId_t reportTo, ActionId_t commandId, ReturnValue_t result = HasReturnvaluesIF::RETURN_OK); + void finish(MessageQueueId_t reportTo, ActionId_t commandId, ReturnValue_t result = HasReturnvaluesIF::RETURN_OK); + void reportData(MessageQueueId_t reportTo, ActionId_t replyId, SerializeIF* data); +protected: + static const uint8_t STEP_OFFSET = 1; + HasActionsIF* owner; + MessageQueue* queueToUse; + StorageManagerIF* ipcStore; + virtual void prepareExecution(MessageQueueId_t commandedBy, ActionId_t actionId, store_address_t dataAddress); + void resetHelper(); +}; + +#endif /* ACTIONHELPER_H_ */ diff --git a/action/ActionMessage.cpp b/action/ActionMessage.cpp new file mode 100644 index 00000000..f9af5e9b --- /dev/null +++ b/action/ActionMessage.cpp @@ -0,0 +1,80 @@ + +#include +#include +#include + +ActionMessage::ActionMessage() { +} + +ActionMessage::~ActionMessage() { +} + +void ActionMessage::setCommand(CommandMessage* message, ActionId_t fid, + store_address_t parameters) { + message->setCommand(EXECUTE_ACTION); + message->setParameter(fid); + message->setParameter2(parameters.raw); +} + +ActionId_t ActionMessage::getActionId(const CommandMessage* message) { + return ActionId_t(message->getParameter()); +} + +store_address_t ActionMessage::getStoreId(const CommandMessage* message) { + store_address_t temp; + temp.raw = message->getParameter2(); + return temp; +} + +void ActionMessage::setStepReply(CommandMessage* message, ActionId_t fid, uint8_t step, + ReturnValue_t result) { + if (result == HasReturnvaluesIF::RETURN_OK) { + message->setCommand(STEP_SUCCESS); + } else { + message->setCommand(STEP_FAILED); + } + message->setParameter(fid); + message->setParameter2((step << 16) + result); +} + +uint8_t ActionMessage::getStep(const CommandMessage* message) { + return uint8_t((message->getParameter2() >> 16) & 0xFF); +} + +ReturnValue_t ActionMessage::getReturnCode(const CommandMessage* message) { + return message->getParameter2() & 0xFFFF; +} + +void ActionMessage::setDataReply(CommandMessage* message, ActionId_t actionId, + store_address_t data) { + message->setCommand(DATA_REPLY); + message->setParameter(actionId); + message->setParameter2(data.raw); +} + +void ActionMessage::setCompletionReply(CommandMessage* message, + ActionId_t fid, ReturnValue_t result) { + if (result == HasReturnvaluesIF::RETURN_OK) { + message->setCommand(COMPLETION_SUCCESS); + } else { + message->setCommand(COMPLETION_FAILED); + } + message->setParameter(fid); + message->setParameter2(result); +} + +void ActionMessage::clear(CommandMessage* message) { + switch(message->getCommand()) { + case EXECUTE_ACTION: + case DATA_REPLY: { + StorageManagerIF *ipcStore = objectManager->get( + objects::IPC_STORE); + if (ipcStore != NULL) { + ipcStore->deleteData(getStoreId(message)); + } + break; + } + default: + break; + } +} diff --git a/action/ActionMessage.h b/action/ActionMessage.h new file mode 100644 index 00000000..5afcff76 --- /dev/null +++ b/action/ActionMessage.h @@ -0,0 +1,33 @@ + +#ifndef ACTIONMESSAGE_H_ +#define ACTIONMESSAGE_H_ + +#include +#include +#include +typedef uint32_t ActionId_t; + +class ActionMessage { +private: + ActionMessage(); +public: + static const uint8_t MESSAGE_ID = FUNCTION_MESSAGE_ID; + static const Command_t EXECUTE_ACTION = MAKE_COMMAND_ID(1); + static const Command_t STEP_SUCCESS = MAKE_COMMAND_ID(2); + static const Command_t STEP_FAILED = MAKE_COMMAND_ID(3); + static const Command_t DATA_REPLY = MAKE_COMMAND_ID(4); + static const Command_t COMPLETION_SUCCESS = MAKE_COMMAND_ID(5); + static const Command_t COMPLETION_FAILED = MAKE_COMMAND_ID(6); + virtual ~ActionMessage(); + static void setCommand(CommandMessage* message, ActionId_t fid, store_address_t parameters); + static ActionId_t getActionId(const CommandMessage* message ); + static store_address_t getStoreId(const CommandMessage* message ); + static void setStepReply(CommandMessage* message, ActionId_t fid, uint8_t step, ReturnValue_t result = HasReturnvaluesIF::RETURN_OK); + static uint8_t getStep(const CommandMessage* message ); + static ReturnValue_t getReturnCode(const CommandMessage* message ); + static void setDataReply(CommandMessage* message, ActionId_t actionId, store_address_t data); + static void setCompletionReply(CommandMessage* message, ActionId_t fid, ReturnValue_t result = HasReturnvaluesIF::RETURN_OK); + static void clear(CommandMessage* message); +}; + +#endif /* ACTIONMESSAGE_H_ */ diff --git a/action/CommandActionHelper.cpp b/action/CommandActionHelper.cpp new file mode 100644 index 00000000..ebd5fb88 --- /dev/null +++ b/action/CommandActionHelper.cpp @@ -0,0 +1,120 @@ + +#include +#include +#include +#include +#include + +CommandActionHelper::CommandActionHelper(CommandsActionsIF* setOwner) : + owner(setOwner), queueToUse(setOwner->getCommandQueuePtr()), ipcStore( + NULL), commandCount(0), lastTarget(0) { +} + +CommandActionHelper::~CommandActionHelper() { +} + +ReturnValue_t CommandActionHelper::commandAction(object_id_t commandTo, + ActionId_t actionId, SerializeIF* data) { + HasActionsIF* receiver = objectManager->get(commandTo); + if (receiver == NULL) { + return CommandsActionsIF::OBJECT_HAS_NO_FUNCTIONS; + } + store_address_t storeId; + uint8_t* storePointer; + uint32_t maxSize = data->getSerializedSize(); + ReturnValue_t result = ipcStore->getFreeElement(&storeId, maxSize, + &storePointer); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + uint32_t size = 0; + result = data->serialize(&storePointer, &size, maxSize, true); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return sendCommand(receiver->getCommandQueue(), actionId, storeId); +} + +ReturnValue_t CommandActionHelper::commandAction(object_id_t commandTo, + ActionId_t actionId, const uint8_t* data, uint32_t size) { +// if (commandCount != 0) { +// return CommandsFunctionsIF::ALREADY_COMMANDING; +// } + HasActionsIF* receiver = objectManager->get(commandTo); + if (receiver == NULL) { + return CommandsActionsIF::OBJECT_HAS_NO_FUNCTIONS; + } + store_address_t storeId; + ReturnValue_t result = ipcStore->addData(&storeId, data, size); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return sendCommand(receiver->getCommandQueue(), actionId, storeId); +} + +ReturnValue_t CommandActionHelper::sendCommand(MessageQueueId_t queueId, + ActionId_t actionId, store_address_t storeId) { + CommandMessage command; + ActionMessage::setCommand(&command, actionId, storeId); + ReturnValue_t result = queueToUse->sendMessage(queueId, &command); + if (result != HasReturnvaluesIF::RETURN_OK) { + ipcStore->deleteData(storeId); + } + lastTarget = queueId; + commandCount++; + return result; +} + +ReturnValue_t CommandActionHelper::initialize() { + ipcStore = objectManager->get(objects::IPC_STORE); + if (ipcStore != NULL) { + return HasReturnvaluesIF::RETURN_OK; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +ReturnValue_t CommandActionHelper::handleReply(CommandMessage* reply) { + if (reply->getSender() != lastTarget) { + return HasReturnvaluesIF::RETURN_FAILED; + } + switch (reply->getCommand()) { + case ActionMessage::COMPLETION_SUCCESS: + commandCount--; + owner->completionSuccessfulReceived(ActionMessage::getActionId(reply)); + return HasReturnvaluesIF::RETURN_OK; + case ActionMessage::COMPLETION_FAILED: + commandCount--; + owner->completionFailedReceived(ActionMessage::getActionId(reply), ActionMessage::getReturnCode(reply)); + return HasReturnvaluesIF::RETURN_OK; + case ActionMessage::STEP_SUCCESS: + owner->stepSuccessfulReceived(ActionMessage::getActionId(reply), + ActionMessage::getStep(reply)); + return HasReturnvaluesIF::RETURN_OK; + case ActionMessage::STEP_FAILED: + commandCount--; + owner->stepFailedReceived(ActionMessage::getActionId(reply), ActionMessage::getStep(reply), + ActionMessage::getReturnCode(reply)); + return HasReturnvaluesIF::RETURN_OK; + case ActionMessage::DATA_REPLY: + extractDataForOwner(ActionMessage::getActionId(reply), ActionMessage::getStoreId(reply)); + return HasReturnvaluesIF::RETURN_OK; + default: + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +uint8_t CommandActionHelper::getCommandCount() const { + return commandCount; +} + +void CommandActionHelper::extractDataForOwner(ActionId_t actionId, store_address_t storeId) { + const uint8_t * data = NULL; + uint32_t size = 0; + ReturnValue_t result = ipcStore->getData(storeId, &data, &size); + if (result != HasReturnvaluesIF::RETURN_OK) { + return; + } + owner->dataReceived(actionId, data, size); + ipcStore->deleteData(storeId); +} diff --git a/action/CommandActionHelper.h b/action/CommandActionHelper.h new file mode 100644 index 00000000..f3f574d3 --- /dev/null +++ b/action/CommandActionHelper.h @@ -0,0 +1,37 @@ + +#ifndef COMMANDACTIONHELPER_H_ +#define COMMANDACTIONHELPER_H_ + +#include +#include +#include +#include +#include +#include + +class CommandsActionsIF; + +class CommandActionHelper { + friend class CommandsActionsIF; +public: + CommandActionHelper(CommandsActionsIF* owner); + virtual ~CommandActionHelper(); + ReturnValue_t commandAction(object_id_t commandTo, + ActionId_t actionId, const uint8_t* data, uint32_t size); + ReturnValue_t commandAction(object_id_t commandTo, + ActionId_t actionId, SerializeIF* data); + ReturnValue_t initialize(); + ReturnValue_t handleReply(CommandMessage* reply); + uint8_t getCommandCount() const; +private: + CommandsActionsIF* owner; + MessageQueue* queueToUse; + StorageManagerIF* ipcStore; + uint8_t commandCount; + MessageQueueId_t lastTarget; + void extractDataForOwner(ActionId_t actionId, store_address_t storeId); + ReturnValue_t sendCommand(MessageQueueId_t queueId, ActionId_t actionId, + store_address_t storeId); +}; + +#endif /* COMMANDACTIONHELPER_H_ */ diff --git a/action/CommandsActionsIF.h b/action/CommandsActionsIF.h new file mode 100644 index 00000000..633876a9 --- /dev/null +++ b/action/CommandsActionsIF.h @@ -0,0 +1,35 @@ + +#ifndef COMMANDSACTIONSIF_H_ +#define COMMANDSACTIONSIF_H_ + +#include +#include +#include + +/** + * Interface to separate commanding actions of other objects. + * In next iteration, IF should be shortened to three calls: + * - dataReceived(data) + * - successReceived(id, step) + * - failureReceived(id, step, cause) + * or even + * - replyReceived(id, step, cause) (if cause == OK, it's a success). + */ +class CommandsActionsIF { + friend class CommandActionHelper; +public: + static const uint8_t INTERFACE_ID = COMMANDS_ACTIONS_IF; + static const ReturnValue_t OBJECT_HAS_NO_FUNCTIONS = MAKE_RETURN_CODE(1); + static const ReturnValue_t ALREADY_COMMANDING = MAKE_RETURN_CODE(2); + virtual ~CommandsActionsIF() {} + virtual MessageQueue* getCommandQueuePtr() = 0; +protected: + virtual void stepSuccessfulReceived(ActionId_t actionId, uint8_t step) = 0; + virtual void stepFailedReceived(ActionId_t actionId, uint8_t step, ReturnValue_t returnCode) = 0; + virtual void dataReceived(ActionId_t actionId, const uint8_t* data, uint32_t size) = 0; + virtual void completionSuccessfulReceived(ActionId_t actionId) = 0; + virtual void completionFailedReceived(ActionId_t actionId, ReturnValue_t returnCode) = 0; +}; + + +#endif /* COMMANDSACTIONSIF_H_ */ diff --git a/action/HasActionsIF.h b/action/HasActionsIF.h new file mode 100644 index 00000000..cb62c522 --- /dev/null +++ b/action/HasActionsIF.h @@ -0,0 +1,35 @@ +/* + * HasActionsIF.h + * + * Created on: 20.02.2014 + * Author: baetz + */ + +#ifndef HASACTIONSIF_H_ +#define HASACTIONSIF_H_ + +#include +#include +#include +#include +#include +class HasActionsIF { +public: + static const uint8_t INTERFACE_ID = HAS_ACTIONS_IF; + static const ReturnValue_t IS_BUSY = MAKE_RETURN_CODE(1); + static const ReturnValue_t INVALID_PARAMETERS = MAKE_RETURN_CODE(2); + static const ReturnValue_t EXECUTION_FINISHED = MAKE_RETURN_CODE(3); + static const ReturnValue_t INVALID_ACTION_ID = MAKE_RETURN_CODE(4); + virtual ~HasActionsIF() { } + virtual MessageQueueId_t getCommandQueue() const = 0; + /** + * Execute or initialize the execution of a certain function. + * Returning #EXECUTION_FINISHED or a failure code, nothing else needs to be done. + * When needing more steps, return RETURN_OK and issue steps and completion manually. One "step failed" or completion report must + * be issued! + */ + virtual ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, uint32_t size) = 0; +}; + + +#endif /* HASACTIONSIF_H_ */ diff --git a/action/SimpleActionHelper.cpp b/action/SimpleActionHelper.cpp new file mode 100644 index 00000000..cb2bac6d --- /dev/null +++ b/action/SimpleActionHelper.cpp @@ -0,0 +1,75 @@ + +#include +#include +SimpleActionHelper::SimpleActionHelper(HasActionsIF* setOwner, + MessageQueue* useThisQueue) : + ActionHelper(setOwner, useThisQueue), isExecuting(false), lastCommander( + 0), lastAction(0), stepCount(0) { +} + +SimpleActionHelper::~SimpleActionHelper() { +} + +void SimpleActionHelper::step(ReturnValue_t result) { + //STEP_OFFESET is subtracted to compensate for adding offset in base method, which is not necessary here. + ActionHelper::step(stepCount - STEP_OFFSET, lastCommander, lastAction, + result); + if (result != HasReturnvaluesIF::RETURN_OK) { + resetHelper(); + } +} + +void SimpleActionHelper::finish(ReturnValue_t result) { + ActionHelper::finish(lastCommander, lastAction, result); + resetHelper(); +} + +void SimpleActionHelper::reportData(SerializeIF* data) { + ActionHelper::reportData(lastCommander, lastAction, data); +} + +void SimpleActionHelper::resetHelper() { + stepCount = 0; + isExecuting = false; + lastAction = 0; + lastCommander = 0; +} + +void SimpleActionHelper::prepareExecution(MessageQueueId_t commandedBy, + ActionId_t actionId, store_address_t dataAddress) { + CommandMessage reply; + if (isExecuting) { + ipcStore->deleteData(dataAddress); + ActionMessage::setStepReply(&reply, actionId, 0, + HasActionsIF::IS_BUSY); + queueToUse->sendMessage(commandedBy, &reply); + } + const uint8_t* dataPtr = NULL; + uint32_t size = 0; + ReturnValue_t result = ipcStore->getData(dataAddress, &dataPtr, &size); + if (result != HasReturnvaluesIF::RETURN_OK) { + ActionMessage::setStepReply(&reply, actionId, 0, result); + queueToUse->sendMessage(commandedBy, &reply); + return; + } + lastCommander = commandedBy; + lastAction = actionId; + result = owner->executeAction(actionId, commandedBy, dataPtr, size); + ipcStore->deleteData(dataAddress); + switch (result) { + case HasReturnvaluesIF::RETURN_OK: + isExecuting = true; + stepCount++; + break; + case HasActionsIF::EXECUTION_FINISHED: + ActionMessage::setCompletionReply(&reply, actionId, + HasReturnvaluesIF::RETURN_OK); + queueToUse->sendMessage(commandedBy, &reply); + break; + default: + ActionMessage::setStepReply(&reply, actionId, 0, result); + queueToUse->sendMessage(commandedBy, &reply); + break; + } + +} diff --git a/action/SimpleActionHelper.h b/action/SimpleActionHelper.h new file mode 100644 index 00000000..8615c486 --- /dev/null +++ b/action/SimpleActionHelper.h @@ -0,0 +1,24 @@ + +#ifndef SIMPLEACTIONHELPER_H_ +#define SIMPLEACTIONHELPER_H_ + +#include + +class SimpleActionHelper: public ActionHelper { +public: + SimpleActionHelper(HasActionsIF* setOwner, MessageQueue* useThisQueue); + virtual ~SimpleActionHelper(); + void step(ReturnValue_t result = HasReturnvaluesIF::RETURN_OK); + void finish(ReturnValue_t result = HasReturnvaluesIF::RETURN_OK); + void reportData(SerializeIF* data); + void resetHelper(); +protected: + void prepareExecution(MessageQueueId_t commandedBy, ActionId_t actionId, store_address_t dataAddress); +private: + bool isExecuting; + MessageQueueId_t lastCommander; + ActionId_t lastAction; + uint8_t stepCount; +}; + +#endif /* SIMPLEACTIONHELPER_H_ */ diff --git a/container/ArrayList.h b/container/ArrayList.h new file mode 100644 index 00000000..75917b0a --- /dev/null +++ b/container/ArrayList.h @@ -0,0 +1,245 @@ +#ifndef ARRAYLIST_H_ +#define ARRAYLIST_H_ + +#include +#include +#include + +/** + * A List that stores its values in an array. + * + * The backend is an array that can be allocated by the class itself or supplied via ctor. + * + * + * @ingroup container + */ +template +class ArrayList { + template friend class SerialArrayListAdapter; +public: + static const uint8_t INTERFACE_ID = ARRAY_LIST; + static const ReturnValue_t FULL = MAKE_RETURN_CODE(0x01); + + /** + * An Iterator to go trough an ArrayList + * + * It stores a pointer to an element and increments the + * pointer when incremented itself. + */ + class Iterator { + public: + /** + * Empty ctor, points to NULL + */ + Iterator() : + value(0) { + + } + + /** + * Initializes the Iterator to point to an element + * + * @param initialize + */ + Iterator(T *initialize) { + value = initialize; + } + + /** + * The current element the iterator points to + */ + T *value; + + Iterator& operator++() { + value++; + return *this; + } + + Iterator operator++(int) { + Iterator tmp(*this); + operator++(); + return tmp; + } + + Iterator& operator--() { + value--; + return *this; + } + + Iterator operator--(int) { + Iterator tmp(*this); + operator--(); + return tmp; + } + + T operator*() { + return *value; + } + + T *operator->() { + return value; + } + + //SHOULDDO this should be implemented as non-member + bool operator==(const typename ArrayList::Iterator& other) { + return (value == other.value); + } + + //SHOULDDO this should be implemented as non-member + bool operator!=(const typename ArrayList::Iterator& other) { + return !(*this == other); + } + } + ; + + /** + * Number of Elements stored in this List + */ + count_t size; + + /** + * This is the allocating constructor; + * + * It allocates an array of the specified size. + * + * @param maxSize + */ + ArrayList(count_t maxSize) : + size(0), maxSize_(maxSize), allocated(true) { + entries = new T[maxSize]; + } + + /** + * This is the non-allocating constructor + * + * It expects a pointer to an array of a certain size and initializes itself to it. + * + * @param storage the array to use as backend + * @param maxSize size of storage + */ + ArrayList(T *storage, count_t maxSize) : + size(0), entries(storage), maxSize_(maxSize), allocated(false) { + } + + /** + * Destructor, if the allocating constructor was used, it deletes the array. + */ + virtual ~ArrayList() { + if (allocated) { + delete[] entries; + } + } + + /** + * Iterator pointing to the first stored elmement + * + * @return Iterator to the first element + */ + Iterator begin() const { + return Iterator(&entries[0]); + } + + /** + * returns an Iterator pointing to the element after the last stored entry + * + * @return Iterator to the element after the last entry + */ + Iterator end() const { + return Iterator(&entries[size]); + } + + T & operator[](count_t i) const { + return entries[i]; + } + + /** + * The first element + * + * @return pointer to the first stored element + */ + T *front() { + return entries; + } + + /** + * The last element + * + * does not return a valid pointer if called on an empty list. + * + * @return pointer to the last stored element + */ + T *back() { + return &entries[size - 1]; + } + + /** + * The maximum number of elements this List can contain + * + * @return maximum number of elements + */ + uint32_t maxSize() const { + return this->maxSize_; + } + + /** + * Insert a new element into the list. + * + * The new element is inserted after the last stored element. + * + * @param entry + * @return + * -@c FULL if the List is full + * -@c RETURN_OK else + */ + ReturnValue_t insert(T entry) { + if (size >= maxSize_) { + return FULL; + } + entries[size] = entry; + ++size; + return HasReturnvaluesIF::RETURN_OK; + } + + /** + * clear the List + * + * This does not actually clear all entries, it only sets the size to 0. + */ + void clear() { + size = 0; + } + + count_t remaining() { + return (maxSize_ - size); + } +protected: + /** + * pointer to the array in which the entries are stored + */ + T *entries; +private: + /** + * This is the copy constructor + * + * It is private, as copying is too ambigous in this case. (Allocate a new backend? Use the same? + * What to do in an modifying call?) + * + * @param other + */ + ArrayList(const ArrayList& other) : + size(other.size), entries(other.entries), maxSize_(other.maxSize_), allocated( + false) { + } +private: + /** + * remembering the maximum size + */ + uint32_t maxSize_; + + /** + * true if the array was allocated and needs to be deleted in the destructor. + */ + bool allocated; + +}; +#endif /* ARRAYLIST_H_ */ diff --git a/container/BinaryTree.h b/container/BinaryTree.h new file mode 100644 index 00000000..137c826a --- /dev/null +++ b/container/BinaryTree.h @@ -0,0 +1,163 @@ +/* + * BinaryTree.h + * + * Created on: 09.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_CONTAINER_BINARYTREE_H_ +#define FRAMEWORK_CONTAINER_BINARYTREE_H_ + +#include +#include +#include +template +class BinaryNode { +public: + BinaryNode(Tp* setValue) : + value(setValue), left(NULL), right(NULL), parent(NULL) { + } + Tp *value; + BinaryNode* left; + BinaryNode* right; + BinaryNode* parent; +}; + +template +class ExplicitNodeIterator { +public: + typedef ExplicitNodeIterator _Self; + typedef BinaryNode _Node; + typedef Tp value_type; + typedef Tp* pointer; + typedef Tp& reference; + ExplicitNodeIterator() : + element(NULL) { + } + ExplicitNodeIterator(_Node* node) : + element(node) { + } + BinaryNode* element; + _Self up() { + return _Self(element->parent); + } + _Self left() { + if (element != NULL) { + return _Self(element->left); + } else { + return _Self(NULL); + } + + } + _Self right() { + if (element != NULL) { + return _Self(element->right); + } else { + return _Self(NULL); + } + + } + bool operator==(const _Self& __x) const { + return element == __x.element; + } + bool operator!=(const _Self& __x) const { + return element != __x.element; + } + pointer + operator->() const { + if (element != NULL) { + return element->value; + } else { + return NULL; + } + } + pointer operator*() const { + return this->operator->(); + } +}; + +/** + * Pretty rudimentary version of a simple binary tree (not a binary search tree!). + */ +template +class BinaryTree { +public: + typedef ExplicitNodeIterator iterator; + typedef BinaryNode Node; + typedef std::pair children; + BinaryTree() : + rootNode(NULL) { + } + BinaryTree(Node* rootNode) : + rootNode(rootNode) { + } + iterator begin() const { + return iterator(rootNode); + } + static iterator end() { + return iterator(NULL); + } + iterator insert(bool insertLeft, iterator parentNode, Node* newNode ) { + newNode->parent = parentNode.element; + //TODO: Why do I delete the child references of the node? This kills reconnection :-p +// newNode->left = NULL; +// newNode->right = NULL; + if (parentNode.element != NULL) { + if (insertLeft) { + parentNode.element->left = newNode; + } else { + parentNode.element->right = newNode; + } + } else { + //Insert first element. + rootNode = newNode; + } + return iterator(newNode); + } + //No recursion to children. Needs to be done externally. + children erase(iterator node) { + if (node.element == rootNode) { + //We're root node + rootNode = NULL; + } else { + //Delete parent's reference + if (node.up().left() == node) { + node.up().element->left = NULL; + } else { + node.up().element->right = NULL; + } + } + return children(node.element->left, node.element->right); + } + static uint32_t countLeft(iterator start) { + if (start == end()) { + return 0; + } + //We also count the start node itself. + uint32_t count = 1; + while (start.left() != end()) { + count++; + start = start.left(); + } + return count; + } + static uint32_t countRight(iterator start) { + if (start == end()) { + return 0; + } + //We also count the start node itself. + uint32_t count = 1; + while (start.right() != end()) { + count++; + start = start.right(); + } + return count; + } + +protected: + Node* rootNode; +}; + + + +#endif /* FRAMEWORK_CONTAINER_BINARYTREE_H_ */ diff --git a/container/FIFO.h b/container/FIFO.h new file mode 100644 index 00000000..eda853bc --- /dev/null +++ b/container/FIFO.h @@ -0,0 +1,62 @@ +#ifndef FIFO_H_ +#define FIFO_H_ + +#include + +template +class FIFO { +private: + uint8_t readIndex, writeIndex, currentSize; + T data[capacity]; + + uint8_t next(uint8_t current) { + ++current; + if (current == capacity) { + current = 0; + } + return current; + } +public: + FIFO() : + readIndex(0), writeIndex(0), currentSize(0) { + } + + bool emtpy() { + return (currentSize == 0); + } + + bool full() { + return (currentSize == capacity); + } + + uint8_t size(){ + return currentSize; + } + + ReturnValue_t insert(T value) { + if (full()) { + return FULL; + } else { + data[writeIndex] = value; + writeIndex = next(writeIndex); + ++currentSize; + return HasReturnvaluesIF::RETURN_OK; + } + } + + ReturnValue_t retrieve(T *value) { + if (emtpy()) { + return EMPTY; + } else { + *value = data[readIndex]; + readIndex = next(readIndex); + --currentSize; + return HasReturnvaluesIF::RETURN_OK; + } + } + static const uint8_t INTERFACE_ID = FIFO_CLASS; + static const ReturnValue_t FULL = MAKE_RETURN_CODE(1); + static const ReturnValue_t EMPTY = MAKE_RETURN_CODE(2); +}; + +#endif /* FIFO_H_ */ diff --git a/container/FixedArrayList.h b/container/FixedArrayList.h new file mode 100644 index 00000000..36f914cd --- /dev/null +++ b/container/FixedArrayList.h @@ -0,0 +1,32 @@ +#ifndef FIXEDARRAYLIST_H_ +#define FIXEDARRAYLIST_H_ + +#include + +template +class FixedArrayList: public ArrayList { +private: + T data[MAX_SIZE]; +public: + FixedArrayList() : + ArrayList(data, MAX_SIZE) { + } + + FixedArrayList(const FixedArrayList& other) : + ArrayList(data, MAX_SIZE) { + memcpy(this->data, other.data, sizeof(this->data)); + this->entries = data; + } + + FixedArrayList& operator=(FixedArrayList other) { + memcpy(this->data, other.data, sizeof(this->data)); + this->entries = data; + return *this; + } + + virtual ~FixedArrayList() { + } + +}; + +#endif /* FIXEDARRAYLIST_H_ */ diff --git a/container/FixedMap.h b/container/FixedMap.h new file mode 100644 index 00000000..45660859 --- /dev/null +++ b/container/FixedMap.h @@ -0,0 +1,197 @@ +#ifndef FIXEDMAP_H_ +#define FIXEDMAP_H_ + +#include +#include +#include + + +template +class FixedMap: public SerializeIF { +public: + static const uint8_t INTERFACE_ID = FIXED_MAP; + static const ReturnValue_t KEY_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t MAP_FULL = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t KEY_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x03); + +private: + static const key_t EMPTY_SLOT = -1; + ArrayList, uint32_t> theMap; + uint32_t _size; + + uint32_t findIndex(key_t key) const { + if (_size == 0) { + return 1; + } + uint32_t i = 0; + for (i = 0; i < _size; ++i) { + if (theMap[i].first == key) { + return i; + } + } + return i; + } +public: + FixedMap(uint32_t maxSize) : + theMap(maxSize), _size(0) { + } + + class Iterator: public ArrayList, uint32_t>::Iterator { + public: + Iterator() : + ArrayList, uint32_t>::Iterator() { + } + + Iterator(std::pair *pair) : + ArrayList, uint32_t>::Iterator(pair) { + } + + T operator*() { + return ArrayList, uint32_t>::Iterator::value->second; + } + + T *operator->() { + return &ArrayList, uint32_t>::Iterator::value->second; + } + + }; + + Iterator begin() const { + return Iterator(&theMap[0]); + } + + Iterator end() const { + return Iterator(&theMap[_size]); + } + + uint32_t size() const { + return _size; + } + + ReturnValue_t insert(key_t key, T value, Iterator *storedValue = NULL) { + if (exists(key) == HasReturnvaluesIF::RETURN_OK) { + return KEY_ALREADY_EXISTS; + } + if (_size == theMap.maxSize()) { + return MAP_FULL; + } + theMap[_size].first = key; + theMap[_size].second = value; + if (storedValue != NULL) { + *storedValue = Iterator(&theMap[_size]); + } + ++_size; + return HasReturnvaluesIF::RETURN_OK; + } + + ReturnValue_t insert(std::pair pair) { + return insert(pair.fist, pair.second); + } + + ReturnValue_t exists(key_t key) const { + ReturnValue_t result = KEY_DOES_NOT_EXIST; + if (findIndex(key) < _size) { + result = HasReturnvaluesIF::RETURN_OK; + } + return result; + } + + ReturnValue_t erase(Iterator *iter) { + uint32_t i; + if ((i = findIndex((*iter).value->first)) >= _size) { + return KEY_DOES_NOT_EXIST; + } + theMap[i] = theMap[_size - 1]; + --_size; + --((*iter).value); + return HasReturnvaluesIF::RETURN_OK; + } + + ReturnValue_t erase(key_t key) { + uint32_t i; + if ((i = findIndex(key)) >= _size) { + return KEY_DOES_NOT_EXIST; + } + theMap[i] = theMap[_size - 1]; + --_size; + return HasReturnvaluesIF::RETURN_OK; + } + + T *findValue(key_t key) const { + return &theMap[findIndex(key)].second; + } + + Iterator find(key_t key) const { + ReturnValue_t result = exists(key); + if (result != HasReturnvaluesIF::RETURN_OK) { + return end(); + } + return Iterator(&theMap[findIndex(key)]); + } + + ReturnValue_t find(key_t key, T **value) const { + ReturnValue_t result = exists(key); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + *value = &theMap[findIndex(key)].second; + return HasReturnvaluesIF::RETURN_OK; + } + + void clear() { + _size = 0; + } + + uint32_t maxSize() const { + return theMap.maxSize(); + } + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + ReturnValue_t result = SerializeAdapter::serialize(&this->_size, + buffer, size, max_size, bigEndian); + uint32_t i = 0; + while ((result == HasReturnvaluesIF::RETURN_OK) && (i < this->_size)) { + result = SerializeAdapter::serialize(&theMap[i].first, buffer, + size, max_size, bigEndian); + result = SerializeAdapter::serialize(&theMap[i].second, buffer, size, + max_size, bigEndian); + ++i; + } + return result; + } + + virtual uint32_t getSerializedSize() const { + uint32_t printSize = sizeof(_size); + uint32_t i = 0; + + for (i = 0; i < _size; ++i) { + printSize += SerializeAdapter::getSerializedSize( + &theMap[i].first); + printSize += SerializeAdapter::getSerializedSize(&theMap[i].second); + } + + return printSize; + } + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + ReturnValue_t result = SerializeAdapter::deSerialize(&this->_size, + buffer, size, bigEndian); + if (this->_size > theMap.maxSize()) { + return SerializeIF::TOO_MANY_ELEMENTS; + } + uint32_t i = 0; + while ((result == HasReturnvaluesIF::RETURN_OK) && (i < this->_size)) { + result = SerializeAdapter::deSerialize(&theMap[i].first, buffer, + size, bigEndian); + result = SerializeAdapter::deSerialize(&theMap[i].second, buffer, size, + bigEndian); + ++i; + } + return result; + } + +}; + +#endif /* FIXEDMAP_H_ */ diff --git a/container/FixedOrderedMultimap.h b/container/FixedOrderedMultimap.h new file mode 100644 index 00000000..99a3fbc2 --- /dev/null +++ b/container/FixedOrderedMultimap.h @@ -0,0 +1,186 @@ +/* + * FixedOrderedMultimap.h + * + * Created on: 22.01.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_CONTAINER_FIXEDORDEREDMULTIMAP_H_ +#define FRAMEWORK_CONTAINER_FIXEDORDEREDMULTIMAP_H_ + +#include +#include +#include + +template> +class FixedOrderedMultimap { +public: + static const uint8_t INTERFACE_ID = FIXED_MAP; + static const ReturnValue_t KEY_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t MAP_FULL = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t KEY_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x03); + +private: + typedef KEY_COMPARE compare; + compare myComp; + ArrayList, uint32_t> theMap; + uint32_t _size; + + uint32_t findFirstIndex(key_t key, uint32_t startAt = 0) const { + if (startAt >= _size) { + return startAt + 1; + } + uint32_t i = startAt; + for (i = startAt; i < _size; ++i) { + if (theMap[i].first == key) { + return i; + } + } + return i; + } + + uint32_t findNicePlace(key_t key) const { + uint32_t i = 0; + for (i = 0; i < _size; ++i) { + if (myComp(key, theMap[i].first)) { + return i; + } + } + return i; + } + + void removeFromPosition(uint32_t position) { + if (_size <= position) { + return; + } + memmove(&theMap[position], &theMap[position + 1], + (_size - position - 1) * sizeof(std::pair)); + --_size; + } +public: + FixedOrderedMultimap(uint32_t maxSize) : + theMap(maxSize), _size(0) { + } + virtual ~FixedOrderedMultimap() { + } + + class Iterator: public ArrayList, uint32_t>::Iterator { + public: + Iterator() : + ArrayList, uint32_t>::Iterator() { + } + + Iterator(std::pair *pair) : + ArrayList, uint32_t>::Iterator(pair) { + } + + T operator*() { + return ArrayList, uint32_t>::Iterator::value->second; + } + + T *operator->() { + return &ArrayList, uint32_t>::Iterator::value->second; + } + + }; + + Iterator begin() const { + return Iterator(&theMap[0]); + } + + Iterator end() const { + return Iterator(&theMap[_size]); + } + + uint32_t size() const { + return _size; + } + + ReturnValue_t insert(key_t key, T value, Iterator *storedValue = NULL) { + if (_size == theMap.maxSize()) { + return MAP_FULL; + } + uint32_t position = findNicePlace(key); + memmove(&theMap[position + 1], &theMap[position], + (_size - position) * sizeof(std::pair)); + theMap[position].first = key; + theMap[position].second = value; + ++_size; + if (storedValue != NULL) { + *storedValue = Iterator(&theMap[position]); + } + return HasReturnvaluesIF::RETURN_OK; + } + + ReturnValue_t insert(std::pair pair) { + return insert(pair.fist, pair.second); + } + + ReturnValue_t exists(key_t key) const { + ReturnValue_t result = KEY_DOES_NOT_EXIST; + if (findFirstIndex(key) < _size) { + result = HasReturnvaluesIF::RETURN_OK; + } + return result; + } + + ReturnValue_t erase(Iterator *iter) { + uint32_t i; + if ((i = findFirstIndex((*iter).value->first)) >= _size) { + return KEY_DOES_NOT_EXIST; + } + removeFromPosition(i); + if (*iter != begin()) { + (*iter)--; + } else { + *iter = begin(); + } + return HasReturnvaluesIF::RETURN_OK; + } + + ReturnValue_t erase(key_t key) { + uint32_t i; + if ((i = findFirstIndex(key)) >= _size) { + return KEY_DOES_NOT_EXIST; + } + do { + removeFromPosition(i); + i = findFirstIndex(key, i); + } while (i < _size); + return HasReturnvaluesIF::RETURN_OK; + } + + //This is potentially unsafe +// T *findValue(key_t key) const { +// return &theMap[findFirstIndex(key)].second; +// } + + + Iterator find(key_t key) const { + ReturnValue_t result = exists(key); + if (result != HasReturnvaluesIF::RETURN_OK) { + return end(); + } + return Iterator(&theMap[findFirstIndex(key)]); + } + + ReturnValue_t find(key_t key, T **value) const { + ReturnValue_t result = exists(key); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + *value = &theMap[findFirstIndex(key)].second; + return HasReturnvaluesIF::RETURN_OK; + } + + void clear() { + _size = 0; + } + + uint32_t maxSize() const { + return theMap.maxSize(); + } + +}; + +#endif /* FRAMEWORK_CONTAINER_FIXEDORDEREDMULTIMAP_H_ */ diff --git a/container/HybridIterator.h b/container/HybridIterator.h new file mode 100644 index 00000000..b34fdfd0 --- /dev/null +++ b/container/HybridIterator.h @@ -0,0 +1,92 @@ +#ifndef HYBRIDITERATOR_H_ +#define HYBRIDITERATOR_H_ + +#include +#include + +template +class HybridIterator: public LinkedElement::Iterator, + public ArrayList::Iterator { +public: + HybridIterator() : + value(NULL), linked(NULL), end(NULL) { + } + + HybridIterator(typename LinkedElement::Iterator *iter) : + LinkedElement::Iterator(*iter), value( + iter->value), linked(true), end(NULL) { + + } + + HybridIterator(LinkedElement *start) : + LinkedElement::Iterator(start), value( + start->value), linked(true), end(NULL) { + + } + + HybridIterator(typename ArrayList::Iterator start, + typename ArrayList::Iterator end) : + ArrayList::Iterator(start), value(start.value), linked( + false), end(end.value) { + if (value == this->end) { + value = NULL; + } + } + + HybridIterator(T *firstElement, T *lastElement) : + ArrayList::Iterator(firstElement), value(firstElement), linked( + false), end(++lastElement) { + if (value == end) { + value = NULL; + } + } + + HybridIterator& operator++() { + if (linked) { + LinkedElement::Iterator::operator++(); + if (LinkedElement::Iterator::value != NULL) { + value = LinkedElement::Iterator::value->value; + } else { + value = NULL; + } + } else { + ArrayList::Iterator::operator++(); + value = ArrayList::Iterator::value; + + if (value == end) { + value = NULL; + } + } + return *this; + } + + HybridIterator operator++(int) { + HybridIterator tmp(*this); + operator++(); + return tmp; + } + + bool operator==(HybridIterator other) { + return value == other->value; + } + + bool operator!=(HybridIterator other) { + return !(*this == other); + } + + T operator*() { + return *value; + } + + T *operator->() { + return value; + } + + T* value; + +private: + bool linked; + T *end; +}; + +#endif /* HYBRIDITERATOR_H_ */ diff --git a/container/IsDerivedFrom.h b/container/IsDerivedFrom.h new file mode 100644 index 00000000..520033db --- /dev/null +++ b/container/IsDerivedFrom.h @@ -0,0 +1,41 @@ +#ifndef ISDERIVEDFROM_H_ +#define ISDERIVEDFROM_H_ + +template +class IsDerivedFrom { + class No { + }; + class Yes { + No no[3]; + }; + + static Yes Test(B*); // declared, but not defined + static No Test(... ); // declared, but not defined + +public: + enum { + Is = sizeof(Test(static_cast(0))) == sizeof(Yes) + }; +}; + +template +struct is_same { + static bool const value = false; +}; + +template +struct is_same { + static bool const value = true; +}; + + +template +struct enable_if { + typedef T type; +}; + +template +struct enable_if { }; + + +#endif /* ISDERIVEDFROM_H_ */ diff --git a/container/LinkedElementDecorator.h b/container/LinkedElementDecorator.h new file mode 100644 index 00000000..9b7daa20 --- /dev/null +++ b/container/LinkedElementDecorator.h @@ -0,0 +1,27 @@ +/** + * @file LinkedElementDecorator.h + * @brief This file defines the LinkedElementDecorator class. + * @date 22.07.2014 + * @author baetz + */ +#ifndef LINKEDELEMENTDECORATOR_H_ +#define LINKEDELEMENTDECORATOR_H_ + +#include +#include + +//TODO: This generates multiple inheritance from non-IF parents. +template +class LinkedElementDecorator : public LinkedElement, public T { +public: + template + LinkedElementDecorator(Args... args) : LinkedElement(this), T(std::forward(args)...) { + } + + virtual ~LinkedElementDecorator() { + } +}; + + + +#endif /* LINKEDELEMENTDECORATOR_H_ */ diff --git a/container/PlacementFactory.h b/container/PlacementFactory.h new file mode 100644 index 00000000..cc92773e --- /dev/null +++ b/container/PlacementFactory.h @@ -0,0 +1,41 @@ +/* + * PlacementFactory.h + * + * Created on: 10.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_CONTAINER_PLACEMENTFACTORY_H_ +#define FRAMEWORK_CONTAINER_PLACEMENTFACTORY_H_ + +#include +#include + +class PlacementFactory { +public: + PlacementFactory(StorageManagerIF* backend) : + dataBackend(backend) { + } + template + T* generate(Args&&... args) { + store_address_t tempId; + uint8_t* pData = NULL; + ReturnValue_t result = dataBackend->getFreeElement(&tempId, sizeof(T), + &pData); + if (result != HasReturnvaluesIF::RETURN_OK) { + return NULL; + } + T* temp = new (pData) T(std::forward(args)...); + return temp; + } + template + ReturnValue_t destroy(T* thisElement) { + //TODO: Shouldn't we call the destructor here first, in case something was allocated by the object (shouldn't do that, however). + uint8_t* pointer = (uint8_t*) (thisElement); + return dataBackend->deleteData(pointer, sizeof(T)); + } +private: + StorageManagerIF* dataBackend; +}; + +#endif /* FRAMEWORK_CONTAINER_PLACEMENTFACTORY_H_ */ diff --git a/container/RingBufferBase.h b/container/RingBufferBase.h new file mode 100644 index 00000000..2cd23afd --- /dev/null +++ b/container/RingBufferBase.h @@ -0,0 +1,103 @@ +/* + * RingBufferBase.h + * + * Created on: 06.02.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_CONTAINER_RINGBUFFERBASE_H_ +#define FRAMEWORK_CONTAINER_RINGBUFFERBASE_H_ + +#include + +template +class RingBufferBase { +public: + RingBufferBase(uint32_t startAddress, uint32_t size, bool overwriteOld) : + start(startAddress), write(startAddress), size(size), overwriteOld(overwriteOld) { + for (uint8_t count = 0; count < N_READ_PTRS; count++) { + read[count] = startAddress; + } + } + ReturnValue_t readData(uint32_t amount, uint8_t n = 0) { + if (availableReadData(n) >= amount) { + incrementRead(amount, n); + return HasReturnvaluesIF::RETURN_OK; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + ReturnValue_t writeData(uint32_t amount) { + if (availableWriteSpace() >= amount || overwriteOld) { + incrementWrite(amount); + return HasReturnvaluesIF::RETURN_OK; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + uint32_t availableReadData(uint8_t n = 0) const { + return ((write + size) - read[n]) % size; + } + uint32_t availableWriteSpace(uint8_t n = 0) const { + //One less to avoid ambiguous full/empty problem. + return (((read[n] + size) - write - 1) % size); + } + bool isFull(uint8_t n = 0) { + return (availableWriteSpace(n) == 0); + } + bool isEmpty(uint8_t n = 0) { + return (availableReadData(n) == 0); + } + virtual ~RingBufferBase() { + + } + uint32_t getRead(uint8_t n = 0) const { + return read[n]; + } + void setRead(uint32_t read, uint8_t n = 0) { + if (read >= start && read < (start+size)) { + this->read[n] = read; + } + } + uint32_t getWrite() const { + return write; + } + void setWrite(uint32_t write) { + this->write = write; + } + void clear() { + write = start; + for (uint8_t count = 0; count < N_READ_PTRS; count++) { + read[count] = start; + } + } + uint32_t writeTillWrap() { + return (start + size) - write; + } + uint32_t readTillWrap(uint8_t n = 0) { + return (start + size) - read[n]; + } + const uint32_t getStart() const { + return start; + } + const bool overwritesOld() const { + return overwriteOld; + } + uint32_t maxSize() const { + return size - 1; + } +private: + const uint32_t start; + uint32_t write; + uint32_t read[N_READ_PTRS]; + const uint32_t size; + const bool overwriteOld; + void incrementWrite(uint32_t amount) { + write = ((write + amount - start) % size) + start; + } + void incrementRead(uint32_t amount, uint8_t n = 0) { + read[n] = ((read[n] + amount - start) % size) + start; + } +}; + +#endif /* FRAMEWORK_CONTAINER_RINGBUFFERBASE_H_ */ diff --git a/container/SinglyLinkedList.h b/container/SinglyLinkedList.h new file mode 100644 index 00000000..e4b3e8db --- /dev/null +++ b/container/SinglyLinkedList.h @@ -0,0 +1,105 @@ +#ifndef SINGLYLINKEDLIST_H_ +#define SINGLYLINKEDLIST_H_ + +#include +#include + +template +class LinkedElement { +public: + T *value; + class Iterator { + public: + LinkedElement *value; + Iterator() : + value(NULL) { + + } + + Iterator(LinkedElement *element) : + value(element) { + } + + Iterator& operator++() { + value = value->getNext(); + return *this; + } + + Iterator operator++(int) { + Iterator tmp(*this); + operator++(); + return tmp; + } + + bool operator==(Iterator other) { + return value == other.value; + } + + bool operator!=(Iterator other) { + return !(*this == other); + } + T *operator->() { + return value->value; + } + }; + + LinkedElement(T* setElement, LinkedElement* setNext = NULL) : value(setElement), + next(setNext) { + } + virtual ~LinkedElement(){ + + } + virtual LinkedElement* const getNext() const { + return next; + } + + virtual void setNext(LinkedElement* next) { + this->next = next; + } + LinkedElement* begin() { + return this; + } + LinkedElement* end() { + return NULL; + } +private: + LinkedElement *next; +}; + +template +class SinglyLinkedList { +public: + SinglyLinkedList() : + start(NULL) { + } + + SinglyLinkedList(typename LinkedElement::Iterator start) : + start(start.value) { + } + SinglyLinkedList(LinkedElement* startElement) : + start(startElement) { + } + typename LinkedElement::Iterator begin() const { + return LinkedElement::Iterator::Iterator(start); + } + typename LinkedElement::Iterator::Iterator end() const { + return LinkedElement::Iterator::Iterator(); + } + + uint32_t getSize() const { + uint32_t size = 0; + LinkedElement *element = start; + while (element != NULL) { + size++; + element = element->getNext(); + } + return size; + } + void setStart(LinkedElement* setStart) { + start = setStart; + } +protected: + LinkedElement *start; +}; + +#endif /* SINGLYLINKEDLIST_H_ */ diff --git a/container/group.h b/container/group.h new file mode 100644 index 00000000..5a39d34e --- /dev/null +++ b/container/group.h @@ -0,0 +1,15 @@ +#ifndef GROUP_H_ +#define GROUP_H_ + +/** + * @defgroup container Container + * + * General Purpose Container to store various elements. + * + * Also contains Adapter classes to print elements to a + * bytestream and to read them from a bytestream, as well + * as an Adapter to swap the endianness. + */ + + +#endif /* GROUP_H_ */ diff --git a/container/listTest.cpp.ignore b/container/listTest.cpp.ignore new file mode 100644 index 00000000..257b733f --- /dev/null +++ b/container/listTest.cpp.ignore @@ -0,0 +1,365 @@ +#include "FixedArrayList.h" +#include "SinglyLinkedList.h" +#include "HybridIterator.h" + +#include "FixedMap.h" + +#include + +/* + +class Packet: public SinglyLinkedList { +public: + SinglyLinkedList::Element element1; + SinglyLinkedList::Element element2; + + Packet() { + this->start = &element1; + element1.next = &element2; + } +}; + +class Packet2: public SinglyLinkedList { +public: + SinglyLinkedList::Element element1; + SinglyLinkedList::Element, 2>> element2; + SinglyLinkedList::Element element3; + + Packet2() { + this->start = &element1; + element1.next = &element2; + element2.next = &element3; + } +}; + +class Packet3: public SinglyLinkedList { +public: + SinglyLinkedList::TypedElement element1; + SinglyLinkedList::TypedElement element2; + + Packet3() { + this->start = &element1; + element1.next = &element2; + } +}; + +void arrayList() { + puts("** Array List **"); + FixedArrayList list; + FixedArrayList list2; + + list.size = 2; + + list[0] = 0xcafecafe; + + list[1] = 0x12345678; + + uint8_t buffer[100]; + uint8_t *pointer = buffer; + uint32_t size = 0; + uint32_t maxSize = 100; + uint32_t i; + int32_t size2; + + printf("printsize: %i\n", list.getPrintSize()); + + list.print(&pointer, &size, 100, true); + + printf("buffer(%i):", size); + for (i = 0; i < size; ++i) { + printf("%02x", buffer[i]); + } + printf("\n"); + + pointer = buffer; + + size2 = size; + + printf("list2 read: %x\n", list2.read(&pointer, &size2, true)); + + printf("list2(%i):", list2.size); + for (ArrayList::Iterator iter = list2.begin(); + iter != list2.end(); iter++) { + printf("0x%04x ", *iter); + } + printf("\n"); + + HybridIterator hiter(list.begin(),list.end()); + + printf("hybrid1: 0x%04x\n", *(hiter++)); + printf("hybrid2: 0x%04x\n", *hiter); + +} + +void allocatingList() { + puts("** Allocating List **"); + ArrayList myList(3), myList2(2); + myList[0] = 0xab; + myList[1] = 0xcd; + myList.size = 2; + + uint8_t buffer[100]; + uint8_t *pointer = buffer; + uint32_t size = 0; + uint32_t maxSize = 100; + uint32_t i; + int32_t size2; + + myList.print(&pointer, &size, 100, true); + + pointer = buffer; + size2 = size; + + printf("Read %x\n", myList2.read(&pointer, &size2, true)); + + printf("%x,%x\n", myList2[0], myList2[1]); + +} + +void linkedList() { + puts("** Linked List **"); + uint8_t buffer[100]; + uint8_t *pointer = buffer; + uint32_t size = 0; + uint32_t maxSize = 100; + uint32_t i; + int32_t size2; + + Packet myPacket; + myPacket.element1.entry = 0x12345678; + myPacket.element2.entry = 0x9abcdef0; + + pointer = buffer; + size = 0; + ReturnValue_t result = myPacket.print(&pointer, &size, 100, true); + + printf("result %02x\n", result); + + printf("printsize: %i\n", myPacket.getPrintSize()); + + printf("buffer(%i):", size); + for (i = 0; i < size; ++i) { + printf("%02x", buffer[i]); + } + printf("\n"); + + Packet3 myPacket3; + + myPacket3.element1.entry = 0x12345678; + myPacket3.element2.entry = 0xabcdeff; + + SinglyLinkedList::TypedIterator titer(&myPacket3.element1); + + printf("0x%04x\n", *titer); + + HybridIterator hiter(&myPacket3.element1); + + printf("hybrid1: 0x%04x\n", *hiter); + hiter++; + printf("hybrid2: 0x%04x\n", *hiter); +} + +void complex() { + puts("** complex **"); + uint8_t buffer[100]; + uint8_t *pointer = buffer; + uint32_t size = 0; + uint32_t maxSize = 100; + uint32_t i; + int32_t size2 = size; + + Packet myPacket2; + + size2 = size; + pointer = buffer; + + myPacket2.read(&pointer, &size2, true); + + printf("packet: 0x%04x, 0x%04x\n", myPacket2.element1.entry, + myPacket2.element2.entry); + + buffer[0] = 0x12; + buffer[1] = 0x34; + buffer[2] = 0x56; + buffer[3] = 0x78; + buffer[4] = 0x2; + buffer[5] = 0x3; + buffer[6] = 0xab; + buffer[7] = 0xcd; + buffer[8] = 0xef; + buffer[9] = 0x2; + buffer[10] = 0x11; + buffer[11] = 0x22; + buffer[12] = 0xca; + buffer[13] = 0xfe; + buffer[14] = 0x5a; + buffer[15] = 0xfe; + + pointer = buffer; + size2 = 23; + + Packet2 p2; + + ReturnValue_t result = p2.read(&pointer, &size2, true); + printf("result is %02x\n", result); + + printf("%04x; %i: %i: %x %x %x; %i: %x %x;; %04x\n", p2.element1.entry, + p2.element2.entry.size, p2.element2.entry[0].size, + p2.element2.entry[0][0], p2.element2.entry[0][1], + p2.element2.entry[0][2], p2.element2.entry[1].size, + p2.element2.entry[1][0], p2.element2.entry[1][1], + p2.element3.entry); + +} +*/ +struct Test { + uint32_t a; + uint32_t b; +}; + +template +void printMap(FixedMap *map) { + typename FixedMap::Iterator iter; + printf("Map (%i): ", map->getSize()); + for (iter = map->begin(); iter != map->end(); ++iter) { + printf("%x:%08x,%08x ", iter.value->first, (*iter).a, (*iter).b); + } + printf("\n"); +} + +template +void map() { + puts("** Map **"); + typename FixedMap::Iterator iter; + ReturnValue_t result; + FixedMap myMap(5); + + printMap(&myMap); + + Test a; + a.a = 0x01234567; + a.b = 0xabcdef89; + + myMap.insert(1, a); + printMap(&myMap); + + a.a = 0; + + myMap.insert(2, a); + printMap(&myMap); + + printf("2 exists: %x\n", myMap.exists(0x02)); + + printf("ff exists: %x\n", myMap.exists(0xff)); + + a.a = 1; + printf("insert 0x2: %x\n", myMap.insert(2, a)); + + result = myMap.insert(0xff, a); + a.a = 0x44; + result = myMap.insert(0xab, a); + result = myMap.insert(0xa, a); + + printMap(&myMap); + + printf("insert 0x5: %x\n", myMap.insert(5, a)); + + printf("erase 0xfe: %x\n", myMap.erase(0xfe)); + + printf("erase 0x2: %x\n", myMap.erase(0x2)); + + printMap(&myMap); + + printf("erase 0xab: %x\n", myMap.erase(0xab)); + printMap(&myMap); + + printf("insert 0x5: %x\n", myMap.insert(5, a)); + printMap(&myMap); + + iter = myMap.begin(); + ++iter; + ++iter; + ++iter; + + printf("iter: %i: %x,%x\n",iter.value->first, iter->a, iter->b); + + myMap.erase(&iter); + + printf("iter: %i: %x,%x\n",iter.value->first, iter->a, iter->b); + printMap(&myMap); + +} + +/* +void mapPrint() { + puts("** Map Print **"); + FixedMap myMap(5); + Packet2 myPacket; + myPacket.element1.entry = 0x12345678; + + myPacket.element2.entry[0][0] = 0xab; + myPacket.element2.entry[0][1] = 0xcd; + myPacket.element2.entry[0].size = 2; + myPacket.element2.entry.size = 1; + + myPacket.element3.entry = 0xabcdef90; + + myMap.insert(0x1234, myPacket); + + uint8_t buffer[100]; + uint32_t size = 0, i; + uint8_t *pointer = buffer; + + printf("printsize: %i\n", myMap.getPrintSize()); + + SerializeAdapter>::print(&myMap, &pointer, + &size, 100, false); + + printf("buffer(%i):", size); + for (i = 0; i < size; ++i) { + printf("%02x", buffer[i]); + } + printf("\n"); + + int32_t size2 = size; + pointer = buffer; + + FixedMap myMap2(5); + + ReturnValue_t result = SerializeAdapter>::read( + &myMap2, &pointer, &size2, false); + + Packet2 *myPacket2 = myMap2.find(0x1234); + + printf("Map (%i): Packet2: %x, Array (%i): Array (%i): %x, %x; %x\n", + myMap2.getSize(), myPacket2->element1.entry, + myPacket2->element2.entry.size, myPacket2->element2.entry[0].size, + myPacket2->element2.entry[0][0], myPacket2->element2.entry[0][1], + myPacket2->element3.entry); + +} + +void empty() { + puts("** Empty **"); + ArrayList list(0); + printf("%p %p\n", list.front(), list.back()); +} +*/ + +int main(void) { + +// arrayList(); +// linkedList(); +// allocatingList(); +// complex(); + + map(); +// +// mapPrint(); + +// empty(); + + + + return 0; +} diff --git a/controller/ControllerBase.cpp b/controller/ControllerBase.cpp new file mode 100644 index 00000000..4b72af3b --- /dev/null +++ b/controller/ControllerBase.cpp @@ -0,0 +1,120 @@ +#include +#include +#include + +ControllerBase::ControllerBase(uint32_t setObjectId, uint32_t parentId, + size_t commandQueueDepth) : + SystemObject(setObjectId), parentId(parentId), mode(MODE_OFF), submode( + SUBMODE_NONE), commandQueue(commandQueueDepth), modeHelper( + this), healthHelper(this, setObjectId) { +} + +ControllerBase::~ControllerBase() { +} + +ReturnValue_t ControllerBase::initialize() { + ReturnValue_t result = SystemObject::initialize(); + if (result != RETURN_OK) { + return result; + } + + MessageQueueId_t parentQueue = 0; + if (parentId != 0) { + SubsystemBase *parent = objectManager->get(parentId); + if (parent == NULL) { + 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; +} + +MessageQueueId_t ControllerBase::getCommandQueue() const { + return commandQueue.getId(); +} + +void ControllerBase::handleQueue() { + CommandMessage message; + ReturnValue_t result; + for (result = commandQueue.receiveMessage(&message); result == RETURN_OK; + result = commandQueue.receiveMessage(&message)) { + + result = modeHelper.handleModeCommand(&message); + if (result == RETURN_OK) { + continue; + } + + result = healthHelper.handleHealthCommand(&message); + if (result == RETURN_OK) { + continue; + } + result = handleCommandMessage(&message); + if (result == RETURN_OK) { + continue; + } + message.clearCommandMessage(); + CommandMessage reply(CommandMessage::REPLY_REJECTED, + CommandMessage::UNKNOW_COMMAND, 0); + commandQueue.reply(&reply); + } + +} + +void ControllerBase::startTransition(Mode_t mode, Submode_t submode) { + triggerEvent(CHANGING_MODE, mode, submode); + modeHelper.modeChanged(mode, submode); + modeChanged(mode, submode); + this->mode = mode; + this->submode = submode; + announceMode(false); +} + +void ControllerBase::getMode(Mode_t* mode, Submode_t* submode) { + *mode = this->mode; + *submode = this->submode; +} + +void ControllerBase::setToExternalControl() { + healthHelper.setHealth(EXTERNAL_CONTROL); +} + +void ControllerBase::announceMode(bool recursive) { + triggerEvent(MODE_INFO, mode, submode); +} + +ReturnValue_t ControllerBase::performOperation() { + handleQueue(); + performControlOperation(); + return RETURN_OK; +} + +void ControllerBase::modeChanged(Mode_t mode, Submode_t submode) { + return; +} + +ReturnValue_t ControllerBase::setHealth(HealthState health) { + switch (health) { + case HEALTHY: + case EXTERNAL_CONTROL: + healthHelper.setHealth(health); + return RETURN_OK; + default: + return INVALID_HEALTH_STATE; + } +} + +HasHealthIF::HealthState ControllerBase::getHealth() { + return healthHelper.getHealth(); +} diff --git a/controller/ControllerBase.h b/controller/ControllerBase.h new file mode 100644 index 00000000..e1167a16 --- /dev/null +++ b/controller/ControllerBase.h @@ -0,0 +1,58 @@ +#ifndef CONTROLLERBASE_H_ +#define CONTROLLERBASE_H_ + +#include +#include +#include +#include +#include +#include + +class ControllerBase: public HasModesIF, + public HasHealthIF, + public ExecutableObjectIF, + public SystemObject, + public HasReturnvaluesIF { +public: + + static const Mode_t MODE_NORMAL = 2; + + ControllerBase(uint32_t setObjectId, uint32_t parentId, size_t commandQueueDepth = 3); + virtual ~ControllerBase(); + + ReturnValue_t initialize(); + + virtual MessageQueueId_t getCommandQueue() const; + + virtual ReturnValue_t performOperation(); + + virtual ReturnValue_t setHealth(HealthState health); + + virtual HasHealthIF::HealthState getHealth(); +protected: + const uint32_t parentId; + + Mode_t mode; + + Submode_t submode; + + MessageQueue commandQueue; + + ModeHelper modeHelper; + + HealthHelper healthHelper; + + void handleQueue(); + + virtual ReturnValue_t handleCommandMessage(CommandMessage *message) = 0; + virtual void performControlOperation() = 0; + virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode) = 0; + virtual void modeChanged(Mode_t mode, Submode_t submode); + virtual void startTransition(Mode_t mode, Submode_t submode); + virtual void getMode(Mode_t *mode, Submode_t *submode); + virtual void setToExternalControl(); + virtual void announceMode(bool recursive); +}; + +#endif /* CONTROLLERBASE_H_ */ diff --git a/coordinates/CoordinateTransformations.cpp b/coordinates/CoordinateTransformations.cpp new file mode 100644 index 00000000..ff906043 --- /dev/null +++ b/coordinates/CoordinateTransformations.cpp @@ -0,0 +1,206 @@ +#include +#include +#include +#include +#include +#include + + +//TODO move time stuff to OSAL + +void CoordinateTransformations::positionEcfToEci(const double* ecfPosition, + double* eciPosition) { + ecfToEci(ecfPosition, eciPosition, NULL); + +} + +void CoordinateTransformations::velocityEcfToEci(const double* ecfVelocity, + const double* ecfPosition, double* eciVelocity) { + ecfToEci(ecfVelocity, eciVelocity, ecfPosition); +} + +double CoordinateTransformations::getEarthRotationAngle(timeval time) { + + double jD2000UTC = (time.tv_sec - 946728000. + time.tv_usec / 1000000.) + / 24. / 3600.; + + //value of unix time at J2000TT + static const double J2000TtUnix = 946727935.816; + + //TT does not have leap seconds + //so we need to add the leap seconds since J2000 to our UTC based clock + //Conveniently, GPS gives us access to the leap seconds since 1980 + //between 1980 and 2000 13 leap seconds happened + uint8_t leapSecondsSinceJ2000 = utcGpsOffset - 13; + + //Julean centuries since J2000 //TODO fails for dates before now? + double TTt2000 = (time.tv_sec + time.tv_usec / 1000000. - J2000TtUnix + + leapSecondsSinceJ2000) / 24. / 3600. / 36525.; + + double theta = 2 * Math::PI + * (0.779057273264 + 1.00273781191135448 * jD2000UTC); + + //Correct theta according to IAU 2000 precession-nutation model + theta = theta + 7.03270725817493E-008 + 0.0223603701 * TTt2000 + + 6.77128219501896E-006 * TTt2000 * TTt2000 + + 4.5300990362875E-010 * TTt2000 * TTt2000 * TTt2000 + + 9.12419347848147E-011 * TTt2000 * TTt2000 * TTt2000 * TTt2000; + return theta; +} + +void CoordinateTransformations::getEarthRotationMatrix(timeval time, + double matrix[][3]) { + double theta = getEarthRotationAngle(time); + + matrix[0][0] = cos(theta); + matrix[0][1] = sin(theta); + matrix[0][2] = 0; + matrix[1][0] = -sin(theta); + matrix[1][1] = cos(theta); + matrix[1][2] = 0; + matrix[2][0] = 0; + matrix[2][1] = 0; + matrix[2][2] = 1; +} + +void CoordinateTransformations::ecfToEci(const double* ecfCoordinates, + double* eciCoordinates, + const double* ecfPositionIfCoordinatesAreVelocity) { + //TODO all calculations only work with a correct time + + timeval time; + OSAL::getClock_timeval(&time); + + //value of unix time at J2000TT + static const double J2000TtUnix = 946727935.816; + + //we need TT which does not have leap seconds + //so we need to add the leap seconds since J2000 to our UTC based clock + //Conveniently, GPS gives us access to the leap seconds since 1980 + //between 1980 and 2000 13 leap seconds happened + uint8_t leapSecondsSinceJ2000 = utcGpsOffset - 13; + + //Julean centuries since J2000 //TODO fails for dates before now? + double TTt2000 = (time.tv_sec + time.tv_usec / 1000000. - J2000TtUnix + + leapSecondsSinceJ2000) / 24. / 3600. / 36525.; + + ////////////////////////////////////////////////////////// + // Calculate Precession Matrix + + double zeta = 0.0111808609 * TTt2000 + + 1.46355554053347E-006 * TTt2000 * TTt2000 + + 8.72567663260943E-008 * TTt2000 * TTt2000 * TTt2000; + double theta_p = 0.0097171735 * TTt2000 + - 2.06845757045384E-006 * TTt2000 * TTt2000 + - 2.02812107218552E-007 * TTt2000 * TTt2000 * TTt2000; + double z = zeta + 3.8436028638364E-006 * TTt2000 * TTt2000 + + 0.000000001 * TTt2000 * TTt2000 * TTt2000; + + double mPrecession[3][3]; + + mPrecession[0][0] = -sin(z) * sin(zeta) + cos(z) * cos(theta_p) * cos(zeta); + mPrecession[1][0] = cos(z) * sin(zeta) + sin(z) * cos(theta_p) * cos(zeta); + mPrecession[2][0] = sin(theta_p) * cos(zeta); + + mPrecession[0][1] = -sin(z) * cos(zeta) - cos(z) * cos(theta_p) * sin(zeta); + mPrecession[1][1] = cos(z) * cos(zeta) - sin(z) * cos(theta_p) * sin(zeta); + mPrecession[2][1] = -sin(theta_p) * sin(zeta); + + mPrecession[0][2] = -cos(z) * sin(theta_p); + mPrecession[1][2] = -sin(z) * sin(theta_p); + mPrecession[2][2] = cos(theta_p); + + ////////////////////////////////////////////////////////// + // Calculate Nutation Matrix + + double omega_moon = 2.1824386244 - 33.7570459338 * TTt2000 + + 3.61428599267159E-005 * TTt2000 * TTt2000 + + 3.87850944887629E-008 * TTt2000 * TTt2000 * TTt2000; + + double deltaPsi = -0.000083388 * sin(omega_moon); + double deltaEpsilon = 4.46174030725106E-005 * cos(omega_moon); + + double epsilon = 0.4090928042 - 0.0002269655 * TTt2000 + - 2.86040071854626E-009 * TTt2000 * TTt2000 + + 8.78967203851589E-009 * TTt2000 * TTt2000 * TTt2000; + + double mNutation[3][3]; + + mNutation[0][0] = cos(deltaPsi); + mNutation[1][0] = cos(epsilon + deltaEpsilon) * sin(deltaPsi); + mNutation[2][0] = sin(epsilon + deltaEpsilon) * sin(deltaPsi); + + mNutation[0][1] = -cos(epsilon) * sin(deltaPsi); + mNutation[1][1] = cos(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi) + + sin(epsilon) * sin(epsilon + deltaEpsilon); + mNutation[2][1] = cos(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi) + - sin(epsilon) * cos(epsilon + deltaEpsilon); + + mNutation[0][2] = -sin(epsilon) * sin(deltaPsi); + mNutation[1][2] = sin(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi) + - cos(epsilon) * sin(epsilon + deltaEpsilon); + mNutation[2][2] = sin(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi) + + cos(epsilon) * cos(epsilon + deltaEpsilon); + + ////////////////////////////////////////////////////////// + // Calculate Earth rotation matrix + + //calculate theta + + double mTheta[3][3]; + getEarthRotationMatrix(time, mTheta); + + //polar motion is neglected + + double Tfi[3][3]; + double Ttemp[3][3]; + double Tif[3][3]; + + MatrixOperations::multiply(mNutation[0], mPrecession[0], Ttemp[0], + 3, 3, 3); + MatrixOperations::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 3, 3); + + MatrixOperations::transpose(Tfi[0], Tif[0], 3); + + MatrixOperations::multiply(Tif[0], ecfCoordinates, eciCoordinates, + 3, 3, 1); + + if (ecfPositionIfCoordinatesAreVelocity != NULL) { + + double Tdotfi[3][3]; + double Tdotif[3][3]; + double Trot[3][3] = { { 0, Earth::OMEGA, 0 }, + { 0 - Earth::OMEGA, 0, 0 }, { 0, 0, 0 } }; + double Ttemp2[3][3]; + + MatrixOperations::multiply(mNutation[0], mPrecession[0], + Ttemp[0], 3, 3, 3); + MatrixOperations::multiply(mTheta[0], Ttemp[0], Ttemp2[0], 3, 3, + 3); + + MatrixOperations::multiply(Trot[0], Ttemp2[0], Tdotfi[0], 3, 3, + 3); + + MatrixOperations::transpose(Tdotfi[0], Tdotif[0], 3); + + double velocityCorrection[3]; + + MatrixOperations::multiply(Tdotif[0], + ecfPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3, + 1); + + VectorOperations::add(velocityCorrection, eciCoordinates, + eciCoordinates, 3); + } +} + +CoordinateTransformations::CoordinateTransformations(uint8_t offset) : + utcGpsOffset(offset) { + +} + +CoordinateTransformations::~CoordinateTransformations() { +} + +void CoordinateTransformations::setUtcGpsOffset(uint8_t offset) { +} diff --git a/coordinates/CoordinateTransformations.h b/coordinates/CoordinateTransformations.h new file mode 100644 index 00000000..d41192c5 --- /dev/null +++ b/coordinates/CoordinateTransformations.h @@ -0,0 +1,29 @@ +#ifndef COORDINATETRANSFORMATIONS_H_ +#define COORDINATETRANSFORMATIONS_H_ + +#include + +class CoordinateTransformations { +public: + CoordinateTransformations(uint8_t utcGpsOffset); + + virtual ~CoordinateTransformations(); + + void positionEcfToEci(const double* ecfCoordinates, double* eciCoordinates); + + void velocityEcfToEci(const double* ecfVelocity, + const double* ecfPosition, + double* eciVelocity); + + double getEarthRotationAngle(timeval time); + + void getEarthRotationMatrix(timeval time, double matrix[][3]); + void setUtcGpsOffset(uint8_t offset); +private: + uint8_t utcGpsOffset; + void ecfToEci(const double* ecfCoordinates, double* eciCoordinates, + const double* ecfPositionIfCoordinatesAreVelocity); + +}; + +#endif /* COORDINATETRANSFORMATIONS_H_ */ diff --git a/coordinates/Sgp4Propagator.cpp b/coordinates/Sgp4Propagator.cpp new file mode 100644 index 00000000..4e1881ed --- /dev/null +++ b/coordinates/Sgp4Propagator.cpp @@ -0,0 +1,228 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +Sgp4Propagator::Sgp4Propagator() : + whichconst(wgs84) { + +} + +Sgp4Propagator::~Sgp4Propagator() { + +} + +//TODO move to OSAL +void jday(int year, int mon, int day, int hr, int minute, double sec, + double& jd) { + jd = 367.0 * year - floor((7 * (year + floor((mon + 9) / 12.0))) * 0.25) + + floor(275 * mon / 9.0) + day + 1721013.5 + + ((sec / 60.0 + minute) / 60.0 + hr) / 24.0; // ut in days + // - 0.5*sgn(100.0*year + mon - 190002.5) + 0.5; +} + +//TODO move to OSAL +void days2mdhms(int year, double days, int& mon, int& day, int& hr, int& minute, + double& sec) { + int i, inttemp, dayofyr; + double temp; + int lmonth[] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }; + + dayofyr = (int) floor(days); + /* ----------------- find month and day of month ---------------- */ + if ((year % 4) == 0) + lmonth[1] = 29; + + i = 1; + inttemp = 0; + while ((dayofyr > inttemp + lmonth[i - 1]) && (i < 12)) { + inttemp = inttemp + lmonth[i - 1]; + i++; + } + mon = i; + day = dayofyr - inttemp; + + /* ----------------- find hours minutes and seconds ------------- */ + temp = (days - dayofyr) * 24.0; + hr = (int) floor(temp); + temp = (temp - hr) * 60.0; + minute = (int) floor(temp); + sec = (temp - minute) * 60.0; +} + +ReturnValue_t Sgp4Propagator::initialize(const uint8_t* line1, + const uint8_t* line2) { + + char longstr1[130]; + char longstr2[130]; + + //need some space for decimal points + memcpy(longstr1, line1, 69); + memcpy(longstr2, line2, 69); + + const double deg2rad = Math::PI / 180.0; // 0.0174532925199433 + const double xpdotp = 1440.0 / (2.0 * Math::PI); // 229.1831180523293 + + double sec, mu, radiusearthkm, tumin, xke, j2, j3, j4, j3oj2; + int cardnumb, numb, j; + long revnum = 0, elnum = 0; + char classification, intldesg[11]; + int year = 0; + int mon, day, hr, minute, nexp, ibexp; + + getgravconst(whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2); + + satrec.error = 0; + + // set the implied decimal points since doing a formated read + // fixes for bad input data values (missing, ...) + for (j = 10; j <= 15; j++) + if (longstr1[j] == ' ') + longstr1[j] = '_'; + + if (longstr1[44] != ' ') + longstr1[43] = longstr1[44]; + longstr1[44] = '.'; + if (longstr1[7] == ' ') + longstr1[7] = 'U'; + if (longstr1[9] == ' ') + longstr1[9] = '.'; + for (j = 45; j <= 49; j++) + if (longstr1[j] == ' ') + longstr1[j] = '0'; + if (longstr1[51] == ' ') + longstr1[51] = '0'; + if (longstr1[53] != ' ') + longstr1[52] = longstr1[53]; + longstr1[53] = '.'; + longstr2[25] = '.'; + for (j = 26; j <= 32; j++) + if (longstr2[j] == ' ') + longstr2[j] = '0'; + if (longstr1[62] == ' ') + longstr1[62] = '0'; + if (longstr1[68] == ' ') + longstr1[68] = '0'; + + sscanf(longstr1, + "%2d %5ld %1c %10s %2d %12lf %11lf %7lf %2d %7lf %2d %2d %6ld ", + &cardnumb, &satrec.satnum, &classification, intldesg, + &satrec.epochyr, &satrec.epochdays, &satrec.ndot, &satrec.nddot, + &nexp, &satrec.bstar, &ibexp, &numb, &elnum); + + if (longstr2[52] == ' ') { + sscanf(longstr2, "%2d %5ld %9lf %9lf %8lf %9lf %9lf %10lf %6ld \n", + &cardnumb, &satrec.satnum, &satrec.inclo, &satrec.nodeo, + &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no, &revnum); + } else { + sscanf(longstr2, "%2d %5ld %9lf %9lf %8lf %9lf %9lf %11lf %6ld \n", + &cardnumb, &satrec.satnum, &satrec.inclo, &satrec.nodeo, + &satrec.ecco, &satrec.argpo, &satrec.mo, &satrec.no, &revnum); + } + + // ---- find no, ndot, nddot ---- + satrec.no = satrec.no / xpdotp; //* rad/min + satrec.nddot = satrec.nddot * pow(10.0, nexp); + satrec.bstar = satrec.bstar * pow(10.0, ibexp); + + // ---- convert to sgp4 units ---- + satrec.a = pow(satrec.no * tumin, (-2.0 / 3.0)); + satrec.ndot = satrec.ndot / (xpdotp * 1440.0); //* ? * minperday + satrec.nddot = satrec.nddot / (xpdotp * 1440.0 * 1440); + + // ---- find standard orbital elements ---- + satrec.inclo = satrec.inclo * deg2rad; + satrec.nodeo = satrec.nodeo * deg2rad; + satrec.argpo = satrec.argpo * deg2rad; + satrec.mo = satrec.mo * deg2rad; + + satrec.alta = satrec.a * (1.0 + satrec.ecco) - 1.0; + satrec.altp = satrec.a * (1.0 - satrec.ecco) - 1.0; + + // ---------------------------------------------------------------- + // find sgp4epoch time of element set + // remember that sgp4 uses units of days from 0 jan 1950 (sgp4epoch) + // and minutes from the epoch (time) + // ---------------------------------------------------------------- + + // ---------------- temp fix for years from 1957-2056 ------------------- + // --------- correct fix will occur when year is 4-digit in tle --------- + if (satrec.epochyr < 57) { + year = satrec.epochyr + 2000; + } else { + year = satrec.epochyr + 1900; + } + + days2mdhms(year, satrec.epochdays, mon, day, hr, minute, sec); + jday(year, mon, day, hr, minute, sec, satrec.jdsatepoch); + + double unixSeconds = (satrec.jdsatepoch - 2451544.5) * 24 * 3600 + + 946684800; + + epoch.tv_sec = unixSeconds; + double subseconds = unixSeconds - epoch.tv_sec; + epoch.tv_usec = subseconds * 1000000; + + // ---------------- initialize the orbit at sgp4epoch ------------------- + uint8_t result = sgp4init(whichconst, satrec.satnum, + satrec.jdsatepoch + - 2433282.5 /*TODO verify, source says it's 2433281.5*/, + satrec.bstar, satrec.ecco, satrec.argpo, satrec.inclo, satrec.mo, + satrec.no, satrec.nodeo, satrec); + + if (result != 00) { + return MAKE_RETURN_CODE(result); + } else { + return HasReturnvaluesIF::RETURN_OK; + } + +} + +ReturnValue_t Sgp4Propagator::propagate(double* position, double* velocity, + timeval time, uint8_t gpsUtcOffset) { + + //Time since epoch in minutes + timeval timeSinceEpoch = time - epoch; + double minutesSinceEpoch = timeSinceEpoch.tv_sec / 60. + timeSinceEpoch.tv_usec / 60000000.; + + double yearsSinceEpoch = minutesSinceEpoch / 60 / 24 / 365; + + if ((yearsSinceEpoch > 1) || (yearsSinceEpoch < -1)) { + return TLE_TOO_OLD; + } + + double positionTEME[3]; + double velocityTEME[3]; + + uint8_t result = sgp4(whichconst, satrec, minutesSinceEpoch, positionTEME, + velocityTEME); + + VectorOperations::mulScalar(positionTEME, 1000, positionTEME, 3); + VectorOperations::mulScalar(velocityTEME, 1000, velocityTEME, 3); + + //Transform to ECF + double earthRotationMatrix[3][3]; + CoordinateTransformations transform(gpsUtcOffset); + transform.getEarthRotationMatrix(time, + earthRotationMatrix); + + MatrixOperations::multiply(earthRotationMatrix[0], positionTEME, + position, 3, 3, 1); + MatrixOperations::multiply(earthRotationMatrix[0], velocityTEME, + velocity, 3, 3, 1); + + double omegaEarth[3] = { 0, 0, Earth::OMEGA }; + double velocityCorrection[3]; + VectorOperations::cross(omegaEarth, position, velocityCorrection); + VectorOperations::subtract(velocity, velocityCorrection, velocity); + + if (result != 0) { + return MAKE_RETURN_CODE(result); + } else { + return HasReturnvaluesIF::RETURN_OK; + } +} diff --git a/coordinates/Sgp4Propagator.h b/coordinates/Sgp4Propagator.h new file mode 100644 index 00000000..f512924e --- /dev/null +++ b/coordinates/Sgp4Propagator.h @@ -0,0 +1,42 @@ +#ifndef SGP4PROPAGATOR_H_ +#define SGP4PROPAGATOR_H_ + +#include +#include +#include + +class Sgp4Propagator { +public: + static const uint8_t INTERFACE_ID = SGP4PROPAGATOR_CLASS; + static const ReturnValue_t INVALID_ECCENTRICITY = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t INVALID_MEAN_MOTION = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t INVALID_PERTURBATION_ELEMENTS = MAKE_RETURN_CODE(0x03); + static const ReturnValue_t INVALID_SEMI_LATUS_RECTUM = MAKE_RETURN_CODE(0x04); + static const ReturnValue_t INVALID_EPOCH_ELEMENTS = MAKE_RETURN_CODE(0x05); + static const ReturnValue_t SATELLITE_HAS_DECAYED = MAKE_RETURN_CODE(0x06); + static const ReturnValue_t TLE_TOO_OLD = MAKE_RETURN_CODE(0xA1); + + + + Sgp4Propagator(); + virtual ~Sgp4Propagator(); + + ReturnValue_t initialize(const uint8_t *line1, const uint8_t *line2); + + /** + * + * @param[out] position in ECF + * @param[out] velocity in ECF + * @param time to which to propagate + * @return + */ + ReturnValue_t propagate(double *position, double *velocity, timeval time, uint8_t gpsUtcOffset); + +private: + timeval epoch; + elsetrec satrec; + gravconsttype whichconst; + +}; + +#endif /* SGP4PROPAGATOR_H_ */ diff --git a/datalinklayer/BCFrame.h b/datalinklayer/BCFrame.h new file mode 100644 index 00000000..bcdf5300 --- /dev/null +++ b/datalinklayer/BCFrame.h @@ -0,0 +1,62 @@ +/** + * @file BCFrame.h + * @brief This file defines the BCFrame class. + * @date 24.04.2013 + * @author baetz + */ + +#ifndef BCFRAME_H_ +#define BCFRAME_H_ + +#include + +/** + * Small helper class to identify a BcFrame. + * @ingroup ccsds_handling + */ +class BcFrame: public CCSDSReturnValuesIF { +private: + static const uint8_t UNLOCK_COMMAND = 0b00000000;//! Identifier for a certain BC Command. + static const uint8_t SET_V_R_1 = 0b10000010;//! Identifier for a certain BC Command. + static const uint8_t SET_V_R_2 = 0b00000000;//! Identifier for a certain BC Command. + +public: + uint8_t byte1; //!< First content byte + uint8_t byte2; //!< Second content byte + uint8_t vR; //!< vR byte + /** + * Simple default constructor. + */ + BcFrame() : + byte1(0), byte2(0), vR(0) { + } + /** + * Main and only useful method of the class. + * With the buffer and size information passed, the class passes the content + * and checks if it is one of the two valid BC Command Frames. + * @param inBuffer Content of the frame to check, + * @param inSize Size of the data to check. + * @return - #BC_ILLEGAL_COMMAND if it is no command. + * - #BC_IS_UNLOCK_COMMAND if it is an unlock command. + * - #BC_IS_SET_VR_COMMAND if it is such. + */ + ReturnValue_t initialize(const uint8_t* inBuffer, uint16_t inSize) { + ReturnValue_t returnValue = BC_ILLEGAL_COMMAND; + if (inSize == 1) { + byte1 = inBuffer[0]; + if (byte1 == UNLOCK_COMMAND) { + returnValue = BC_IS_UNLOCK_COMMAND; + } + } else if (inSize == 3) { + byte1 = inBuffer[0]; + byte2 = inBuffer[1]; + vR = inBuffer[2]; + if (byte1 == SET_V_R_1 && byte2 == SET_V_R_2) { + returnValue = BC_IS_SET_VR_COMMAND; + } + } + return returnValue; + } +}; + +#endif /* BCFRAME_H_ */ diff --git a/datalinklayer/CCSDSReturnValuesIF.h b/datalinklayer/CCSDSReturnValuesIF.h new file mode 100644 index 00000000..50361f26 --- /dev/null +++ b/datalinklayer/CCSDSReturnValuesIF.h @@ -0,0 +1,57 @@ +/** + * @file CCSDSReturnValuesIF.h + * @brief This file defines the CCSDSReturnValuesIF class. + * @date 24.04.2013 + * @author baetz + */ + +#ifndef CCSDSRETURNVALUESIF_H_ +#define CCSDSRETURNVALUESIF_H_ + +#include +/** + * This is a helper class to collect special return values that come up during CCSDS Handling. + * @ingroup ccsds_handling + */ +class CCSDSReturnValuesIF: public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = CCSDS_HANDLER_IF; //!< Basic ID of the interface. + static const ReturnValue_t FRAME_OK = RETURN_OK; //!< A value to indicate that a frame is ok. + + static const ReturnValue_t BC_IS_SET_VR_COMMAND = MAKE_RETURN_CODE( 0x01 ); //!< A value to describe a BC frame. + static const ReturnValue_t BC_IS_UNLOCK_COMMAND = MAKE_RETURN_CODE( 0x02 ); //!< A value to describe a BC frame. + static const ReturnValue_t BC_ILLEGAL_COMMAND = MAKE_RETURN_CODE( 0xB0 );//!< A value to describe an illegal BC frame. + static const ReturnValue_t BOARD_READING_NOT_FINISHED = MAKE_RETURN_CODE( 0xB1 ); //! The CCSDS Board is not yet finished reading, it requires another cycle. + + static const ReturnValue_t NS_POSITIVE_W = MAKE_RETURN_CODE( 0xF0 );//!< NS is in the positive window + static const ReturnValue_t NS_NEGATIVE_W = MAKE_RETURN_CODE( 0xF1 );//!< NS is in the negative window + static const ReturnValue_t NS_LOCKOUT = MAKE_RETURN_CODE( 0xF2 ); //!< NS is in lockout state + static const ReturnValue_t FARM_IN_LOCKOUT = MAKE_RETURN_CODE( 0xF3 );//!< FARM-1 is currently in lockout state + static const ReturnValue_t FARM_IN_WAIT = MAKE_RETURN_CODE( 0xF4 ); //!< FARM-1 is currently in wait state + + static const ReturnValue_t WRONG_SYMBOL = MAKE_RETURN_CODE( 0xE0 ); //!< An error code in the FrameFinder. + static const ReturnValue_t DOUBLE_START = MAKE_RETURN_CODE( 0xE1 ); //!< An error code in the FrameFinder. + static const ReturnValue_t START_SYMBOL_MISSED = MAKE_RETURN_CODE( 0xE2 );//!< An error code in the FrameFinder. + static const ReturnValue_t END_WITHOUT_START = MAKE_RETURN_CODE( 0xE3 );//!< An error code in the FrameFinder. + static const ReturnValue_t TOO_LARGE = MAKE_RETURN_CODE( 0xE4 );//!< An error code for a frame. + static const ReturnValue_t TOO_SHORT = MAKE_RETURN_CODE( 0xE5 );//!< An error code for a frame. + static const ReturnValue_t WRONG_TF_VERSION = MAKE_RETURN_CODE( 0xE6 ); //!< An error code for a frame. + static const ReturnValue_t WRONG_SPACECRAFT_ID = MAKE_RETURN_CODE( 0xE7 );//!< An error code for a frame. + static const ReturnValue_t NO_VALID_FRAME_TYPE = MAKE_RETURN_CODE( 0xE8 );//!< An error code for a frame. + static const ReturnValue_t CRC_FAILED = MAKE_RETURN_CODE( 0xE9 );//!< An error code for a frame. + static const ReturnValue_t VC_NOT_FOUND = MAKE_RETURN_CODE( 0xEA ); //!< An error code for a frame. + static const ReturnValue_t FORWARDING_FAILED = MAKE_RETURN_CODE( 0xEB );//!< An error code for a frame. + static const ReturnValue_t CONTENT_TOO_LARGE = MAKE_RETURN_CODE( 0xEC );//!< An error code for a frame. + static const ReturnValue_t RESIDUAL_DATA = MAKE_RETURN_CODE( 0xED );//!< An error code for a frame. + static const ReturnValue_t DATA_CORRUPTED = MAKE_RETURN_CODE( 0xEE );//!< An error code for a frame. + static const ReturnValue_t ILLEGAL_SEGMENTATION_FLAG = MAKE_RETURN_CODE( 0xEF );//!< An error code for a frame. + static const ReturnValue_t ILLEGAL_FLAG_COMBINATION = MAKE_RETURN_CODE( 0xD0 ); //!< An error code for a frame. + static const ReturnValue_t SHORTER_THAN_HEADER = MAKE_RETURN_CODE( 0xD1 ); //!< An error code for a frame. + static const ReturnValue_t TOO_SHORT_BLOCKED_PACKET = MAKE_RETURN_CODE( 0xD2 ); //!< An error code for a frame. + static const ReturnValue_t TOO_SHORT_MAP_EXTRACTION = MAKE_RETURN_CODE( 0xD3 ); //!< An error code for a frame. + + virtual ~CCSDSReturnValuesIF() { + } //!< Empty virtual destructor +}; + +#endif /* CCSDSRETURNVALUESIF_H_ */ diff --git a/datalinklayer/Clcw.cpp b/datalinklayer/Clcw.cpp new file mode 100644 index 00000000..448d4d7b --- /dev/null +++ b/datalinklayer/Clcw.cpp @@ -0,0 +1,63 @@ +/** + * @file Clcw.cpp + * @brief This file defines the Clcw class. + * @date 17.04.2013 + * @author baetz + */ + + + +#include +#include + +Clcw::Clcw() { + content.raw = 0; + content.status = STATUS_FIELD_DEFAULT; +} + +Clcw::~Clcw() { +} + +void Clcw::setVirtualChannel(uint8_t setChannel) { + content.virtualChannelIdSpare = ((setChannel & 0x3F) << 2); +} + +void Clcw::setLockoutFlag(bool lockout) { + content.flags = (content.flags & LOCKOUT_FLAG_MASK) | (lockout << LOCKOUT_FLAG_POSITION); +} + +void Clcw::setWaitFlag(bool waitFlag) { + content.flags = (content.flags & WAIT_FLAG_MASK) | (waitFlag << WAIT_FLAG_POSITION); +} + +void Clcw::setRetransmitFlag(bool retransmitFlag) { + content.flags = (content.flags & RETRANSMIT_FLAG_MASK) | (retransmitFlag << RETRANSMIT_FLAG_POSITION); +} + +void Clcw::setFarmBCount(uint8_t count) { + content.flags = (content.flags & FARM_B_COUNT_MASK) | ((count & 0x03) << 1); +} + +void Clcw::setReceiverFrameSequenceNumber(uint8_t vR) { + content.vRValue = vR; +} + +uint32_t Clcw::getAsWhole() { + return content.raw; +} + +void Clcw::setRFAvailable(bool rfAvailable) { + content.flags = (content.flags & NO_RF_AVIALABLE_MASK) | (!rfAvailable << NO_RF_AVIALABLE_POSITION); +} + +void Clcw::setBitLock(bool bitLock) { + content.flags = (content.flags & NO_BIT_LOCK_MASK) | (!bitLock << NO_BIT_LOCK_POSITION); +} + +void Clcw::print() { + debug << "Clcw::print: Clcw is: " << std::hex << getAsWhole() << std::dec << std::endl; +} + +void Clcw::setWhole(uint32_t rawClcw) { + content.raw = rawClcw; +} diff --git a/datalinklayer/Clcw.h b/datalinklayer/Clcw.h new file mode 100644 index 00000000..f19f37b6 --- /dev/null +++ b/datalinklayer/Clcw.h @@ -0,0 +1,66 @@ +/** + * @file Clcw.h + * @brief This file defines the Clcw class. + * @date 17.04.2013 + * @author baetz + */ + +#ifndef CLCW_H_ +#define CLCW_H_ + +#include +/** + * Small helper method to handle the Clcw values. + * It has a content struct that manages the register and can be set externally. + * @ingroup ccsds_handling + */ +class Clcw : public ClcwIF { +private: + static const uint8_t STATUS_FIELD_DEFAULT = 0b00000001; //!< Default for the status field. + static const uint8_t NO_RF_AVIALABLE_POSITION = 7; //!< Position of a flag in the register (starting with 0). + static const uint8_t NO_BIT_LOCK_POSITION = 6; //!< Position of a flag in the register (starting with 0). + static const uint8_t LOCKOUT_FLAG_POSITION = 5; //!< Position of a flag in the register (starting with 0). + static const uint8_t WAIT_FLAG_POSITION = 4; //!< Position of a flag in the register (starting with 0). + static const uint8_t RETRANSMIT_FLAG_POSITION = 3; //!< Position of a flag in the register (starting with 0). + static const uint8_t NO_RF_AVIALABLE_MASK = 0xFF xor (1 << NO_RF_AVIALABLE_POSITION); //!< Mask for a flag in the register. + static const uint8_t NO_BIT_LOCK_MASK = 0xFF xor (1 << NO_BIT_LOCK_POSITION); //!< Mask for a flag in the register. + static const uint8_t LOCKOUT_FLAG_MASK = 0xFF xor (1 << LOCKOUT_FLAG_POSITION); //!< Mask for a flag in the register. + static const uint8_t WAIT_FLAG_MASK = 0xFF xor (1 << WAIT_FLAG_POSITION); //!< Mask for a flag in the register. + static const uint8_t RETRANSMIT_FLAG_MASK = 0xFF xor (1 << RETRANSMIT_FLAG_POSITION); //!< Mask for a flag in the register. + static const uint8_t FARM_B_COUNT_MASK = 0b11111001; //!< Mask for a counter in the register. + /** + * This is the data structure of the CLCW register. + */ + union clcwContent { + uint32_t raw; + struct { + uint8_t status; + uint8_t virtualChannelIdSpare; + uint8_t flags; + uint8_t vRValue; + }; + }; + clcwContent content; //!< Represents the content of the register. +public: + /** + * The constructor sets everything to default values. + */ + Clcw(); + /** + * Nothing happens in the destructor. + */ + ~Clcw(); + void setVirtualChannel( uint8_t setChannel ); + void setLockoutFlag( bool lockout ); + void setWaitFlag( bool waitFlag ); + void setRetransmitFlag( bool retransmitFlag ); + void setFarmBCount( uint8_t count ); + void setReceiverFrameSequenceNumber( uint8_t vR ); + void setRFAvailable( bool rfAvailable ); + void setBitLock( bool bitLock ); + uint32_t getAsWhole(); + void setWhole( uint32_t rawClcw ); + void print(); +}; + +#endif /* CLCW_H_ */ diff --git a/datalinklayer/ClcwIF.h b/datalinklayer/ClcwIF.h new file mode 100644 index 00000000..fc517856 --- /dev/null +++ b/datalinklayer/ClcwIF.h @@ -0,0 +1,70 @@ +/** + * @file ClcwIF.h + * @brief This file defines the ClcwIF class. + * @date 17.04.2013 + * @author baetz + */ + +#ifndef CLCWIF_H_ +#define CLCWIF_H_ + +#include + +/** + * Interface to manage a CLCW register. + * @ingroup ccsds_handling + */ +class ClcwIF { +public: + /** + * Empty virtual destructor. + */ + virtual ~ClcwIF() { } + /** + * Simple setter. + * @param setChannel The virtual channel id to set. + */ + virtual void setVirtualChannel( uint8_t setChannel ) = 0; + /** + * Simple setter. + * @param lockout status of the flag. + */ + virtual void setLockoutFlag( bool lockout ) = 0; + /** + * Simple setter. + * @param waitFlag status of the flag. + */ + virtual void setWaitFlag( bool waitFlag ) = 0; + /** + * Simple setter. + * @param retransmitFlag status of the flag. + */ + virtual void setRetransmitFlag( bool retransmitFlag ) = 0; + /** + * Sets the farm B count. + * @param count A full 8bit counter value can be passed. Only the last three bits are used. + */ + virtual void setFarmBCount( uint8_t count ) = 0; + /** + * Simple setter. + * @param vR Value of vR. + */ + virtual void setReceiverFrameSequenceNumber( uint8_t vR ) = 0; + /** + * Returns the register as a full 32bit value. + * @return The value. + */ + virtual uint32_t getAsWhole() = 0; + /** + * Sets the whole content to this value. + * @param rawClcw The value to set the content. + */ + virtual void setWhole( uint32_t rawClcw ) = 0; + /** + * Debug method to print the CLCW. + */ + virtual void print() = 0; +}; + + +#endif /* CLCWIF_H_ */ diff --git a/datalinklayer/DataLinkLayer.cpp b/datalinklayer/DataLinkLayer.cpp new file mode 100644 index 00000000..de04ae59 --- /dev/null +++ b/datalinklayer/DataLinkLayer.cpp @@ -0,0 +1,147 @@ +/* + * DataLinkLayer.cpp + * + * Created on: 02.03.2012 + * Author: baetz + */ + +#include +#include +#include +#include + +DataLinkLayer::DataLinkLayer(uint8_t* set_frame_buffer, ClcwIF* setClcw, + uint8_t set_start_sequence_length, uint16_t set_scid) : + spacecraftId(set_scid), frameBuffer(set_frame_buffer), clcw(setClcw), receivedDataLength(0), currentFrame( + NULL), startSequenceLength(set_start_sequence_length) { + //Nothing to do except from setting the values above. +} + +DataLinkLayer::~DataLinkLayer() { + +} + +ReturnValue_t DataLinkLayer::frameDelimitingAndFillRemoval() { + if ((receivedDataLength - startSequenceLength) < FRAME_PRIMARY_HEADER_LENGTH) { + return SHORTER_THAN_HEADER; + } + //Removing start sequence + //TODO: What does the start sequence look like? Better search for the pattern. + while ( *frameBuffer == START_SEQUENCE_PATTERN ) { + frameBuffer++; + } + TcTransferFrame frame_candidate(frameBuffer); + this->currentFrame = frame_candidate; //should work with shallow copy. + + return FRAME_OK; +} + +ReturnValue_t DataLinkLayer::frameValidationCheck() { + //Check TF_version number + if (this->currentFrame.getVersionNumber() != FRAME_VERSION_NUMBER_DEFAULT) { + return WRONG_TF_VERSION; + } + //Check SpaceCraft ID + if (this->currentFrame.getSpacecraftId() != this->spacecraftId) { + return WRONG_SPACECRAFT_ID; + } + //Check other header limitations: + if (!this->currentFrame.bypassFlagSet() && this->currentFrame.controlCommandFlagSet()) { + return NO_VALID_FRAME_TYPE; + } + //- Spares are zero + if (!this->currentFrame.spareIsZero()) { + return NO_VALID_FRAME_TYPE; + } + //Compare detected frame length with the one in the header + uint16_t length = currentFrame.getFullSize(); + if (length > receivedDataLength) { + //Frame is too long or just right +// error << "frameValidationCheck: Too short."; +// currentFrame.print(); + return TOO_SHORT; + } + if (USE_CRC) { + return this->frameCheckCRC(); + } + return FRAME_OK; +} + +ReturnValue_t DataLinkLayer::frameCheckCRC() { + uint16_t checkValue = ::Calculate_CRC(this->currentFrame.getFullFrame(), + this->currentFrame.getFullSize()); + if (checkValue == 0) { + return FRAME_OK; + } else { + return CRC_FAILED; + } + +} + +ReturnValue_t DataLinkLayer::allFramesReception() { + ReturnValue_t status = this->frameDelimitingAndFillRemoval(); + if (status != FRAME_OK) { + return status; + } + return this->frameValidationCheck(); +} + +ReturnValue_t DataLinkLayer::masterChannelDemultiplexing() { + //Nothing to do at present. Ideally, there would be a map of MCID's identifying which MC to use. + return virtualChannelDemultiplexing(); +} + +ReturnValue_t DataLinkLayer::virtualChannelDemultiplexing() { + uint8_t vcId = currentFrame.getVirtualChannelId(); + virtualChannelIterator iter = virtualChannels.find(vcId); + if (iter == virtualChannels.end()) { + //Do not report because passive board will get this error all the time. + return FRAME_OK; + } else { + return (iter->second)->frameAcceptanceAndReportingMechanism(¤tFrame, clcw); + } +} + +ReturnValue_t DataLinkLayer::processFrame(uint16_t length) { + receivedDataLength = length; + ReturnValue_t status = allFramesReception(); + if (status != FRAME_OK) { + error << "DataLinkLayer::processFrame: frame reception failed. Error code: " << std::hex + << status << std::dec << std::endl; +// currentFrame.print(); + return status; + } else { + return masterChannelDemultiplexing(); + } +} + +ReturnValue_t DataLinkLayer::addVirtualChannel(uint8_t virtualChannelId, + VirtualChannelReceptionIF* object) { + std::pair returnValue = virtualChannels.insert( + std::pair(virtualChannelId, object)); + if (returnValue.second == true) { + return RETURN_OK; + } else { + return RETURN_FAILED; + } +} + +ReturnValue_t DataLinkLayer::initialize() { + ReturnValue_t returnValue = RETURN_FAILED; + //Set Virtual Channel ID to first virtual channel instance in this DataLinkLayer instance to avoid faulty information (e.g. 0) in the VCID. + if ( virtualChannels.begin() != virtualChannels.end() ) { + clcw->setVirtualChannel( virtualChannels.begin()->second->getChannelId() ); + } else { + error << "DataLinkLayer::initialize: No VC assigned to this DLL instance! " << std::endl; + return RETURN_FAILED; + } + + for (virtualChannelIterator iterator = virtualChannels.begin(); + iterator != virtualChannels.end(); iterator++) { + returnValue = iterator->second->initialize(); + if (returnValue != RETURN_OK) + break; + } + return returnValue; + +} diff --git a/datalinklayer/DataLinkLayer.h b/datalinklayer/DataLinkLayer.h new file mode 100644 index 00000000..a9ac825a --- /dev/null +++ b/datalinklayer/DataLinkLayer.h @@ -0,0 +1,119 @@ +/* + * DataLinkLayer.h + * + * Created on: Feb 29, 2012 + * Author: baetz + */ + +#ifndef DATALINKLAYER_H_ +#define DATALINKLAYER_H_ + +#include +#include +#include +#include +#include +#include + + +class VirtualChannelReception; +/** + * A complete representation of the CCSDS Data Link Layer. + * The operations of this layer are defined in the CCSDS TC Space Data Link Protocol + * document. It is configured to handle a VC Demultiplexing function. All reception + * steps are performed. + */ +class DataLinkLayer : public CCSDSReturnValuesIF { +public: + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_1; + static const Event RF_AVAILABLE = MAKE_EVENT(0, SEVERITY::INFO); //!< The CCSDS Board detected a RF available signal. + static const Event RF_LOST = MAKE_EVENT(1, SEVERITY::INFO); //!< The CCSDS Board lost a previously found RF available signal. + static const Event BIT_LOCK = MAKE_EVENT(2, SEVERITY::INFO); //!< The CCSDS Board detected a Bit Lock signal. + static const Event BIT_LOCK_LOST = MAKE_EVENT(3, SEVERITY::INFO); //!< The CCSDS Board lost a previously found Bit Lock signal. + static const Event RF_CHAIN_LOST = MAKE_EVENT(4, SEVERITY::INFO); //!< The CCSDS Board detected that either bit lock or RF available or both are lost. + static const Event FRAME_PROCESSING_FAILED = MAKE_EVENT(5, SEVERITY::LOW); //!< The CCSDS Board could not interpret a TC + /** + * The Constructor sets the passed parameters and nothing else. + * @param set_frame_buffer The buffer in which incoming frame candidates are stored. + * @param setClcw The CLCW class to work on when returning CLCW information. + * @param set_start_sequence_length Length of the Start sequence in front of every TC Transfer Frame. + * @param set_scid The SCID to operate on. + */ + DataLinkLayer( uint8_t* set_frame_buffer, ClcwIF* setClcw, uint8_t set_start_sequence_length, uint16_t set_scid ); + /** + * Empty virtual destructor. + */ + ~DataLinkLayer(); + /** + * This method tries to process a frame that is placed in #frameBuffer. + * The procedures described in the Standard are performed. + * @param length Length of the incoming frame candidate. + * @return @c RETURN_OK on successful handling, otherwise the return codes of the higher methods. + */ + ReturnValue_t processFrame( uint16_t length ); + /** + * Configuration method to add a new TC Virtual Channel. + * Shall only be called during initialization. As soon as the method was called, the layer can + * handle Frames directed to this VC. + * @param virtualChannelId Id of the VC. Shall be smaller than 64. + * @param object Reference to the object that handles the Frame. + * @return @c RETURN_OK on success, @c RETURN_FAILED otherwise. + */ + ReturnValue_t addVirtualChannel( uint8_t virtualChannelId, VirtualChannelReceptionIF* object ); + /** + * The initialization method calls the @c initialize routine of all virtual channels. + * @return The return code of the first failed VC initialization or @c RETURN_OK. + */ + ReturnValue_t initialize(); +private: + typedef std::map::iterator virtualChannelIterator; //!< Typedef to simplify handling the #virtualChannels map. + static const uint8_t FRAME_VERSION_NUMBER_DEFAULT = 0x00; //!< Constant for the default value of Frame Version Numbers. + static const uint8_t FRAME_PRIMARY_HEADER_LENGTH = 5; //!< Length of the frame's primary header. + static const uint8_t START_SEQUENCE_PATTERN = 0x00; //!< The start sequence pattern which might be with the frame. + static const bool USE_CRC = true; //!< A global, so called "Managed Parameter" that identifies if incoming frames have CRC's or not. + uint16_t spacecraftId; //!< The Space Craft Identifier (SCID) configured. + uint8_t* frameBuffer; //!< A pointer to point to the current incoming frame. + ClcwIF* clcw; //!< Pointer to store the CLCW to work on. + uint16_t receivedDataLength; //!< Stores the length of the currently processed frame. + TcTransferFrame currentFrame; //!< Stores a more convenient access to the current frame. + uint8_t startSequenceLength; //!< Configured length of the start sequence. Maybe this must be done more variable. + std::map virtualChannels; //!< Map of all virtual channels assigned. + /** + * Method that performs all possible frame validity checks (as specified). + * @return Various error codes or @c FRAME_OK on success. + */ + ReturnValue_t frameValidationCheck(); + /** + * First method to call. + * Removes start sequence bytes and checks if the complete frame was received. + * TODO: Maybe handling the start sequence must be done more variable. + * @return @c FRAME_OK or @c TOO_SHORT. + */ + ReturnValue_t frameDelimitingAndFillRemoval(); + /** + * Small helper method to check the CRC of the Frame. + * @return @c FRAME_OK or @c CRC_FAILED. + */ + ReturnValue_t frameCheckCRC(); + /** + * Method that groups the reception process of all Frames. + * Calls #frameDelimitingAndFillRemoval and #frameValidationCheck. + * @return The return codes of the sub calls. + */ + ReturnValue_t allFramesReception(); + /** + * Dummy method for master channel demultiplexing. + * As there's only one Master Channel here, the method calls #virtualChannelDemultiplexing. + * @return The return codes of #virtualChannelDemultiplexing. + */ + ReturnValue_t masterChannelDemultiplexing(); + /** + * Method to demultiplex the Frames to Virtual Channels (VC's). + * Looks up the requested VC in #virtualChannels and forwards the Frame to its + * #frameAcceptanceAndReportingMechanism method, if found. + * @return The higher method codes or @c VC_NOT_FOUND. + */ + ReturnValue_t virtualChannelDemultiplexing(); +}; + +#endif /* DATALINKLAYER_H_ */ diff --git a/datalinklayer/Farm1StateIF.h b/datalinklayer/Farm1StateIF.h new file mode 100644 index 00000000..25836e6a --- /dev/null +++ b/datalinklayer/Farm1StateIF.h @@ -0,0 +1,54 @@ +/** + * @file Farm1StateIF.h + * @brief This file defines the Farm1StateIF class. + * @date 24.04.2013 + * @author baetz + */ + +#ifndef FARM1STATEIF_H_ +#define FARM1STATEIF_H_ + +#include +class VirtualChannelReception; +class TcTransferFrame; +class ClcwIF; + +/** + * This is the interface for states of the FARM-1 state machine. + * Classes implementing this interface can be used as FARM-1 states. This is a simple implementation + * of the state pattern. + */ +class Farm1StateIF : public CCSDSReturnValuesIF { +public: + /** + * A method that shall handle an incoming frame as AD Frame. + * @param frame The frame to handle. + * @param clcw Any changes to the CLCW shall be done with the help of this interface. + * @return If forwarding to a MAP Channel is required, the return value shall be #FRAME_OK. + * Otherwise, an appropriate return value or error code shall be generated. + */ + virtual ReturnValue_t handleADFrame( TcTransferFrame* frame, ClcwIF* clcw ) = 0; + /** + * This method shall handle frames that have been successfully identified as BC Unlock frames. + * @param clcw Any changes to the CLCW shall be done with the help of this interface. + * @return If forwarding to a MAP Channel is required, the return value shall be #FRAME_OK. + * Otherwise, an appropriate return value or error code shall be generated. + */ + virtual ReturnValue_t handleBCUnlockCommand( ClcwIF* clcw ) = 0; + /** + * This method shall handle frames that have been successfully identified as BC Set VR frames. + * @param clcw Any changes to the CLCW shall be done with the help of this interface. + * @param vr The V(r) value found in the frame. + * @return If forwarding to a MAP Channel is required, the return value shall be #FRAME_OK. + * Otherwise, an appropriate return value or error code shall be generated. + */ + virtual ReturnValue_t handleBCSetVrCommand( ClcwIF* clcw, uint8_t vr ) = 0; + /** + * Empty virtual destructor. + */ + virtual ~Farm1StateIF() { + + } +}; + +#endif /* FARM1STATEIF_H_ */ diff --git a/datalinklayer/Farm1StateLockout.cpp b/datalinklayer/Farm1StateLockout.cpp new file mode 100644 index 00000000..45ae47f9 --- /dev/null +++ b/datalinklayer/Farm1StateLockout.cpp @@ -0,0 +1,35 @@ +/** + * @file Farm1StateLockout.cpp + * @brief This file defines the Farm1StateLockout class. + * @date 24.04.2013 + * @author baetz + */ + + + +#include +#include +#include +#include +Farm1StateLockout::Farm1StateLockout(VirtualChannelReception* setMyVC) : myVC(setMyVC) { +} + +ReturnValue_t Farm1StateLockout::handleADFrame(TcTransferFrame* frame, + ClcwIF* clcw) { + return FARM_IN_LOCKOUT; +} + +ReturnValue_t Farm1StateLockout::handleBCUnlockCommand(ClcwIF* clcw) { + myVC->farmBCounter++; + clcw->setRetransmitFlag(false); + clcw->setLockoutFlag( false ); + clcw->setWaitFlag( false ); + myVC->currentState = &(myVC->openState); + return BC_IS_UNLOCK_COMMAND; +} + +ReturnValue_t Farm1StateLockout::handleBCSetVrCommand(ClcwIF* clcw, + uint8_t vr) { + myVC->farmBCounter++; + return BC_IS_SET_VR_COMMAND; +} diff --git a/datalinklayer/Farm1StateLockout.h b/datalinklayer/Farm1StateLockout.h new file mode 100644 index 00000000..aac8ed74 --- /dev/null +++ b/datalinklayer/Farm1StateLockout.h @@ -0,0 +1,59 @@ +/** + * @file Farm1StateLockout.h + * @brief This file defines the Farm1StateLockout class. + * @date 24.04.2013 + * @author baetz + */ + +#ifndef FARM1STATELOCKOUT_H_ +#define FARM1STATELOCKOUT_H_ + +#include + +/** + * This class represents the FARM-1 "Lockout" State. + * The Lockout state is reached if the received Transfer Frame Sequence Number is completely wrong + * (i.e. within the Lockout Window). No AD Frames are forwarded. To leave the State, a BC Unlock + * command is required. + */ +class Farm1StateLockout : public Farm1StateIF { +private: + /** + * This is a reference to the "owner" class the State works on. + */ + VirtualChannelReception* myVC; +public: + /** + * The default constructor if the State. + * Sets the "owner" of the State. + * @param setMyVC The "owner" class. + */ + Farm1StateLockout( VirtualChannelReception* setMyVC ); + /** + * All AD Frames are rejected with FARM_IN_LOCKOUT + * @param frame The frame to handle. + * @param clcw Any changes to the CLCW shall be done with the help of this interface. + * @return FARM_IN_LOCKOUT + */ + ReturnValue_t handleADFrame( TcTransferFrame* frame, ClcwIF* clcw ); + /** + * These commands are handled as specified. + * State changes to Farm1StateOpen. + * @param clcw Any changes to the CLCW shall be done with the help of this interface. + * @return As the frame needs no forwarding to a MAP Channel, #BC_IS_UNLOCK_COMMAND + * is returned. + */ + ReturnValue_t handleBCUnlockCommand( ClcwIF* clcw ); + /** + * These commands are handled as specified. + * The V(r) value is not set in Lockout State, even though the Command itself is accepted. + * @param clcw Any changes to the CLCW shall be done with the help of this interface. + * @param vr The V(r) value found in the frame. + * @return As the frame needs no forwarding to a MAP Channel, #BC_IS_SET_VR_COMMAND + * is returned. + */ + ReturnValue_t handleBCSetVrCommand( ClcwIF* clcw, uint8_t vr ); +}; + + +#endif /* FARM1STATELOCKOUT_H_ */ diff --git a/datalinklayer/Farm1StateOpen.cpp b/datalinklayer/Farm1StateOpen.cpp new file mode 100644 index 00000000..d0f847ae --- /dev/null +++ b/datalinklayer/Farm1StateOpen.cpp @@ -0,0 +1,49 @@ +/** + * @file Farm1StateOpen.cpp + * @brief This file defines the Farm1StateOpen class. + * @date 24.04.2013 + * @author baetz + */ + + + + +#include +#include +#include +#include + +Farm1StateOpen::Farm1StateOpen(VirtualChannelReception* setMyVC) : myVC(setMyVC) { +} + +ReturnValue_t Farm1StateOpen::handleADFrame(TcTransferFrame* frame, + ClcwIF* clcw) { + int8_t diff = frame->getSequenceNumber() - myVC->vR; + if (diff == 0 ) { + myVC->vR++; + clcw->setRetransmitFlag(false); + return FRAME_OK; + } else if (diff < myVC->positiveWindow && diff > 0 ) { + clcw->setRetransmitFlag(true); + return NS_POSITIVE_W; + } else if (diff < 0 && diff >= -myVC->negativeWindow) { + return NS_NEGATIVE_W; + } else { + clcw->setLockoutFlag(true); + myVC->currentState = &(myVC->lockoutState); + return NS_LOCKOUT; + } +} + +ReturnValue_t Farm1StateOpen::handleBCUnlockCommand( ClcwIF* clcw ) { + myVC->farmBCounter++; + clcw->setRetransmitFlag(false); + return BC_IS_UNLOCK_COMMAND; +} + +ReturnValue_t Farm1StateOpen::handleBCSetVrCommand( ClcwIF* clcw, uint8_t vr ) { + myVC->farmBCounter++; + clcw->setRetransmitFlag(false); + myVC->vR = vr; + return BC_IS_SET_VR_COMMAND; +} diff --git a/datalinklayer/Farm1StateOpen.h b/datalinklayer/Farm1StateOpen.h new file mode 100644 index 00000000..f3d9a66e --- /dev/null +++ b/datalinklayer/Farm1StateOpen.h @@ -0,0 +1,62 @@ +/** + * @file Farm1StateOpen.h + * @brief This file defines the Farm1StateOpen class. + * @date 24.04.2013 + * @author baetz + */ + +#ifndef FARM1STATEOPEN_H_ +#define FARM1STATEOPEN_H_ + +#include + +/** + * This class represents the FARM-1 "Open" State. + * The Open state is the state of normal operation. It handles all types of frames, + * including AD Frames. If a wrong Frame Sequence Number is detected in an AD Frame, the + * State reacts as specified. + */ +class Farm1StateOpen : public Farm1StateIF { +private: + /** + * This is a reference to the "owner" class the State works on. + */ + VirtualChannelReception* myVC; +public: + /** + * The default constructor if the State. + * Sets the "owner" of the State. + * @param setMyVC The "owner" class. + */ + Farm1StateOpen( VirtualChannelReception* setMyVC ); + /** + * Method to check the validity of AD Frames. + * It checks the Frame Sequence Number and reacts as specified in the standard. The state may + * change to Farm1StateLockout. + * @param frame The frame to handle. + * @param clcw Any changes to the CLCW shall be done with the help of this interface. + * @return If the Sequence Number is ok, it returns #FRAME_OK. Otherwise either #NS_POSITIVE_W, + * #NS_NEGATIVE_W or NS_LOCKOUT is returned. + */ + ReturnValue_t handleADFrame( TcTransferFrame* frame, ClcwIF* clcw ); + /** + * These commands are handled as specified. + * State does not change. + * @param clcw Any changes to the CLCW shall be done with the help of this interface. + * @return As the frame needs no forwarding to a MAP Channel, #BC_IS_UNLOCK_COMMAND + * is returned. + */ + ReturnValue_t handleBCUnlockCommand( ClcwIF* clcw ); + /** + * These commands are handled as specified. + * State does not change. + * @param clcw Any changes to the CLCW shall be done with the help of this interface. + * @param vr The V(r) value found in the frame. + * @return As the frame needs no forwarding to a MAP Channel, #BC_IS_SET_VR_COMMAND + * is returned. + */ + ReturnValue_t handleBCSetVrCommand( ClcwIF* clcw, uint8_t vr ); +}; + + +#endif /* FARM1STATEOPEN_H_ */ diff --git a/datalinklayer/Farm1StateWait.cpp b/datalinklayer/Farm1StateWait.cpp new file mode 100644 index 00000000..1c72e68f --- /dev/null +++ b/datalinklayer/Farm1StateWait.cpp @@ -0,0 +1,43 @@ +/** + * @file Farm1StateWait.cpp + * @brief This file defines the Farm1StateWait class. + * @date 24.04.2013 + * @author baetz + */ + + +#include +#include +#include +#include + +Farm1StateWait::Farm1StateWait(VirtualChannelReception* setMyVC) : myVC(setMyVC) { +} + +ReturnValue_t Farm1StateWait::handleADFrame(TcTransferFrame* frame, + ClcwIF* clcw) { + + int8_t diff = frame->getSequenceNumber() - myVC->vR; + if ( diff < -myVC->negativeWindow || diff >= myVC->positiveWindow ) { + clcw->setLockoutFlag(true); + myVC->currentState = &(myVC->lockoutState); + } + return FARM_IN_WAIT; +} + +ReturnValue_t Farm1StateWait::handleBCUnlockCommand(ClcwIF* clcw) { + myVC->farmBCounter++; + clcw->setRetransmitFlag(false); + clcw->setWaitFlag( false ); + myVC->currentState = &(myVC->openState); + return BC_IS_UNLOCK_COMMAND; +} + +ReturnValue_t Farm1StateWait::handleBCSetVrCommand(ClcwIF* clcw, uint8_t vr) { + myVC->farmBCounter++; + clcw->setWaitFlag( false ); + clcw->setRetransmitFlag(false); + myVC->vR = vr; + myVC->currentState = &(myVC->openState); + return BC_IS_SET_VR_COMMAND; +} diff --git a/datalinklayer/Farm1StateWait.h b/datalinklayer/Farm1StateWait.h new file mode 100644 index 00000000..f1201791 --- /dev/null +++ b/datalinklayer/Farm1StateWait.h @@ -0,0 +1,58 @@ +/** + * @file Farm1StateWait.h + * @brief This file defines the Farm1StateWait class. + * @date 24.04.2013 + * @author baetz + */ + +#ifndef FARM1STATEWAIT_H_ +#define FARM1STATEWAIT_H_ + +#include + +/** + * This class represents the FARM-1 "Wait" State. + * The Wait state is reached if higher level procedures inform the FARM-1 Machine to wait + * for a certain period. Currently, it is not in use. + */ +class Farm1StateWait : public Farm1StateIF { +private: + /** + * This is a reference to the "owner" class the State works on. + */ + VirtualChannelReception* myVC; +public: + /** + * The default constructor if the State. + * Sets the "owner" of the State. + * @param setMyVC The "owner" class. + */ + Farm1StateWait( VirtualChannelReception* setMyVC ); + /** + * AD Frames are always discarded. + * If the frame number is in the lockout window, the state changes to Farm1StateLockout. + * @param frame The frame to handle. + * @param clcw Any changes to the CLCW shall be done with the help of this interface. + * @return Always returns FARM_IN_WAIT. + */ + ReturnValue_t handleADFrame( TcTransferFrame* frame, ClcwIF* clcw ); + /** + * These commands are handled as specified. + * State changes to Farm1StateOpen. + * @param clcw Any changes to the CLCW shall be done with the help of this interface. + * @return As the frame needs no forwarding to a MAP Channel, #BC_IS_UNLOCK_COMMAND + * is returned. + */ + ReturnValue_t handleBCUnlockCommand( ClcwIF* clcw ); + /** + * These commands are handled as specified. + * @param clcw Any changes to the CLCW shall be done with the help of this interface. + * @param vr The V(r) value found in the frame. + * @return As the frame needs no forwarding to a MAP Channel, #BC_IS_SET_VR_COMMAND + * is returned. + */ + ReturnValue_t handleBCSetVrCommand( ClcwIF* clcw, uint8_t vr ); +}; + + +#endif /* FARM1STATEWAIT_H_ */ diff --git a/datalinklayer/MapPacketExtraction.cpp b/datalinklayer/MapPacketExtraction.cpp new file mode 100644 index 00000000..99c47464 --- /dev/null +++ b/datalinklayer/MapPacketExtraction.cpp @@ -0,0 +1,152 @@ +/** + * @file MapPacketExtraction.cpp + * @brief This file defines the MapPacketExtraction class. + * @date 26.03.2013 + * @author baetz + */ + +#include +#include +#include +#include +#include +#include + +MapPacketExtraction::MapPacketExtraction(uint8_t setMapId, + object_id_t setPacketDestination) : + lastSegmentationFlag(NO_SEGMENTATION), mapId(setMapId), packetLength(0), bufferPosition( + packetBuffer), packetDestination(setPacketDestination), packetStore( + NULL) { + memset(packetBuffer, 0, sizeof(packetBuffer)); +} + +ReturnValue_t MapPacketExtraction::extractPackets(TcTransferFrame* frame) { + uint8_t segmentationFlag = frame->getSequenceFlags(); + ReturnValue_t status = TOO_SHORT_MAP_EXTRACTION; + switch (segmentationFlag) { + case NO_SEGMENTATION: + status = unpackBlockingPackets(frame); + break; + case FIRST_PORTION: + packetLength = frame->getDataLength(); + if (packetLength <= MAX_PACKET_SIZE) { + memcpy(packetBuffer, frame->getDataField(), packetLength); + bufferPosition = &packetBuffer[packetLength]; + status = FRAME_OK; + } else { + error + << "MapPacketExtraction::extractPackets. Packet too large! Size: " + << packetLength << std::endl; + clearBuffers(); + status = CONTENT_TOO_LARGE; + } + break; + case CONTINUING_PORTION: + case LAST_PORTION: + if (lastSegmentationFlag == FIRST_PORTION + || lastSegmentationFlag == CONTINUING_PORTION) { + packetLength += frame->getDataLength(); + if (packetLength <= MAX_PACKET_SIZE) { + memcpy(bufferPosition, frame->getDataField(), + frame->getDataLength()); + bufferPosition = &packetBuffer[packetLength]; + if (segmentationFlag == LAST_PORTION) { + status = sendCompletePacket(packetBuffer, packetLength); + clearBuffers(); + } + status = FRAME_OK; + } else { + error + << "MapPacketExtraction::extractPackets. Packet too large! Size: " + << packetLength << std::endl; + clearBuffers(); + status = CONTENT_TOO_LARGE; + } + } else { + error + << "MapPacketExtraction::extractPackets. Illegal segment! Last flag: " + << (int) lastSegmentationFlag << std::endl; + clearBuffers(); + status = ILLEGAL_SEGMENTATION_FLAG; + } + break; + default: + error + << "MapPacketExtraction::extractPackets. Illegal segmentationFlag: " + << (int) segmentationFlag << std::endl; + clearBuffers(); + status = DATA_CORRUPTED; + break; + } + lastSegmentationFlag = segmentationFlag; + return status; +} + +ReturnValue_t MapPacketExtraction::unpackBlockingPackets( + TcTransferFrame* frame) { + ReturnValue_t status = TOO_SHORT_BLOCKED_PACKET; + uint32_t totalLength = frame->getDataLength(); + if (totalLength > MAX_PACKET_SIZE) + return CONTENT_TOO_LARGE; + uint8_t* position = frame->getDataField(); + while ((totalLength > SpacePacketBase::MINIMUM_SIZE)) { + SpacePacketBase packet(position); + uint32_t packetSize = packet.getFullSize(); + if (packetSize <= totalLength) { + status = sendCompletePacket(packet.getWholeData(), + packet.getFullSize()); + totalLength -= packet.getFullSize(); + position += packet.getFullSize(); + status = FRAME_OK; + } else { + status = DATA_CORRUPTED; + totalLength = 0; + } + } + if (totalLength > 0) { + status = RESIDUAL_DATA; + } + return status; +} + +ReturnValue_t MapPacketExtraction::sendCompletePacket(uint8_t* data, + uint32_t size) { + store_address_t store_id; + ReturnValue_t status = this->packetStore->addData(&store_id, data, size); + if (status == RETURN_OK) { + TmTcMessage message(store_id); + status = this->tcQueue.sendToDefault(&message); + } + return status; +} + +void MapPacketExtraction::clearBuffers() { + memset(packetBuffer, 0, sizeof(packetBuffer)); + bufferPosition = packetBuffer; + packetLength = 0; + lastSegmentationFlag = NO_SEGMENTATION; +} + +ReturnValue_t MapPacketExtraction::initialize() { + packetStore = objectManager->get(objects::TC_STORE); + AcceptsTelecommandsIF* distributor = objectManager->get< + AcceptsTelecommandsIF>(packetDestination); + if ((packetStore != NULL) && (distributor != NULL)) { + tcQueue.setDefaultDestination(distributor->getRequestQueue()); + return RETURN_OK; + } else { + return RETURN_FAILED; + } +} + +void MapPacketExtraction::printPacketBuffer(void) { + debug << "DLL: packet_buffer contains: " << std::endl; + for (uint32_t i = 0; i < this->packetLength; ++i) { + debug << "packet_buffer[" << std::dec << i << "]: 0x" << std::hex + << (uint16_t) this->packetBuffer[i] << std::endl; + } +} + +uint8_t MapPacketExtraction::getMapId() const { + return mapId; +} diff --git a/datalinklayer/MapPacketExtraction.h b/datalinklayer/MapPacketExtraction.h new file mode 100644 index 00000000..1fc9d2a0 --- /dev/null +++ b/datalinklayer/MapPacketExtraction.h @@ -0,0 +1,77 @@ +/** + * @file MapPacketExtraction.h + * @brief This file defines the MapPacketExtraction class. + * @date 26.03.2013 + * @author baetz + */ + +#ifndef MAPPACKETEXTRACTION_H_ +#define MAPPACKETEXTRACTION_H_ + +#include +#include +#include +#include +class StorageManagerIF; + +/** + * Implementation of a MAP Packet Extraction class. + * The class implements the full MAP Packet Extraction functionality as described in the CCSDS + * TC Space Data Link Protocol. It internally stores incomplete segmented packets until they are + * fully received. All found packets are forwarded to a single distribution entity. + */ +class MapPacketExtraction: public MapPacketExtractionIF { +private: + static const uint32_t MAX_PACKET_SIZE = 4096; + uint8_t lastSegmentationFlag; //!< The segmentation flag of the last received frame. + uint8_t mapId; //!< MAP ID of this MAP Channel. + uint32_t packetLength; //!< Complete length of the current Space Packet. + uint8_t* bufferPosition; //!< Position to write to in the internal Packet buffer. + uint8_t packetBuffer[MAX_PACKET_SIZE]; //!< The internal Space Packet Buffer. + object_id_t packetDestination; + StorageManagerIF* packetStore; //!< Pointer to the store where full TC packets are stored. + MessageQueueSender tcQueue; //!< Sender Queue to send found packets to the distributor. + /** + * Debug method to print the packet Buffer's content. + */ + void printPacketBuffer(); + /** + * Method that is called if the segmentation flag is @c NO_SEGMENTATION. + * The method extracts one or more packets within the frame and forwards them to the OBSW. + * @param frame The TC Transfer Frame to work on. + * @return @c FRAME_OK if all Packets were extracted. If something is entirely wrong, + * @c DATA_CORRUPTED is returned, if some bytes are left over @c RESIDUAL_DATA. + */ + ReturnValue_t unpackBlockingPackets(TcTransferFrame* frame); + /** + * Helper method to forward a complete packet to the OBSW. + * @param data Pointer to the data, either directly from the frame or from the packetBuffer. + * @param size Complete total size of the packet. + * @return Return Code of the Packet Store or the Message Queue. + */ + ReturnValue_t sendCompletePacket( uint8_t* data, uint32_t size ); + /** + * Helper method to reset the internal buffer. + */ + void clearBuffers(); +public: + /** + * Default constructor. + * Members are set to default values. + * @param setMapId The MAP ID of the instance. + */ + MapPacketExtraction( uint8_t setMapId, object_id_t setPacketDestination ); + ReturnValue_t extractPackets(TcTransferFrame* frame); + /** + * The #packetStore and the default destination of #tcQueue are initialized here. + * @return @c RETURN_OK on success, @c RETURN_FAILED otherwise. + */ + ReturnValue_t initialize(); + /** + * Getter. + * @return The MAP ID of this instance. + */ + uint8_t getMapId() const; +}; + +#endif /* MAPPACKETEXTRACTION_H_ */ diff --git a/datalinklayer/MapPacketExtractionIF.h b/datalinklayer/MapPacketExtractionIF.h new file mode 100644 index 00000000..802a893d --- /dev/null +++ b/datalinklayer/MapPacketExtractionIF.h @@ -0,0 +1,47 @@ +/** + * @file MapPacketExtractionIF.h + * @brief This file defines the MapPacketExtractionIF class. + * @date 25.03.2013 + * @author baetz + */ + +#ifndef MAPPACKETEXTRACTIONIF_H_ +#define MAPPACKETEXTRACTIONIF_H_ + +#include +#include + +/** + * This is the interface for MAP Packet Extraction classes. + * All classes implementing this interface shall be able to extract blocked or segmented Space + * Packets on a certain MAP channel. This is done in accordance with the CCSDS TC Space Data Link + * Protocol. + */ +class MapPacketExtractionIF : public CCSDSReturnValuesIF { +protected: + static const uint8_t FIRST_PORTION = 0b01; //!< Identification of the first part of a segmented Packet. + static const uint8_t CONTINUING_PORTION = 0b00; //!< Identification of a continuing part of segmented Packets. + static const uint8_t LAST_PORTION = 0b10; //!< The last portion of a segmented Packet. + static const uint8_t NO_SEGMENTATION = 0b11; //!< A Frame without segmentation but maybe with blocking. +public: + /** + * Empty virtual destructor. + */ + virtual ~MapPacketExtractionIF() { + } + /** + * Method to call to handle a single Transfer Frame. + * The method tries to extract Packets from the frame as stated in the Standard. + * @param frame + * @return + */ + virtual ReturnValue_t extractPackets( TcTransferFrame* frame ) = 0; + /** + * Any post-instantiation initialization shall be done in this method. + * @return + */ + virtual ReturnValue_t initialize() = 0; +}; + + +#endif /* MAPPACKETEXTRACTIONIF_H_ */ diff --git a/datalinklayer/TcTransferFrame.cpp b/datalinklayer/TcTransferFrame.cpp new file mode 100644 index 00000000..9ff06448 --- /dev/null +++ b/datalinklayer/TcTransferFrame.cpp @@ -0,0 +1,102 @@ +/** + * @file TcTransferFrame.cpp + * @brief This file defines the TcTransferFrame class. + * @date 27.04.2013 + * @author baetz + */ + + + +#include +#include + +TcTransferFrame::TcTransferFrame() { + frame = NULL; +} + +TcTransferFrame::TcTransferFrame(uint8_t* setData) { + this->frame = (tc_transfer_frame*)setData; +} + +uint8_t TcTransferFrame::getVersionNumber() { + return (this->frame->header.flagsAndScid & 0b11000000) >> 6; +} + +bool TcTransferFrame::bypassFlagSet() { + return (this->frame->header.flagsAndScid & 0b00100000) != 0; +} + +bool TcTransferFrame::controlCommandFlagSet() { + return (this->frame->header.flagsAndScid & 0b00010000) != 0; +} + +bool TcTransferFrame::spareIsZero() { + return ( (this->frame->header.flagsAndScid & 0b00001100) == 0 ); +} + +uint16_t TcTransferFrame::getSpacecraftId() { + return ( (this->frame->header.flagsAndScid & 0b00000011) << 8 ) + this->frame->header.spacecraftId_l; +} + +uint8_t TcTransferFrame::getVirtualChannelId() { + return (this->frame->header.vcidAndLength_h & 0b11111100) >> 2; +} + +uint16_t TcTransferFrame::getFrameLength() { + return ( (this->frame->header.vcidAndLength_h & 0b00000011) << 8 ) + this->frame->header.length_l; +} + +uint16_t TcTransferFrame::getDataLength() { + return this->getFrameLength() - this->getHeaderSize() -1 - FRAME_CRC_SIZE + 1; // -1 for the segment header. +} + +uint8_t TcTransferFrame::getSequenceNumber() { + return this->frame->header.sequenceNumber; +} + +uint8_t TcTransferFrame::getSequenceFlags() { + return (this->frame->dataField & 0b11000000)>>6; +} + +uint8_t TcTransferFrame::getMAPId() { + return this->frame->dataField & 0b00111111; +} + +uint8_t* TcTransferFrame::getDataField() { + return &(this->frame->dataField) + 1; +} + +uint8_t* TcTransferFrame::getFullFrame() { + return (uint8_t*)this->frame; +} + +uint16_t TcTransferFrame::getFullSize() { + return this->getFrameLength() + 1; +} + +uint16_t TcTransferFrame::getHeaderSize() { + return sizeof(frame->header); +} + +uint16_t TcTransferFrame::getFullDataLength() { + return this->getFrameLength() - this->getHeaderSize() - FRAME_CRC_SIZE + 1; +} + +uint8_t* TcTransferFrame::getFullDataField() { + return &frame->dataField; +} + +void TcTransferFrame::print() { + debug << "Raw Frame: " << std::hex << std::endl; + for (uint16_t count = 0; count < this->getFullSize(); count++ ) { + debug << (uint16_t)this->getFullFrame()[count] << " "; + } + debug << std::dec << std::endl; +// debug << "Frame Header:" << std::endl; +// debug << "Version Number: " << std::hex << (uint16_t)this->current_frame.getVersionNumber() << std::endl; +// debug << "Bypass Flag set?| Ctrl Cmd Flag set?: " << (uint16_t)this->current_frame.bypassFlagSet() << " | " << (uint16_t)this->current_frame.controlCommandFlagSet() << std::endl; +// debug << "SCID : " << this->current_frame.getSpacecraftId() << std::endl; +// debug << "VCID : " << (uint16_t)this->current_frame.getVirtualChannelId() << std::endl; +// debug << "Frame length: " << std::dec << this->current_frame.getFrameLength() << std::endl; +// debug << "Sequence Number: " << (uint16_t)this->current_frame.getSequenceNumber() << std::endl; +} diff --git a/datalinklayer/TcTransferFrame.h b/datalinklayer/TcTransferFrame.h new file mode 100644 index 00000000..b58441fc --- /dev/null +++ b/datalinklayer/TcTransferFrame.h @@ -0,0 +1,137 @@ +#ifndef TCTRANSFERFRAME_H_ +#define TCTRANSFERFRAME_H_ + +#include +#include + +/** + * The TcTransferFrame class simplifies handling of such Frames. + * It operates on any buffer passed on construction. The data length + * is determined by the length field in the frame itself. + * It has a lot of getters for convenient access to the content. + * @ingroup ccsds_handling + */ +class TcTransferFrame { +protected: + /** + * The struct that defines the Frame's Primary Header. + */ + struct TcTransferFramePrimaryHeader { + uint8_t flagsAndScid; //!< Highest byte with Flags and part of SCID. + uint8_t spacecraftId_l; //!< Byte with rest of SCID + uint8_t vcidAndLength_h; //!< Byte with VCID and part of length. + uint8_t length_l; //!< Byte with rest of length. + uint8_t sequenceNumber; //!< Lowest byte with Frame Sequence Number N(S). + }; + /** + * The struct defining the whole Transfer Frame. + */ + struct tc_transfer_frame { + TcTransferFramePrimaryHeader header; //!< The header struct. + uint8_t dataField; //!< The data field of the Transfer Frame. + }; + tc_transfer_frame* frame; //!< Pointer to a buffer where a Frame is placed. +public: + static const uint8_t FRAME_CRC_SIZE = 2; //!< Constant for the CRC size. + /** + * Empty Constructor that sets the data pointer to NULL. + */ + TcTransferFrame(); + /** + * The data pointer passed in this Constructor is casted to the #tc_transfer_frame struct. + * @param setData The data on which the class shall operate. + */ + TcTransferFrame(uint8_t* setData); + /** + * Getter. + * @return The Version number. + */ + uint8_t getVersionNumber(); + /** + * Getter. + * @return If the bypass flag is set or not. + */ + bool bypassFlagSet(); + /** + * Getter. + * @return If the control command flag is set or not. + */ + bool controlCommandFlagSet(); + /** + * Getter. + * @return If the spare bits in the Header are zero or not. + */ + bool spareIsZero(); + /** + * Getter. + * @return The Spacecraft Identifier. + */ + uint16_t getSpacecraftId(); + /** + * Getter. + * @return The Virtual Channel Identifier. + */ + uint8_t getVirtualChannelId(); + /** + * Getter. + * @return The Frame length as stored in the Header. + */ + uint16_t getFrameLength(); + /** + * Getter. + * @return The length of pure data (without CRC), assuming that a Segment Header is present. + */ + uint16_t getDataLength(); + /** + * Getter. + * @return The length of pure data (without CRC), assuming that no Segment Header is present (for BC Frames). + */ + uint16_t getFullDataLength(); + /** + * Getter. + * @return The sequence number from the header. + */ + uint8_t getSequenceNumber(); + /** + * Getter. + * @return The Sequence Flags in the Segment Header byte (right aligned). + */ + uint8_t getSequenceFlags(); + /** + * Getter. + * @return The Multiplexer Access Point Identifier from the Segment Header byte. + */ + uint8_t getMAPId(); + /** + * Getter. + * @return A pointer to the date field AFTER a Segment Header. + */ + uint8_t* getDataField(); + /** + * Getter. + * @return A pointer to the first byte in the Data Field (ignoring potential Segment Headers, for BC Frames). + */ + uint8_t* getFullDataField(); + /** + * Getter. + * @return A pointer to the beginning of the Frame. + */ + uint8_t* getFullFrame(); + /** + * Getter + * @return The total size of the Frame, which is the size stated in the Header + 1. + */ + uint16_t getFullSize(); + /** + * Getter. + * @return Size of the #TcTransferFramePrimaryHeader. + */ + uint16_t getHeaderSize(); + /** + * Debug method to print the whole Frame to screen. + */ + void print(); + +}; + +#endif /* TCTRANSFERFRAME_H_ */ diff --git a/datalinklayer/TcTransferFrameLocal.cpp b/datalinklayer/TcTransferFrameLocal.cpp new file mode 100644 index 00000000..a53c5b28 --- /dev/null +++ b/datalinklayer/TcTransferFrameLocal.cpp @@ -0,0 +1,49 @@ +/** + * @file TcTransferFrameLocal.cpp + * @brief This file defines the TcTransferFrameLocal class. + * @date 27.04.2013 + * @author baetz + */ + +#include +#include +#include +#include + +TcTransferFrameLocal::TcTransferFrameLocal(bool bypass, bool controlCommand, uint16_t scid, + uint8_t vcId, uint8_t sequenceNumber, uint8_t setSegmentHeader, uint8_t* data, uint16_t dataSize, uint16_t forceCrc) { + this->frame = (tc_transfer_frame*)&localData; + frame->header.flagsAndScid = (bypass << 5) + (controlCommand << 4) + ((scid & 0x0300) >> 8); + frame->header.spacecraftId_l = (scid & 0x00FF); + frame->header.vcidAndLength_h = (vcId & 0b00111111) << 2; + frame->header.length_l = sizeof(TcTransferFramePrimaryHeader) -1; + frame->header.sequenceNumber = sequenceNumber; + frame->dataField = setSegmentHeader; + if (data != NULL) { + if (bypass && controlCommand) { + memcpy(&(frame->dataField), data, dataSize); + uint16_t totalSize = sizeof(TcTransferFramePrimaryHeader) + dataSize + FRAME_CRC_SIZE -1; + frame->header.vcidAndLength_h |= (totalSize & 0x0300) >> 8; + frame->header.length_l = (totalSize & 0x00FF); + uint16_t crc = ::Calculate_CRC(getFullFrame(), getFullSize() -2); + this->getFullFrame()[getFullSize()-2] = (crc & 0xFF00) >> 8; + this->getFullFrame()[getFullSize()-1] = (crc & 0x00FF); + } else if (dataSize <= 1016) { + memcpy(&(frame->dataField) +1, data, dataSize); + uint16_t dataCrcSize = sizeof(TcTransferFramePrimaryHeader) + 1 + dataSize + FRAME_CRC_SIZE -1; + frame->header.vcidAndLength_h |= (dataCrcSize & 0x0300) >> 8; + frame->header.length_l = (dataCrcSize & 0x00FF); + uint16_t crc = ::Calculate_CRC(getFullFrame(), getFullSize() -2); + this->getFullFrame()[getFullSize()-2] = (crc & 0xFF00) >> 8; + this->getFullFrame()[getFullSize()-1] = (crc & 0x00FF); + } else { + debug << "TcTransferFrameLocal: dataSize too large: " << dataSize << std::endl; + } + } else { + //No data in frame + } + if (forceCrc != 0 ) { + localData.data[getFullSize()-2] = (forceCrc & 0xFF00) >> 8; + localData.data[getFullSize()-1] = (forceCrc & 0x00FF); + } +} diff --git a/datalinklayer/TcTransferFrameLocal.h b/datalinklayer/TcTransferFrameLocal.h new file mode 100644 index 00000000..f20f12ec --- /dev/null +++ b/datalinklayer/TcTransferFrameLocal.h @@ -0,0 +1,49 @@ +/** + * @file TcTransferFrameLocal.h + * @brief This file defines the TcTransferFrameLocal class. + * @date 27.04.2013 + * @author baetz + */ + +#ifndef TCTRANSFERFRAMELOCAL_H_ +#define TCTRANSFERFRAMELOCAL_H_ + +#include + +/** + * This is a helper class to locally create TC Transfer Frames. + * This is mainly required for testing purposes and therefore not very sophisticated. + * @ingroup ccsds_handling + */ +class TcTransferFrameLocal : public TcTransferFrame { +private: + /** + * A stuct to locally store the complete data. + */ + struct frameData { + TcTransferFramePrimaryHeader header; //!< The primary header. + uint8_t data[1019]; //!< The data field. + }; +public: + frameData localData; //!< The local data in the Frame. + /** + * The default Constructor. + * All parameters in the Header are passed. + * If a BC Frame is detected no segment header is created. + * Otherwise (AD and BD), the Segment Header is set. + * @param bypass The bypass flag. + * @param controlCommand The Control Command flag. + * @param scid The SCID. + * @param vcId The VCID. + * @param sequenceNumber The Frame Sequence Number N(s) + * @param setSegmentHeader A value for the Segment Header. + * @param data Data to put into the Frame Data Field. + * @param dataSize Size of the Data. + * @param forceCrc if != 0, the value is used as CRC. + */ + TcTransferFrameLocal(bool bypass, bool controlCommand, uint16_t scid, uint8_t vcId, uint8_t sequenceNumber, + uint8_t setSegmentHeader = 0xC0, uint8_t* data = NULL, uint16_t dataSize = 0, uint16_t forceCrc = 0); +}; + + +#endif /* TCTRANSFERFRAMELOCAL_H_ */ diff --git a/datalinklayer/VirtualChannelReception.cpp b/datalinklayer/VirtualChannelReception.cpp new file mode 100644 index 00000000..a4b7eb92 --- /dev/null +++ b/datalinklayer/VirtualChannelReception.cpp @@ -0,0 +1,114 @@ +/** + * @file VirtualChannelReception.cpp + * @brief This file defines the VirtualChannelReception class. + * @date 26.03.2013 + * @author baetz + */ + +#include +#include +#include + +VirtualChannelReception::VirtualChannelReception(uint8_t setChannelId, + uint8_t setSlidingWindowWidth) : + channelId(setChannelId), slidingWindowWidth(setSlidingWindowWidth), positiveWindow( + setSlidingWindowWidth / 2), negativeWindow(setSlidingWindowWidth / 2), currentState( + &openState), openState(this), waitState(this), lockoutState(this), vR(0), farmBCounter( + 0) { + internalClcw.setVirtualChannel(channelId); +} + +ReturnValue_t VirtualChannelReception::mapDemultiplexing(TcTransferFrame* frame) { + uint8_t mapId = frame->getMAPId(); + mapChannelIterator iter = mapChannels.find(mapId); + if (iter == mapChannels.end()) { +// error << "VirtualChannelReception::mapDemultiplexing on VC " << std::hex << (int) channelId +// << ": MapChannel " << (int) mapId << std::dec << " not found." << std::endl; + return VC_NOT_FOUND; + } else { + return (iter->second)->extractPackets(frame); + } +} + +ReturnValue_t VirtualChannelReception::doFARM(TcTransferFrame* frame, ClcwIF* clcw) { + uint8_t bypass = frame->bypassFlagSet(); + uint8_t controlCommand = frame->controlCommandFlagSet(); + uint8_t typeValue = (bypass << 1) + controlCommand; + switch (typeValue) { + case AD_FRAME: + return currentState->handleADFrame(frame, clcw); + case BD_FRAME: + return handleBDFrame(frame, clcw); + case BC_FRAME: + return handleBCFrame(frame, clcw); + default: + return ILLEGAL_FLAG_COMBINATION; + } +} + +ReturnValue_t VirtualChannelReception::frameAcceptanceAndReportingMechanism(TcTransferFrame* frame, + ClcwIF* clcw) { + ReturnValue_t status = FRAME_OK; + status = doFARM(frame, &internalClcw); + internalClcw.setReceiverFrameSequenceNumber(vR); + internalClcw.setFarmBCount(farmBCounter); + clcw->setWhole(internalClcw.getAsWhole()); + if (status == FRAME_OK) { + status = mapDemultiplexing(frame); + } + return status; +} + +ReturnValue_t VirtualChannelReception::addMapChannel(uint8_t mapId, MapPacketExtractionIF* object) { + std::pair returnValue = mapChannels.insert( + std::pair(mapId, object)); + if (returnValue.second == true) { + return RETURN_OK; + } else { + return RETURN_FAILED; + } +} + +ReturnValue_t VirtualChannelReception::handleBDFrame(TcTransferFrame* frame, ClcwIF* clcw) { + farmBCounter++; + return FRAME_OK; +} + +ReturnValue_t VirtualChannelReception::handleBCFrame(TcTransferFrame* frame, ClcwIF* clcw) { + BcFrame content; + ReturnValue_t returnValue = content.initialize(frame->getFullDataField(), + frame->getFullDataLength()); + if (returnValue == BC_IS_UNLOCK_COMMAND) { + returnValue = currentState->handleBCUnlockCommand(clcw); + } else if (returnValue == BC_IS_SET_VR_COMMAND) { + returnValue = currentState->handleBCSetVrCommand(clcw, content.vR); + } else { + //Do nothing + } + return returnValue; +} + +uint8_t VirtualChannelReception::getChannelId() const { + return channelId; +} + +ReturnValue_t VirtualChannelReception::initialize() { + ReturnValue_t returnValue = RETURN_FAILED; + if ((slidingWindowWidth > 254) || (slidingWindowWidth % 2 != 0)) { + error << "VirtualChannelReception::initialize: Illegal sliding window width: " + << (int) slidingWindowWidth << std::endl; + return RETURN_FAILED; + } + for (mapChannelIterator iterator = mapChannels.begin(); iterator != mapChannels.end(); + iterator++) { + returnValue = iterator->second->initialize(); + if (returnValue != RETURN_OK) + break; + } + return returnValue; +} + +void VirtualChannelReception::setToWaitState() { + internalClcw.setWaitFlag(true); + this->currentState = &waitState; +} diff --git a/datalinklayer/VirtualChannelReception.h b/datalinklayer/VirtualChannelReception.h new file mode 100644 index 00000000..2e9c2121 --- /dev/null +++ b/datalinklayer/VirtualChannelReception.h @@ -0,0 +1,114 @@ +/** + * @file VirtualChannelReception.h + * @brief This file defines the VirtualChannelReception class. + * @date 25.03.2013 + * @author baetz + */ + +#ifndef VIRTUALCHANNELRECEPTION_H_ +#define VIRTUALCHANNELRECEPTION_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +/** + * Implementation of a TC Virtual Channel. + * This is a full implementation of a virtual channel as specified in the CCSDS TC Space Data Link + * Protocol. It is designed to operate within an instance of the @c DataLinkLayer class. + * Features: + * - any (6bit) Virtual Channel ID is assignable. + * - Supports an arbitrary number of MAP Channels (with a map). + * - Has a complete FARM-1 Machine built-in. + * + * The FARM-1 state machine uses the State Pattern. + */ +class VirtualChannelReception : public VirtualChannelReceptionIF, public CCSDSReturnValuesIF { + friend class Farm1StateOpen; + friend class Farm1StateWait; + friend class Farm1StateLockout; +private: + uint8_t channelId; //!< Stores the VCID that was assigned on construction. + uint8_t slidingWindowWidth; //!< A constant to set the FARM-1 sliding window width. + uint8_t positiveWindow; //!< The positive window for the FARM-1 machine. + uint8_t negativeWindow; //!< The negative window for the FARM-1 machine. +protected: + Farm1StateIF* currentState; //!< The current state. To change, one of the other states must be assigned to this pointer. + Farm1StateOpen openState; //!< Instance of the FARM-1 State "Open". + Farm1StateWait waitState; //!< Instance of the FARM-1 State "Wait". + Farm1StateLockout lockoutState; //!< Instance of the FARM-1 State "Lockout". + Clcw internalClcw; //!< A CLCW class to internally set the values before writing them back to the TTC System. + uint8_t vR; //!< The Receiver Frame Sequence Number V(R) as it shall be maintained for every Virtual Channel. + uint8_t farmBCounter; //!< The FARM-B COunter as it shall be maintained for every Virtual Channel. + typedef std::map::iterator mapChannelIterator; //!< Typedef to simplify handling of the mapChannels map. + std::map mapChannels; //!< A map that maintains all map Channels. Channels must be configured on initialization. MAy be omitted in a simplified version. + /** + * This method handles demultiplexing to different map channels. + * It parses the entries of #mapChannels and forwards the Frame to a found MAP Channel. + * @param frame The frame to forward. + * @return #VC_NOT_FOUND or the return value of the map channel extraction. + */ + ReturnValue_t mapDemultiplexing( TcTransferFrame* frame ); + /** + * A sub-method that actually does the FARM-1 handling for different Frame types. + * @param frame The Tc Transfer Frame to handle. + * @param clcw Any changes on the CLCW shall be done with this method. + * @return The return code of higher methods or @c ILLEGAL_FLAG_COMBINATION. + */ + ReturnValue_t doFARM(TcTransferFrame* frame, ClcwIF* clcw); + /** + * Handles incoming BD Frames. + * Handling these Frames is independent of the State, so no subcall to #currentState is + * required. + * @param frame The Tc Transfer Frame to handle. + * @param clcw Any changes on the CLCW shall be done with this method. + * @return Always returns @c FRAME_OK. + */ + ReturnValue_t handleBDFrame( TcTransferFrame* frame, ClcwIF* clcw ); + /** + * Handles incoming BC Frames. + * The type of the BC Frame is detected and checked first, then methods of #currentState are called. + * @param frame The Tc Transfer Frame to handle. + * @param clcw Any changes on the CLCW shall be done with this method. + * @return The failure code of BC Frame interpretation or the return code of higher methods. + */ + ReturnValue_t handleBCFrame( TcTransferFrame* frame, ClcwIF* clcw ); +public: + /** + * Default constructor. + * Only sets the channelId of the channel. Setting the Sliding Window width is possible as well. + * @param setChannelId Virtual Channel Identifier (VCID) of the channel. + */ + VirtualChannelReception( uint8_t setChannelId, uint8_t setSlidingWindowWidth ); + ReturnValue_t frameAcceptanceAndReportingMechanism( TcTransferFrame* frame, ClcwIF* clcw ); + /** + * Helper method to simplify adding a mapChannel during construction. + * @param mapId The mapId of the object to add. + * @param object Pointer to the MapPacketExtraction object itself. + * @return @c RETURN_OK if the channel was successfully inserted, @c RETURN_FAILED otherwise. + */ + ReturnValue_t addMapChannel( uint8_t mapId, MapPacketExtractionIF* object ); + /** + * The initialization routine checks the set #slidingWindowWidth and initializes all MAP + * channels. + * @return @c RETURN_OK on successful initialization, @c RETURN_FAILED otherwise. + */ + ReturnValue_t initialize(); + /** + * Getter for the VCID. + * @return The #channelId. + */ + uint8_t getChannelId() const; + /** + * Small method to set the state to Farm1StateWait. + */ + void setToWaitState(); +}; + + +#endif /* VIRTUALCHANNELRECEPTION_H_ */ diff --git a/datalinklayer/VirtualChannelReceptionIF.h b/datalinklayer/VirtualChannelReceptionIF.h new file mode 100644 index 00000000..b010a9ee --- /dev/null +++ b/datalinklayer/VirtualChannelReceptionIF.h @@ -0,0 +1,57 @@ +/** + * @file VirtualChannelReceptionIF.h + * @brief This file defines the VirtualChannelReceptionIF class. + * @date 25.03.2013 + * @author baetz + */ + +#ifndef VIRTUALCHANNELRECEPTIONIF_H_ +#define VIRTUALCHANNELRECEPTIONIF_H_ + +#include +#include +#include + +/** + * This is the interface for Virtual Channel reception classes. + * It represents a single TC Virtual Channel that operates on one IO + */ +class VirtualChannelReceptionIF { +public: + /** + * Enum including all valid types of frames. + * The type is made up by two flags, so 0b1111 is definitely illegal. + */ + enum frameType { + AD_FRAME = 0b00, + BC_FRAME = 0b11, + BD_FRAME = 0b10, + ILLEGAL_FRAME = 0b1111 + }; + /** + * Empty virtual destructor. + */ + virtual ~VirtualChannelReceptionIF() { + } + /** + * This method shall accept frames and do all FARM-1 stuff. + * Handling the Frame includes forwarding to higher-level procedures. + * @param frame The Tc Transfer Frame that was received and checked. + * @param clcw Any changes to the CLCW value are forwarded by using this parameter. + * @return The return Value shall indicate successful processing with @c FRAME_OK. + */ + virtual ReturnValue_t frameAcceptanceAndReportingMechanism( TcTransferFrame* frame, ClcwIF* clcw ) = 0; + /** + * If any other System Objects are required for operation they shall be initialized here. + * @return @c RETURN_OK for successful initialization. + */ + virtual ReturnValue_t initialize() = 0; + /** + * Getter for the VCID. + * @return The #channelId. + */ + virtual uint8_t getChannelId() const = 0; +}; + + +#endif /* VIRTUALCHANNELRECEPTIONIF_H_ */ diff --git a/datapool/DataPool.cpp b/datapool/DataPool.cpp new file mode 100644 index 00000000..268b29f0 --- /dev/null +++ b/datapool/DataPool.cpp @@ -0,0 +1,136 @@ +/* + * DataPool.cpp + * + * Created on: 17.10.2012 + * Author: baetz + */ + + +#include +#include +DataPool::DataPool( void ( *initFunction )( std::map* pool_map ) ) { + this->mutex = new MutexId_t; + OSAL::createMutex( OSAL::buildName('M','T','X','0'), ( this->mutex ) ); + if (initFunction != NULL ) { + initFunction( &this->data_pool ); + } +} + +DataPool::~DataPool() { + OSAL::deleteMutex( this->mutex ); + delete this->mutex; + for ( std::map::iterator it = this->data_pool.begin(); it != this->data_pool.end(); ++it ) { + delete it->second; + } +} + +//The function checks PID, type and array length before returning a copy of the PoolEntry. In failure case, it returns a temp-Entry with size 0 and NULL-ptr. +template PoolEntry* DataPool::getData( uint32_t data_pool_id, uint8_t sizeOrPosition ) { + std::map::iterator it = this->data_pool.find( data_pool_id ); + if ( it != this->data_pool.end() ) { + PoolEntry* entry = dynamic_cast< PoolEntry* >( it->second ); + if (entry != NULL ) { + if ( sizeOrPosition <= entry->length ) { + return entry; + } + } + } + return NULL; +} + +PoolEntryIF* DataPool::getRawData( uint32_t data_pool_id ) { + std::map::iterator it = this->data_pool.find( data_pool_id ); + if ( it != this->data_pool.end() ) { + return it->second; + } else { + return NULL; + } +} + +//uint8_t DataPool::getRawData( uint32_t data_pool_id, uint8_t* address, uint16_t* size, uint32_t max_size ) { +// std::map::iterator it = this->data_pool.find( data_pool_id ); +// if ( it != this->data_pool.end() ) { +// if ( it->second->getByteSize() <= max_size ) { +// *size = it->second->getByteSize(); +// memcpy( address, it->second->getRawData(), *size ); +// return DP_SUCCESSFUL; +// } +// } +// *size = 0; +// return DP_FAILURE; +//} + +ReturnValue_t DataPool::freeDataPoolLock() { + ReturnValue_t status = OSAL::unlockMutex( this->mutex ); + if ( status != RETURN_OK ) { + error << "DataPool::DataPool: unlock of mutex failed with error code: " << status << std::endl; + } + return status; +} + +ReturnValue_t DataPool::lockDataPool() { + ReturnValue_t status = OSAL::lockMutex( this->mutex, OSAL::NO_TIMEOUT ); + if ( status != RETURN_OK ) { + error << "DataPool::DataPool: lock of mutex failed with error code: " << status << std::endl; + } + return status; +} + +void DataPool::print() { + debug << "DataPool contains: " << std::endl; + std::map::iterator dataPoolIt; + dataPoolIt = this->data_pool.begin(); + while( dataPoolIt != this->data_pool.end() ) { + debug << std::hex << dataPoolIt->first << std::dec << " |"; + dataPoolIt->second->print(); + dataPoolIt++; + } +} + +template PoolEntry* DataPool::getData( uint32_t data_pool_id, uint8_t size ); +template PoolEntry* DataPool::getData( uint32_t data_pool_id, uint8_t size ); +template PoolEntry* DataPool::getData( uint32_t data_pool_id, uint8_t size ); +template PoolEntry* DataPool::getData(uint32_t data_pool_id, + uint8_t size); +template PoolEntry* DataPool::getData( uint32_t data_pool_id, uint8_t size ); +template PoolEntry* DataPool::getData( uint32_t data_pool_id, uint8_t size ); +template PoolEntry* DataPool::getData( uint32_t data_pool_id, uint8_t size ); +template PoolEntry* DataPool::getData( uint32_t data_pool_id, uint8_t size ); +template PoolEntry* DataPool::getData(uint32_t data_pool_id, + uint8_t size); + + +uint32_t DataPool::PIDToDataPoolId(uint32_t parameter_id) { + return (parameter_id >> 8) & 0x00FFFFFF; +} + +uint8_t DataPool::PIDToArrayIndex(uint32_t parameter_id) { + return (parameter_id & 0x000000FF); +} + +uint32_t DataPool::poolIdAndPositionToPid(uint32_t poolId, uint8_t index) { + return (poolId << 8) + index; +} + +//TODO: This is not 100% clean. Should return returnValue and type by passing... +//TODO: Do we need a mutex lock here... I don't think so, as we only check static const values of elements in a list that do not change. +Type DataPool::getType(uint32_t parameter_id) { + std::map::iterator it = this->data_pool.find( PIDToDataPoolId(parameter_id)); + if ( it != this->data_pool.end() ) { + return it->second->getType(); + } else { + return Type::UNKNOWN_TYPE; + } +} + +bool DataPool::exists(uint32_t parameterId) { + uint32_t poolId = PIDToDataPoolId(parameterId); + uint32_t index = PIDToArrayIndex(parameterId); + std::map::iterator it = this->data_pool.find( poolId ); + if (it != data_pool.end()) { + if (it->second->getSize() >= index) { + return true; + } + } + return false; +} diff --git a/datapool/DataPool.h b/datapool/DataPool.h new file mode 100644 index 00000000..d3cceeed --- /dev/null +++ b/datapool/DataPool.h @@ -0,0 +1,129 @@ +/** + * \file DataPool.h + * + * \date 10/17/2012 + * \author Bastian Baetz + * + * \brief This file contains the definition of the DataPool class and (temporarily) + * the "extern" definition of the global dataPool instance. + */ + +#ifndef DATAPOOL_H_ +#define DATAPOOL_H_ + +#include +#include +#include +#include + +/** + * \defgroup data_pool Data Pool + * This is the group, where all classes associated with Data Pool Handling belong to. + * This includes classes to access Data Pool variables. + */ + +#define DP_SUCCESSFUL 0 +#define DP_FAILURE 1 + +/** + * \brief This class represents the OBSW global data-pool. + * + * \details All variables are registered and space is allocated in an initialization + * function, which is passed do the constructor. + * Space for the variables is allocated on the heap (with a new call). + * The data is found by a data pool id, which uniquely represents a variable. + * Data pool variables should be used with a blackboard logic in mind, + * which means read data is valid (if flagged so), but not necessarily up-to-date. + * Variables are either single values or arrays. + * \ingroup data_pool + */ +class DataPool : public HasReturnvaluesIF { +private: + /** + * \brief This is the actual data pool itself. + * \details It is represented by a map + * with the data pool id as index and a pointer to a single PoolEntry as value. + */ + std::map data_pool; +public: + /** + * \brief The mutex is created in the constructor and makes access mutual exclusive. + * \details Locking and unlocking the pool is only done by the DataSet class. + */ + MutexId_t* mutex; + /** + * \brief In the classes constructor, the passed initialization function is called. + * \details To enable filling the pool, + * a pointer to the map is passed, allowing direct access to the pool's content. + * On runtime, adding or removing variables is forbidden. + */ + DataPool( void ( *initFunction )( std::map* pool_map ) ); + /** + * \brief The destructor iterates through the data_pool map and calls all Entries destructors to clean up the heap. + */ + ~DataPool(); + /** + * \brief This is the default call to access the pool. + * \details A pointer to the PoolEntry object is returned. + * The call checks data pool id, type and array size. Returns NULL in case of failure. + * \param data_pool_id The data pool id to search. + * \param sizeOrPosition The array size (not byte size!) of the pool entry, or the position the user wants to read. + * If smaller than the entry size, everything's ok. + */ + template PoolEntry* getData( uint32_t data_pool_id, uint8_t sizeOrPosition ); + /** + * \brief An alternative call to get a data pool entry in case the type is not implicitly known + * (i.e. in Housekeeping Telemetry). + * \details It returns a basic interface and does NOT perform + * a size check. The caller has to assure he does not copy too much data. + * Returns NULL in case the entry is not found. + * \param data_pool_id The data pool id to search. + */ + PoolEntryIF* getRawData( uint32_t data_pool_id ); + /** + * \brief This is a small helper function to facilitate locking the global data pool. + * \details It fetches the pool's mutex id and tries to acquire the mutex. + */ + ReturnValue_t lockDataPool(); + /** + * \brief This is a small helper function to facilitate unlocking the global data pool. + * \details It fetches the pool's mutex id and tries to free the mutex. + */ + ReturnValue_t freeDataPoolLock(); + /** + * \brief The print call is a simple debug method. + * \details It prints the current content of the data pool. + * It iterates through the data_pool map and calls each entry's print() method. + */ + void print(); + /** + * Extracts the data pool id from a SCOS 2000 PID. + * @param parameter_id The passed Parameter ID. + * @return The data pool id as used within the OBSW. + */ + static uint32_t PIDToDataPoolId( uint32_t parameter_id ); + /** + * Extracts an array index out of a SCOS 2000 PID. + * @param parameter_id The passed Parameter ID. + * @return The index of the corresponding data pool entry. + */ + static uint8_t PIDToArrayIndex( uint32_t parameter_id ); + /** + * Retransforms a data pool id and an array index to a SCOS 2000 PID. + */ + static uint32_t poolIdAndPositionToPid( uint32_t poolId, uint8_t index ); + + Type getType( uint32_t parameter_id ); + /** + * Method to check if a PID exists. + * Does not lock, as there's no possibility to alter the list that is checked during run-time. + * @param parameterId The PID (not pool id!) of a parameter. + * @return true if exists, false else. + */ + bool exists(uint32_t parameterId); +}; + +//TODO: Remove this, if data pool is get by Satellite Manager +//Better make clean Singleton. +extern DataPool dataPool; +#endif /* DATAPOOL_H_ */ diff --git a/datapool/DataPoolAdmin.cpp b/datapool/DataPoolAdmin.cpp new file mode 100644 index 00000000..34a1cfde --- /dev/null +++ b/datapool/DataPoolAdmin.cpp @@ -0,0 +1,112 @@ +/* + * DataPoolAdmin.cpp + * + * Created on: 05.12.2013 + * Author: baetz + */ + + + +#include +#include +#include +#include +#include + + +DataPoolAdmin::DataPoolAdmin(object_id_t objectId ) : SystemObject(objectId), commandQueue(), memoryHelper(this, &commandQueue){ +} + +ReturnValue_t DataPoolAdmin::performOperation() { + handleCommand(); + return RETURN_OK; +} + +MessageQueueId_t DataPoolAdmin::getCommandQueue() const { + return commandQueue.getId(); +} + +void DataPoolAdmin::handleCommand() { + CommandMessage command; + ReturnValue_t result = commandQueue.receiveMessage(&command); + if (result != RETURN_OK) { + return; + } + result = memoryHelper.handleMemoryCommand(&command); + if (result != RETURN_OK) { + command.setToUnknownCommand(command.getCommand()); + commandQueue.reply( &command ); + } +} + +ReturnValue_t DataPoolAdmin::handleMemoryLoad(uint32_t address, + const uint8_t* data, uint32_t size, uint8_t** dataPointer) { + uint32_t poolId = ::dataPool.PIDToDataPoolId( address ); + uint8_t arrayIndex = ::dataPool.PIDToArrayIndex( address ); + DataSet testSet; + PoolRawAccess varToGetSize( poolId, arrayIndex, &testSet, PoolVariableIF::VAR_READ ); + ReturnValue_t status = testSet.read(); + if (status != RETURN_OK) { + return INVALID_ADDRESS; + } + uint8_t typeSize = varToGetSize.getSizeOfType(); + if ( size > varToGetSize.getSizeTillEnd() ) { + return INVALID_SIZE; + } + const uint8_t* readPosition = data; + for ( ; size > 0; size -= typeSize ) { + DataSet rawSet; + PoolRawAccess variable( poolId, arrayIndex, &rawSet, PoolVariableIF::VAR_READ_WRITE ); + status = rawSet.read(); + if (status == RETURN_OK) { + status = variable.setEntryFromBigEndian( readPosition, typeSize ); + if (status == RETURN_OK) { + status = rawSet.commit(PoolVariableIF::VALID); + } + } + arrayIndex += 1; + readPosition += typeSize; + } + return ACTIVITY_COMPLETED; +} + +ReturnValue_t DataPoolAdmin::handleMemoryDump(uint32_t address, uint32_t size, + uint8_t** dataPointer, uint8_t* copyHere) { + uint32_t poolId = ::dataPool.PIDToDataPoolId( address ); + uint8_t arrayIndex = ::dataPool.PIDToArrayIndex( address ); + DataSet testSet; + PoolRawAccess varToGetSize( poolId, arrayIndex, &testSet, PoolVariableIF::VAR_READ ); + ReturnValue_t status = testSet.read(); + if (status != RETURN_OK) { + return INVALID_ADDRESS; + } + uint8_t typeSize = varToGetSize.getSizeOfType(); + if ( size > varToGetSize.getSizeTillEnd() ) { + return INVALID_SIZE; + } + uint8_t* ptrToCopy = copyHere; + for ( ; size > 0; size -= typeSize ) { + DataSet rawSet; + PoolRawAccess variable( poolId, arrayIndex, &rawSet, PoolVariableIF::VAR_READ ); + status = rawSet.read(); + if (status == RETURN_OK) { + uint32_t temp = 0; + status = variable.getEntryEndianSafe( ptrToCopy, &temp, size); + if ( status != RETURN_OK ) { + return RETURN_FAILED; + } + } else { + //Error reading parameter. + } + arrayIndex += 1; + ptrToCopy += typeSize; + } + return ACTIVITY_COMPLETED; +} + +ReturnValue_t DataPoolAdmin::initialize() { + if (memoryHelper.initialize() == RETURN_OK) { + return SystemObject::initialize(); + } + return RETURN_FAILED; +} diff --git a/datapool/DataPoolAdmin.h b/datapool/DataPoolAdmin.h new file mode 100644 index 00000000..8a066c78 --- /dev/null +++ b/datapool/DataPoolAdmin.h @@ -0,0 +1,35 @@ +/* + * DataPoolAdmin.h + * + * Created on: 05.12.2013 + * Author: baetz + */ + +#ifndef DATAPOOLADMIN_H_ +#define DATAPOOLADMIN_H_ + +#include +#include +#include +#include +#include + +class DataPoolAdmin : public ExecutableObjectIF, public AcceptsMemoryMessagesIF, public HasReturnvaluesIF, public SystemObject { +private: + MessageQueue commandQueue; + MemoryHelper memoryHelper; + void handleCommand(); +public: + DataPoolAdmin(object_id_t objectId); + + ReturnValue_t performOperation(); + + MessageQueueId_t getCommandQueue() const; + + ReturnValue_t handleMemoryLoad(uint32_t address, const uint8_t* data, uint32_t size, uint8_t** dataPointer); + ReturnValue_t handleMemoryDump(uint32_t address, uint32_t size, uint8_t** dataPointer, uint8_t* copyHere ); + ReturnValue_t initialize(); +}; + + +#endif /* DATAPOOLADMIN_H_ */ diff --git a/datapool/DataSet.cpp b/datapool/DataSet.cpp new file mode 100644 index 00000000..4f9e3792 --- /dev/null +++ b/datapool/DataSet.cpp @@ -0,0 +1,153 @@ +/* + * DataSet.cpp + * + * Created on: 24.10.2012 + * Author: baetz + */ + +#include +#include + +DataSet::DataSet() : + fill_count(0), state(DATA_SET_UNINITIALISED) { + for (unsigned count = 0; count < DATA_SET_MAX_SIZE; count++) { + registeredVariables[count] = NULL; + } +} + +DataSet::~DataSet() { + //Don't do anything with your variables, they are dead already! (Destructor is already called) +} + +ReturnValue_t DataSet::read() { + ReturnValue_t result = RETURN_OK; + if (state == DATA_SET_UNINITIALISED) { + lockDataPool(); + for (uint16_t count = 0; count < fill_count; count++) { + if (registeredVariables[count]->getReadWriteMode() + != PoolVariableIF::VAR_WRITE + && registeredVariables[count]->getDataPoolId() + != PoolVariableIF::NO_PARAMETER) { + ReturnValue_t status = registeredVariables[count]->read(); + if (status != RETURN_OK) { + result = INVALID_PARAMETER_DEFINITION; + break; + } + } + } + state = DATA_SET_WAS_READ; + freeDataPoolLock(); + } else { + error << "DataSet::read(): Call made in wrong position." << std::endl; + result = SET_WAS_ALREADY_READ; + } + return result; +} + +ReturnValue_t DataSet::commit(uint8_t valid) { + for (uint16_t count = 0; count < fill_count; count++) { + if (registeredVariables[count]->getReadWriteMode() + != PoolVariableIF::VAR_READ) { + registeredVariables[count]->setValid(valid); + } + } + return commit(); +} + +ReturnValue_t DataSet::commit() { + if (state == DATA_SET_WAS_READ) { + lockDataPool(); + for (uint16_t count = 0; count < fill_count; count++) { + if (registeredVariables[count]->getReadWriteMode() + != PoolVariableIF::VAR_READ + && registeredVariables[count]->getDataPoolId() + != PoolVariableIF::NO_PARAMETER) { + registeredVariables[count]->commit(); + } + } + state = DATA_SET_UNINITIALISED; + freeDataPoolLock(); + return RETURN_OK; + } else { + ReturnValue_t result = RETURN_OK; + lockDataPool(); + for (uint16_t count = 0; count < fill_count; count++) { + if (registeredVariables[count]->getReadWriteMode() + == PoolVariableIF::VAR_WRITE + && registeredVariables[count]->getDataPoolId() + != PoolVariableIF::NO_PARAMETER) { + registeredVariables[count]->commit(); + } else if (registeredVariables[count]->getDataPoolId() + != PoolVariableIF::NO_PARAMETER) { + if (result != COMMITING_WITHOUT_READING) { + error + << "DataSet::commit(): commit-without-read call made with non write-only variable." + << std::endl; + result = COMMITING_WITHOUT_READING; + } + } + } + state = DATA_SET_UNINITIALISED; + freeDataPoolLock(); + return result; + } + +} + +void DataSet::registerVariable(PoolVariableIF* variable) { + if (state == DATA_SET_UNINITIALISED) { + if (variable != NULL) { + if (fill_count < DATA_SET_MAX_SIZE) { + registeredVariables[fill_count] = variable; + fill_count++; + return; + } + } + } + error + << "DataSet::registerVariable: failed. Either NULL, or set is full, or call made in wrong position." + << std::endl; + return; +} + +uint8_t DataSet::freeDataPoolLock() { + return ::dataPool.freeDataPoolLock(); +} + +uint8_t DataSet::lockDataPool() { + return ::dataPool.lockDataPool(); +} + +ReturnValue_t DataSet::serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + ReturnValue_t result = RETURN_FAILED; + for (uint16_t count = 0; count < fill_count; count++) { + result = registeredVariables[count]->serialize(buffer, size, max_size, + bigEndian); + if (result != RETURN_OK) { + return result; + } + } + return result; +} + +uint32_t DataSet::getSerializedSize() const { + uint32_t size = 0; + for (uint16_t count = 0; count < fill_count; count++) { + size += registeredVariables[count]->getSerializedSize(); + } + return size; +} + +ReturnValue_t DataSet::deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + ReturnValue_t result = RETURN_FAILED; + for (uint16_t count = 0; count < fill_count; count++) { + result = registeredVariables[count]->deSerialize(buffer, size, + bigEndian); + if (result != RETURN_OK) { + return result; + } + } + return result; +} diff --git a/datapool/DataSet.h b/datapool/DataSet.h new file mode 100644 index 00000000..a15e023f --- /dev/null +++ b/datapool/DataSet.h @@ -0,0 +1,152 @@ +/* + * \file DataSet.h + * + * \brief This file contains the DataSet class and a small structure called DataSetContent. + * + * \date 10/17/2012 + * + * \author Bastian Baetz + * + */ + +#ifndef DATASET_H_ +#define DATASET_H_ + +#include +#include +#include +#include +#include +#include +#include +/** + * \brief The DataSet class manages a set of locally checked out variables. + * + * \details This class manages a list, where a set of local variables (or pool variables) are + * registered. They are checked-out (i.e. their values are looked up and copied) + * with the read call. After the user finishes working with the pool variables, + * he can write back all variable values to the pool with the commit call. + * The data set manages locking and freeing the data pool, to ensure that all values + * are read and written back at once. + * An internal state manages usage of this class. Variables may only be registered before + * the read call is made, and the commit call only after the read call. + * If pool variables are writable and not committed until destruction of the set, the + * DataSet class automatically sets the valid flag in the data pool to invalid (without) + * changing the variable's value. + * + * \ingroup data_pool + */ +class DataSet: public DataSetIF, public HasReturnvaluesIF, public SerializeIF { +private: + //TODO we could use a linked list of datapool variables + static const uint8_t DATA_SET_MAX_SIZE = 63; //!< This definition sets the maximum number of variables to register in one DataSet. + + /** + * \brief This array represents all pool variables registered in this set. + * \details It has a maximum size of DATA_SET_MAX_SIZE. + */ + PoolVariableIF* registeredVariables[DATA_SET_MAX_SIZE]; + /** + * \brief The fill_count attribute ensures that the variables register in the correct array + * position and that the maximum number of variables is not exceeded. + */ + uint16_t fill_count; + /** + * States of the seet. + */ + enum States { + DATA_SET_UNINITIALISED, //!< DATA_SET_UNINITIALISED + DATA_SET_WAS_READ //!< DATA_SET_WAS_READ + }; + /** + * \brief state manages the internal state of the data set, which is important e.g. for the + * behavior on destruction. + */ + States state; + /** + * \brief This is a small helper function to facilitate locking the global data pool. + * \details It makes use of the lockDataPool method offered by the DataPool class. + */ + uint8_t lockDataPool(); + /** + * \brief This is a small helper function to facilitate unlocking the global data pool. + * \details It makes use of the freeDataPoolLock method offered by the DataPool class. + */ + uint8_t freeDataPoolLock(); + +public: + static const uint8_t INTERFACE_ID = DATA_SET_CLASS; + static const ReturnValue_t INVALID_PARAMETER_DEFINITION = + MAKE_RETURN_CODE( 0x01 ); + static const ReturnValue_t SET_WAS_ALREADY_READ = MAKE_RETURN_CODE( 0x02 ); + static const ReturnValue_t COMMITING_WITHOUT_READING = + MAKE_RETURN_CODE(0x03); + + /** + * \brief The constructor simply sets the fill_count to zero and sets the state to "uninitialized". + */ + DataSet(); + /** + * \brief The destructor automatically manages writing the valid information of variables. + * \details In case the data set was read out, but not committed (indicated by state), + * the destructor parses all variables that are still registered to the set. + * For each, the valid flag in the data pool is set to "invalid". + */ + ~DataSet(); + /** + * \brief The read call initializes reading out all registered variables. + * \details It iterates through the list of registered variables and calls all read() + * functions of the registered pool variables (which read out their values from the + * data pool) which are not write-only. In case of an error (e.g. a wrong data type, + * or an invalid data pool id), the operation is aborted and + * \c INVALID_PARAMETER_DEFINITION returned. + * The data pool is locked during the whole read operation and freed afterwards. + * The state changes to "was written" after this operation. + * \return - \c RETURN_OK if all variables were read successfully. + * - \c INVALID_PARAMETER_DEFINITION if PID, size or type of the + * requested variable is invalid. + * - \c SET_WAS_ALREADY_READ if read() is called twice without calling + * commit() in between + */ + ReturnValue_t read(); + /** + * \brief The commit call initializes writing back the registered variables. + * \details It iterates through the list of registered variables and calls + * the commit() method of the remaining registered variables (which write back + * their values to the pool). + * The data pool is locked during the whole commit operation and freed afterwards. + * The state changes to "was committed" after this operation. + * If the set does contain at least one variable which is not write-only commit() + * can only be called after read(). If the set only contains variables which are + * write only, commit() can be called without a preceding read() call. + * \return - \c RETURN_OK if all variables were read successfully. + * - \c COMMITING_WITHOUT_READING if set was not read yet and contains non write-only + * variables + */ + ReturnValue_t commit(void); + /** + * Variant of method above which sets validity of all elements of the set. + * @param valid Validity information from PoolVariableIF. + * \return - \c RETURN_OK if all variables were read successfully. + * - \c COMMITING_WITHOUT_READING if set was not read yet and contains non write-only + * variables + */ + ReturnValue_t commit(uint8_t valid); + /** + * \brief This operation is used to register the local variables in the set. + * \details It copies all required information to the currently + * free space in the registeredVariables list. + */ + void registerVariable(PoolVariableIF* variable); + + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const; + + uint32_t getSerializedSize() const; + + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian); + +}; + +#endif /* DATASET_H_ */ diff --git a/datapool/DataSetIF.h b/datapool/DataSetIF.h new file mode 100644 index 00000000..dfbafc2d --- /dev/null +++ b/datapool/DataSetIF.h @@ -0,0 +1,42 @@ +/** + * \file DataSetIF.h + * + * \brief This file contains the small interface to access the DataSet class. + * + * \date 10/23/2012 + * + * \author Bastian Baetz + * + */ + +#ifndef DATASETIF_H_ +#define DATASETIF_H_ + + +#include + +class PoolVariableIF; + +/** + * \brief This class defines a small interface to register on a DataSet. + * + * \details Currently, the only purpose of this interface is to provide a method for locally + * checked-out variables to register on a data set. Still, it may become useful for + * other purposes as well. + * + * \ingroup data_pool + */ +class DataSetIF { +public: + /** + * \brief This is an empty virtual destructor, as it is proposed for C++ interfaces. + */ + virtual ~DataSetIF() {} + /** + * \brief This operation provides a method to register local data pool variables + * to register in a data set by passing itself to this DataSet operation. + */ + virtual void registerVariable( PoolVariableIF* variable ) = 0; +}; + +#endif /* DATASETIF_H_ */ diff --git a/datapool/Makefile b/datapool/Makefile new file mode 100755 index 00000000..97775d60 --- /dev/null +++ b/datapool/Makefile @@ -0,0 +1,25 @@ +#!/bin/bash +# +# OPUS makefile +# +# Created on: Mar 04, 2010 +# Author: ziemke +# Author: Claas Ziemke +# Copyright 2010, Claas Ziemke +# + +BASEDIR=../../ +include $(BASEDIR)/options.mk + +OBJ = $(BUILDDIR)/OPUSDataPool.o \ + $(BUILDDIR)/OPUSDataPoolItem.o \ + $(BUILDDIR)/OPUSDataSet.o \ + $(BUILDDIR)/OPUSRegVar.o + +all: $(OBJ) + +$(BUILDDIR)/%.o: %.cpp %.h + $(CPP) $(CFLAGS) $(DEFINES) $(CCOPT) ${INCLUDE} -c $< -o $@ + +clean: + $(RM) *.o *.gcno *.gcda diff --git a/datapool/PIDReader.h b/datapool/PIDReader.h new file mode 100644 index 00000000..452abe40 --- /dev/null +++ b/datapool/PIDReader.h @@ -0,0 +1,152 @@ +/* + * PIDReader.h + * + * Created on: 14.05.2014 + * Author: baetz + */ + +#ifndef PIDREADER_H_ +#define PIDREADER_H_ +#include +#include +#include +#include +#include +#include + +template class PIDReaderList; + +template +class PIDReader: public PoolVariableIF { + template friend class PIDReaderList; +protected: + uint32_t parameterId; + uint8_t valid; + ReturnValue_t read() { + uint8_t arrayIndex = DataPool::PIDToArrayIndex(parameterId); + PoolEntry* read_out = ::dataPool.getData( + DataPool::PIDToDataPoolId(parameterId), arrayIndex); + if (read_out != NULL) { + valid = read_out->valid; + value = read_out->address[arrayIndex]; + return HasReturnvaluesIF::RETURN_OK; + } else { + value = 0; + valid = false; + error << "PIDReader: read of PID 0x" << std::hex << parameterId + << std::dec << " failed." << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + } + /** + * Never commit, is read-only. + * Reason is the possibility to access a single DP vector element, but if we commit, + * we set validity of the whole vector. + */ + ReturnValue_t commit() { + return HasReturnvaluesIF::RETURN_FAILED; + } + /** + * Empty ctor for List initialization + */ + PIDReader() : + parameterId(PoolVariableIF::NO_PARAMETER), valid(PoolVariableIF::INVALID), value(0) { + + } +public: + /** + * \brief This is the local copy of the data pool entry. + */ + T value; + /** + * \brief In the constructor, the variable can register itself in a DataSet (if not NULL is + * passed). + * \details It DOES NOT fetch the current value from the data pool, but sets the value + * attribute to default (0). The value is fetched within the read() operation. + * \param set_id This is the id in the global data pool this instance of the access class + * corresponds to. + * \param dataSet The data set in which the variable shall register itself. If NULL, + * the variable is not registered. + * \param setWritable If this flag is set to true, changes in the value attribute can be + * written back to the data pool, otherwise not. + */ + PIDReader(uint32_t setParameterId, DataSetIF* dataSet) : + parameterId(setParameterId), valid( + PoolVariableIF::INVALID), value(0) { + if (dataSet != NULL) { + dataSet->registerVariable(this); + } + } + + /** + * Copy ctor to copy classes containing Pool Variables. + */ + PIDReader(const PIDReader& rhs) : + parameterId(rhs.parameterId), valid(rhs.valid), value(rhs.value) { + } + + /** + * \brief The classes destructor is empty. + */ + ~PIDReader() { + + } + /** + * \brief This operation returns the data pool id of the variable. + */ + uint32_t getDataPoolId() const { + return DataPool::PIDToDataPoolId(parameterId); + } + uint32_t getParameterId() const { + return parameterId; + } + /** + * This method returns if the variable is write-only, read-write or read-only. + */ + ReadWriteMode_t getReadWriteMode() const { + return VAR_READ; + } + /** + * \brief With this call, the valid information of the variable is returned. + */ + bool isValid() const { + if (valid) + return true; + else + return false; + } + + uint8_t getValid() { + return valid; + } + + void setValid(uint8_t valid) { + this->valid = valid; + } + + operator T() { + return value; + } + + PIDReader &operator=(T newValue) { + value = newValue; + return *this; + } + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + return SerializeAdapter::serialize(&value, buffer, size, max_size, + bigEndian); + } + + virtual uint32_t getSerializedSize() const { + return SerializeAdapter::getSerializedSize(&value); + } + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return SerializeAdapter::deSerialize(&value, buffer, size, bigEndian); + } +}; + +#endif /* PIDREADER_H_ */ diff --git a/datapool/PIDReaderList.h b/datapool/PIDReaderList.h new file mode 100644 index 00000000..3eb8fc5d --- /dev/null +++ b/datapool/PIDReaderList.h @@ -0,0 +1,34 @@ +/* + * PIDReaderList.h + * + * Created on: 15.07.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_DATAPOOL_PIDREADERLIST_H_ +#define FRAMEWORK_DATAPOOL_PIDREADERLIST_H_ + +#include +#include +template +class PIDReaderList { +private: + PIDReader variables[n_var]; +public: + PIDReaderList( const uint32_t setPid[n_var], DataSetIF* dataSet) { + //I really should have a look at the new init list c++ syntax. + if (dataSet == NULL) { + return; + } + for (uint8_t count = 0; count < n_var; count++) { + variables[count].parameterId = setPid[count]; + dataSet->registerVariable(&variables[count]); + } + } + + PIDReader &operator [](int i) { return variables[i]; } +}; + + + +#endif /* FRAMEWORK_DATAPOOL_PIDREADERLIST_H_ */ diff --git a/datapool/PoolEntry.cpp b/datapool/PoolEntry.cpp new file mode 100644 index 00000000..bc882ae3 --- /dev/null +++ b/datapool/PoolEntry.cpp @@ -0,0 +1,73 @@ +/* + * PoolEntry.cpp + * + * Created on: Oct 25, 2012 + * Author: baetz + */ + +#include +#include + +template +PoolEntry::PoolEntry( T* initValue, uint8_t set_length, uint8_t set_valid ) : length(set_length), valid(set_valid) { + this->address = new T[this->length]; + if (initValue != NULL) { + memcpy(this->address, initValue, this->getByteSize() ); + } else { + memset(this->address, 0, this->getByteSize() ); + } +} + +//As the data pool is global, this dtor is only be called on program exit. +//Warning! Never copy pool entries! +template +PoolEntry::~PoolEntry() { + delete[] this->address; +} + +template +uint16_t PoolEntry::getByteSize() { + return ( sizeof(T) * this->length ); +} + +template +uint8_t PoolEntry::getSize() { + return this->length; +} + +template +void* PoolEntry::getRawData() { + return this->address; +} + +template +void PoolEntry::setValid( uint8_t isValid ) { + this->valid = isValid; +} + +template +uint8_t PoolEntry::getValid() { + return valid; +} + +template +void PoolEntry::print() { + for (uint8_t size = 0; size < this->length; size++ ) { + debug << "| " << std::hex << (double)this->address[size] << (this->valid? " (valid) " : " (invalid) "); + } + debug << std::dec << std::endl; +} + +template +Type PoolEntry::getType() { + return PodTypeConversion::type; +} + +template class PoolEntry; +template class PoolEntry; +template class PoolEntry; +template class PoolEntry; +template class PoolEntry; +template class PoolEntry; +template class PoolEntry; +template class PoolEntry; diff --git a/datapool/PoolEntry.h b/datapool/PoolEntry.h new file mode 100644 index 00000000..5277d28b --- /dev/null +++ b/datapool/PoolEntry.h @@ -0,0 +1,84 @@ +#ifndef POOLENTRY_H_ +#define POOLENTRY_H_ + + +#include +#include +/** + * \brief This is a small helper class that defines a single data pool entry. + * + * \details The helper is used to store all information together with the data as a single data pool entry. + * The content's type is defined by the template argument. + * It is prepared for use with plain old data types, + * but may be extended to complex types if necessary. + * It can be initialized with a certain value, size and validity flag. + * It holds a pointer to the real data and offers methods to access this data and to acquire + * additional information (such as validity and array/byte size). + * It is NOT intended to be used outside the DataPool class. + * + * \ingroup data_pool + * + */ +template +class PoolEntry : public PoolEntryIF { +public: + /** + * \brief In the classe's constructor, space is allocated on the heap and + * potential init values are copied to that space. + * \param initValue A pointer to the single value or array that holds the init value. + * With the default value (NULL), the entry is initalized with all 0. + * \param set_length Defines the array length of this entry. + * \param set_valid Sets the initialization flag. It is invalid (0) by default. + */ + PoolEntry( T* initValue = NULL, uint8_t set_length = 1, uint8_t set_valid = 0 ); + /** + * \brief The allocated memory for the variable is freed in the destructor. + * \details As the data pool is global, this dtor is only called on program exit. + * PoolEntries shall never be copied, as a copy might delete the variable on the heap. + */ + ~PoolEntry(); + /** + * \brief This is the address pointing to the allocated memory. + */ + T* address; + /** + * \brief This attribute stores the length information. + */ + uint8_t length; + /** + * \brief Here, the validity information for a variable is stored. + * Every entry (single variable or vector) has one valid flag. + */ + uint8_t valid; + /** + * \brief getSize returns the array size of the entry. + * \details A single parameter has size 1. + */ + uint8_t getSize(); + /** + * \brief This operation returns the size in bytes. + * \details The size is calculated by sizeof(type) * array_size. + */ + uint16_t getByteSize(); + /** + * \brief This operation returns a the address pointer casted to void*. + */ + void* getRawData(); + /** + * \brief This method allows to set the valid information of the pool entry. + */ + void setValid( uint8_t isValid ); + /** + * \brief This method allows to get the valid information of the pool entry. + */ + uint8_t getValid(); + /** + * \brief This is a debug method that prints all values and the valid information to the screen. + * It prints all array entries in a row. + */ + void print(); + + Type getType(); +}; + +#endif /* POOLENTRY_H_ */ diff --git a/datapool/PoolEntryIF.h b/datapool/PoolEntryIF.h new file mode 100644 index 00000000..6ea1c8b7 --- /dev/null +++ b/datapool/PoolEntryIF.h @@ -0,0 +1,67 @@ +/** + * \file PoolEntryIF.h + * + * \brief This file holds the class that defines the Interface for Pool Entry elements. + * + * \date 10/18/2012 + * + * \author Bastian Baetz + */ + +#ifndef POOLENTRYIF_H_ +#define POOLENTRYIF_H_ + +#include +#include + + +/** + * \brief This interface defines the access possibilities to a single data pool entry. + * + * \details The interface provides methods to determine the size and the validity information of a value. + * It also defines a method to receive a pointer to the raw data content. + * It is mainly used by DataPool itself, but also as a return pointer. + * + * \ingroup data_pool + * + */ +class PoolEntryIF { +public: + /** + * \brief This is an empty virtual destructor, as it is proposed for C++ interfaces. + */ + virtual ~PoolEntryIF() { + } + /** + * \brief getSize returns the array size of the entry. A single variable parameter has size 1. + */ + virtual uint8_t getSize() = 0; + /** + * \brief This operation returns the size in bytes, which is calculated by + * sizeof(type) * array_size. + */ + virtual uint16_t getByteSize() = 0; + /** + * \brief This operation returns a the address pointer casted to void*. + */ + virtual void* getRawData() = 0; + /** + * \brief This method allows to set the valid information of the pool entry. + */ + virtual void setValid(uint8_t isValid) = 0; + /** + * \brief This method allows to set the valid information of the pool entry. + */ + virtual uint8_t getValid() = 0; + /** + * \brief This is a debug method that prints all values and the valid information to the screen. + * It prints all array entries in a row. + */ + virtual void print() = 0; + /** + * Returns the type of the entry. + */ + virtual Type getType() = 0; +}; + +#endif /* POOLENTRYIF_H_ */ diff --git a/datapool/PoolRawAccess.cpp b/datapool/PoolRawAccess.cpp new file mode 100644 index 00000000..d1e35b82 --- /dev/null +++ b/datapool/PoolRawAccess.cpp @@ -0,0 +1,194 @@ +/* + * PoolRawAccess.cpp + * + * Created on: 29.10.2012 + * Author: baetz + */ + +#include +#include +#include +#include +PoolRawAccess::PoolRawAccess(uint32_t set_id, uint8_t setArrayEntry, + DataSetIF* data_set, ReadWriteMode_t setReadWriteMode) : + dataPoolId(set_id), arrayEntry(setArrayEntry), valid(false), typeSize( + 0), sizeTillEnd(0), readWriteMode(setReadWriteMode) { + memset(value, 0, sizeof(value)); + if (data_set != NULL) { + data_set->registerVariable(this); + } +} + +PoolRawAccess::~PoolRawAccess() { + +} + +ReturnValue_t PoolRawAccess::read() { + PoolEntryIF* read_out = ::dataPool.getRawData(dataPoolId); + if (read_out != NULL) { + valid = read_out->getValid(); + if (read_out->getSize() > arrayEntry) { + typeSize = read_out->getByteSize() / read_out->getSize(); + if (typeSize <= sizeof(value)) { + uint16_t arrayPosition = arrayEntry * typeSize; + sizeTillEnd = read_out->getByteSize() - arrayPosition; + uint8_t* ptr = + &((uint8_t*) read_out->getRawData())[arrayPosition]; + memcpy(value, ptr, typeSize); + return HasReturnvaluesIF::RETURN_OK; + } else { + //Error value type too large. + } + } else { + //Error index requested too large + } + } else { + //Error entry does not exist. + } + error << "PoolRawAccess: read of DP Variable 0x" << std::hex << dataPoolId + << std::dec << " failed." << std::endl; + valid = INVALID; + typeSize = 0; + sizeTillEnd = 0; + memset(value, 0, sizeof(value)); + return HasReturnvaluesIF::RETURN_FAILED; +} + +ReturnValue_t PoolRawAccess::commit() { + PoolEntryIF* write_back = ::dataPool.getRawData(dataPoolId); + if ((write_back != NULL) && (readWriteMode != VAR_READ)) { + write_back->setValid(valid); + uint8_t array_position = arrayEntry * typeSize; + uint8_t* ptr = &((uint8_t*) write_back->getRawData())[array_position]; + memcpy(ptr, value, typeSize); + return HasReturnvaluesIF::RETURN_OK; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +uint8_t* PoolRawAccess::getEntry() { + return value; +} + +ReturnValue_t PoolRawAccess::getEntryEndianSafe(uint8_t* buffer, + uint32_t* writtenBytes, uint32_t max_size) { + uint8_t* data_ptr = getEntry(); +// debug << "PoolRawAccess::getEntry: Array position: " << index * size_of_type << " Size of T: " << (int)size_of_type << " ByteSize: " << byte_size << " Position: " << *size << std::endl; + if (typeSize == 0) + return DATA_POOL_ACCESS_FAILED; + if (typeSize > max_size) + return INCORRECT_SIZE; +#ifndef BYTE_ORDER +#error BYTE_ORDER not defined +#elif BYTE_ORDER == LITTLE_ENDIAN + for (uint8_t count = 0; count < typeSize; count++) { + buffer[count] = data_ptr[typeSize - count - 1]; + } +#elif BYTE_ORDER == BIG_ENDIAN + memcpy(buffer, data_ptr, typeSize); +#endif + *writtenBytes = typeSize; + return HasReturnvaluesIF::RETURN_OK; +} + +uint8_t PoolRawAccess::getSizeOfType() { + return typeSize; +} + +uint32_t PoolRawAccess::getDataPoolId() const { + return dataPoolId; +} + +PoolVariableIF::ReadWriteMode_t PoolRawAccess::getReadWriteMode() const { + return readWriteMode; +} + +ReturnValue_t PoolRawAccess::setEntryFromBigEndian(const uint8_t* buffer, + uint32_t setSize) { + if (typeSize == setSize) { +#ifndef BYTE_ORDER +#error BYTE_ORDER not defined +#elif BYTE_ORDER == LITTLE_ENDIAN + for (uint8_t count = 0; count < typeSize; count++) { + value[count] = buffer[typeSize - count - 1]; + } +#elif BYTE_ORDER == BIG_ENDIAN + memcpy(value, buffer, typeSize); +#endif + return HasReturnvaluesIF::RETURN_OK; + } else { + error << "PoolRawAccess::setEntryFromBigEndian: Illegal sizes: Internal" + << (uint32_t) typeSize << ", Requested: " << setSize + << std::endl; + return INCORRECT_SIZE; + } +} + +bool PoolRawAccess::isValid() const { + if (valid != INVALID) + return true; + else + return false; +} + +void PoolRawAccess::setValid(uint8_t valid) { + this->valid = valid; +} + +uint16_t PoolRawAccess::getSizeTillEnd() const { + return sizeTillEnd; +} + +ReturnValue_t PoolRawAccess::serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + if (typeSize + *size <= max_size) { + if (bigEndian) { +#ifndef BYTE_ORDER +#error BYTE_ORDER not defined +#elif BYTE_ORDER == LITTLE_ENDIAN + for (uint8_t count = 0; count < typeSize; count++) { + (*buffer)[count] = value[typeSize - count - 1]; + } +#elif BYTE_ORDER == BIG_ENDIAN + memcpy(*buffer, value, typeSize); +#endif + } else { + memcpy(*buffer, value, typeSize); + } + *size += typeSize; + (*buffer) += typeSize; + return HasReturnvaluesIF::RETURN_OK; + } else { + return SerializeIF::BUFFER_TOO_SHORT; + } +} + +uint32_t PoolRawAccess::getSerializedSize() const { + return typeSize; +} + +ReturnValue_t PoolRawAccess::deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + *size -= typeSize; + if (*size >= 0) { + + if (bigEndian) { +#ifndef BYTE_ORDER +#error BYTE_ORDER not defined +#elif BYTE_ORDER == LITTLE_ENDIAN + for (uint8_t count = 0; count < typeSize; count++) { + value[count] = (*buffer)[typeSize - count - 1]; + } +#elif BYTE_ORDER == BIG_ENDIAN + memcpy(value, *buffer, typeSize); +#endif + } else { + memcpy(value, *buffer, typeSize); + } + *buffer += typeSize; + return HasReturnvaluesIF::RETURN_OK; + } else { + return SerializeIF::STREAM_TOO_SHORT; + } +} diff --git a/datapool/PoolRawAccess.h b/datapool/PoolRawAccess.h new file mode 100644 index 00000000..1cc51c85 --- /dev/null +++ b/datapool/PoolRawAccess.h @@ -0,0 +1,135 @@ +#ifndef POOLRAWACCESS_H_ +#define POOLRAWACCESS_H_ + +#include +#include + +/** + * This class allows accessing Data Pool variables as raw bytes. + * This is necessary to have an access method for HK data, as the PID's alone do not + * provide a type information. + * \ingroup data_pool + */ +class PoolRawAccess: public PoolVariableIF { +private: + /** + * \brief To access the correct data pool entry on read and commit calls, the data pool id + * is stored. + */ + uint32_t dataPoolId; + /** + * \brief The array entry that is fetched from the data pool. + */ + uint8_t arrayEntry; + /** + * \brief The valid information as it was stored in the data pool is copied to this attribute. + */ + uint8_t valid; + /** + * \brief This value contains the size of the data pool entry in bytes. + */ + uint8_t typeSize; + /** + * The size (in bytes) from the selected entry till the end of this DataPool variable. + */ + uint16_t sizeTillEnd; + /** + * \brief The information whether the class is read-write or read-only is stored here. + */ + ReadWriteMode_t readWriteMode; + static const uint8_t RAW_MAX_SIZE = sizeof(double); +protected: + /** + * \brief This is a call to read the value from the global data pool. + * \details When executed, this operation tries to fetch the pool entry with matching + * data pool id from the global data pool and copies the value and the valid + * information to its local attributes. In case of a failure (wrong type or + * pool id not found), the variable is set to zero and invalid. + * The operation does NOT provide any mutual exclusive protection by itself. + */ + ReturnValue_t read(); + /** + * \brief The commit call writes back the variable's value to the data pool. + * \details It checks type and size, as well as if the variable is writable. If so, + * the value is copied and the valid flag is automatically set to "valid". + * The operation does NOT provide any mutual exclusive protection by itself. + * + */ + ReturnValue_t commit(); +public: + static const uint8_t INTERFACE_ID = POOL_RAW_ACCESS_CLASS; + static const ReturnValue_t INCORRECT_SIZE = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t DATA_POOL_ACCESS_FAILED = MAKE_RETURN_CODE(0x02); + uint8_t value[RAW_MAX_SIZE]; + PoolRawAccess(uint32_t data_pool_id, uint8_t arrayEntry, + DataSetIF* data_set, ReadWriteMode_t setReadWriteMode = + PoolVariableIF::VAR_READ); + /** + * \brief The classes destructor is empty. If commit() was not called, the local value is + * discarded and not written back to the data pool. + */ + ~PoolRawAccess(); + /** + * \brief This operation returns a pointer to the entry fetched. + * \details This means, it does not return a pointer to byte "index", but to the start byte of + * array entry "index". Example: If the original data pool array consists of an double + * array of size four, getEntry(1) returns &(this->value[8]). + */ + uint8_t* getEntry(); + /** + * \brief This operation returns the fetched entry from the data pool and + * flips the bytes, if necessary. + * \details It makes use of the getEntry call of this function, but additionally flips the + * bytes to big endian, which is the default for external communication (as House- + * keeping telemetry). To achieve this, the data is copied directly to the passed + * buffer, if it fits in the given max_size. + * \param buffer A pointer to a buffer to write to + * \param writtenBytes The number of bytes written is returned with this value. + * \param max_size The maximum size that the function may write to buffer. + * \return - \c RETURN_OK if entry could be acquired + * - \c RETURN_FAILED else. + */ + ReturnValue_t getEntryEndianSafe(uint8_t* buffer, uint32_t* size, + uint32_t max_size); + /** + * With this method, the content can be set from a big endian buffer safely. + * @param buffer Pointer to the data to set + * @param size Size of the data to write. Must fit this->size. + * @return - \c RETURN_OK on success + * - \c RETURN_FAILED on failure + */ + ReturnValue_t setEntryFromBigEndian(const uint8_t* buffer, + uint32_t setSize); + /** + * \brief This operation returns the size of the entry currently stored. + */ + uint8_t getSizeOfType(); + /** + * \brief This operation returns the data pool id of the variable. + */ + uint32_t getDataPoolId() const; + /** + * This method returns if the variable is read-write or read-only. + */ + ReadWriteMode_t getReadWriteMode() const; + /** + * \brief With this call, the valid information of the variable is returned. + */ + bool isValid() const; + + void setValid(uint8_t valid); + /** + * Getter for the remaining size. + */ + uint16_t getSizeTillEnd() const; + + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const; + + uint32_t getSerializedSize() const; + + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian); +}; + +#endif /* POOLRAWACCESS_H_ */ diff --git a/datapool/PoolVarList.h b/datapool/PoolVarList.h new file mode 100644 index 00000000..1a8bd880 --- /dev/null +++ b/datapool/PoolVarList.h @@ -0,0 +1,35 @@ +/* + * PoolVarList.h + * + * Created on: 06.03.2014 + * Author: baetz + */ + +#ifndef POOLVARLIST_H_ +#define POOLVARLIST_H_ + +#include +#include +template +class PoolVarList { +private: + PoolVariable variables[n_var]; +public: + PoolVarList( const uint32_t set_id[n_var], DataSetIF* dataSet, PoolVariableIF::ReadWriteMode_t setReadWriteMode ) { + //I really should have a look at the new init list c++ syntax. + if (dataSet == NULL) { + return; + } + for (uint8_t count = 0; count < n_var; count++) { + variables[count].dataPoolId = set_id[count]; + variables[count].readWriteMode = setReadWriteMode; + dataSet->registerVariable(&variables[count]); + } + } + + PoolVariable &operator [](int i) { return variables[i]; } +}; + + + +#endif /* POOLVARLIST_H_ */ diff --git a/datapool/PoolVariable.h b/datapool/PoolVariable.h new file mode 100644 index 00000000..25345c0a --- /dev/null +++ b/datapool/PoolVariable.h @@ -0,0 +1,295 @@ +/* + * \file PoolVariable.h + * + * \brief This file contains the PoolVariable class, which locally represents a non-array data pool variable. + * + * \date 10/17/2012 + * + * \author Bastian Baetz + */ + +#ifndef POOLVARIABLE_H_ +#define POOLVARIABLE_H_ + +#include +#include +#include +#include +#include + +template class PoolVarList; + +/** + * \brief This is the access class for non-array data pool entries. + * + * \details To ensure safe usage of the data pool, operation is not done directly on the data pool + * entries, but on local copies. This class provides simple type-safe access to single + * data pool entries (i.e. entries with length = 1). + * The class can be instantiated as read-write and read only. + * It provides a commit-and-roll-back semantic, which means that the variable's value in + * the data pool is not changed until the commit call is executed. + * \tparam T The template parameter sets the type of the variable. Currently, all plain data types + * are supported, but in principle any type is possible. + * \ingroup data_pool + */ +template +class PoolVariable: public PoolVariableIF { + template friend class PoolVarList; +protected: + /** + * \brief To access the correct data pool entry on read and commit calls, the data pool id + * is stored. + */ + uint32_t dataPoolId; + /** + * \brief The valid information as it was stored in the data pool is copied to this attribute. + */ + uint8_t valid; + /** + * \brief The information whether the class is read-write or read-only is stored here. + */ + ReadWriteMode_t readWriteMode; + /** + * \brief This is a call to read the value from the global data pool. + * \details When executed, this operation tries to fetch the pool entry with matching + * data pool id from the global data pool and copies the value and the valid + * information to its local attributes. In case of a failure (wrong type or + * pool id not found), the variable is set to zero and invalid. + * The operation does NOT provide any mutual exclusive protection by itself. + */ + ReturnValue_t read() { + PoolEntry* read_out = ::dataPool.getData(dataPoolId, 1); + if (read_out != NULL) { + valid = read_out->valid; + value = *(read_out->address); + return HasReturnvaluesIF::RETURN_OK; + } else { + value = 0; + valid = false; + error << "PoolVariable: read of DP Variable 0x" << std::hex + << dataPoolId << std::dec << " failed." << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + } + /** + * \brief The commit call writes back the variable's value to the data pool. + * \details It checks type and size, as well as if the variable is writable. If so, + * the value is copied and the valid flag is automatically set to "valid". + * The operation does NOT provide any mutual exclusive protection by itself. + * + */ + ReturnValue_t commit() { + PoolEntry* write_back = ::dataPool.getData(dataPoolId, 1); + if ((write_back != NULL) && (readWriteMode != VAR_READ)) { + write_back->valid = valid; + *(write_back->address) = value; + return HasReturnvaluesIF::RETURN_OK; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + /** + * Empty ctor for List initialization + */ + PoolVariable() : + dataPoolId(PoolVariableIF::NO_PARAMETER), valid( + PoolVariableIF::INVALID), readWriteMode(VAR_READ), value(0) { + + } +public: + /** + * \brief This is the local copy of the data pool entry. + * \details The user can work on this attribute + * just like he would on a simple local variable. + */ + T value; + /** + * \brief In the constructor, the variable can register itself in a DataSet (if not NULL is + * passed). + * \details It DOES NOT fetch the current value from the data pool, but sets the value + * attribute to default (0). The value is fetched within the read() operation. + * \param set_id This is the id in the global data pool this instance of the access class + * corresponds to. + * \param dataSet The data set in which the variable shall register itself. If NULL, + * the variable is not registered. + * \param setWritable If this flag is set to true, changes in the value attribute can be + * written back to the data pool, otherwise not. + */ + PoolVariable(uint32_t set_id, DataSetIF* dataSet, + ReadWriteMode_t setReadWriteMode) : + dataPoolId(set_id), valid(PoolVariableIF::INVALID), readWriteMode( + setReadWriteMode), value(0) { + if (dataSet != NULL) { + dataSet->registerVariable(this); + } + } + /** + * Copy ctor to copy classes containing Pool Variables. + */ + PoolVariable(const PoolVariable& rhs) : + dataPoolId(rhs.dataPoolId), valid(rhs.valid), readWriteMode( + rhs.readWriteMode), value(rhs.value) { + } + + /** + * \brief The classes destructor is empty. + * \details If commit() was not called, the local value is + * discarded and not written back to the data pool. + */ + ~PoolVariable() { + + } + /** + * \brief This operation returns the data pool id of the variable. + */ + uint32_t getDataPoolId() const { + return dataPoolId; + } + /** + * This operation sets the data pool id of the variable. + * The method is necessary to set id's of data pool member variables with bad initialization. + */ + void setDataPoolId(uint32_t poolId) { + dataPoolId = poolId; + } + /** + * This method returns if the variable is write-only, read-write or read-only. + */ + ReadWriteMode_t getReadWriteMode() const { + return readWriteMode; + } + /** + * \brief With this call, the valid information of the variable is returned. + */ + bool isValid() const { + if (valid) + return true; + else + return false; + } + + uint8_t getValid() { + return valid; + } + + void setValid(uint8_t valid) { + this->valid = valid; + } + + operator T() { + return value; + } + + operator T() const { + return value; + } + + PoolVariable &operator=(T newValue) { + value = newValue; + return *this; + } + + PoolVariable &operator=(PoolVariable newPoolVariable) { + value = newPoolVariable.value; + return *this; + } + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + return SerializeAdapter::serialize(&value, buffer, size, max_size, + bigEndian); + } + + virtual uint32_t getSerializedSize() const { + return SerializeAdapter::getSerializedSize(&value); + } + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return SerializeAdapter::deSerialize(&value, buffer, size, bigEndian); + } +}; + +typedef PoolVariable db_uint8_t; +typedef PoolVariable db_uint16_t; +typedef PoolVariable db_uint32_t; +typedef PoolVariable db_int8_t; +typedef PoolVariable db_int16_t; +typedef PoolVariable db_int32_t; +typedef PoolVariable db_bool_t; +typedef PoolVariable db_float_t; +typedef PoolVariable db_double_t; +//Alternative (but I thing this is not as useful: code duplication, differences too small): + +//template +//class PoolReader : public PoolVariableIF { +//private: +// uint32_t parameter_id; +// uint8_t valid; +//public: +// T value; +// PoolReader( uint32_t set_id, DataSetIF* set ) : parameter_id(set_id), valid(false), value(0) { +// set->registerVariable( this ); +// } +// +// ~PoolReader() {}; +// +// uint8_t commit() { +// return HasReturnvaluesIF::RETURN_OK; +// } +// +// uint8_t read() { +// PoolEntry* read_out = ::dataPool.getData( parameter_id, 1 ); +// if ( read_out != NULL ) { +// valid = read_out->valid; +// value = *(read_out->address); +// return HasReturnvaluesIF::RETURN_OK; +// } else { +// value = 0; +// valid = false; +// return CHECKOUT_FAILED; +// } +// } +// uint32_t getParameterId() { return parameter_id; } +// bool isWritable() { return false; }; +// bool isValid() { if (valid) return true; else return false; } +//}; +// +//template +//class PoolWriter : public PoolVariableIF { +//private: +// uint32_t parameter_id; +//public: +// T value; +// PoolWriter( uint32_t set_id, DataSetIF* set ) : parameter_id(set_id), value(0) { +// set->registerVariable( this ); +// } +// +// ~PoolWriter() {}; +// +// uint8_t commit() { +// PoolEntry* write_back = ::dataPool.getData( parameter_id, 1 ); +// if ( write_back != NULL ) { +// write_back->valid = true; +// *(write_back->address) = value; +// return HasReturnvaluesIF::RETURN_OK; +// } else { +// return CHECKOUT_FAILED; +// } +// } +// uint8_t read() { +// PoolEntry* read_out = ::dataPool.getData( parameter_id, 1 ); +// if ( read_out != NULL ) { +// value = *(read_out->address); +// return HasReturnvaluesIF::RETURN_OK; +// } else { +// value = 0; +// return CHECKOUT_FAILED; +// } +// } +// uint32_t getParameterId() { return parameter_id; } +// bool isWritable() { return true; }; +// bool isValid() { return false; } +//}; + +#endif /* POOLVARIABLE_H_ */ diff --git a/datapool/PoolVariableIF.h b/datapool/PoolVariableIF.h new file mode 100644 index 00000000..7626c9c1 --- /dev/null +++ b/datapool/PoolVariableIF.h @@ -0,0 +1,71 @@ +/* + * \file PoolVariableIF.h + * + * \brief This file contains the interface definition for pool variables. + * + * \date 10/17/2012 + * + * \author Bastian Baetz + */ + +#ifndef POOLVARIABLEIF_H_ +#define POOLVARIABLEIF_H_ + +#include +#include + +/** + * \brief This interface is used to control local data pool variable representations. + * + * \details To securely handle data pool variables, all pool entries are locally managed by + * data pool variable access classes, which are called pool variables. To ensure a + * common state of a set of variables needed in a function, these local pool variables + * again are managed by other classes, e.g. the DataSet. This interface provides unified + * access to local pool variables for such manager classes. + * \ingroup data_pool + */ +class PoolVariableIF : public SerializeIF { + friend class DataSet; +protected: + /** + * \brief The commit call shall write back a newly calculated local value to the data pool. + */ + virtual ReturnValue_t commit() = 0; + /** + * \brief The read call shall read the value of this parameter from the data pool and store + * the content locally. + */ + virtual ReturnValue_t read() = 0; +public: + static const uint8_t VALID = 1; + static const uint8_t INVALID = 0; + static const uint32_t NO_PARAMETER = 0; + enum ReadWriteMode_t { + VAR_READ, VAR_WRITE, VAR_READ_WRITE + }; + + /** + * \brief This is an empty virtual destructor, as it is proposed for C++ interfaces. + */ + virtual ~PoolVariableIF() { + } + /** + * \brief This method returns if the variable is write-only, read-write or read-only. + */ + virtual ReadWriteMode_t getReadWriteMode() const = 0; + /** + * \brief This operation shall return the data pool id of the variable. + */ + virtual uint32_t getDataPoolId() const = 0; + /** + * \brief With this call, the valid information of the variable is returned. + */ + virtual bool isValid() const = 0; + /** + * \brief With this call, the valid information of the variable is set. + */ + virtual void setValid(uint8_t validity) = 0; + +}; + +#endif /* POOLVARIABLEIF_H_ */ diff --git a/datapool/PoolVector.h b/datapool/PoolVector.h new file mode 100644 index 00000000..d3617d17 --- /dev/null +++ b/datapool/PoolVector.h @@ -0,0 +1,233 @@ +/* + * \file PoolVector.h + * + * \brief This file contains the PoolVector class, the header only class to handle data pool vectors. + * + * \date 10/23/2012 + * + * \author Bastian Baetz + */ + +#ifndef POOLVECTOR_H_ +#define POOLVECTOR_H_ + +#include +#include +#include +#include +#include + +/** + * \brief This is the access class for array-type data pool entries. + * + * \details To ensure safe usage of the data pool, operation is not done directly on the data pool + * entries, but on local copies. This class provides simple type- and length-safe access + * to vector-style data pool entries (i.e. entries with length > 1). + * The class can be instantiated as read-write and read only. + * It provides a commit-and-roll-back semantic, which means that no array entry in + * the data pool is changed until the commit call is executed. + * There are two template parameters: + * \tparam T This template parameter specifies the data type of an array entry. Currently, all + * plain data types are supported, but in principle any type is possible. + * \tparam vector_size This template parameter specifies the vector size of this entry. + * Using a template parameter for this is not perfect, but avoids dynamic memory allocation. + * \ingroup data_pool + */ +template +class PoolVector: public PoolVariableIF { +private: + /** + * \brief To access the correct data pool entry on read and commit calls, the data pool id + * is stored. + */ + uint32_t dataPoolId; + /** + * \brief The valid information as it was stored in the data pool is copied to this attribute. + */ + uint8_t valid; + /** + * \brief The information whether the class is read-write or read-only is stored here. + */ + ReadWriteMode_t readWriteMode; + +protected: + /** + * \brief This is a call to read the array's values from the global data pool. + * \details When executed, this operation tries to fetch the pool entry with matching + * data pool id from the global data pool and copies all array values and the valid + * information to its local attributes. In case of a failure (wrong type, size or + * pool id not found), the variable is set to zero and invalid. + * The operation does NOT provide any mutual exclusive protection by itself. + */ + ReturnValue_t read() { + PoolEntry* read_out = ::dataPool.getData(this->dataPoolId, + vector_size); + if (read_out != NULL) { + this->valid = read_out->valid; + memcpy(this->value, read_out->address, read_out->getByteSize()); + + return HasReturnvaluesIF::RETURN_OK; + + } else { + memset(this->value, 0, vector_size * sizeof(T)); + error << "PoolVector: read of DP Variable 0x" << std::hex + << dataPoolId << std::dec << " failed." << std::endl; + this->valid = INVALID; + return HasReturnvaluesIF::RETURN_FAILED; + } + } + /** + * \brief The commit call copies the array values back to the data pool. + * \details It checks type and size, as well as if the variable is writable. If so, + * the value is copied and the valid flag is automatically set to "valid". + * The operation does NOT provide any mutual exclusive protection by itself. + * + */ + ReturnValue_t commit() { + PoolEntry* write_back = ::dataPool.getData(this->dataPoolId, + vector_size); + if ((write_back != NULL) && (this->readWriteMode != VAR_READ)) { + write_back->valid = valid; + memcpy(write_back->address, this->value, write_back->getByteSize()); + return HasReturnvaluesIF::RETURN_OK; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } + } +public: + /** + * \brief This is the local copy of the data pool entry. + * \detials The user can work on this attribute + * just like he would on a local array of this type. + */ + T value[vector_size]; + /** + * \brief In the constructor, the variable can register itself in a DataSet (if not NULL is + * passed). + * \details It DOES NOT fetch the current value from the data pool, but sets the value + * attribute to default (0). The value is fetched within the read() operation. + * \param set_id This is the id in the global data pool this instance of the access class + * corresponds to. + * \param dataSet The data set in which the variable shall register itself. If NULL, + * the variable is not registered. + * \param setWritable If this flag is set to true, changes in the value attribute can be + * written back to the data pool, otherwise not. + */ + PoolVector(uint32_t set_id, DataSetIF* set, + ReadWriteMode_t setReadWriteMode) : + dataPoolId(set_id), valid(false), readWriteMode(setReadWriteMode) { + memset(this->value, 0, vector_size * sizeof(T)); + if (set != NULL) { + set->registerVariable(this); + } + } + /** + * Copy ctor to copy classes containing Pool Variables. + */ +// PoolVector(const PoolVector& rhs) { +// PoolVector temp(rhs.dataPoolId, rhs.) +// memcpy(value, rhs.value, sizeof(T)*vector_size); +// } + /** + * \brief The classes destructor is empty. + * \details If commit() was not called, the local value is + * discarded and not written back to the data pool. + */ + ~PoolVector() { + } + ; + /** + * \brief The operation returns the number of array entries in this variable. + */ + uint8_t getSize() { + return vector_size; + } + /** + * \brief This operation returns the data pool id of the variable. + */ + uint32_t getDataPoolId() const { + return dataPoolId; + } + /** + * This operation sets the data pool id of the variable. + * The method is necessary to set id's of data pool member variables with bad initialization. + */ + void setDataPoolId(uint32_t poolId) { + dataPoolId = poolId; + } + /** + * This method returns if the variable is write-only, read-write or read-only. + */ + ReadWriteMode_t getReadWriteMode() const { + return readWriteMode; + } + ; + /** + * \brief With this call, the valid information of the variable is returned. + */ + bool isValid() const { + if (valid != INVALID) + return true; + else + return false; + } + + void setValid(uint8_t valid) { + this->valid = valid; + } + + uint8_t getValid() { + return valid; + } + + T &operator [](int i) { + return value[i]; + } + + const T &operator [](int i) const { + return value[i]; + } + + PoolVector &operator=( + PoolVector newPoolVector) { + + for (uint16_t i = 0; i < vector_size; i++) { + this->value[i] = newPoolVector.value[i]; + } + return *this; + } + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + uint16_t i; + ReturnValue_t result; + for (i = 0; i < vector_size; i++) { + result = SerializeAdapter::serialize(&(value[i]), buffer, size, + max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + } + return result; + } + + virtual uint32_t getSerializedSize() const { + return vector_size * SerializeAdapter::getSerializedSize(value); + } + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + uint16_t i; + ReturnValue_t result; + for (i = 0; i < vector_size; i++) { + result = SerializeAdapter::deSerialize(&(value[i]), buffer, size, + bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + } + return result; + } +}; + +#endif /* POOLVECTOR_H_ */ diff --git a/devicehandlers/AssemblyBase.cpp b/devicehandlers/AssemblyBase.cpp new file mode 100644 index 00000000..ae43e896 --- /dev/null +++ b/devicehandlers/AssemblyBase.cpp @@ -0,0 +1,280 @@ +#include + +AssemblyBase::AssemblyBase(object_id_t objectId, object_id_t parentId, + uint16_t commandQueueDepth) : + SubsystemBase(objectId, parentId, MODE_OFF, commandQueueDepth), internalState( + STATE_NONE), recoveryState(RECOVERY_IDLE), recoveringDevice( + childrenMap.end()), targetMode(MODE_OFF), targetSubmode( + SUBMODE_NONE) { + recoveryOffTimer.setTimeout(POWER_OFF_TIME_MS); +} + +AssemblyBase::~AssemblyBase() { +} + +ReturnValue_t AssemblyBase::handleCommandMessage(CommandMessage* message) { + return handleHealthReply(message); +} + +void AssemblyBase::performChildOperation() { + if (isInTransition()) { + handleChildrenTransition(); + } else { + handleChildrenChanged(); + } +} + +void AssemblyBase::startTransition(Mode_t mode, Submode_t submode) { + doStartTransition(mode, submode); + if (modeHelper.isForced()) { + triggerEvent(FORCING_MODE, mode, submode); + } else { + triggerEvent(CHANGING_MODE, mode, submode); + } +} + +void AssemblyBase::doStartTransition(Mode_t mode, Submode_t submode) { + targetMode = mode; + targetSubmode = submode; + internalState = STATE_SINGLE_STEP; + ReturnValue_t result = commandChildren(mode, submode); + if (result == NEED_SECOND_STEP) { + internalState = STATE_NEED_SECOND_STEP; + } else if (result != RETURN_OK) { + //TODO: Debug + debug << std::hex << getObjectId() + << ": AssemblyBase::commandChildren returned: " << result + << std::dec << std::endl; + } +} + +bool AssemblyBase::isInTransition() { + return (internalState != STATE_NONE) || (recoveryState != RECOVERY_IDLE); +} + +bool AssemblyBase::handleChildrenChanged() { + if (childrenChangedMode) { + ReturnValue_t result = checkChildrenState(); + if (result != RETURN_OK) { + handleChildrenLostMode(result); + } + return true; + } else { + return handleChildrenChangedHealth(); + } +} + +void AssemblyBase::handleChildrenLostMode(ReturnValue_t result) { + triggerEvent(CANT_KEEP_MODE, mode, submode); + startTransition(MODE_OFF, SUBMODE_NONE); +} + +bool AssemblyBase::handleChildrenChangedHealth() { + auto iter = childrenMap.begin(); + for (; iter != childrenMap.end(); iter++) { + if (iter->second.healthChanged) { + iter->second.healthChanged = false; + break; + } + } + if (iter == childrenMap.end()) { + return false; + } + HealthState healthState = healthHelper.healthTable->getHealth(iter->first); + if (healthState == HasHealthIF::NEEDS_RECOVERY) { + triggerEvent(TRYING_RECOVERY); + recoveryState = RECOVERY_STARTED; + recoveringDevice = iter; + doStartTransition(targetMode, targetSubmode); + } else { + triggerEvent(CHILD_CHANGED_HEALTH); + doStartTransition(mode, submode); + } + if (modeHelper.isForced()) { + triggerEvent(FORCING_MODE, targetMode, targetSubmode); + } + return true; +} + +void AssemblyBase::handleChildrenTransition() { + if (commandsOutstanding <= 0) { + switch (internalState) { + case STATE_NEED_SECOND_STEP: + internalState = STATE_SECOND_STEP; + commandChildren(targetMode, targetSubmode); + return; + case STATE_OVERWRITE_HEALTH: { + internalState = STATE_SINGLE_STEP; + ReturnValue_t result = commandChildren(mode, submode); + if (result == NEED_SECOND_STEP) { + internalState = STATE_NEED_SECOND_STEP; + } + return; + } + case STATE_NONE: + //Valid state, used in recovery. + case STATE_SINGLE_STEP: + case STATE_SECOND_STEP: + if (checkAndHandleRecovery()) { + return; + } + break; + } + ReturnValue_t result = checkChildrenState(); + if (result == RETURN_OK) { + handleModeReached(); + } else { + handleModeTransitionFailed(result); + } + } +} + +void AssemblyBase::handleModeReached() { + internalState = STATE_NONE; + setMode(targetMode, targetSubmode); +} + +void AssemblyBase::handleModeTransitionFailed(ReturnValue_t result) { +//always accept transition to OFF, there is nothing we can do except sending an info event +//In theory this should never happen, but we would risk an infinite loop otherwise + if (targetMode == MODE_OFF) { + triggerEvent(CHILD_PROBLEMS, result); + internalState = STATE_NONE; + //TODO: Maybe go to ERROR_ON here. Does this cause problems in subsystem? + setMode(targetMode, targetSubmode); + } else { + if (handleChildrenChangedHealth()) { + //If any health change is pending, handle that first. + return; + } + triggerEvent(MODE_TRANSITION_FAILED, result); + startTransition(MODE_OFF, SUBMODE_NONE); + } +} + +void AssemblyBase::sendHealthCommand(MessageQueueId_t sendTo, + HealthState health) { + CommandMessage command; + HealthMessage::setHealthMessage(&command, HealthMessage::HEALTH_SET, + health); + if (commandQueue.sendMessage(sendTo, &command) == RETURN_OK) { + commandsOutstanding++; + } +} + +ReturnValue_t AssemblyBase::checkChildrenState() { + if (targetMode == MODE_OFF) { + return checkChildrenStateOff(); + } else { + return checkChildrenStateOn(targetMode, targetSubmode); + } +} + +ReturnValue_t AssemblyBase::checkChildrenStateOff() { + for (std::map::iterator iter = childrenMap.begin(); + iter != childrenMap.end(); iter++) { + if (checkChildOff(iter->first) != RETURN_OK) { + return NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE; + } + } + return RETURN_OK; +} + +ReturnValue_t AssemblyBase::checkChildOff(uint32_t objectId) { + ChildInfo childInfo = childrenMap.find(objectId)->second; + if (healthHelper.healthTable->isCommandable(objectId)) { + if (childInfo.submode != SUBMODE_NONE) { + return RETURN_FAILED; + } else { + if ((childInfo.mode != MODE_OFF) + && (childInfo.mode != DeviceHandlerIF::MODE_ERROR_ON)) { + return RETURN_FAILED; + } + } + } + return RETURN_OK; +} + +ReturnValue_t AssemblyBase::checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t* msToReachTheMode) { + +//always accept transition to OFF + if (mode == MODE_OFF) { + if (submode != SUBMODE_NONE) { + return INVALID_SUBMODE; + } + return RETURN_OK; + } + + if ((mode != MODE_ON) && (mode != DeviceHandlerIF::MODE_NORMAL)) { + return INVALID_MODE; + } + + if (internalState != STATE_NONE) { + return IN_TRANSITION; + } + + return isModeCombinationValid(mode, submode); +} + +ReturnValue_t AssemblyBase::handleHealthReply(CommandMessage* message) { + if (message->getCommand() == HealthMessage::HEALTH_INFO) { + HealthState health = HealthMessage::getHealth(message); + if (health != EXTERNAL_CONTROL) { + updateChildChangedHealth(message->getSender(), true); + } + return RETURN_OK; + } + if (message->getCommand() == HealthMessage::REPLY_HEALTH_SET + || (message->getCommand() == CommandMessage::REPLY_REJECTED + && message->getParameter2() == HealthMessage::HEALTH_SET)) { + if (isInTransition()) { + commandsOutstanding--; + } + return RETURN_OK; + } + return RETURN_FAILED; +} + +bool AssemblyBase::checkAndHandleRecovery() { + switch (recoveryState) { + case RECOVERY_STARTED: + recoveryState = RECOVERY_WAIT; + recoveryOffTimer.resetTimer(); + return true; + case RECOVERY_WAIT: + if (recoveryOffTimer.isBusy()) { + return true; + } + triggerEvent(RECOVERY_STEP, 0); + sendHealthCommand(recoveringDevice->second.commandQueue, HEALTHY); + internalState = STATE_NONE; + recoveryState = RECOVERY_ONGOING; + //Don't check state! + return true; + case RECOVERY_ONGOING: + triggerEvent(RECOVERY_STEP, 1); + recoveryState = RECOVERY_ONGOING_2; + recoveringDevice->second.healthChanged = false; + //Device should be healthy again, so restart a transition. + //Might be including second step, but that's already handled. + doStartTransition(targetMode, targetSubmode); + return true; + case RECOVERY_ONGOING_2: + triggerEvent(RECOVERY_DONE); + //Now we're through, but not sure if it was successful. + recoveryState = RECOVERY_IDLE; + return false; + case RECOVERY_IDLE: + default: + return false; + } +} + +void AssemblyBase::overwriteDeviceHealth(object_id_t objectId, + HasHealthIF::HealthState oldHealth) { + triggerEvent(OVERWRITING_HEALTH, objectId, oldHealth); + internalState = STATE_OVERWRITE_HEALTH; + modeHelper.setForced(true); + sendHealthCommand(childrenMap[objectId].commandQueue, EXTERNAL_CONTROL); +} diff --git a/devicehandlers/AssemblyBase.h b/devicehandlers/AssemblyBase.h new file mode 100644 index 00000000..131f0818 --- /dev/null +++ b/devicehandlers/AssemblyBase.h @@ -0,0 +1,132 @@ +#ifndef ASSEMBLYBASE_H_ +#define ASSEMBLYBASE_H_ + +#include +#include +#include + +class AssemblyBase: public SubsystemBase { +public: + static const uint8_t INTERFACE_ID = ASSEMBLY_BASE; + static const ReturnValue_t NEED_SECOND_STEP = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t NEED_TO_RECONFIGURE = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t MODE_FALLBACK = MAKE_RETURN_CODE(0x03); + static const ReturnValue_t CHILD_NOT_COMMANDABLE = MAKE_RETURN_CODE(0x04); + static const ReturnValue_t NEED_TO_CHANGE_HEALTH = MAKE_RETURN_CODE(0x05); + static const ReturnValue_t NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE = + MAKE_RETURN_CODE(0xa1); + + AssemblyBase(object_id_t objectId, object_id_t parentId, uint16_t commandQueueDepth = 8); + virtual ~AssemblyBase(); + +protected: + enum InternalState { + STATE_NONE, + STATE_OVERWRITE_HEALTH, + STATE_NEED_SECOND_STEP, + STATE_SINGLE_STEP, + STATE_SECOND_STEP, + } internalState; + + enum RecoveryState { + RECOVERY_IDLE, + RECOVERY_STARTED, + RECOVERY_ONGOING, + RECOVERY_ONGOING_2, + RECOVERY_WAIT + } recoveryState; //!< Indicates if one of the children requested a recovery. + ChildrenMap::iterator recoveringDevice; + /** + * the mode the current transition is trying to achieve. + * Can be different from the modehelper.commandedMode! + */ + Mode_t targetMode; + + /** + * the submode the current transition is trying to achieve. + * Can be different from the modehelper.commandedSubmode! + */ + Submode_t targetSubmode; + + Countdown recoveryOffTimer; + + static const uint32_t POWER_OFF_TIME_MS = 1000; + + virtual ReturnValue_t handleCommandMessage(CommandMessage *message); + + virtual ReturnValue_t handleHealthReply(CommandMessage *message); + + virtual void performChildOperation(); + + bool handleChildrenChanged(); + + /** + * This method is called if the children changed its mode in a way that the current + * mode can't be kept. + * Default behavior is to go to MODE_OFF. + * @param result The failure code which was returned by checkChildrenState. + */ + virtual void handleChildrenLostMode(ReturnValue_t result); + + bool handleChildrenChangedHealth(); + + virtual void handleChildrenTransition(); + + ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode); + + virtual ReturnValue_t isModeCombinationValid(Mode_t mode, + Submode_t submode) = 0; + + virtual void startTransition(Mode_t mode, Submode_t submode); + + virtual void doStartTransition(Mode_t mode, Submode_t submode); + + virtual bool isInTransition(); + + virtual void handleModeReached(); + + virtual void handleModeTransitionFailed(ReturnValue_t result); + + void sendHealthCommand(MessageQueueId_t sendTo, HealthState health); + + //SHOULDDO: Change that OVERWRITE_HEALTH may be returned (or return internalState directly?) + /** + * command children to reach mode,submode + * + * set #commandsOutstanding correctly, or use executeTable() + * + * @param mode + * @param submode + * @return + * - @c RETURN_OK if ok + * - @c NEED_SECOND_STEP if children need to be commanded again + */ + virtual ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) = 0; + + //SHOULDDO: Remove wantedMode, wantedSubmode, as targetMode/submode is available? + virtual ReturnValue_t checkChildrenStateOn(Mode_t wantedMode, + Submode_t wantedSubmode) = 0; + + virtual ReturnValue_t checkChildrenStateOff(); + + ReturnValue_t checkChildrenState(); + + virtual ReturnValue_t checkChildOff(uint32_t objectId); + + /** + * Manages recovery of a device + * @return true if recovery is still ongoing, false else. + */ + bool checkAndHandleRecovery(); + + /** + * Helper method to overwrite health state of one of the children. + * Also sets state to STATE_OVERWRITE_HEATH. + * @param objectId Must be a registered child. + */ + void overwriteDeviceHealth(object_id_t objectId, HasHealthIF::HealthState oldHealth); + +}; + +#endif /* ASSEMBLYBASE_H_ */ diff --git a/devicehandlers/ChildHandlerBase.cpp b/devicehandlers/ChildHandlerBase.cpp new file mode 100644 index 00000000..c879c597 --- /dev/null +++ b/devicehandlers/ChildHandlerBase.cpp @@ -0,0 +1,42 @@ +#include +#include +#include + +ChildHandlerBase::ChildHandlerBase(uint32_t ioBoardAddress, + object_id_t setObjectId, object_id_t deviceCommunication, + uint32_t maxDeviceReplyLen, uint8_t setDeviceSwitch, + uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, + uint32_t parent, FDIRBase* customFdir, uint32_t cmdQueueSize) : + DeviceHandlerBase(ioBoardAddress, setObjectId, maxDeviceReplyLen, + setDeviceSwitch, deviceCommunication, thermalStatePoolId, + thermalRequestPoolId, (customFdir == NULL? &childHandlerFdir : customFdir), cmdQueueSize), parentId( + parent), childHandlerFdir(setObjectId) { +} + +ChildHandlerBase::~ChildHandlerBase() { +} + +ReturnValue_t ChildHandlerBase::initialize() { + ReturnValue_t result = DeviceHandlerBase::initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + MessageQueueId_t parentQueue = 0; + + if (parentId != 0) { + SubsystemBase *parent = objectManager->get(parentId); + if (parent == NULL) { + return RETURN_FAILED; + } + parentQueue = parent->getCommandQueue(); + + parent->registerChild(getObjectId()); + } + + healthHelper.setParentQeueue(parentQueue); + + modeHelper.setParentQueue(parentQueue); + + return RETURN_OK; +} diff --git a/devicehandlers/ChildHandlerBase.h b/devicehandlers/ChildHandlerBase.h new file mode 100644 index 00000000..7b73d559 --- /dev/null +++ b/devicehandlers/ChildHandlerBase.h @@ -0,0 +1,25 @@ +#ifndef PAYLOADHANDLERBASE_H_ +#define PAYLOADHANDLERBASE_H_ + +#include +#include + +class ChildHandlerBase: public DeviceHandlerBase { +public: + ChildHandlerBase(uint32_t ioBoardAddress, object_id_t setObjectId, + object_id_t deviceCommunication, uint32_t maxDeviceReplyLen, + uint8_t setDeviceSwitch, uint32_t thermalStatePoolId, + uint32_t thermalRequestPoolId, uint32_t parent, + FDIRBase* customFdir = NULL, + uint32_t cmdQueueSize = 20); + virtual ~ChildHandlerBase(); + + virtual ReturnValue_t initialize(); + +protected: + const uint32_t parentId; + ChildHandlerFDIR childHandlerFdir; + +}; + +#endif /* PAYLOADHANDLERBASE_H_ */ diff --git a/devicehandlers/ChildHandlerFDIR.cpp b/devicehandlers/ChildHandlerFDIR.cpp new file mode 100644 index 00000000..ece87c19 --- /dev/null +++ b/devicehandlers/ChildHandlerFDIR.cpp @@ -0,0 +1,17 @@ +/* + * ChildHandlerFDIR.cpp + * + * Created on: 08.02.2016 + * Author: baetz + */ + +#include + +ChildHandlerFDIR::ChildHandlerFDIR(object_id_t owner, object_id_t faultTreeParent, uint32_t recoveryCount) : + DeviceHandlerFDIR(owner, faultTreeParent) { + recoveryCounter.setFailureThreshold(recoveryCount); +} + +ChildHandlerFDIR::~ChildHandlerFDIR() { +} + diff --git a/devicehandlers/ChildHandlerFDIR.h b/devicehandlers/ChildHandlerFDIR.h new file mode 100644 index 00000000..78b1c655 --- /dev/null +++ b/devicehandlers/ChildHandlerFDIR.h @@ -0,0 +1,27 @@ +/* + * ChildHandlerFDIR.h + * + * Created on: 08.02.2016 + * Author: baetz + */ + +#ifndef FRAMEWORK_DEVICEHANDLERS_CHILDHANDLERFDIR_H_ +#define FRAMEWORK_DEVICEHANDLERS_CHILDHANDLERFDIR_H_ + +#include + +/** + * Very simple extension to normal FDIR. + * Does not have a default fault tree parent and + * allows to make the recovery count settable to 0. + */ +class ChildHandlerFDIR: public DeviceHandlerFDIR { +public: + ChildHandlerFDIR(object_id_t owner, object_id_t faultTreeParent = + NO_FAULT_TREE_PARENT, uint32_t recoveryCount = 0); + ~ChildHandlerFDIR(); +protected: + static const object_id_t NO_FAULT_TREE_PARENT = 0; +}; + +#endif /* FRAMEWORK_DEVICEHANDLERS_CHILDHANDLERFDIR_H_ */ diff --git a/devicehandlers/Cookie.h b/devicehandlers/Cookie.h new file mode 100644 index 00000000..495c8c40 --- /dev/null +++ b/devicehandlers/Cookie.h @@ -0,0 +1,10 @@ +#ifndef COOKIE_H_ +#define COOKIE_H_ + +class Cookie{ +public: + virtual ~Cookie(){} +}; + + +#endif /* COOKIE_H_ */ diff --git a/devicehandlers/DeviceCommunicationIF.h b/devicehandlers/DeviceCommunicationIF.h new file mode 100644 index 00000000..2ff943b8 --- /dev/null +++ b/devicehandlers/DeviceCommunicationIF.h @@ -0,0 +1,63 @@ +#ifndef DEVICECOMMUNICATIONIF_H_ +#define DEVICECOMMUNICATIONIF_H_ + +#include +#include + +class DeviceCommunicationIF: public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = DEVICE_COMMUNICATION_IF; + + static const ReturnValue_t INVALID_COOKIE_TYPE = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t NOT_ACTIVE = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t INVALID_ADDRESS = MAKE_RETURN_CODE(0x03); + static const ReturnValue_t TOO_MUCH_DATA = MAKE_RETURN_CODE(0x04); + static const ReturnValue_t NULLPOINTER = MAKE_RETURN_CODE(0x05); + static const ReturnValue_t PROTOCOL_ERROR = MAKE_RETURN_CODE(0x06); + static const ReturnValue_t CANT_CHANGE_REPLY_LEN = MAKE_RETURN_CODE(0x07); + + virtual ~DeviceCommunicationIF() { + + } + + virtual ReturnValue_t open(Cookie **cookie, uint32_t address, + uint32_t maxReplyLen) = 0; + + /** + * Use an existing cookie to open a connection to a new DeviceCommunication. + * The previous connection must not be closed. + * If the returnvalue is not RETURN_OK, the cookie is unchanged and + * can be used with the previous connection. + * + * @param cookie + * @param address + * @param maxReplyLen + * @return + */ + virtual ReturnValue_t reOpen(Cookie *cookie, uint32_t address, + uint32_t maxReplyLen) = 0; + + virtual void close(Cookie *cookie) = 0; + + //TODO can data be const? + virtual ReturnValue_t sendMessage(Cookie *cookie, uint8_t *data, + uint32_t len) = 0; + + virtual ReturnValue_t getSendSuccess(Cookie *cookie) = 0; + + virtual ReturnValue_t requestReceiveMessage(Cookie *cookie) = 0; + + virtual ReturnValue_t readReceivedMessage(Cookie *cookie, uint8_t **buffer, + uint32_t *size) = 0; + + virtual ReturnValue_t setAddress(Cookie *cookie, uint32_t address) = 0; + + virtual uint32_t getAddress(Cookie *cookie) = 0; + + virtual ReturnValue_t setParameter(Cookie *cookie, uint32_t parameter) = 0; + + virtual uint32_t getParameter(Cookie *cookie) = 0; + +}; + +#endif /* DEVICECOMMUNICATIONIF_H_ */ diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp new file mode 100644 index 00000000..cffe8220 --- /dev/null +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -0,0 +1,1246 @@ +/* + * DeviceHandlerBase.cpp + * + * Created on: 30.10.2012 + * Author: mohr + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +DeviceHandlerBase::DeviceHandlerBase(uint32_t ioBoardAddress, + object_id_t setObjectId, uint32_t maxDeviceReplyLen, + uint8_t setDeviceSwitch, object_id_t deviceCommunication, + uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, + FDIRBase* fdirInstance, uint32_t cmdQueueSize) : + SystemObject(setObjectId), rawPacket(0), rawPacketLen(0), mode( + MODE_OFF), submode(SUBMODE_NONE), pstStep(0), maxDeviceReplyLen( + maxDeviceReplyLen), wiretappingMode(OFF), theOneWhoReceivesRawTraffic( + 0), storedRawData(StorageManagerIF::INVALID_ADDRESS), theOneWhoWantsToReadRawTraffic( + 0), powerSwitcher(NULL), IPCStore(NULL), deviceCommunicationId( + deviceCommunication), communicationInterface(NULL), cookie( + NULL), commandQueue(cmdQueueSize, CommandMessage::MAX_MESSAGE_SIZE), deviceThermalStatePoolId( + thermalStatePoolId), deviceThermalRequestPoolId( + thermalRequestPoolId), healthHelper(this, setObjectId), modeHelper( + this), parameterHelper(this), childTransitionFailure(RETURN_OK), ignoreMissedRepliesCount( + 0), fdirInstance(fdirInstance), defaultFDIRUsed( + fdirInstance == NULL), switchOffWasReported( + false), cookieInfo(), ioBoardAddress(ioBoardAddress), timeoutStart(0), childTransitionDelay( + 5000), transitionSourceMode(_MODE_POWER_DOWN), transitionSourceSubMode( + SUBMODE_NONE), deviceSwitch(setDeviceSwitch), actionHelper( + this, &commandQueue) { + cookieInfo.state = COOKIE_UNUSED; + insertInCommandMap(RAW_COMMAND_ID); + if (this->fdirInstance == NULL) { + this->fdirInstance = new DeviceHandlerFDIR(setObjectId); + } +} + +DeviceHandlerBase::~DeviceHandlerBase() { + communicationInterface->close(cookie); + if (defaultFDIRUsed) { + delete fdirInstance; + } + +} + +void DeviceHandlerBase::performInPST(uint8_t counter) { + this->pstStep = counter; + + if (counter == 0) { + cookieInfo.state = COOKIE_UNUSED; + readCommandQueue(); + doStateMachine(); + checkSwitchState(); + decrementDeviceReplyMap(); + fdirInstance->checkForFailures(); + } + if (mode == MODE_OFF) { + return; + } + switch (getRmapAction()) { + case SEND_WRITE: + if ((cookieInfo.state == COOKIE_UNUSED)) { + buildInternalCommand(); + } + doSendWrite(); + break; + case GET_WRITE: + doGetWrite(); + break; + case SEND_READ: + doSendRead(); + break; + case GET_READ: + doGetRead(); + cookieInfo.state = COOKIE_UNUSED; + break; + default: + break; + } + +} + +void DeviceHandlerBase::decrementDeviceReplyMap() { + for (std::map::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 != 0) { + iter->second.delayCycles = iter->second.maxDelayCycles; + } + replyToReply(iter, TIMEOUT); + missedReply(iter->first); + } + } + } +} + +void DeviceHandlerBase::readCommandQueue() { + + if (dontCheckQueue()) { + return; + } + + CommandMessage message; + ReturnValue_t result = commandQueue.receiveMessage(&message); + if (result != RETURN_OK) { + return; + } + + result = healthHelper.handleHealthCommand(&message); + if (result == RETURN_OK) { + return; + } + + result = modeHelper.handleModeCommand(&message); + if (result == RETURN_OK) { + return; + } + + result = actionHelper.handleActionMessage(&message); + if (result == RETURN_OK) { + return; + } + + result = parameterHelper.handleParameterMessage(&message); + if (result == RETURN_OK) { + return; + } + + result = handleDeviceHandlerMessage(&message); + if (result == RETURN_OK) { + return; + } + + result = letChildHandleMessage(&message); + if (result == RETURN_OK) { + return; + } + + replyReturnvalueToCommand(CommandMessage::UNKNOW_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; + OSAL::getUptime(¤tUptime); + 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; + OSAL::getUptime(¤tUptime); + if (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; + OSAL::getUptime(¤tUptime); + 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, + uint8_t 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, periodic); + } else { + return insertInReplyMap(deviceCommand, maxDelayCycles, periodic); + } +} + +ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId, + uint16_t maxDelayCycles, uint8_t periodic) { + DeviceReplyInfo info; + info.maxDelayCycles = maxDelayCycles; + info.periodic = periodic; + info.delayCycles = 0; + info.command = deviceCommandMap.end(); + std::pair::iterator, bool> returnValue; + returnValue = deviceReplyMap.insert( + std::pair(replyId, info)); + if (returnValue.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; + std::pair::iterator, bool> returnValue; + returnValue = deviceCommandMap.insert( + std::pair(deviceCommand, + info)); + if (returnValue.second) { + return RETURN_OK; + } else { + return RETURN_FAILED; + } +} + +ReturnValue_t DeviceHandlerBase::updateReplyMapEntry( + DeviceCommandId_t deviceReply, uint16_t delayCycles, + uint16_t maxDelayCycles, uint8_t periodic) { + std::map::iterator iter = + deviceReplyMap.find(deviceReply); + if (iter == deviceReplyMap.end()) { + triggerEvent(INVALID_DEVICE_COMMAND, deviceReply); + return RETURN_FAILED; + } else { + DeviceReplyInfo *info = &(iter->second); + if (maxDelayCycles != 0) { + info->maxDelayCycles = maxDelayCycles; + } + info->delayCycles = delayCycles; + info->periodic = periodic; + return 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) { + submode = newSubmode; + mode = newMode; + modeChanged(); + setNormalDatapoolEntriesInvalid(); + if (!isTransitionalMode()) { + modeHelper.modeChanged(newMode, newSubmode); + announceMode(false); + } + OSAL::getUptime(&timeoutStart); + + if (mode == MODE_OFF) { + DataSet mySet; + PoolVariable thermalRequest(deviceThermalRequestPoolId, &mySet, + PoolVariableIF::VAR_READ_WRITE); + mySet.read(); + if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { + thermalRequest = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL; + } + mySet.commit(PoolVariableIF::VALID); + } +} + +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) { +//No need to check if iter exists, as this is checked by callers. If someone else uses the method, add check. + if (iter->second.command == deviceCommandMap.end()) { + //Is most likely periodic reply. Silent return. + return; + } +//Check if more replies are expected. If so, do nothing. + DeviceCommandInfo* info = &(iter->second.command->second); + if (--info->expectedReplies == 0) { + //Check if it was transition or internal command. Don't send any replies in that case. + if (info->sendReplyTo != NO_COMMANDER) { + actionHelper.finish(info->sendReplyTo, iter->first, status); + } + info->isExecuting = false; + } +} + +void DeviceHandlerBase::doSendWrite() { + if (cookieInfo.state == COOKIE_WRITE_READY) { + + ReturnValue_t result = communicationInterface->sendMessage(cookie, + rawPacket, rawPacketLen); + + if (result == RETURN_OK) { + cookieInfo.state = COOKIE_WRITE_SENT; + } else { + //always generate a failure event, so that FDIR knows what's up + triggerEvent(DEVICE_SENDING_COMMAND_FAILED, result, + cookieInfo.pendingCommand->first); + replyToCommand(result); + cookieInfo.state = COOKIE_UNUSED; + cookieInfo.pendingCommand->second.isExecuting = false; + } + } +} + +void DeviceHandlerBase::doGetWrite() { + if (cookieInfo.state != COOKIE_WRITE_SENT) { + return; + } + cookieInfo.state = COOKIE_UNUSED; + ReturnValue_t result = communicationInterface->getSendSuccess(cookie); + if (result == RETURN_OK) { + if (wiretappingMode == RAW) { + replyRawData(rawPacket, rawPacketLen, + theOneWhoWantsToReadRawTraffic, true); + } + //We need to distinguish here, because a raw command never expects a reply. (Could be done in eRIRM, but then child implementations need to be careful. + result = enableReplyInReplyMap(cookieInfo.pendingCommand); + } else { + //always generate a failure event, so that FDIR knows what's up + triggerEvent(DEVICE_SENDING_COMMAND_FAILED, result, + cookieInfo.pendingCommand->first); + } + if (result != RETURN_OK) { + cookieInfo.pendingCommand->second.isExecuting = false; + } + replyToCommand(result); +} + +void DeviceHandlerBase::doSendRead() { + ReturnValue_t result; + + result = communicationInterface->requestReceiveMessage(cookie); + if (result == RETURN_OK) { + cookieInfo.state = COOKIE_READ_SENT; + } else { + triggerEvent(DEVICE_REQUESTING_REPLY_FAILED, result); + //We can't inform anyone, because we don't know which command was sent last. + //So, we need to wait for a timeout. + //but I think we can allow to ignore one missedReply. + ignoreMissedRepliesCount++; + cookieInfo.state = COOKIE_UNUSED; + } +} + +void DeviceHandlerBase::doGetRead() { + uint32_t receivedDataLen; + uint8_t *receivedData; + DeviceCommandId_t foundId = 0xFFFFFFFF; + uint32_t foundLen = 0; + ReturnValue_t result; + + if (cookieInfo.state != COOKIE_READ_SENT) { + cookieInfo.state = COOKIE_UNUSED; + return; + } + + cookieInfo.state = COOKIE_UNUSED; + + result = communicationInterface->readReceivedMessage(cookie, &receivedData, + &receivedDataLen); + + if (result != RETURN_OK) { + triggerEvent(DEVICE_REQUESTING_REPLY_FAILED, result); + //I think we can allow to ignore one missedReply. + ignoreMissedRepliesCount++; + return; + } + + if (receivedDataLen == 0) + return; + + if (wiretappingMode == RAW) { + replyRawData(receivedData, receivedDataLen, + theOneWhoWantsToReadRawTraffic); + } + + if (mode == MODE_RAW) { + if ((wiretappingMode == RAW) + && (theOneWhoReceivesRawTraffic + == theOneWhoWantsToReadRawTraffic)) { + //The raw packet was already sent by the wiretapping service + } else { + replyRawData(receivedData, receivedDataLen, + theOneWhoReceivesRawTraffic); + } + } else { + //The loop may not execute more often than the number of received bytes (worst case). + //This approach avoids infinite loops due to buggy scanForReply routines (seen in bug 1077). + uint32_t remainingLength = receivedDataLen; + for (uint32_t count = 0; count < receivedDataLen; count++) { + result = scanForReply(receivedData, remainingLength, &foundId, + &foundLen); + switch (result) { + case RETURN_OK: + handleReply(receivedData, foundId); + break; + case APERIODIC_REPLY: { + DataSet dataSet; + result = interpretDeviceReply(foundId, receivedData); + if (result != RETURN_OK) { + triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, + foundId); + } + } + break; + case IGNORE_REPLY_DATA: + break; + default: + //We need to wait for timeout.. don't know what command failed and who sent it. + triggerEvent(DEVICE_READING_REPLY_FAILED, result, foundLen); + break; + } + receivedData += foundLen; + if (remainingLength > foundLen) { + remainingLength -= foundLen; + } else { + return; + } + } + + } +} + +ReturnValue_t DeviceHandlerBase::getStorageData(store_address_t storageAddress, + uint8_t * *data, uint32_t * len) { + uint32_t lenTmp; + + if (IPCStore == NULL) { + *data = NULL; + *len = 0; + return RETURN_FAILED; + } + ReturnValue_t result = IPCStore->modifyData(storageAddress, data, &lenTmp); + if (result == RETURN_OK) { + *len = lenTmp; + return RETURN_OK; + } else { + triggerEvent(StorageManagerIF::GET_DATA_FAILED, result, + storageAddress.raw); + *data = NULL; + *len = 0; + return result; + } + +} + +ReturnValue_t DeviceHandlerBase::initialize() { + ReturnValue_t result = SystemObject::initialize(); + if (result != RETURN_OK) { + return result; + } + + communicationInterface = objectManager->get( + deviceCommunicationId); + if (communicationInterface == NULL) { + return RETURN_FAILED; + } + + result = communicationInterface->open(&cookie, ioBoardAddress, + maxDeviceReplyLen); + if (result != RETURN_OK) { + return result; + } + + IPCStore = objectManager->get(objects::IPC_STORE); + if (IPCStore == NULL) { + return RETURN_FAILED; + } + + AcceptsDeviceResponsesIF *rawReceiver = objectManager->get< + AcceptsDeviceResponsesIF>(objects::PUS_DEVICE_COMMAND_SERVICE); + + if (rawReceiver == NULL) { + return RETURN_FAILED; + } + + theOneWhoReceivesRawTraffic = rawReceiver->getDeviceQueue(); + + powerSwitcher = objectManager->get(objects::PCDU_HANDLER); + if (powerSwitcher == NULL) { + return RETURN_FAILED; + } + + result = healthHelper.initialize(); + if (result != RETURN_OK) { + return result; + } + + result = modeHelper.initialize(); + if (result != RETURN_OK) { + return result; + } + result = actionHelper.initialize(); + 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; + } + + fillCommandAndReplyMap(); + + //Set temperature target state to NON_OP. + DataSet mySet; + PoolVariable thermalRequest(deviceThermalRequestPoolId, &mySet, + PoolVariableIF::VAR_WRITE); + mySet.read(); + thermalRequest = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL; + mySet.commit(PoolVariableIF::VALID); + + return RETURN_OK; + +} + +void DeviceHandlerBase::replyRawData(const uint8_t *data, size_t len, + MessageQueueId_t sendTo, bool isCommand) { + if (IPCStore == NULL || len == 0) { + return; + } + store_address_t address; + ReturnValue_t result = IPCStore->addData(&address, data, len); + + if (result != RETURN_OK) { + triggerEvent(StorageManagerIF::STORE_DATA_FAILED, result); + return; + } + + Command_t handlerCommand = DeviceHandlerMessage::REPLY_RAW_REPLY; + + if (isCommand) { + handlerCommand = DeviceHandlerMessage::REPLY_RAW_COMMAND; + } + + CommandMessage message; + + DeviceHandlerMessage::setDeviceHandlerRawReplayMessage(&message, + getObjectId(), address, isCommand); + +// this->DeviceHandlerCommand = CommandMessage::CMD_NONE; + + result = commandQueue.sendMessage(sendTo, &message); + + if (result != RETURN_OK) { + IPCStore->deleteData(address); + triggerEvent(MessageQueue::SEND_MSG_FAILED, result, sendTo); + } +} + +//Default child implementations + +DeviceHandlerBase::RmapAction_t DeviceHandlerBase::getRmapAction() { + switch (pstStep) { + case 0: + return SEND_WRITE; + break; + case 1: + return GET_WRITE; + break; + case 2: + return SEND_READ; + break; + case 3: + return GET_READ; + break; + default: + + break; + } + return NOTHING; +} + +MessageQueueId_t DeviceHandlerBase::getCommandQueue() const { + return commandQueue.getId(); +} + +void DeviceHandlerBase::handleReply(const uint8_t* receivedData, + DeviceCommandId_t foundId) { + ReturnValue_t result; + DataSet dataSet; + DeviceReplyMap::iterator iter = deviceReplyMap.find(foundId); + + if (iter == deviceReplyMap.end()) { + triggerEvent(DEVICE_UNKNOWN_REPLY, foundId); + return; + } + + DeviceReplyInfo *info = &(iter->second); + + if (info->delayCycles != 0) { + + if (info->periodic != 0) { + info->delayCycles = info->maxDelayCycles; + } else { + info->delayCycles = 0; + } + result = interpretDeviceReply(foundId, receivedData); + if (result != RETURN_OK) { + //Report failed interpretation to FDIR. + triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, foundId); + } + replyToReply(iter, result); + } else { + //Other completion failure messages are created by timeout. + //Powering down the device might take some time during which periodic replys may still come in. + if (mode != _MODE_WAIT_OFF) { + triggerEvent(DEVICE_UNREQUESTED_REPLY, foundId); + } + } +} + +ReturnValue_t DeviceHandlerBase::switchCookieChannel(object_id_t newChannelId) { + DeviceCommunicationIF *newCommunication = objectManager->get< + DeviceCommunicationIF>(newChannelId); + + if (newCommunication != NULL) { + ReturnValue_t result = newCommunication->reOpen(cookie, ioBoardAddress, + maxDeviceReplyLen); + if (result != RETURN_OK) { + return result; + } + return RETURN_OK; + } + return RETURN_FAILED; +} + +void DeviceHandlerBase::buildRawDeviceCommand(CommandMessage* commandMessage) { + storedRawData = DeviceHandlerMessage::getStoreAddress(commandMessage); + ReturnValue_t result = getStorageData(storedRawData, &rawPacket, + &rawPacketLen); + if (result != RETURN_OK) { + replyReturnvalueToCommand(result, RAW_COMMAND_ID); + storedRawData.raw = StorageManagerIF::INVALID_ADDRESS; + } else { + cookieInfo.pendingCommand = deviceCommandMap.find( + (DeviceCommandId_t) RAW_COMMAND_ID); + cookieInfo.pendingCommand->second.isExecuting = true; + cookieInfo.state = COOKIE_WRITE_READY; + } +} + +void DeviceHandlerBase::commandSwitch(ReturnValue_t onOff) { + const uint8_t *switches; + uint8_t numberOfSwitches = 0; + ReturnValue_t result = getSwitches(&switches, &numberOfSwitches); + if (result == RETURN_OK) { + while (numberOfSwitches > 0) { + powerSwitcher->sendSwitchCommand(switches[numberOfSwitches - 1], + onOff); + numberOfSwitches--; + } + } +} + +ReturnValue_t DeviceHandlerBase::getSwitches(const uint8_t **switches, + uint8_t *numberOfSwitches) { + *switches = &deviceSwitch; + *numberOfSwitches = 1; + return RETURN_OK; +} + +void DeviceHandlerBase::modeChanged(void) { +} + +ReturnValue_t DeviceHandlerBase::enableReplyInReplyMap( + DeviceCommandMap::iterator command, uint8_t expectedReplies, + bool useAlternativeId, DeviceCommandId_t alternativeReply) { + DeviceReplyMap::iterator iter; + if (useAlternativeId) { + iter = deviceReplyMap.find(alternativeReply); + } else { + iter = deviceReplyMap.find(command->first); + } + if (iter != deviceReplyMap.end()) { + DeviceReplyInfo *info = &(iter->second); + info->delayCycles = info->maxDelayCycles; + info->command = command; + command->second.expectedReplies = expectedReplies; + return RETURN_OK; + } else { + return NO_REPLY_EXPECTED; + } +} + +void DeviceHandlerBase::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { + setMode(getBaseMode(mode)); +} + +uint32_t DeviceHandlerBase::getTransitionDelayMs(Mode_t modeFrom, + Mode_t modeTo) { + return 0; +} + +ReturnValue_t DeviceHandlerBase::getStateOfSwitches(void) { + uint8_t numberOfSwitches = 0; + const uint8_t *switches; + + ReturnValue_t result = getSwitches(&switches, &numberOfSwitches); + if ((result == RETURN_OK) && (numberOfSwitches != 0)) { + while (numberOfSwitches > 0) { + if (powerSwitcher->getSwitchState(switches[numberOfSwitches - 1]) + == PowerSwitchIF::SWITCH_OFF) { + return PowerSwitchIF::SWITCH_OFF; + } + numberOfSwitches--; + } + return PowerSwitchIF::SWITCH_ON; + } + + return NO_SWITCH; +} + +Mode_t DeviceHandlerBase::getBaseMode(Mode_t transitionMode) { +//only child action special modes are handled, as a child should never see any base action modes + if (transitionMode == _MODE_START_UP) { + return _MODE_TO_ON; + } + if (transitionMode == _MODE_SHUT_DOWN) { + return _MODE_POWER_DOWN; + } + return transitionMode + & ~(TRANSITION_MODE_BASE_ACTION_MASK + | TRANSITION_MODE_CHILD_ACTION_MASK); +} + +//SHOULDDO: Allow transition from OFF to NORMAL to reduce complexity in assemblies. And, by the way, throw away DHB and write a new one: +// - Include power and thermal completely, but more modular :-) +// - Don't use modes for state transitions, reduce FSM (Finte State Machine) complexity. +// - Modularization? +ReturnValue_t DeviceHandlerBase::checkModeCommand(Mode_t commandedMode, + Submode_t commandedSubmode, uint32_t* msToReachTheMode) { + if (isTransitionalMode()) { + return IN_TRANSITION; + } + if ((mode == MODE_ERROR_ON) && (commandedMode != MODE_OFF)) { + return TRANS_NOT_ALLOWED; + } + if ((commandedMode == MODE_NORMAL) && (mode == MODE_OFF)) { + return TRANS_NOT_ALLOWED; + } + + if ((commandedMode == MODE_ON) && (mode == MODE_OFF) + && (deviceThermalStatePoolId != PoolVariableIF::NO_PARAMETER)) { + DataSet mySet; + PoolVariable thermalState(deviceThermalStatePoolId, &mySet, + PoolVariableIF::VAR_READ); + PoolVariable thermalRequest(deviceThermalRequestPoolId, &mySet, + PoolVariableIF::VAR_READ); + mySet.read(); + if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { + if (!ThermalComponentIF::isOperational(thermalState)) { + triggerEvent(ThermalComponentIF::TEMP_NOT_IN_OP_RANGE, thermalState); + return NON_OP_TEMPERATURE; + } + } + } + + return isModeCombinationValid(commandedMode, commandedSubmode); +} + +void DeviceHandlerBase::startTransition(Mode_t commandedMode, + Submode_t commandedSubmode) { + switch (commandedMode) { + case MODE_ON: + if (mode == MODE_OFF) { + transitionSourceMode = _MODE_POWER_DOWN; + transitionSourceSubMode = SUBMODE_NONE; + setMode(_MODE_POWER_ON, commandedSubmode); + //already set the delay for the child transition so we don't need to call it twice + childTransitionDelay = getTransitionDelayMs(_MODE_START_UP, + MODE_ON); + triggerEvent(CHANGING_MODE, commandedMode, commandedSubmode); + DataSet mySet; + PoolVariable thermalRequest(deviceThermalRequestPoolId, + &mySet, PoolVariableIF::VAR_READ_WRITE); + mySet.read(); + if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { + thermalRequest = ThermalComponentIF::STATE_REQUEST_OPERATIONAL; + mySet.commit(PoolVariableIF::VALID); + } + } else { + setTransition(MODE_ON, commandedSubmode); + } + break; + case MODE_OFF: + if (mode == MODE_OFF) { + triggerEvent(CHANGING_MODE, commandedMode, commandedSubmode); + setMode(_MODE_POWER_DOWN, commandedSubmode); + } else { + //already set the delay for the child transition so we don't need to call it twice + childTransitionDelay = getTransitionDelayMs(mode, _MODE_POWER_DOWN); + transitionSourceMode = _MODE_POWER_DOWN; + transitionSourceSubMode = commandedSubmode; + childTransitionFailure = CHILD_TIMEOUT; + setMode(_MODE_SHUT_DOWN, commandedSubmode); + triggerEvent(CHANGING_MODE, commandedMode, commandedSubmode); + } + break; + case MODE_RAW: + if (mode != MODE_OFF) { + setTransition(MODE_RAW, commandedSubmode); + } else { + setMode(MODE_RAW, commandedSubmode); + } + break; + case MODE_NORMAL: + if (mode != MODE_OFF) { + setTransition(MODE_NORMAL, commandedSubmode); + } else { + replyReturnvalueToCommand(HasModesIF::TRANS_NOT_ALLOWED); + } + break; + } +} + +void DeviceHandlerBase::getMode(Mode_t* mode, Submode_t* submode) { + *mode = this->mode; + *submode = this->submode; +} + +void DeviceHandlerBase::setToExternalControl() { + healthHelper.setHealth(EXTERNAL_CONTROL); +} + +void DeviceHandlerBase::announceMode(bool recursive) { + triggerEvent(MODE_INFO, mode, submode); +} + +bool DeviceHandlerBase::dontCheckQueue() { + return false; +} + +void DeviceHandlerBase::missedReply(DeviceCommandId_t id) { + if (ignoreMissedRepliesCount > 0) { + ignoreMissedRepliesCount--; + } else { + triggerEvent(DEVICE_MISSED_REPLY, id); + } +} + +HasHealthIF::HealthState DeviceHandlerBase::getHealth() { + return healthHelper.getHealth(); +} + +ReturnValue_t DeviceHandlerBase::setHealth(HealthState health) { + healthHelper.setHealth(health); + return HasReturnvaluesIF::RETURN_OK; +} + +void DeviceHandlerBase::checkSwitchState() { + if ((mode == MODE_ON || mode == MODE_NORMAL)) { + //We only check in ON and NORMAL, ignore RAW and ERROR_ON. + ReturnValue_t result = getStateOfSwitches(); + if (result == PowerSwitchIF::SWITCH_OFF && !switchOffWasReported) { + triggerEvent(PowerSwitchIF::SWITCH_WENT_OFF); + switchOffWasReported = true; + } + } else { + switchOffWasReported = false; + } +} + +void DeviceHandlerBase::doOnActivity() { +} + +ReturnValue_t DeviceHandlerBase::acceptExternalDeviceCommands() { + if ((mode != MODE_ON) && (mode != MODE_NORMAL)) { + return WRONG_MODE_FOR_COMMAND; + } + return RETURN_OK; +} + +ReturnValue_t DeviceHandlerBase::handleDeviceHandlerMessage( + CommandMessage * message) { + ReturnValue_t result; + switch (message->getCommand()) { + case DeviceHandlerMessage::CMD_WIRETAPPING: + switch (DeviceHandlerMessage::getWiretappingMode(message)) { + case RAW: + wiretappingMode = RAW; + theOneWhoWantsToReadRawTraffic = commandQueue.getLastPartner(); + break; + case TM: + wiretappingMode = TM; + theOneWhoWantsToReadRawTraffic = commandQueue.getLastPartner(); + break; + case OFF: + wiretappingMode = OFF; + break; + default: + replyReturnvalueToCommand(INVALID_COMMAND_PARAMETER); + wiretappingMode = OFF; + return RETURN_OK; + } + replyReturnvalueToCommand(RETURN_OK); + return RETURN_OK; + case DeviceHandlerMessage::CMD_SWITCH_IOBOARD: + if (mode != MODE_OFF) { + replyReturnvalueToCommand(WRONG_MODE_FOR_COMMAND); + } else { + result = switchCookieChannel( + DeviceHandlerMessage::getIoBoardObjectId(message)); + if (result == RETURN_OK) { + replyReturnvalueToCommand(RETURN_OK); + } else { + replyReturnvalueToCommand(CANT_SWITCH_IOBOARD); + } + } + return RETURN_OK; + case DeviceHandlerMessage::CMD_RAW: + if ((mode != MODE_RAW)) { + DeviceHandlerMessage::clear(message); + replyReturnvalueToCommand(WRONG_MODE_FOR_COMMAND); + } else { + buildRawDeviceCommand(message); + } + return RETURN_OK; + default: + return RETURN_FAILED; + } +} + +void DeviceHandlerBase::setParentQueue(MessageQueueId_t parentQueueId) { + modeHelper.setParentQueue(parentQueueId); + healthHelper.setParentQeueue(parentQueueId); +} + +bool DeviceHandlerBase::isAwaitingReply() { + std::map::iterator iter; + for (iter = deviceReplyMap.begin(); iter != deviceReplyMap.end(); ++iter) { + if (iter->second.delayCycles != 0) { + return true; + } + } + return false; +} + +ReturnValue_t DeviceHandlerBase::letChildHandleMessage( + CommandMessage * message) { + return RETURN_FAILED; +} + +void DeviceHandlerBase::handleDeviceTM(SerializeIF* data, + DeviceCommandId_t replyId, bool neverInDataPool) { + DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId); + if (iter == deviceReplyMap.end()) { + triggerEvent(DEVICE_UNKNOWN_REPLY, replyId); + return; + } + if (iter->second.command != deviceCommandMap.end()) { + MessageQueueId_t queueId = iter->second.command->second.sendReplyTo; + DeviceTmReportingWrapper wrapper(getObjectId(), replyId, data); + if (queueId != NO_COMMANDER) { + actionHelper.reportData(queueId, replyId, data); + } + //This check should make sure we get any TM but don't get anything doubled. + if (wiretappingMode == TM + && (theOneWhoWantsToReadRawTraffic != queueId)) { + actionHelper.reportData(theOneWhoWantsToReadRawTraffic, + replyId, &wrapper); + } + } +//Try to cast to DataSet and commit data. + if (!neverInDataPool) { + DataSet* dataSet = dynamic_cast(data); + if (dataSet != NULL) { + dataSet->commit(PoolVariableIF::VALID); + } + } +} + +ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, + MessageQueueId_t commandedBy, const uint8_t* data, uint32_t size) { + ReturnValue_t result = acceptExternalDeviceCommands(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + DeviceCommandMap::iterator iter = deviceCommandMap.find(actionId); + if (iter == deviceCommandMap.end()) { + result = COMMAND_NOT_SUPPORTED; + } else if (iter->second.isExecuting) { + result = COMMAND_ALREADY_SENT; + } else { + result = buildCommandFromCommand(actionId, data, size); + } + if (result == RETURN_OK) { + iter->second.sendReplyTo = commandedBy; + iter->second.isExecuting = true; + cookieInfo.pendingCommand = iter; + cookieInfo.state = COOKIE_WRITE_READY; + } + return result; +} + +void DeviceHandlerBase::buildInternalCommand(void) { +//Neither Raw nor Direct could build a command + ReturnValue_t result = NOTHING_TO_SEND; + DeviceCommandId_t deviceCommandId = NO_COMMAND_ID; + if (mode == MODE_NORMAL) { + result = buildNormalDeviceCommand(&deviceCommandId); + if (result == BUSY) { + debug << std::hex << getObjectId() + << ": DHB::buildInternalCommand busy" << std::endl; //so we can track misconfigurations + result = NOTHING_TO_SEND; //no need to report this + } + } else if (mode == MODE_RAW) { + result = buildChildRawCommand(); + deviceCommandId = RAW_COMMAND_ID; + } else if (mode & TRANSITION_MODE_CHILD_ACTION_MASK) { + result = buildTransitionDeviceCommand(&deviceCommandId); + } else { + return; + } + if (result == NOTHING_TO_SEND) { + return; + } + if (result == RETURN_OK) { + DeviceCommandMap::iterator iter = deviceCommandMap.find( + deviceCommandId); + if (iter == deviceCommandMap.end()) { + result = COMMAND_NOT_SUPPORTED; + } else if (iter->second.isExecuting) { + debug << std::hex << getObjectId() + << ": DHB::buildInternalCommand: Command " << deviceCommandId << " isExecuting" << std::endl; //so we can track misconfigurations + return; //this is an internal command, no need to report a failure here, missed reply will track if a reply is too late, otherwise, it's ok + } else { + iter->second.sendReplyTo = NO_COMMANDER; + iter->second.isExecuting = true; + cookieInfo.pendingCommand = iter; + cookieInfo.state = COOKIE_WRITE_READY; + } + } + if (result != RETURN_OK) { + triggerEvent(DEVICE_BUILDING_COMMAND_FAILED, result, deviceCommandId); + } +} + +ReturnValue_t DeviceHandlerBase::buildChildRawCommand() { + return NOTHING_TO_SEND; +} + +uint8_t DeviceHandlerBase::getReplyDelayCycles( + DeviceCommandId_t deviceCommand) { + DeviceReplyMap::iterator iter = deviceReplyMap.find(deviceCommand); + if (iter == deviceReplyMap.end()) { + return 0; + } + return iter->second.delayCycles; +} + +Mode_t DeviceHandlerBase::getTransitionSourceMode() const { + return transitionSourceMode; +} + +Submode_t DeviceHandlerBase::getTransitionSourceSubMode() const { + return transitionSourceSubMode; +} + +void DeviceHandlerBase::triggerEvent(Event event, uint32_t parameter1, + uint32_t parameter2) { + fdirInstance->triggerEvent(event, parameter1, parameter2); +} + +void DeviceHandlerBase::forwardEvent(Event event, uint32_t parameter1, + uint32_t parameter2) const { + fdirInstance->triggerEvent(event, parameter1, parameter2); +} + +void DeviceHandlerBase::doOffActivity() { +} + +ReturnValue_t DeviceHandlerBase::getParameter(uint8_t domainId, + uint16_t parameterId, ParameterWrapper* parameterWrapper, + const ParameterWrapper* newValues, uint16_t startAtIndex) { + ReturnValue_t result = fdirInstance->getParameter(domainId, parameterId, + parameterWrapper, newValues, startAtIndex); + if (result != INVALID_DOMAIN_ID) { + return result; + } + return INVALID_DOMAIN_ID; + +} + +bool DeviceHandlerBase::isTransitionalMode() { + return ((mode & (TRANSITION_MODE_BASE_ACTION_MASK | TRANSITION_MODE_CHILD_ACTION_MASK)) != 0); +} + +bool DeviceHandlerBase::commandIsExecuting(DeviceCommandId_t commandId) { + auto iter = deviceCommandMap.find(commandId); + if (iter != deviceCommandMap.end()) { + return iter->second.isExecuting; + } else { + return false; + } + +} diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h new file mode 100644 index 00000000..f6c33879 --- /dev/null +++ b/devicehandlers/DeviceHandlerBase.h @@ -0,0 +1,971 @@ +#ifndef DEVICEHANDLERBASE_H_ +#define DEVICEHANDLERBASE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class StorageManagerIF; + +/** + * This is the abstract base class for device handlers. + * + * It features handling of @link DeviceHandlerIF::Mode_t Modes @endlink, the RMAP communication and the + * communication with commanding objects. + * It inherits SystemObject and thus can be created by the ObjectManagerIF. + * + * This class is called by the PollingSequenceTable periodically. Thus, the execution is divided into PST cycles and steps within a cycle. + * For each step an RMAP action is selected and executed. If data has been received (eg in case of an RMAP Read), the data will be interpreted. + * The action for each step can be defined by the child class but as most device handlers share a 4-call (Read-getRead-write-getWrite) structure, + * a default implementation is provided. + * + * Device handler instances should extend this class and implement the abstract functions. + */ +class DeviceHandlerBase: public DeviceHandlerIF, + public HasReturnvaluesIF, + public PollingSequenceExecutableIF, + public SystemObject, + public HasModesIF, + public HasHealthIF, + public HasActionsIF, + public ReceivesParameterMessagesIF { +public: + /** + * The constructor passes the objectId to the SystemObject(). + * + * @param setObjectId the ObjectId to pass to the SystemObject() Constructor + * @param maxDeviceReplyLen the length the RMAP getRead call will be sent with + * @param setDeviceSwitch the switch the device is connected to, for devices using two switches, overwrite getSwitches() + */ + DeviceHandlerBase(uint32_t ioBoardAddress, object_id_t setObjectId, + uint32_t maxDeviceReplyLen, uint8_t setDeviceSwitch, + object_id_t deviceCommunication, uint32_t thermalStatePoolId = + PoolVariableIF::NO_PARAMETER, + uint32_t thermalRequestPoolId = PoolVariableIF::NO_PARAMETER, FDIRBase* fdirInstance = NULL, uint32_t cmdQueueSize = 20); + + virtual MessageQueueId_t getCommandQueue(void) const; + + virtual void performInPST(uint8_t counter); + + virtual ReturnValue_t initialize(); + + /** + * MUST be called after initialize(), not before! TODO: Is this statement still valid? + * + * @param parentQueueId + */ + virtual void setParentQueue(MessageQueueId_t parentQueueId); + /** + * Destructor. + */ + virtual ~DeviceHandlerBase(); + + ReturnValue_t executeAction(ActionId_t actionId, + MessageQueueId_t commandedBy, const uint8_t* data, uint32_t size); + Mode_t getTransitionSourceMode() const; + Submode_t getTransitionSourceSubMode() const; + virtual void getMode(Mode_t *mode, Submode_t *submode); + HealthState getHealth(); + ReturnValue_t setHealth(HealthState health); + virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, + ParameterWrapper *parameterWrapper, + const ParameterWrapper *newValues, uint16_t startAtIndex); +protected: + /** + * The Returnvalues id of this class, required by HasReturnvaluesIF + */ + static const uint8_t INTERFACE_ID = DEVICE_HANDLER_BASE; + + static const ReturnValue_t INVALID_CHANNEL = MAKE_RETURN_CODE(4); + static const ReturnValue_t APERIODIC_REPLY = MAKE_RETURN_CODE(5); + static const ReturnValue_t IGNORE_REPLY_DATA = MAKE_RETURN_CODE(6); +// static const ReturnValue_t ONE_SWITCH = MAKE_RETURN_CODE(8); +// static const ReturnValue_t TWO_SWITCHES = MAKE_RETURN_CODE(9); + static const ReturnValue_t NO_SWITCH = MAKE_RETURN_CODE(10); + static const ReturnValue_t COMMAND_MAP_ERROR = MAKE_RETURN_CODE(11); + static const ReturnValue_t NOTHING_TO_SEND = MAKE_RETURN_CODE(12); + + //Mode handling error Codes + static const ReturnValue_t CHILD_TIMEOUT = MAKE_RETURN_CODE(0xE1); + static const ReturnValue_t SWITCH_FAILED = MAKE_RETURN_CODE(0xE2); + + static const DeviceCommandId_t RAW_COMMAND_ID = -1; + static const DeviceCommandId_t NO_COMMAND_ID = -2; + static const MessageQueueId_t NO_COMMANDER = 0; + + /** + * RMAP Action that will be executed. + * + * This is used by the child class to tell the base class what to do. + */ + enum RmapAction_t { + SEND_WRITE, //!< RMAP send write + GET_WRITE, //!< RMAP get write + SEND_READ, //!< RMAP send read + GET_READ, //!< RMAP get read + NOTHING //!< Do nothing. + }; + + /** + * Pointer to the raw packet that will be sent. + */ + uint8_t *rawPacket; + /** + * Size of the #rawPacket. + */ + uint32_t rawPacketLen; + + /** + * The mode the device handler is currently in. + * + * This should never be changed directly but only with setMode() + */ + Mode_t mode; + + /** + * The submode the device handler is currently in. + * + * This should never be changed directly but only with setMode() + */ + Submode_t submode; + + /** + * This is the counter value from performInPST(). + */ + uint8_t pstStep; + + /** + * This will be used in the RMAP getRead command as expected length, is set by the constructor, can be modiefied at will. + */ + const uint32_t maxDeviceReplyLen; + + /** + * wiretapping flag: + * + * indicates either that all raw messages to and from the device should be sent to #theOneWhoWantsToReadRawTraffic + * or that all device TM should be downlinked to #theOneWhoWantsToReadRawTraffic + */ + enum WiretappingMode { + OFF = 0, RAW = 1, TM = 2 + } wiretappingMode; + + /** + * the message queue which commanded raw mode + * + * This is the one to receive raw replies + */ + MessageQueueId_t theOneWhoReceivesRawTraffic; + + store_address_t storedRawData; + + /** + * the message queue which wants to read all raw traffic + * + * if #isWiretappingActive all raw communication from and to the device will be sent to this queue + */ + MessageQueueId_t theOneWhoWantsToReadRawTraffic; + + /** + * the object used to set power switches + */ + PowerSwitchIF *powerSwitcher; + + /** + * Pointer to the IPCStore. + * + * This caches the pointer received from the objectManager in the constructor. + */ + StorageManagerIF *IPCStore; + + /** + * cached for init + */ + object_id_t deviceCommunicationId; + + /** + * Communication object used for device communication + */ + DeviceCommunicationIF *communicationInterface; + + /** + * Cookie used for communication + */ + Cookie *cookie; + + /** + * The MessageQueue used to receive device handler commands and to send replies. + */ + MessageQueue commandQueue; + + /** + * this is the datapool variable with the thermal state of the device + * + * can be set to PoolVariableIF::NO_PARAMETER to deactivate thermal checking + */ + uint32_t deviceThermalStatePoolId; + + /** + * this is the datapool variable with the thermal request of the device + * + * can be set to PoolVariableIF::NO_PARAMETER to deactivate thermal checking + */ + uint32_t deviceThermalRequestPoolId; + + /** + * Taking care of the health + */ + HealthHelper healthHelper; + + ModeHelper modeHelper; + + ParameterHelper parameterHelper; + + /** + * Optional Error code + * Can be set in doStartUp(), doShutDown() and doTransition() to signal cause for Transition failure. + */ + ReturnValue_t childTransitionFailure; + + uint32_t ignoreMissedRepliesCount; //!< Counts if communication channel lost a reply, so some missed replys can be ignored. + + FDIRBase* fdirInstance; //!< Pointer to the used FDIR instance. If not provided by child, default class is instantiated. + + bool defaultFDIRUsed; //!< To correctly delete the default instance. + + bool switchOffWasReported; //!< Indicates if SWITCH_WENT_OFF was already thrown. + + /** + * Helper function to report a missed reply + * + * Can be overwritten by children to act on missed replies or to fake reporting Id. + * + * @param id of the missed reply + */ + virtual void missedReply(DeviceCommandId_t id); + + /** + * Send a reply to a received device handler command. + * + * This also resets #DeviceHandlerCommand to 0. + * + * @param reply the reply type + * @param parameter parameter for the reply + */ + void replyReturnvalueToCommand(ReturnValue_t status, + uint32_t parameter = 0); + + /** + * + + * @param parameter2 additional parameter + */ + void replyToCommand(ReturnValue_t status, uint32_t parameter = 0); + + /** + * Set the device handler mode + * + * Sets #timeoutStart with every call. + * + * Sets #transitionTargetMode if necessary so transitional states can be entered from everywhere without breaking the state machine + * (which relies on a correct #transitionTargetMode). + * + * The submode is left unchanged. + * + * + * @param newMode + */ + void setMode(Mode_t newMode); + + /** + * @overload + * @param submode + */ + void setMode(Mode_t newMode, Submode_t submode); + + /** + * This is used to let the child class handle the transition from mode @c _MODE_START_UP to @c MODE_ON + * + * It is only called when the device handler is in mode @c _MODE_START_UP. That means, the device switch(es) are already set to on. + * Device handler commands are read and can be handled by the child class. If the child class handles a command, it should also send + * an reply accordingly. + * If an Command is not handled (ie #DeviceHandlerCommand is not @c CMD_NONE, the base class handles rejecting the command and sends a reply. + * The replies for mode transitions are handled by the base class. + * + * If the device is started and ready for operation, the mode should be set to MODE_ON. It is possible to set the mode to _MODE_TO_ON to + * use the to on transition if available. + * If the power-up fails, the mode should be set to _MODE_POWER_DOWN which will lead to the device being powered off. + * If the device does not change the mode, the mode will be changed to _MODE_POWER_DOWN, after the timeout (from getTransitionDelay()) has passed. + * + * #transitionFailure can be set to a failure code indicating the reason for a failed transition + */ + virtual void doStartUp() = 0; + + /** + * This is used to let the child class handle the transition from mode @c _MODE_SHUT_DOWN to @c _MODE_POWER_DOWN + * + * It is only called when the device handler is in mode @c _MODE_SHUT_DOWN. + * Device handler commands are read and can be handled by the child class. If the child class handles a command, it should also send + * an reply accordingly. + * If an Command is not handled (ie #DeviceHandlerCommand is not @c CMD_NONE, the base class handles rejecting the command and sends a reply. + * The replies for mode transitions are handled by the base class. + * + * If the device ready to be switched off, the mode should be set to _MODE_POWER_DOWN. + * If the device should not be switched off, the mode can be changed to _MODE_TO_ON (or MODE_ON if no transition is needed). + * If the device does not change the mode, the mode will be changed to _MODE_POWER_DOWN, when the timeout (from getTransitionDelay()) has passed. + * + * #transitionFailure can be set to a failure code indicating the reason for a failed transition + */ + virtual void doShutDown() = 0; + + /** + * Do the transition to the main modes (MODE_ON, MODE_NORMAL and MODE_RAW). + * + * If the transition is complete, the mode should be set to the target mode, which can be deduced from the current mode which is + * [_MODE_TO_ON, _MODE_TO_NORMAL, _MODE_TO_RAW] + * + * The intended target submode is already set. The origin submode can be read in subModeFrom. + * + * If the transition can not be completed, the child class can try to reach an working mode by setting the mode either directly + * or setting the mode to an transitional mode (TO_ON, TO_NORMAL, TO_RAW) if the device needs to be reconfigured. + * + * If nothing works, the child class can wait for the timeout and the base class will reset the mode to the mode where the transition + * originated from (the child should report the reason for the failed transition). + * + * The intended way to send commands is to set a flag (enum) indicating which command is to be sent here + * and then to check in buildTransitionCommand() for the flag. This flag can also be used by doStartUp() and + * doShutDown() to get a nice and clean implementation of buildTransitionCommand() without switching through modes. + * + * When the the condition for the completion of the transition is met, the mode can be set, for example in the parseReply() function. + * + * The default implementation goes into the target mode; + * + * #transitionFailure can be set to a failure code indicating the reason for a failed transition + * + * @param modeFrom the mode the transition originated from: [MODE_ON, MODE_NORMAL, MODE_RAW and _MODE_POWER_DOWN (if the mode changed from _MODE_START_UP to _MODE_TO_ON)] + * @param subModeFrom the subMode of modeFrom + */ + virtual void doTransition(Mode_t modeFrom, Submode_t subModeFrom); + + /** + * Get the time needed to transit from modeFrom to modeTo. + * + * Used for the following transitions: + * modeFrom -> modeTo: + * MODE_ON -> [MODE_ON, MODE_NORMAL, MODE_RAW, _MODE_POWER_DOWN] + * MODE_NORMAL -> [MODE_ON, MODE_NORMAL, MODE_RAW, _MODE_POWER_DOWN] + * MODE_RAW -> [MODE_ON, MODE_NORMAL, MODE_RAW, _MODE_POWER_DOWN] + * _MODE_START_UP -> MODE_ON (do not include time to set the switches, the base class got you covered) + * + * The default implementation returns 0; + * + * @param modeFrom + * @param modeTo + * @return time in ms + */ + virtual uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo); + + /** + * Is the combination of mode and submode valid? + * + * @param mode + * @param submode + * @return + * - @c RETURN_OK if valid + * - @c RETURN_FAILED if invalid + */ + virtual ReturnValue_t isModeCombinationValid(Mode_t mode, + Submode_t submode); + + /** + * Get the Rmap action for the current step. + * + * The step number can be read from #pstStep. + * + * @return The Rmap action to execute in this step + */ + virtual RmapAction_t getRmapAction(); + + /** + * Build the device command to send for normal mode. + * + * This is only called in @c MODE_NORMAL. If multiple submodes for @c MODE_NORMAL are supported, + * different commands can built returned depending on the submode. + * + * #rawPacket and #rawPacketLen must be set by this method to the packet to be sent. + * + * @param[out] id the device command id that has been built + * @return + * - @c RETURN_OK when a command is to be sent + * - not @c RETURN_OK when no command is to be sent + */ + virtual ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) = 0; + + /** + * Build the device command to send for a transitional mode. + * + * This is only called in @c _MODE_TO_NORMAL, @c _MODE_TO_ON, @c _MODE_TO_RAW, + * @c _MODE_START_UP and @c _MODE_TO_POWER_DOWN. So it is used by doStartUp() and doShutDown() as well as doTransition() + * + * A good idea is to implement a flag indicating a command has to be built and a variable containing the command number to be built + * and filling them in doStartUp(), doShutDown() and doTransition() so no modes have to be checked here. + * + * #rawPacket and #rawPacketLen must be set by this method to the packet to be sent. + * + * @param[out] id the device command id built + * @return + * - @c RETURN_OK when a command is to be sent + * - not @c RETURN_OK when no command is to be sent + */ + virtual ReturnValue_t buildTransitionDeviceCommand( + DeviceCommandId_t * id) = 0; + + /** + * Build the device command to send for raw mode. + * + * This is only called in @c MODE_RAW. It is for the rare case that in raw mode packets + * are to be sent by the handler itself. It is NOT needed for the raw commanding service. + * Its only current use is in the STR handler which gets its raw packets from a different + * source. + * Also it can be used for transitional commands, to get the device ready for @c MODE_RAW + * + * As it is almost never used, there is a default implementation returning @c NOTHING_TO_SEND. + * + * #rawPacket and #rawPacketLen must be set by this method to the packet to be sent. + * + * @param[out] id the device command id built + * @return + * - @c RETURN_OK when a command is to be sent + * - not @c NOTHING_TO_SEND when no command is to be sent + */ + virtual ReturnValue_t buildChildRawCommand(); + + /** + * Build a device command packet from data supplied by a direct command. + * + * #rawPacket and #rawPacketLen should be set by this method to the packet to be sent. + * + * @param deviceCommand the command to build, already checked against deviceCommandMap + * @param commandData pointer to the data from the direct command + * @param commandDataLen length of commandData + * @return + * - @c RETURN_OK when #rawPacket is valid + * - @c RETURN_FAILED when #rawPacket is invalid and no data should be sent + */ + virtual ReturnValue_t buildCommandFromCommand( + DeviceCommandId_t deviceCommand, const uint8_t * commandData, + size_t commandDataLen) = 0; + + /** + * fill the #deviceCommandMap + * + * called by the initialize() of the base class + * + * This is used to let the base class know which replies are expected. + * There are different scenarios regarding this: + * - "Normal" commands. These are commands, that trigger a direct reply from the device. In this case, the id of the command should be added to the command map + * with a commandData_t where maxDelayCycles is set to the maximum expected number of PST cycles the reply will take. Then, scanForReply returns the id of the command and the base class can handle time-out and missing replies. + * - Periodic, unrequested replies. These are replies that, once enabled, are sent by the device on its own in a defined interval. In this case, the id of the reply or a placeholder id should be added to the deviceCommandMap + * with a commandData_t where maxDelayCycles is set to the maximum expected number of PST cycles between two replies (also a tolerance should be added, as an FDIR message will be generated if it is missed). + * As soon as the replies are enabled, DeviceCommandInfo.periodic must be set to 1, DeviceCommandInfo.delayCycles to DeviceCommandInfo.MaxDelayCycles. From then on, the base class handles the reception. + * Then, scanForReply returns the id of the reply or the placeholder id and the base class will take care of checking that all replies are received and the interval is correct. + * When the replies are disabled, DeviceCommandInfo.periodic must be set to 0, DeviceCommandInfo.delayCycles to 0; + * - Aperiodic, unrequested replies. These are replies that are sent by the device without any preceding command and not in a defined interval. These are not entered in the deviceCommandMap but handled by returning @c APERIODIC_REPLY in scanForReply(). + * + */ + virtual void fillCommandAndReplyMap() = 0; + + /** + * This is a helper method to facilitate inserting entries in the command map. + * @param deviceCommand Identifier of the command to add. + * @param maxDelayCycles The maximum number of delay cycles the command waits until it times out. + * @param periodic Indicates if the command is periodic (i.e. it is sent by the device repeatedly without request) or not. + * Default is aperiodic (0) + * @return RETURN_OK when the command was successfully inserted, COMMAND_MAP_ERROR else. + */ + ReturnValue_t insertInCommandAndReplyMap(DeviceCommandId_t deviceCommand, + uint16_t maxDelayCycles, uint8_t periodic = 0, + bool hasDifferentReplyId = false, DeviceCommandId_t replyId = 0); + /** + * This is a helper method to insert replies in the reply map. + * @param deviceCommand Identifier of the reply to add. + * @param maxDelayCycles The maximum number of delay cycles the reply waits until it times out. + * @param periodic Indicates if the command is periodic (i.e. it is sent by the device repeatedly without request) or not. + * Default is aperiodic (0) + * @return RETURN_OK when the command was successfully inserted, COMMAND_MAP_ERROR else. + */ + ReturnValue_t insertInReplyMap(DeviceCommandId_t deviceCommand, + uint16_t maxDelayCycles, uint8_t periodic = 0); + /** + * A simple command to add a command to the commandList. + * @param deviceCommand The command to add + * @return RETURN_OK if the command was successfully inserted, RETURN_FAILED else. + */ + ReturnValue_t insertInCommandMap(DeviceCommandId_t deviceCommand); + /** + * This is a helper method to facilitate updating entries in the reply map. + * @param deviceCommand Identifier of the reply to update. + * @param delayCycles The current number of delay cycles to wait. As stated in #fillCommandAndCookieMap, to disable periodic commands, this is set to zero. + * @param maxDelayCycles The maximum number of delay cycles the reply waits until it times out. By passing 0 the entry remains untouched. + * @param periodic Indicates if the command is periodic (i.e. it is sent by the device repeatedly without request) or not. + * Default is aperiodic (0). Warning: The setting always overrides the value that was entered in the map. + * @return RETURN_OK when the reply was successfully updated, COMMAND_MAP_ERROR else. + */ + ReturnValue_t updateReplyMapEntry(DeviceCommandId_t deviceReply, + uint16_t delayCycles, uint16_t maxDelayCycles, + uint8_t periodic = 0); + /** + * Returns the delay cycle count of a reply. + * A count != 0 indicates that the command is already executed. + * @param deviceCommand The command to look for + * @return The current delay count. If the command does not exist (should never happen) it returns 0. + */ + uint8_t getReplyDelayCycles(DeviceCommandId_t deviceCommand); + /** + * Scans a buffer for a valid reply. + * + * This is used by the base class to check the data received from the RMAP stack for valid packets. + * It only checks if a valid packet starts at @c start. + * It also only checks the structural validy of the packet, eg checksums lengths and protocol data. No + * information check is done, eg range checks etc. + * + * Errors should be reported directly, the base class does NOT report any errors based on the return + * value of this function. + * + * @param start start of data + * @param len length of data + * @param[out] foundId the id of the packet starting at @c start + * @param[out] foundLen length of the packet found + * @return + * - @c RETURN_OK a valid packet was found at @c start, @c foundLen is valid + * - @c NO_VALID_REPLY no reply could be found starting at @c start, implies @c foundLen is not valid, base class will call scanForReply() again with ++start + * - @c INVALID_REPLY a packet was found but it is invalid, eg checksum error, implies @c foundLen is valid, can be used to skip some bytes + * - @c TOO_SHORT @c len is too short for any valid packet + * - @c APERIODIC_REPLY if a valid reply is received that has not been requested by a command, but should be handled anyway (@see also fillCommandAndCookieMap() ) + */ + virtual ReturnValue_t scanForReply(const uint8_t *start, uint32_t len, + DeviceCommandId_t *foundId, uint32_t *foundLen) = 0; + + /** + * Interpret a reply from the device. + * + * This is called after scanForReply() found a valid packet, it can be assumed that the length and structure is valid. + * This routine extracts the data from the packet into a DataSet and then calls handleDeviceTM(), which either sends + * a TM packet or stores the data in the DataPool depending on whether the it was an external command. + * No packet length is given, as it should be defined implicitly by the id. + * + * @param id the id found by scanForReply() + * @param packet + * @param commander the one who initiated the command, is 0 if not external commanded + * @return + * - @c RETURN_OK when the reply was interpreted. + * - @c RETURN_FAILED when the reply could not be interpreted, eg. logical errors or range violations occurred + */ + virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) = 0; + + /** + * Construct a command reply containing a raw reply. + * + * It gets space in the #IPCStore, copies data there, then sends a raw reply + * containing the store address. + * + * This method is virtual, as the STR has a different channel to send raw replies + * and overwrites it accordingly. + * + * @param data data to send + * @param len length of @c data + * @param sendTo the messageQueueId of the one to send to + * @param isCommand marks the raw data as a command, the message then will be of type raw_command + */ + virtual void replyRawData(const uint8_t *data, size_t len, + MessageQueueId_t sendTo, bool isCommand = false); + + /** + * Return the switches connected to the device. + * + * The default implementation returns one switch set in the ctor. + * + * + * @param[out] switches pointer to an array of switches + * @param[out] numberOfSwitches length of returned array + * @return + * - @c RETURN_OK if the parameters were set + * - @c RETURN_FAILED if no switches exist + */ + virtual ReturnValue_t getSwitches(const uint8_t **switches, + uint8_t *numberOfSwitches); + + /** + * notify child about mode change + */ + virtual void modeChanged(void); + + struct DeviceCommandInfo { + bool isExecuting; //!< Indicates if the command is already executing. + uint8_t expectedReplies; //!< Dynamic value to indicate how many replies are expected. + MessageQueueId_t sendReplyTo; //!< if this is != NO_COMMANDER, DHB was commanded externally and shall report everything to commander. + }; + + typedef std::map DeviceCommandMap; + /** + * Enable the reply checking for a command + * + * Is only called, if the command was sent (ie the getWriteReply was successful). + * Must ensure that all replies are activated and correctly linked to the command that initiated it. + * The default implementation looks for a reply with the same id as the command id in the replyMap or + * uses the alternativeReplyId if flagged so. + * When found, copies maxDelayCycles to delayCycles in the reply information and sets the command to + * expect one reply. + * + * Can be overwritten by the child, if a command activates multiple replies or replyId differs from + * commandId. + * Notes for child implementations: + * - If the command was not found in the reply map, NO_REPLY_EXPECTED MUST be returned. + * - A failure code may be returned if something went fundamentally wrong. + * + * @param deviceCommand + * @return - RETURN_OK if a reply was activated. + * - NO_REPLY_EXPECTED if there was no reply found. This is not an error case as many commands + * do not expect a reply. + */ + virtual ReturnValue_t enableReplyInReplyMap(DeviceCommandMap::iterator cmd, + uint8_t expectedReplies = 1, bool useAlternateId = false, + DeviceCommandId_t alternateReplyID = 0); + + /** + * get the state of the PCDU switches in the datapool + * + * @return + * - @c PowerSwitchIF::SWITCH_ON if all switches specified by #switches are on + * - @c PowerSwitchIF::SWITCH_OFF one of the switches specified by #switches are off + * - @c PowerSwitchIF::RETURN_FAILED if an error occured + */ + ReturnValue_t getStateOfSwitches(void); + + /** + * set all datapool variables that are update periodically in normal mode invalid + * + * Child classes should provide an implementation which sets all those variables invalid + * which are set periodically during any normal mode. + */ + virtual void setNormalDatapoolEntriesInvalid() = 0; + + /** + * Children can overwrite this function to suppress checking of the command Queue + * + * This can be used when the child does not want to receive a command in a certain + * situation. Care must be taken that checking is not permanentely disabled as this + * would render the handler unusable. + * + * @return whether checking the queue should NOT be done + */ + virtual bool dontCheckQueue(); + + Mode_t getBaseMode(Mode_t transitionMode); + + bool isAwaitingReply(); + + void handleDeviceTM(SerializeIF *dataSet, DeviceCommandId_t commandId, + bool neverInDataPool = false); + + virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode); + virtual void startTransition(Mode_t mode, Submode_t submode); + virtual void setToExternalControl(); + virtual void announceMode(bool recursive); + + virtual ReturnValue_t letChildHandleMessage(CommandMessage *message); + + /** + * Overwrites SystemObject::triggerEvent in order to inform FDIR"Helper" faster about executed events. + * This is a bit sneaky, but improves responsiveness of the device FDIR. + * @param event The event to be thrown + * @param parameter1 Optional parameter 1 + * @param parameter2 Optional parameter 2 + */ + void triggerEvent(Event event, uint32_t parameter1 = 0, + uint32_t parameter2 = 0); + /** + * Same as triggerEvent, but for forwarding if object is used as proxy. + */ + virtual void forwardEvent(Event event, uint32_t parameter1 = 0, + uint32_t parameter2 = 0) const; + /** + * Checks state of switches in conjunction with mode and triggers an event if they don't fit. + */ + virtual void checkSwitchState(); + + /** + * Reserved for the rare case where a device needs to perform additional operation cyclically in OFF mode. + */ + virtual void doOffActivity(); + + /** + * Reserved for the rare case where a device needs to perform additional operation cyclically in ON mode. + */ + virtual void doOnActivity(); + + /** + * Checks if current mode is transitional mode. + * @return true if mode is transitional, false else. + */ + bool isTransitionalMode(); + + /** + * Checks if current handler state allows reception of external device commands. + * Default implementation allows commands only in plain MODE_ON and MODE_NORMAL. + * @return RETURN_OK if commands are accepted, anything else otherwise. + */ + virtual ReturnValue_t acceptExternalDeviceCommands(); + + bool commandIsExecuting(DeviceCommandId_t commandId); +private: + + /** + * Information about commands + */ + DeviceCommandMap deviceCommandMap; + + /** + * Information about expected replies + * + * This is used to keep track of pending replies + */ + struct DeviceReplyInfo { + uint16_t maxDelayCycles; //!< The maximum number of cycles the handler should wait for a reply to this command. + uint16_t delayCycles; //!< The currently remaining cycles the handler should wait for a reply, 0 means there is no reply expected + uint8_t periodic; //!< if this is !=0, the delayCycles will not be reset to 0 but to maxDelayCycles + DeviceCommandMap::iterator command; //!< The command that expects this reply. + }; + /** + * Definition for the important reply Map. + */ + typedef std::map DeviceReplyMap; + /** + * This map is used to check and track correct reception of all replies. + * + * It has multiple use: + * - it stores the information on pending replies. If a command is sent, the DeviceCommandInfo.count is incremented. + * - it is used to time-out missing replies. If a command is sent, the DeviceCommandInfo.DelayCycles is set to MaxDelayCycles. + * - it is queried to check if a reply from the device can be interpreted. scanForReply() returns the id of the command a reply was found for. + * The reply is ignored in the following cases: + * - No entry for the returned id was found + * - The deviceReplyInfo.delayCycles is == 0 + */ + DeviceReplyMap deviceReplyMap; + + /** + * State a cookie is in. + * + * Used to keep track of the state of the RMAP communication. + */ + enum CookieState_t { + COOKIE_UNUSED, //!< The Cookie is unused + COOKIE_WRITE_READY, //!< There's data available to send. + COOKIE_READ_SENT, //!< A sendRead command was sent with this cookie + COOKIE_WRITE_SENT //!< A sendWrite command was sent with this cookie + }; + /** + * Information about a cookie. + * + * This is stored in a map for each cookie, to not only track the state, but also information + * about the sent command. Tracking this information is needed as + * the state of a commandId (waiting for reply) is done when a RMAP write reply is received. + */ + struct CookieInfo { + CookieState_t state; + DeviceCommandMap::iterator pendingCommand; + }; + + /** + * Info about the #cookie + * + * Used to track the state of the communication + */ + CookieInfo cookieInfo; + + /** + * cached from ctor for initialize() + */ + const uint32_t ioBoardAddress; + + /** + * Used for timing out mode transitions. + * + * Set when setMode() is called. + */ + uint32_t timeoutStart; + + /** + * Delay for the current mode transition, used for time out + */ + uint32_t childTransitionDelay; + + /** + * The mode the current transition originated from + * + * This is private so the child can not change it and fuck up the timeouts + * + * IMPORTANT: This is not valid during _MODE_SHUT_DOWN and _MODE_START_UP!! (it is _MODE_POWER_DOWN during this modes) + * + * is element of [MODE_ON, MODE_NORMAL, MODE_RAW] + */ + Mode_t transitionSourceMode; + + /** + * the submode of the source mode during a transition + */ + Submode_t transitionSourceSubMode; + + /** + * the switch of the device + * + * for devices using two switches override getSwitches() + */ + const uint8_t deviceSwitch; + + ActionHelper actionHelper; + + /** + * read the command queue + */ + void readCommandQueue(void); + + /** + * Handle the device handler mode. + * + * - checks whether commands are valid for the current mode, rejects them accordingly + * - checks whether commanded mode transitions are required and calls handleCommandedModeTransition() + * - does the necessary action for the current mode or calls doChildStateMachine in modes @c MODE_TO_ON and @c MODE_TO_OFF + * - actions that happen in transitions (eg setting a timeout) are handled in setMode() + */ + void doStateMachine(void); + + void buildRawDeviceCommand(CommandMessage* message); + void buildInternalCommand(void); + +// /** +// * Send a reply with the current mode and submode. +// */ +// void announceMode(void); + + /** + * Decrement the counter for the timout of replies. + * + * This is called at the beginning of each cycle. It checks whether a reply has timed out (that means a reply was expected + * but not received). + */ + void decrementDeviceReplyMap(void); + + /** + * Convenience function to handle a reply. + * + * Called after scanForReply() has found a packet. Checks if the found id is in the #deviceCommandMap, if so, + * calls interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) for further action. + * + * It also resets the timeout counter for the command id. + * + * @param data the found packet + * @param id the found id + */ + void handleReply(const uint8_t *data, DeviceCommandId_t id); + + void replyToReply(DeviceReplyMap::iterator iter, ReturnValue_t status); + /** + * Build and send a command to the device. + * + * This routine checks whether a raw or direct command has been received, checks the content of the received command and + * calls buildCommandFromCommand() for direct commands or sets #rawpacket to the received raw packet. + * If no external command is received or the received command is invalid and the current mode is @c MODE_NORMAL or a transitional mode, + * it asks the child class to build a command (via getNormalDeviceCommand() or getTransitionalDeviceCommand() and buildCommand()) and + * sends the command via RMAP. + */ + void doSendWrite(void); + + /** + * Check if the RMAP sendWrite action was successful. + * + * Depending on the result, the following is done + * - if the device command was external commanded, a reply is sent indicating the result + * - if the action was successful, the reply timout counter is initialized + */ + void doGetWrite(void); + + /** + * Send a RMAP getRead command. + * + * The size of the getRead command is #maxDeviceReplyLen. + * This is always executed, independently from the current mode. + */ + void doSendRead(void); + + /** + * Check the getRead reply and the contained data. + * + * If data was received scanForReply() and, if successful, handleReply() are called. + * If the current mode is @c MODE_RAW, the received packet is sent to the commanding object + * via commandQueue. + */ + void doGetRead(void); + + /** + * Retrive data from the #IPCStore. + * + * @param storageAddress + * @param[out] data + * @param[out] len + * @return + * - @c RETURN_OK @c data is valid + * - @c RETURN_FAILED IPCStore is NULL + * - the return value from the IPCStore if it was not @c RETURN_OK + */ + ReturnValue_t getStorageData(store_address_t storageAddress, uint8_t **data, + uint32_t *len); + + /** + * set all switches returned by getSwitches() + * + * @param onOff on == @c SWITCH_ON; off != @c SWITCH_ON + */ + void commandSwitch(ReturnValue_t onOff); + + /** + * @param modeTo either @c MODE_ON, MODE_NORMAL or MODE_RAW NOTHING ELSE!!! + */ + void setTransition(Mode_t modeTo, Submode_t submodeTo); + + /** + * calls the right child function for the transitional submodes + */ + void callChildStatemachine(); + + /** + * Switches the channel of the cookie used for the communication + * + * + * @param newChannel the object Id of the channel to switch to + * @return + * - @c RETURN_OK when cookie was changed + * - @c RETURN_FAILED when cookies could not be changed, eg because the newChannel is not enabled + * - @c returnvalues of RMAPChannelIF::isActive() + */ + ReturnValue_t switchCookieChannel(object_id_t newChannelId); + + ReturnValue_t handleDeviceHandlerMessage(CommandMessage *message); +}; + +#endif /* DEVICEHANDLERBASE_H_ */ + diff --git a/devicehandlers/DeviceHandlerFDIR.cpp b/devicehandlers/DeviceHandlerFDIR.cpp new file mode 100644 index 00000000..c69045fc --- /dev/null +++ b/devicehandlers/DeviceHandlerFDIR.cpp @@ -0,0 +1,239 @@ +/* + * DeviceHandlerFDIR.cpp + * + * Created on: 09.09.2015 + * Author: baetz + */ + +#include +#include +#include +#include +#include +#include + +//TODO: Mechanisms have no power FDIR.. +DeviceHandlerFDIR::DeviceHandlerFDIR(object_id_t owner, object_id_t parent) : + FDIRBase(owner, parent), strangeReplyCount(MAX_STRANGE_REPLIES, + STRANGE_REPLIES_TIME_MS, parameterDomainBase++), missedReplyCount( + MAX_MISSED_REPLY_COUNT, MISSED_REPLY_TIME_MS, + parameterDomainBase++), recoveryCounter(MAX_REBOOT, + REBOOT_TIME_MS, parameterDomainBase++), fdirState(NONE), powerConfirmation( + 0) { +} + +DeviceHandlerFDIR::~DeviceHandlerFDIR() { +} + +ReturnValue_t DeviceHandlerFDIR::eventReceived(EventMessage* event) { + if (fdirState != NONE) { + //Only wait for those events, ignore all others. + if (event->getParameter1() == HasHealthIF::HEALTHY + && event->getEvent() == HasHealthIF::HEALTH_INFO) { + setFdirState(NONE); + } + if (event->getEvent() == HasModesIF::MODE_INFO + && fdirState != RECOVERY_ONGOING) { + setFdirState(NONE); + } + return RETURN_OK; + } + if (owner->getHealth() == HasHealthIF::FAULTY + || owner->getHealth() == HasHealthIF::PERMANENT_FAULTY) { + //Ignore all events in case device is already faulty. + return RETURN_OK; + } + ReturnValue_t result = RETURN_FAILED; + switch (event->getEvent()) { + case HasModesIF::MODE_TRANSITION_FAILED: + case HasModesIF::OBJECT_IN_INVALID_MODE: + //We'll try a recovery as long as defined in MAX_REBOOT. + //Might cause some AssemblyBase cycles, so keep number low. + handleRecovery(event->getEvent()); + break; + case DeviceHandlerIF::DEVICE_INTERPRETING_REPLY_FAILED: + case DeviceHandlerIF::DEVICE_READING_REPLY_FAILED: + case DeviceHandlerIF::DEVICE_UNREQUESTED_REPLY: + case DeviceHandlerIF::DEVICE_UNKNOWN_REPLY: //Some DH's generate generic reply-ids. + case DeviceHandlerIF::DEVICE_BUILDING_COMMAND_FAILED: + //These faults all mean that there were stupid replies from a device. + if (strangeReplyCount.incrementAndCheck()) { + handleRecovery(event->getEvent()); + } + break; + case DeviceHandlerIF::DEVICE_SENDING_COMMAND_FAILED: + case DeviceHandlerIF::DEVICE_REQUESTING_REPLY_FAILED: + //The two above should never be confirmed. + case DeviceHandlerIF::DEVICE_MISSED_REPLY: + result = sendConfirmationRequest(event); + if (result == HasReturnvaluesIF::RETURN_OK) { + break; + } + //else + if (missedReplyCount.incrementAndCheck()) { + handleRecovery(event->getEvent()); + } + break; + case StorageManagerIF::GET_DATA_FAILED: + case StorageManagerIF::STORE_DATA_FAILED: + case MessageQueue::SEND_MSG_FAILED: + //Rather strange bugs, occur in RAW mode only. TODO:? By now: Ignore. + break; + case DeviceHandlerIF::INVALID_DEVICE_COMMAND: + //Ignore, is bad configuration. We can't do anything in flight. + break; + case DeviceHandlerIF::MONITORING_AMBIGUOUS: + case HasHealthIF::HEALTH_INFO: + case HasModesIF::MODE_INFO: + case HasModesIF::CHANGING_MODE: + //Do nothing, but mark as handled. + break; + //****Power***** + case PowerSwitchIF::SWITCH_WENT_OFF: + result = sendConfirmationRequest(event, powerConfirmation); + if (result == RETURN_OK) { + setFdirState(DEVICE_MIGHT_BE_OFF); + } + break; + case Fuse::FUSE_WENT_OFF: + //Not so good, because PCDU reacted. + case Fuse::POWER_ABOVE_HIGH_LIMIT: + //Better, because software detected over-current. + setFaulty(event->getEvent()); + break; + case Fuse::POWER_BELOW_LOW_LIMIT: + //Device might got stuck during boot, retry. + handleRecovery(event->getEvent()); + break; + //****Thermal***** + case ThermalComponentIF::COMPONENT_TEMP_LOW: + case ThermalComponentIF::COMPONENT_TEMP_HIGH: + case ThermalComponentIF::COMPONENT_TEMP_OOL_LOW: + case ThermalComponentIF::COMPONENT_TEMP_OOL_HIGH: + //Well, the device is not really faulty, but it is required to stay off as long as possible. + //*******ACS***** + case DeviceHandlerIF::MONITORING_LIMIT_EXCEEDED: + setFaulty(event->getEvent()); + break; + case ThermalComponentIF::TEMP_NOT_IN_OP_RANGE: + //Ignore, is information only. + break; + default: + //We don't know the event, someone else should handle it. + return RETURN_FAILED; + } + return RETURN_OK; +} + +void DeviceHandlerFDIR::eventConfirmed(EventMessage* event) { + switch (event->getEvent()) { + case DeviceHandlerIF::DEVICE_SENDING_COMMAND_FAILED: + case DeviceHandlerIF::DEVICE_REQUESTING_REPLY_FAILED: + case DeviceHandlerIF::DEVICE_MISSED_REPLY: + if (missedReplyCount.incrementAndCheck()) { + handleRecovery(event->getEvent()); + } + break; + case PowerSwitchIF::SWITCH_WENT_OFF: + //This means the switch went off only for one device. + handleRecovery(event->getEvent()); + break; + default: + break; + } +} + +void DeviceHandlerFDIR::decrementFaultCounters() { + strangeReplyCount.checkForDecrement(); + missedReplyCount.checkForDecrement(); + recoveryCounter.checkForDecrement(); +} + +void DeviceHandlerFDIR::handleRecovery(Event reason) { + clearFaultCounters(); + if (!recoveryCounter.incrementAndCheck()) { + startRecovery(reason); + } else { + setFaulty(reason); + } +} + +void DeviceHandlerFDIR::wasParentsFault(EventMessage* event) { + //We'll better ignore the SWITCH_WENT_OFF event and await a system-wide reset. + //This means, no fault message will come through until a MODE_ or HEALTH_INFO message comes through -> Is that ok? + //Same issue in TxFailureIsolation! +// if ((event->getEvent() == PowerSwitchIF::SWITCH_WENT_OFF) +// && (fdirState != RECOVERY_ONGOING)) { +// setFdirState(NONE); +// } +} + +void DeviceHandlerFDIR::clearFaultCounters() { + strangeReplyCount.clear(); + missedReplyCount.clear(); +} + +ReturnValue_t DeviceHandlerFDIR::initialize() { + ReturnValue_t result = FDIRBase::initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + ConfirmsFailuresIF* power = objectManager->get( + objects::PCDU_HANDLER); + if (power == NULL) { + return RETURN_FAILED; + } + powerConfirmation = power->getEventReceptionQueue(); + return RETURN_OK; +} + +void DeviceHandlerFDIR::setFdirState(FDIRState state) { + FDIRBase::throwFdirEvent(FDIR_CHANGED_STATE, state, fdirState); + fdirState = state; +} + +void DeviceHandlerFDIR::triggerEvent(Event event, uint32_t parameter1, + uint32_t parameter2) { + //Do not throw error events if fdirState != none. + //This will still forward MODE and HEALTH INFO events in any case. + if (fdirState == NONE || EVENT::getSeverity(event) == SEVERITY::INFO) { + FDIRBase::triggerEvent(event, parameter1, parameter2); + } +} + +bool DeviceHandlerFDIR::isFdirActionInProgress() { + return (fdirState != NONE); +} + +void DeviceHandlerFDIR::startRecovery(Event reason) { + throwFdirEvent(FDIR_STARTS_RECOVERY, EVENT::getEventId(reason)); + setOwnerHealth(HasHealthIF::NEEDS_RECOVERY); + setFdirState(RECOVERY_ONGOING); +} + +ReturnValue_t DeviceHandlerFDIR::getParameter(uint8_t domainId, + uint16_t parameterId, ParameterWrapper* parameterWrapper, + const ParameterWrapper* newValues, uint16_t startAtIndex) { + ReturnValue_t result = strangeReplyCount.getParameter(domainId, parameterId, + parameterWrapper, newValues, startAtIndex); + if (result != INVALID_DOMAIN_ID) { + return result; + } + result = missedReplyCount.getParameter(domainId, parameterId, + parameterWrapper, newValues, startAtIndex); + if (result != INVALID_DOMAIN_ID) { + return result; + } + result = recoveryCounter.getParameter(domainId, parameterId, + parameterWrapper, newValues, startAtIndex); + if (result != INVALID_DOMAIN_ID) { + return result; + } + return INVALID_DOMAIN_ID; +} + +void DeviceHandlerFDIR::setFaulty(Event reason) { + throwFdirEvent(FDIR_TURNS_OFF_DEVICE, EVENT::getEventId(reason)); + setOwnerHealth(HasHealthIF::FAULTY); + setFdirState(AWAIT_SHUTDOWN); +} diff --git a/devicehandlers/DeviceHandlerFDIR.h b/devicehandlers/DeviceHandlerFDIR.h new file mode 100644 index 00000000..3e33c46a --- /dev/null +++ b/devicehandlers/DeviceHandlerFDIR.h @@ -0,0 +1,54 @@ +/* + * DeviceHandlerFDIR.h + * + * Created on: 09.09.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_DEVICEHANDLERS_DEVICEHANDLERFDIR_H_ +#define FRAMEWORK_DEVICEHANDLERS_DEVICEHANDLERFDIR_H_ + +#include +#include + +class DeviceHandlerFDIR: public FDIRBase { +public: + DeviceHandlerFDIR(object_id_t owner, object_id_t parent = objects::IO_ASSEMBLY); + ~DeviceHandlerFDIR(); + virtual ReturnValue_t eventReceived(EventMessage* event); + void eventConfirmed(EventMessage* event); + void wasParentsFault(EventMessage* event); + void decrementFaultCounters(); + ReturnValue_t initialize(); + void triggerEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0); + bool isFdirActionInProgress(); + virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, + ParameterWrapper *parameterWrapper, + const ParameterWrapper *newValues, uint16_t startAtIndex); +protected: + FaultCounter strangeReplyCount; + FaultCounter missedReplyCount; + FaultCounter recoveryCounter; + enum FDIRState { + NONE, + RECOVERY_ONGOING, + DEVICE_MIGHT_BE_OFF, + AWAIT_SHUTDOWN + }; + FDIRState fdirState; + MessageQueueId_t powerConfirmation; + //TODO: Arbitrary numbers! Adjust! + static const uint32_t MAX_REBOOT = 1; + static const uint32_t REBOOT_TIME_MS = 180000; + static const uint32_t MAX_STRANGE_REPLIES = 10; + static const uint32_t STRANGE_REPLIES_TIME_MS = 10000; + static const uint32_t MAX_MISSED_REPLY_COUNT = 5; + static const uint32_t MISSED_REPLY_TIME_MS = 10000; + void handleRecovery(Event reason); + virtual void clearFaultCounters(); + void setFdirState(FDIRState state); + void startRecovery(Event reason); + void setFaulty(Event reason); +}; + +#endif /* FRAMEWORK_DEVICEHANDLERS_DEVICEHANDLERFDIR_H_ */ diff --git a/devicehandlers/DeviceHandlerIF.h b/devicehandlers/DeviceHandlerIF.h new file mode 100644 index 00000000..4922556a --- /dev/null +++ b/devicehandlers/DeviceHandlerIF.h @@ -0,0 +1,104 @@ +#ifndef DEVICEHANDLERIF_H_ +#define DEVICEHANDLERIF_H_ + +#include +#include +#include +#include +#include + +/** + * This is the Interface used to communicate with a device handler. + * + */ +class DeviceHandlerIF { +public: + + static const uint8_t TRANSITION_MODE_CHILD_ACTION_MASK = 0x20; + static const uint8_t TRANSITION_MODE_BASE_ACTION_MASK = 0x10; + + /** + * @brief This is the mode the device handler is in. + * + * @details The mode of the device handler must not be confused with the mode the device is in. + * The mode of the device itself is transparent to the user but related to the mode of the handler. + */ +// MODE_ON = 0, //!< The device is powered and ready to perform operations. In this mode, no commands are sent by the device handler itself, but direct commands van be commanded and will be interpreted +// MODE_OFF = 1, //!< The device is powered off. The only command accepted in this mode is a mode change to on. + static const Mode_t MODE_NORMAL = 2; //!< The device is powered on and the device handler periodically sends commands. The commands to be sent are selected by the handler according to the submode. + static const Mode_t MODE_RAW = 3; //!< The device is powered on and ready to perform operations. In this mode, raw commands can be sent. The device handler will send all replies received from the command back to the commanding object. + static const Mode_t MODE_ERROR_ON = 4; //!4< The device is shut down but the switch could not be turned off, so the device still is powered. In this mode, only a mode change to @c MODE_OFF can be commanded, which tries to switch off the device again. + static const Mode_t _MODE_START_UP = TRANSITION_MODE_CHILD_ACTION_MASK | 5; //!< This is a transitional state which can not be commanded. The device handler performs all commands to get the device in a state ready to perform commands. When this is completed, the mode changes to @c MODE_ON. + static const Mode_t _MODE_SHUT_DOWN = TRANSITION_MODE_CHILD_ACTION_MASK | 6; //!< This is a transitional state which can not be commanded. The device handler performs all actions and commands to get the device shut down. When the device is off, the mode changes to @c MODE_OFF. + static const Mode_t _MODE_TO_ON = TRANSITION_MODE_CHILD_ACTION_MASK | HasModesIF::MODE_ON; + static const Mode_t _MODE_TO_RAW = TRANSITION_MODE_CHILD_ACTION_MASK | MODE_RAW; + static const Mode_t _MODE_TO_NORMAL = TRANSITION_MODE_CHILD_ACTION_MASK | MODE_NORMAL; + static const Mode_t _MODE_POWER_DOWN = TRANSITION_MODE_BASE_ACTION_MASK | 1; //!< This is a transitional state which can not be commanded. The device is shut down and ready to be switched off. After the command to set the switch off has been sent, the mode changes to @c MODE_WAIT_OFF + static const Mode_t _MODE_POWER_ON = TRANSITION_MODE_BASE_ACTION_MASK | 2; //!< This is a transitional state which can not be commanded. The device will be switched on in this state. After the command to set the switch on has been sent, the mode changes to @c MODE_WAIT_ON + static const Mode_t _MODE_WAIT_OFF = TRANSITION_MODE_BASE_ACTION_MASK | 3; //!< This is a transitional state which can not be commanded. The switch has been commanded off and the handler waits for it to be off. When the switch is off, the mode changes to @c MODE_OFF. + static const Mode_t _MODE_WAIT_ON = TRANSITION_MODE_BASE_ACTION_MASK | 4; //!< This is a transitional state which can not be commanded. The switch has been commanded on and the handler waits for it to be on. When the switch is on, the mode changes to @c MODE_TO_ON. + static const Mode_t _MODE_SWITCH_IS_OFF = TRANSITION_MODE_BASE_ACTION_MASK | 5; //!< This is a transitional state which can not be commanded. The switch has been commanded off and is off now. This state is only to do an RMAP cycle once more where the doSendRead() function will set the mode to MODE_OFF. The reason to do this is to get rid of stuck packets in the IO Board + + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::CDH; + static const Event DEVICE_BUILDING_COMMAND_FAILED = MAKE_EVENT(0, SEVERITY::LOW); + static const Event DEVICE_SENDING_COMMAND_FAILED = MAKE_EVENT(1, SEVERITY::LOW); + static const Event DEVICE_REQUESTING_REPLY_FAILED = MAKE_EVENT(2, SEVERITY::LOW); + static const Event DEVICE_READING_REPLY_FAILED = MAKE_EVENT(3, SEVERITY::LOW); + static const Event DEVICE_INTERPRETING_REPLY_FAILED = MAKE_EVENT(4, SEVERITY::LOW); + static const Event DEVICE_MISSED_REPLY = MAKE_EVENT(5, SEVERITY::LOW); + static const Event DEVICE_UNKNOWN_REPLY = MAKE_EVENT(6, SEVERITY::LOW); + static const Event DEVICE_UNREQUESTED_REPLY = MAKE_EVENT(7, SEVERITY::LOW); + static const Event INVALID_DEVICE_COMMAND = MAKE_EVENT(8, SEVERITY::LOW); //!< Indicates a SW bug in child class. + static const Event MONITORING_LIMIT_EXCEEDED = MAKE_EVENT(9, SEVERITY::LOW); + static const Event MONITORING_AMBIGUOUS = MAKE_EVENT(10, SEVERITY::HIGH); + + static const uint8_t INTERFACE_ID = DEVICE_HANDLER_IF; + static const ReturnValue_t NO_COMMAND_DATA = MAKE_RETURN_CODE(0xA0); + static const ReturnValue_t COMMAND_NOT_SUPPORTED = MAKE_RETURN_CODE(0xA1); + static const ReturnValue_t COMMAND_ALREADY_SENT = MAKE_RETURN_CODE(0xA2); + static const ReturnValue_t COMMAND_WAS_NOT_SENT = MAKE_RETURN_CODE(0xA3); + static const ReturnValue_t CANT_SWITCH_IOBOARD = MAKE_RETURN_CODE(0xA4); + static const ReturnValue_t WRONG_MODE_FOR_COMMAND = MAKE_RETURN_CODE(0xA5); + static const ReturnValue_t TIMEOUT = MAKE_RETURN_CODE(0xA6); + static const ReturnValue_t BUSY = MAKE_RETURN_CODE(0xA7); + static const ReturnValue_t NO_REPLY_EXPECTED = MAKE_RETURN_CODE(0xA8); //!< Used to indicate that this is a command-only command. + static const ReturnValue_t NON_OP_TEMPERATURE = MAKE_RETURN_CODE(0xA9); + + //standard codes used in scan for reply + // static const ReturnValue_t TOO_SHORT = MAKE_RETURN_CODE(0xB1); + static const ReturnValue_t CHECKSUM_ERROR = MAKE_RETURN_CODE(0xB2); + static const ReturnValue_t LENGTH_MISSMATCH = MAKE_RETURN_CODE(0xB3); + static const ReturnValue_t INVALID_DATA = MAKE_RETURN_CODE(0xB4); + static const ReturnValue_t PROTOCOL_ERROR = MAKE_RETURN_CODE(0xB5); + + //standard codes used in interpret device reply + static const ReturnValue_t DEVICE_DID_NOT_EXECUTE = MAKE_RETURN_CODE(0xC1); //the device reported, that it did not execute the command + static const ReturnValue_t DEVICE_REPORTED_ERROR = MAKE_RETURN_CODE(0xC2); + static const ReturnValue_t UNKNOW_DEVICE_REPLY = MAKE_RETURN_CODE(0xC3); //the deviceCommandId reported by scanforReply is unknown + static const ReturnValue_t DEVICE_REPLY_INVALID = MAKE_RETURN_CODE(0xC4); //syntax etc is correct but still not ok, eg parameters where none are expected + + //Standard codes used in buildCommandFromCommand + static const ReturnValue_t INVALID_COMMAND_PARAMETER = MAKE_RETURN_CODE( + 0xD0); + static const ReturnValue_t INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS = + MAKE_RETURN_CODE(0xD1); + /** + * Default Destructor + */ + virtual ~DeviceHandlerIF() { + + } + + /** + * This MessageQueue is used to command the device handler. + * + * To command a device handler, a DeviceHandlerCommandMessage can be sent to this Queue. + * The handler replies with a DeviceHandlerCommandMessage containing the DeviceHandlerCommand_t reply. + * + * @return the id of the MessageQueue + */ + virtual MessageQueueId_t getCommandQueue() const = 0; + +}; + +#endif /* DEVICEHANDLERIF_H_ */ diff --git a/devicehandlers/DeviceHandlerMessage.cpp b/devicehandlers/DeviceHandlerMessage.cpp new file mode 100644 index 00000000..30a5c413 --- /dev/null +++ b/devicehandlers/DeviceHandlerMessage.cpp @@ -0,0 +1,101 @@ +#include +#include +#include + +DeviceHandlerMessage::DeviceHandlerMessage() { +} + +store_address_t DeviceHandlerMessage::getStoreAddress( + const CommandMessage* message) { + return store_address_t(message->getParameter2()); +} + +uint32_t DeviceHandlerMessage::getDeviceCommandId( + const CommandMessage* message) { + return message->getParameter(); +} + +object_id_t DeviceHandlerMessage::getIoBoardObjectId( + const CommandMessage* message) { + return message->getParameter(); +} + +uint8_t DeviceHandlerMessage::getWiretappingMode( + const CommandMessage* message) { + return message->getParameter(); +} + +//void DeviceHandlerMessage::setDeviceHandlerDirectCommandMessage( +// CommandMessage* message, DeviceCommandId_t deviceCommand, +// store_address_t commandParametersStoreId) { +// message->setCommand(CMD_DIRECT); +// message->setParameter(deviceCommand); +// message->setParameter2(commandParametersStoreId.raw); +//} + +void DeviceHandlerMessage::setDeviceHandlerRawCommandMessage( + CommandMessage* message, store_address_t rawPacketStoreId) { + message->setCommand(CMD_RAW); + message->setParameter2(rawPacketStoreId.raw); +} + +void DeviceHandlerMessage::setDeviceHandlerWiretappingMessage( + CommandMessage* message, uint8_t wiretappingMode) { + message->setCommand(CMD_WIRETAPPING); + message->setParameter(wiretappingMode); +} + +void DeviceHandlerMessage::setDeviceHandlerSwitchIoBoardMessage( + CommandMessage* message, uint32_t ioBoardIdentifier) { + message->setCommand(CMD_SWITCH_IOBOARD); + message->setParameter(ioBoardIdentifier); +} + +object_id_t DeviceHandlerMessage::getDeviceObjectId( + const CommandMessage* message) { + return message->getParameter(); +} + +void DeviceHandlerMessage::setDeviceHandlerRawReplayMessage( + CommandMessage* message, object_id_t deviceObjectid, + store_address_t rawPacketStoreId, bool isCommand) { + if (isCommand) { + message->setCommand(REPLY_RAW_COMMAND); + } else { + message->setCommand(REPLY_RAW_REPLY); + } + message->setParameter(deviceObjectid); + message->setParameter2(rawPacketStoreId.raw); +} + +void DeviceHandlerMessage::setDeviceHandlerDirectCommandReply( + CommandMessage* message, object_id_t deviceObjectid, + store_address_t commandParametersStoreId) { + message->setCommand(REPLY_DIRECT_COMMAND_DATA); + message->setParameter(deviceObjectid); + message->setParameter2(commandParametersStoreId.raw); +} + +void DeviceHandlerMessage::clear(CommandMessage* message) { + switch (message->getCommand()) { + case CMD_RAW: +// case CMD_DIRECT: + case REPLY_RAW_COMMAND: + case REPLY_RAW_REPLY: + case REPLY_DIRECT_COMMAND_DATA: { + StorageManagerIF *ipcStore = objectManager->get( + objects::IPC_STORE); + if (ipcStore != NULL) { + ipcStore->deleteData(getStoreAddress(message)); + } + } + /* NO BREAK*/ + case CMD_SWITCH_IOBOARD: + case CMD_WIRETAPPING: + message->setCommand(CommandMessage::CMD_NONE); + message->setParameter(0); + message->setParameter2(0); + break; + } + +} diff --git a/devicehandlers/DeviceHandlerMessage.h b/devicehandlers/DeviceHandlerMessage.h new file mode 100644 index 00000000..e521ff97 --- /dev/null +++ b/devicehandlers/DeviceHandlerMessage.h @@ -0,0 +1,91 @@ +#ifndef DEVICEHANDLERMESSAGE_H_ +#define DEVICEHANDLERMESSAGE_H_ + +#include +#include +#include +#include +//TODO: rework the static constructors to name the type of command they are building, maybe even hide setting the commandID. + +/** + * This is used to uniquely identify commands that are sent to a device + * + * The values are defined in the device-specific implementations + */ +typedef uint32_t DeviceCommandId_t; + +/** + * The DeviceHandlerMessage is used to send Commands to a DeviceHandlerIF + */ +class DeviceHandlerMessage { +private: + DeviceHandlerMessage(); +public: + + /** + * These are the commands that can be sent to a DeviceHandlerBase + */ + static const uint8_t MESSAGE_ID = DEVICE_HANDLER_COMMAND_MESSAGE_ID; + static const Command_t CMD_RAW = MAKE_COMMAND_ID( 1 ); //!< Sends a raw command, setParameter is a ::store_id_t containing the raw packet to send +// static const Command_t CMD_DIRECT = MAKE_COMMAND_ID( 2 ); //!< Sends a direct command, setParameter is a ::DeviceCommandId_t, setParameter2 is a ::store_id_t containing the data needed for the command + static const Command_t CMD_SWITCH_IOBOARD = MAKE_COMMAND_ID( 3 ); //!< Requests a IO-Board switch, setParameter() is the IO-Board identifier + static const Command_t CMD_WIRETAPPING = MAKE_COMMAND_ID( 4 ); //!< (De)Activates the monitoring of all raw traffic in DeviceHandlers, setParameter is 0 to deactivate, 1 to activate + + /*static const Command_t REPLY_SWITCHED_IOBOARD = MAKE_COMMAND_ID(1 );//!< Reply to a @c CMD_SWITCH_IOBOARD, indicates switch was successful, getParameter() contains the board switched to (0: nominal, 1: redundant) + static const Command_t REPLY_CANT_SWITCH_IOBOARD = MAKE_COMMAND_ID( 2); //!< Reply to a @c CMD_SWITCH_IOBOARD, indicating the switch could not be performed, getParameter() contains the error message + static const Command_t REPLY_WIRETAPPING = MAKE_COMMAND_ID( 3); //!< Reply to a @c CMD_WIRETAPPING, getParameter() is the current state, 1 enabled, 0 disabled + + static const Command_t REPLY_COMMAND_WAS_SENT = MAKE_COMMAND_ID(4 );//!< Reply to a @c CMD_RAW or @c CMD_DIRECT, indicates the command was successfully sent to the device, getParameter() contains the ::DeviceCommandId_t + static const Command_t REPLY_COMMAND_NOT_SUPPORTED = MAKE_COMMAND_ID(5 );//!< Reply to a @c CMD_DIRECT, the requested ::DeviceCommand_t is not supported, getParameter() contains the requested ::DeviceCommand_t, getParameter2() contains the ::DeviceCommandId_t + static const Command_t REPLY_COMMAND_WAS_NOT_SENT = MAKE_COMMAND_ID(6 );//!< Reply to a @c CMD_RAW or @c CMD_DIRECT, indicates the command was not sent, getParameter contains the RMAP Return code (@see rmap.h), getParameter2() contains the ::DeviceCommandId_t + + static const Command_t REPLY_COMMAND_ALREADY_SENT = MAKE_COMMAND_ID(7 );//!< Reply to a @c CMD_DIRECT, the requested ::DeviceCommand_t has already been sent to the device and not ye been answered + static const Command_t REPLY_WRONG_MODE_FOR_CMD = MAKE_COMMAND_ID(8 );//!< Reply to a @c CMD_RAW or @c CMD_DIRECT, indicates that the requested command can not be sent in the curent mode, getParameter() contains the DeviceHandlerCommand_t + static const Command_t REPLY_NO_DATA = MAKE_COMMAND_ID(9 ); //!< Reply to a CMD_RAW or @c CMD_DIRECT, indicates that the ::store_id_t was invalid, getParameter() contains the ::DeviceCommandId_t, getPrameter2() contains the error code + */ + static const Command_t REPLY_DIRECT_COMMAND_SENT = ActionMessage::STEP_SUCCESS; //!< Signals that a direct command was sent + static const Command_t REPLY_RAW_COMMAND = MAKE_COMMAND_ID(0x11 ); //!< Contains a raw command sent to the Device + static const Command_t REPLY_RAW_REPLY = MAKE_COMMAND_ID( 0x12); //!< Contains a raw reply from the Device, getParameter() is the ObjcetId of the sender, getParameter2() is a ::store_id_t containing the raw packet received + static const Command_t REPLY_DIRECT_COMMAND_DATA = ActionMessage::DATA_REPLY; + + /** + * Default Destructor + */ + virtual ~DeviceHandlerMessage() { + } + + static store_address_t getStoreAddress(const CommandMessage* message); + static uint32_t getDeviceCommandId(const CommandMessage* message); + static object_id_t getDeviceObjectId(const CommandMessage *message); + static object_id_t getIoBoardObjectId(const CommandMessage* message); + static uint8_t getWiretappingMode(const CommandMessage* message); + +// static void setDeviceHandlerDirectCommandMessage(CommandMessage* message, +// DeviceCommandId_t deviceCommand, +// store_address_t commandParametersStoreId); + + static void setDeviceHandlerDirectCommandReply(CommandMessage* message, + object_id_t deviceObjectid, + store_address_t commandParametersStoreId); + + static void setDeviceHandlerRawCommandMessage(CommandMessage* message, + store_address_t rawPacketStoreId); + + static void setDeviceHandlerRawReplayMessage(CommandMessage* message, + object_id_t deviceObjectid, store_address_t rawPacketStoreId, + bool isCommand); + +// static void setDeviceHandlerMessage(CommandMessage* message, +// Command_t command, DeviceCommandId_t deviceCommand, +// store_address_t commandParametersStoreId); +// static void setDeviceHandlerMessage(CommandMessage* message, +// Command_t command, store_address_t rawPacketStoreId); + static void setDeviceHandlerWiretappingMessage(CommandMessage* message, + uint8_t wiretappingMode); + static void setDeviceHandlerSwitchIoBoardMessage(CommandMessage* message, + object_id_t ioBoardIdentifier); + + static void clear(CommandMessage* message); +}; + +#endif /* DEVICEHANDLERMESSAGE_H_ */ diff --git a/devicehandlers/DeviceTmReportingWrapper.cpp b/devicehandlers/DeviceTmReportingWrapper.cpp new file mode 100644 index 00000000..2c9e820f --- /dev/null +++ b/devicehandlers/DeviceTmReportingWrapper.cpp @@ -0,0 +1,46 @@ +#include +#include +#include + +DeviceTmReportingWrapper::DeviceTmReportingWrapper(object_id_t objectId, + ActionId_t actionId, SerializeIF* data) : + objectId(objectId), actionId(actionId), data(data) { +} + +DeviceTmReportingWrapper::~DeviceTmReportingWrapper() { + +} + +ReturnValue_t DeviceTmReportingWrapper::serialize(uint8_t** buffer, + uint32_t* size, const uint32_t max_size, bool bigEndian) const { + ReturnValue_t result = SerializeAdapter::serialize(&objectId, + buffer, size, max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = SerializeAdapter::serialize(&actionId, buffer, + size, max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return data->serialize(buffer, size, max_size, bigEndian); +} + +uint32_t DeviceTmReportingWrapper::getSerializedSize() const { + return sizeof(objectId) + sizeof(ActionId_t) + data->getSerializedSize(); +} + +ReturnValue_t DeviceTmReportingWrapper::deSerialize(const uint8_t** buffer, + int32_t* size, bool bigEndian) { + ReturnValue_t result = SerializeAdapter::deSerialize(&objectId, + buffer, size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = SerializeAdapter::deSerialize(&actionId, buffer, + size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return data->deSerialize(buffer, size, bigEndian); +} diff --git a/devicehandlers/DeviceTmReportingWrapper.h b/devicehandlers/DeviceTmReportingWrapper.h new file mode 100644 index 00000000..1cd9470d --- /dev/null +++ b/devicehandlers/DeviceTmReportingWrapper.h @@ -0,0 +1,27 @@ +#ifndef DEVICETMREPORTINGWRAPPER_H_ +#define DEVICETMREPORTINGWRAPPER_H_ + +#include +#include +#include + +class DeviceTmReportingWrapper: public SerializeIF { +public: + DeviceTmReportingWrapper(object_id_t objectId, ActionId_t actionId, + SerializeIF *data); + virtual ~DeviceTmReportingWrapper(); + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const; + + virtual uint32_t getSerializedSize() const; + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian); +private: + object_id_t objectId; + ActionId_t actionId; + SerializeIF *data; +}; + +#endif /* DEVICETMREPORTINGWRAPPER_H_ */ diff --git a/devicehandlers/HealthDevice.cpp b/devicehandlers/HealthDevice.cpp new file mode 100644 index 00000000..dacdcf19 --- /dev/null +++ b/devicehandlers/HealthDevice.cpp @@ -0,0 +1,57 @@ +#include + +HealthDevice::HealthDevice(object_id_t setObjectId, + MessageQueueId_t parentQueue) : + SystemObject(setObjectId), lastHealth(HEALTHY), parentQueue( + parentQueue), commandQueue(3, + CommandMessage::COMMAND_MESSAGE_SIZE), healthHelper(this, setObjectId) { +} + +HealthDevice::~HealthDevice() { +} + +ReturnValue_t HealthDevice::performOperation() { + CommandMessage message; + ReturnValue_t result = commandQueue.receiveMessage(&message); + if (result == HasReturnvaluesIF::RETURN_OK) { + healthHelper.handleHealthCommand(&message); + } + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t HealthDevice::initialize() { + ReturnValue_t result = SystemObject::initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if (parentQueue != 0) { + return healthHelper.initialize(parentQueue); + } else { + return healthHelper.initialize(); + } +} + +MessageQueueId_t HealthDevice::getCommandQueue() const { + return commandQueue.getId(); +} + +void HealthDevice::setParentQueue(MessageQueueId_t parentQueue) { + healthHelper.setParentQeueue(parentQueue); +} + +bool HealthDevice::hasHealthChanged() { + bool changed; + HealthState currentHealth = healthHelper.getHealth(); + changed = currentHealth != lastHealth; + lastHealth = currentHealth; + return changed; +} + +ReturnValue_t HealthDevice::setHealth(HealthState health) { + healthHelper.setHealth(health); + return HasReturnvaluesIF::RETURN_OK; +} + +HasHealthIF::HealthState HealthDevice::getHealth() { + return healthHelper.getHealth(); +} diff --git a/devicehandlers/HealthDevice.h b/devicehandlers/HealthDevice.h new file mode 100644 index 00000000..6b9f5860 --- /dev/null +++ b/devicehandlers/HealthDevice.h @@ -0,0 +1,39 @@ +#ifndef HEALTHDEVICE_H_ +#define HEALTHDEVICE_H_ + +#include +#include +#include +#include + +class HealthDevice: public SystemObject, + public ExecutableObjectIF, + public HasHealthIF { +public: + HealthDevice(object_id_t setObjectId, MessageQueueId_t parentQueue); + virtual ~HealthDevice(); + + ReturnValue_t performOperation(); + + ReturnValue_t initialize(); + + virtual MessageQueueId_t getCommandQueue() const; + + void setParentQueue(MessageQueueId_t parentQueue); + + bool hasHealthChanged(); + + virtual ReturnValue_t setHealth(HealthState health); + + virtual HealthState getHealth(); + +protected: + HealthState lastHealth; + + MessageQueueId_t parentQueue; + MessageQueue commandQueue; +public: + HealthHelper healthHelper; +}; + +#endif /* HEALTHDEVICE_H_ */ diff --git a/devicehandlers/Makefile b/devicehandlers/Makefile new file mode 100755 index 00000000..9a2c5bf4 --- /dev/null +++ b/devicehandlers/Makefile @@ -0,0 +1,28 @@ +#!/bin/bash +# +# OPUS makefile +# +# Created on: Mar 04, 2010 +# Author: ziemke +# Author: Claas Ziemke +# Copyright 2010, Claas Ziemke +# + +BASEDIR=../../ +include $(BASEDIR)options.mk + +OBJ = $(BUILDDIR)/ChannelResetHandler.o \ + $(BUILDDIR)/RawCommandHandler.o \ + $(BUILDDIR)/RawDataHandler.o \ + $(BUILDDIR)/OPUSPollingSequence.o \ + $(BUILDDIR)/PollingTask.o \ + $(BUILDDIR)/Device.o \ + $(BUILDDIR)/RmapCookie.o \ + +all: $(OBJ) + +$(BUILDDIR)/%.o: %.cpp %.h + $(CPP) $(CFLAGS) $(DEFINES) $(CCOPT) ${INCLUDE} -c $< -o $@ + +clean: + $(RM) *.o *.gcno *.gcda diff --git a/devicehandlers/PollingSequence.cpp b/devicehandlers/PollingSequence.cpp new file mode 100644 index 00000000..7493bd6a --- /dev/null +++ b/devicehandlers/PollingSequence.cpp @@ -0,0 +1,115 @@ +/** + * @file PollingSequence.cpp + * @brief This file defines the PollingSequence class. + * @date 19.12.2012 + * @author baetz + */ + +#include +#include + + +uint32_t PollingSequenceExecutableIF::pollingSequenceLengthMs = 0; +uint32_t PollingSequenceExecutableIF::payloadSequenceLengthMs = 0; + +PollingSequence::PollingSequence(object_id_t setObjectId, Interval_t setLength, ReturnValue_t (*initFunction)(PollingSequence *thisSequence)) : SystemObject( setObjectId ), + initFunction(initFunction){ + this->length = setLength; +// PollingSequenceExecutableIF::pollingSequenceLengthMs = (setLength * 1000) +// / OSAL::getTicksPerSecond(); + current = slotList.begin(); +} + +PollingSequence::~PollingSequence() { + std::list::iterator slotIt; + //Iterate through slotList and delete all entries. + slotIt = this->slotList.begin(); + while (slotIt != this->slotList.end()) { + delete (*slotIt); + slotIt++; + } +} + +void PollingSequence::addSlot(PollingSlot* setSlot) { + //The slot is added to the end of the list. + this->slotList.push_back(setSlot); + this->current = slotList.begin(); +} + +void PollingSequence::pollAndAdvance() { +// (*this->current)->print(); + (*this->current)->handler->performInPST( (*this->current)->opcode ); +// if (returnValue != RETURN_OK) { +// this->sendErrorMessage( returnValue ); +// } + //Increment the polling Sequence iterator + this->current++; + //Set it to the beginning, if the list's end is reached. + if (this->current == this->slotList.end()) { + this->current = this->slotList.begin(); + } +} + +Interval_t PollingSequence::getInterval() { + Interval_t oldTime; + std::list::iterator it; + it = this->current; + // Get the pollingTime of the current slot object. + oldTime = (*it)->pollingTime; + // Advance to the next object. + it++; + // Find the next interval which is not 0. + while ( it != slotList.end() ) { + if ( oldTime != (*it)->pollingTime ) { + return (*it)->pollingTime - oldTime; + } else { + it++; + } + } + // If the list end is reached (this is definitely an interval != 0), + // the interval is calculated by subtracting the remaining time of the PST + // and adding the start time of the first handler in the list. + it = slotList.begin(); + return this->length - oldTime + (*it)->pollingTime; +} + +bool PollingSequence::slotFollowsImmediately() { + Interval_t currentTime = (*current)->pollingTime; + std::list::iterator it; + it = this->current; + // Get the pollingTime of the current slot object. + if ( it == slotList.begin() ) return false; + it--; + if ( (*it)->pollingTime == currentTime ) { + return true; + } else { + return false; + } +} + +Interval_t PollingSequence::getLength() { + return this->length; +} + +void PollingSequence::print() { + debug << "Polling sequence contains:" << std::endl; + + //Iterate through slotList and start all PollingSlot::print() methods. + do { + (*current)->print(); + current++; + } while (current != slotList.end()); + current = slotList.begin(); +} + +//std::string PollingSequence::getObjectTypeAsString() { +// return "PollingSequence"; +//} + +ReturnValue_t PollingSequence::initialize() { + ReturnValue_t result = SystemObject::initialize(); + if (result != HasReturnvaluesIF::RETURN_OK){ + return result; + } + return initFunction(this); +} diff --git a/devicehandlers/PollingSequence.h b/devicehandlers/PollingSequence.h new file mode 100644 index 00000000..3f424c07 --- /dev/null +++ b/devicehandlers/PollingSequence.h @@ -0,0 +1,149 @@ +/** + * @file PollingSequence.h + * @brief This file defines the PollingSequence class. + * @date 19.12.2012 + * @author baetz + */ + +#ifndef POLLINGSEQUENCE_H_ +#define POLLINGSEQUENCE_H_ + +#include +#include +#include +#include +#include + + +/** + * \brief This class is the representation of a Polling Sequence Table in software. + * + * \details The PollingSequence object maintains the dynamic execution of device handler objects. + * The main idea is to create a list of device handlers, to announce all handlers to the + * polling sequence and to maintain a list of polling slot objects. This slot list represents the + * Polling Sequence Table in software. Each polling slot contains information to indicate when and + * which device handler shall be executed within a given polling period. + * The sequence is then executed by iterating through this slot list. + * Handlers are invoking by calling a certain function stored in the handler list. + */ +class PollingSequence : public SystemObject { + friend class PollingTask; + friend ReturnValue_t pollingSequenceInitFunction(PollingSequence *thisSequence); + friend ReturnValue_t payloadSequenceInitFunction(PollingSequence *thisSequence); +protected: + + /** + * \brief This list contains all OPUSPollingSlot objects, defining order and execution time of the + * device handler objects. + * + * \details The slot list is a std:list object that contains all created OPUSPollingSlot instances. + * They are NOT ordered automatically, so by adding entries, the correct order needs to be ensured. + * By iterating through this list the polling sequence is executed. Two entries with identical + * polling times are executed immediately one after another. + */ + std::list slotList; + + /** + * \brief An iterator that indicates the current polling slot to execute. + * + * \details This is an iterator for slotList and always points to the polling slot which is executed next. + */ + std::list::iterator current; + + /** + * \brief The period of the Polling Sequence Table in clock ticks. + * + * \details This attribute is set within the constructor, defining the main period length of the + * Polling Sequence Table. The length is expressed in clock ticks. + * + */ + Interval_t length; + + /** + * \brief The init function passed by the ctor + * + * \details This function will be called during initialize() + * + */ + ReturnValue_t (*initFunction)(PollingSequence *thisSequence); + +public: + + /** + * \brief The constructor of the PollingSequence object. + * + * \details The constructor takes two arguments, the period length and the init function. + * + * \param setLength The period length, expressed in clock ticks. + */ + PollingSequence( object_id_t setObjectId, Interval_t setLength, ReturnValue_t (*initFunction)(PollingSequence *thisSequence) ); + + /** + * \brief The destructor of the PollingSequence object. + * + * \details The destructor frees all allocated memory by iterating through + * handlerMap and slotList and deleting all allocated resources. + */ + virtual ~PollingSequence(); + + /** + * \brief This is a method to add an OPUSPollingSlot object to slotList. + * + * \details Here, a polling slot object is added to the slot list. It is appended + * to the end of the list. The list is currently NOT reordered. + * Afterwards, the iterator current is set to the beginning of the list. + * + * \param setSlot This is a pointer to a OPUSPollingSlot object. + */ + void addSlot( PollingSlot* setSlot ); + + /** + * Checks if the current slot shall be executed immediately after the one before. + * This allows to distinguish between grouped and not grouped handlers. + * @return - @c true if the slot has the same polling time as the previous + * - @c false else + */ + bool slotFollowsImmediately(); + + /** + * \brief This method returns the time until the next device handler object is invoked. + * + * \details This method is vitally important for the operation of the PST. By fetching the polling time + * of the current slot and that of the next one (or the first one, if the list end is reached) + * it calculates and returns the interval in clock ticks within which the handler execution + * shall take place. + */ + Interval_t getInterval(); + + /** + * \brief This method returns the length of this PollingSequence instance. + */ + Interval_t getLength(); + + /** + * \brief The method to execute the device handler entered in the current OPUSPollingSlot object. + * + * \details Within this method the device handler object to be executed is chosen by looking up the + * handler address of the current slot in the handlerMap. Either the device handler's + * talkToInterface or its listenToInterface method is invoked, depending on the isTalking flag + * of the polling slot. After execution the iterator current is increased or, by reaching the + * end of slotList, reset to the beginning. + */ + void pollAndAdvance(); + + /** + * \brief This is a method to print debug output. + * + * \details print is a simple debug method to print the whole polling sequence to the debug interface. + * It iterates through slotList and calls all print()} functions of the announced polling slot + * instances. + * + */ + void print(); + + ReturnValue_t initialize(); + //std::string getObjectTypeAsString(); +}; + + +#endif /* POLLINGSEQUENCE_H_ */ diff --git a/devicehandlers/PollingSequenceExecutableIF.h b/devicehandlers/PollingSequenceExecutableIF.h new file mode 100644 index 00000000..7f6db57c --- /dev/null +++ b/devicehandlers/PollingSequenceExecutableIF.h @@ -0,0 +1,30 @@ +/* + * PollingSequenceExecutableIF.h + * + * Created on: Mar 30, 2012 + * Author: baetz + */ + +#ifndef POLLINGSEQUENCEEXECUTABLE_H_ +#define POLLINGSEQUENCEEXECUTABLE_H_ + +//TODO clean this whole file up + +//TODO maybe define a type to make it look cleaner and use it in the PST +#define SEND_WRITE_CMD 0 +#define GET_WRITE_REPLY 1 +#define SEND_READ_CMD 2 +#define GET_READ_REPLY 3 + + +#include + +class PollingSequenceExecutableIF { +public: + static uint32_t pollingSequenceLengthMs; + static uint32_t payloadSequenceLengthMs; + virtual void performInPST( uint8_t ) = 0; + virtual ~PollingSequenceExecutableIF() { } +}; + +#endif /* POLLINGSEQUENCEEXECUTABLE_H_ */ diff --git a/devicehandlers/PollingSlot.cpp b/devicehandlers/PollingSlot.cpp new file mode 100644 index 00000000..1d219187 --- /dev/null +++ b/devicehandlers/PollingSlot.cpp @@ -0,0 +1,29 @@ +/** + * @file PollingSlot.cpp + * @brief This file defines the PollingSlot class. + * @date 19.12.2012 + * @author baetz + */ + +#include +#include +#include + +PollingSlot::PollingSlot( object_id_t handlerId, Interval_t setTime, int8_t setSequenceId ) { + //Set all attributes directly on object creation. + this->handler = objectManager->get( handlerId ); + this->pollingTime = setTime; + this->opcode = setSequenceId; +} + +PollingSlot::~PollingSlot() { +} + +void PollingSlot::print() const { + SystemObjectIF * systemObject = dynamic_cast(handler); + object_id_t id = (systemObject == NULL) ? 0xffffffff : systemObject->getObjectId(); + debug << "HandlerID: " << std::hex << id << std::dec << "; Polling Time: " + << this->pollingTime << "; Opcode: " << (uint16_t) this->opcode + << std::endl; +} + diff --git a/devicehandlers/PollingSlot.h b/devicehandlers/PollingSlot.h new file mode 100644 index 00000000..125e9222 --- /dev/null +++ b/devicehandlers/PollingSlot.h @@ -0,0 +1,61 @@ +/** + * @file PollingSlot.h + * @brief This file defines the PollingSlot class. + * @date 19.12.2012 + * @author baetz + */ + +#ifndef POLLINGSLOT_H_ +#define POLLINGSLOT_H_ + +#include +#include +#include + +class PollingSequence; + +/** + * \brief This class is the representation of a single polling sequence table entry. + * + * \details The PollingSlot class is the representation of a single polling sequence table entry. + * It contains three attributes and a debug method to print its content. + */ +class PollingSlot { + friend class PollingSequence; + friend class PollingTask; + friend ReturnValue_t pollingSequenceInitFunction(PollingSequence *thisSequence); + friend ReturnValue_t payloadSequenceInitFunction(PollingSequence *thisSequence); +protected: + /** + * \brief \c handler identifies which device handler object is executed in this slot. + */ + PollingSequenceExecutableIF* handler; + + /** + * \brief This attribute defines when a device handler object is executed. + * + * \details The pollingTime attribute identifies the time the handler is executed in clock ticks. It must be + * smaller than the period length of the polling sequence, what is ensured by automated calculation + * from a database. + */ + Interval_t pollingTime; + + /** + * \brief This value defines the type of device communication. + * + * \details The state of this value decides what communication routine is called in the PST executable or the device handler object. + */ + uint8_t opcode; + +public: + PollingSlot( object_id_t handlerId, Interval_t setTime, int8_t setSequenceId ); + virtual ~PollingSlot(); + + /** + * \brief This is a small debug method to print the PollingSlot's content to a debug interface. + */ + void print() const; +}; + + +#endif /* POLLINGSLOT_H_ */ diff --git a/devicehandlers/PollingTask.cpp b/devicehandlers/PollingTask.cpp new file mode 100644 index 00000000..e6142205 --- /dev/null +++ b/devicehandlers/PollingTask.cpp @@ -0,0 +1,103 @@ +/** + * \file OPUSPollingTask.cpp + * + * \brief This file contains the implementation for the OPUSPollingTask class. + * + * \author Bastian Baetz + * + * \date 17.03.2011 + * + */ + +#include +#include + +uint32_t PollingTask::deadlineMissedCount = 0; + +PollingTask::PollingTask( const char *name, TaskPriority_t setPriority, size_t setStack, void (*setDeadlineMissedFunc)(), object_id_t getPst ) : + TaskBase( setPriority, setStack, name ), periodId(0) { + // All additional attributes are applied to the object. + this->deadlineMissedFunc = setDeadlineMissedFunc; + this->pst = objectManager->get( getPst ); +} + +PollingTask::~PollingTask() { + // The PollingSequence destructor is called, if a sequence is announced. + if ( this->pst != NULL ) { + this->pst->~PollingSequence(); + } else { + // There was no memory allocation, so there's nothing to do. + } +} + +TaskReturn_t PollingTask::taskEntryPoint( TaskArgument_t argument ) { + + //The argument is re-interpreted as PollingTask. + PollingTask *originalTask( reinterpret_cast( argument ) ); + if ( originalTask->pst != NULL ) { + //The task's functionality is called. + originalTask->taskFunctionality(); + } else { + + } + debug << "Polling task " << originalTask->getId() << " returned from taskFunctionality." << std::endl; +} + +void PollingTask::missedDeadlineCounter() { + PollingTask::deadlineMissedCount++; + if (PollingTask::deadlineMissedCount%10 == 0) { + error << "PST missed " << PollingTask::deadlineMissedCount << " deadlines." << std::endl; + } +} + +ReturnValue_t PollingTask::startTask() { + this->setRunning( true ); + ReturnValue_t status; + status = OSAL::startTask( &( this->id ), PollingTask::taskEntryPoint, TaskArgument_t( ( void *)this ) ); + if( status != RETURN_OK ) { + //TODO: Call any FDIR routine? + error << "PollingTask::startTask for " << std::hex << this->getId() << std::dec << " failed." << std::endl; + } else { +// debug << "PollingTask::startTask for " << std::hex << this->getId() << std::dec << " successful" << std::endl; + } + return status; +} + +void PollingTask::taskFunctionality() { + ReturnValue_t status = OSAL::TIMEOUT; + Interval_t interval; + // A local iterator for the Polling Sequence Table is created to find the start time for the first entry. + std::list::iterator it; + + it = this->pst->current; + //The start time for the first entry is read. + interval = ( *it )->pollingTime; + //The period is set up and started with the system call. + status = OSAL::setAndStartPeriod( interval, &( this->periodId ) ); + if( status == RETURN_OK ) { + //The task's "infinite" inner loop is entered. + while( this->isRunning ) { + if ( pst->slotFollowsImmediately() ) { + //Do nothing + } else { + //The interval for the next polling slot is selected. + interval = this->pst->getInterval(); + //The period is checked and restarted with the new interval. + //If the deadline was missed, the deadlineMissedFunc is called. + status = OSAL::checkAndRestartPeriod( this->periodId, interval ); + if( status == OSAL::TIMEOUT ) { + if( this->deadlineMissedFunc != NULL ) { + this->deadlineMissedFunc(); + } + } + } + //The device handler for this slot is executed and the next one is chosen. + this->pst->pollAndAdvance(); + } + } else { + error << "PollingTask::setAndStartPeriod failed with status "<< status << std::endl; + } + //Any operating system object for periodic execution is deleted. + debug << "Deleting the PollingTask's period." << std::endl; + OSAL::deletePeriod( &(this->id) ); +} diff --git a/devicehandlers/PollingTask.h b/devicehandlers/PollingTask.h new file mode 100644 index 00000000..55d71d86 --- /dev/null +++ b/devicehandlers/PollingTask.h @@ -0,0 +1,115 @@ +/** + * @file PollingTask.h + * + * @brief This file contains the definition for the PollingTask class. + * + * @author Claas Ziemke, Bastian Baetz + * + * @date 17.03.2011 + * + * Copyright 2009,2010, Claas Ziemke + * All rights reserved + * + */ +#ifndef POLLINGTASK_H_ +#define POLLINGTASK_H_ + +#include +#include + +/** + * + * @brief This class represents a specialized thread to execute polling sequences. + * + * @image latex seq_PST_dynamic.eps "Sequence diagram of polling sequence operation" width=1@textwidth + * @image html seq_PST_dynamic.png "Sequence diagram of polling sequence operation" + * + * @details The Polling Sequence Table is executed in a task of special type, called PollingTask. + * After creation the polling task initializes the PST and starts taskFunctionality. + * An infinite loop is entered within which the iteration through the PST is done by repetitive calls of + * getInterval() and pollAndAdvance(). + * @ingroup task_handling + */ +class PollingTask: public TaskBase { +protected: + /** + * @brief id of the associated OS period + */ + PeriodId_t periodId; + + /** + * @brief This attribute is the pointer to the complete polling sequence table object. + * + * @details The most important attribute of the thread object. + * It holds a pointer to the complete polling sequence table object. + */ + PollingSequence* pst; + + /** + * @brief This attribute holds a function pointer that is executed when a deadline was missed. + * + * @details Another function may be announced to determine the actions to perform when a deadline was missed. + * Currently, only one function for missing any deadline is allowed. + * If not used, it shall be declared NULL. + */ + void ( *deadlineMissedFunc )( void ); + /** + * @brief This is the entry point in a new polling thread. + * + * @details This method, that is the general entry point in the new thread, is here set to generate + * and link the Polling Sequence Table to the thread object and start taskFunctionality() + * on success. If operation of the task is ended for some reason, + * the destructor is called to free allocated memory. + */ + static TaskReturn_t taskEntryPoint( TaskArgument_t argument ); + + /** + * @brief This function holds the main functionality of the thread. + * + * + * @details Holding the main functionality of the task, this method is most important. + * It links the functionalities provided by PollingSequence with the OS's System Calls + * to keep the timing of the periods. + */ + void taskFunctionality( void ); + +public: + /** + * @brief The standard constructor of the class. + * + * @details This is the general constructor of the class. In addition to the TaskBase parameters, + * the following variables are passed: + * + * @param (*setDeadlineMissedFunc)() The function pointer to the deadline missed function that shall be assigned. + * + * @param getPst The object id of the completely initialized polling sequence. + */ + PollingTask( const char *name, TaskPriority_t setPriority, size_t setStack, void (*setDeadlineMissedFunc)(), object_id_t getPst ); + + /** + * @brief The destructor of the class. + * + * @details The destructor frees all heap memory that was allocated on thread initialization for the PST and + * the device handlers. This is done by calling the PST's destructor. + */ + virtual ~PollingTask( void ); + + /** + * @brief The function to actually start a new task. + * + * @details As described in TaskBase this method invokes the operating systems method to start a new task. + * Entry point is taskEntryPoint(). + */ + ReturnValue_t startTask( void ); + /** + * This static function can be used as #deadlineMissedFunc. + * It counts missedDeadlines and prints the number of missed deadlines every 10th time. + */ + static void missedDeadlineCounter(); + /** + * A helper variable to count missed deadlines. + */ + static uint32_t deadlineMissedCount; +}; + +#endif /* POLLINGTASK_H_ */ diff --git a/events/Event.cpp b/events/Event.cpp new file mode 100644 index 00000000..55b0a0cd --- /dev/null +++ b/events/Event.cpp @@ -0,0 +1,24 @@ +/* + * Event.cpp + * + * Created on: 25.08.2015 + * Author: baetz + */ + + + + +#include +namespace EVENT { +EventId_t getEventId(Event event) { + return (event & 0xFFFF); +} + +EventSeverity_t getSeverity(Event event) { + return ((event >> 16) & 0xFF); +} + +Event makeEvent(EventId_t eventId, EventSeverity_t eventSeverity) { + return (eventSeverity << 16) + (eventId & 0xFFFF); +} +} diff --git a/events/Event.h b/events/Event.h new file mode 100644 index 00000000..22ef921e --- /dev/null +++ b/events/Event.h @@ -0,0 +1,49 @@ +/* + * Event.h + * + * Created on: 18.08.2015 + * Author: baetz + */ + +#ifndef EVENTOBJECT_EVENT_H_ +#define EVENTOBJECT_EVENT_H_ + +#include +#include + +typedef uint16_t EventId_t; +typedef uint8_t EventSeverity_t; + +#define MAKE_EVENT(id, severity) (((severity)<<16)+(SUBSYSTEM_ID*100)+(id)) + +typedef uint32_t Event; + +namespace EVENT { +EventId_t getEventId(Event event); + +EventSeverity_t getSeverity(Event event); + +Event makeEvent(EventId_t eventId, EventSeverity_t eventSeverity); + +} +namespace SEVERITY { + static const EventSeverity_t INFO = 1; + static const EventSeverity_t LOW = 2; + static const EventSeverity_t MEDIUM = 3; + static const EventSeverity_t HIGH = 4; +} + +//Unfortunately, this does not work nicely because of the inability to define static classes in headers. +//struct Event { +// Event(uint8_t domain, uint8_t counter, EventSeverity_t severity) : +// id(domain*100+counter), severity(severity) { +// } +// EventId_t id; +// EventSeverity_t severity; +// static const EventSeverity_t INFO = 1; +// static const EventSeverity_t LOW = 2; +// static const EventSeverity_t MEDIUM = 3; +// static const EventSeverity_t HIGH = 4; +//}; + +#endif /* EVENTOBJECT_EVENT_H_ */ diff --git a/events/EventManager.cpp b/events/EventManager.cpp new file mode 100644 index 00000000..c177ad64 --- /dev/null +++ b/events/EventManager.cpp @@ -0,0 +1,145 @@ +#include +#include + +#include +#include +#include + +const uint16_t EventManager::POOL_SIZES[N_POOLS] = { + sizeof(EventMatchTree::Node), sizeof(EventIdRangeMatcher), + sizeof(ReporterRangeMatcher) }; +//TODO: Rather arbitrary. Adjust! +const uint16_t EventManager::N_ELEMENTS[N_POOLS] = { 240, 120, 120 }; + +EventManager::EventManager(object_id_t setObjectId) : + SystemObject(setObjectId), eventReportQueue(MAX_EVENTS_PER_CYCLE, + EventMessage::EVENT_MESSAGE_SIZE), mutex(NULL), factoryBackend( + 0, POOL_SIZES, N_ELEMENTS, false, true) { + mutex = new MutexId_t; + OSAL::createMutex(setObjectId + 1, (mutex)); +} + +EventManager::~EventManager() { + OSAL::deleteMutex(mutex); + delete mutex; +} + +MessageQueueId_t EventManager::getEventReportQueue() { + return eventReportQueue.getId(); +} + +ReturnValue_t EventManager::performOperation() { + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + while (result == HasReturnvaluesIF::RETURN_OK) { + EventMessage message; + result = eventReportQueue.receiveMessage(&message); + if (result == HasReturnvaluesIF::RETURN_OK) { + printEvent(&message); + notifyListeners(&message); + } + } + return HasReturnvaluesIF::RETURN_OK; +} + +void EventManager::notifyListeners(EventMessage* message) { + lockMutex(); + for (auto iter = listenerList.begin(); iter != listenerList.end(); ++iter) { + if (iter->second.match(message)) { + eventForwardingSender.sendMessage(iter->first, message, + message->getSender()); + } + } + unlockMutex(); +} + +ReturnValue_t EventManager::registerListener(MessageQueueId_t listener, +bool forwardAllButSelected) { + auto result = listenerList.insert( + std::pair(listener, + EventMatchTree(&factoryBackend, forwardAllButSelected))); + if (!result.second) { + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t EventManager::subscribeToEvent(MessageQueueId_t listener, + EventId_t event) { + return subscribeToEventRange(listener, event); +} + +ReturnValue_t EventManager::subscribeToAllEventsFrom(MessageQueueId_t listener, + object_id_t object) { + return subscribeToEventRange(listener, 0, 0, true, object); +} + +ReturnValue_t EventManager::subscribeToEventRange(MessageQueueId_t listener, + EventId_t idFrom, EventId_t idTo, bool idInverted, + object_id_t reporterFrom, object_id_t reporterTo, + bool reporterInverted) { + auto iter = listenerList.find(listener); + if (iter == listenerList.end()) { + return LISTENER_NOT_FOUND; + } + lockMutex(); + ReturnValue_t result = iter->second.addMatch(idFrom, idTo, idInverted, + reporterFrom, reporterTo, reporterInverted); + unlockMutex(); + return result; +} + +ReturnValue_t EventManager::unsubscribeFromEventRange(MessageQueueId_t listener, + EventId_t idFrom, EventId_t idTo, bool idInverted, + object_id_t reporterFrom, object_id_t reporterTo, + bool reporterInverted) { + auto iter = listenerList.find(listener); + if (iter == listenerList.end()) { + return LISTENER_NOT_FOUND; + } + lockMutex(); + ReturnValue_t result = iter->second.removeMatch(idFrom, idTo, idInverted, + reporterFrom, reporterTo, reporterInverted); + unlockMutex(); + return result; +} + +void EventManager::printEvent(EventMessage* message) { + const char *string = 0; + switch (message->getSeverity()) { + case SEVERITY::INFO: + string = translateObject(message->getReporter()); + info << "EVENT: "; + if (string != 0) { + info << string; + } else { + info << "0x" << std::hex << message->getReporter() << std::dec; + } + info << " reported " << translateEvents(message->getEvent()) << " (" + << std::dec << message->getEventId() << std::hex << ") P1: 0x" + << message->getParameter1() << " P2: 0x" + << message->getParameter2() << std::dec << std::endl; + break; + default: + string = translateObject(message->getReporter()); + error << "EVENT: "; + if (string != 0) { + error << string; + } else { + error << "0x" << std::hex << message->getReporter() << std::dec; + } + error << " reported " << translateEvents(message->getEvent()) << " (" + << std::dec << message->getEventId() << std::hex << ") P1: 0x" + << message->getParameter1() << " P2: 0x" + << message->getParameter2() << std::dec << std::endl; + break; + } + +} + +void EventManager::lockMutex() { + OSAL::lockMutex(this->mutex, OSAL::NO_TIMEOUT); +} + +void EventManager::unlockMutex() { + OSAL::unlockMutex(this->mutex); +} diff --git a/events/EventManager.h b/events/EventManager.h new file mode 100644 index 00000000..185e15ea --- /dev/null +++ b/events/EventManager.h @@ -0,0 +1,61 @@ +#ifndef EVENTMANAGER_H_ +#define EVENTMANAGER_H_ + +#include +#include +#include +#include +#include +#include +#include + +class EventManager: public EventManagerIF, + public ExecutableObjectIF, + public SystemObject { +public: + static const uint16_t MAX_EVENTS_PER_CYCLE = 150; + + EventManager(object_id_t setObjectId); + virtual ~EventManager(); + + MessageQueueId_t getEventReportQueue(); + + ReturnValue_t registerListener(MessageQueueId_t listener, bool forwardAllButSelected = false); + ReturnValue_t subscribeToEvent(MessageQueueId_t listener, EventId_t event); + ReturnValue_t subscribeToAllEventsFrom(MessageQueueId_t listener, + object_id_t object); + ReturnValue_t subscribeToEventRange(MessageQueueId_t listener, + EventId_t idFrom = 0, EventId_t idTo = 0, bool idInverted = false, + object_id_t reporterFrom = 0, object_id_t reporterTo = 0, + bool reporterInverted = false); + ReturnValue_t unsubscribeFromEventRange(MessageQueueId_t listener, + EventId_t idFrom = 0, EventId_t idTo = 0, bool idInverted = false, + object_id_t reporterFrom = 0, object_id_t reporterTo = 0, + bool reporterInverted = false); + ReturnValue_t performOperation(); + +protected: + + MessageQueue eventReportQueue; + + MessageQueueSender eventForwardingSender; + + std::map listenerList; + + MutexId_t* mutex; + + static const uint8_t N_POOLS = 3; + LocalPool factoryBackend; + static const uint16_t POOL_SIZES[N_POOLS]; + static const uint16_t N_ELEMENTS[N_POOLS]; + + void notifyListeners(EventMessage *message); + + void printEvent(EventMessage *message); + + void lockMutex(); + + void unlockMutex(); +}; + +#endif /* EVENTMANAGER_H_ */ diff --git a/events/EventManagerIF.h b/events/EventManagerIF.h new file mode 100644 index 00000000..ac6fb0cb --- /dev/null +++ b/events/EventManagerIF.h @@ -0,0 +1,53 @@ +#ifndef EVENTMANAGERIF_H_ +#define EVENTMANAGERIF_H_ + +#include +#include +#include +#include + +class EventManagerIF { +public: + + static const uint8_t INTERFACE_ID = EVENT_MANAGER_IF; + static const ReturnValue_t LISTENER_NOT_FOUND = MAKE_RETURN_CODE(1); + virtual ~EventManagerIF() { + } + + virtual MessageQueueId_t getEventReportQueue() = 0; + + virtual ReturnValue_t registerListener(MessageQueueId_t listener, bool forwardAllButSelected = false) = 0; + virtual ReturnValue_t subscribeToEvent(MessageQueueId_t listener, + EventId_t event) = 0; + virtual ReturnValue_t subscribeToAllEventsFrom(MessageQueueId_t listener, + object_id_t object) = 0; + virtual ReturnValue_t subscribeToEventRange(MessageQueueId_t listener, + EventId_t idFrom = 0, EventId_t idTo = 0, bool idInverted = false, + object_id_t reporterFrom = 0, object_id_t reporterTo = 0, + bool reporterInverted = false) = 0; + virtual ReturnValue_t unsubscribeFromEventRange(MessageQueueId_t listener, + EventId_t idFrom = 0, EventId_t idTo = 0, bool idInverted = false, + object_id_t reporterFrom = 0, object_id_t reporterTo = 0, + bool reporterInverted = false) = 0; + + static void triggerEvent(object_id_t reportingObject, Event event, + uint32_t parameter1 = 0, uint32_t parameter2 = 0, MessageQueueId_t sentFrom = 0) { + EventMessage message(event, reportingObject, parameter1, parameter2); + triggerEvent(&message, sentFrom); + } + static void triggerEvent(EventMessage* message, MessageQueueId_t sentFrom = 0) { + static MessageQueueId_t eventmanagerQueue = 0; + if (eventmanagerQueue == 0) { + EventManagerIF *eventmanager = objectManager->get( + objects::EVENT_MANAGER); + if (eventmanager != NULL) { + eventmanagerQueue = eventmanager->getEventReportQueue(); + } + } + MessageQueueSender sender(eventmanagerQueue); + sender.sendToDefault(message, sentFrom); + } + +}; + +#endif /* EVENTMANAGERIF_H_ */ diff --git a/events/EventMessage.cpp b/events/EventMessage.cpp new file mode 100644 index 00000000..854c000e --- /dev/null +++ b/events/EventMessage.cpp @@ -0,0 +1,114 @@ +#include +#include + +EventMessage::EventMessage() { + messageSize = EVENT_MESSAGE_SIZE; + clearEventMessage(); +} + +EventMessage::EventMessage(Event event, object_id_t reporter, + uint32_t parameter1, uint32_t parameter2) { + messageSize = EVENT_MESSAGE_SIZE; + setMessageId(EVENT_MESSAGE); + setEvent(event); + setReporter(reporter); + setParameter1(parameter1); + setParameter2(parameter2); +} + +EventMessage::~EventMessage() { +} + +Event EventMessage::getEvent() { + Event event; + memcpy(&event, getData(), sizeof(Event)); + return (event & 0xFFFFFF); +} + +void EventMessage::setEvent(Event event) { + Event tempEvent; + memcpy(&tempEvent, getData(), sizeof(Event)); + tempEvent = (tempEvent & 0xFF000000) + (event & 0xFFFFFF); + memcpy(getData(), &tempEvent, sizeof(Event)); +} + +uint8_t EventMessage::getMessageId() { + Event event; + memcpy(&event, getData(), sizeof(Event)); + return (event & 0xFF000000) >> 24; +} + +void EventMessage::setMessageId(uint8_t id) { + Event event; + memcpy(&event, getData(), sizeof(Event)); + event = (event & 0x00FFFFFF) + (id << 24); + memcpy(getData(), &event, sizeof(Event)); +} + +EventSeverity_t EventMessage::getSeverity() { + Event event; + memcpy(&event, getData(), sizeof(Event)); + return EVENT::getSeverity(event); +} + +void EventMessage::setSeverety(EventSeverity_t severity) { + Event event; + memcpy(&event, getData(), sizeof(Event)); + event = (event & 0xFF00FFFF) + (severity << 16); + memcpy(getData(), &event, sizeof(Event)); +} + +EventId_t EventMessage::getEventId() { + Event event; + memcpy(&event, getData(), sizeof(Event)); + return EVENT::getEventId(event); +} + +void EventMessage::setEventId(EventId_t eventId) { + Event event; + memcpy(&event, getData(), sizeof(Event)); + event = (event & 0xFFFF0000) + eventId; + memcpy(getData(), &event, sizeof(Event)); +} + +object_id_t EventMessage::getReporter() { + object_id_t parameter; + memcpy(¶meter, getData() + sizeof(Event), sizeof(object_id_t)); + return parameter; +} + +void EventMessage::setReporter(object_id_t reporter) { + memcpy(getData() + sizeof(Event), &reporter, sizeof(object_id_t)); +} + +uint32_t EventMessage::getParameter1() { + uint32_t parameter; + memcpy(¶meter, getData() + sizeof(Event) + sizeof(object_id_t), 4); + return parameter; +} + +void EventMessage::setParameter1(uint32_t parameter) { + memcpy(getData() + sizeof(Event) + sizeof(object_id_t), ¶meter, 4); +} + +uint32_t EventMessage::getParameter2() { + uint32_t parameter; + memcpy(¶meter, getData() + sizeof(Event) + sizeof(object_id_t) + 4, 4); + return parameter; +} + +void EventMessage::setParameter2(uint32_t parameter) { + memcpy(getData() + sizeof(Event) + sizeof(object_id_t) + 4, ¶meter, 4); +} + +void EventMessage::clearEventMessage() { + setEvent(INVALID_EVENT); +} + +bool EventMessage::isClearedEventMessage() { + return getEvent() == INVALID_EVENT; +} + +size_t EventMessage::getMinimumMessageSize() { + return EVENT_MESSAGE_SIZE; +} diff --git a/events/EventMessage.h b/events/EventMessage.h new file mode 100644 index 00000000..ffe3ff64 --- /dev/null +++ b/events/EventMessage.h @@ -0,0 +1,52 @@ +#ifndef EVENTMESSAGE_H_ +#define EVENTMESSAGE_H_ + +#include +#include +#include + +/** + * Passing on events through IPC. + * Event Id, severity and message type retrieval is a bit difficult. + */ +class EventMessage: public MessageQueueMessage { +public: + static const uint8_t EVENT_MESSAGE = 0; //!< Normal event + static const uint8_t CONFIRMATION_REQUEST = 1; //!< Request to parent if event is caused by child or not. + static const uint8_t YOUR_FAULT = 2; //!< The fault was caused by child, parent believes it's ok. + static const uint8_t MY_FAULT = 3; //!< The fault was caused by the parent, child is ok. + //TODO: Add other messageIDs here if necessary. + static const uint8_t EVENT_MESSAGE_SIZE = HEADER_SIZE + sizeof(Event) + + 3 * sizeof(uint32_t); + + EventMessage(); +// EventMessage(EventId_t event, EventSeverity_t severity, +// object_id_t reporter, uint32_t parameter1, uint32_t parameter2 = 0); + EventMessage(Event event, object_id_t reporter, uint32_t parameter1, + uint32_t parameter2 = 0); + virtual ~EventMessage(); + Event getEvent(); + void setEvent(Event event); + uint8_t getMessageId(); + void setMessageId(uint8_t id); + EventSeverity_t getSeverity(); + void setSeverety(EventSeverity_t severity); + EventId_t getEventId(); + void setEventId(EventId_t event); + object_id_t getReporter(); + void setReporter(object_id_t reporter); + uint32_t getParameter1(); + void setParameter1(uint32_t parameter); + uint32_t getParameter2(); + void setParameter2(uint32_t parameter); + + void clearEventMessage(); + bool isClearedEventMessage(); + +protected: + static const Event INVALID_EVENT = 0; + virtual size_t getMinimumMessageSize(); + +}; + +#endif /* EVENTMESSAGE_H_ */ diff --git a/events/EventReportingProxyIF.h b/events/EventReportingProxyIF.h new file mode 100644 index 00000000..9ab783ab --- /dev/null +++ b/events/EventReportingProxyIF.h @@ -0,0 +1,26 @@ +/* + * EventReportingProxyIF.h + * + * Created on: 19.08.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_EVENTS_EVENTREPORTINGPROXYIF_H_ +#define FRAMEWORK_EVENTS_EVENTREPORTINGPROXYIF_H_ + +#include + + +class EventReportingProxyIF { +public: + virtual ~EventReportingProxyIF() { + } + + virtual void forwardEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0) const = 0; + +}; + + + + +#endif /* FRAMEWORK_EVENTS_EVENTREPORTINGPROXYIF_H_ */ diff --git a/events/eventmatching/EventIdRangeMatcher.cpp b/events/eventmatching/EventIdRangeMatcher.cpp new file mode 100644 index 00000000..9a9a802f --- /dev/null +++ b/events/eventmatching/EventIdRangeMatcher.cpp @@ -0,0 +1,19 @@ +/* + * EventIdRangeMatcher.cpp + * + * Created on: 27.08.2015 + * Author: baetz + */ + +#include + +EventIdRangeMatcher::EventIdRangeMatcher(EventId_t lower, EventId_t upper, + bool inverted) : EventRangeMatcherBase(lower, upper, inverted) { +} + +EventIdRangeMatcher::~EventIdRangeMatcher() { +} + +bool EventIdRangeMatcher::match(EventMessage* message) { + return rangeMatcher.match(message->getEventId()); +} diff --git a/events/eventmatching/EventIdRangeMatcher.h b/events/eventmatching/EventIdRangeMatcher.h new file mode 100644 index 00000000..ad93de79 --- /dev/null +++ b/events/eventmatching/EventIdRangeMatcher.h @@ -0,0 +1,20 @@ +/* + * EventIdRangeMatcher.h + * + * Created on: 27.08.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_EVENTS_EVENTMATCHING_EVENTIDRANGEMATCHER_H_ +#define FRAMEWORK_EVENTS_EVENTMATCHING_EVENTIDRANGEMATCHER_H_ + +#include + +class EventIdRangeMatcher: public EventRangeMatcherBase { +public: + EventIdRangeMatcher(EventId_t lower, EventId_t upper, bool inverted); + ~EventIdRangeMatcher(); + bool match(EventMessage* message); +}; + +#endif /* FRAMEWORK_EVENTS_EVENTMATCHING_EVENTIDRANGEMATCHER_H_ */ diff --git a/events/eventmatching/EventMatchTree.cpp b/events/eventmatching/EventMatchTree.cpp new file mode 100644 index 00000000..5360aeca --- /dev/null +++ b/events/eventmatching/EventMatchTree.cpp @@ -0,0 +1,151 @@ +/* + * EventMatchTree.cpp + * + * Created on: 27.08.2015 + * Author: baetz + */ + +#include +#include +#include +#include + +EventMatchTree::EventMatchTree(StorageManagerIF* storageBackend, bool invertedMatch) : + MatchTree(end(), 1), factory(storageBackend), invertedMatch(invertedMatch) { +} + +EventMatchTree::~EventMatchTree() { +} + +bool EventMatchTree::match(EventMessage* number) { + if (invertedMatch) { + return !MatchTree::match(number); + } else { + return MatchTree::match(number); + } +} + +ReturnValue_t EventMatchTree::addMatch(EventId_t idFrom, + EventId_t idTo, bool idInverted, object_id_t reporterFrom, + object_id_t reporterTo, bool reporterInverted) { + if (idFrom == 0) { + //Assuming all events shall be forwarded. + idTo = 0; + idInverted = true; + } + if (idTo == 0) { + idTo = idFrom; + } + iterator lastTest; + ReturnValue_t result = findOrInsertRangeMatcher( + begin(), idFrom, idTo, idInverted, &lastTest); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if (reporterFrom == 0) { + //No need to add another AND branch + return RETURN_OK; + } + if (reporterTo == 0) { + reporterTo = reporterFrom; + } + return findOrInsertRangeMatcher( + lastTest.left(), reporterFrom, reporterTo, reporterInverted, + &lastTest); +} + +ReturnValue_t EventMatchTree::removeMatch(EventId_t idFrom, + EventId_t idTo, bool idInverted, object_id_t reporterFrom, + object_id_t reporterTo, bool reporterInverted) { + + iterator foundElement; + + if (idFrom == 0) { + //Assuming a "forward all events" request. + idTo = 0; + idInverted = true; + } + if (idTo == 0) { + idTo = idFrom; + } + foundElement = findRangeMatcher( + begin(), idFrom, idTo, idInverted); + if (foundElement == end()) { + return NO_MATCH; //Can't tell if too detailed or just not found. + } + if (reporterFrom == 0) { + if (foundElement.left() == end()) { + return removeElementAndReconnectChildren(foundElement); + } else { + return TOO_GENERAL_REQUEST; + } + } + if (reporterTo == 0) { + reporterTo = reporterFrom; + } + foundElement = findRangeMatcher( + foundElement.left(), reporterFrom, reporterTo, reporterInverted); + if (foundElement == end()) { + return NO_MATCH; + } else { + return removeElementAndReconnectChildren(foundElement); + } +} + +template +inline ReturnValue_t EventMatchTree::findOrInsertRangeMatcher(iterator start, + VALUE_T idFrom, VALUE_T idTo, bool inverted, iterator* lastTest) { + bool attachToBranch = AND; + iterator iter = start; + while (iter != end()) { + INSERTION_T* matcher = static_cast(*iter); + attachToBranch = OR; + *lastTest = iter; + if ((matcher->rangeMatcher.lowerBound == idFrom) + && (matcher->rangeMatcher.upperBound == idTo) + && (matcher->rangeMatcher.inverted == inverted)) { + return RETURN_OK; + } else { + iter = iter.right(); + } + } + //Only reached if nothing was found. + SerializeableMatcherIF* newContent = factory.generate< + INSERTION_T>(idFrom, idTo, inverted); + if (newContent == NULL) { + return FULL; + } + Node* newNode = factory.generate(newContent); + if (newNode == NULL) { + return FULL; + } + *lastTest = insert(attachToBranch, *lastTest, newNode); + if (*lastTest == end()) { + //This actaully never fails, so creating a dedicated returncode seems an overshoot. + return RETURN_FAILED; + } + return RETURN_OK; +} + +template +EventMatchTree::iterator EventMatchTree::findRangeMatcher(iterator start, + VALUE_T idFrom, VALUE_T idTo, bool inverted) { + iterator iter = start; + while (iter != end()) { + INSERTION_T* matcher = static_cast(*iter); + if ((matcher->rangeMatcher.lowerBound == idFrom) + && (matcher->rangeMatcher.upperBound == idTo) + && (matcher->rangeMatcher.inverted == inverted)) { + break; + } else { + iter = iter.right(); //next OR element + } + } + return iter; +} + +ReturnValue_t EventMatchTree::cleanUpElement(iterator position) { + //TODO: What if first deletion fails? + factory.destroy(position.element->value); + return factory.destroy(position.element); +} diff --git a/events/eventmatching/EventMatchTree.h b/events/eventmatching/EventMatchTree.h new file mode 100644 index 00000000..696fa505 --- /dev/null +++ b/events/eventmatching/EventMatchTree.h @@ -0,0 +1,43 @@ +/* + * EventMatchTree.h + * + * Created on: 27.08.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_EVENTS_EVENTMATCHING_EVENTMATCHTREE_H_ +#define FRAMEWORK_EVENTS_EVENTMATCHING_EVENTMATCHTREE_H_ + +#include +#include +#include +#include +class StorageManagerIF; + +class EventMatchTree: public MatchTree, public HasReturnvaluesIF { +public: + EventMatchTree(StorageManagerIF* storageBackend, bool invertedMatch); + virtual ~EventMatchTree(); + ReturnValue_t addMatch(EventId_t idFrom = 0, EventId_t idTo = 0, bool idInverted = false, + object_id_t reporterFrom = 0, object_id_t reporterTo = 0, + bool reporterInverted = false); + ReturnValue_t removeMatch(EventId_t idFrom = 0, EventId_t idTo = 0, bool idInverted = false, + object_id_t reporterFrom = 0, object_id_t reporterTo = 0, + bool reporterInverted = false); + bool match(EventMessage* number); +protected: + ReturnValue_t cleanUpElement(iterator position); +private: + PlacementFactory factory; + bool invertedMatch; + template + ReturnValue_t findOrInsertRangeMatcher(iterator start, VALUE_T idFrom, + VALUE_T idTo, bool inverted, iterator* lastTest); + + template + iterator findRangeMatcher(iterator start, VALUE_T idFrom, VALUE_T idTo, + bool inverted); + +}; + +#endif /* FRAMEWORK_EVENTS_EVENTMATCHING_EVENTMATCHTREE_H_ */ diff --git a/events/eventmatching/EventRangeMatcherBase.h b/events/eventmatching/EventRangeMatcherBase.h new file mode 100644 index 00000000..5a348ebd --- /dev/null +++ b/events/eventmatching/EventRangeMatcherBase.h @@ -0,0 +1,36 @@ +/* + * EventRangeMatcherBase.h + * + * Created on: 27.08.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_EVENTS_EVENTMATCHING_EVENTRANGEMATCHERBASE_H_ +#define FRAMEWORK_EVENTS_EVENTMATCHING_EVENTRANGEMATCHERBASE_H_ + +#include +#include +#include + +template +class EventRangeMatcherBase: public SerializeableMatcherIF { + friend class EventMatchTree; +public: + EventRangeMatcherBase(T from, T till, bool inverted) : rangeMatcher(from, till, inverted) { } + virtual ~EventRangeMatcherBase() { } + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + return rangeMatcher.serialize(buffer, size, max_size, bigEndian); + } + uint32_t getSerializedSize() const { + return rangeMatcher.getSerializedSize(); + } + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return rangeMatcher.deSerialize(buffer, size, bigEndian); + } +protected: + RangeMatcher rangeMatcher; +}; + +#endif /* FRAMEWORK_EVENTS_EVENTMATCHING_EVENTRANGEMATCHERBASE_H_ */ diff --git a/events/eventmatching/ReporterRangeMatcher.cpp b/events/eventmatching/ReporterRangeMatcher.cpp new file mode 100644 index 00000000..b31b5cc3 --- /dev/null +++ b/events/eventmatching/ReporterRangeMatcher.cpp @@ -0,0 +1,20 @@ +/* + * ReporterRangeMatcher.cpp + * + * Created on: 27.08.2015 + * Author: baetz + */ + +#include + +ReporterRangeMatcher::ReporterRangeMatcher(object_id_t lower, object_id_t upper, + bool inverted) : EventRangeMatcherBase(lower, upper, inverted) { +} + +ReporterRangeMatcher::~ReporterRangeMatcher() { + +} + +bool ReporterRangeMatcher::match(EventMessage* message) { + return rangeMatcher.match(message->getReporter()); +} diff --git a/events/eventmatching/ReporterRangeMatcher.h b/events/eventmatching/ReporterRangeMatcher.h new file mode 100644 index 00000000..d2c3fcf0 --- /dev/null +++ b/events/eventmatching/ReporterRangeMatcher.h @@ -0,0 +1,20 @@ +/* + * ReporterRangeMatcher.h + * + * Created on: 27.08.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_EVENTS_EVENTMATCHING_REPORTERRANGEMATCHER_H_ +#define FRAMEWORK_EVENTS_EVENTMATCHING_REPORTERRANGEMATCHER_H_ + +#include + +class ReporterRangeMatcher: public EventRangeMatcherBase { +public: + ReporterRangeMatcher(object_id_t lower, object_id_t upper, bool inverted); + ~ReporterRangeMatcher(); + bool match(EventMessage* message); +}; + +#endif /* FRAMEWORK_EVENTS_EVENTMATCHING_REPORTERRANGEMATCHER_H_ */ diff --git a/events/eventmatching/SeverityRangeMatcher.cpp b/events/eventmatching/SeverityRangeMatcher.cpp new file mode 100644 index 00000000..a4d9c3e8 --- /dev/null +++ b/events/eventmatching/SeverityRangeMatcher.cpp @@ -0,0 +1,21 @@ +/* + * SeverityRangeMatcher.cpp + * + * Created on: 27.08.2015 + * Author: baetz + */ + +#include +#include +#include + +SeverityRangeMatcher::SeverityRangeMatcher(EventSeverity_t from, + EventSeverity_t till, bool inverted) : EventRangeMatcherBase(from, till, inverted) { +} + +SeverityRangeMatcher::~SeverityRangeMatcher() { +} + +bool SeverityRangeMatcher::match(EventMessage* message) { + return rangeMatcher.match(message->getSeverity()); +} diff --git a/events/eventmatching/SeverityRangeMatcher.h b/events/eventmatching/SeverityRangeMatcher.h new file mode 100644 index 00000000..213fbb1e --- /dev/null +++ b/events/eventmatching/SeverityRangeMatcher.h @@ -0,0 +1,20 @@ +/* + * SeverityRangeMatcher.h + * + * Created on: 27.08.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_EVENTS_EVENTMATCHING_SEVERITYRANGEMATCHER_H_ +#define FRAMEWORK_EVENTS_EVENTMATCHING_SEVERITYRANGEMATCHER_H_ + +#include + +class SeverityRangeMatcher: public EventRangeMatcherBase { +public: + SeverityRangeMatcher(EventSeverity_t from, EventSeverity_t till, bool inverted); + ~SeverityRangeMatcher(); + bool match(EventMessage* message); +}; + +#endif /* FRAMEWORK_EVENTS_EVENTMATCHING_SEVERITYRANGEMATCHER_H_ */ diff --git a/events/eventmatching/eventmatching.h b/events/eventmatching/eventmatching.h new file mode 100644 index 00000000..435c7457 --- /dev/null +++ b/events/eventmatching/eventmatching.h @@ -0,0 +1,10 @@ +#ifndef EVENTMATCHING_H_ +#define EVENTMATCHING_H_ + +#include +#include +#include +#include + + +#endif /* EVENTMATCHING_H_ */ diff --git a/fdir/ConfirmsFailuresIF.h b/fdir/ConfirmsFailuresIF.h new file mode 100644 index 00000000..e9fb4109 --- /dev/null +++ b/fdir/ConfirmsFailuresIF.h @@ -0,0 +1,26 @@ +/* + * ConfirmsFailuresIF.h + * + * Created on: 10.09.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_FDIR_CONFIRMSFAILURESIF_H_ +#define FRAMEWORK_FDIR_CONFIRMSFAILURESIF_H_ + +#include +#include + +class ConfirmsFailuresIF { +public: + static const uint8_t INTERFACE_ID = HANDLES_FAILURES_IF; + static const ReturnValue_t YOUR_FAULT = MAKE_RETURN_CODE(0); + static const ReturnValue_t MY_FAULT = MAKE_RETURN_CODE(1); + static const ReturnValue_t CONFIRM_LATER = MAKE_RETURN_CODE(2); + virtual ~ConfirmsFailuresIF() {} + virtual MessageQueueId_t getEventReceptionQueue() = 0; +}; + + + +#endif /* FRAMEWORK_FDIR_CONFIRMSFAILURESIF_H_ */ diff --git a/fdir/EventCorrelation.cpp b/fdir/EventCorrelation.cpp new file mode 100644 index 00000000..c707bed1 --- /dev/null +++ b/fdir/EventCorrelation.cpp @@ -0,0 +1,51 @@ +/* + * EventCorrelation.cpp + * + * Created on: 15.10.2015 + * Author: baetz + */ + +#include + +EventCorrelation::EventCorrelation(uint32_t timeout) : + eventPending(false) { + correlationTimer.setTimeout(timeout); +} + +EventCorrelation::~EventCorrelation() { +} + +EventCorrelation::State EventCorrelation::doesEventCorrelate() { + if (correlationTimer.isBusy()) { + eventPending = false; + return CORRELATED; + } else { + if (eventPending) { + return ALREADY_STARTED; + } else { + eventPending = true; + correlationTimer.resetTimer(); + return CORRELATION_STARTED; + } + } +} + +bool EventCorrelation::isEventPending() { + if (eventPending) { + eventPending = false; + return true; + } else { + correlationTimer.resetTimer(); + return false; + } +} + +bool EventCorrelation::hasPendingEventTimedOut() { + if (correlationTimer.hasTimedOut()) { + bool temp = eventPending; + eventPending = false; + return temp; + } else { + return false; + } +} diff --git a/fdir/EventCorrelation.h b/fdir/EventCorrelation.h new file mode 100644 index 00000000..b40adfc8 --- /dev/null +++ b/fdir/EventCorrelation.h @@ -0,0 +1,30 @@ +/* + * EventCorrelation.h + * + * Created on: 15.10.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_FDIR_EVENTCORRELATION_H_ +#define FRAMEWORK_FDIR_EVENTCORRELATION_H_ + +#include + +class EventCorrelation { +public: + enum State { + CORRELATION_STARTED, + CORRELATED, + ALREADY_STARTED + }; + EventCorrelation(uint32_t timeout); + ~EventCorrelation(); + EventCorrelation::State doesEventCorrelate(); + bool isEventPending(); + bool hasPendingEventTimedOut(); + Countdown correlationTimer; +private: + bool eventPending; +}; + +#endif /* FRAMEWORK_FDIR_EVENTCORRELATION_H_ */ diff --git a/fdir/FDIRBase.cpp b/fdir/FDIRBase.cpp new file mode 100644 index 00000000..bb742254 --- /dev/null +++ b/fdir/FDIRBase.cpp @@ -0,0 +1,156 @@ +/* + * FDIRBase.cpp + * + * Created on: 09.09.2015 + * Author: baetz + */ + +#include +#include +#include +#include +#include +FDIRBase::FDIRBase(object_id_t owner, object_id_t parent, uint8_t messageDepth, uint8_t parameterDomainBase) : + eventQueue(messageDepth, EventMessage::EVENT_MESSAGE_SIZE), ownerId( + owner), owner(NULL), faultTreeParent(parent), parameterDomainBase(parameterDomainBase) { +} + +FDIRBase::~FDIRBase() { +} + +ReturnValue_t FDIRBase::initialize() { + EventManagerIF* manager = objectManager->get( + objects::EVENT_MANAGER); + if (manager == NULL) { + return RETURN_FAILED; + } + ReturnValue_t result = manager->registerListener(eventQueue.getId()); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if (ownerId != 0) { + result = manager->subscribeToAllEventsFrom(eventQueue.getId(), ownerId); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + owner = objectManager->get(ownerId); + if (owner == NULL) { + return RETURN_FAILED; + } + } + if (faultTreeParent != 0) { + ConfirmsFailuresIF* parentIF = objectManager->get( + faultTreeParent); + if (parentIF == NULL) { + return RETURN_FAILED; + } + eventQueue.setDefaultDestination(parentIF->getEventReceptionQueue()); + } + return RETURN_OK; +} + +void FDIRBase::checkForFailures() { + EventMessage event; + for (ReturnValue_t result = eventQueue.receiveMessage(&event); + result == RETURN_OK; result = eventQueue.receiveMessage(&event)) { + if (event.getSender() == eventQueue.getId()) { + //We already got this event, because we sent it. + continue; + } + switch (event.getMessageId()) { + case EventMessage::EVENT_MESSAGE: + if (isFdirDisabledForSeverity(event.getSeverity())) { + //We do not handle events when disabled. + continue; + } + eventReceived(&event); + break; + case EventMessage::CONFIRMATION_REQUEST: + doConfirmFault(&event); + break; + case EventMessage::YOUR_FAULT: + eventConfirmed(&event); + break; + case EventMessage::MY_FAULT: + wasParentsFault(&event); + break; + default: + break; + } + } + decrementFaultCounters(); +} + +void FDIRBase::setOwnerHealth(HasHealthIF::HealthState health) { + if (owner != NULL) { + owner->setHealth(health); + } + //else: owner has no health. + +} + +MessageQueueId_t FDIRBase::getEventReceptionQueue() { + return eventQueue.getId(); +} + +ReturnValue_t FDIRBase::sendConfirmationRequest(EventMessage* event, + MessageQueueId_t destination) { + event->setMessageId(EventMessage::CONFIRMATION_REQUEST); + if (destination != 0) { + return eventQueue.sendMessage(destination, event); + } else if (faultTreeParent != 0) { + return eventQueue.sendToDefault(event); + } + return RETURN_FAILED; +} + +void FDIRBase::eventConfirmed(EventMessage* event) { +} + +void FDIRBase::wasParentsFault(EventMessage* event) { +} + +void FDIRBase::doConfirmFault(EventMessage* event) { + ReturnValue_t result = confirmFault(event); + if (result == YOUR_FAULT) { + event->setMessageId(EventMessage::YOUR_FAULT); + eventQueue.reply(event); + } else if (result == MY_FAULT) { + event->setMessageId(EventMessage::MY_FAULT); + eventQueue.reply(event); + } else { + + } +} + +ReturnValue_t FDIRBase::confirmFault(EventMessage* event) { + return YOUR_FAULT; +} + +void FDIRBase::triggerEvent(Event event, uint32_t parameter1, + uint32_t parameter2) { + //With this mechanism, all events are disabled for a certain device. + //That's not so good for visibility. + if (isFdirDisabledForSeverity(EVENT::getSeverity(event))) { + return; + } + EventMessage message(event, ownerId, parameter1, parameter2); + EventManagerIF::triggerEvent(&message, eventQueue.getId()); + eventReceived(&message); +} + +bool FDIRBase::isFdirDisabledForSeverity(EventSeverity_t severity) { + if ((owner != NULL) && (severity != SEVERITY::INFO)) { + if (owner->getHealth() == HasHealthIF::EXTERNAL_CONTROL) { + //External control disables handling of fault messages. + return true; + } + } + return false; +} + +void FDIRBase::throwFdirEvent(Event event, uint32_t parameter1, + uint32_t parameter2) { + EventMessage message(event, ownerId, parameter1, parameter2); + EventManagerIF::triggerEvent(&message, eventQueue.getId()); +} diff --git a/fdir/FDIRBase.h b/fdir/FDIRBase.h new file mode 100644 index 00000000..54686796 --- /dev/null +++ b/fdir/FDIRBase.h @@ -0,0 +1,49 @@ +/* + * FDIRBase.h + * + * Created on: 09.09.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_FDIR_FDIRBASE_H_ +#define FRAMEWORK_FDIR_FDIRBASE_H_ + +#include +#include +#include +#include +#include +#include +#include +class FDIRBase: public HasReturnvaluesIF, public ConfirmsFailuresIF, public HasParametersIF { +public: + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::FDIR_1; + static const Event FDIR_CHANGED_STATE = MAKE_EVENT(1, SEVERITY::INFO); //!< FDIR has an internal state, which changed from par2 (oldState) to par1 (newState). + static const Event FDIR_STARTS_RECOVERY = MAKE_EVENT(2, SEVERITY::MEDIUM); //!< FDIR tries to restart device. Par1: event that caused recovery. + static const Event FDIR_TURNS_OFF_DEVICE = MAKE_EVENT(3, SEVERITY::MEDIUM); //!< FDIR turns off device. Par1: event that caused recovery. + FDIRBase(object_id_t owner, object_id_t parent = 0, uint8_t messageDepth = 10, uint8_t parameterDomainBase = 0xF0); + virtual ~FDIRBase(); + virtual ReturnValue_t initialize(); + void checkForFailures(); + MessageQueueId_t getEventReceptionQueue(); + virtual void triggerEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0); +protected: + MessageQueue eventQueue; + object_id_t ownerId; + HasHealthIF* owner; + object_id_t faultTreeParent; + uint8_t parameterDomainBase; + void setOwnerHealth(HasHealthIF::HealthState health); + virtual ReturnValue_t eventReceived(EventMessage* event) = 0; + virtual void eventConfirmed(EventMessage* event); + virtual void wasParentsFault(EventMessage* event); + virtual ReturnValue_t confirmFault(EventMessage* event); + virtual void decrementFaultCounters() = 0; + ReturnValue_t sendConfirmationRequest(EventMessage* event, MessageQueueId_t destination = 0); + void throwFdirEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0); +private: + void doConfirmFault(EventMessage* event); + bool isFdirDisabledForSeverity(EventSeverity_t severity); +}; + +#endif /* FRAMEWORK_FDIR_FDIRBASE_H_ */ diff --git a/fdir/FaultCounter.cpp b/fdir/FaultCounter.cpp new file mode 100644 index 00000000..671aa6a4 --- /dev/null +++ b/fdir/FaultCounter.cpp @@ -0,0 +1,93 @@ +/* + * FaultCounter.cpp + * + * Created on: 15.09.2015 + * Author: baetz + */ + +#include + +FaultCounter::FaultCounter(uint32_t failureThreshold, uint32_t decrementAfterMs, + uint8_t setParameterDomain) : + parameterDomain(setParameterDomain), timer(), faultCount(0), failureThreshold( + failureThreshold) { + timer.setTimeout(decrementAfterMs); +} + +FaultCounter::~FaultCounter() { +} + +void FaultCounter::increment(uint32_t amount) { + if (faultCount == 0) { + timer.resetTimer(); + } + faultCount += amount; +} + +bool FaultCounter::checkForDecrement() { + if (timer.hasTimedOut()) { + timer.resetTimer(); + if (faultCount > 0) { + faultCount--; + return true; + } + } + return false; +} + +bool FaultCounter::incrementAndCheck(uint32_t amount) { + increment(amount); + return aboveThreshold(); +} + +bool FaultCounter::aboveThreshold() { + if (faultCount > failureThreshold) { + faultCount = 0; + return true; + } else { + return false; + } +} + +void FaultCounter::clear() { + faultCount = 0; +} + +void FaultCounter::setFailureThreshold(uint32_t failureThreshold) { + this->failureThreshold = failureThreshold; +} + +void FaultCounter::setFaultDecrementTimeMs(uint32_t timeMs) { + timer.setTimeout(timeMs); +} + +FaultCounter::FaultCounter() : + parameterDomain(0), timer(), faultCount(0), failureThreshold(0) { +} + +ReturnValue_t FaultCounter::getParameter(uint8_t domainId, uint16_t parameterId, + ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues, + uint16_t startAtIndex) { + if (domainId != parameterDomain) { + return INVALID_DOMAIN_ID; + } + + switch (parameterId) { + case 0: + parameterWrapper->set(failureThreshold); + break; + case 1: + parameterWrapper->set(faultCount); + break; + case 2: + parameterWrapper->set(timer.timeout); + break; + default: + return INVALID_MATRIX_ID; + } + return HasReturnvaluesIF::RETURN_OK; +} + +void FaultCounter::setParameterDomain(uint8_t domain) { + parameterDomain = domain; +} diff --git a/fdir/FaultCounter.h b/fdir/FaultCounter.h new file mode 100644 index 00000000..d63dfa66 --- /dev/null +++ b/fdir/FaultCounter.h @@ -0,0 +1,45 @@ +/* + * FaultCounter.h + * + * Created on: 15.09.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_FDIR_FAULTCOUNTER_H_ +#define FRAMEWORK_FDIR_FAULTCOUNTER_H_ + +#include +#include + +class FaultCounter: public HasParametersIF { +public: + FaultCounter(); + FaultCounter(uint32_t failureThreshold, uint32_t decrementAfterMs, + uint8_t setParameterDomain = 0); + virtual ~FaultCounter(); + + bool incrementAndCheck(uint32_t amount = 1); + + void increment(uint32_t amount = 1); + + bool checkForDecrement(); + + bool aboveThreshold(); + + void clear(); + void setFailureThreshold(uint32_t failureThreshold); + void setFaultDecrementTimeMs(uint32_t timeMs); + + virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, + ParameterWrapper *parameterWrapper, + const ParameterWrapper *newValues, uint16_t startAtIndex); + + void setParameterDomain(uint8_t domain); +private: + uint8_t parameterDomain; + Countdown timer; + uint32_t faultCount; + uint32_t failureThreshold; +}; + +#endif /* FRAMEWORK_FDIR_FAULTCOUNTER_H_ */ diff --git a/globalfunctions/AsciiConverter.cpp b/globalfunctions/AsciiConverter.cpp new file mode 100644 index 00000000..1459a241 --- /dev/null +++ b/globalfunctions/AsciiConverter.cpp @@ -0,0 +1,230 @@ +#include +#include +#include + +template +ReturnValue_t AsciiConverter::scanAsciiDecimalNumber(const uint8_t** dataPtr, + uint8_t len, T* value) { + if (len > std::numeric_limits().digits10) { + return TOO_LONG_FOR_TARGET_TYPE; + } + + double temp; + + ReturnValue_t result = scanAsciiDecimalNumber_(dataPtr, len, &temp); + + *value = temp; + + return result; +} + +ReturnValue_t AsciiConverter::scanAsciiHexByte(const uint8_t** dataPtr, + uint8_t* value) { + int8_t tmp; + + tmp = convertHexChar(*dataPtr); + (*dataPtr)++; + if (tmp == -1) { + return INVALID_CHARACTERS; + } + if (tmp == -2) { + tmp = 0; + } + + *value = tmp << 4; + + tmp = convertHexChar(*dataPtr); + (*dataPtr)++; + if (tmp == -1) { + return INVALID_CHARACTERS; + } + if (tmp != -2) { + *value += tmp; + } else { + *value = *value >> 4; + } + + return RETURN_OK; +} + +ReturnValue_t AsciiConverter::scanAsciiDecimalNumber_(uint8_t const ** dataPtr, + uint8_t len, double* value) { + + uint8_t const *ptr = *dataPtr; + int8_t sign = 1; + float decimal = 0; + bool abort = false; + + *value = 0; + + //ignore leading space + ptr = clearSpace(ptr, len); + + while ((ptr - *dataPtr < len) && !abort) { + switch (*ptr) { + case '+': + sign = 1; + break; + case '-': + sign = -1; + break; + case '.': + decimal = 1; + break; + case ' ': + case 0x0d: + case 0x0a: + //ignore trailing space + ptr = clearSpace(ptr, len - (ptr - *dataPtr)) - 1; //before aborting the loop, ptr will be incremented + abort = true; + break; + default: + if ((*ptr < 0x30) || (*ptr > 0x39)) { + return INVALID_CHARACTERS; + } + *value = *value * 10 + (*ptr - 0x30); + if (decimal > 0) { + decimal *= 10; + } + break; + } + ptr++; + } + + if (decimal == 0) { + decimal = 1; + } + + *value = *value / (decimal) * sign; + + *dataPtr = ptr; + + return RETURN_OK; +} + +ReturnValue_t AsciiConverter::printFloat(uint8_t* buffer, uint32_t bufferLength, + float value, uint8_t decimalPlaces, uint32_t *printedSize) { + *printedSize = 0; + uint32_t streamposition = 0, integerSize; + bool negative = (value < 0); + int32_t digits = bufferLength - decimalPlaces - 1; + if (digits <= 0) { + return BUFFER_TOO_SMALL; + } + if (negative) { + digits -= 1; + buffer[streamposition++] = '-'; + value = -value; + } + float maximumNumber = pow(10, digits); + if (value >= maximumNumber) { + return BUFFER_TOO_SMALL; + } + //print the numbers before the decimal point; + ReturnValue_t result = printInteger(buffer + streamposition, + bufferLength - streamposition - decimalPlaces - 1, value, + &integerSize); + if (result != RETURN_OK) { + return result; + } + streamposition += integerSize; + //The decimal Point + buffer[streamposition++] = '.'; + + //Print the decimals + uint32_t integerValue = value; + value -= integerValue; + value = value * pow(10, decimalPlaces); + result = printInteger(buffer + streamposition, decimalPlaces, round(value), + &integerSize, true); + *printedSize = integerSize + streamposition; + return result; +} + +ReturnValue_t AsciiConverter::printInteger(uint8_t* buffer, + uint32_t bufferLength, uint32_t value, uint32_t *printedSize, + bool leadingZeros) { + *printedSize = 0; + if (bufferLength == 0) { + return BUFFER_TOO_SMALL; + } + uint32_t maximumNumber = -1; + if (bufferLength < 10) { + maximumNumber = pow(10, bufferLength); + if (value >= maximumNumber) { + return BUFFER_TOO_SMALL; + } + maximumNumber /= 10; + } else { + if (!(value <= maximumNumber)) { + return BUFFER_TOO_SMALL; + } + maximumNumber = 1000000000; + } + if (!leadingZeros && (value == 0)) { + buffer[(*printedSize)++] = '0'; + return RETURN_OK; + } + while (maximumNumber >= 1) { + uint8_t number = value / maximumNumber; + value = value - (number * maximumNumber); + if (!leadingZeros && number == 0) { + maximumNumber /= 10; + } else { + leadingZeros = true; + buffer[(*printedSize)++] = '0' + number; + maximumNumber /= 10; + } + } + return RETURN_OK; +} + +ReturnValue_t AsciiConverter::printSignedInteger(uint8_t* buffer, + uint32_t bufferLength, int32_t value, uint32_t *printedSize) { + bool negative = false; + if ((bufferLength > 0) && (value < 0)) { + *buffer++ = '-'; + bufferLength--; + value = -value; + negative = true; + } + ReturnValue_t result = printInteger(buffer, bufferLength, value, + printedSize); + if (negative) { + (*printedSize)++; + } + return result; +} + +int8_t AsciiConverter::convertHexChar(const uint8_t* character) { + if ((*character > 0x60) && (*character < 0x67)) { + return *character - 0x61 + 10; + } else if ((*character > 0x40) && (*character < 0x47)) { + return *character - 0x41 + 10; + } else if ((*character > 0x2F) && (*character < 0x3A)) { + return *character - 0x30; + } else if (*character == ' ') { + return -2; + } + return -1; +} + +template ReturnValue_t AsciiConverter::scanAsciiDecimalNumber( + const uint8_t** dataPtr, uint8_t len, float* value); +template ReturnValue_t AsciiConverter::scanAsciiDecimalNumber( + const uint8_t** dataPtr, uint8_t len, uint8_t* value); +template ReturnValue_t AsciiConverter::scanAsciiDecimalNumber( + const uint8_t** dataPtr, uint8_t len, uint16_t* value); +template ReturnValue_t AsciiConverter::scanAsciiDecimalNumber( + const uint8_t** dataPtr, uint8_t len, double* value); + +const uint8_t* AsciiConverter::clearSpace(const uint8_t* data, uint8_t len) { + while (len > 0) { + if ((*data != ' ') && (*data != 0x0a) && (*data != 0x0d)) { + return data; + } + data++; + len--; + } + return data; +} diff --git a/globalfunctions/AsciiConverter.h b/globalfunctions/AsciiConverter.h new file mode 100644 index 00000000..8ba58cb1 --- /dev/null +++ b/globalfunctions/AsciiConverter.h @@ -0,0 +1,39 @@ +#ifndef ASCIICONVERTER_H_ +#define ASCIICONVERTER_H_ + +#include + +class AsciiConverter: public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = ASCII_CONVERTER; + static const ReturnValue_t TOO_LONG_FOR_TARGET_TYPE = MAKE_RETURN_CODE(1); + static const ReturnValue_t INVALID_CHARACTERS = MAKE_RETURN_CODE(2); + static const ReturnValue_t BUFFER_TOO_SMALL = MAKE_RETURN_CODE(0x3); + + template + static ReturnValue_t scanAsciiDecimalNumber(const uint8_t **dataPtr, + uint8_t len, T *value); + + static ReturnValue_t scanAsciiHexByte(const uint8_t **dataPtr, + uint8_t *value); + + static ReturnValue_t printFloat(uint8_t *buffer, uint32_t bufferLength, + float value, uint8_t decimalPlaces, uint32_t *printedSize); + + static ReturnValue_t printInteger(uint8_t *buffer, uint32_t bufferLength, + uint32_t value, uint32_t *printedSize, bool leadingZeros = false); + + static ReturnValue_t printSignedInteger(uint8_t *buffer, + uint32_t bufferLength, int32_t value, uint32_t *printedSize); + +private: + AsciiConverter(); + static ReturnValue_t scanAsciiDecimalNumber_(const uint8_t **dataPtr, + uint8_t len, double *value); + + static int8_t convertHexChar(const uint8_t *character); + + static const uint8_t *clearSpace(const uint8_t *data, uint8_t len); +}; + +#endif /* ASCIICONVERTER_H_ */ diff --git a/globalfunctions/DleEncoder.cpp b/globalfunctions/DleEncoder.cpp new file mode 100644 index 00000000..a68f1524 --- /dev/null +++ b/globalfunctions/DleEncoder.cpp @@ -0,0 +1,95 @@ +#include + +DleEncoder::DleEncoder() { +} + +DleEncoder::~DleEncoder() { +} + +ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream, + uint32_t sourceStreamLen, uint32_t *readLen, uint8_t *destStream, + uint32_t maxDestStreamlen, uint32_t *decodedLen) { + uint32_t encodedIndex = 0, decodedIndex = 0; + uint8_t nextByte; + if (*sourceStream != STX) { + return RETURN_FAILED; + } + ++encodedIndex; + while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen) + && (sourceStream[encodedIndex] != ETX) + && (sourceStream[encodedIndex] != STX)) { + if (sourceStream[encodedIndex] == DLE) { + nextByte = sourceStream[encodedIndex + 1]; + if (nextByte == 0x10) { + destStream[decodedIndex] = nextByte; + } else { + if ((nextByte == 0x42) || (nextByte == 0x43) + || (nextByte == 0x4D)) { + destStream[decodedIndex] = nextByte - 0x40; + } else { + return RETURN_FAILED; + } + } + ++encodedIndex; + } else { + destStream[decodedIndex] = sourceStream[encodedIndex]; + } + ++encodedIndex; + ++decodedIndex; + } + if (sourceStream[encodedIndex] != ETX) { + return RETURN_FAILED; + } else { + *readLen = ++encodedIndex; + *decodedLen = decodedIndex; + return RETURN_OK; + } +} + +ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream, + uint32_t sourceLen, uint8_t* destStream, uint32_t maxDestLen, + uint32_t* encodedLen, bool addStxEtx) { + if (maxDestLen < 2) { + return RETURN_FAILED; + } + uint32_t encodedIndex = 0, sourceIndex = 0; + uint8_t nextByte; + if (addStxEtx) { + destStream[0] = STX; + ++encodedIndex; + } + while ((encodedIndex < maxDestLen) && (sourceIndex < sourceLen)) { + nextByte = sourceStream[sourceIndex]; + if ((nextByte == STX) || (nextByte == ETX) || (nextByte == 0x0D)) { + if (encodedIndex + 1 >= maxDestLen) { + return RETURN_FAILED; + } else { + destStream[encodedIndex] = DLE; + ++encodedIndex; + destStream[encodedIndex] = nextByte + 0x40; + } + } else if (nextByte == DLE) { + if (encodedIndex + 1 >= maxDestLen) { + return RETURN_FAILED; + } else { + destStream[encodedIndex] = DLE; + ++encodedIndex; + destStream[encodedIndex] = DLE; + } + } else { + destStream[encodedIndex] = nextByte; + } + ++encodedIndex; + ++sourceIndex; + } + if ((sourceIndex == sourceLen) && (encodedIndex < maxDestLen)) { + if (addStxEtx) { + destStream[encodedIndex] = ETX; + ++encodedIndex; + } + *encodedLen = encodedIndex; + return RETURN_OK; + } else { + return RETURN_FAILED; + } +} diff --git a/globalfunctions/DleEncoder.h b/globalfunctions/DleEncoder.h new file mode 100644 index 00000000..fc155600 --- /dev/null +++ b/globalfunctions/DleEncoder.h @@ -0,0 +1,25 @@ +#ifndef DLEENCODER_H_ +#define DLEENCODER_H_ + +#include + +class DleEncoder: public HasReturnvaluesIF { +private: + DleEncoder(); + virtual ~DleEncoder(); + +public: + static const uint8_t STX = 0x02; + static const uint8_t ETX = 0x03; + static const uint8_t DLE = 0x10; + + static ReturnValue_t decode(const uint8_t *sourceStream, + uint32_t sourceStreamLen, uint32_t *readLen, uint8_t *destStream, + uint32_t maxDestStreamlen, uint32_t *decodedLen); + + static ReturnValue_t encode(const uint8_t *sourceStream, uint32_t sourceLen, + uint8_t *destStream, uint32_t maxDestLen, uint32_t *encodedLen, + bool addStxEtx = true); +}; + +#endif /* DLEENCODER_H_ */ diff --git a/globalfunctions/Makefile b/globalfunctions/Makefile new file mode 100755 index 00000000..8e94bbcb --- /dev/null +++ b/globalfunctions/Makefile @@ -0,0 +1,24 @@ +#!/bin/bash +# +# OSAL makefile +# +# Created on: Mar 04, 2010 +# Author: ziemke +# Author: Claas Ziemke +# Copyright 2010, Claas Ziemke +# + +BASEDIR=../../ +include $(BASEDIR)options.mk + +OBJ = $(BUILDDIR)/crc_ccitt.o\ + $(BUILDDIR)/conversion.o\ + $(BUILDDIR)/math_custom.o\ + +all: $(OBJ) + +$(BUILDDIR)/%.o: %.cpp %.h + $(CPP) $(CFLAGS) $(DEFINES) $(CCOPT) ${INCLUDE} -c $< -o $@ + +clean: + $(RM) *.o *.gcno *.gcda diff --git a/globalfunctions/Type.cpp b/globalfunctions/Type.cpp new file mode 100644 index 00000000..814d26f4 --- /dev/null +++ b/globalfunctions/Type.cpp @@ -0,0 +1,182 @@ +#include +#include +#include + +Type::Type() : + actualType(UNKNOWN_TYPE) { +} + +Type::Type(ActualType_t actualType) : + actualType(actualType) { +} + +Type::Type(const Type& type) : + actualType(type.actualType) { +} + +Type& Type::operator =(Type rhs) { + this->actualType = rhs.actualType; + return *this; +} + +Type& Type::operator =(ActualType_t actualType) { + this->actualType = actualType; + return *this; +} + +Type::operator Type::ActualType_t() const { + return actualType; +} + +bool Type::operator ==(const Type& rhs) { + return this->actualType == rhs.actualType; +} + +bool Type::operator !=(const Type& rhs) { + return !operator==(rhs); +} + +uint8_t Type::getSize() const { + switch (actualType) { + case UINT8_T: + return sizeof(uint8_t); + case INT8_T: + return sizeof(int8_t); + case UINT16_T: + return sizeof(uint16_t); + case INT16_T: + return sizeof(int16_t); + case UINT32_T: + return sizeof(uint32_t); + case INT32_T: + return sizeof(int32_t); + case FLOAT: + return sizeof(float); + case DOUBLE: + return sizeof(double); + default: + return 0; + } +} + +ReturnValue_t Type::serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + uint8_t ptc; + uint8_t pfc; + ReturnValue_t result = getPtcPfc(&ptc, &pfc); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + result = SerializeAdapter::serialize(&ptc, buffer, size, max_size, + bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + result = SerializeAdapter::serialize(&pfc, buffer, size, max_size, + bigEndian); + + return result; + +} + +uint32_t Type::getSerializedSize() const { + uint8_t dontcare = 0; + return 2 * SerializeAdapter::getSerializedSize(&dontcare); +} + +ReturnValue_t Type::deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + uint8_t ptc; + uint8_t pfc; + ReturnValue_t result = SerializeAdapter::deSerialize(&ptc, buffer, + size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + result = SerializeAdapter::deSerialize(&pfc, buffer, size, + bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + actualType = getActualType(ptc, pfc); + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Type::getPtcPfc(uint8_t* ptc, uint8_t* pfc) const { + switch (actualType) { + case UINT8_T: + *ptc = 3; + *pfc = 4; + break; + case INT8_T: + *ptc = 4; + *pfc = 4; + break; + case UINT16_T: + *ptc = 3; + *pfc = 12; + break; + case INT16_T: + *ptc = 4; + *pfc = 12; + break; + case UINT32_T: + *ptc = 3; + *pfc = 14; + break; + case INT32_T: + *ptc = 4; + *pfc = 14; + break; + case FLOAT: + *ptc = 5; + *pfc = 1; + break; + case DOUBLE: + *ptc = 5; + *pfc = 2; + break; + default: + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; +} + +Type::ActualType_t Type::getActualType(uint8_t ptc, uint8_t pfc) { + switch (ptc) { + case 3: + switch (pfc) { + case 4: + return UINT8_T; + case 12: + return UINT16_T; + case 14: + return UINT32_T; + } + break; + case 4: + switch (pfc) { + case 4: + return INT8_T; + case 12: + return INT16_T; + case 14: + return INT32_T; + } + break; + case 5: + switch (pfc) { + case 1: + return FLOAT; + case 2: + return DOUBLE; + } + break; + } + return UNKNOWN_TYPE; +} diff --git a/globalfunctions/Type.h b/globalfunctions/Type.h new file mode 100644 index 00000000..88df07b6 --- /dev/null +++ b/globalfunctions/Type.h @@ -0,0 +1,91 @@ +#ifndef TYPE_H_ +#define TYPE_H_ + +#include +#include + +class Type: public SerializeIF { +public: + enum ActualType_t { + UINT8_T, + INT8_T, + UINT16_T, + INT16_T, + UINT32_T, + INT32_T, + FLOAT, + DOUBLE, + UNKNOWN_TYPE + }; + + Type(); + + Type(ActualType_t actualType); + + Type(const Type& type); + + Type& operator=(Type rhs); + + Type& operator=(ActualType_t actualType); + + operator ActualType_t() const; + + bool operator==(const Type& rhs); + bool operator!=(const Type& rhs); + + uint8_t getSize() const; + + ReturnValue_t getPtcPfc(uint8_t *ptc, uint8_t *pfc) const; + + static ActualType_t getActualType(uint8_t ptc, uint8_t pfc); + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const; + + virtual uint32_t getSerializedSize() const; + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian); + +private: + ActualType_t actualType; +}; + +template +struct PodTypeConversion { + static const Type::ActualType_t type = Type::UNKNOWN_TYPE; +}; +template<> +struct PodTypeConversion { + static const Type::ActualType_t type = Type::UINT8_T; +}; +template<> +struct PodTypeConversion { + static const Type::ActualType_t type = Type::UINT16_T; +}; +template<> +struct PodTypeConversion { + static const Type::ActualType_t type = Type::UINT32_T; +}; +template<> +struct PodTypeConversion { + static const Type::ActualType_t type = Type::INT8_T; +}; +template<> +struct PodTypeConversion { + static const Type::ActualType_t type = Type::INT16_T; +}; +template<> +struct PodTypeConversion { + static const Type::ActualType_t type = Type::INT32_T; +}; +template<> +struct PodTypeConversion { + static const Type::ActualType_t type = Type::FLOAT; +}; +template<> +struct PodTypeConversion { + static const Type::ActualType_t type = Type::DOUBLE; +}; + +#endif /* TYPE_H_ */ diff --git a/globalfunctions/constants.h b/globalfunctions/constants.h new file mode 100644 index 00000000..ae488a78 --- /dev/null +++ b/globalfunctions/constants.h @@ -0,0 +1,14 @@ +#ifndef CONSTANTS_H_ +#define CONSTANTS_H_ + +namespace Math { +static const double PI = 3.1415926535897932384626433; + +} +namespace Earth { +static const double OMEGA = 7.2921158553E-5; +static const double GRAVITATIONAL_CONSTANT = 3.987095392E14; +static const double MEAN_RADIUS = 6371000; +} + +#endif /* CONSTANTS_H_ */ diff --git a/globalfunctions/conversion.cpp b/globalfunctions/conversion.cpp new file mode 100644 index 00000000..8278e796 --- /dev/null +++ b/globalfunctions/conversion.cpp @@ -0,0 +1,106 @@ +/* + * conversion.cpp + * + * Created on: 07.05.2012 + * Author: baetz + */ + +#include +//TODO: This shall be optimized (later)! +void convertToByteStream( uint16_t value, uint8_t* buffer, uint32_t* size ) { + buffer[0] = (value & 0xFF00) >> 8; + buffer[1] = (value & 0x00FF); + *size += 2; +} + +void convertToByteStream( uint32_t value, uint8_t* buffer, uint32_t* size ) { + buffer[0] = (value & 0xFF000000) >> 24; + buffer[1] = (value & 0x00FF0000) >> 16; + buffer[2] = (value & 0x0000FF00) >> 8; + buffer[3] = (value & 0x000000FF); + *size +=4; +} + +void convertToByteStream( int16_t value, uint8_t* buffer, uint32_t* size ) { + buffer[0] = (value & 0xFF00) >> 8; + buffer[1] = (value & 0x00FF); + *size += 2; +} + +void convertToByteStream( int32_t value, uint8_t* buffer, uint32_t* size ) { + buffer[0] = (value & 0xFF000000) >> 24; + buffer[1] = (value & 0x00FF0000) >> 16; + buffer[2] = (value & 0x0000FF00) >> 8; + buffer[3] = (value & 0x000000FF); + *size += 4; +} + +//void convertToByteStream( uint64_t value, uint8_t* buffer, uint32_t* size ) { +// buffer[0] = (value & 0xFF00000000000000) >> 56; +// buffer[1] = (value & 0x00FF000000000000) >> 48; +// buffer[2] = (value & 0x0000FF0000000000) >> 40; +// buffer[3] = (value & 0x000000FF00000000) >> 32; +// buffer[4] = (value & 0x00000000FF000000) >> 24; +// buffer[5] = (value & 0x0000000000FF0000) >> 16; +// buffer[6] = (value & 0x000000000000FF00) >> 8; +// buffer[7] = (value & 0x00000000000000FF); +// *size+=8; +//} +// +//void convertToByteStream( int64_t value, uint8_t* buffer, uint32_t* size ) { +// buffer[0] = (value & 0xFF00000000000000) >> 56; +// buffer[1] = (value & 0x00FF000000000000) >> 48; +// buffer[2] = (value & 0x0000FF0000000000) >> 40; +// buffer[3] = (value & 0x000000FF00000000) >> 32; +// buffer[4] = (value & 0x00000000FF000000) >> 24; +// buffer[5] = (value & 0x0000000000FF0000) >> 16; +// buffer[6] = (value & 0x000000000000FF00) >> 8; +// buffer[7] = (value & 0x00000000000000FF); +// *size+=8; +//} + +void convertToByteStream( float in_value, uint8_t* buffer, uint32_t* size ) { +#ifndef BYTE_ORDER + #error BYTE_ORDER not defined +#elif BYTE_ORDER == LITTLE_ENDIAN + union float_union { + float value; + uint8_t chars[4]; + }; + float_union temp; + temp.value = in_value; + buffer[0] = temp.chars[3]; + buffer[1] = temp.chars[2]; + buffer[2] = temp.chars[1]; + buffer[3] = temp.chars[0]; + *size += 4; +#elif BYTE_ORDER == BIG_ENDIAN + memcpy(buffer, &in_value, sizeof(in_value)); + *size += sizeof(in_value); +#endif +} + +void convertToByteStream( double in_value, uint8_t* buffer, uint32_t* size ) { +#ifndef BYTE_ORDER + #error Endianess not defined +#elif BYTE_ORDER == LITTLE_ENDIAN + union double_union { + double value; + uint8_t chars[8]; + }; + double_union temp; + temp.value = in_value; + buffer[0] = temp.chars[7]; + buffer[1] = temp.chars[6]; + buffer[2] = temp.chars[5]; + buffer[3] = temp.chars[4]; + buffer[4] = temp.chars[3]; + buffer[5] = temp.chars[2]; + buffer[6] = temp.chars[1]; + buffer[7] = temp.chars[0]; + *size += 8; +#elif BYTE_ORDER == BIG_ENDIAN + memcpy(buffer, &in_value, sizeof(in_value)); + *size += sizeof(in_value); +#endif +} diff --git a/globalfunctions/conversion.h b/globalfunctions/conversion.h new file mode 100644 index 00000000..33daf94b --- /dev/null +++ b/globalfunctions/conversion.h @@ -0,0 +1,31 @@ +/* + * conversion.h + * + * Created on: 07.05.2012 + * Author: baetz + */ + +#ifndef CONVERSION_H_ +#define CONVERSION_H_ + + +#include + + +void convertToByteStream( uint16_t value, uint8_t* buffer, uint32_t* size ); + +void convertToByteStream( uint32_t value, uint8_t* buffer, uint32_t* size ); + +void convertToByteStream( int16_t value, uint8_t* buffer, uint32_t* size ); + +void convertToByteStream( int32_t value, uint8_t* buffer, uint32_t* size ); + +//void convertToByteStream( uint64_t value, uint8_t* buffer, uint32_t* size ); +// +//void convertToByteStream( int64_t value, uint8_t* buffer, uint32_t* size ); + +void convertToByteStream( float value, uint8_t* buffer, uint32_t* size ); + +void convertToByteStream( double value, uint8_t* buffer, uint32_t* size ); + +#endif /* CONVERSION_H_ */ diff --git a/globalfunctions/crc_ccitt.cpp b/globalfunctions/crc_ccitt.cpp new file mode 100644 index 00000000..a894c2d2 --- /dev/null +++ b/globalfunctions/crc_ccitt.cpp @@ -0,0 +1,147 @@ +/* + * crc.cpp + * + * Created on: 03.04.2012 + * Author: bucher + */ + +#include +#include + +static const uint16_t crc_table[256] = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, + 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, + 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, + 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, + 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, + 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, + 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, + 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, + 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, + 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, + 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, + 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, + 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, + 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, + 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, + 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, + 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, + 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, + 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, + 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, + 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, + 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, + 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, + 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, + 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0 +}; + + +// CRC implementation +uint16_t Calculate_CRC(uint8_t const input[], uint32_t length) +{ + uint16_t crc = 0xFFFF; + uint8_t *data = (uint8_t *)input; + unsigned int tbl_idx; + + while (length--) { + tbl_idx = ((crc >> 8) ^ *data) & 0xff; + crc = (crc_table[tbl_idx] ^ (crc << 8)) & 0xffff; + + data++; + } + return crc & 0xffff; + + //The part below is not used! +// bool temr[16]; +// bool xor_out[16]; +// bool r[16]; +// bool d[8]; +// uint16_t crc_value = 0; +// +// +// for (int i=0; i<16 ;i++) { +// temr[i] = false; +// xor_out[i] = false; +// } +// +// +// for (int i=0; i<16 ;i++) +// r[i] = true; // initialize with 0xFFFF +// +// +// +// for (int j=0; j + +uint16_t Calculate_CRC(uint8_t const input[], uint32_t length); + + +#endif /* CRC_H_ */ diff --git a/globalfunctions/matching/BinaryMatcher.h b/globalfunctions/matching/BinaryMatcher.h new file mode 100644 index 00000000..df7325cf --- /dev/null +++ b/globalfunctions/matching/BinaryMatcher.h @@ -0,0 +1,41 @@ +#ifndef BINARYMATCHER_H_ +#define BINARYMATCHER_H_ + +#include + +template +class BinaryMatcher: public MatcherIF { +public: + bool inverted; + T mask, matchField; + + BinaryMatcher() : + inverted(false), mask(0), matchField(0) { + } + + BinaryMatcher(T mask, T match, bool inverted = false) : + inverted(inverted), mask(mask), matchField(match) { + } + + bool match(T input) { + if (inverted) { + return ~doMatch(input, mask, matchField); + } else { + return doMatch(input, mask, matchField); + } + } + +protected: + + bool doMatch(T input, T mask, T match) { + match = match & mask; + input = input & mask; + if (input == match) { + return true; + } else { + return false; + } + } +}; + +#endif /* BINARYMATCHER_H_ */ diff --git a/globalfunctions/matching/DecimalMatcher.h b/globalfunctions/matching/DecimalMatcher.h new file mode 100644 index 00000000..f4722e6b --- /dev/null +++ b/globalfunctions/matching/DecimalMatcher.h @@ -0,0 +1,50 @@ +#ifndef DECIMALMATCHER_H_ +#define DECIMALMATCHER_H_ + +#include + +template +class DecimalMatcher: public MatcherIF { +public: + bool inverted; + T mask, matchField; + + DecimalMatcher() : + inverted(false), mask(0), matchField(0) { + } + + DecimalMatcher(T mask, T match, bool inverted = false) : + inverted(inverted), mask(mask), matchField(match) { + } + + bool match(T input) { + if (inverted) { + return ~doMatch(input, mask, matchField); + } else { + return doMatch(input, mask, matchField); + } + } + +protected: + bool doMatch(T input, T mask, T match) { + T decimal = 1, remainderMask, remainderMatch, remainderInput; + + while (mask != 0) { + remainderMask = mask % (decimal * 10); + remainderMatch = match % (decimal * 10); + remainderInput = input % (decimal * 10); + if (remainderMask != 0) { + if (remainderMatch != remainderInput) { + return false; + } + } + mask -= remainderMask; + match -= remainderMatch; + input -= remainderInput; + decimal *= 10; + } + return true; + } +}; + +#endif /* DECIMALMATCHER_H_ */ diff --git a/globalfunctions/matching/MatchTree.h b/globalfunctions/matching/MatchTree.h new file mode 100644 index 00000000..6732d317 --- /dev/null +++ b/globalfunctions/matching/MatchTree.h @@ -0,0 +1,208 @@ +/* + * MatchTree.h + * + * Created on: 09.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_GLOBALFUNCTIONS_MATCHING_MATCHTREE_H_ +#define FRAMEWORK_GLOBALFUNCTIONS_MATCHING_MATCHTREE_H_ + +#include +#include +#include + +template +class MatchTree: public SerializeableMatcherIF, public BinaryTree< + SerializeableMatcherIF> { +public: + + static const uint8_t INTERFACE_ID = MATCH_TREE_CLASS; + static const ReturnValue_t TOO_DETAILED_REQUEST = MAKE_RETURN_CODE(1); + static const ReturnValue_t TOO_GENERAL_REQUEST = MAKE_RETURN_CODE(2); + static const ReturnValue_t NO_MATCH = MAKE_RETURN_CODE(3); + static const ReturnValue_t FULL = MAKE_RETURN_CODE(4); + + typedef typename BinaryTree>::iterator iterator; + typedef BinaryNode> Node; + static const bool AND = true; //LEFT + static const bool OR = false; //RIGHT + MatchTree(BinaryNode>* root, + uint8_t maxDepth = -1) : + BinaryTree>(root), maxDepth(maxDepth) { + } + MatchTree(iterator root, uint8_t maxDepth = -1) : + BinaryTree>(root.element), maxDepth( + maxDepth) { + } + MatchTree() : + BinaryTree>(), maxDepth(-1) { + } + virtual ~MatchTree() { + } + virtual bool match(T number) { + return matchesTree(number, NULL, NULL); + } + bool matchesTree(T number, iterator* lastTest, uint8_t* hierarchyLevel) { + bool match = false; + iterator iter = this->begin(); + while (iter != this->end()) { + if (lastTest != NULL) { + *lastTest = iter; + } + match = iter->match(number); + if (match) { + iter = iter.left(); + if (hierarchyLevel != NULL) { + (*hierarchyLevel)++; + } + } else { + iter = iter.right(); + } + } + return match; + } + + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + iterator iter = this->begin(); + uint8_t count = this->countRight(iter); + ReturnValue_t result = SerializeAdapter::serialize(&count, + buffer, size, max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if (iter == this->end()) { + return HasReturnvaluesIF::RETURN_OK; + } + result = iter->serialize(buffer, size, max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if (maxDepth > 0) { + MatchTree temp(iter.left(), maxDepth - 1); + result = temp.serialize(buffer, size, max_size, bigEndian); + } + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + iter = iter.right(); + while (iter != this->end()) { + result = iter->serialize(buffer, size, max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if (maxDepth > 0) { + MatchTree temp(iter.left(), maxDepth - 1); + result = temp.serialize(buffer, size, max_size, bigEndian); + } + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + iter = iter.right(); + } + return result; + } + + uint32_t getSerializedSize() const { + //Analogous to serialize! + uint32_t size = 1; //One for count + iterator iter = this->begin(); + if (iter == this->end()) { + return size; + } + //Count object itself + size += iter->getSerializedSize(); + //Handle everything below on AND side + if (maxDepth > 0) { + MatchTree temp(iter.left(), maxDepth - 1); + size += temp.getSerializedSize(); + } + //Handle everything on OR side + iter = iter.right(); + //Iterate over every object on the OR branch + while (iter != this->end()) { + size += iter->getSerializedSize(); + if (maxDepth > 0) { + //If we are allowed to go deeper, handle AND elements. + MatchTree temp(iter.left(), maxDepth - 1); + size += temp.getSerializedSize(); + } + iter = iter.right(); + } + return size; + } + + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return HasReturnvaluesIF::RETURN_OK; + } + +protected: + + bool isOnAndBranch(iterator position) { + if ((position == this->end()) || (position.up() == this->end())) { + return false; + } + if (position.up().left() == position) { + return true; + } else { + return false; + } + } + + //TODO: What to do if insertion/deletion fails. Throw event? + ReturnValue_t removeElementAndAllChildren(iterator position) { + auto children = erase(position); + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + if (children.first != this->end()) { + result = removeElementAndAllChildren(children.first); + } + if (children.second != this->end()) { + result = removeElementAndAllChildren(children.second); + } + //Delete element itself. + return cleanUpElement(position); + } + + ReturnValue_t removeElementAndReconnectChildren(iterator position) { + if (position == this->end()) { + return HasReturnvaluesIF::RETURN_OK; + } + //Delete everything from the AND branch. + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + if (position.left() != this->end()) { + result = removeElementAndAllChildren(position.left()); + } + if (position.right() != this->end()) { + //There's something at the OR branch, reconnect to parent. + if (isOnAndBranch(position)) { + //Either one hierarchy up AND branch... + insert(AND, position.up(), position.right().element); + } else { + //or on another OR'ed element (or install new root node). + insert(OR, position.up(), position.right().element); + } + } else { + if (isOnAndBranch(position)) { + //Recursively delete parent node as well, because it is not expected to be there anymore. + return removeElementAndReconnectChildren(position.up()); + } else { + //simply delete self. + erase(position); + } + + } + //Delete element itself. + return cleanUpElement(position); + } + + virtual ReturnValue_t cleanUpElement(iterator position) { + return HasReturnvaluesIF::RETURN_OK; + } + +private: + uint8_t maxDepth; +}; + +#endif /* FRAMEWORK_GLOBALFUNCTIONS_MATCHING_MATCHTREE_H_ */ diff --git a/globalfunctions/matching/MatcherIF.h b/globalfunctions/matching/MatcherIF.h new file mode 100644 index 00000000..4a6a05af --- /dev/null +++ b/globalfunctions/matching/MatcherIF.h @@ -0,0 +1,16 @@ +#ifndef MATCHERIF_H_ +#define MATCHERIF_H_ + +template +class MatcherIF { +public: + virtual ~MatcherIF() { + } + + virtual bool match(T number) { + return false; + } + +}; + +#endif /* MATCHERIF_H_ */ diff --git a/globalfunctions/matching/RangeMatcher.h b/globalfunctions/matching/RangeMatcher.h new file mode 100644 index 00000000..1fe6675c --- /dev/null +++ b/globalfunctions/matching/RangeMatcher.h @@ -0,0 +1,65 @@ +#ifndef RANGEMATCHER_H_ +#define RANGEMATCHER_H_ + +#include +#include + + +template +class RangeMatcher: public SerializeableMatcherIF { +public: + bool inverted; + T lowerBound; + T upperBound; + + RangeMatcher() : + inverted(true), lowerBound(1), upperBound(0) { + } + RangeMatcher(T lowerBound, T upperBound, bool inverted = false) : + inverted(inverted), lowerBound(lowerBound), upperBound(upperBound) { + } + + bool match(T input) { + if (inverted) { + return ~doMatch(input); + } else { + return doMatch(input); + } + } + + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + ReturnValue_t result = SerializeAdapter::serialize(&lowerBound, buffer, size, max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = SerializeAdapter::serialize(&upperBound, buffer, size, max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return SerializeAdapter::serialize(&inverted, buffer, size, max_size, bigEndian); + } + + uint32_t getSerializedSize() const { + return sizeof(lowerBound) + sizeof(upperBound) + sizeof(bool); + } + + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + ReturnValue_t result = SerializeAdapter::deSerialize(&lowerBound, buffer, size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = SerializeAdapter::deSerialize(&upperBound, buffer, size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return SerializeAdapter::deSerialize(&inverted, buffer, size, bigEndian); + } +protected: + bool doMatch(T input) { + return (input >= lowerBound) && (input <= upperBound); + } +}; + +#endif /* RANGEMATCHER_H_ */ diff --git a/globalfunctions/matching/SerializeableMatcherIF.h b/globalfunctions/matching/SerializeableMatcherIF.h new file mode 100644 index 00000000..6b3035e9 --- /dev/null +++ b/globalfunctions/matching/SerializeableMatcherIF.h @@ -0,0 +1,20 @@ +/* + * SerializeableMatcherIF.h + * + * Created on: 09.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_GLOBALFUNCTIONS_MATCHING_SERIALIZEABLEMATCHERIF_H_ +#define FRAMEWORK_GLOBALFUNCTIONS_MATCHING_SERIALIZEABLEMATCHERIF_H_ + +#include +#include + +template +class SerializeableMatcherIF : public MatcherIF, public SerializeIF { +public: + virtual ~SerializeableMatcherIF() {} +}; + +#endif /* FRAMEWORK_GLOBALFUNCTIONS_MATCHING_SERIALIZEABLEMATCHERIF_H_ */ diff --git a/globalfunctions/math/MatrixOperations.h b/globalfunctions/math/MatrixOperations.h new file mode 100644 index 00000000..e4d82234 --- /dev/null +++ b/globalfunctions/math/MatrixOperations.h @@ -0,0 +1,46 @@ +#ifndef MATRIXOPERATIONS_H_ +#define MATRIXOPERATIONS_H_ + +#include +#include + +template +class MatrixOperations { +public: + virtual ~MatrixOperations() { + } + + //do not use with result == matrix1 or matrix2 //TODO? + static void multiply(const T *matrix1, const T *matrix2, T *result, + uint8_t rows1, uint8_t columns1, uint8_t columns2) { + for (uint8_t resultColumn = 0; resultColumn < columns2; + resultColumn++) { + for (uint8_t resultRow = 0; resultRow < rows1; resultRow++) { + result[resultColumn + columns2 * resultRow] = 0; + for (uint8_t i = 0; i < columns1; i++) { + result[resultColumn + columns2 * resultRow] += matrix1[i + + resultRow * columns1] + * matrix2[resultColumn + i * columns2]; + } + } + } + } + + static void transpose(const T *matrix, T *transposed, uint8_t size) { + uint8_t row, column; + transposed[0] = matrix[0]; + for (column = 1; column < size; column++) { + transposed[column + size * column] = matrix[column + size * column]; + for (row = 0; row < column; row++) { + T temp = matrix[column + size * row]; + transposed[column + size * row] = matrix[row + size * column]; + transposed[row + size * column] = temp; + } + } + } + +private: + MatrixOperations(); +}; + +#endif /* MATRIXOPERATIONS_H_ */ diff --git a/globalfunctions/math/QuaternionOperations.cpp b/globalfunctions/math/QuaternionOperations.cpp new file mode 100644 index 00000000..cb06a22e --- /dev/null +++ b/globalfunctions/math/QuaternionOperations.cpp @@ -0,0 +1,157 @@ +#include +#include +#include "VectorOperations.h" + +#include +#include +#include + +QuaternionOperations::~QuaternionOperations() { +} + +void QuaternionOperations::multiply(const double* q1, const double* q2, + double* q) { + double out[4]; + + out[0] = q1[3] * q2[0] + q1[2] * q2[1] - q1[1] * q2[2] + q1[0] * q2[3]; + out[1] = -q1[2] * q2[0] + q1[3] * q2[1] + q1[0] * q2[2] + q1[1] * q2[3]; + out[2] = q1[1] * q2[0] - q1[0] * q2[1] + q1[3] * q2[2] + q1[2] * q2[3]; + out[3] = -q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] + q1[3] * q2[3]; + + memcpy(q, out, 4 * sizeof(*q)); +} + +void QuaternionOperations::toDcm(const double* quaternion, double dcm[][3]) { + dcm[0][0] = 2 + * (quaternion[0] * quaternion[0] + quaternion[3] * quaternion[3]) + - 1; + dcm[0][1] = 2 + * (quaternion[0] * quaternion[1] + quaternion[2] * quaternion[3]); + dcm[0][2] = 2 + * (quaternion[0] * quaternion[2] - quaternion[1] * quaternion[3]); + + dcm[1][0] = 2 + * (quaternion[0] * quaternion[1] - quaternion[2] * quaternion[3]); + dcm[1][1] = 2 + * (quaternion[1] * quaternion[1] + quaternion[3] * quaternion[3]) + - 1; + dcm[1][2] = 2 + * (quaternion[1] * quaternion[2] + quaternion[0] * quaternion[3]); + + dcm[2][0] = 2 + * (quaternion[0] * quaternion[2] + quaternion[1] * quaternion[3]); + dcm[2][1] = 2 + * (quaternion[1] * quaternion[2] - quaternion[0] * quaternion[3]); + dcm[2][2] = 2 + * (quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3]) + - 1; +} + +void QuaternionOperations::inverse(const double* quaternion, + double* inverseQuaternion) { + memcpy(inverseQuaternion, quaternion, 4 * sizeof(*quaternion)); + VectorOperations::mulScalar(inverseQuaternion, -1, + inverseQuaternion, 3); +} + +QuaternionOperations::QuaternionOperations() { + +} + +void QuaternionOperations::normalize(const double* quaternion, + double* unitQuaternion) { + VectorOperations::normalize(quaternion, unitQuaternion, 4); +} + +float QuaternionOperations::norm(const double* quaternion) { + return VectorOperations::norm(quaternion, 4); +} + +void QuaternionOperations::fromDcm(const double dcm[][3], double* quaternion, + uint8_t *index) { + + double a[4]; + + a[0] = 1 + dcm[0][0] - dcm[1][1] - dcm[2][2]; + a[1] = 1 - dcm[0][0] + dcm[1][1] - dcm[2][2]; + a[2] = 1 - dcm[0][0] - dcm[1][1] + dcm[2][2]; + a[3] = 1 + dcm[0][0] + dcm[1][1] + dcm[2][2]; + + uint8_t maxAIndex = 0; + + VectorOperations::maxAbsValue(a, 4, &maxAIndex); + + if (index != 0) { + *index = maxAIndex; + } + + switch (maxAIndex) { + case 0: + quaternion[0] = 0.5 * sqrt(a[0]); + quaternion[1] = (dcm[0][1] + dcm[1][0]) / (2 * sqrt(a[0])); + quaternion[2] = (dcm[0][2] + dcm[2][0]) / (2 * sqrt(a[0])); + quaternion[3] = (dcm[1][2] - dcm[2][1]) / (2 * sqrt(a[0])); + break; + case 1: + quaternion[0] = (dcm[0][1] + dcm[1][0]) / (2 * sqrt(a[1])); + quaternion[1] = 0.5 * sqrt(a[1]); + quaternion[2] = (dcm[1][2] + dcm[2][1]) / (2 * sqrt(a[1])); + quaternion[3] = (dcm[2][0] - dcm[0][2]) / (2 * sqrt(a[1])); + break; + case 2: + quaternion[0] = (dcm[0][2] + dcm[2][0]) / (2 * sqrt(a[2])); + quaternion[1] = (dcm[1][2] + dcm[2][1]) / (2 * sqrt(a[2])); + quaternion[2] = 0.5 * sqrt(a[2]); + quaternion[3] = (dcm[0][1] - dcm[1][0]) / (2 * sqrt(a[2])); + break; + case 3: + quaternion[0] = (dcm[1][2] - dcm[2][1]) / (2 * sqrt(a[3])); + quaternion[1] = (dcm[2][0] - dcm[0][2]) / (2 * sqrt(a[3])); + quaternion[2] = (dcm[0][1] - dcm[1][0]) / (2 * sqrt(a[3])); + quaternion[3] = 0.5 * sqrt(a[3]); + break; + } + +} + +void QuaternionOperations::toDcm(const double* quaternion, float dcm[][3]) { + dcm[0][0] = 2 + * (quaternion[0] * quaternion[0] + quaternion[3] * quaternion[3]) + - 1; + dcm[0][1] = 2 + * (quaternion[0] * quaternion[1] + quaternion[2] * quaternion[3]); + dcm[0][2] = 2 + * (quaternion[0] * quaternion[2] - quaternion[1] * quaternion[3]); + + dcm[1][0] = 2 + * (quaternion[0] * quaternion[1] - quaternion[2] * quaternion[3]); + dcm[1][1] = 2 + * (quaternion[1] * quaternion[1] + quaternion[3] * quaternion[3]) + - 1; + dcm[1][2] = 2 + * (quaternion[1] * quaternion[2] + quaternion[0] * quaternion[3]); + + dcm[2][0] = 2 + * (quaternion[0] * quaternion[2] + quaternion[1] * quaternion[3]); + dcm[2][1] = 2 + * (quaternion[1] * quaternion[2] - quaternion[0] * quaternion[3]); + dcm[2][2] = 2 + * (quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3]) + - 1; +} + +void QuaternionOperations::normalize(double* quaternion) { + normalize(quaternion, quaternion); +} + +double QuaternionOperations::getAngle(const double* quaternion, bool abs) { + if (quaternion[3] >= 0) { + return 2 * acos(quaternion[3]); + } else { + if (abs) { + return 2 * acos(-quaternion[3]); + } else { + return -2 * acos(-quaternion[3]); + } + } +} diff --git a/globalfunctions/math/QuaternionOperations.h b/globalfunctions/math/QuaternionOperations.h new file mode 100644 index 00000000..9b3192aa --- /dev/null +++ b/globalfunctions/math/QuaternionOperations.h @@ -0,0 +1,35 @@ +#ifndef QUATERNIONOPERATIONS_H_ +#define QUATERNIONOPERATIONS_H_ + +#include + +class QuaternionOperations { +public: + virtual ~QuaternionOperations(); + + static void multiply(const double *q1, const double *q2, double *q); + + static void fromDcm(const double dcm[][3],double *quaternion, uint8_t *index = 0); + + static void toDcm(const double *quaternion, double dcm[][3]); + + static void toDcm(const double *quaternion, float dcm[][3]); + + static float norm(const double *quaternion); + + static void normalize(double *quaternion); + + static void normalize(const double *quaternion, double *unitQuaternion); + + static void inverse(const double *quaternion, double *inverseQuaternion); + + /** + * returns angle in ]-Pi;Pi] or [0;Pi] if abs == true + */ + static double getAngle(const double *quaternion, bool abs = false); + +private: + QuaternionOperations(); +}; + +#endif /* QUATERNIONOPERATIONS_H_ */ diff --git a/globalfunctions/math/VectorOperations.h b/globalfunctions/math/VectorOperations.h new file mode 100644 index 00000000..d81bdbfa --- /dev/null +++ b/globalfunctions/math/VectorOperations.h @@ -0,0 +1,83 @@ +#ifndef VECTOROPERATIONS_ +#define VECTOROPERATIONS_ + +#include +#include + +template +class VectorOperations { +public: + virtual ~VectorOperations() { + } + + static void cross(const T left[], const T right[], T out[]) { + T temp[3] = { 0, 0, 0 }; + temp[0] = left[1] * right[2] - left[2] * right[1]; + temp[1] = left[2] * right[0] - left[0] * right[2]; + temp[2] = left[0] * right[1] - left[1] * right[0]; + out[0] = temp[0]; + out[1] = temp[1]; + out[2] = temp[2]; + } + + static T dot(const T a[], const T b[]) { + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; + } + + static void mulScalar(const T vector[], T scalar, T out[], uint8_t size) { + for (; size > 0; size--) { + out[size - 1] = vector[size - 1] * scalar; + } + } + + static void add(const T vector1[], const T vector2[], T sum[], + uint8_t size = 3) { + for (; size > 0; size--) { + sum[size - 1] = vector1[size - 1] + vector2[size - 1]; + } + } + + static void subtract(const T vector1[], const T vector2[], T sum[], + uint8_t size = 3) { + for (; size > 0; size--) { + sum[size - 1] = vector1[size - 1] - vector2[size - 1]; + } + } + + static T norm(const T *vector, uint8_t size) { + T result = 0; + for (; size > 0; size--) { + result += vector[size - 1] * vector[size - 1]; + } + result = sqrt(result); + return result; + } + + static void normalize(const T *vector, T *normalizedVector, uint8_t size) { + mulScalar(vector, 1 / norm(vector, size), normalizedVector, size); + } + + static T maxAbsValue(const T *vector, uint8_t size, uint8_t *index = 0) { + + T max = -1; + + for (; size > 0; size--) { + T abs = vector[size - 1]; + if (abs < 0) { + abs = -abs; + } + if (abs > max) { + max = abs; + if (index != 0) { + *index = size - 1; + } + } + } + return max; + } + +private: + VectorOperations(); +}; + +#endif /* VECTOROPERATIONS_ */ diff --git a/globalfunctions/sign.h b/globalfunctions/sign.h new file mode 100644 index 00000000..8828fca1 --- /dev/null +++ b/globalfunctions/sign.h @@ -0,0 +1,8 @@ +#ifndef SIGN_H_ +#define SIGN_H_ + +template int sign(T val) { + return (T(0) < val) - (val < T(0)); +} + +#endif /* SIGN_H_ */ diff --git a/globalfunctions/timevalOperations.cpp b/globalfunctions/timevalOperations.cpp new file mode 100644 index 00000000..0ebd8caa --- /dev/null +++ b/globalfunctions/timevalOperations.cpp @@ -0,0 +1,96 @@ +#include +#include +#include + +//TODO test with large numbers + +timeval& operator+=(timeval& lhs, const timeval& rhs) { + int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec; + sum += rhs.tv_sec * 1000000. + rhs.tv_usec; + lhs.tv_sec = sum / 1000000; + lhs.tv_usec = sum - lhs.tv_sec * 1000000; + return lhs; +} + +timeval operator+(timeval lhs, const timeval& rhs) { + lhs += rhs; + return lhs; +} + +timeval& operator-=(timeval& lhs, const timeval& rhs) { + int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec; + sum -= rhs.tv_sec * 1000000. + rhs.tv_usec; + lhs.tv_sec = sum / 1000000; + lhs.tv_usec = sum - lhs.tv_sec * 1000000; + return lhs; +} + +timeval operator-(timeval lhs, const timeval& rhs) { + lhs -= rhs; + return lhs; +} + +double operator/(const timeval& lhs, const timeval& rhs) { + double lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; + double rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; + return lhs64 / rhs64; +} + +timeval& operator/=(timeval& lhs, double scalar) { + int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec; + product /= scalar; + lhs.tv_sec = product / 1000000; + lhs.tv_usec = product - lhs.tv_sec * 1000000; + return lhs; +} + +timeval operator/(timeval lhs, double scalar) { + lhs /= scalar; + return lhs; +} + +timeval& operator*=(timeval& lhs, double scalar) { + int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec; + product *= scalar; + lhs.tv_sec = product / 1000000; + lhs.tv_usec = product - lhs.tv_sec * 1000000; + return lhs; +} + +timeval operator*(timeval lhs, double scalar) { + lhs *= scalar; + return lhs; +} + +timeval operator*(double scalar, timeval rhs) { + rhs *= scalar; + return rhs; +} + +bool operator==(const timeval& lhs, const timeval& rhs) { + int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; + int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; + return lhs64 == rhs64; +} +bool operator!=(const timeval& lhs, const timeval& rhs) { + return !operator==(lhs, rhs); +} +bool operator<(const timeval& lhs, const timeval& rhs) { + int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; + int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; + return lhs64 < rhs64; +} +bool operator>(const timeval& lhs, const timeval& rhs) { + return operator<(rhs, lhs); +} +bool operator<=(const timeval& lhs, const timeval& rhs) { + return !operator>(lhs, rhs); +} +bool operator>=(const timeval& lhs, const timeval& rhs) { + return !operator<(lhs, rhs); +} + +double timevalOperations::toDouble(const timeval timeval) { + double result = timeval.tv_sec * 1000000. + timeval.tv_usec; + return result / 1000000.; +} diff --git a/globalfunctions/timevalOperations.h b/globalfunctions/timevalOperations.h new file mode 100644 index 00000000..674220fb --- /dev/null +++ b/globalfunctions/timevalOperations.h @@ -0,0 +1,38 @@ +#ifndef TIMEVALOPERATIONS_H_ +#define TIMEVALOPERATIONS_H_ + +#include +#include + +timeval& operator+=(timeval& lhs, const timeval& rhs); + +timeval operator+(timeval lhs, const timeval& rhs); + +timeval& operator-=(timeval& lhs, const timeval& rhs); + +timeval operator-(timeval lhs, const timeval& rhs); + +double operator/(const timeval& lhs, const timeval& rhs); + +timeval& operator/=(timeval& lhs, double scalar); + +timeval operator/(timeval lhs, double scalar); + +timeval& operator*=(timeval& lhs, double scalar); + +timeval operator*(timeval lhs, double scalar); + +timeval operator*(double scalar, timeval rhs); + +bool operator==(const timeval& lhs, const timeval& rhs); +bool operator!=(const timeval& lhs, const timeval& rhs); +bool operator<(const timeval& lhs, const timeval& rhs); +bool operator>(const timeval& lhs, const timeval& rhs); +bool operator<=(const timeval& lhs, const timeval& rhs); +bool operator>=(const timeval& lhs, const timeval& rhs); + +namespace timevalOperations { +double toDouble(const timeval timeval); +} + +#endif /* TIMEVALOPERATIONS_H_ */ diff --git a/health/HasHealthIF.h b/health/HasHealthIF.h new file mode 100644 index 00000000..12ed1cb2 --- /dev/null +++ b/health/HasHealthIF.h @@ -0,0 +1,50 @@ +#ifndef HASHEALTHIF_H_ +#define HASHEALTHIF_H_ + +#include +#include +#include + +class HasHealthIF { +public: + + typedef enum { + HEALTHY = 1, FAULTY = 0, EXTERNAL_CONTROL = 2, NEEDS_RECOVERY = 3, PERMANENT_FAULTY = 4 + } HealthState; + + static const uint8_t INTERFACE_ID = HAS_HEALTH_IF; + static const ReturnValue_t OBJECT_NOT_HEALTHY = MAKE_RETURN_CODE(1); + static const ReturnValue_t INVALID_HEALTH_STATE = MAKE_RETURN_CODE(2); + + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_MANAGER_1; + static const Event HEALTH_INFO = MAKE_EVENT(6, SEVERITY::INFO); + static const Event CHILD_CHANGED_HEALTH = MAKE_EVENT(7, SEVERITY::INFO); + static const Event CHILD_PROBLEMS = MAKE_EVENT(8, SEVERITY::LOW); + static const Event OVERWRITING_HEALTH = MAKE_EVENT(9, SEVERITY::LOW); //!< Assembly overwrites health information of children to keep satellite alive. + static const Event TRYING_RECOVERY = MAKE_EVENT(10, SEVERITY::MEDIUM); + static const Event RECOVERY_STEP = MAKE_EVENT(11, SEVERITY::MEDIUM); + static const Event RECOVERY_DONE = MAKE_EVENT(12, SEVERITY::MEDIUM); + + virtual ~HasHealthIF() { + } + + virtual MessageQueueId_t getCommandQueue() const = 0; + + /** + * set the Health State + * + * The parent will be informed, if the Health changes + * + * @param health + */ + virtual ReturnValue_t setHealth(HealthState health) = 0; + + /** + * get Health State + * + * @return Health State of the object + */ + virtual HasHealthIF::HealthState getHealth() = 0; +}; + +#endif /* HASHEALTHIF_H_ */ diff --git a/health/HealthHelper.cpp b/health/HealthHelper.cpp new file mode 100644 index 00000000..e769d2e5 --- /dev/null +++ b/health/HealthHelper.cpp @@ -0,0 +1,99 @@ +#include +#include +#include +#include +HealthHelper::HealthHelper(HasHealthIF* owner, object_id_t objectId) : + healthTable(NULL), eventSender(NULL), objectId(objectId), parentQueue( + 0), owner(owner) { +} + +HealthHelper::~HealthHelper() { +} + +ReturnValue_t HealthHelper::handleHealthCommand(CommandMessage* message) { + switch (message->getCommand()) { + case HealthMessage::HEALTH_SET: + handleSetHealthCommand(message); + return HasReturnvaluesIF::RETURN_OK; + case HealthMessage::HEALTH_ANNOUNCE: { + eventSender->forwardEvent(HasHealthIF::HEALTH_INFO, getHealth(), + getHealth()); + } + return HasReturnvaluesIF::RETURN_OK; + default: + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +HasHealthIF::HealthState HealthHelper::getHealth() { + return healthTable->getHealth(objectId); +} + +ReturnValue_t HealthHelper::initialize(MessageQueueId_t parentQueue) { + setParentQeueue(parentQueue); + return initialize(); +} + +void HealthHelper::setParentQeueue(MessageQueueId_t parentQueue) { + this->parentQueue = parentQueue; +} + +ReturnValue_t HealthHelper::initialize() { + healthTable = objectManager->get(objects::HEALTH_TABLE); + eventSender = objectManager->get(objectId); + if ((healthTable == NULL) || eventSender == NULL) { + return HasReturnvaluesIF::RETURN_FAILED; + } + ReturnValue_t result = healthTable->registerObject(objectId, + HasHealthIF::HEALTHY); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return HasReturnvaluesIF::RETURN_OK; +} + +void HealthHelper::setHealth(HasHealthIF::HealthState health) { + HasHealthIF::HealthState oldHealth = getHealth(); + eventSender->forwardEvent(HasHealthIF::HEALTH_INFO, health, oldHealth); + if (health != oldHealth) { + healthTable->setHealth(objectId, health); + informParent(health, oldHealth); + } +} + +void HealthHelper::informParent(HasHealthIF::HealthState health, + HasHealthIF::HealthState oldHealth) { + if (parentQueue == 0) { + return; + } + CommandMessage message; + MessageQueueSender sender(parentQueue); + HealthMessage::setHealthMessage(&message, HealthMessage::HEALTH_INFO, + health, oldHealth); + if (sender.sendToDefault(&message, owner->getCommandQueue()) + != HasReturnvaluesIF::RETURN_OK) { + debug << "HealthHelper::informParent: sending health reply failed." + << std::endl; + } +} + +void HealthHelper::handleSetHealthCommand(CommandMessage* message) { + ReturnValue_t result = owner->setHealth(HealthMessage::getHealth(message)); + if (message->getSender() == 0) { + return; + } + CommandMessage reply; + if (result == HasReturnvaluesIF::RETURN_OK) { + HealthMessage::setHealthMessage(&reply, + HealthMessage::REPLY_HEALTH_SET); + } else { + reply.setReplyRejected(result, message->getCommand()); + } + MessageQueueSender sender(message->getSender()); + if (sender.sendToDefault(&reply, owner->getCommandQueue()) + != HasReturnvaluesIF::RETURN_OK) { + debug + << "HealthHelper::handleHealthCommand: sending health reply failed." + << std::endl; + } +} diff --git a/health/HealthHelper.h b/health/HealthHelper.h new file mode 100644 index 00000000..a1ca2c52 --- /dev/null +++ b/health/HealthHelper.h @@ -0,0 +1,120 @@ +#ifndef HEALTHHELPER_H_ +#define HEALTHHELPER_H_ + +#include +#include +#include +#include +#include +#include +#include + +/** + * Helper class for Objects that implement HasHealthIF + * + * It takes care of registering with the Health Table as well as handling health commands + * (including replying to the sender) and updating the Health Table. + * + * If a parent is set in the ctor, the parent will be informed with a @c HEALTH_INFO message + * about changes in the health state. Note that a @c HEALTH_INFO is only generated if the Health + * changes, not for all @c HEALTH_SET commands received. + * + * It does NOT handle @c HEALTH_INFO messages + */ +class HealthHelper { +public: + + /** + * ctor + * + * @param objectId the object Id to use when communication with the HealthTable + * @param useAsFrom id to use as from id when sending replies, can be set to 0 + */ + HealthHelper(HasHealthIF* owner, object_id_t objectId); + + virtual ~HealthHelper(); + + /** + * Pointer to the Health Table + * + * only valid after initialize() has been called + */ + HealthTableIF *healthTable; + + /** + * Proxy to forward events. + */ + EventReportingProxyIF* eventSender; + + /** + * Try to handle the message. + * + * This function handles @c HEALTH_SET and @c HEALTH_READ commands. + * it updates the Health Table and generates a reply to the sender. + * + * @param message + * @return + * -@c RETURN_OK if the message was handled + * -@c RETURN_FAILED if the message could not be handled (ie it was not a @c HEALTH_SET or @c HEALTH_READ message) + */ + ReturnValue_t handleHealthCommand(CommandMessage *message); + + /** + * set the Health State + * + * The parent will be informed, if the Health changes + * + * @param health + */ + void setHealth(HasHealthIF::HealthState health); + + /** + * get Health State + * + * @return Health State of the object + */ + HasHealthIF::HealthState getHealth(); + + /** + * @param parentQueue the Queue id of the parent object. Set to 0 if no parent present + */ + void setParentQeueue(MessageQueueId_t parentQueue); + + /** + * + * @param parentQueue the Queue id of the parent object. Set to 0 if no parent present + * @return + * -@c RETURN_OK if the Health Table was found and the object could be registered + * -@c RETURN_FAILED else + */ + ReturnValue_t initialize(MessageQueueId_t parentQueue ); + + ReturnValue_t initialize(); + +private: + /** + * the object id to use when communicating with the Health Table + */ + object_id_t objectId; + + /** + * The Queue of the parent + */ + MessageQueueId_t parentQueue; + + /** + * The one using the healthHelper. + */ + HasHealthIF* owner; + + /** + * if the #parentQueue is not NULL, a @c HEALTH_INFO message will be sent to this queue + * @param health the health is passed as parameter so that the number of calls to the health table can be minimized + * @param oldHealth information of the previous health state. + */ + void informParent(HasHealthIF::HealthState health, HasHealthIF::HealthState oldHealth); + + void handleSetHealthCommand(CommandMessage *message); +}; + +#endif /* HEALTHHELPER_H_ */ diff --git a/health/HealthMessage.cpp b/health/HealthMessage.cpp new file mode 100644 index 00000000..7e33cafb --- /dev/null +++ b/health/HealthMessage.cpp @@ -0,0 +1,35 @@ +/* + * HealthMessage.cpp + * + * Created on: 15.07.2013 + * Author: tod + */ + +#include + +void HealthMessage::setHealthMessage(CommandMessage* message, Command_t command, + HasHealthIF::HealthState health, HasHealthIF::HealthState oldHealth) { + message->setCommand(command); + message->setParameter(health); + message->setParameter2(oldHealth); +} + +void HealthMessage::setHealthMessage(CommandMessage* message, Command_t command) { + message->setCommand(command); +} + +HasHealthIF::HealthState HealthMessage::getHealth(const CommandMessage* message) { + return (HasHealthIF::HealthState) message->getParameter(); +} + +void HealthMessage::clear(CommandMessage* message) { + message->setCommand(CommandMessage::CMD_NONE); +} + +HealthMessage::HealthMessage() { +} + +HasHealthIF::HealthState HealthMessage::getOldHealth( + const CommandMessage* message) { + return (HasHealthIF::HealthState) message->getParameter2(); +} diff --git a/health/HealthMessage.h b/health/HealthMessage.h new file mode 100644 index 00000000..1cad099e --- /dev/null +++ b/health/HealthMessage.h @@ -0,0 +1,37 @@ +/* + * HealthMessage.h + * + * Created on: 15.07.2013 + * Author: tod + */ + +#ifndef HEALTHMESSAGE_H_ +#define HEALTHMESSAGE_H_ + +#include +#include + +class HealthMessage { +public: + static const uint8_t MESSAGE_ID = HEALTH_COMMAND_MESSAGE_ID; + static const Command_t HEALTH_SET = MAKE_COMMAND_ID(1);//REPLY_COMMAND_OK/REPLY_REJECTED + static const Command_t HEALTH_ANNOUNCE = MAKE_COMMAND_ID(3); //NO REPLY! + static const Command_t HEALTH_INFO = MAKE_COMMAND_ID(5); + static const Command_t REPLY_HEALTH_SET = MAKE_COMMAND_ID(6); + + static void setHealthMessage(CommandMessage *message, Command_t command, + HasHealthIF::HealthState health, HasHealthIF::HealthState oldHealth = HasHealthIF::FAULTY); + + static void setHealthMessage(CommandMessage *message, Command_t command); + + static HasHealthIF::HealthState getHealth(const CommandMessage *message); + + static HasHealthIF::HealthState getOldHealth(const CommandMessage *message); + + static void clear(CommandMessage *message); + +private: + HealthMessage(); +}; + +#endif /* HEALTHMESSAGE_H_ */ diff --git a/health/HealthTable.cpp b/health/HealthTable.cpp new file mode 100644 index 00000000..a26cb906 --- /dev/null +++ b/health/HealthTable.cpp @@ -0,0 +1,101 @@ +#include +#include + +HealthTable::HealthTable(object_id_t objectid) : + SystemObject(objectid) { + mutex = new MutexId_t; + OSAL::createMutex(objectid + 1, mutex); + + mapIterator = healthMap.begin(); +} + +HealthTable::~HealthTable() { + OSAL::deleteMutex(mutex); + delete mutex; +} + +ReturnValue_t HealthTable::registerObject(object_id_t object, + HasHealthIF::HealthState initilialState) { + if (healthMap.count(object) != 0) { + return HasReturnvaluesIF::RETURN_FAILED; + } + healthMap.insert( + std::pair(object, + initilialState)); + return HasReturnvaluesIF::RETURN_OK; +} + +void HealthTable::setHealth(object_id_t object, + HasHealthIF::HealthState newState) { + OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT); + HealthMap::iterator iter = healthMap.find(object); + if (iter != healthMap.end()) { + iter->second = newState; + } + OSAL::unlockMutex(mutex); +} + +HasHealthIF::HealthState HealthTable::getHealth(object_id_t object) { + HasHealthIF::HealthState state = HasHealthIF::HEALTHY; + OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT); + HealthMap::iterator iter = healthMap.find(object); + if (iter != healthMap.end()) { + state = iter->second; + } + OSAL::unlockMutex(mutex); + return state; +} + +uint32_t HealthTable::getPrintSize() { + OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT); + uint32_t size = healthMap.size() * 5 + 2; + OSAL::unlockMutex(mutex); + return size; +} + +bool HealthTable::hasHealth(object_id_t object) { + bool exits = false; + OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT); + HealthMap::iterator iter = healthMap.find(object); + if (iter != healthMap.end()) { + exits = true; + } + OSAL::unlockMutex(mutex); + return exits; +} + +void HealthTable::printAll(uint8_t* pointer, uint32_t maxSize) { + OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT); + uint32_t size = 0; + uint16_t count = healthMap.size(); + ReturnValue_t result = SerializeAdapter::serialize(&count, + &pointer, &size, maxSize, true); + HealthMap::iterator iter; + for (iter = healthMap.begin(); + iter != healthMap.end() && result == HasReturnvaluesIF::RETURN_OK; + ++iter) { + result = SerializeAdapter::serialize(&iter->first, + &pointer, &size, maxSize, true); + uint8_t health = iter->second; + result = SerializeAdapter::serialize(&health, &pointer, &size, + maxSize, true); + } + OSAL::unlockMutex(mutex); +} + +ReturnValue_t HealthTable::iterate( + std::pair *value, bool reset) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + OSAL::lockMutex(mutex, OSAL::NO_TIMEOUT); + if (reset) { + mapIterator = healthMap.begin(); + } + if (mapIterator == healthMap.end()) { + result = HasReturnvaluesIF::RETURN_FAILED; + } + *value = *mapIterator; + mapIterator++; + OSAL::unlockMutex(mutex); + + return result; +} diff --git a/health/HealthTable.h b/health/HealthTable.h new file mode 100644 index 00000000..4df0ae9e --- /dev/null +++ b/health/HealthTable.h @@ -0,0 +1,35 @@ +#ifndef HEALTHTABLE_H_ +#define HEALTHTABLE_H_ + +#include +#include +#include +#include + +typedef std::map HealthMap; + +class HealthTable: public HealthTableIF, public SystemObject { +public: + HealthTable(object_id_t objectid); + virtual ~HealthTable(); + + virtual ReturnValue_t registerObject(object_id_t object, + HasHealthIF::HealthState initilialState = HasHealthIF::HEALTHY); + + virtual bool hasHealth(object_id_t object); + virtual void setHealth(object_id_t object, HasHealthIF::HealthState newState); + virtual HasHealthIF::HealthState getHealth(object_id_t); + + virtual uint32_t getPrintSize(); + virtual void printAll(uint8_t *pointer, uint32_t maxSize); + +protected: + MutexId_t* mutex; + HealthMap healthMap; + + HealthMap::iterator mapIterator; + + virtual ReturnValue_t iterate(std::pair *value, bool reset = false); +}; + +#endif /* HEALTHTABLE_H_ */ diff --git a/health/HealthTableIF.h b/health/HealthTableIF.h new file mode 100644 index 00000000..cca41c13 --- /dev/null +++ b/health/HealthTableIF.h @@ -0,0 +1,33 @@ +/* + * HealthTabelIF.h + * + * Created on: 15.07.2013 + * Author: tod + */ + +#ifndef HEALTHTABLEIF_H_ +#define HEALTHTABLEIF_H_ + +#include +#include +#include +#include + + +class HealthTableIF: public ManagesHealthIF { + friend class HealthCommandingService; +public: + virtual ~HealthTableIF() { + } + + virtual ReturnValue_t registerObject(object_id_t object, + HasHealthIF::HealthState initilialState = HasHealthIF::HEALTHY) = 0; + + virtual uint32_t getPrintSize() = 0; + virtual void printAll(uint8_t *pointer, uint32_t maxSize) = 0; + +protected: + virtual ReturnValue_t iterate(std::pair *value, bool reset = false) = 0; +}; + +#endif /* HEALTHTABLEIF_H_ */ diff --git a/health/ManagesHealthIF.h b/health/ManagesHealthIF.h new file mode 100644 index 00000000..6463e853 --- /dev/null +++ b/health/ManagesHealthIF.h @@ -0,0 +1,64 @@ +/* + * ManagesHealthIF.h + * + * Created on: 13.10.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_HEALTH_MANAGESHEALTHIF_H_ +#define FRAMEWORK_HEALTH_MANAGESHEALTHIF_H_ + +#include +#include +class ManagesHealthIF { +public: + virtual ~ManagesHealthIF() { + } + virtual bool hasHealth(object_id_t object) = 0; + virtual void setHealth(object_id_t object, + HasHealthIF::HealthState newState) = 0; + virtual HasHealthIF::HealthState getHealth(object_id_t) = 0; + + virtual bool isHealthy(object_id_t object) { + return (getHealth(object) == HasHealthIF::HEALTHY); + } + + virtual bool isHealthy(HasHealthIF::HealthState health) { + return (health == HasHealthIF::HEALTHY); + } + + virtual bool isFaulty(object_id_t object) { + HasHealthIF::HealthState health = getHealth(object); + return isFaulty(health); + } + + virtual bool isPermanentFaulty(object_id_t object) { + HasHealthIF::HealthState health = getHealth(object); + return isPermanentFaulty(health); + } + + virtual bool isPermanentFaulty(HasHealthIF::HealthState health) { + return (health == HasHealthIF::PERMANENT_FAULTY); + } + + virtual bool shouldBeMonitored(object_id_t object) { + HasHealthIF::HealthState health = getHealth(object); + return shouldBeMonitored(health); + } + + static bool isFaulty(HasHealthIF::HealthState health) { + return ((health == HasHealthIF::FAULTY) + || (health == HasHealthIF::PERMANENT_FAULTY) + || (health == HasHealthIF::NEEDS_RECOVERY)); + } + + virtual bool isCommandable(object_id_t object) { + return (getHealth(object) != HasHealthIF::EXTERNAL_CONTROL); + } + + virtual bool isCommandable(HasHealthIF::HealthState health) { + return (health != HasHealthIF::EXTERNAL_CONTROL); + } +}; + +#endif /* FRAMEWORK_HEALTH_MANAGESHEALTHIF_H_ */ diff --git a/ipc/CommandMessage.cpp b/ipc/CommandMessage.cpp new file mode 100644 index 00000000..cd0c82e1 --- /dev/null +++ b/ipc/CommandMessage.cpp @@ -0,0 +1,115 @@ +/** + * @file CommandMessage.cpp + * @brief This file defines the CommandMessage class. + * @date 20.06.2013 + * @author baetz + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +CommandMessage::CommandMessage() { + this->messageSize = COMMAND_MESSAGE_SIZE; + setCommand(CMD_NONE); +} + +CommandMessage::CommandMessage(Command_t command, uint32_t parameter1, + uint32_t parameter2) { + this->messageSize = COMMAND_MESSAGE_SIZE; + setCommand(command); + setParameter(parameter1); + setParameter2(parameter2); +} + +Command_t CommandMessage::getCommand() const { + Command_t command; + memcpy(&command, getData(), sizeof(Command_t)); + return command; +} + +void CommandMessage::setCommand(Command_t command) { + memcpy(getData(), &command, sizeof(command)); +} + +uint32_t CommandMessage::getParameter() const { + uint32_t parameter1; + memcpy(¶meter1, getData() + sizeof(Command_t), sizeof(parameter1)); + return parameter1; +} + +void CommandMessage::setParameter(uint32_t parameter1) { + memcpy(getData() + sizeof(Command_t), ¶meter1, sizeof(parameter1)); +} + +uint32_t CommandMessage::getParameter2() const { + uint32_t parameter2; + memcpy(¶meter2, getData() + sizeof(Command_t) + sizeof(uint32_t), + sizeof(parameter2)); + return parameter2; +} + +void CommandMessage::setParameter2(uint32_t parameter2) { + memcpy(getData() + sizeof(Command_t) + sizeof(uint32_t), ¶meter2, + sizeof(parameter2)); +} + +void CommandMessage::clearCommandMessage() { + switch((getCommand()>>8) & 0xff){ + case MODE_COMMAND_MESSAGE_ID: + ModeMessage::clear(this); + break; + case HEALTH_COMMAND_MESSAGE_ID: + HealthMessage::clear(this); + break; + case MODE_SEQUENCE_MESSAGE_ID: + ModeSequenceMessage::clear(this); + break; + case FUNCTION_MESSAGE_ID: + ActionMessage::clear(this); + break; + case DEVICE_HANDLER_COMMAND_MESSAGE_ID: + DeviceHandlerMessage::clear(this); + break; + case MEMORY_MESSAGE_ID: + MemoryMessage::clear(this); + break; + case PAYLOAD_HANDLER_MESSAGE_ID: + PayloadHandlerMessage::clear(this); + break; + case LIMIT_MESSAGE_ID: + MonitoringMessage::clear(this); + break; + case TM_STORE_MESSAGE_ID: + TmStoreMessage::clear(this); + break; + default: + setCommand(CMD_NONE); + break; + } +} + +bool CommandMessage::isClearedCommandMessage() { + return getCommand() == CMD_NONE; +} + +size_t CommandMessage::getMinimumMessageSize() const { + return COMMAND_MESSAGE_SIZE; +} + +void CommandMessage::setToUnknownCommand(Command_t initialCommand) { + setReplyRejected(UNKNOW_COMMAND, initialCommand); +} + +void CommandMessage::setReplyRejected(ReturnValue_t reason, + Command_t initialCommand) { + setCommand(REPLY_REJECTED); + setParameter(reason); + setParameter2(initialCommand); +} diff --git a/ipc/CommandMessage.h b/ipc/CommandMessage.h new file mode 100644 index 00000000..3cee803a --- /dev/null +++ b/ipc/CommandMessage.h @@ -0,0 +1,137 @@ +/** + * @file CommandMessage.h + * @brief This file defines the CommandMessage class. + * @date 20.06.2013 + * @author baetz + */ + +#ifndef COMMANDMESSAGE_H_ +#define COMMANDMESSAGE_H_ + +//Remember to add new Message Types to the clear function! +#define MODE_COMMAND_MESSAGE_ID 1 +#define HEALTH_COMMAND_MESSAGE_ID 2 +#define MODE_SEQUENCE_MESSAGE_ID 3 +#define FUNCTION_MESSAGE_ID 4 +#define TM_STORE_MESSAGE_ID 5 +#define TTC_TM_MESSAGE_ID 0x10 +#define DEVICE_HANDLER_COMMAND_MESSAGE_ID 0x44 +#define LIMIT_MESSAGE_ID 0x4C +#define MEMORY_MESSAGE_ID 0x4D +#define PAYLOAD_HANDLER_MESSAGE_ID 0x50 +#define PARAMETER_MESSAGE_ID 0x60 + + +#include + +#define MAKE_COMMAND_ID( number ) ((MESSAGE_ID << 8) + (number)) +typedef ReturnValue_t Command_t; + +class CommandMessage : public MessageQueueMessage { +public: + static const uint8_t INTERFACE_ID = COMMAND_MESSAGE; + static const ReturnValue_t UNKNOW_COMMAND = MAKE_RETURN_CODE(0x01); + + + static const uint8_t MESSAGE_ID = 0; + static const Command_t CMD_NONE = MAKE_COMMAND_ID( 0 );//!< Used internally, will be ignored + static const Command_t REPLY_COMMAND_OK = MAKE_COMMAND_ID( 3 ); + static const Command_t REPLY_REJECTED = MAKE_COMMAND_ID( 0xD1 );//!< Reply indicating that the current command was rejected, par1 should contain the error code + + /** + * This is the size of a message as it is seen by the MessageQueue + */ + static const size_t COMMAND_MESSAGE_SIZE = HEADER_SIZE + + sizeof(Command_t) + 2 * sizeof(uint32_t); + + /** + * Default Constructor, does not initialize anything. + * + * This constructor should be used when receiving a Message, as the content is filled by the MessageQueue. + */ + CommandMessage(); + /** + * This constructor creates a new message with all message content initialized + * + * @param command The DeviceHandlerCommand_t that will be sent + * @param parameter1 The first parameter + * @param parameter2 The second parameter + */ + CommandMessage(Command_t command, + uint32_t parameter1, uint32_t parameter2); + + /** + * Default Destructor + */ + virtual ~CommandMessage() { + } + + /** + * Read the DeviceHandlerCommand_t that is stored in the message, usually used after receiving + * + * @return the Command stored in the Message + */ + Command_t getCommand() const; + + /** + * Set the DeviceHandlerCOmmand_t of the message + * + * @param the Command to be sent + */ + void setCommand(Command_t command); + + /** + * Get the first parameter of the message + * + * @return the first Parameter of the message + */ + uint32_t getParameter() const; + + /** + * Set the first parameter of the message + * + * @param the first parameter of the message + */ + void setParameter(uint32_t parameter1); + + /** + * Get the second parameter of the message + * + * @return the second Parameter of the message + */ + uint32_t getParameter2() const; + + /** + * Set the second parameter of the message + * + * @param the second parameter of the message + */ + void setParameter2(uint32_t parameter2); + + /** + * Set the command to CMD_NONE and try to find + * the correct class to handle a more detailed + * clear. + * + */ + void clearCommandMessage(); + + /** + * check if a message was cleared + * + * @return if the command is CMD_NONE + */ + bool isClearedCommandMessage(); + + + /** + * Sets the command to REPLY_REJECTED with parameter UNKNOWN_COMMAND. + * Is needed quite often, so we better code it once only. + */ + void setToUnknownCommand(Command_t initialCommand); + void setReplyRejected(ReturnValue_t reason, Command_t initialCommand = CMD_NONE); + size_t getMinimumMessageSize() const; +}; + + +#endif /* COMMANDMESSAGE_H_ */ diff --git a/ipc/Makefile b/ipc/Makefile new file mode 100755 index 00000000..f13a850c --- /dev/null +++ b/ipc/Makefile @@ -0,0 +1,22 @@ +#!/bin/bash +# +# OSAL makefile +# +# Created on: Mar 04, 2010 +# Author: ziemke +# Author: Claas Ziemke +# Copyright 2010, Claas Ziemke +# + +BASEDIR=../../ +include $(BASEDIR)options.mk + +OBJ = $(BUILDDIR)/OPUSMessageQueue.o + +all: $(OBJ) + +$(BUILDDIR)/%.o: %.cpp %.h + $(CPP) $(CFLAGS) $(DEFINES) $(CCOPT) ${INCLUDE} -c $< -o $@ + +clean: + $(RM) *.o *.gcno *.gcda diff --git a/ipc/MessageProxy.cpp b/ipc/MessageProxy.cpp new file mode 100644 index 00000000..5a5a6f12 --- /dev/null +++ b/ipc/MessageProxy.cpp @@ -0,0 +1,102 @@ +/* + * MessageProxy.cpp + * + * Created on: 18.03.2015 + * Author: baetz + */ + +#include + +MessageProxy::~MessageProxy() { + +} + +MessageQueueId_t MessageProxy::getReceiver() const { + return receiver; +} + +MessageQueueId_t MessageProxy::getCommandQueue() const { + return commandQueue.getId(); +} + +void MessageProxy::setReceiver(MessageQueueId_t configuredReceiver) { + this->receiver = configuredReceiver; +} + +MessageProxy::MessageProxy(size_t queueDepth, MessageQueueId_t setReceiver) : + receiver(setReceiver), currentRequest(0), commandQueue(queueDepth) { +} + +void MessageProxy::checkCommandQueue() { + CommandMessage message; + MessageQueueId_t senderId; + ReturnValue_t result = commandQueue.receiveMessage(&message, &senderId); + if (result != HasReturnvaluesIF::RETURN_OK) { + return; + } + if (senderId != receiver) { + //It's a command. + if (currentRequest == 0) { + result = commandQueue.sendMessage(receiver, &message); + if (result == HasReturnvaluesIF::RETURN_OK) { + currentRequest = senderId; + } + } else { + result = commandFifo.insert(message); + } + if (result != HasReturnvaluesIF::RETURN_OK) { + message.clear(); + CommandMessage reply; + reply.setReplyRejected(result, message.getCommand()); + commandQueue.reply(&reply); + } + } else { + //It's a reply. + if (currentRequest != 0) { + //Failed forwarding is ignored. + commandQueue.sendMessage(currentRequest, &message); + //This request is finished. + currentRequest = 0; + //Check if there's another request in FIFO: + result = commandFifo.retrieve(&message); + if (result != HasReturnvaluesIF::RETURN_OK) { + //Nothing in FIFO. + return; + } + currentRequest = message.getSender(); + result = commandQueue.sendMessage(receiver, &message); + if (result != HasReturnvaluesIF::RETURN_OK) { + message.clear(); + CommandMessage reply; + reply.setReplyRejected(result, message.getCommand()); + commandQueue.reply(&reply); + currentRequest = 0; + } + } else { + //We don't expect a reply. Ignore. + } + } +} + +void MessageProxy::flush() { + CommandMessage command; + CommandMessage reply; + if (currentRequest != 0) { + commandQueue.sendMessage(currentRequest, &reply); + currentRequest = 0; + } + for (ReturnValue_t result = commandQueue.receiveMessage(&command); + result == HasReturnvaluesIF::RETURN_OK; + result = commandQueue.receiveMessage(&command)) { + reply.setReplyRejected(FLUSHED, command.getCommand()); + commandQueue.reply(&reply); + command.clear(); + } + for (ReturnValue_t result = commandFifo.retrieve(&command); + result == HasReturnvaluesIF::RETURN_OK; + result = commandFifo.retrieve(&command)) { + reply.setReplyRejected(FLUSHED, command.getCommand()); + commandQueue.sendMessage(command.getSender(), &reply); + command.clear(); + } +} diff --git a/ipc/MessageProxy.h b/ipc/MessageProxy.h new file mode 100644 index 00000000..88cabd0b --- /dev/null +++ b/ipc/MessageProxy.h @@ -0,0 +1,43 @@ +/* + * MessageProxy.h + * + * Created on: 18.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_IPC_MESSAGEPROXY_H_ +#define FRAMEWORK_IPC_MESSAGEPROXY_H_ + +#include +#include +#include +/** + * Simple "one message forwarding" proxy. + * Could be extended to forwarding to multiple recipients in parallel + * with a small forwarding map. + */ +class MessageProxy { +public: + MessageProxy(size_t queueDepth = DEFAULT_QUEUE_DEPTH, MessageQueueId_t setReceiver = 0); + virtual ~MessageProxy(); + MessageQueueId_t getCommandQueue() const; + MessageQueueId_t getReceiver() const; + void setReceiver(MessageQueueId_t configuredReceiver); + /** + * Checks the commandQueue for commands from other stuff or replies from the Receiver. + * There's the implicit assumption, that we get commands from anywhere, but replies only from the + * #configuredReceiver. + */ + void checkCommandQueue(); + void flush(); + static const uint8_t INTERFACE_ID = MESSAGE_PROXY; + static const ReturnValue_t FLUSHED = MAKE_RETURN_CODE(1); +private: + static const size_t DEFAULT_QUEUE_DEPTH = 5; + MessageQueueId_t receiver; + MessageQueueId_t currentRequest; + MessageQueue commandQueue; + FIFO commandFifo; //!< Required because there might be small bursts of commands coming in parallel. +}; + +#endif /* FRAMEWORK_IPC_MESSAGEPROXY_H_ */ diff --git a/ipc/MessageQueue.cpp b/ipc/MessageQueue.cpp new file mode 100644 index 00000000..40ed91cb --- /dev/null +++ b/ipc/MessageQueue.cpp @@ -0,0 +1,87 @@ +/* + * MessageQueue.cpp + * + * Created on: Oct 2, 2012 + * Author: baetz + */ + +#include +#include +#include + + +MessageQueue::MessageQueue( size_t message_depth, size_t max_message_size ) : + id(0), lastPartner(0) { + Name_t name = ('Q' << 24) + (queueCounter++ << 8); + ReturnValue_t status = OSAL::createMessageQueue(name, message_depth, max_message_size, 0, &(this->id)); + if (status != RETURN_OK) { + error << "MessageQueue::MessageQueue: Creating Queue " << std::hex << name << std::dec << " failed with status:" << (uint32_t)status << std::endl; + this->id = 0; + } else { + //Creating the MQ was successful +// if (id == 0x220100f8) { +// debug << "Queue found! " << std::endl; +// } + } +} + +MessageQueue::~MessageQueue() { + OSAL::deleteMessageQueue(&this->id); +} + +ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, + + MessageQueueMessage* message) { + return this->MessageQueueSender::sendMessage(sendTo, message, this->getId() ); +} + +ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessage* message) { + return this->MessageQueueSender::sendToDefault(message, this->getId() ); +} + +ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) { + if (this->lastPartner != 0) { + return this->MessageQueueSender::sendMessage( this->lastPartner, message, this->getId() ); + } else { + return OSAL::INCORRECT_STATE; + } +} + + +ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message, + MessageQueueId_t* receivedFrom) { + ReturnValue_t status = this->receiveMessage(message); + *receivedFrom = this->lastPartner; + return status; +} + +ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message) { + ReturnValue_t status = OSAL::receiveMessage(this->id, message->getBuffer(), message->MAX_MESSAGE_SIZE, + &(message->messageSize), OSAL::NO_WAIT, 1); + if (status == RETURN_OK) { + this->lastPartner = message->getSender(); + //Check size of incoming message. + if ( message->messageSize < message->getMinimumMessageSize() ) { + status = RETURN_FAILED; + } + } else { + //No message was received. Keep lastPartner anyway, I might send something later. + //But still, delete packet content. + memset(message->getData(),0,message->MAX_DATA_SIZE); + } + return status; +} + +MessageQueueId_t MessageQueue::getLastPartner() { + return this->lastPartner; +} + +ReturnValue_t MessageQueue::flush(uint32_t* count) { + return OSAL::flushMessageQueue(this->id, count); +} + +MessageQueueId_t MessageQueue::getId() const { + return this->id; +} + +uint16_t MessageQueue::queueCounter = 0; diff --git a/ipc/MessageQueue.h b/ipc/MessageQueue.h new file mode 100644 index 00000000..6df8cca8 --- /dev/null +++ b/ipc/MessageQueue.h @@ -0,0 +1,127 @@ +/** + * @file MessageQueue.h + * + * @date 10/02/2012 + * @author Bastian Baetz + * + * @brief This file contains the definition of the MessageQueue class. + */ + +#ifndef MESSAGEQUEUE_H_ +#define MESSAGEQUEUE_H_ + +#include + +/** + * @brief This class manages sending and receiving of message queue messages. + * + * @details Message queues are used to pass asynchronous messages between processes. + * They work like post boxes, where all incoming messages are stored in FIFO + * order. This class creates a new receiving queue and provides methods to fetch + * received messages. Being a child of MessageQueueSender, this class also provides + * methods to send a message to a user-defined or a default destination. In addition + * it also provides a reply method to answer to the queue it received its last message + * from. + * The MessageQueue should be used as "post box" for a single owning object. So all + * message queue communication is "n-to-one". + * For creating the queue, as well as sending and receiving messages, the class makes + * use of the operating system calls provided. + * \ingroup message_queue + */ +class MessageQueue : public MessageQueueSender { +private: + /** + * @brief The class stores the queue id it got assigned from the operating system in this attribute. + * If initialization fails, the queue id is set to zero. + */ + MessageQueueId_t id; + /** + * @brief In this attribute, the queue id of the last communication partner is stored + * to allow for replying. + */ + MessageQueueId_t lastPartner; + /** + * @brief The message queue's name -a user specific information for the operating system- is + * generated automatically with the help of this static counter. + */ + static uint16_t queueCounter; +public: + /** + * @brief The constructor initializes and configures the message queue. + * @details By making use of the according operating system call, a message queue is created + * and initialized. The message depth - the maximum number of messages to be + * buffered - may be set with the help of a parameter, whereas the message size is + * automatically set to the maximum message queue message size. The operating system + * sets the message queue id, or i case of failure, it is set to zero. + * @param message_depth The number of messages to be buffered before passing an error to the + * sender. Default is three. + * @param max_message_size With this parameter, the maximum message size can be adjusted. + * This should be left default. + */ + MessageQueue( size_t message_depth = 3, size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE ); + /** + * @brief The destructor deletes the formerly created message queue. + * @details This is accomplished by using the delete call provided by the operating system. + */ + virtual ~MessageQueue(); + /** + * @brief This operation sends a message to the given destination. + * @details It directly uses the sendMessage call of the MessageQueueSender parent, but passes its + * queue id as "sentFrom" parameter. + * @param sendTo This parameter specifies the message queue id of the destination message queue. + * @param message A pointer to a previously created message, which is sent. + */ + ReturnValue_t sendMessage( MessageQueueId_t sendTo, MessageQueueMessage* message ); + /** + * @brief This operation sends a message to the default destination. + * @details As in the sendMessage method, this function uses the sendToDefault call of the + * MessageQueueSender parent class and adds its queue id as "sentFrom" information. + * @param message A pointer to a previously created message, which is sent. + */ + ReturnValue_t sendToDefault( MessageQueueMessage* message ); + /** + * @brief This operation sends a message to the last communication partner. + * @details This operation simplifies answering an incoming message by using the stored + * lastParnter information as destination. If there was no message received yet + * (i.e. lastPartner is zero), an error code is returned. + * @param message A pointer to a previously created message, which is sent. + */ + ReturnValue_t reply( MessageQueueMessage* message ); + + /** + * @brief This function reads available messages from the message queue and returns the sender. + * @details It works identically to the other receiveMessage call, but in addition returns the + * sender's queue id. + * @param message A pointer to a message in which the received data is stored. + * @param receivedFrom A pointer to a queue id in which the sender's id is stored. + */ + ReturnValue_t receiveMessage(MessageQueueMessage* message, + MessageQueueId_t *receivedFrom); + + /** + * @brief This function reads available messages from the message queue. + * @details If data is available it is stored in the passed message pointer. The message's + * original content is overwritten and the sendFrom information is stored in the + * lastPartner attribute. Else, the lastPartner information remains untouched, the + * message's content is cleared and the function returns immediately. + * @param message A pointer to a message in which the received data is stored. + */ + ReturnValue_t receiveMessage(MessageQueueMessage* message); + /** + * Deletes all pending messages in the queue. + * @param count The number of flushed messages. + * @return RETURN_OK on success. + */ + ReturnValue_t flush(uint32_t* count); + /** + * @brief This method returns the message queue id of the last communication partner. + */ + MessageQueueId_t getLastPartner(); + /** + * @brief This method returns the message queue id of this class's message queue. + */ + MessageQueueId_t getId() const; + +}; + +#endif /* MESSAGEQUEUE_H_ */ diff --git a/ipc/MessageQueueMessage.cpp b/ipc/MessageQueueMessage.cpp new file mode 100644 index 00000000..a0e45baa --- /dev/null +++ b/ipc/MessageQueueMessage.cpp @@ -0,0 +1,69 @@ +/* + * MessageQueueMessage.cpp + * + * Created on: 22.11.2012 + * Author: baetz + */ +#include +#include + +MessageQueueMessage::MessageQueueMessage() : + messageSize(this->HEADER_SIZE) { + memset(this->internalBuffer, 0, sizeof(this->internalBuffer)); +} + +MessageQueueMessage::~MessageQueueMessage() { +} + +const uint8_t* MessageQueueMessage::getBuffer() const { + return this->internalBuffer; +} + +uint8_t* MessageQueueMessage::getBuffer() { + return this->internalBuffer; +} + +const uint8_t* MessageQueueMessage::getData() const { + return this->internalBuffer + this->HEADER_SIZE; +} + +uint8_t* MessageQueueMessage::getData() { + return this->internalBuffer + this->HEADER_SIZE; +} + +MessageQueueId_t MessageQueueMessage::getSender() const { + MessageQueueId_t temp_id; + memcpy(&temp_id, this->internalBuffer, sizeof(MessageQueueId_t)); + return temp_id; +} + +void MessageQueueMessage::setSender(MessageQueueId_t setId) { + memcpy(this->internalBuffer, &setId, sizeof(MessageQueueId_t)); +} + +MessageQueueMessage::MessageQueueMessage(uint8_t* data, uint32_t size) : + messageSize(this->HEADER_SIZE + size) { + if (size <= this->MAX_DATA_SIZE) { + memcpy(this->getData(), data, size); + } else { + memset(this->internalBuffer, 0, sizeof(this->internalBuffer)); + this->messageSize = this->HEADER_SIZE; + } +} + +size_t MessageQueueMessage::getMinimumMessageSize() { + return this->HEADER_SIZE; +} + +void MessageQueueMessage::print() { + debug << "MessageQueueMessage has size: " << this->messageSize << std::hex + << std::endl; + for (uint8_t count = 0; count < this->messageSize; count++) { + debug << (uint32_t) this->internalBuffer[count] << ":"; + } + debug << std::dec << std::endl; +} + +void MessageQueueMessage::clear() { + memset(this->getBuffer(), 0, this->MAX_MESSAGE_SIZE); +} diff --git a/ipc/MessageQueueMessage.h b/ipc/MessageQueueMessage.h new file mode 100644 index 00000000..9f0c812f --- /dev/null +++ b/ipc/MessageQueueMessage.h @@ -0,0 +1,122 @@ +#ifndef MESSAGEQUEUEMESSAGE_H_ +#define MESSAGEQUEUEMESSAGE_H_ + +#include + +class MessageQueue; +class MessageQueueSender; +/** + * \brief This class is the representation and data organizer for interprocess messages. + * + * \details To facilitate and standardize interprocess communication, this class was created + * to handle a lightweight "interprocess message protocol". It adds a header with the + * sender's queue id to every sent message and defines the maximum total message size. + * Specialized messages, such as device commanding messages, can be created by inheriting + * from this class and filling the buffer provided by getData with additional content. + * If larger amounts of data must be sent between processes, the data shall be stored in + * the IPC Store object and only the storage id is passed in a queue message. + * The class is used both to generate and send messages and to receive messages from + * other tasks. + * \ingroup message_queue + */ +class MessageQueueMessage { + friend class MessageQueue; + friend class MessageQueueSender; +public: + /** + * \brief This constant defines the maximum size of the data content, excluding the header. + * \details It may be changed if necessary, but in general should be kept as small as possible. + */ + static const size_t MAX_DATA_SIZE = 24; + /** + * \brief This constants defines the size of the header, which is added to every message. + */ + static const size_t HEADER_SIZE = sizeof(MessageQueueId_t); + /** + * \brief This constant defines the maximum total size in bytes of a sent message. + * \details It is the sum of the maximum data and the header size. Be aware that this constant + * is used to define the buffer sizes for every message queue in the system. So, a change + * here may have significant impact on the required resources. + */ + static const size_t MAX_MESSAGE_SIZE = MAX_DATA_SIZE + HEADER_SIZE; +private: + /** + * \brief This is the internal buffer that contains the actual message data. + */ + uint8_t internalBuffer[MAX_MESSAGE_SIZE]; +protected: + /** + * \brief This method is used to set the sender's message queue id information prior to + * sending the message. + * \param setId The message queue id that identifies the sending message queue. + */ + void setSender(MessageQueueId_t setId); + /** + * \brief This helper function is used by the MessageQueue class to check the size of an + * incoming message. + * \details The method must be overwritten by child classes if size checks shall be more strict. + * @return The default implementation returns HEADER_SIZE. + */ + virtual size_t getMinimumMessageSize(); +public: + /** + * \brief The size information of each message is stored in this attribute. + * \details It is public to simplify usage and to allow for passing the variable's address as a + * pointer. Care must be taken when inheriting from this class, as every child class is + * responsible for managing the size information by itself. When using the class to + * receive a message, the size information is updated automatically. + */ + size_t messageSize; + /** + * \brief The class is initialized empty with this constructor. + * \details The messageSize attribute is set to the header's size and the whole content is set to + * zero. + */ + MessageQueueMessage(); + /** + * \brief With this constructor the class is initialized with the given content. + * \details If the passed message size fits into the buffer, the passed data is copied to the + * internal buffer and the messageSize information is set. Otherwise, messageSize + * is set to the header's size and the whole content is set to zero. + * \param data The data to be put in the message. + * \param size Size of the data to be copied. Must be smaller than MAX_MESSAGE_SIZE. + */ + MessageQueueMessage(uint8_t* data, uint32_t size); + /** + * \brief As no memory is allocated in this class, the destructor is empty. + */ + virtual ~MessageQueueMessage(); + /** + * \brief This method is used to get the complete data of the message. + */ + const uint8_t* getBuffer() const; + /** + * \brief This method is used to get the complete data of the message. + */ + uint8_t* getBuffer(); + /** + * \brief This method is used to fetch the data content of the message. + * \details It shall be used by child classes to add data at the right position. + */ + const uint8_t* getData() const; + /** + * \brief This method is used to fetch the data content of the message. + * \details It shall be used by child classes to add data at the right position. + */ + uint8_t* getData(); + /** + * \brief This method is used to extract the sender's message queue id information from a + * received message. + */ + MessageQueueId_t getSender() const; + /** + * \brief With this method, the whole content and the message size is set to zero. + */ + void clear(); + /** + * \brief This is a debug method that prints the content (till messageSize) to the debug output. + */ + void print(); +}; + +#endif /* MESSAGEQUEUEMESSAGE_H_ */ diff --git a/ipc/MessageQueueSender.cpp b/ipc/MessageQueueSender.cpp new file mode 100644 index 00000000..854bf135 --- /dev/null +++ b/ipc/MessageQueueSender.cpp @@ -0,0 +1,56 @@ +/* + * MessageQueueSender.cpp + * + * Created on: 22.11.2012 + * Author: baetz + */ + +#include +#include +#include + + +MessageQueueSender::MessageQueueSender(MessageQueueId_t set_default_destination) : + default_destination(set_default_destination) { + //Nothing to do in ctor. +} + +void MessageQueueSender::setDefaultDestination( + MessageQueueId_t defaultDestination) { + default_destination = defaultDestination; +} + +MessageQueueSender::~MessageQueueSender() { + //Nothing to do in dtor. +} + +ReturnValue_t MessageQueueSender::sendMessage(MessageQueueId_t sendTo, + MessageQueueMessage* message, MessageQueueId_t sentFrom) { + + message->setSender(sentFrom); + ReturnValue_t result = OSAL::sendMessage(sendTo, message->getBuffer(), + message->messageSize); + if (result != RETURN_OK) { + debug << "MessageQueueSender. Sending message from " << std::hex + << sentFrom << " to " << sendTo << " failed with " << result + << std::endl; + } + return result; +} + +ReturnValue_t MessageQueueSender::sendToDefault(MessageQueueMessage* message, + MessageQueueId_t sentFrom) { + message->setSender(sentFrom); + ReturnValue_t result = OSAL::sendMessage(this->default_destination, + message->getBuffer(), message->messageSize); + if (result != RETURN_OK) { + debug << "MessageQueueSender. Sending message from " << std::hex + << sentFrom << " to " << default_destination << " failed with " + << result << std::endl; + } + return result; +} + +MessageQueueId_t MessageQueueSender::getDefaultDestination() { + return this->default_destination; +} diff --git a/ipc/MessageQueueSender.h b/ipc/MessageQueueSender.h new file mode 100644 index 00000000..61a7ea55 --- /dev/null +++ b/ipc/MessageQueueSender.h @@ -0,0 +1,79 @@ +#ifndef MESSAGEQUEUESENDER_H_ +#define MESSAGEQUEUESENDER_H_ + +#include +#include +#include +#include + +/** + * \defgroup message_queue Message Queues + * This group contains all Message Queue elements, but also the different message + * types sent over these queues. + */ +/** + * \brief This class manages sending of messages to receiving message queues. + * + * \details Message queues are a typical method of interprocess communication. + * They work like post boxes, where all incoming messages are stored in FIFO + * order. This class provides an interface to simplify sending messages to + * receiving queues without the necessity of owing a "post box" itself. It makes + * use of the underlying operating system's message queue features. + * \ingroup message_queue + */ +class MessageQueueSender : public HasReturnvaluesIF { +private: + /** + * \brief This attribute stores a default destination to send messages to. + * \details It is stored to simplify sending to always-the-same receiver. The attribute may + * be set in the constructor or by a setter call to setDefaultDestination. + */ + MessageQueueId_t default_destination; +public: + + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::OBSW_1; + static const Event SEND_MSG_FAILED = MAKE_EVENT(0, SEVERITY::LOW); + static const MessageQueueId_t NO_QUEUE = 0; + /** + * \brief In the constructor of the class, the default destination may be set. + * \details As the MessageQueueSender class has no receiving queue by itself, no + * operating system call to create a message queue is required. + * \param set_default_destination With this parameter, the default destination is set. + * If no value is provided, it is set to zero. + */ + MessageQueueSender( MessageQueueId_t set_default_destination = NO_QUEUE ); + /** + * \brief As almost nothing is done in the constructor, there's nothing done in the destructor as well. + */ + virtual ~MessageQueueSender(); + /** + * \brief With the sendMessage call, a queue message is sent to a receiving queue. + * \details This method takes the message provided, adds the sentFrom information and passes + * it on to the destination provided with an operating system call. The OS's return + * value is returned. + * \param sendTo This parameter specifies the message queue id to send the message to. + * \param message This is a pointer to a previously created message, which is sent. + * \param sentFrom The sentFrom information can be set to inject the sender's queue id into the message. + * This variable is set to zero by default. + */ + virtual ReturnValue_t sendMessage( MessageQueueId_t sendTo, MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE ); + /** + * \brief The sendToDefault method sends a queue message to the default destination. + * \details In all other aspects, it works identical to the sendMessage method. + * \param message This is a pointer to a previously created message, which is sent. + * \param sentFrom The sentFrom information can be set to inject the sender's queue id into the message. + * This variable is set to zero by default. + */ + virtual ReturnValue_t sendToDefault( MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE ); + /** + * \brief This method is a simple setter for the default destination. + */ + void setDefaultDestination(MessageQueueId_t defaultDestination); + /** + * \brief This method is a simple getter for the default destination. + */ + MessageQueueId_t getDefaultDestination(); +}; + + +#endif /* MESSAGEQUEUESENDER_H_ */ diff --git a/ipc/ReplyMessage.h b/ipc/ReplyMessage.h new file mode 100644 index 00000000..83c02f8b --- /dev/null +++ b/ipc/ReplyMessage.h @@ -0,0 +1,29 @@ +/** + * @file ReplyMessage.h + * @brief This file defines the ReplyMessage class. + * @date 20.06.2013 + * @author baetz + */ + +#ifndef REPLYMESSAGE_H_ +#define REPLYMESSAGE_H_ + +#include + +class ReplyMessage: public CommandMessage { +public: + static const uint8_t MESSAGE_ID = 1; + static const Command_t REPLY_MODE = MAKE_COMMAND_ID( 3 ); //!< Reply to a @c CMD_MODE or @c CMD_READ_MODE, getParameter contains a DeviceHandlerIF::DeviceHandlerMode_t, getParameter2 the submode + static const Command_t REPLY_TRANSITION_DELAY = MAKE_COMMAND_ID( 4 ); //!< Reply to a @c CMD_MODE, indicates that the transition will take some time, getParameter contains the maximum duration in ms + static const Command_t REPLY_INVALID_MODE = MAKE_COMMAND_ID( 5 ); //!< Reply to a @c CMD_MODE, indicates that the requested DeviceHandlerIF::DeviceHandlerMode_t was invalid + static const Command_t REPLY_CANT_REACH_MODE = MAKE_COMMAND_ID( 6 ); //!< Reply to a @c CMD_MODE, indicates that the requested DeviceHandlerIF::DeviceHandlerMode_t can not be reached from the current mode, getParameter() is the DeviceHandlerIF::DeviceHandlerMode_t, getParameter2() is the submode number + ReplyMessage() : + CommandMessage() { + } + ReplyMessage(Command_t command, uint32_t parameter1, uint32_t parameter2) : + CommandMessage(command, parameter1, parameter2) { + } + virtual ~ReplyMessage() {} +}; + +#endif /* REPLYMESSAGE_H_ */ diff --git a/memory/AcceptsMemoryMessagesIF.h b/memory/AcceptsMemoryMessagesIF.h new file mode 100644 index 00000000..22acab01 --- /dev/null +++ b/memory/AcceptsMemoryMessagesIF.h @@ -0,0 +1,21 @@ +/** + * @file AcceptsMemoryMessagesIF.h + * @brief This file defines the AcceptsMemoryMessagesIF class. + * @date 11.07.2013 + * @author baetz + */ + +#ifndef ACCEPTSMEMORYMESSAGESIF_H_ +#define ACCEPTSMEMORYMESSAGESIF_H_ + +#include +#include +#include + +class AcceptsMemoryMessagesIF : public HasMemoryIF { +public: + virtual MessageQueueId_t getCommandQueue() const = 0; +}; + + +#endif /* ACCEPTSMEMORYMESSAGESIF_H_ */ diff --git a/memory/HasMemoryIF.h b/memory/HasMemoryIF.h new file mode 100644 index 00000000..2c80d4f5 --- /dev/null +++ b/memory/HasMemoryIF.h @@ -0,0 +1,53 @@ +/* + * HasMemoryIF.h + * + * Created on: 12.07.2013 + * Author: Bastian + */ + +#ifndef HASMEMORYIF_H_ +#define HASMEMORYIF_H_ + +#include + +class HasMemoryIF { +public: + static const uint8_t INTERFACE_ID = HAS_MEMORY_IF; + static const ReturnValue_t DO_IT_MYSELF = MAKE_RETURN_CODE(1); + static const ReturnValue_t POINTS_TO_VARIABLE = MAKE_RETURN_CODE(2); + static const ReturnValue_t POINTS_TO_MEMORY = MAKE_RETURN_CODE(3); + static const ReturnValue_t ACTIVITY_COMPLETED = MAKE_RETURN_CODE(4); + static const ReturnValue_t POINTS_TO_VECTOR_UINT8 = MAKE_RETURN_CODE(5); + static const ReturnValue_t POINTS_TO_VECTOR_UINT16 = MAKE_RETURN_CODE(6); + static const ReturnValue_t POINTS_TO_VECTOR_UINT32 = MAKE_RETURN_CODE(7); + static const ReturnValue_t POINTS_TO_VECTOR_FLOAT = MAKE_RETURN_CODE(8); + static const ReturnValue_t INVALID_SIZE = MAKE_RETURN_CODE(0xE0); + static const ReturnValue_t INVALID_ADDRESS = MAKE_RETURN_CODE(0xE1); + static const ReturnValue_t INVALID_CONTENT = MAKE_RETURN_CODE(0xE2); + static const ReturnValue_t UNALIGNED_ACCESS = MAKE_RETURN_CODE(0xE3); + static const ReturnValue_t WRITE_PROTECTED = MAKE_RETURN_CODE(0xE4); +// static const ReturnValue_t TARGET_BUSY = MAKE_RETURN_CODE(0xE5); + virtual ~HasMemoryIF() {} + virtual ReturnValue_t handleMemoryLoad(uint32_t address, const uint8_t* data, uint32_t size, uint8_t** dataPointer) = 0; + virtual ReturnValue_t handleMemoryDump(uint32_t address, uint32_t size, uint8_t** dataPointer, uint8_t* dumpTarget ) = 0; + /** + * Sets the address of the memory, if possible. + * startAddress is a proposal for an address, or the base address if multiple addresses are set. + */ + virtual ReturnValue_t setAddress( uint32_t* startAddress ) { return HasReturnvaluesIF::RETURN_FAILED; } + static bool memAccessWasSuccessful(ReturnValue_t result) { + switch (result) { + case DO_IT_MYSELF: + case POINTS_TO_MEMORY: + case POINTS_TO_VARIABLE: + case HasReturnvaluesIF::RETURN_OK: + case ACTIVITY_COMPLETED: + return true; + default: + return false; + } + } +}; + + +#endif /* HASMEMORYIF_H_ */ diff --git a/memory/LocalMemory.cpp b/memory/LocalMemory.cpp new file mode 100644 index 00000000..1cc3fcda --- /dev/null +++ b/memory/LocalMemory.cpp @@ -0,0 +1,79 @@ +/* + * LocalMemory.cpp + * + * Created on: 05.11.2013 + * Author: Bastian + */ + +#include +#include +#include + +LocalMemory::LocalMemory(object_id_t setObjectId) : + SystemObject(setObjectId), commandQueue(), memoryHelper(this, + &commandQueue) { +} + +ReturnValue_t LocalMemory::performOperation() { + ReturnValue_t handleResult; + CommandMessage message; + for (ReturnValue_t result = commandQueue.receiveMessage(&message); + result == HasReturnvaluesIF::RETURN_OK; + result = commandQueue.receiveMessage(&message)) { + handleResult = memoryHelper.handleMemoryCommand(&message); + if (handleResult != HasReturnvaluesIF::RETURN_OK) { + message.setToUnknownCommand(message.getCommand()); + commandQueue.reply(&message); + } + } + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t LocalMemory::handleMemoryLoad(uint32_t address, + const uint8_t* data, uint32_t size, uint8_t** dataPointer) { + ReturnValue_t result = checkWriteAccess(address, size); + if (result == HasReturnvaluesIF::RETURN_OK) { + uint32_t value = 0; + for (uint32_t temp_address = address; temp_address < (address + size); + temp_address += 4, data += 4) { + value = (data[0] << 24) + (data[1] << 16) + (data[2] << 8) + data[3]; + *((uint32_t*) temp_address) = value; + } + } + return result; +} + +ReturnValue_t LocalMemory::handleMemoryDump(uint32_t address, uint32_t size, + uint8_t** dataPointer, uint8_t* dumpTarget) { + *dataPointer = (uint8_t*) address; + return POINTS_TO_MEMORY; +} + +ReturnValue_t LocalMemory::initialize() { + return memoryHelper.initialize(); +} + +MessageQueueId_t LocalMemory::getCommandQueue() const { + return commandQueue.getId(); +} + +ReturnValue_t LocalMemory::checkWriteAccess(uint32_t address, uint32_t size) { + + if ((address % 4) != 0) { + return UNALIGNED_ACCESS; + } + + if ((size % 4) != 0) { + return INVALID_SIZE; + } + + if (address < 0x40000000) { + HwProm prom(false); + if (prom.getPromWriteEnabled() != HwProm::WRITE_ENABLED) { + return WRITE_PROTECTED; + } + } + + return HasReturnvaluesIF::RETURN_OK; +} + diff --git a/memory/LocalMemory.h b/memory/LocalMemory.h new file mode 100644 index 00000000..695bb64f --- /dev/null +++ b/memory/LocalMemory.h @@ -0,0 +1,32 @@ +/* + * LocalMemory.h + * + * Created on: 05.11.2013 + * Author: Bastian + */ + +#ifndef LOCALMEMORY_H_ +#define LOCALMEMORY_H_ + +#include +#include +#include +#include +#include +#include +class LocalMemory : public AcceptsMemoryMessagesIF, public ExecutableObjectIF, public SystemObject { +private: + MessageQueue commandQueue; + MemoryHelper memoryHelper; + ReturnValue_t checkWriteAccess(uint32_t address, uint32_t size); +public: + LocalMemory( object_id_t setObjectId ); + ReturnValue_t performOperation(); + ReturnValue_t initialize(); + MessageQueueId_t getCommandQueue() const; + ReturnValue_t handleMemoryLoad(uint32_t address, const uint8_t* data, uint32_t size, uint8_t** dataPointer); + ReturnValue_t handleMemoryDump(uint32_t address, uint32_t size, uint8_t** dataPointer, uint8_t* dumpTarget); +}; + + +#endif /* LOCALMEMORY_H_ */ diff --git a/memory/MemoryHelper.cpp b/memory/MemoryHelper.cpp new file mode 100644 index 00000000..e5a5b446 --- /dev/null +++ b/memory/MemoryHelper.cpp @@ -0,0 +1,176 @@ +/* + * MemoryHelper.cpp + * + * Created on: 29.10.2013 + * Author: Bastian + */ + +#include +#include +#include +#include +#include + +MemoryHelper::MemoryHelper(HasMemoryIF* workOnThis, MessageQueue* useThisQueue) : + workOnThis(workOnThis), queueToUse(useThisQueue), ipcStore(NULL), ipcAddress(), lastCommand( + CommandMessage::CMD_NONE), lastSender(0), reservedSpaceInIPC( + NULL) { +} + +ReturnValue_t MemoryHelper::handleMemoryCommand(CommandMessage* message) { + lastSender = message->getSender(); + lastCommand = message->getCommand(); + switch (lastCommand) { + case MemoryMessage::CMD_MEMORY_DUMP: + handleMemoryCheckOrDump(message); + return RETURN_OK; + case MemoryMessage::CMD_MEMORY_LOAD: + handleMemoryLoad(message); + return RETURN_OK; + case MemoryMessage::CMD_MEMORY_CHECK: + handleMemoryCheckOrDump(message); + return RETURN_OK; + default: + lastCommand = CommandMessage::CMD_NONE; + return UNKNOWN_CMD; + } +} + +ReturnValue_t MemoryHelper::initialize() { + ipcStore = objectManager->get(objects::IPC_STORE); + if (ipcStore != NULL) { + return RETURN_OK; + } else { + return RETURN_FAILED; + } +} + +void MemoryHelper::completeLoad(ReturnValue_t errorCode, + const uint8_t* dataToCopy, const uint32_t size, uint8_t* copyHere) { + switch (errorCode) { + case HasMemoryIF::DO_IT_MYSELF: + return; + case HasMemoryIF::POINTS_TO_MEMORY: + memcpy(copyHere, dataToCopy, size); + break; + case HasMemoryIF::POINTS_TO_VARIABLE: + EndianSwapper::swap(copyHere, dataToCopy, size); + break; + case HasMemoryIF::ACTIVITY_COMPLETED: + case RETURN_OK: + break; + default: + ipcStore->deleteData(ipcAddress); + CommandMessage reply; + MemoryMessage::setMemoryReplyFailed(&reply, errorCode, + MemoryMessage::CMD_MEMORY_LOAD); + queueToUse->sendMessage(lastSender, &reply); + return; + } + //Only reached on success + CommandMessage reply(CommandMessage::REPLY_COMMAND_OK, 0, 0); + queueToUse->sendMessage(lastSender, &reply); + ipcStore->deleteData(ipcAddress); +} + +void MemoryHelper::completeDump(ReturnValue_t errorCode, + const uint8_t* dataToCopy, const uint32_t size) { + CommandMessage reply; + MemoryMessage::setMemoryReplyFailed(&reply, errorCode, lastCommand); + switch (errorCode) { + case HasMemoryIF::DO_IT_MYSELF: + return; + case HasReturnvaluesIF::RETURN_OK: + case HasMemoryIF::POINTS_TO_MEMORY: + case HasMemoryIF::POINTS_TO_VARIABLE: + //"data" must be valid pointer! + if (errorCode == HasMemoryIF::POINTS_TO_VARIABLE) { + EndianSwapper::swap(reservedSpaceInIPC, dataToCopy, size); + } else { + memcpy(reservedSpaceInIPC, dataToCopy, size); + } + case HasMemoryIF::ACTIVITY_COMPLETED: + switch (lastCommand) { + case MemoryMessage::CMD_MEMORY_DUMP: { + MemoryMessage::setMemoryDumpReply(&reply, ipcAddress); + break; + } + case MemoryMessage::CMD_MEMORY_CHECK: { + uint16_t crc = ::Calculate_CRC(reservedSpaceInIPC, size); + //Delete data immediately, was temporary. + ipcStore->deleteData(ipcAddress); + MemoryMessage::setMemoryCheckReply(&reply, crc); + break; + } + default: + //This should never happen! + //Is it ok to send message? Otherwise: return; + ipcStore->deleteData(ipcAddress); + reply.setParameter(STATE_MISMATCH); + break; + } + break; + default: + //Reply is already set to REJECTED. + ipcStore->deleteData(ipcAddress); + break; + } + if (queueToUse->sendMessage(lastSender, &reply) != RETURN_OK) { + reply.clearCommandMessage(); + } +} + +void MemoryHelper::swapMatrixCopy(uint8_t* out, const uint8_t *in, + uint32_t totalSize, uint8_t datatypeSize) { + if (totalSize % datatypeSize != 0){ + return; + } + + while (totalSize > 0){ + EndianSwapper::swap(out,in,datatypeSize); + out += datatypeSize; + in += datatypeSize; + totalSize -= datatypeSize; + } +} + +MemoryHelper::~MemoryHelper() { + //Nothing to destroy +} + +void MemoryHelper::handleMemoryLoad(CommandMessage* message) { + uint32_t address = MemoryMessage::getAddress(message); + ipcAddress = MemoryMessage::getStoreID(message); + const uint8_t* p_data = NULL; + uint8_t* dataPointer = NULL; + uint32_t size = 0; + ReturnValue_t returnCode = ipcStore->getData(ipcAddress, &p_data, &size); + if (returnCode == RETURN_OK) { + returnCode = workOnThis->handleMemoryLoad(address, p_data, size, + &dataPointer); + completeLoad(returnCode, p_data, size, dataPointer); + } else { + //At least inform sender. + CommandMessage reply; + MemoryMessage::setMemoryReplyFailed(&reply, returnCode, + MemoryMessage::CMD_MEMORY_LOAD); + queueToUse->sendMessage(lastSender, &reply); + } +} + +void MemoryHelper::handleMemoryCheckOrDump(CommandMessage* message) { + uint32_t address = MemoryMessage::getAddress(message); + uint32_t size = MemoryMessage::getLength(message); + uint8_t* dataPointer = NULL; + ReturnValue_t returnCode = ipcStore->getFreeElement(&ipcAddress, size, + &reservedSpaceInIPC); + if (returnCode == RETURN_OK) { + returnCode = workOnThis->handleMemoryDump(address, size, &dataPointer, + reservedSpaceInIPC); + completeDump(returnCode, dataPointer, size); + } else { + CommandMessage reply; + MemoryMessage::setMemoryReplyFailed(&reply, returnCode, lastCommand); + queueToUse->sendMessage(lastSender, &reply); + } +} diff --git a/memory/MemoryHelper.h b/memory/MemoryHelper.h new file mode 100644 index 00000000..7797ae98 --- /dev/null +++ b/memory/MemoryHelper.h @@ -0,0 +1,42 @@ +/* + * MemoryHelper.h + * + * Created on: 29.10.2013 + * Author: Bastian + */ + +#ifndef MEMORYHELPER_H_ +#define MEMORYHELPER_H_ +#include +#include +#include +#include +#include + +class MemoryHelper : public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = MEMORY_HELPER; + static const ReturnValue_t UNKNOWN_CMD = MAKE_RETURN_CODE(0xE0); + static const ReturnValue_t INVALID_ADDRESS = MAKE_RETURN_CODE(0xE1); + static const ReturnValue_t INVALID_SIZE = MAKE_RETURN_CODE(0xE2); + static const ReturnValue_t STATE_MISMATCH = MAKE_RETURN_CODE(0xE3); +private: + HasMemoryIF* workOnThis; + MessageQueue* queueToUse; + StorageManagerIF* ipcStore; + store_address_t ipcAddress; + Command_t lastCommand; + MessageQueueId_t lastSender; + uint8_t* reservedSpaceInIPC; + void handleMemoryLoad(CommandMessage* message); + void handleMemoryCheckOrDump(CommandMessage* message); +public: + ReturnValue_t handleMemoryCommand(CommandMessage* message); + void completeLoad( ReturnValue_t errorCode, const uint8_t* dataToCopy = NULL, const uint32_t size = 0, uint8_t* copyHere = NULL ); + void completeDump( ReturnValue_t errorCode, const uint8_t* dataToCopy = NULL, const uint32_t size = 0); + void swapMatrixCopy( uint8_t *out, const uint8_t *in, uint32_t totalSize, uint8_t datatypeSize); + ReturnValue_t initialize(); + MemoryHelper( HasMemoryIF* workOnThis, MessageQueue* useThisQueue ); + ~MemoryHelper(); +}; +#endif /* MEMORYHELPER_H_ */ diff --git a/memory/MemoryListAdapter.h b/memory/MemoryListAdapter.h new file mode 100644 index 00000000..02ed47a7 --- /dev/null +++ b/memory/MemoryListAdapter.h @@ -0,0 +1,49 @@ +/* + * MemoryListAdapter.h + * + * Created on: 21.03.2014 + * Author: baetz + */ + +#ifndef MEMORYLISTADAPTER_H_ +#define MEMORYLISTADAPTER_H_ + +#include +#include + +template +class MemoryListAdapter : public SinglyLinkedList, public HasMemoryIF { +public: + MemoryListAdapter(typename LinkedElement::Iterator start) : SinglyLinkedList(start) { + } + MemoryListAdapter() : SinglyLinkedList() { + } + + ReturnValue_t handleMemoryLoad(uint32_t address, const uint8_t* data, uint32_t size, uint8_t** dataPointer) { + return handleMemoryLoad(SinglyLinkedList::start, address, data, size, dataPointer); + } + + static ReturnValue_t handleMemoryLoad(LinkedElement* element, uint32_t address, const uint8_t* data, uint32_t size, uint8_t** dataPointer) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED; + while (!HasMemoryIF::memAccessWasSuccessful(result) && (element != NULL)) { + result = element->value->handleMemoryLoad(address, data, size, dataPointer); + element = element->getNext(); + } + return result; + } + + ReturnValue_t handleMemoryDump(uint32_t address, uint32_t size, uint8_t** dataPointer, uint8_t* dumpTarget ) { + return handleMemoryDump(SinglyLinkedList::start, address, size, dataPointer, dumpTarget); + } + static ReturnValue_t handleMemoryDump(LinkedElement* element, uint32_t address, uint32_t size, uint8_t** dataPointer, uint8_t* dumpTarget ) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED; + while (!HasMemoryIF::memAccessWasSuccessful(result) && (element != NULL)) { + result = element->value->handleMemoryDump(address, size, dataPointer, dumpTarget); + element = element->getNext(); + } + return result; + } +}; + + +#endif /* MEMORYLISTADAPTER_H_ */ diff --git a/memory/MemoryMessage.cpp b/memory/MemoryMessage.cpp new file mode 100644 index 00000000..a39d3615 --- /dev/null +++ b/memory/MemoryMessage.cpp @@ -0,0 +1,104 @@ +/* + * MemoryMessage.cpp + * + * Created on: 17.07.2013 + * Author: Bastian + */ + + +#include +#include +MemoryMessage::MemoryMessage() { +} + +uint32_t MemoryMessage::getAddress(const CommandMessage* message) { + return message->getParameter(); +} + +store_address_t MemoryMessage::getStoreID(const CommandMessage* message) { + store_address_t temp; + temp.raw = message->getParameter2(); + return temp; +} + +uint32_t MemoryMessage::getLength(const CommandMessage* message) { + return message->getParameter2(); +} + +ReturnValue_t MemoryMessage::setMemoryDumpCommand(CommandMessage* message, + uint32_t address, uint32_t length) { + message->setCommand(CMD_MEMORY_DUMP); + message->setParameter( address ); + message->setParameter2( length ); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MemoryMessage::setMemoryDumpReply(CommandMessage* message, store_address_t storageID) { + message->setCommand(REPLY_MEMORY_DUMP); + message->setParameter2( storageID.raw ); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MemoryMessage::setMemoryLoadCommand(CommandMessage* message, + uint32_t address, store_address_t storageID) { + message->setCommand(CMD_MEMORY_LOAD); + message->setParameter( address ); + message->setParameter2( storageID.raw ); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MemoryMessage::getErrorCode(const CommandMessage* message) { + return message->getParameter(); +} + +void MemoryMessage::clear(CommandMessage* message) { + switch (message->getCommand()) { + case CMD_MEMORY_LOAD: + case REPLY_MEMORY_DUMP: { + StorageManagerIF *ipcStore = objectManager->get( + objects::IPC_STORE); + if (ipcStore != NULL) { + ipcStore->deleteData(getStoreID(message)); + } + } + /* NO BREAK*/ + case CMD_MEMORY_DUMP: + case CMD_MEMORY_CHECK: + case REPLY_MEMORY_CHECK: + message->setCommand(CommandMessage::CMD_NONE); + message->setParameter(0); + message->setParameter2(0); + break; + } +} + +ReturnValue_t MemoryMessage::setMemoryCheckCommand(CommandMessage* message, + uint32_t address, uint32_t length) { + message->setCommand(CMD_MEMORY_CHECK); + message->setParameter( address ); + message->setParameter2( length ); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MemoryMessage::setMemoryCheckReply(CommandMessage* message, + uint16_t crc) { + message->setCommand(REPLY_MEMORY_CHECK); + message->setParameter( crc ); + return HasReturnvaluesIF::RETURN_OK; +} + +uint16_t MemoryMessage::getCrc(const CommandMessage* message) { + return (uint16_t)(message->getParameter()); +} + +Command_t MemoryMessage::getInitialCommand(const CommandMessage* message) { + return message->getParameter2(); +} + +ReturnValue_t MemoryMessage::setMemoryReplyFailed(CommandMessage* message, + ReturnValue_t errorCode, Command_t initialCommand) { + message->setCommand(REPLY_MEMORY_FAILED); + message->setParameter(errorCode); + message->setParameter2(initialCommand); + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/memory/MemoryMessage.h b/memory/MemoryMessage.h new file mode 100644 index 00000000..eb4e541d --- /dev/null +++ b/memory/MemoryMessage.h @@ -0,0 +1,42 @@ +/* + * MemoryMessage.h + * + * Created on: 17.07.2013 + * Author: Bastian + */ + +#ifndef MEMORYMESSAGE_H_ +#define MEMORYMESSAGE_H_ + +#include +#include + + +class MemoryMessage { +private: + MemoryMessage(); //A private ctor inhibits instantiation +public: + static const uint8_t MESSAGE_ID = MEMORY_MESSAGE_ID; + static const Command_t CMD_MEMORY_LOAD = MAKE_COMMAND_ID( 0x01 ); + static const Command_t CMD_MEMORY_DUMP = MAKE_COMMAND_ID( 0x02 ); + static const Command_t CMD_MEMORY_CHECK = MAKE_COMMAND_ID( 0x03 ); + static const Command_t REPLY_MEMORY_DUMP = MAKE_COMMAND_ID( 0x10 ); + static const Command_t REPLY_MEMORY_CHECK = MAKE_COMMAND_ID( 0x30 ); + static const Command_t REPLY_MEMORY_FAILED = MAKE_COMMAND_ID( 0xE0 ); + + static uint32_t getAddress( const CommandMessage* message ); + static store_address_t getStoreID( const CommandMessage* message ); + static uint32_t getLength( const CommandMessage* message ); + static ReturnValue_t getErrorCode( const CommandMessage* message ); + static ReturnValue_t setMemoryDumpCommand( CommandMessage* message, uint32_t address, uint32_t length ); + static ReturnValue_t setMemoryDumpReply( CommandMessage* message, store_address_t storageID ); + static ReturnValue_t setMemoryLoadCommand( CommandMessage* message, uint32_t address, store_address_t storageID ); + static ReturnValue_t setMemoryCheckCommand( CommandMessage* message, uint32_t address, uint32_t length ); + static ReturnValue_t setMemoryCheckReply( CommandMessage* message, uint16_t crc ); + static ReturnValue_t setMemoryReplyFailed( CommandMessage* message, ReturnValue_t errorCode, Command_t initialCommand ); + static uint16_t getCrc( const CommandMessage* message ); + static Command_t getInitialCommand( const CommandMessage* message ); + static void clear(CommandMessage* message); +}; + +#endif /* MEMORYMESSAGE_H_ */ diff --git a/memory/MemoryProxyIF.h b/memory/MemoryProxyIF.h new file mode 100644 index 00000000..69db64b8 --- /dev/null +++ b/memory/MemoryProxyIF.h @@ -0,0 +1,29 @@ +/* + * MemoryProxyIF.h + * + * Created on: 18.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_MEMORY_MEMORYPROXYIF_H_ +#define FRAMEWORK_MEMORY_MEMORYPROXYIF_H_ + +#include + +/** + * This was a nice idea to transparently forward incoming messages to another object. + * But it doesn't work like that. + */ +class MemoryProxyIF : public AcceptsMemoryMessagesIF { +public: + virtual MessageQueueId_t getProxyQueue() const = 0; + MessageQueueId_t getCommandQueue() const { + return getProxyQueue(); + } + virtual ~MemoryProxyIF() {} + +}; + + + +#endif /* FRAMEWORK_MEMORY_MEMORYPROXYIF_H_ */ diff --git a/modes/HasModesIF.h b/modes/HasModesIF.h new file mode 100644 index 00000000..4c9716f5 --- /dev/null +++ b/modes/HasModesIF.h @@ -0,0 +1,58 @@ +/** + * @file HasModesIF.h + * @brief This file defines the HasModesIF class. + * @date 20.06.2013 + * @author baetz + */ + +#ifndef HASMODESIF_H_ +#define HASMODESIF_H_ + +#include +#include +#include +#include +#include + + +class HasModesIF { + friend class ModeHelper; +public: + static const uint8_t INTERFACE_ID = HAS_MODES_IF; + static const ReturnValue_t INVALID_MODE = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t TRANS_NOT_ALLOWED = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t IN_TRANSITION = MAKE_RETURN_CODE(0x03); + static const ReturnValue_t INVALID_SUBMODE = MAKE_RETURN_CODE(0x04); + + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_MANAGER; + static const Event CHANGING_MODE = MAKE_EVENT(0, SEVERITY::INFO); //!< An object announces changing the mode. p1: target mode. p2: target submode + static const Event MODE_INFO = MAKE_EVENT(1, SEVERITY::INFO); //!< An Object announces its mode; parameter1 is mode, parameter2 is submode + static const Event FALLBACK_FAILED = MAKE_EVENT(2, SEVERITY::HIGH); + static const Event MODE_TRANSITION_FAILED = MAKE_EVENT(3, SEVERITY::LOW); + static const Event CANT_KEEP_MODE = MAKE_EVENT(4, SEVERITY::HIGH); + static const Event OBJECT_IN_INVALID_MODE = MAKE_EVENT(5, SEVERITY::LOW); //!< Indicates a bug or configuration failure: Object is in a mode it should never be in. + static const Event FORCING_MODE = MAKE_EVENT(6, SEVERITY::MEDIUM); //!< The mode is changed, but for some reason, the change is forced, i.e. EXTERNAL_CONTROL ignored. p1: target mode. p2: target submode + static const Event MODE_CMD_REJECTED = MAKE_EVENT(7, SEVERITY::LOW); //!< A mode command was rejected by the called object. Par1: called object id, Par2: return code. + + static const Mode_t MODE_ON = 1; //!< The device is powered and ready to perform operations. In this mode, no commands are sent by the device handler itself, but direct commands van be commanded and will be interpreted + static const Mode_t MODE_OFF = 0; //!< The device is powered off. The only command accepted in this mode is a mode change to on. + static const Submode_t SUBMODE_NONE = 0; //!< To avoid checks against magic number "0". + virtual ~HasModesIF() { + } + virtual MessageQueueId_t getCommandQueue() const = 0; + virtual void getMode(Mode_t *mode, Submode_t *submode) { + } +protected: + virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode) { + return HasReturnvaluesIF::RETURN_FAILED; + } + virtual void startTransition(Mode_t mode, Submode_t submode) { + } + virtual void setToExternalControl() { + } + virtual void announceMode(bool recursive) { + } +}; + +#endif /* HASMODESIF_H_ */ diff --git a/modes/ModeHelper.cpp b/modes/ModeHelper.cpp new file mode 100644 index 00000000..2b4894b2 --- /dev/null +++ b/modes/ModeHelper.cpp @@ -0,0 +1,118 @@ +#include +#include + +ModeHelper::ModeHelper(HasModesIF *owner) : + theOneWhoCommandedAMode(0), commandedMode(HasModesIF::MODE_OFF), commandedSubmode( + HasModesIF::SUBMODE_NONE), owner(owner), parentQueueId(0), forced( + false) { +} + +ModeHelper::~ModeHelper() { + +} + +ReturnValue_t ModeHelper::handleModeCommand(CommandMessage* message) { + CommandMessage reply; + Mode_t mode; + Submode_t submode; + switch (message->getCommand()) { + case ModeMessage::CMD_MODE_COMMAND_FORCED: + forced = true; + /*NO BREAK*/ + case ModeMessage::CMD_MODE_COMMAND: { + mode = ModeMessage::getMode(message); + submode = ModeMessage::getSubmode(message); + uint32_t timeout; + ReturnValue_t result = owner->checkModeCommand(mode, submode, &timeout); + if (result != HasReturnvaluesIF::RETURN_OK) { + ModeMessage::cantReachMode(&reply, result); + sender.sendMessage(message->getSender(), &reply, + owner->getCommandQueue()); + break; + } + //Free to start transition + theOneWhoCommandedAMode = message->getSender(); + commandedMode = mode; + commandedSubmode = submode; + + if ((parentQueueId != MessageQueueSender::NO_QUEUE) + && (theOneWhoCommandedAMode != parentQueueId)) { + owner->setToExternalControl(); + } + + countdown.setTimeout(timeout); + + owner->startTransition(mode, submode); + } + break; + case ModeMessage::CMD_MODE_READ: { + owner->getMode(&mode, &submode); + ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_REPLY, mode, + submode); + sender.sendMessage(message->getSender(), &reply, + owner->getCommandQueue()); + } + break; + case ModeMessage::CMD_MODE_ANNOUNCE: + owner->announceMode(false); + break; + case ModeMessage::CMD_MODE_ANNOUNCE_RECURSIVELY: + owner->announceMode(true); + break; + default: + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t ModeHelper::initialize(MessageQueueId_t parentQueueId) { + setParentQueue(parentQueueId); + return initialize(); +} + +void ModeHelper::modeChanged(Mode_t mode, Submode_t submode) { + forced = false; + CommandMessage reply; + if (theOneWhoCommandedAMode != MessageQueueSender::NO_QUEUE) { + if ((mode != commandedMode) || (submode != commandedSubmode)) { + ModeMessage::setModeMessage(&reply, + ModeMessage::REPLY_WRONG_MODE_REPLY, mode, submode); + } else { + ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_REPLY, + mode, submode); + } + sender.sendMessage(theOneWhoCommandedAMode, &reply, + owner->getCommandQueue()); + } + if (theOneWhoCommandedAMode != parentQueueId + && parentQueueId != MessageQueueSender::NO_QUEUE) { + ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_INFO, mode, + submode); + sender.sendMessage(parentQueueId, &reply, owner->getCommandQueue()); + } + theOneWhoCommandedAMode = MessageQueueSender::NO_QUEUE; +} + +void ModeHelper::startTimer(uint32_t timeoutMs) { + countdown.setTimeout(timeoutMs); +} + +void ModeHelper::setParentQueue(MessageQueueId_t parentQueueId) { + this->parentQueueId = parentQueueId; +} + +ReturnValue_t ModeHelper::initialize(void) { + return HasReturnvaluesIF::RETURN_OK; +} + +bool ModeHelper::isTimedOut() { + return countdown.hasTimedOut(); +} + +bool ModeHelper::isForced() { + return forced; +} + +void ModeHelper::setForced(bool forced) { + this->forced = forced; +} diff --git a/modes/ModeHelper.h b/modes/ModeHelper.h new file mode 100644 index 00000000..6cff1858 --- /dev/null +++ b/modes/ModeHelper.h @@ -0,0 +1,51 @@ +#ifndef MODEHELPER_H_ +#define MODEHELPER_H_ + +#include +#include +#include +#include + +class HasModesIF; + +class ModeHelper { +public: + MessageQueueId_t theOneWhoCommandedAMode; + Mode_t commandedMode; + Submode_t commandedSubmode; + + ModeHelper(HasModesIF *owner); + virtual ~ModeHelper(); + + ReturnValue_t handleModeCommand(CommandMessage *message); + + /** + * + * @param parentQueue the Queue id of the parent object. Set to 0 if no parent present + */ + void setParentQueue(MessageQueueId_t parentQueueId); + + ReturnValue_t initialize(MessageQueueId_t parentQueueId); + + ReturnValue_t initialize(void); //void is there to stop eclipse CODAN from falsely reporting an error + + void modeChanged(Mode_t mode, Submode_t submode); + + void startTimer(uint32_t timeoutMs); + + bool isTimedOut(); + + bool isForced(); + + void setForced(bool forced); +protected: + HasModesIF *owner; + MessageQueueId_t parentQueueId; + + Countdown countdown; + MessageQueueSender sender; + + bool forced; +}; + +#endif /* MODEHELPER_H_ */ diff --git a/modes/ModeMessage.cpp b/modes/ModeMessage.cpp new file mode 100644 index 00000000..fc0f3aca --- /dev/null +++ b/modes/ModeMessage.cpp @@ -0,0 +1,34 @@ +/* + * ModeMessage.cpp + * + * Created on: 12.07.2013 + * Author: tod + */ + +#include + +Mode_t ModeMessage::getMode(const CommandMessage* message) { + return message->getParameter(); +} + +Submode_t ModeMessage::getSubmode(const CommandMessage* message) { + return message->getParameter2(); +} + +ReturnValue_t ModeMessage::setModeMessage(CommandMessage* message, Command_t command, + Mode_t mode, Submode_t submode) { + message->setCommand( command ); + message->setParameter( mode ); + message->setParameter2( submode ); + return HasReturnvaluesIF::RETURN_OK; +} + +void ModeMessage::clear(CommandMessage* message) { + message->setCommand(CommandMessage::CMD_NONE); +} + +void ModeMessage::cantReachMode(CommandMessage* message, ReturnValue_t reason) { + message->setCommand(REPLY_CANT_REACH_MODE); + message->setParameter(reason); + message->setParameter2(0); +} diff --git a/modes/ModeMessage.h b/modes/ModeMessage.h new file mode 100644 index 00000000..c2ebbf24 --- /dev/null +++ b/modes/ModeMessage.h @@ -0,0 +1,40 @@ +/** + * @file ModeMessage.h + * @brief This file defines the ModeMessage class. + * @date 17.07.2013 + * @author baetz + */ + +#ifndef MODEMESSAGE_H_ +#define MODEMESSAGE_H_ + +#include + +typedef uint32_t Mode_t; +typedef uint8_t Submode_t; + +class ModeMessage { +private: + ModeMessage(); +public: + static const uint8_t MESSAGE_ID = MODE_COMMAND_MESSAGE_ID; + static const Command_t CMD_MODE_COMMAND = MAKE_COMMAND_ID(0x01);//!> Command to set the specified Mode, replies are: REPLY_MODE_REPLY, REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any replies, as this will break the subsystem mode machine!! + static const Command_t CMD_MODE_COMMAND_FORCED = MAKE_COMMAND_ID(0xF1);//!> Command to set the specified Mode, regardless of external control flag, replies are: REPLY_MODE_REPLY, REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any replies, as this will break the subsystem mode machine!! + static const Command_t REPLY_MODE_REPLY = MAKE_COMMAND_ID(0x02);//!> Reply to a CMD_MODE_COMMAND or CMD_MODE_READ + static const Command_t REPLY_MODE_INFO = MAKE_COMMAND_ID(0x03); //!> Unrequested info about the current mode (used for composites to inform their container of a changed mode) + static const Command_t REPLY_CANT_REACH_MODE = MAKE_COMMAND_ID(0x04); //!> Reply in case a mode command can't be executed. Par1: returnCode, Par2: 0 + //TODO is there a way we can transmit a returnvalue when responding that the mode is wrong, so we can give a nice failure code when commanded by PUS? + static const Command_t REPLY_WRONG_MODE_REPLY = MAKE_COMMAND_ID(0x05);//!> Reply to a CMD_MODE_COMMAND, indicating that a mode was commanded and a transition started but was aborted; the parameters contain the mode that was reached + static const Command_t CMD_MODE_READ = MAKE_COMMAND_ID(0x06);//!> Command to read the current mode and reply with a REPLY_MODE_REPLY + static const Command_t CMD_MODE_ANNOUNCE = MAKE_COMMAND_ID(0x07);//!> Command to trigger an ModeInfo Event. This command does NOT have a reply. + static const Command_t CMD_MODE_ANNOUNCE_RECURSIVELY = MAKE_COMMAND_ID(0x08);//!> Command to trigger an ModeInfo Event and to send this command to every child. This command does NOT have a reply. + + static Mode_t getMode(const CommandMessage* message); + static Submode_t getSubmode(const CommandMessage* message); + static ReturnValue_t setModeMessage(CommandMessage* message, + Command_t command, Mode_t mode, Submode_t submode); + static void cantReachMode(CommandMessage* message, ReturnValue_t reason); + static void clear(CommandMessage* message); +}; + +#endif /* MODEMESSAGE_H_ */ diff --git a/monitoring/AbsLimitMonitor.h b/monitoring/AbsLimitMonitor.h new file mode 100644 index 00000000..88bdcaac --- /dev/null +++ b/monitoring/AbsLimitMonitor.h @@ -0,0 +1,75 @@ + +#ifndef FRAMEWORK_MONITORING_ABSLIMITMONITOR_H_ +#define FRAMEWORK_MONITORING_ABSLIMITMONITOR_H_ + +#include +#include + +template +class AbsLimitMonitor: public MonitorBase { +public: + AbsLimitMonitor(object_id_t reporterId, uint8_t monitorId, uint32_t parameterId, + uint16_t confirmationLimit, T limit, Event violationEvent = MonitoringIF::VALUE_OUT_OF_RANGE, bool aboveIsViolation = true) : + MonitorBase(reporterId, monitorId, parameterId, confirmationLimit), limit(limit), violationEvent(violationEvent), aboveIsViolation(aboveIsViolation) { + } + virtual ~AbsLimitMonitor() { + } + virtual ReturnValue_t checkSample(T sample, T* crossedLimit) { + if (aboveIsViolation) { + if ((std::abs(sample) > limit)) { + *crossedLimit = limit; + return MonitoringIF::OUT_OF_RANGE; + } + } else { + if ((std::abs(sample) < limit)) { + *crossedLimit = limit; + return MonitoringIF::OUT_OF_RANGE; + } + } + return HasReturnvaluesIF::RETURN_OK; //We're not out of range. + } + + virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, + ParameterWrapper *parameterWrapper, + const ParameterWrapper *newValues, uint16_t startAtIndex) { + ReturnValue_t result = this->MonitorBase::getParameter(domainId, + parameterId, parameterWrapper, newValues, startAtIndex); + //We'll reuse the DOMAIN_ID of MonitorReporter, as we know the parameterIds used there. + if (result != this->INVALID_MATRIX_ID) { + return result; + } + switch (parameterId) { + case 10: + parameterWrapper->set(this->limit); + break; + default: + return this->INVALID_MATRIX_ID; + } + return HasReturnvaluesIF::RETURN_OK; + } + bool isOutOfLimits() { + if (this->oldState == MonitoringIF::OUT_OF_RANGE) { + return true; + } else { + return false; + } + } + void setLimit(T value) { + limit = value; + } +protected: + void sendTransitionEvent(T currentValue, ReturnValue_t state) { + switch (state) { + case MonitoringIF::OUT_OF_RANGE: + EventManagerIF::triggerEvent(this->reportingId, violationEvent, this->parameterId); + break; + default: + break; + } + } + T limit; + const Event violationEvent; + const bool aboveIsViolation; +}; + +#endif /* FRAMEWORK_MONITORING_ABSLIMITMONITOR_H_ */ diff --git a/monitoring/DeltaCheckMonitor.h b/monitoring/DeltaCheckMonitor.h new file mode 100644 index 00000000..34f2f338 --- /dev/null +++ b/monitoring/DeltaCheckMonitor.h @@ -0,0 +1,129 @@ +/* + * DeltaCheckMonitor.h + * + * Created on: 21.07.2014 + * Author: baetz + */ + +#ifndef DELTACHECKMONITOR_H_ +#define DELTACHECKMONITOR_H_ + +#include +#include +#include +#include +#include +#include +#include + + + +//TODO: Lots of old, unfixed stuff. Do not use! +template +class DeltaCheckMonitor: public MonitoringIF { +public: + DeltaCheckMonitor(HasMonitorsIF* owner, uint32_t poolId, + T initialMinDelta, T initialMaxDelta, uint8_t arrayPos = 0, + uint8_t initialStrategy = REPORT_ALL, Event eventToTrigger = 0) : + owner(owner), parameter(poolId, NULL, arrayPos), oldValue( + 0), comparisonValueValid(false), minDelta(initialMinDelta), maxDelta( + initialMaxDelta), reportingStrategy(initialStrategy), checkingState( + CHECKING_STATUS_OK), event(eventToTrigger) { + initialMinDelta.setNext(&initialMaxDelta); + } + virtual ~DeltaCheckMonitor() { + + } + ReturnValue_t check() { + DataSet mySet; + mySet.registerVariable(¶meter); + mySet.read(); + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + uint8_t oldState = 0; + if (!parameter.isValid()) { + checkingState = CHECKING_STATUS_INVALID; + comparisonValueValid = false; + result = PARAMETER_INVALID; + } else { + if (!comparisonValueValid) { + oldValue = parameter; + comparisonValueValid = true; + return FIRST_SAMPLE; + } + oldState = checkingState; + if ((parameter.value - oldValue) > maxDelta.entry) { + checkingState = CHECKING_STATUS_ABOVE_HIGH_THRESHOLD; + result = HIGH_LIMIT; + } else if ((parameter.value - oldValue) < minDelta.entry) { + checkingState = CHECKING_STATUS_BELOW_LOW_THRESHOLD; + result = LOW_LIMIT; + } else { + checkingState = CHECKING_STATUS_OK; + } + if (oldState != checkingState) { + reportViolation(oldState); + } + } + if (reportingStrategy == REPORT_NONE) { + checkingState = CHECKING_STATUS_UNSELECTED; + } + return result; + } + ReturnValue_t setLimits(uint8_t type, const uint8_t* data, uint32_t size) { + if (type != getLimitType()) { + return WRONG_TYPE; + } + UpdateLimitMonitorContent content; + int32_t tempSize = size; + const uint8_t* pBuffer = data; + ReturnValue_t result = content.deSerialize(&pBuffer, &tempSize, true); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + minDelta = content.lowValue.entry; + maxDelta = content.highValue.entry; + return HasReturnvaluesIF::RETURN_OK; + } + ReturnValue_t setChecking(uint8_t strategy) { + reportingStrategy = strategy; + return HasReturnvaluesIF::RETURN_OK; + } + uint32_t getPID() { + return parameter.getParameterId(); + } + const uint8_t getLimitType() const { + return LIMIT_TYPE_DELTA_CHECK; + } +private: + HasMonitorsIF* owner; + PIDReader parameter; + T oldValue; + bool comparisonValueValid; + SerializeElement minDelta; + SerializeElement maxDelta; + uint8_t reportingStrategy; + uint8_t checkingState; + EventId_t event; + void reportViolation(uint8_t oldState) { + if ((reportingStrategy & REPORT_REPORTS_ONLY) != 0) { + if (checkingState == CHECKING_STATUS_ABOVE_HIGH_THRESHOLD) { + MonitoringReportContent report(parameter.getParameterId(), + parameter.value, maxDelta.entry, oldState, + checkingState); + LimitViolationReporter::sendLimitViolationReport(&report); + } else { + MonitoringReportContent report(parameter.getParameterId(), + parameter.value, minDelta.entry, oldState, + checkingState); + LimitViolationReporter::sendLimitViolationReport(&report); + }; + } + if ((this->reportingStrategy & REPORT_EVENTS_ONLY) != 0) { + if (checkingState == CHECKING_STATUS_ABOVE_HIGH_THRESHOLD || checkingState == CHECKING_STATUS_BELOW_LOW_THRESHOLD) { + owner->forwardEvent(event, (oldState << 8) + checkingState); + } + } + } +}; + +#endif /* DELTACHECKMONITOR_H_ */ diff --git a/monitoring/HasMonitorsIF.h b/monitoring/HasMonitorsIF.h new file mode 100644 index 00000000..d6d39d86 --- /dev/null +++ b/monitoring/HasMonitorsIF.h @@ -0,0 +1,30 @@ +/** + * @file HasMonitorsIF.h + * @brief This file defines the HasMonitorsIF class. + * @date 28.07.2014 + * @author baetz + */ +#ifndef HASMONITORSIF_H_ +#define HASMONITORSIF_H_ + +#include +#include +#include + +class HasMonitorsIF { +public: + static const uint8_t MAX_N_PARAMETER = 10; +// static const uint8_t MAX_N_LIMIT_ID = 10; + virtual ReturnValue_t setCheckingOfParameters(uint8_t checkingStrategy, + bool forOnePid = false, uint32_t parameterId = 0) = 0; + virtual ReturnValue_t modifyParameterMonitor(uint8_t limitType, + uint32_t parameterId, const uint8_t* data, uint32_t size) = 0; + virtual ReturnValue_t modifyObjectMonitor(uint32_t objectId, + const uint8_t* data, const uint32_t size) = 0; + virtual void setAllMonitorsToUnchecked() = 0; + virtual MessageQueueId_t getCommandQueue() const = 0; + virtual ~HasMonitorsIF() { + } +}; + +#endif /* HASMONITORSIF_H_ */ diff --git a/monitoring/LimitCheckMonitor.h b/monitoring/LimitCheckMonitor.h new file mode 100644 index 00000000..1ab78b87 --- /dev/null +++ b/monitoring/LimitCheckMonitor.h @@ -0,0 +1,271 @@ +/* + * LimitCheckMonitor.h + * + * Created on: 21.07.2014 + * Author: baetz + */ + +#ifndef LIMITCHECKMONITOR_H_ +#define LIMITCHECKMONITOR_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +template +class LimitCheckMonitor: public MonitoringIF { +public: + struct CheckParameters { + + CheckParameters() : + oldState(HasReturnvaluesIF::RETURN_OK), parameterId( + PoolVariableIF::NO_PARAMETER), sendTransitionreport( + false), sendTransitionEvent(false), lowerTransitionEvent( + 0), upperTransitionEvent(0), reportingObject(0), currentCounter( + 0), maxCounter(0) { + } + + CheckParameters(ReturnValue_t oldState, const uint32_t parameterId, + bool sendTransitionreport, bool sendTransitionEvent, + Event lowerTransitionEvent, Event upperTransitionEvent, + const object_id_t reportingObject, uint16_t maxCounter = 0) : + oldState(oldState), parameterId(parameterId), sendTransitionreport( + sendTransitionreport), sendTransitionEvent( + sendTransitionEvent), lowerTransitionEvent( + lowerTransitionEvent), upperTransitionEvent( + upperTransitionEvent), reportingObject(reportingObject), currentCounter( + 0), maxCounter(maxCounter) { + } + + ReturnValue_t oldState; + const uint32_t parameterId; + bool sendTransitionreport; + bool sendTransitionEvent; + Event lowerTransitionEvent; + Event upperTransitionEvent; + const object_id_t reportingObject; + uint16_t currentCounter; + uint16_t maxCounter; + }; + + LimitCheckMonitor(object_id_t reportAs, uint32_t parameterId, + T initialLower, T initialUpper, bool generateReports = true, + bool throwEvents = false, Event lowerTransitionEvent = 0, + Event upperTransitionEvent = 0, uint16_t confirmationNumber = 0) : + lowerLimit(initialLower), upperLimit(initialUpper), parameters( + UNCHECKED, parameterId, generateReports, throwEvents, + lowerTransitionEvent, upperTransitionEvent, reportAs, + confirmationNumber) { + lowerLimit.setNext(&upperLimit); + } + + virtual ~LimitCheckMonitor() { + } + + virtual ReturnValue_t check() { + DataSet mySet; + PIDReader parameter(parameters.parameterId, &mySet); + mySet.read(); + if (!parameter.isValid()) { + if (parameters.oldState != INVALID) { + MonitoringReportContent report(parameter.getParameterId(), 0, + 0, parameters.oldState, INVALID); + LimitViolationReporter::sendLimitViolationReport(&report); + parameters.oldState = INVALID; + } + } else { + parameters.oldState = doCheck(parameter.value, lowerLimit, + upperLimit, ¶meters); + } + return parameters.oldState; + } + + static ReturnValue_t doCheck(T value, T lowerLimit, T upperLimit, + CheckParameters* checkParameters, ReturnValue_t lowerReturnCode = + BELOW_LOW_LIMIT, ReturnValue_t upperReturnCode = + ABOVE_HIGH_LIMIT) { + return doCheck(value, lowerLimit, upperLimit, checkParameters->oldState, + checkParameters->parameterId, + checkParameters->sendTransitionreport, + checkParameters->sendTransitionEvent, + checkParameters->lowerTransitionEvent, + checkParameters->upperTransitionEvent, + checkParameters->reportingObject, + &checkParameters->currentCounter, checkParameters->maxCounter, + lowerReturnCode, upperReturnCode); + } + + static ReturnValue_t doCheck(T value, T lowerLimit, T upperLimit, + ReturnValue_t oldState, uint32_t parameterId, + bool sendTransitionreport, bool sendTransitionEvent, + Event lowerTransitionEvent, Event upperTransitionEvent, + object_id_t reportingObject, uint16_t *currentCounter = NULL, + uint16_t maxCounter = 0, ReturnValue_t lowerReturnCode = + BELOW_LOW_LIMIT, ReturnValue_t upperReturnCode = + ABOVE_HIGH_LIMIT) { + uint16_t tempCounter = 0; + if (currentCounter == NULL) { + currentCounter = &tempCounter; + maxCounter = 0; + } + + ReturnValue_t currentState = HasReturnvaluesIF::RETURN_OK; + T crossedLimit = 0; + if (value > upperLimit) { + currentState = upperReturnCode; + crossedLimit = upperLimit; + } else if (value < lowerLimit) { + currentState = lowerReturnCode; + crossedLimit = lowerLimit; + } + + if (oldState != currentState) { + //confirmation + *currentCounter += 1; + if (*currentCounter > maxCounter || oldState == UNCHECKED) { + *currentCounter = 0; + // Distinction of 3 cases: up/down/(ok or default) + if (currentState == upperReturnCode) { + if (sendTransitionEvent) { + EventManagerIF::triggerEvent(reportingObject, + upperTransitionEvent, 0, + 0); + } + } else if (currentState == lowerReturnCode) { + if (sendTransitionEvent) { + EventManagerIF::triggerEvent(reportingObject, + lowerTransitionEvent, 0, + 0); + } + } else { + // RETURN_OK or all other cases + if (oldState == lowerReturnCode) { + crossedLimit = lowerLimit; + } else { + crossedLimit = upperLimit; + } + } + + if (sendTransitionreport) { + MonitoringReportContent report(parameterId, value, + crossedLimit, oldState, currentState); + LimitViolationReporter::sendLimitViolationReport(&report); + } else { + currentState = UNSELECTED; + } + } else { + currentState = oldState; + } + } else { + *currentCounter = 0; + } + return currentState; + } + + ReturnValue_t setLimits(uint8_t type, const uint8_t* data, uint32_t size) { + if (type != getLimitType()) { + return WRONG_TYPE; + } + + UpdateLimitMonitorContent content; + + if (size != content.getSerializedSize()) { + return INVALID_SIZE; + } + + int32_t tempSize = size; + const uint8_t* pBuffer = data; + ReturnValue_t result = content.deSerialize(&pBuffer, &tempSize, true); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + setLimits(content.lowValue, content.highValue); + return HasReturnvaluesIF::RETURN_OK; + } + + void setLimits(T lower, T upper) { + lowerLimit = lower; + upperLimit = upper; + } + + ReturnValue_t setChecking(uint8_t strategy) { + if ((strategy & REPORT_EVENTS_ONLY) != 0) { + parameters.sendTransitionEvent = true; + } else { + parameters.sendTransitionEvent = false; + } + if ((strategy & REPORT_REPORTS_ONLY) != 0) { + parameters.sendTransitionreport = true; + } else { + parameters.sendTransitionreport = false; + } + return HasReturnvaluesIF::RETURN_OK; + } + + ReturnValue_t setToUnchecked() { + return setToState(UNCHECKED, ¶meters); + } + + static ReturnValue_t setToUnchecked(CheckParameters* parameters) { + if (parameters->oldState != UNCHECKED) { + MonitoringReportContent report(parameters->parameterId, 0, 0, + parameters->oldState, UNCHECKED); + LimitViolationReporter::sendLimitViolationReport(&report); + parameters->oldState = UNCHECKED; + } + return HasReturnvaluesIF::RETURN_OK; + } + + static ReturnValue_t setToState(ReturnValue_t newState, CheckParameters* parameters) { + if (parameters->oldState != newState) { + MonitoringReportContent report(parameters->parameterId, 0, 0, + parameters->oldState, newState); + LimitViolationReporter::sendLimitViolationReport(&report); + parameters->oldState = newState; + } + return HasReturnvaluesIF::RETURN_OK; + } + + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + return SerialLinkedListAdapter::serialize(&lowerLimit, + buffer, size, max_size, bigEndian); + } + + uint32_t getSerializedSize() const { + return SerialLinkedListAdapter::getSerializedSize( + &lowerLimit); + } + + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return SerialLinkedListAdapter::deSerialize(&lowerLimit, + buffer, size, bigEndian); + } + const uint8_t getLimitType() const { + return LIMIT_TYPE_LIMIT_CHECK; + } + const uint32_t getLimitId() const { + return parameters.parameterId; + } + T getLowerLimit() { + return lowerLimit; + } + T getUpperLimit() { + return upperLimit; + } +private: + SerializeElement lowerLimit; + SerializeElement upperLimit; + CheckParameters parameters; +}; + +//TODO: This is for float only, as this is currently the only need. +typedef LinkedElementDecorator, MonitoringIF> LinkedLimitMonitor; + +#endif /* LIMITCHECKMONITOR_H_ */ diff --git a/monitoring/LimitMonitor.h b/monitoring/LimitMonitor.h new file mode 100644 index 00000000..e64a7937 --- /dev/null +++ b/monitoring/LimitMonitor.h @@ -0,0 +1,85 @@ + +#ifndef FRAMEWORK_MONITORING_LIMITMONITOR_H_ +#define FRAMEWORK_MONITORING_LIMITMONITOR_H_ + +#include + +/** + * Variant of a limit checking class. + * Newer version as compared to LimitCheckMonitor. + * Functionality is more or less the same, but does not use + * heavy weight MonitoringIF. + */ +template +class LimitMonitor: public MonitorBase { +public: + LimitMonitor(object_id_t reporterId, uint8_t monitorId, uint32_t parameterId, + uint16_t confirmationLimit, T lowerLimit, T upperLimit, + Event belowLowEvent = MonitoringIF::VALUE_BELOW_LOW_LIMIT, + Event aboveHighEvent = MonitoringIF::VALUE_ABOVE_HIGH_LIMIT) : + MonitorBase(reporterId, monitorId, parameterId, confirmationLimit), lowerLimit( + lowerLimit), upperLimit(upperLimit), belowLowEvent( + belowLowEvent), aboveHighEvent(aboveHighEvent) { + } + virtual ~LimitMonitor() { + } + virtual ReturnValue_t checkSample(T sample, T* crossedLimit) { + if (sample > upperLimit) { + *crossedLimit = upperLimit; + return MonitoringIF::ABOVE_HIGH_LIMIT; + } else if (sample < lowerLimit) { + *crossedLimit = lowerLimit; + return MonitoringIF::BELOW_LOW_LIMIT; + } else { + return HasReturnvaluesIF::RETURN_OK; //Within limits. + } + } + + virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, + ParameterWrapper *parameterWrapper, + const ParameterWrapper *newValues, uint16_t startAtIndex) { + ReturnValue_t result = this->MonitorBase::getParameter(domainId, + parameterId, parameterWrapper, newValues, startAtIndex); + //We'll reuse the DOMAIN_ID of MonitorReporter, as we know the parameterIds used there. + if (result != this->INVALID_MATRIX_ID) { + return result; + } + switch (parameterId) { + case 10: + parameterWrapper->set(this->lowerLimit); + break; + case 11: + parameterWrapper->set(this->lowerLimit); + break; + default: + return this->INVALID_MATRIX_ID; + } + return HasReturnvaluesIF::RETURN_OK; + } + bool isOutOfLimits() { + if (this->oldState == MonitoringIF::ABOVE_HIGH_LIMIT || this->oldState == MonitoringIF::BELOW_LOW_LIMIT) { + return true; + } else { + return false; + } + } +protected: + void sendTransitionEvent(T currentValue, ReturnValue_t state) { + switch (state) { + case MonitoringIF::BELOW_LOW_LIMIT: + EventManagerIF::triggerEvent(this->reportingId, belowLowEvent, this->parameterId); + break; + case MonitoringIF::ABOVE_HIGH_LIMIT: + EventManagerIF::triggerEvent(this->reportingId, aboveHighEvent, this->parameterId); + break; + default: + break; + } + } + T lowerLimit; + T upperLimit; + const Event belowLowEvent; + const Event aboveHighEvent; +}; + +#endif /* FRAMEWORK_MONITORING_LIMITMONITOR_H_ */ diff --git a/monitoring/LimitViolationReporter.cpp b/monitoring/LimitViolationReporter.cpp new file mode 100644 index 00000000..d2dc6845 --- /dev/null +++ b/monitoring/LimitViolationReporter.cpp @@ -0,0 +1,59 @@ +/** + * @file LimitViolationReporter.cpp + * @brief This file defines the LimitViolationReporter class. + * @date 17.07.2014 + * @author baetz + */ +#include +#include +#include +#include +#include + +ReturnValue_t LimitViolationReporter::sendLimitViolationReport(const SerializeIF* data) { + ReturnValue_t result = checkClassLoaded(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + store_address_t storeId; + uint8_t* dataTarget = NULL; + uint32_t maxSize = data->getSerializedSize(); + if (maxSize > MonitoringIF::VIOLATION_REPORT_MAX_SIZE) { + return MonitoringIF::INVALID_SIZE; + } + result = ipcStore->getFreeElement(&storeId, maxSize, + &dataTarget); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + uint32_t size = 0; + result = data->serialize(&dataTarget, &size, maxSize, true); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + CommandMessage report; + MonitoringMessage::setLimitViolationReport(&report, storeId); + return reportQueue.sendToDefault(&report); +} + +ReturnValue_t LimitViolationReporter::checkClassLoaded() { + if (reportQueue.getDefaultDestination() == 0) { + ReceivesMonitoringReportsIF* receiver = objectManager->get< + ReceivesMonitoringReportsIF>(objects::PUS_MONITORING_SERVICE); + if (receiver == NULL) { + return ObjectManagerIF::NOT_FOUND; + } + reportQueue.setDefaultDestination(receiver->getCommandQueue()); + } + if (ipcStore == NULL) { + ipcStore = objectManager->get(objects::IPC_STORE); + if (ipcStore == NULL) { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + return HasReturnvaluesIF::RETURN_OK; +} + +//Lazy initialization. +MessageQueueSender LimitViolationReporter::reportQueue; +StorageManagerIF* LimitViolationReporter::ipcStore = NULL; diff --git a/monitoring/LimitViolationReporter.h b/monitoring/LimitViolationReporter.h new file mode 100644 index 00000000..43d0865c --- /dev/null +++ b/monitoring/LimitViolationReporter.h @@ -0,0 +1,25 @@ +/** + * @file LimitViolationReporter.h + * @brief This file defines the LimitViolationReporter class. + * @date 17.07.2014 + * @author baetz + */ +#ifndef LIMITVIOLATIONREPORTER_H_ +#define LIMITVIOLATIONREPORTER_H_ + +#include +#include +#include +#include + +class LimitViolationReporter { +public: + static ReturnValue_t sendLimitViolationReport(const SerializeIF* data); +private: + static MessageQueueSender reportQueue; + static StorageManagerIF* ipcStore; + static ReturnValue_t checkClassLoaded(); + LimitViolationReporter(); +}; + +#endif /* LIMITVIOLATIONREPORTER_H_ */ diff --git a/monitoring/MonitorBase.h b/monitoring/MonitorBase.h new file mode 100644 index 00000000..0bbcbf67 --- /dev/null +++ b/monitoring/MonitorBase.h @@ -0,0 +1,69 @@ +/* + * MonitorBase.h + * + * Created on: 25.07.2014 + * Author: baetz + */ + +#ifndef MONITORBASE_H_ +#define MONITORBASE_H_ + +#include +#include +#include +#include +#include +#include + +/** + * Base class for monitoring of parameters. + * Can be used anywhere, specializations need to implement checkSample and should override sendTransitionEvent. + * Manages state handling, enabling and disabling of events/reports and forwarding of transition + * reports via MonitorReporter. In addition, it provides default implementations for fetching the parameter sample from + * the data pool and a simple confirmation counter. + */ +template +class MonitorBase: public MonitorReporter { +public: + MonitorBase(object_id_t reporterId, uint8_t monitorId, + uint32_t parameterId, uint16_t confirmationLimit) : + MonitorReporter(reporterId, monitorId, parameterId, confirmationLimit) { + } + virtual ~MonitorBase() { + } + virtual ReturnValue_t check() { + //1. Fetch sample of type T, return validity. + T sample = 0; + ReturnValue_t validity = fetchSample(&sample); + + //2. If returning from fetch != OK, parameter is invalid. Report (if oldState is != invalidity). + if (validity != HasReturnvaluesIF::RETURN_OK) { + monitorStateIs(validity, sample, 0); + //3. Otherwise, check sample. + } else { + this->oldState = doCheck(sample); + } + return this->oldState; + } + virtual ReturnValue_t doCheck(T sample) { + T crossedLimit; + ReturnValue_t currentState = checkSample(sample, &crossedLimit); + return this->monitorStateIs(currentState,sample, crossedLimit); + } + //Abstract or default. + virtual ReturnValue_t checkSample(T sample, T* crossedLimit) = 0; + +protected: + virtual ReturnValue_t fetchSample(T* sample) { + DataSet mySet; + PIDReader parameter(this->parameterId, &mySet); + mySet.read(); + if (!parameter.isValid()) { + return MonitoringIF::INVALID; + } + *sample = parameter.value; + return HasReturnvaluesIF::RETURN_OK; + } +}; + +#endif /* MONITORBASE_H_ */ diff --git a/monitoring/MonitorReporter.h b/monitoring/MonitorReporter.h new file mode 100644 index 00000000..d0abc623 --- /dev/null +++ b/monitoring/MonitorReporter.h @@ -0,0 +1,162 @@ + +#ifndef FRAMEWORK_MONITORING_MONITORREPORTER_H_ +#define FRAMEWORK_MONITORING_MONITORREPORTER_H_ + +#include +#include +#include +#include +#include + +template +class MonitorReporter: public HasParametersIF { +public: + + MonitorReporter(object_id_t reportingId, uint8_t monitorId, uint32_t parameterId, uint16_t confirmationLimit) : + monitorId(monitorId), parameterId(parameterId), reportingId( + reportingId), oldState(MonitoringIF::UNCHECKED), reportingEnabled( + true), eventEnabled(true), currentCounter(0), confirmationLimit( + confirmationLimit) { + } + + virtual ~MonitorReporter() { + } + + ReturnValue_t monitorStateIs(ReturnValue_t state, T parameterValue = 0, + T crossedLimit = 0) { + if (state != oldState) { + if (isConfirmed(state)) { + if (this->eventEnabled) { + sendTransitionEvent(parameterValue, state); + } + if (this->reportingEnabled) { + sendTransitionReport(parameterValue, crossedLimit, state); + } + } else { + //This is to ensure confirmation works. + //Needs to be reset to be able to confirm against oldState again next time. + return oldState; + } + } else { + resetConfirmation(); + } + return state; + } + + virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, + ParameterWrapper *parameterWrapper, + const ParameterWrapper *newValues, uint16_t startAtIndex) { + if (domainId != monitorId) { + return INVALID_DOMAIN_ID; + } + switch (parameterId) { + case 1: + parameterWrapper->set(this->confirmationLimit); + break; + case 2: + parameterWrapper->set(this->reportingEnabled); + break; + case 3: + parameterWrapper->set(this->eventEnabled); + break; + default: + return INVALID_MATRIX_ID; + } + return HasReturnvaluesIF::RETURN_OK; + } + virtual ReturnValue_t setToUnchecked() { + return setToState(MonitoringIF::UNCHECKED); + } + virtual ReturnValue_t setToInvalid() { + return setToState(MonitoringIF::INVALID); + } + object_id_t getReporterId() const { + return reportingId; + } +protected: + const uint8_t monitorId; + const uint32_t parameterId; + object_id_t reportingId; + ReturnValue_t oldState; + + bool reportingEnabled; + + bool eventEnabled; + + uint16_t currentCounter; + uint16_t confirmationLimit; + + bool isConfirmed(ReturnValue_t state) { + //Confirm INVALID and UNCHECKED immediately. + if (state == MonitoringIF::INVALID + || state == MonitoringIF::UNCHECKED) { + currentCounter = 0; + return true; + } + return doesChildConfirm(state); + } + + /** + * This is the most simple form of confirmation. + * A counter counts any violation and compares the number to maxCounter. + * @param state The state, indicating the type of violation. Not used here. + * @return true if counter > maxCounter, else false. + */ + virtual bool doesChildConfirm(ReturnValue_t state) { + currentCounter += 1; + if (currentCounter > confirmationLimit) { + currentCounter = 0; + return true; + } else { + return false; + } + } + /** + * This method needs to reset the confirmation in case a valid sample was found. + * Here, simply resets the current counter. + */ + virtual void resetConfirmation() { + currentCounter = 0; + } + /** + * Default version of sending transitional events. + * Should be overridden from specialized monitors. + * @param currentValue The current value which was monitored. + * @param state The state the monitor changed to. + */ + virtual void sendTransitionEvent(T currentValue, ReturnValue_t state) { + switch(state) { + case MonitoringIF::UNCHECKED: + case MonitoringIF::UNSELECTED: + case MonitoringIF::INVALID: + case HasReturnvaluesIF::RETURN_OK: + break; + default: + EventManagerIF::triggerEvent(reportingId, MonitoringIF::MONITOR_CHANGED_STATE, state); + break; + } + } + /** + * Default implementation for sending transition report. + * May be overridden, but is seldom necessary. + * @param parameterValue Current value of the parameter + * @param crossedLimit The limit crossed (if applicable). + * @param state Current state the monitor is in. + */ + virtual void sendTransitionReport(T parameterValue, T crossedLimit, ReturnValue_t state) { + MonitoringReportContent report(parameterId, + parameterValue, crossedLimit, oldState, state); + LimitViolationReporter::sendLimitViolationReport(&report); + } + ReturnValue_t setToState(ReturnValue_t state) { + if (oldState != state) { + MonitoringReportContent report(parameterId, 0, 0, oldState, + state); + LimitViolationReporter::sendLimitViolationReport(&report); + oldState = state; + } + return HasReturnvaluesIF::RETURN_OK; + } +}; + +#endif /* FRAMEWORK_MONITORING_MONITORREPORTER_H_ */ diff --git a/monitoring/MonitoringHelper.cpp b/monitoring/MonitoringHelper.cpp new file mode 100644 index 00000000..2e96756f --- /dev/null +++ b/monitoring/MonitoringHelper.cpp @@ -0,0 +1,133 @@ +/* + * MonitoringHelper.cpp + * + * Created on: 07.07.2014 + * Author: baetz + */ + +#include +#include +#include +#include +#include +#include + +MonitoringHelper::MonitoringHelper(HasMonitorsIF* limitOwner) : + owner(limitOwner), ipcStore(NULL) { +} + +MonitoringHelper::~MonitoringHelper() { + +} + +ReturnValue_t MonitoringHelper::handleMessage(CommandMessage* message) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED; + switch (message->getCommand()) { + case MonitoringMessage::CHANGE_REPORTING_STRATEGY: + result = handleReportingStrategyMessage(MonitoringMessage::getReportingStategy(message), + MonitoringMessage::getStoreId(message)); + break; + case MonitoringMessage::UPDATE_PARAMETER_MONITOR: + result = handleUpdateParameterMonitor(MonitoringMessage::getStoreId(message)); + break; + case MonitoringMessage::UPDATE_OBJECT_MONITOR: + result = handleUpdateObjectMonitor(MonitoringMessage::getStoreId(message)); + break; + default: + return HasReturnvaluesIF::RETURN_FAILED; + } + replyReturnValue(message, result); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MonitoringHelper::handleReportingStrategyMessage(uint8_t strategy, + store_address_t storeId) { + const uint8_t* data = NULL; + uint32_t size = 0; + ReturnValue_t result = ipcStore->getData(storeId, &data, &size); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + EnableDisableContent content; + int32_t tSize = size; + result = content.deSerialize(&data, &tSize, true); + if (result != HasReturnvaluesIF::RETURN_OK) { + ipcStore->deleteData(storeId); + return result; + } + if (content.funkyList.size == 0) { + result = owner->setCheckingOfParameters(strategy); + } else { + for (EnableDisableContent::EnableDisableList::Iterator iter = content.funkyList.begin(); + iter != content.funkyList.end(); iter++) { + result = owner->setCheckingOfParameters(strategy, true, *iter); + if (result != HasReturnvaluesIF::RETURN_OK) { + //TODO: SW event (as stated in pus) + } + break; + } + result = HasReturnvaluesIF::RETURN_OK; + } + ipcStore->deleteData(storeId); + return result; +} + +ReturnValue_t MonitoringHelper::handleUpdateParameterMonitor(store_address_t storeId) { + const uint8_t* data = NULL; + uint32_t size = 0; + ReturnValue_t result = ipcStore->getData(storeId, &data, &size); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + uint8_t limitType = *data; + data++; + size -= 1; + const uint8_t* pData = data; + int32_t pSize = size; + uint32_t parameterId = 0; + result = SerializeAdapter::deSerialize(¶meterId, &pData, &pSize, + true); + if (result == HasReturnvaluesIF::RETURN_OK) { + result = owner->modifyParameterMonitor(limitType, parameterId, pData, pSize); + } + ipcStore->deleteData(storeId); + return result; +} + +ReturnValue_t MonitoringHelper::handleUpdateObjectMonitor(store_address_t storeId) { + const uint8_t* data = NULL; + uint32_t size = 0; + ReturnValue_t result = ipcStore->getData(storeId, &data, &size); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + object_id_t objectId = 0; + const uint8_t* pData = data; + int32_t tSize = size; + result = SerializeAdapter::deSerialize(&objectId, &pData, &tSize, true); + if (result == HasReturnvaluesIF::RETURN_OK) { + result = owner->modifyObjectMonitor(objectId, pData, tSize); + } + ipcStore->deleteData(storeId); + return result; +} + +void MonitoringHelper::replyReturnValue(CommandMessage* message, ReturnValue_t result) { + CommandMessage reply; + if (result == HasReturnvaluesIF::RETURN_OK) { + reply.setCommand(CommandMessage::REPLY_COMMAND_OK); + } else { + reply.setReplyRejected(result, message->getCommand()); + } + MessageQueueSender sender(message->getSender()); + sender.sendToDefault(&reply, owner->getCommandQueue()); +} + +ReturnValue_t MonitoringHelper::initialize() { + ipcStore = objectManager->get(objects::IPC_STORE); + if (ipcStore != NULL) { + return HasReturnvaluesIF::RETURN_OK; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } +} diff --git a/monitoring/MonitoringHelper.h b/monitoring/MonitoringHelper.h new file mode 100644 index 00000000..20093ecc --- /dev/null +++ b/monitoring/MonitoringHelper.h @@ -0,0 +1,30 @@ +/* + * MonitoringHelper.h + * + * Created on: 07.07.2014 + * Author: baetz + */ + +#ifndef MONITORINGHELPER_H_ +#define MONITORINGHELPER_H_ + +#include +#include +#include + +class MonitoringHelper { +public: + MonitoringHelper(HasMonitorsIF* limitOwner); + virtual ~MonitoringHelper(); + ReturnValue_t handleMessage(CommandMessage* message); + ReturnValue_t initialize(); +private: + HasMonitorsIF* owner; + StorageManagerIF* ipcStore; //might be static + ReturnValue_t handleReportingStrategyMessage(uint8_t strategy, store_address_t storeId); + ReturnValue_t handleUpdateParameterMonitor(store_address_t storeId); + ReturnValue_t handleUpdateObjectMonitor(store_address_t storeId); + void replyReturnValue(CommandMessage* message, ReturnValue_t result); +}; + +#endif /* MONITORINGHELPER_H_ */ diff --git a/monitoring/MonitoringIF.h b/monitoring/MonitoringIF.h new file mode 100644 index 00000000..b545e97d --- /dev/null +++ b/monitoring/MonitoringIF.h @@ -0,0 +1,67 @@ +#ifndef MONITORINGIF_H_ +#define MONITORINGIF_H_ + +#include +#include +#include + +class MonitoringIF : public SerializeIF { +public: + static const uint8_t VIOLATION_REPORT_MAX_SIZE = 30; + static const uint8_t LIMIT_TYPE_NO_TYPE = 0xFF; + static const uint8_t LIMIT_TYPE_LIMIT_CHECK = 0; + static const uint8_t LIMIT_TYPE_DELTA_CHECK = 1; + static const uint8_t LIMIT_TYPE_ABSOLUTE_CHECK = 2; + static const uint8_t LIMIT_TYPE_OBJECT = 128; + + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::FDIR_2; + static const Event MONITOR_CHANGED_STATE = MAKE_EVENT(1, SEVERITY::LOW); + static const Event VALUE_BELOW_LOW_LIMIT = MAKE_EVENT(2, SEVERITY::LOW); + static const Event VALUE_ABOVE_HIGH_LIMIT = MAKE_EVENT(3, SEVERITY::LOW); + static const Event VALUE_OUT_OF_RANGE = MAKE_EVENT(4, SEVERITY::LOW); + + static const uint8_t INTERFACE_ID = LIMITS_IF; + static const ReturnValue_t UNCHECKED = MAKE_RETURN_CODE(1); + static const ReturnValue_t INVALID = MAKE_RETURN_CODE(2); + static const ReturnValue_t UNSELECTED = MAKE_RETURN_CODE(3); + static const ReturnValue_t BELOW_LOW_LIMIT = MAKE_RETURN_CODE(4); +// static const ReturnValue_t CHECKING_STATUS_BELOW_LOW_THRESHOLD = MAKE_RETURN_CODE(4); +// static const ReturnValue_t CHECKING_STATUS_ABOVE_HIGH_THRESHOLD = MAKE_RETURN_CODE(5); + static const ReturnValue_t ABOVE_HIGH_LIMIT = MAKE_RETURN_CODE(5); + static const ReturnValue_t UNEXPECTED_VALUE = MAKE_RETURN_CODE(6); + static const ReturnValue_t OUT_OF_RANGE = MAKE_RETURN_CODE(7); + + + static const ReturnValue_t FIRST_SAMPLE = MAKE_RETURN_CODE(0xA0); + static const ReturnValue_t INVALID_SIZE = MAKE_RETURN_CODE(0xE0); + static const ReturnValue_t WRONG_TYPE = MAKE_RETURN_CODE(0xE1); + static const ReturnValue_t WRONG_PID = MAKE_RETURN_CODE(0xE2); + static const ReturnValue_t WRONG_LIMIT_ID = MAKE_RETURN_CODE(0xE3); + static const ReturnValue_t MONITOR_NOT_FOUND = MAKE_RETURN_CODE(0xEE); + + static const uint8_t REPORT_NONE = 0; + static const uint8_t REPORT_EVENTS_ONLY = 1; + static const uint8_t REPORT_REPORTS_ONLY = 2; + static const uint8_t REPORT_ALL = 3; + +// static const ReturnValue_t STILL_IN_LOW_WARNING = MAKE_RETURN_CODE(0x11); +// static const ReturnValue_t STILL_IN_LOW_LIMIT = MAKE_RETURN_CODE(0x12); +// static const ReturnValue_t STILL_IN_HIGH_WARNING = MAKE_RETURN_CODE(0x13); +// static const ReturnValue_t STILL_IN_HIGH_LIMIT = MAKE_RETURN_CODE(0x14); +// static const ReturnValue_t VARIABLE_IS_INVALID = MAKE_RETURN_CODE(0xE0); +// static const ReturnValue_t INVALID_SIZE = MAKE_RETURN_CODE(0xE1); +// static const ReturnValue_t INVALID_ID = MAKE_RETURN_CODE(0xE2); + virtual ReturnValue_t check() = 0; + virtual ReturnValue_t setLimits( uint8_t type, const uint8_t* data, uint32_t size) = 0; + virtual ReturnValue_t setChecking(uint8_t strategy) = 0; + virtual ReturnValue_t setToUnchecked() = 0; + virtual const uint8_t getLimitType() const = 0; + virtual const uint32_t getLimitId() const = 0; +// virtual ReturnValue_t setEventReporting(bool active) = 0; + virtual ~MonitoringIF() { + } +}; + + + +#endif /* MONITORINGIF_H_ */ diff --git a/monitoring/MonitoringListAdapter.cpp b/monitoring/MonitoringListAdapter.cpp new file mode 100644 index 00000000..258cbfbd --- /dev/null +++ b/monitoring/MonitoringListAdapter.cpp @@ -0,0 +1,79 @@ +/* + * MonitoringListAdapter.cpp + * + * Created on: 09.07.2014 + * Author: baetz + */ + +#include + +ReturnValue_t MonitoringListAdapter::check() { + LinkedElement* element = start; + ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED; + bool atLeastOneFailed = false; + while ((element != NULL)) { + result = element->value->check(); + if (result != HasReturnvaluesIF::RETURN_OK) { + atLeastOneFailed = true; + } + element = element->getNext(); + } + return (atLeastOneFailed) ? + (ReturnValue_t) HasReturnvaluesIF::RETURN_FAILED : + (ReturnValue_t) HasReturnvaluesIF::RETURN_OK; +} + +//ReturnValue_t MonitoringListAdapter::serialize(uint8_t** buffer, uint32_t* size, +// const uint32_t max_size, bool bigEndian) const { +// return SerialListAdapter::serialize(start, buffer, size, max_size, bigEndian); +//} +// +//uint32_t MonitoringListAdapter::getSerializedSize() const { +// return SerialListAdapter::getSerializedSize(start); +//} +// +//ReturnValue_t MonitoringListAdapter::deSerialize(const uint8_t** buffer, int32_t* size, +// bool bigEndian) { +// return SerialListAdapter::deSerialize(start, buffer, size, bigEndian); +//} + +ReturnValue_t MonitoringListAdapter::updateMonitor(uint32_t parameterId, + uint8_t limitType, const uint8_t* data, uint32_t size) { + LinkedElement* element = start; + while ((element != NULL)) { + if ((element->value->getLimitId() == parameterId)) { + return element->value->setLimits(limitType, data, size); + } + element = element->getNext(); + } + return MonitoringIF::MONITOR_NOT_FOUND; +} + +ReturnValue_t MonitoringListAdapter::setChecking(uint8_t strategy, uint32_t parameterId) { + LinkedElement* element = start; + while ((element != NULL)) { + if ((element->value->getLimitId() == parameterId)) { + return element->value->setChecking(strategy); + } + element = element->getNext(); + } + return MonitoringIF::MONITOR_NOT_FOUND; +} + +ReturnValue_t MonitoringListAdapter::setChecking(uint8_t strategy) { + LinkedElement* element = start; + ReturnValue_t result = MonitoringIF::MONITOR_NOT_FOUND; + bool atLeastOneUpdated = false; + while ((element != NULL)) { + result = element->value->setChecking(strategy); + if (result == HasReturnvaluesIF::RETURN_OK) { + atLeastOneUpdated = true; + } + element = element->getNext(); + } + result = + (atLeastOneUpdated) ? + (ReturnValue_t) HasReturnvaluesIF::RETURN_OK : + (ReturnValue_t) MonitoringIF::MONITOR_NOT_FOUND; + return result; +} diff --git a/monitoring/MonitoringListAdapter.h b/monitoring/MonitoringListAdapter.h new file mode 100644 index 00000000..e290f4f9 --- /dev/null +++ b/monitoring/MonitoringListAdapter.h @@ -0,0 +1,59 @@ +/* + * MonitoringListAdapter.h + * + * Created on: 09.07.2014 + * Author: baetz + */ + +#ifndef MONITORINGLISTADAPTER_H_ +#define MONITORINGLISTADAPTER_H_ + +#include +#include + +class MonitoringListAdapter : public SerialLinkedListAdapter { +public: + MonitoringListAdapter(LinkedElement* start) : SerialLinkedListAdapter(start) { + } + MonitoringListAdapter() : SerialLinkedListAdapter() { + } + /** + * Checks all elements. + * @return Returns RETURN_FAILED if at least one check failed. + */ + ReturnValue_t check(); + /** + * Iterates the list to update the requested monitor. + * @param parameterId The parameter id to monitor. + * @param limitId The limit id for the given PID. + * @param data contains the new values + * @param size size of the new values + * @return The return code of the monitor if it was found and an update attempt, MONITOR_NOT_FOUND else. + */ + ReturnValue_t updateMonitor( uint32_t parameterId, uint8_t limitType, const uint8_t* data, uint32_t size); + /** + * Iterates the list to set the checking strategies for one monitor. + * @param strategy the reporting strategy. + * @param parameterId The PID. + * @param limitId The limit id for the given PID. + * @return The return code of the monitor if it was found and an update attempt, MONITOR_NOT_FOUND else. + */ +// ReturnValue_t setChecking(uint8_t strategy, uint32_t parameterId, uint32_t limitId); + /** + * Iterates the list to set the checking strategies for all monitors of a given parameter. + * @param strategy the reporting strategy. + * @param parameterId The PID. + * @return RETURN_OK if at least one monitor was updated, MONITOR_NOT_FOUND else. + */ + ReturnValue_t setChecking(uint8_t strategy, uint32_t parameterId); + /** + * Iterates the list to set the checking strategies for all monitors- + * @param strategy the reporting strategy. + * @return RETURN_OK if at least one monitor was updated, MONITOR_NOT_FOUND else. + */ + ReturnValue_t setChecking(uint8_t strategy); +}; + + + +#endif /* MONITORINGLISTADAPTER_H_ */ diff --git a/monitoring/MonitoringMessage.cpp b/monitoring/MonitoringMessage.cpp new file mode 100644 index 00000000..e9359fc6 --- /dev/null +++ b/monitoring/MonitoringMessage.cpp @@ -0,0 +1,56 @@ +#include +#include + +MonitoringMessage::~MonitoringMessage() { +} + +void MonitoringMessage::setAddLimitCommand(CommandMessage* message, + store_address_t storeId) { + setTypicalMessage(message, ADD_MONITOR, storeId); +} + +void MonitoringMessage::setLimitViolationReport(CommandMessage* message, + store_address_t storeId) { + setTypicalMessage(message, LIMIT_VIOLATION_REPORT, storeId); +} + +void MonitoringMessage::setTypicalMessage(CommandMessage* message, + Command_t type, store_address_t storeId) { + message->setCommand(type); + message->setParameter2(storeId.raw); +} + +store_address_t MonitoringMessage::getStoreId(const CommandMessage* message) { + store_address_t temp; + temp.raw = message->getParameter2(); + return temp; +} + +void MonitoringMessage::clear(CommandMessage* message) { + message->setCommand(CommandMessage::CMD_NONE); + switch (message->getCommand()) { + case MonitoringMessage::ADD_MONITOR: + case MonitoringMessage::UPDATE_PARAMETER_MONITOR: + case MonitoringMessage::LIMIT_VIOLATION_REPORT: { + StorageManagerIF *ipcStore = objectManager->get( + objects::IPC_STORE); + if (ipcStore != NULL) { + ipcStore->deleteData(getStoreId(message)); + } + break; + } + default: + break; + } +} + +void MonitoringMessage::setChangeReportingStrategy(CommandMessage* message, + uint8_t strategy, store_address_t storeId) { + message->setCommand(CHANGE_REPORTING_STRATEGY); + message->setParameter(strategy); + message->setParameter2(storeId.raw); +} + +uint8_t MonitoringMessage::getReportingStategy(CommandMessage* message) { + return (message->getParameter() & 0xFF); +} diff --git a/monitoring/MonitoringMessage.h b/monitoring/MonitoringMessage.h new file mode 100644 index 00000000..2e13a66b --- /dev/null +++ b/monitoring/MonitoringMessage.h @@ -0,0 +1,31 @@ +#ifndef MONITORINGMESSAGE_H_ +#define MONITORINGMESSAGE_H_ + +#include +#include + +class MonitoringMessage: public CommandMessage { +public: + static const uint8_t MESSAGE_ID = LIMIT_MESSAGE_ID; + static const Command_t ADD_MONITOR = MAKE_COMMAND_ID(1); //PID(uint32_t), Data(type, {LIMIT_ID(uint8_t), initialLimits(data, depends)}) + static const Command_t UPDATE_PARAMETER_MONITOR = MAKE_COMMAND_ID(2); //PID(uint32_t), Data{type, n_entries {LIMIT_ID(uint8_t), TYPE(uint8_t) newLimits(data, depends) + static const Command_t UPDATE_OBJECT_MONITOR = MAKE_COMMAND_ID(3); + static const Command_t CHANGE_REPORTING_STRATEGY = MAKE_COMMAND_ID(4); //PID(uint32_t), Data{type, n_entries {LIMIT_ID(uint8_t), TYPE(uint8_t) newLimits(data, depends) + //Optional +// static const Command_t REPORT_LIMIT_DEFINITIONS = MAKE_COMMAND_ID(3); //N_PIDS, PID(uint32_t) +// static const Command_t LIMIT_DEFINITION_REPORT = MAKE_COMMAND_ID(3); //Eventually multiple reports per type or even per definition. + //Object id could be useful, but we better manage that on service level (register potential reporters). + static const Command_t LIMIT_VIOLATION_REPORT = MAKE_COMMAND_ID(10); + virtual ~MonitoringMessage(); + static void setAddLimitCommand(CommandMessage* message, store_address_t storeId); + static void setChangeReportingStrategy(CommandMessage* message, uint8_t strategy, store_address_t storeId); + static void setLimitViolationReport(CommandMessage* message, store_address_t storeId); + static void clear(CommandMessage* message); + static store_address_t getStoreId(const CommandMessage* message); + static uint8_t getReportingStategy(CommandMessage* message); + static void setTypicalMessage(CommandMessage* message, Command_t type, store_address_t storeId); + +}; + + +#endif /* MONITORINGMESSAGE_H_ */ diff --git a/monitoring/MonitoringMessageContent.h b/monitoring/MonitoringMessageContent.h new file mode 100644 index 00000000..07b0bfb1 --- /dev/null +++ b/monitoring/MonitoringMessageContent.h @@ -0,0 +1,108 @@ +#ifndef MONITORINGMESSAGECONTENT_H_ +#define MONITORINGMESSAGECONTENT_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//PID(uint32_t), TYPE, LIMIT_ID, value,limitValue, previous, later, timestamp +template +class MonitoringReportContent: public SerialLinkedListAdapter { +public: + SerializeElement monitorId; + SerializeElement parameterId; + SerializeElement parameterValue; + SerializeElement limitValue; + SerializeElement oldState; + SerializeElement newState; + uint8_t rawTimestamp[TimeStamperIF::MISSION_TIMESTAMP_SIZE]; + SerializeElement timestampSerializer; + TimeStamperIF* timeStamper; + MonitoringReportContent() : + SerialLinkedListAdapter( + LinkedElement::Iterator(¶meterId)), monitorId(0), parameterId( + 0), parameterValue(0), limitValue(0), oldState(0), newState( + 0), rawTimestamp( { 0 }), timestampSerializer(rawTimestamp, + sizeof(rawTimestamp)), timeStamper(NULL) { + setAllNext(); + } + MonitoringReportContent(uint32_t setPID, T value, T limitValue, + ReturnValue_t oldState, ReturnValue_t newState) : + SerialLinkedListAdapter( + LinkedElement::Iterator(¶meterId)), parameterId( + setPID), parameterValue(value), limitValue(limitValue), oldState( + oldState), newState(newState), rawTimestamp( { 0 }), timestampSerializer(rawTimestamp, + sizeof(rawTimestamp)), timeStamper(NULL) { + setAllNext(); + if (checkAndSetStamper()) { + timeStamper->addTimeStamp(rawTimestamp, sizeof(rawTimestamp)); + } + } +private: + void setAllNext() { + parameterId.setNext(¶meterValue); + parameterValue.setNext(&limitValue); + limitValue.setNext(&oldState); + oldState.setNext(&newState); + newState.setNext(×tampSerializer); + } + bool checkAndSetStamper() { + if (timeStamper == NULL) { + //TODO: Adjust name? + timeStamper = objectManager->get( objects::TIME_MANAGER ); + if ( timeStamper == NULL ) { + error << "MonitoringReportContent::checkAndSetStamper: Stamper not found!" << std::endl; + return false; + } + } + return true; + } +}; + +//TODO: Next message would be update_limit message. +//PID(uint32_t), Data{type, n_entries {LIMIT_ID(uint8_t), TYPE(uint8_t) newLimits(data, depends) + +template +class UpdateLimitMonitorContent: public SerialLinkedListAdapter { +public: + SerializeElement lowValue; + SerializeElement highValue; + UpdateLimitMonitorContent() : + SerialLinkedListAdapter(&lowValue), lowValue(0), highValue( + 0) { + lowValue.setNext(&highValue); + } +private: +}; + +//Not used at the moment. +//class EnableDisableInner: public SerialLinkedListAdapter { +//public: +// SerializeElement parameterId; +// typedef FixedArrayList LimitIdList; +// SerializeElement limitList; +// EnableDisableInner() : +// SerialLinkedListAdapter(¶meterId) { +// parameterId.setNext(&limitList); +// } +//}; + +class EnableDisableContent { +public: + typedef SerialFixedArrayListAdapter EnableDisableList; + EnableDisableList funkyList; + EnableDisableContent() { + } + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return funkyList.deSerialize(buffer, size, bigEndian); + } +}; + +#endif /* MONITORINGMESSAGECONTENT_H_ */ diff --git a/monitoring/OneParameterMonitorList.h b/monitoring/OneParameterMonitorList.h new file mode 100644 index 00000000..71792093 --- /dev/null +++ b/monitoring/OneParameterMonitorList.h @@ -0,0 +1,119 @@ +/* + * OneParameterMonitorList.h + * + * Created on: 25.07.2014 + * Author: baetz + */ + +#ifndef ONEPARAMETERMONITORLIST_H_ +#define ONEPARAMETERMONITORLIST_H_ + +#include +#include + +class OneParameterMonitorList: public SinglyLinkedList, + public MonitoringIF { +public: + OneParameterMonitorList(uint32_t setParameterId, + LinkedElement* start) : + SinglyLinkedList(start), parameterId( + setParameterId) { + } + + OneParameterMonitorList() : + SinglyLinkedList(), parameterId(0) { + } + + ReturnValue_t check() { + LinkedElement* element = start; + ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED; + bool atLeastOneFailed = false; + while ((element != NULL)) { + result = element->value->check(); + if (result != HasReturnvaluesIF::RETURN_OK) { + atLeastOneFailed = true; + } + element = element->getNext(); + } + return (atLeastOneFailed) ? + (ReturnValue_t) HasReturnvaluesIF::RETURN_FAILED : + (ReturnValue_t) HasReturnvaluesIF::RETURN_OK; + } + + ReturnValue_t setLimits(uint8_t type, const uint8_t* data, uint32_t size) { + uint8_t position = *data; + data++; + size--; + uint8_t currentPosition = 1; + LinkedElement* element = start; + while ((element != NULL)) { + if ((element->value->getLimitType() == type)) { + if (position == currentPosition++) { + return element->value->setLimits(type, data, size); + } + } + element = element->getNext(); + } + return MonitoringIF::MONITOR_NOT_FOUND; + } + + ReturnValue_t setChecking(uint8_t strategy) { + LinkedElement* element = start; + ReturnValue_t result = MonitoringIF::MONITOR_NOT_FOUND; + bool atLeastOneUpdated = false; + while ((element != NULL)) { + result = element->value->setChecking(strategy); + if (result == HasReturnvaluesIF::RETURN_OK) { + atLeastOneUpdated = true; + } + element = element->getNext(); + } + result = + (atLeastOneUpdated) ? + (ReturnValue_t) HasReturnvaluesIF::RETURN_OK : + (ReturnValue_t) MonitoringIF::MONITOR_NOT_FOUND; + return result; + } + + ReturnValue_t setToUnchecked() { + LinkedElement* element = start; + ReturnValue_t result = MonitoringIF::MONITOR_NOT_FOUND; + bool atLeastOneUpdated = false; + while ((element != NULL)) { + result = element->value->setToUnchecked(); + if (result == HasReturnvaluesIF::RETURN_OK) { + atLeastOneUpdated = true; + } + element = element->getNext(); + } + result = + (atLeastOneUpdated) ? + (ReturnValue_t) HasReturnvaluesIF::RETURN_OK : + (ReturnValue_t) MonitoringIF::MONITOR_NOT_FOUND; + return result; + } + + const uint8_t getLimitType() const { + return LIMIT_TYPE_NO_TYPE; + } + const uint32_t getLimitId() const { + return parameterId; + } + + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + return SerialLinkedListAdapter::serialize(this->start,buffer,size, max_size,bigEndian); + } + + uint32_t getSerializedSize() const { + return SerialLinkedListAdapter::getSerializedSize(this->start); + } + + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return SerialLinkedListAdapter::deSerialize(this->start, buffer, size, bigEndian); + } +private: + uint32_t parameterId; +}; +#endif /* ONEPARAMETERMONITORLIST_H_ */ diff --git a/monitoring/ReceivesMonitoringReportsIF.h b/monitoring/ReceivesMonitoringReportsIF.h new file mode 100644 index 00000000..568c9007 --- /dev/null +++ b/monitoring/ReceivesMonitoringReportsIF.h @@ -0,0 +1,22 @@ +/* + * ReceivesMonitoringReportsIF.h + * + * Created on: 07.07.2014 + * Author: baetz + */ + +#ifndef RECEIVESMONITORINGREPORTSIF_H_ +#define RECEIVESMONITORINGREPORTSIF_H_ + +#include + +class ReceivesMonitoringReportsIF { +public: + virtual MessageQueueId_t getCommandQueue() const = 0; + virtual ~ReceivesMonitoringReportsIF() { + } +}; + + + +#endif /* RECEIVESMONITORINGREPORTSIF_H_ */ diff --git a/monitoring/TwoValueLimitMonitor.h b/monitoring/TwoValueLimitMonitor.h new file mode 100644 index 00000000..cc055b77 --- /dev/null +++ b/monitoring/TwoValueLimitMonitor.h @@ -0,0 +1,45 @@ + +#ifndef FRAMEWORK_MONITORING_TWOVALUELIMITMONITOR_H_ +#define FRAMEWORK_MONITORING_TWOVALUELIMITMONITOR_H_ + +#include + +template +class TwoValueLimitMonitor: public LimitMonitor { +public: + TwoValueLimitMonitor(object_id_t reporterId, uint8_t monitorId, + uint32_t lowParameterId, uint32_t highParameterId, + uint16_t confirmationLimit, T lowerLimit, T upperLimit, + Event belowLowEvent = MonitoringIF::VALUE_BELOW_LOW_LIMIT, + Event aboveHighEvent = MonitoringIF::VALUE_ABOVE_HIGH_LIMIT) : + LimitMonitor(reporterId, monitorId, lowParameterId, + confirmationLimit, lowerLimit, upperLimit, belowLowEvent, + aboveHighEvent), highValueParameterId(highParameterId) { + } + virtual ~TwoValueLimitMonitor() { + } + ReturnValue_t doCheck(T lowSample, T highSample) { + T crossedLimit; + ReturnValue_t currentState = checkSample(lowSample, &crossedLimit); + if (currentState != HasReturnvaluesIF::RETURN_OK) { + return this->monitorStateIs(currentState, lowSample, crossedLimit); + } + currentState = checkSample(highSample, &crossedLimit); + return this->monitorStateIs(currentState, highSample, crossedLimit); + } +protected: + virtual void sendTransitionReport(T parameterValue, T crossedLimit, + ReturnValue_t state) { + uint32_t usedParameterId = this->parameterId; + if (state == MonitoringIF::ABOVE_HIGH_LIMIT) { + usedParameterId = this->highValueParameterId; + } + MonitoringReportContent report(usedParameterId, parameterValue, + crossedLimit, this->oldState, state); + LimitViolationReporter::sendLimitViolationReport(&report); + } +private: + const uint32_t highValueParameterId; +}; + +#endif /* FRAMEWORK_MONITORING_TWOVALUELIMITMONITOR_H_ */ diff --git a/objectmanager/ObjectManager.cpp b/objectmanager/ObjectManager.cpp new file mode 100644 index 00000000..cb505c04 --- /dev/null +++ b/objectmanager/ObjectManager.cpp @@ -0,0 +1,106 @@ +/* + * ObjectManager.cpp + * + * Created on: Sep 18, 2012 + * Author: baetz + */ + + + + +#include +#include +#include + +ObjectManager::ObjectManager( void (*setProducer)() ) : produceObjects(setProducer) { + //There's nothing special to do in the constructor. +} + + +ObjectManager::~ObjectManager() { + std::map::iterator it; + for (it = this->objectList.begin(); it != this->objectList.end(); it++) { + delete it->second; + } +} + +ReturnValue_t ObjectManager::insert( object_id_t id, SystemObjectIF* object) { + bool insert_return = this->objectList.insert( std::pair< object_id_t, SystemObjectIF* >( id, object ) ).second; + if (insert_return == true) { +// debug << "ObjectManager::insert: Object " << std::hex << (int)id << std::dec << " inserted." << std::endl; + return this->RETURN_OK; + } else { + error << "ObjectManager::insert: Object id " << std::hex << (int)id << std::dec << " is already in use!" << std::endl; + exit(0); //This is very severe and difficult to handle in other places. + return this->INSERTION_FAILED; + } +} + +ReturnValue_t ObjectManager::remove( object_id_t id ) { + std::map::iterator it = this->objectList.find( id ); + if ( this->getSystemObject(id) != NULL ) { + this->objectList.erase( id ); + debug << "ObjectManager::removeObject: Object " << std::hex << (int)id << std::dec << " removed." << std::endl; + return RETURN_OK; + } else { + error << "ObjectManager::removeObject: Requested object "<< std::hex << (int)id << std::dec << " not found." << std::endl; + return NOT_FOUND; + } +} + + + +SystemObjectIF* ObjectManager::getSystemObject( object_id_t id ) { + std::map::iterator it = this->objectList.find( id ); + if (it == this->objectList.end() ) { + //Changed for testing different method. +// SystemObjectIF* object = this->produceObjects( id ); +// return object; + return NULL; + } else { + return it->second; + } +} + +ObjectManager::ObjectManager( ) : produceObjects(NULL) { + +} + +void ObjectManager::initialize() { + //TODO: Include check if already initialized? + this->produceObjects(); + ReturnValue_t return_value = RETURN_FAILED; + uint32_t error_count = 0; + for (std::map::iterator it = this->objectList.begin(); it != objectList.end(); it++ ) { + return_value = it->second->initialize(); + if ( return_value != RETURN_OK ) { + object_id_t var = it->first; + error << "Object " << std::hex << (int) var << " failed to initialize with code 0x" << return_value << std::dec << std::endl; + error_count++; + } + } + if (error_count > 0) { + error << "ObjectManager::ObjectManager: Counted " << error_count << " failed initializations." << std::endl; + } + //Init was successful. Now check successful interconnections. + error_count = 0; + for (std::map::iterator it = this->objectList.begin(); it != objectList.end(); it++ ) { + return_value = it->second->checkObjectConnections(); + if ( return_value != RETURN_OK ) { + error << "Object " << std::hex << (int) it->first << " connection check failed with code 0x" << return_value << std::dec << std::endl; + error_count++; + } + } + if (error_count > 0) { + error << "ObjectManager::ObjectManager: Counted " << error_count << " failed connection checks." << std::endl; + } + +} + +void ObjectManager::printList() { + std::map::iterator it; + debug << "ObjectManager: Object List contains:" << std::endl; + for (it = this->objectList.begin(); it != this->objectList.end(); it++) { + debug << std::hex << it->first << " | " << it->second << std::endl; + } +} diff --git a/objectmanager/ObjectManager.h b/objectmanager/ObjectManager.h new file mode 100644 index 00000000..de22939e --- /dev/null +++ b/objectmanager/ObjectManager.h @@ -0,0 +1,67 @@ +/** + * @file ObjectManager.h + * @brief This file contains the implementation of the ObjectManager class + * @date 18.09.2012 + * @author Bastian Baetz + */ + +#ifndef OBJECTMANAGER_H_ +#define OBJECTMANAGER_H_ + +#include +#include +#include + +/** + * @brief This class implements a global object manager. + * @details This manager handles a list of available objects with system-wide + * relevance, such as device handlers, and TM/TC services. Objects can + * be inserted, removed and retrieved from the list. In addition, the + * class holds a so-called factory, that creates and inserts new + * objects if they are not already in the list. This feature automates + * most of the system initialization. + * As the system is static after initialization, no new objects are + * created or inserted into the list after startup. + * \ingroup system_objects + */ +class ObjectManager : public ObjectManagerIF { +private: + //comparison? + /** + * \brief This is the map of all initialized objects in the manager. + * \details Objects in the List must inherit the SystemObjectIF. + */ + std::map objectList; +protected: + SystemObjectIF* getSystemObject( object_id_t id ); + /** + * @brief This attribute is initialized with the factory function + * that creates new objects. + * @details The function is called if an object was requested with + * getSystemObject, but not found in objectList. + * @param The id of the object to be created. + * @return Returns a pointer to the newly created object or NULL. + */ + void (*produceObjects)(); +public: + /** + * @brief Apart from setting the producer function, nothing special + * happens in the constructor. + * @param setProducer A pointer to a factory function. + */ + ObjectManager( void (*produce)() ); + ObjectManager(); + /** + * @brief In the class's destructor, all objects in the list are deleted. + */ + //TODO: Check, if deleting works! + virtual ~ObjectManager( void ); + ReturnValue_t insert( object_id_t id, SystemObjectIF* object ); + ReturnValue_t remove( object_id_t id ); + void initialize(); + void printList(); +}; + + + +#endif /* OBJECTMANAGER_H_ */ diff --git a/objectmanager/ObjectManagerIF.h b/objectmanager/ObjectManagerIF.h new file mode 100644 index 00000000..81482ba0 --- /dev/null +++ b/objectmanager/ObjectManagerIF.h @@ -0,0 +1,91 @@ +/** + * @file ObjectManagerIF.h + * @brief This file contains the implementation of the ObjectManagerIF interface + * @date 19.09.2012 + * @author Bastian Baetz + */ + +#ifndef OBJECTMANAGERIF_H_ +#define OBJECTMANAGERIF_H_ + +#include +#include +#include + +/** + * @brief This class provides an interface to the global object manager. + * @details This manager handles a list of available objects with system-wide + * relevance, such as device handlers, and TM/TC services. They can be + * inserted, removed and retrieved from the list. On getting the + * object, the call checks if the object implements the requested + * interface. + * \ingroup system_objects + */ +class ObjectManagerIF : public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = OBJECT_MANAGER_IF; + static const ReturnValue_t INSERTION_FAILED = MAKE_RETURN_CODE( 1 ); + static const ReturnValue_t NOT_FOUND = MAKE_RETURN_CODE( 2 ); +protected: + /** + * @brief This method is used to hide the template-based get call from + * a specific implementation. + * @details So, an implementation only has to implement this call. + * @param id The object id of the requested object. + * @return The method returns a pointer to an object implementing (at + * least) the SystemObjectIF, or NULL. + */ + virtual SystemObjectIF* getSystemObject( object_id_t id ) = 0; +public: + /** + * @brief This is the empty virtual destructor as requested by C++ interfaces. + */ + virtual ~ObjectManagerIF( void ) {}; + /** + * @brief With this call, new objects are inserted to the list. + * @details The implementation shall return an error code in case the + * object can't be added (e.g. the id is already in use). + * @param id The new id to be added to the list. + * @param object A pointer to the object to be added. + * @return @li INSERTION_FAILED in case the object could not be inserted. + * @li RETURN_OK in case the object was successfully inserted + */ + virtual ReturnValue_t insert( object_id_t id, SystemObjectIF* object ) = 0; + /** + * @brief With the get call, interfaces of an object can be retrieved in + * a type-safe manner. + * @details With the template-based call, the object list is searched with the + * getSystemObject method and afterwards it is checked, if the object + * implements the requested interface (with a dynamic_cast). + * @param id The object id of the requested object. + * @return The method returns a pointer to an object implementing the + * requested interface, or NULL. + */ + template T* get( object_id_t id ); + /** + * @brief With this call, an object is removed from the list. + * @param id The object id of the object to be removed. + * @return \li NOT_FOUND in case the object was not found + * \li RETURN_OK in case the object was successfully removed + */ + virtual ReturnValue_t remove( object_id_t id ) = 0; + virtual void initialize() = 0; + /** + * @brief This is a debug function, that prints the current content of the + * object list. + */ + virtual void printList() = 0; +}; + +template +T* ObjectManagerIF::get( object_id_t id ) { + SystemObjectIF* temp = this->getSystemObject(id); + return dynamic_cast(temp); +} + +/** + * @brief This is the forward declaration of the global objectManager instance. + */ +extern ObjectManagerIF *objectManager; + +#endif /* OBJECTMANAGERIF_H_ */ diff --git a/objectmanager/SystemObject.cpp b/objectmanager/SystemObject.cpp new file mode 100644 index 00000000..c6495098 --- /dev/null +++ b/objectmanager/SystemObject.cpp @@ -0,0 +1,44 @@ +/* + * SystemObject.cpp + * + * Created on: 07.11.2012 + * Author: mohr + */ + +#include +#include +#include + +SystemObject::SystemObject(object_id_t setObjectId, bool doRegister) : + objectId(setObjectId), registered(doRegister) { + if (registered) { + objectManager->insert(objectId, this); + } +} + +SystemObject::~SystemObject() { + if (registered) { + objectManager->remove(objectId); + } +} + +const object_id_t SystemObject::getObjectId() const { + return objectId; +} + +void SystemObject::triggerEvent(Event event, uint32_t parameter1, + uint32_t parameter2) { + EventManagerIF::triggerEvent(objectId, event, parameter1, parameter2); +} + +ReturnValue_t SystemObject::initialize() { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t SystemObject::checkObjectConnections() { + return HasReturnvaluesIF::RETURN_OK; +} + +void SystemObject::forwardEvent(Event event, uint32_t parameter1, uint32_t parameter2) const { + EventManagerIF::triggerEvent(objectId, event, parameter1, parameter2); +} diff --git a/objectmanager/SystemObject.h b/objectmanager/SystemObject.h new file mode 100644 index 00000000..01f7c327 --- /dev/null +++ b/objectmanager/SystemObject.h @@ -0,0 +1,61 @@ +/** + * @file SystemObject.h + * @brief This file contains the definition of the SystemObject class. + * @date 07.11.2012 + * @author Ulrich Mohr + */ + +#ifndef SYSTEMOBJECT_H_ +#define SYSTEMOBJECT_H_ + +#include +#include +#include +#include + +/** + * @brief This class automates insertion into the ObjectManager and + * management of the object id. + * @details This class is more a base class, which shall be inherited by any + * class that is announced to ObjectManager. It automatically includes + * itself (and therefore the inheriting class) in the object manager's + * list. + * \ingroup system_objects + */ +class SystemObject: public SystemObjectIF { +private: + /** + * @brief This is the id the class instant got assigned. + */ + const object_id_t objectId; + const bool registered; +public: + + /** + * Helper function to send Event Messages to the Event Manager + * @param event + * @param parameter1 + * @param parameter2 + */ + virtual void triggerEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0); + + /** + * @brief The class's constructor. + * @details In the constructor, the object id is set and the class is + * inserted in the object manager. + * @param setObjectId The id the object shall have. + * @param doRegister Determines if the object is registered in the global object manager. + */ + SystemObject(object_id_t setObjectId, bool doRegister = true); + /** + * @brief On destruction, the object removes itself from the list. + */ + virtual ~SystemObject(); + const object_id_t getObjectId() const; + virtual ReturnValue_t initialize(); + virtual ReturnValue_t checkObjectConnections(); + + virtual void forwardEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0) const; +}; + +#endif /* SYSTEMOBJECT_H_ */ diff --git a/objectmanager/SystemObjectIF.h b/objectmanager/SystemObjectIF.h new file mode 100644 index 00000000..a0c924fc --- /dev/null +++ b/objectmanager/SystemObjectIF.h @@ -0,0 +1,64 @@ +/** + * @file SystemObjectIF.h + * @brief This file contains the definition of the SystemObjectIF interface. + * @date 18.09.2012 + * @author Bastian Baetz + */ + +#ifndef SYSTEMOBJECTIF_H_ +#define SYSTEMOBJECTIF_H_ + +#include +#include +#include +/** + * \defgroup system_objects Software System Object Management + * The classes to create System Objects and classes to manage these are contained in this group. + * System Objects are software elements that can be controlled externally. They all have a unique + * object identifier. + */ + +/** + * This is the typedef for object identifiers. + * \ingroup system_objects + */ +typedef uint32_t object_id_t; + +/** + * This interface allows a class to be included in the object manager + * list. + * It does not provide any method definitions, still it is required to + * perform a type check with dynamic_cast. + * \ingroup system_objects + */ +class SystemObjectIF : public EventReportingProxyIF { +public: + /** + * This is a simple getter to return the object identifier. + * @return Returns the object id of this object. + */ + virtual const object_id_t getObjectId() const = 0; + /** + * The empty virtual destructor as required for C++ interfaces. + */ + virtual ~SystemObjectIF() { + } + /** + * Initializes all inter-object dependencies. + * This is necessary to avoid circular dependencies of not-fully + * initialized objects on start up. + * @return - \c RETURN_OK in case the initialization was successful + * - \c RETURN_FAILED otherwise + */ + virtual ReturnValue_t initialize() = 0; + /** + * Checks, if all object-object interconnections are satisfying for operation. + * Some objects need certain other objects (or a certain number), to be registered as children. + * These checks can be done in this method. + * @return - \c RETURN_OK in case the check was successful + * - \c any other code otherwise + */ + virtual ReturnValue_t checkObjectConnections() = 0; +}; + +#endif /* SYSTEMOBJECTIF_H_ */ diff --git a/osal/CpuUsage.cpp b/osal/CpuUsage.cpp new file mode 100644 index 00000000..40ac34aa --- /dev/null +++ b/osal/CpuUsage.cpp @@ -0,0 +1,183 @@ +#include +#include +#include +#include + +extern "C" { +#include +} + +int handlePrint(void * token, const char *format, ...) { + CpuUsage *cpuUsage = (CpuUsage *) token; + + if (cpuUsage->counter == 0) { + //header + cpuUsage->counter++; + return 0; + } + + if (cpuUsage->counter % 2 == 1) { + { + //we can not tell when the last call is so we assume it be every uneven time + va_list vl; + va_start(vl, format); + float timeSinceLastReset = va_arg(vl,uint32_t); + uint32_t timeSinceLastResetDecimals = va_arg(vl,uint32_t); + + timeSinceLastReset = timeSinceLastReset + + (timeSinceLastResetDecimals / 1000.); + + cpuUsage->timeSinceLastReset = timeSinceLastReset; + + va_end(vl); + } + //task name and id + va_list vl; + va_start(vl, format); + + cpuUsage->cachedValue.id = va_arg(vl,uint32_t); + const char *name = va_arg(vl,const char *); + memcpy(cpuUsage->cachedValue.name, name, + CpuUsage::ThreadData::MAX_LENGTH_OF_THREAD_NAME); + + va_end(vl); + + } else { + //statistics + va_list vl; + va_start(vl, format); + float run = va_arg(vl,uint32_t); + uint32_t runDecimals = va_arg(vl,uint32_t); + float percent = va_arg(vl,uint32_t); + uint32_t percent_decimals = va_arg(vl,uint32_t); + + run = run + (runDecimals / 1000.); + percent = percent + (percent_decimals / 1000.); + + cpuUsage->cachedValue.percentUsage = percent; + cpuUsage->cachedValue.timeRunning = run; + + cpuUsage->threadData.insert(cpuUsage->cachedValue); + + va_end(vl); + } + cpuUsage->counter++; + + return 0; +} + +CpuUsage::CpuUsage() : + counter(0), timeSinceLastReset(0) { + +} + +CpuUsage::~CpuUsage() { + +} + +void CpuUsage::resetCpuUsage() { + rtems_cpu_usage_reset(); +} + +void CpuUsage::read() { + rtems_cpu_usage_report_with_plugin(this, &handlePrint); +} + +void CpuUsage::clear() { + counter = 0; + timeSinceLastReset = 0; + threadData.clear(); +} + +ReturnValue_t CpuUsage::serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + ReturnValue_t result = SerializeAdapter::serialize( + &timeSinceLastReset, buffer, size, max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return SerialArrayListAdapter::serialize(&threadData, buffer, + size, max_size, bigEndian); +} + +uint32_t CpuUsage::getSerializedSize() const { + uint32_t size = 0; + + size += sizeof(timeSinceLastReset); + size += SerialArrayListAdapter::getSerializedSize(&threadData); + + return size; +} + +ReturnValue_t CpuUsage::deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + ReturnValue_t result = SerializeAdapter::deSerialize( + &timeSinceLastReset, buffer, size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return SerialArrayListAdapter::deSerialize(&threadData, buffer, + size, bigEndian); +} + +ReturnValue_t CpuUsage::ThreadData::serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + ReturnValue_t result = SerializeAdapter::serialize(&id, buffer, + size, max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if (*size + MAX_LENGTH_OF_THREAD_NAME > max_size) { + return BUFFER_TOO_SHORT; + } + memcpy(*buffer, name, MAX_LENGTH_OF_THREAD_NAME); + *size += MAX_LENGTH_OF_THREAD_NAME; + *buffer += MAX_LENGTH_OF_THREAD_NAME; + result = SerializeAdapter::serialize(&timeRunning, + buffer, size, max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = SerializeAdapter::serialize(&percentUsage, + buffer, size, max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return HasReturnvaluesIF::RETURN_OK; +} + +uint32_t CpuUsage::ThreadData::getSerializedSize() const { + uint32_t size = 0; + + size += sizeof(id); + size += MAX_LENGTH_OF_THREAD_NAME; + size += sizeof(timeRunning); + size += sizeof(percentUsage); + + return size; +} + +ReturnValue_t CpuUsage::ThreadData::deSerialize(const uint8_t** buffer, + int32_t* size, bool bigEndian) { + ReturnValue_t result = SerializeAdapter::deSerialize(&id, buffer, + size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if ((*size = *size - MAX_LENGTH_OF_THREAD_NAME) < 0) { + return STREAM_TOO_SHORT; + } + memcpy(name, *buffer, MAX_LENGTH_OF_THREAD_NAME); + *buffer -= MAX_LENGTH_OF_THREAD_NAME; + result = SerializeAdapter::deSerialize(&timeRunning, + buffer, size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = SerializeAdapter::deSerialize(&percentUsage, + buffer, size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/osal/CpuUsage.h b/osal/CpuUsage.h new file mode 100644 index 00000000..3d317e4e --- /dev/null +++ b/osal/CpuUsage.h @@ -0,0 +1,53 @@ +#ifndef CPUUSAGE_H_ +#define CPUUSAGE_H_ + +#include +#include +#include + +class CpuUsage : public SerializeIF { +public: + static const uint8_t MAXIMUM_NUMBER_OF_THREADS = 30; + + class ThreadData: public SerializeIF { + public: + static const uint8_t MAX_LENGTH_OF_THREAD_NAME = 4; + + uint32_t id; + char name[MAX_LENGTH_OF_THREAD_NAME]; + float timeRunning; + float percentUsage; + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const; + + virtual uint32_t getSerializedSize() const; + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian); + }; + + CpuUsage(); + virtual ~CpuUsage(); + + uint8_t counter; + float timeSinceLastReset; + FixedArrayList threadData; + ThreadData cachedValue; + + static void resetCpuUsage(); + + void read(); + + void clear(); + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const; + + virtual uint32_t getSerializedSize() const; + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian); +}; + +#endif /* CPUUSAGE_H_ */ diff --git a/osal/InternalErrorCodes.cpp b/osal/InternalErrorCodes.cpp new file mode 100644 index 00000000..4be890dc --- /dev/null +++ b/osal/InternalErrorCodes.cpp @@ -0,0 +1,59 @@ +#include +#include + +ReturnValue_t InternalErrorCodes::translate(uint8_t code) { + switch (code) { + case INTERNAL_ERROR_NO_CONFIGURATION_TABLE: + return NO_CONFIGURATION_TABLE; + case INTERNAL_ERROR_NO_CPU_TABLE: + return NO_CPU_TABLE; + case INTERNAL_ERROR_INVALID_WORKSPACE_ADDRESS: + return INVALID_WORKSPACE_ADDRESS; + case INTERNAL_ERROR_TOO_LITTLE_WORKSPACE: + return TOO_LITTLE_WORKSPACE; + case INTERNAL_ERROR_WORKSPACE_ALLOCATION: + return WORKSPACE_ALLOCATION; + case INTERNAL_ERROR_INTERRUPT_STACK_TOO_SMALL: + return INTERRUPT_STACK_TOO_SMALL; + case INTERNAL_ERROR_THREAD_EXITTED: + return THREAD_EXITTED; + case INTERNAL_ERROR_INCONSISTENT_MP_INFORMATION: + return INCONSISTENT_MP_INFORMATION; + case INTERNAL_ERROR_INVALID_NODE: + return INVALID_NODE; + case INTERNAL_ERROR_NO_MPCI: + return NO_MPCI; + case INTERNAL_ERROR_BAD_PACKET: + return BAD_PACKET; + case INTERNAL_ERROR_OUT_OF_PACKETS: + return OUT_OF_PACKETS; + case INTERNAL_ERROR_OUT_OF_GLOBAL_OBJECTS: + return OUT_OF_GLOBAL_OBJECTS; + case INTERNAL_ERROR_OUT_OF_PROXIES: + return OUT_OF_PROXIES; + case INTERNAL_ERROR_INVALID_GLOBAL_ID: + return INVALID_GLOBAL_ID; + case INTERNAL_ERROR_BAD_STACK_HOOK: + return BAD_STACK_HOOK; + case INTERNAL_ERROR_BAD_ATTRIBUTES: + return BAD_ATTRIBUTES; + case INTERNAL_ERROR_IMPLEMENTATION_KEY_CREATE_INCONSISTENCY: + return IMPLEMENTATION_KEY_CREATE_INCONSISTENCY; + case INTERNAL_ERROR_IMPLEMENTATION_BLOCKING_OPERATION_CANCEL: + return IMPLEMENTATION_BLOCKING_OPERATION_CANCEL; + case INTERNAL_ERROR_MUTEX_OBTAIN_FROM_BAD_STATE: + return MUTEX_OBTAIN_FROM_BAD_STATE; + case INTERNAL_ERROR_UNLIMITED_AND_MAXIMUM_IS_0: + return UNLIMITED_AND_MAXIMUM_IS_0; + default: + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +InternalErrorCodes::InternalErrorCodes() { +} + +InternalErrorCodes::~InternalErrorCodes() { + +} + diff --git a/osal/InternalErrorCodes.h b/osal/InternalErrorCodes.h new file mode 100644 index 00000000..2c4f2d2c --- /dev/null +++ b/osal/InternalErrorCodes.h @@ -0,0 +1,39 @@ +#ifndef INTERNALERRORCODES_H_ +#define INTERNALERRORCODES_H_ + +#include + +class InternalErrorCodes { +public: + static const uint8_t INTERFACE_ID = INTERNAL_ERROR_CODES; + +static const ReturnValue_t NO_CONFIGURATION_TABLE = MAKE_RETURN_CODE(0x01 ); +static const ReturnValue_t NO_CPU_TABLE = MAKE_RETURN_CODE(0x02 ); +static const ReturnValue_t INVALID_WORKSPACE_ADDRESS = MAKE_RETURN_CODE(0x03 ); +static const ReturnValue_t TOO_LITTLE_WORKSPACE = MAKE_RETURN_CODE(0x04 ); +static const ReturnValue_t WORKSPACE_ALLOCATION = MAKE_RETURN_CODE(0x05 ); +static const ReturnValue_t INTERRUPT_STACK_TOO_SMALL = MAKE_RETURN_CODE(0x06 ); +static const ReturnValue_t THREAD_EXITTED = MAKE_RETURN_CODE(0x07 ); +static const ReturnValue_t INCONSISTENT_MP_INFORMATION = MAKE_RETURN_CODE(0x08 ); +static const ReturnValue_t INVALID_NODE = MAKE_RETURN_CODE(0x09 ); +static const ReturnValue_t NO_MPCI = MAKE_RETURN_CODE(0x0a ); +static const ReturnValue_t BAD_PACKET = MAKE_RETURN_CODE(0x0b ); +static const ReturnValue_t OUT_OF_PACKETS = MAKE_RETURN_CODE(0x0c ); +static const ReturnValue_t OUT_OF_GLOBAL_OBJECTS = MAKE_RETURN_CODE(0x0d ); +static const ReturnValue_t OUT_OF_PROXIES = MAKE_RETURN_CODE(0x0e ); +static const ReturnValue_t INVALID_GLOBAL_ID = MAKE_RETURN_CODE(0x0f ); +static const ReturnValue_t BAD_STACK_HOOK = MAKE_RETURN_CODE(0x10 ); +static const ReturnValue_t BAD_ATTRIBUTES = MAKE_RETURN_CODE(0x11 ); +static const ReturnValue_t IMPLEMENTATION_KEY_CREATE_INCONSISTENCY = MAKE_RETURN_CODE(0x12 ); +static const ReturnValue_t IMPLEMENTATION_BLOCKING_OPERATION_CANCEL = MAKE_RETURN_CODE(0x13 ); +static const ReturnValue_t MUTEX_OBTAIN_FROM_BAD_STATE = MAKE_RETURN_CODE(0x14 ); +static const ReturnValue_t UNLIMITED_AND_MAXIMUM_IS_0 = MAKE_RETURN_CODE(0x15 ); + + virtual ~InternalErrorCodes(); + + static ReturnValue_t translate(uint8_t code); +private: + InternalErrorCodes(); +}; + +#endif /* INTERNALERRORCODES_H_ */ diff --git a/osal/OSAL.cpp b/osal/OSAL.cpp new file mode 100644 index 00000000..6c8507f4 --- /dev/null +++ b/osal/OSAL.cpp @@ -0,0 +1,282 @@ +/** + * @file OSAL.cpp + * @brief This file defines the OSAL class. + * @date 19.12.2012 + * @author baetz + */ + +#include + +#ifndef API +#error Please specify Operating System API. Supported: API=RTEMS_API +#elif API == RTEMS_API + +#include +//#include + +ReturnValue_t OSAL::convertReturnCode(uint8_t inValue) { + if (inValue == RTEMS_SUCCESSFUL) { + return OSAL::RETURN_OK; + } else { + return MAKE_RETURN_CODE(inValue); + } +} + +Name_t OSAL::buildName(uint8_t c1, uint8_t c2, uint8_t c3, uint8_t c4) { + return rtems_build_name(c1, c2, c3, c4); +} + +Interval_t OSAL::getTicksPerSecond() { + Interval_t ticks_per_second; + (void) rtems_clock_get(RTEMS_CLOCK_GET_TICKS_PER_SECOND, &ticks_per_second); + return ticks_per_second; +} + +ReturnValue_t OSAL::setClock(TimeOfDay_t* time) { + ReturnValue_t status = rtems_clock_set(time); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::setClock(timeval* time) { + timespec newTime; + newTime.tv_sec = time->tv_sec; + newTime.tv_nsec = time->tv_usec * TOD_NANOSECONDS_PER_MICROSECOND; + //TODO: Not sure if we need to protect this call somehow (by thread lock or something). + _TOD_Set(&newTime); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t OSAL::getClock_timeval(timeval* time) { + ReturnValue_t status = rtems_clock_get_tod_timeval(time); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::getClock_usecs(uint64_t* time) { + timeval temp_time; + uint8_t returnValue = rtems_clock_get_tod_timeval(&temp_time); + *time = ((uint64_t) temp_time.tv_sec * 1000000) + temp_time.tv_usec; + return OSAL::convertReturnCode(returnValue); +} + +ReturnValue_t OSAL::getDateAndTime(TimeOfDay_t* time) { + ReturnValue_t status = rtems_clock_get_tod(time); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::sleepFor(Interval_t ticks) { + ReturnValue_t status = rtems_task_wake_after(ticks); + status = convertReturnCode(status); + return status; +} +ReturnValue_t OSAL::createTask(Name_t name, TaskPriority_t initial_priority, + size_t stack_size, OpusMode_t initial_modes, Attribute_t attribute_set, + TaskId_t* id) { + if (initial_priority >= 0 && initial_priority <= 99) { + ReturnValue_t status = rtems_task_create(name, + (0xFF - 2 * initial_priority), stack_size, initial_modes, + attribute_set, id); + status = convertReturnCode(status); + return status; + } else { + return OSAL::UNSATISFIED; + } +} + +ReturnValue_t OSAL::findTask(Name_t name, TaskId_t* id) { + ReturnValue_t status = rtems_task_ident(name, RTEMS_SEARCH_ALL_NODES, id); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::startTask(TaskId_t* id, TaskEntry_t entry_point, + TaskArgument_t argument) { + return rtems_task_start(*id, entry_point, argument); +} + +ReturnValue_t OSAL::deleteTask(TaskId_t* id) { + if (id == RTEMS_SELF) { + return rtems_task_delete(RTEMS_SELF); + } else { + return rtems_task_delete(*id); + } +} + +ReturnValue_t OSAL::setAndStartPeriod(Interval_t period, PeriodId_t *periodId, + Name_t name) { + uint8_t status; + status = rtems_rate_monotonic_create(name, periodId); + if (status == RTEMS_SUCCESSFUL) { + //This is necessary to avoid a call with period = 0, which does not start the period. + status = rtems_rate_monotonic_period(*periodId, period + 1); + } + return OSAL::convertReturnCode(status); +} + +ReturnValue_t OSAL::checkAndRestartPeriod(PeriodId_t periodId, + Interval_t period) { + ReturnValue_t status = rtems_rate_monotonic_period(periodId, period); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::deletePeriod(TaskId_t* id) { + rtems_name period_name; + rtems_id period_id; + rtems_object_get_classic_name(*id, &period_name); + period_name = (period_name & 0xFFFFFF00) + 0x50; + rtems_rate_monotonic_ident(period_name, &period_id); + ReturnValue_t status = rtems_rate_monotonic_delete(period_id); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::reportPeriodStatistics() { + rtems_rate_monotonic_report_statistics(); + return OSAL::SUCCESSFUL; +} + +ReturnValue_t OSAL::createMessageQueue(Name_t name, uint32_t count, + size_t max_message_size, Attribute_t attribute_set, + MessageQueueId_t* id) { + ReturnValue_t status = rtems_message_queue_create(name, count, + max_message_size, attribute_set, id); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::findMessageQueue(Name_t name, MessageQueueId_t* id) { + ReturnValue_t status = rtems_message_queue_ident(name, + RTEMS_SEARCH_ALL_NODES, id); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::sendMessage(MessageQueueId_t id, const void* buffer, + size_t size) { + ReturnValue_t status = rtems_message_queue_send(id, buffer, size); + return OSAL::convertReturnCode(status); +} + +ReturnValue_t OSAL::receiveMessage(MessageQueueId_t id, void* buffer, + size_t bufSize, size_t* recSize, Option_t option_set, + Interval_t timeout) { + ReturnValue_t status = rtems_message_queue_receive(id, buffer, recSize, + option_set, timeout); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::flushMessageQueue(MessageQueueId_t id, uint32_t* count) { + ReturnValue_t status = rtems_message_queue_flush(id, count); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::deleteMessageQueue(MessageQueueId_t* id) { + ReturnValue_t status = rtems_message_queue_delete(*id); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::createMutex(Name_t name, MutexId_t* id) { + ReturnValue_t status = rtems_semaphore_create(name, 1, + RTEMS_SIMPLE_BINARY_SEMAPHORE, 0, id); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::deleteMutex(MutexId_t* id) { + ReturnValue_t status = rtems_semaphore_delete(*id); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::lockMutex(MutexId_t* id, Interval_t timeout) { + ReturnValue_t status = rtems_semaphore_obtain(*id, WAIT, timeout); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::getUptime(timeval* uptime) { + timespec time; + ReturnValue_t status = rtems_clock_get_uptime(&time); + uptime->tv_sec = time.tv_sec; + time.tv_nsec = time.tv_nsec / 1000; + uptime->tv_usec = time.tv_nsec; + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::getUptime(uint32_t* uptimeMs) { + *uptimeMs = rtems_clock_get_ticks_since_boot(); + return RTEMS_SUCCESSFUL; +} + +ReturnValue_t OSAL::unlockMutex(MutexId_t* id) { + ReturnValue_t status = rtems_semaphore_release(*id); + status = convertReturnCode(status); + return status; +} + +ReturnValue_t OSAL::setInterruptServiceRoutine(IsrHandler_t handler, + InterruptNumber_t interrupt, IsrHandler_t *oldHandler) { + IsrHandler_t oldHandler_local; + if (oldHandler == NULL) { + oldHandler = &oldHandler_local; + } + //+ 0x10 comes because of trap type assignment to IRQs in UT699 processor + ReturnValue_t status = rtems_interrupt_catch(handler, interrupt + 0x10, + oldHandler); + status = convertReturnCode(status); + return status; +} + +//TODO: Make default values (edge, polarity) settable? +ReturnValue_t OSAL::enableInterrupt(InterruptNumber_t interrupt) { + volatile uint32_t* irqMask = hw_irq_mask; + uint32_t expectedValue = *irqMask | (1 << interrupt); + *irqMask = expectedValue; + uint32_t tempValue = *irqMask; + if (tempValue == expectedValue) { + volatile hw_gpio_port_t* ioPorts = hw_gpio_port; + ioPorts->direction &= ~(1 << interrupt); //Direction In + ioPorts->interrupt_edge |= 1 << interrupt; //Edge triggered + ioPorts->interrupt_polarity |= 1 << interrupt; //Trigger on rising edge + ioPorts->interrupt_mask |= 1 << interrupt; //Enable + return RETURN_OK; + } else { + return RETURN_FAILED; + } +} + +ReturnValue_t OSAL::disableInterrupt(InterruptNumber_t interrupt) { + volatile uint32_t* irqMask = hw_irq_mask; + uint32_t expectedValue = *irqMask & ~(1 << interrupt); + *irqMask = expectedValue; + uint32_t tempValue = *irqMask; + if (tempValue == expectedValue) { + //Disable gpio IRQ + volatile hw_gpio_port_t* ioPorts = hw_gpio_port; + ioPorts->interrupt_mask &= ~(1 << interrupt); + return RETURN_OK; + } else { + return RETURN_FAILED; + } +} + +bool OSAL::isInterruptInProgress() { + return rtems_interrupt_is_in_progress(); +} + +ReturnValue_t OSAL::convertTimeOfDayToTimeval(const TimeOfDay_t* from, + timeval* to) { + //Fails in 2038.. + to->tv_sec = _TOD_To_seconds(from); + to->tv_usec = 0; + return RETURN_OK; +} + +#endif /* API */ diff --git a/osal/OSAL.h b/osal/OSAL.h new file mode 100644 index 00000000..01ab2c4d --- /dev/null +++ b/osal/OSAL.h @@ -0,0 +1,430 @@ +/** + * @file OSAL.h + * @brief This file defines the OSAL class. + * @date 19.12.2012 + * @author baetz + */ + +#ifndef OSAL_H_ +#define OSAL_H_ + +#include + +#define RTEMS_API 1 + +#ifndef API +#error Please specify Operating System API. Supported: API=RTEMS_API +#elif API == RTEMS_API +#include +#include +#include +#include +#include +#include + +//include RMAP + +#define opus_main Init2 +#define opus_main_arguments rtems_task_argument + +//Typedefs for RTEMS: //TODO: these are global, shouldn't they have some prefix? +/** + * A typedef to pass options to RTEMS elements. + */ +typedef rtems_option Option_t; +/** + * A typedef to pass attributes to RTEMS elements. + */ +typedef rtems_attribute Attribute_t; +/** + * A typedef for RTEMS task modes. + */ +typedef rtems_mode OpusMode_t; +/** + * A typedef for time intervals. In clock ticks. + */ +typedef rtems_interval Interval_t; +typedef rtems_time_of_day TimeOfDay_t; +typedef rtems_id TaskId_t; +typedef rtems_id PeriodId_t; +typedef rtems_id MutexId_t; +typedef rtems_id MessageQueueId_t; +typedef rtems_name Name_t; + +typedef rtems_task_argument TaskArgument_t; +typedef rtems_task_entry TaskEntry_t; +typedef rtems_task TaskReturn_t; +typedef rtems_task_priority TaskPriority_t; +typedef rtems_isr_entry IsrHandler_t; +typedef rtems_isr IsrReturn_t; +typedef rtems_vector_number InterruptNumber_t; + +/** + * This class contains encapsulates all System Calls to the Operating System. + * It is currently tailored to the RTEMS Operating system, but should in general + * support different OS. + * For more detailed information on the Operating System calls, please refer to the + * RTEMS documentation at www.rtems.org . + */ +class OSAL: public HasReturnvaluesIF { +private: + /** + * A method to convert an OS-specific return code to the frameworks return value concept. + * @param inValue The return code coming from the OS. + * @return The converted return value. + */ + static ReturnValue_t convertReturnCode(uint8_t inValue); +public: + + static const uint8_t INTERFACE_ID = OPERATING_SYSTEM_ABSTRACTION; + //Interrupts: + static const uint32_t INTERRUPT_MASK_REGISTER_ADDRESS = 0x80000240; + //API Status codes: + static const ReturnValue_t SUCCESSFUL = RTEMS_SUCCESSFUL; + static const ReturnValue_t TASK_EXITTED = + MAKE_RETURN_CODE( RTEMS_TASK_EXITTED ); + static const ReturnValue_t MP_NOT_CONFIGURED = + MAKE_RETURN_CODE( RTEMS_MP_NOT_CONFIGURED ); + static const ReturnValue_t INVALID_NAME = + MAKE_RETURN_CODE( RTEMS_INVALID_NAME ); + static const ReturnValue_t INVALID_ID = MAKE_RETURN_CODE( RTEMS_INVALID_ID ); + static const ReturnValue_t TOO_MANY = MAKE_RETURN_CODE( RTEMS_TOO_MANY ); + static const ReturnValue_t TIMEOUT = MAKE_RETURN_CODE( RTEMS_TIMEOUT ); + static const ReturnValue_t OBJECT_WAS_DELETED = + MAKE_RETURN_CODE( RTEMS_OBJECT_WAS_DELETED ); + static const ReturnValue_t INVALID_SIZE = + MAKE_RETURN_CODE( RTEMS_INVALID_SIZE ); + static const ReturnValue_t INVALID_ADDRESS = + MAKE_RETURN_CODE( RTEMS_INVALID_ADDRESS ); + static const ReturnValue_t INVALID_NUMBER = + MAKE_RETURN_CODE( RTEMS_INVALID_NUMBER ); + static const ReturnValue_t NOT_DEFINED = + MAKE_RETURN_CODE( RTEMS_NOT_DEFINED ); + static const ReturnValue_t RESOURCE_IN_USE = + MAKE_RETURN_CODE( RTEMS_RESOURCE_IN_USE ); + static const ReturnValue_t UNSATISFIED = + MAKE_RETURN_CODE( RTEMS_UNSATISFIED ); + static const ReturnValue_t QUEUE_EMPTY = + MAKE_RETURN_CODE( RTEMS_UNSATISFIED ); + static const ReturnValue_t INCORRECT_STATE = + MAKE_RETURN_CODE( RTEMS_INCORRECT_STATE ); + static const ReturnValue_t ALREADY_SUSPENDED = + MAKE_RETURN_CODE( RTEMS_ALREADY_SUSPENDED ); + static const ReturnValue_t ILLEGAL_ON_SELF = + MAKE_RETURN_CODE( RTEMS_ILLEGAL_ON_SELF ); + static const ReturnValue_t ILLEGAL_ON_REMOTE_OBJECT = + MAKE_RETURN_CODE( RTEMS_ILLEGAL_ON_REMOTE_OBJECT ); + static const ReturnValue_t CALLED_FROM_ISR = + MAKE_RETURN_CODE( RTEMS_CALLED_FROM_ISR ); + static const ReturnValue_t INVALID_PRIORITY = + MAKE_RETURN_CODE( RTEMS_INVALID_PRIORITY ); + static const ReturnValue_t INVALID_CLOCK = + MAKE_RETURN_CODE( RTEMS_INVALID_CLOCK ); + static const ReturnValue_t INVALID_NODE = + MAKE_RETURN_CODE( RTEMS_INVALID_NODE ); + static const ReturnValue_t NOT_CONFIGURED = + MAKE_RETURN_CODE( RTEMS_NOT_CONFIGURED ); + static const ReturnValue_t NOT_OWNER_OF_RESOURCE = + MAKE_RETURN_CODE( RTEMS_NOT_OWNER_OF_RESOURCE ); + static const ReturnValue_t NOT_IMPLEMENTED = + MAKE_RETURN_CODE( RTEMS_NOT_IMPLEMENTED ); + static const ReturnValue_t INTERNAL_ERROR = + MAKE_RETURN_CODE( RTEMS_INTERNAL_ERROR ); + static const ReturnValue_t NO_MEMORY = MAKE_RETURN_CODE( RTEMS_NO_MEMORY ); + static const ReturnValue_t IO_ERROR = MAKE_RETURN_CODE( RTEMS_IO_ERROR ); + //API options: + static const Option_t DEFAULT_OPTIONS = RTEMS_DEFAULT_OPTIONS; + static const Option_t WAIT = RTEMS_WAIT; + static const Option_t NO_WAIT = RTEMS_NO_WAIT; + static const Option_t EVENT_ALL = RTEMS_EVENT_ALL; + static const Option_t EVENT_ANY = RTEMS_EVENT_ANY; + //API Attributes: + static const Attribute_t DEFAULT_ATTRIBUTES = RTEMS_DEFAULT_ATTRIBUTES; + static const Attribute_t LOCAL = RTEMS_LOCAL; + static const Attribute_t GLOBAL = RTEMS_GLOBAL; + static const Attribute_t FIFO = RTEMS_FIFO; + static const Attribute_t PRIORITY = RTEMS_PRIORITY; + static const Attribute_t NO_FLOATING_POINT = RTEMS_NO_FLOATING_POINT; + static const Attribute_t FLOATING_POINT = RTEMS_FLOATING_POINT; + //API Modes: + static const OpusMode_t ALL_MODE_MASKS = RTEMS_ALL_MODE_MASKS; + static const OpusMode_t DEFAULT_MODES = RTEMS_DEFAULT_MODES; + static const OpusMode_t CURRENT_MODE = RTEMS_CURRENT_MODE; + static const OpusMode_t PREEMPT = RTEMS_PREEMPT; + static const OpusMode_t NO_PREEMPT = RTEMS_NO_PREEMPT; + static const OpusMode_t NO_TIMESLICE = RTEMS_NO_TIMESLICE; + static const OpusMode_t TIMESLICE = RTEMS_TIMESLICE; + static const OpusMode_t ASR = RTEMS_ASR; + static const OpusMode_t NO_ASR = RTEMS_NO_ASR; + //API Time and Timing + static const Interval_t MILISECOND_WAIT = 1; + static const Interval_t NO_TIMEOUT = RTEMS_NO_TIMEOUT; + static const TaskId_t TASK_MYSELF = RTEMS_SELF; + static const size_t MINIMUM_STACK_SIZE = RTEMS_MINIMUM_STACK_SIZE; + /** + * This is a helper method to build a qualified name out of single characters + * @param c1 The first character + * @param c2 The second character + * @param c3 The third character + * @param c4 The fourth character + * @return A name suitable for use for the Operating System + */ + static Name_t buildName(uint8_t c1, uint8_t c2, uint8_t c3, uint8_t c4); + /** + * This method returns the number of clock ticks per second. + * In RTEMS, this is typically 1000. + * @return The number of ticks. + */ + static Interval_t getTicksPerSecond(void); + /** + * This system call sets the system time. + * To set the time, it uses a TimeOfDay_t struct. + * @param time The struct with the time settings to set. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t setClock(TimeOfDay_t* time); + /** + * This system call sets the system time. + * To set the time, it uses a timeval struct. + * @param time The struct with the time settings to set. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t setClock(timeval* time); + /** + * This system call returns the current system clock in timeval format. + * The timval format has the fields \c tv_sec with seconds and \c tv_usec with + * microseconds since an OS-defined epoch. + * @param time A pointer to a timeval struct where the current time is stored. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t getClock_timeval(timeval* time); + + /** + * Get the time since boot in a timeval struct + * + * @param[out] time A pointer to a timeval struct where the uptime is stored. + * @return\c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t getUptime(timeval* uptime); + + /** + * Get the time since boot in milliseconds + * + * This value can overflow! Still, it can be used to calculate time intervalls + * between two calls up to 49 days by always using uint32_t in the calculation + * + * @param ms uptime in ms + * @return RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t getUptime(uint32_t* uptimeMs); + + /** + * Returns the time in microseconds since an OS-defined epoch. + * The time is returned in a 64 bit unsigned integer. + * @param time A pointer to a 64 bit unisigned integer where the data is stored. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t getClock_usecs(uint64_t* time); + /** + * Returns the time in a TimeOfDay_t struct. + * @param time A pointer to a TimeOfDay_t struct. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t getDateAndTime(TimeOfDay_t* time); + /** + * Converts a time of day struct to POSIX seconds. + * @param time The time of day as input + * @param timeval The corresponding seconds since the epoch. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t convertTimeOfDayToTimeval(const TimeOfDay_t* from, timeval* to); + /** + * Commands the calling task to sleep for a certain number of clock ticks. + * Typically other tasks are executed then. + * @param ticks The number of clock ticks to sleep. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t sleepFor(Interval_t ticks); + + /** + * With this call, a new task is created. + * The task is created (its resources are acquired), but it is not started. + * @param name A name as specified in the OS. + * @param initial_priority The task's priority. Ranging from 0 (lowest) to 99 (highest). + * @param stack_size The stack size reserved for this task. + * @param initial_modes Options for the task's mode (preemptible, time slicing, etc..) + * @param attribute_set Options for the task's attributes (floating point, local/global) + * @param id The OS returns the task id, that uniquely identifies the task. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t createTask(Name_t name, + TaskPriority_t initial_priority, size_t stack_size, + OpusMode_t initial_modes, Attribute_t attribute_set, TaskId_t *id); + /** + * Finds a task with the help of its name. + * @param name Name of the task to find + * @param id The OS returns the task id, that uniquely identifies the task. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t findTask(Name_t name, TaskId_t *id); + /** + * Starts a task. + * The task immediately starts running. + * @param id The task id. + * @param entry_point A pointer to the (static) method that is executed by the task. + * @param argument One argument (a void* pointer) may be passed to the task. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t startTask(TaskId_t *id, TaskEntry_t entry_point, + TaskArgument_t argument); + /** + * With this call, tasks are deleted from the system + * @param id The task id. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t deleteTask(TaskId_t *id); + /** + * Checks if the current executing context is an ISR. + * @return true if handling an interrupt, false else. + */ + static bool isInterruptInProgress(); + /** + * An task is not executed periodically by default. + * This is activated with this call. This is managed internally. + * @param period The task's period in clock ticks. + * @param[out] periodId The newly created period's id + * @param name optional name for the period + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t setAndStartPeriod(Interval_t period, PeriodId_t *periodId, Name_t name = (('P' << 24) + ('e' << 16) + ('r' << 8) + 'd')); + /** + * This call must be made in a periodic task, when activities of one cycle are finished. + * This is managed internally. + * @param periodId Id of the period as returned by setAndStartPeriod() + * @param period The period duration for the next cycle. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t checkAndRestartPeriod(PeriodId_t periodId, Interval_t period); + /** + * This call deletes the period. + * This is managed internally. + * @param id Pointer to the task identifier the period belongs to. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t deletePeriod(TaskId_t *id); + /** + * With this call the period statistics (and therefore the periodic task + * statistics) are printed to the screen. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t reportPeriodStatistics(); + + /** + * With this call, a new message queue is created. + * @param name A qualified name for the message queue. + * @param count Number of messages the queue can store before it rejects new messages. + * @param max_message_size Maximum size of a single message. + * @param attribute_set Attributes for the message queue (fifo/priority, local/global) + * @param id A unique message queue identifier returned by the OS. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t createMessageQueue(Name_t name, uint32_t count, + size_t max_message_size, Attribute_t attribute_set, + MessageQueueId_t *id); + /** + * Returns a message queue id by its name. + * @param name The queue's name. + * @param id A pointer to the queue id to return to. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t findMessageQueue(Name_t name, MessageQueueId_t *id); + /** + * Sends a message to the queue given by id. + * @param id Id of the queue to send to + * @param buffer A pointer to any kind of data to send over the queue. + * @param size Size of the data to send. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t sendMessage(MessageQueueId_t id, const void *buffer, + size_t size); + /** + * Checks, if a message was received by a queue with identifier id. + * @param id The id of the checked task. + * @param buffer Pointer to the buffer to store to. + * @param bufSize Maximum size of the buffer. + * @param recSize The actual message size is returned here. + * @param option_set Specifies, if the task waits for a message (WAIT/ NO_WAIT). + * @param timeout If the task waits, this interval specifies how long (in clock ticks). + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t receiveMessage(MessageQueueId_t id, void *buffer, + size_t bufSize, size_t *recSize, Option_t option_set, + Interval_t timeout); + /** + * Deletes all pending messages in a certain queue. + * @param id Id of the queue to flush + * @param count Number of flushed messages. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t flushMessageQueue( MessageQueueId_t id, uint32_t* count ); + /** + * Deletes a message queue from the system. + * @param id Id of the queue to delete. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t deleteMessageQueue(MessageQueueId_t *id); + + /** + * Creates a new mutual exclusive lock (or semaphore). + * With these locks, concurrent access to system resources (data pool, ...) can be + * controlled. + * @param name A qualified name for the mutex. + * @param id The mutex's id as returned by the OS. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t createMutex(Name_t name, MutexId_t *id); + /** + * Deletes the mutex identified by id. + * @param id Id of the mutex to delete. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t deleteMutex(MutexId_t *id); + /** + * With this call, a task tries to acquire the mutex. + * Must be used in conjunction with unlockMutex. + * @param id Id of the mutex to acquire. + * @param timeout Specifies how long a task waits for the mutex. Default is NO_TIMEOUT. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t lockMutex(MutexId_t *id, Interval_t timeout); + /** + * Releases a mutex. + * Must be used in conjunction with lockMutex. + * @param id Id of the mutex to release. + * @return \c RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t unlockMutex(MutexId_t *id); + /** + * Establishes a new interrupt service routine. + * @param handler The service routine to establish + * @param interrupt The interrupt (NOT trap type) the routine shall react to. + * @return RETURN_OK on success. Otherwise, the OS failure code is returned. + */ + static ReturnValue_t setInterruptServiceRoutine(IsrHandler_t handler, InterruptNumber_t interrupt, IsrHandler_t *oldHandler = NULL ); + /** + * Enables the interrupt given. + * The function tests, if the InterruptMask register was written successfully. + * @param interrupt The interrupt to enable. + * @return RETURN_OK if the interrupt was set successfully. RETURN_FAILED else. + */ + static ReturnValue_t enableInterrupt( InterruptNumber_t interrupt ); + /** + * Disables the interrupt given. + * @param interrupt The interrupt to disable. + * @return RETURN_OK if the interrupt was set successfully. RETURN_FAILED else. + */ + static ReturnValue_t disableInterrupt( InterruptNumber_t interrupt ); +}; + +#endif /* API */ +#endif /* OSAL_H_ */ diff --git a/parameters/HasParametersIF.h b/parameters/HasParametersIF.h new file mode 100644 index 00000000..ad77b879 --- /dev/null +++ b/parameters/HasParametersIF.h @@ -0,0 +1,59 @@ +/* + * HasParametersIF.h + * + * Created on: 26.11.2015 + * Author: mohr + */ + +#ifndef HASPARAMETERSIF_H_ +#define HASPARAMETERSIF_H_ + +#include +#include +#include + +typedef uint32_t ParameterId_t; + +class HasParametersIF { +public: + static const uint8_t INTERFACE_ID = HAS_PARAMETERS_IF; + static const ReturnValue_t INVALID_MATRIX_ID = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t INVALID_DOMAIN_ID = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t INVALID_VALUE = MAKE_RETURN_CODE(0x03); + + + + + static uint8_t getDomain(ParameterId_t id){ + return id >> 24; + } + + static uint16_t getMatrixId(ParameterId_t id){ + return id >> 8; + } + + static uint8_t getIndex(ParameterId_t id){ + return id; + } + + + virtual ~HasParametersIF() { + } + + /** + * Always set parameter before checking newValues! + * + * + * @param domainId + * @param parameterId + * @param parameterWrapper + * @param newValues + * @param startAtIndex + * @return + */ + virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, + ParameterWrapper *parameterWrapper, + const ParameterWrapper *newValues, uint16_t startAtIndex) = 0; +}; + +#endif /* HASPARAMETERSIF_H_ */ diff --git a/parameters/ParameterHelper.cpp b/parameters/ParameterHelper.cpp new file mode 100644 index 00000000..f2aebdde --- /dev/null +++ b/parameters/ParameterHelper.cpp @@ -0,0 +1,138 @@ +/* + * ParameterHelper.cpp + * + * Created on: 28.11.2015 + * Author: mohr + */ + +#include +#include +#include + +ParameterHelper::ParameterHelper(ReceivesParameterMessagesIF* owner) : + owner(owner), storage(NULL) { + ownerQueueId = owner->getCommandQueue(); +} + +ParameterHelper::~ParameterHelper() { +} + +ReturnValue_t ParameterHelper::handleParameterMessage(CommandMessage *message) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED; + switch (message->getCommand()) { + case ParameterMessage::CMD_PARAMETER_DUMP: { + ParameterWrapper description; + uint8_t domain = HasParametersIF::getDomain( + ParameterMessage::getParameterId(message)); + uint16_t parameterId = HasParametersIF::getMatrixId( + ParameterMessage::getParameterId(message)); + ReturnValue_t result = owner->getParameter(domain, parameterId, + &description, &description, 0); + if (result == HasReturnvaluesIF::RETURN_OK) { + result = sendParameter(message->getSender(), + ParameterMessage::getParameterId(message), &description); + } + } + break; + case ParameterMessage::CMD_PARAMETER_LOAD: { + + uint8_t domain = HasParametersIF::getDomain( + ParameterMessage::getParameterId(message)); + uint16_t parameterId = HasParametersIF::getMatrixId( + ParameterMessage::getParameterId(message)); + uint8_t index = HasParametersIF::getIndex( + ParameterMessage::getParameterId(message)); + + const uint8_t *storedStream; + uint32_t storedStreamSize; + ReturnValue_t result = storage->getData( + ParameterMessage::getStoreId(message), &storedStream, + &storedStreamSize); + if (result != HasReturnvaluesIF::RETURN_OK) { + break; + } + + ParameterWrapper streamWrapper; + result = streamWrapper.set(storedStream, storedStreamSize); + if (result != HasReturnvaluesIF::RETURN_OK) { + storage->deleteData(ParameterMessage::getStoreId(message)); + break; + } + + ParameterWrapper ownerWrapper; + result = owner->getParameter(domain, parameterId, &ownerWrapper, + &streamWrapper, index); + if (result != HasReturnvaluesIF::RETURN_OK) { + storage->deleteData(ParameterMessage::getStoreId(message)); + break; + } + + result = ownerWrapper.copyFrom(&streamWrapper, index); + + storage->deleteData(ParameterMessage::getStoreId(message)); + + if (result == HasReturnvaluesIF::RETURN_OK) { + result = sendParameter(message->getSender(), + ParameterMessage::getParameterId(message), &ownerWrapper); + } + } + break; + default: + return HasReturnvaluesIF::RETURN_FAILED; + } + + if (result != HasReturnvaluesIF::RETURN_OK) { + rejectCommand(message->getSender(), result, message->getCommand()); + } + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t ParameterHelper::sendParameter(MessageQueueId_t to, uint32_t id, + const ParameterWrapper* description) { + uint32_t serializedSize = description->getSerializedSize(); + + uint8_t *storeElement; + store_address_t address; + + ReturnValue_t result = storage->getFreeElement(&address, serializedSize, + &storeElement); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + uint32_t storeElementSize = 0; + + result = description->serialize(&storeElement, &storeElementSize, + serializedSize, true); + + if (result != HasReturnvaluesIF::RETURN_OK) { + storage->deleteData(address); + return result; + } + + CommandMessage reply; + + ParameterMessage::setParameterDumpReply(&reply, id, address); + + MessageQueueSender sender; + sender.sendMessage(to, &reply, ownerQueueId); + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t ParameterHelper::initialize() { + storage = objectManager->get(objects::IPC_STORE); + if (storage == NULL) { + return HasReturnvaluesIF::RETURN_FAILED; + } else { + return HasReturnvaluesIF::RETURN_OK; + } +} + +void ParameterHelper::rejectCommand(MessageQueueId_t to, ReturnValue_t reason, Command_t initialCommand) { + CommandMessage reply; + reply.setReplyRejected(reason, initialCommand); + MessageQueueSender sender; + sender.sendMessage(to, &reply, ownerQueueId); +} diff --git a/parameters/ParameterHelper.h b/parameters/ParameterHelper.h new file mode 100644 index 00000000..d96a4794 --- /dev/null +++ b/parameters/ParameterHelper.h @@ -0,0 +1,34 @@ +/* + * ParameterHelper.h + * + * Created on: 28.11.2015 + * Author: mohr + */ + +#ifndef PARAMETERHELPER_H_ +#define PARAMETERHELPER_H_ + +#include +#include + +class ParameterHelper { +public: + ParameterHelper(ReceivesParameterMessagesIF *owner); + virtual ~ParameterHelper(); + + ReturnValue_t handleParameterMessage(CommandMessage *message); + + ReturnValue_t initialize(); +private: + ReceivesParameterMessagesIF *owner; + + MessageQueueId_t ownerQueueId; + + StorageManagerIF *storage; + + ReturnValue_t sendParameter(MessageQueueId_t to, uint32_t id, const ParameterWrapper *description); + + void rejectCommand(MessageQueueId_t to, ReturnValue_t reason, Command_t initialCommand); +}; + +#endif /* PARAMETERHELPER_H_ */ diff --git a/parameters/ParameterMessage.cpp b/parameters/ParameterMessage.cpp new file mode 100644 index 00000000..38dd81a0 --- /dev/null +++ b/parameters/ParameterMessage.cpp @@ -0,0 +1,38 @@ +/* + * ParameterMessage.cpp + * + * Created on: 28.11.2015 + * Author: mohr + */ + +#include + +ParameterId_t ParameterMessage::getParameterId(const CommandMessage* message) { + return message->getParameter(); +} + +store_address_t ParameterMessage::getStoreId(const CommandMessage* message) { + store_address_t address; + address.raw = message->getParameter2(); + return address; +} + +void ParameterMessage::setParameterDumpCommand(CommandMessage* message, + ParameterId_t id) { + message->setCommand(CMD_PARAMETER_DUMP); + message->setParameter(id); +} + +void ParameterMessage::setParameterDumpReply(CommandMessage* message, + ParameterId_t id, store_address_t storageID) { + message->setCommand(REPLY_PARAMETER_DUMP); + message->setParameter(id); + message->setParameter2(storageID.raw); +} + +void ParameterMessage::setParameterLoadCommand(CommandMessage* message, + ParameterId_t id, store_address_t storageID) { + message->setCommand(CMD_PARAMETER_LOAD); + message->setParameter(id); + message->setParameter2(storageID.raw); +} diff --git a/parameters/ParameterMessage.h b/parameters/ParameterMessage.h new file mode 100644 index 00000000..bf595352 --- /dev/null +++ b/parameters/ParameterMessage.h @@ -0,0 +1,35 @@ +/* + * ParameterMessage.h + * + * Created on: 28.11.2015 + * Author: mohr + */ + +#ifndef PARAMETERMESSAGE_H_ +#define PARAMETERMESSAGE_H_ + +#include +#include +#include + +class ParameterMessage { +private: + ParameterMessage(); +public: + static const uint8_t MESSAGE_ID = PARAMETER_MESSAGE_ID; + static const Command_t CMD_PARAMETER_LOAD = MAKE_COMMAND_ID( 0x01 ); + static const Command_t CMD_PARAMETER_DUMP = MAKE_COMMAND_ID( 0x02 ); + static const Command_t REPLY_PARAMETER_DUMP = MAKE_COMMAND_ID( 0x03 ); + + static ParameterId_t getParameterId(const CommandMessage* message); + static store_address_t getStoreId(const CommandMessage* message); + static void setParameterDumpCommand(CommandMessage* message, + ParameterId_t id); + static void setParameterDumpReply(CommandMessage* message, + ParameterId_t id, store_address_t storageID); + static void setParameterLoadCommand(CommandMessage* message, + ParameterId_t id, store_address_t storageID); + +}; + +#endif /* PARAMETERMESSAGE_H_ */ diff --git a/parameters/ParameterWrapper.cpp b/parameters/ParameterWrapper.cpp new file mode 100644 index 00000000..3b2f6685 --- /dev/null +++ b/parameters/ParameterWrapper.cpp @@ -0,0 +1,275 @@ +#include + +ParameterWrapper::ParameterWrapper() : + pointsToStream(false), type(Type::UNKNOWN_TYPE), rows(0), columns(0), data( + NULL), readonlyData(NULL) { +} + +ParameterWrapper::ParameterWrapper(Type type, uint8_t rows, uint8_t columns, + void* data) : + pointsToStream(false), type(type), rows(rows), columns(columns), data( + data), readonlyData(data) { +} + +ParameterWrapper::ParameterWrapper(Type type, uint8_t rows, uint8_t columns, + const void* data) : + pointsToStream(false), type(type), rows(rows), columns(columns), data( + NULL), readonlyData(data) { +} + +ParameterWrapper::~ParameterWrapper() { +} + +ReturnValue_t ParameterWrapper::serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + ReturnValue_t result; + + result = SerializeAdapter::serialize(&type, buffer, size, max_size, + bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + result = SerializeAdapter::serialize(&columns, buffer, size, + max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = SerializeAdapter::serialize(&rows, buffer, size, max_size, + bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + //serialize uses readonlyData, as it is always valid + if (readonlyData == NULL) { + return NOT_SET; + } + switch (type) { + case Type::UINT8_T: + result = serializeData(buffer, size, max_size, bigEndian); + break; + case Type::INT8_T: + result = serializeData(buffer, size, max_size, bigEndian); + break; + case Type::UINT16_T: + result = serializeData(buffer, size, max_size, bigEndian); + break; + case Type::INT16_T: + result = serializeData(buffer, size, max_size, bigEndian); + break; + case Type::UINT32_T: + result = serializeData(buffer, size, max_size, bigEndian); + break; + case Type::INT32_T: + result = serializeData(buffer, size, max_size, bigEndian); + break; + case Type::FLOAT: + result = serializeData(buffer, size, max_size, bigEndian); + break; + case Type::DOUBLE: + result = serializeData(buffer, size, max_size, bigEndian); + break; + default: + result = UNKNOW_DATATYPE; + break; + } + return result; +} + +uint32_t ParameterWrapper::getSerializedSize() const { + uint32_t serializedSize = 0; + serializedSize += type.getSerializedSize(); + serializedSize += sizeof(rows); + serializedSize += sizeof(columns); + serializedSize += rows * columns * type.getSize(); + + return serializedSize; +} + +template +ReturnValue_t ParameterWrapper::serializeData(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + const T *element = (const T*) readonlyData; + ReturnValue_t result; + uint16_t dataSize = columns * rows; + while (dataSize != 0) { + result = SerializeAdapter::serialize(element, buffer, size, max_size, + bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + element++; + dataSize--; + } + return result; +} + +template +ReturnValue_t ParameterWrapper::deSerializeData(uint8_t startingRow, + uint8_t startingColumn, const void *from, uint8_t fromRows, + uint8_t fromColumns) { + + //treat from as a continuous Stream as we copy all of it + const uint8_t *fromAsStream = (const uint8_t *) from; + int32_t streamSize = fromRows * fromColumns * sizeof(T); + + ReturnValue_t result; + + for (uint8_t fromRow = 0; fromRow < fromRows; fromRow++) { + + //get the start element of this row in data + T *dataWithDataType = ((T *) data) + + (((startingRow + fromRow) * columns) + startingColumn); + + for (uint8_t fromColumn = 0; fromColumn < fromColumns; fromColumn++) { + result = SerializeAdapter::deSerialize( + dataWithDataType + fromColumn, &fromAsStream, &streamSize, + true); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + } + } + + return result; + +} + +ReturnValue_t ParameterWrapper::deSerialize(const uint8_t** buffer, + int32_t* size, bool bigEndian) { + return deSerialize(buffer, size, bigEndian, 0); +} + +ReturnValue_t ParameterWrapper::deSerialize(const uint8_t** buffer, + int32_t* size, bool bigEndian, uint16_t startWritingAtIndex) { + ParameterWrapper streamDescription; + + ReturnValue_t result = streamDescription.set(*buffer, *size, buffer, size); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + return copyFrom(&streamDescription, startWritingAtIndex); +} + +ReturnValue_t ParameterWrapper::set(const uint8_t* stream, int32_t streamSize, + const uint8_t **remainingStream, int32_t *remainingSize) { + ReturnValue_t result = SerializeAdapter::deSerialize(&type, &stream, + &streamSize, true); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + result = SerializeAdapter::deSerialize(&columns, &stream, + &streamSize, true); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = SerializeAdapter::deSerialize(&rows, &stream, &streamSize, + true); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + int32_t dataSize = type.getSize() * rows * columns; + + if (streamSize < dataSize) { + return SerializeIF::STREAM_TOO_SHORT; + } + + data = NULL; + readonlyData = stream; + pointsToStream = true; + + stream += dataSize; + if (remainingStream != NULL) { + *remainingStream = stream; + } + streamSize -= dataSize; + if (remainingSize != NULL) { + *remainingSize = streamSize; + } + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper* from, + uint16_t startWritingAtIndex) { + if (data == NULL) { + return READONLY; + } + + if (from->readonlyData == NULL) { + return SOURCE_NOT_SET; + } + + if (type != from->type) { + return DATATYPE_MISSMATCH; + } + + //check if from fits into this + uint8_t startingRow = startWritingAtIndex / columns; + uint8_t startingColumn = startWritingAtIndex % columns; + + if ((from->rows > (rows - startingRow)) + || (from->columns > (columns - startingColumn))) { + return TOO_BIG; + } + + uint8_t typeSize = type.getSize(); + + ReturnValue_t result; + //copy data + if (from->pointsToStream) { + switch (type) { + case Type::UINT8_T: + result = deSerializeData(startingRow, startingColumn, + from->readonlyData, from->rows, from->columns); + break; + case Type::INT8_T: + result = deSerializeData(startingRow, startingColumn, + from->readonlyData, from->rows, from->columns); + break; + case Type::UINT16_T: + result = deSerializeData(startingRow, startingColumn, + from->readonlyData, from->rows, from->columns); + break; + case Type::INT16_T: + result = deSerializeData(startingRow, startingColumn, + from->readonlyData, from->rows, from->columns); + break; + case Type::UINT32_T: + result = deSerializeData(startingRow, startingColumn, + from->readonlyData, from->rows, from->columns); + break; + case Type::INT32_T: + result = deSerializeData(startingRow, startingColumn, + from->readonlyData, from->rows, from->columns); + break; + case Type::FLOAT: + result = deSerializeData(startingRow, startingColumn, + from->readonlyData, from->rows, from->columns); + break; + case Type::DOUBLE: + result = deSerializeData(startingRow, startingColumn, + from->readonlyData, from->rows, from->columns); + break; + default: + result = UNKNOW_DATATYPE; + break; + } + } else { + //need a type to do arithmetic + uint8_t *toDataWithType = (uint8_t *) data; + for (uint8_t fromRow = 0; fromRow < from->rows; fromRow++) { + memcpy( + toDataWithType + + (((startingRow + fromRow) * columns) + + startingColumn) * typeSize, + from->readonlyData, typeSize * from->columns); + } + } + + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/parameters/ParameterWrapper.h b/parameters/ParameterWrapper.h new file mode 100644 index 00000000..e4c62064 --- /dev/null +++ b/parameters/ParameterWrapper.h @@ -0,0 +1,139 @@ +#ifndef PARAMETERWRAPPER_H_ +#define PARAMETERWRAPPER_H_ + +#include +#include +#include +#include +#include +#include + +class ParameterWrapper: public SerializeIF { +public: + static const uint8_t INTERFACE_ID = PARAMETER_WRAPPER; + static const ReturnValue_t UNKNOW_DATATYPE = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t DATATYPE_MISSMATCH = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t READONLY = MAKE_RETURN_CODE(0x03); + static const ReturnValue_t TOO_BIG = MAKE_RETURN_CODE(0x04); + static const ReturnValue_t SOURCE_NOT_SET = MAKE_RETURN_CODE(0x05); + static const ReturnValue_t OUT_OF_BOUNDS = MAKE_RETURN_CODE(0x06); + static const ReturnValue_t NOT_SET = MAKE_RETURN_CODE(0x07); + + ParameterWrapper(); + ParameterWrapper(Type type, uint8_t rows, uint8_t columns, void *data); + ParameterWrapper(Type type, uint8_t rows, uint8_t columns, + const void *data); + virtual ~ParameterWrapper(); + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const; + + virtual uint32_t getSerializedSize() const; + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian); + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian, uint16_t startWritingAtIndex = 0); + + template + ReturnValue_t getElement(T *value, uint8_t row = 0, uint8_t column = 0) const { + if (readonlyData == NULL){ + return NOT_SET; + } + + if (PodTypeConversion::type != type) { + return DATATYPE_MISSMATCH; + } + + if ((row > rows) || (column > columns)) { + return OUT_OF_BOUNDS; + } + + if (pointsToStream) { + const uint8_t *streamWithtype = (const uint8_t *) readonlyData; + streamWithtype += (row * columns + column) * type.getSize(); + int32_t size = type.getSize(); + return SerializeAdapter::deSerialize(value, &streamWithtype, + &size, true); + } else { + const T *dataWithType = (const T *) readonlyData; + *value = dataWithType[row * columns + column]; + return HasReturnvaluesIF::RETURN_OK; + } + } + + template + void set(T *data, uint8_t rows, uint8_t columns) { + this->data = data; + this->readonlyData = data; + this->type = PodTypeConversion::type; + this->rows = rows; + this->columns = columns; + this->pointsToStream = false; + } + + template + void set(const T *readonlyData, uint8_t rows, uint8_t columns) { + this->data = NULL; + this->readonlyData = readonlyData; + this->type = PodTypeConversion::type; + this->rows = rows; + this->columns = columns; + this->pointsToStream = false; + } + + template + void set(T& member) { + this->set(&member, 1, 1); + } + + template + void set(const T& readonlyMember) { + this->set(&readonlyMember, 1, 1); + } + + template + void setVector(T& member) { + this->set(member, 1, sizeof(member)/sizeof(member[0])); + } + + template + void setVector(const T& member) { + this->set(member, 1, sizeof(member)/sizeof(member[0])); + } + template + void setMatrix(T& member) { + this->set(member[0], sizeof(member)/sizeof(member[0]), sizeof(member[0])/sizeof(member[0][0])); + } + + template + void setMatrix(const T& member) { + this->set(member[0], sizeof(member)/sizeof(member[0]), sizeof(member[0])/sizeof(member[0][0])); + } + ReturnValue_t set(const uint8_t *stream, int32_t streamSize, + const uint8_t **remainingStream = NULL, int32_t *remainingSize = + NULL); + + ReturnValue_t copyFrom(const ParameterWrapper *from, + uint16_t startWritingAtIndex); + +private: + bool pointsToStream; + + Type type; + uint8_t rows; + uint8_t columns; + void *data; + const void *readonlyData; + + template + ReturnValue_t serializeData(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const; + + template + ReturnValue_t deSerializeData(uint8_t startingRow, uint8_t startingColumn, + const void *from, uint8_t fromRows, uint8_t fromColumns); +}; + +#endif /* PARAMETERWRAPPER_H_ */ diff --git a/parameters/ReceivesParameterMessagesIF.h b/parameters/ReceivesParameterMessagesIF.h new file mode 100644 index 00000000..b76d2967 --- /dev/null +++ b/parameters/ReceivesParameterMessagesIF.h @@ -0,0 +1,18 @@ +#ifndef RECEIVESPARAMETERMESSAGESIF_H_ +#define RECEIVESPARAMETERMESSAGESIF_H_ + + +#include +#include + +class ReceivesParameterMessagesIF : public HasParametersIF { +public: + + virtual ~ReceivesParameterMessagesIF() { + } + + virtual MessageQueueId_t getCommandQueue() const = 0; +}; + + +#endif /* RECEIVESPARAMETERMESSAGESIF_H_ */ diff --git a/power/HasPowerSwitchIF.h b/power/HasPowerSwitchIF.h new file mode 100644 index 00000000..55a54b47 --- /dev/null +++ b/power/HasPowerSwitchIF.h @@ -0,0 +1,20 @@ +/** + * @file HasPowerSwitchIF.h + * @brief This file defines the HasPowerSwitchIF class. + * @date 25.02.2014 + * @author baetz + */ +#ifndef HASPOWERSWITCHIF_H_ +#define HASPOWERSWITCHIF_H_ + + +class HasPowerSwitchIF { +public: + virtual ~HasPowerSwitchIF() {} + virtual ReturnValue_t getSwitches(uint8_t *firstSwitch, + uint8_t *secondSwitch) = 0; + virtual void getPowerLimit(float* low, float* high) = 0; +}; + + +#endif /* HASPOWERSWITCHIF_H_ */ diff --git a/power/PowerSwitchIF.h b/power/PowerSwitchIF.h new file mode 100644 index 00000000..884d13a6 --- /dev/null +++ b/power/PowerSwitchIF.h @@ -0,0 +1,75 @@ +/** + * @file PowerSwitchIF.h + * @brief This file defines the PowerSwitchIF class. + * @date 20.03.2013 + * @author baetz + */ + +#ifndef POWERSWITCHIF_H_ +#define POWERSWITCHIF_H_ + +#include +#include +/** + * This interface defines a connection to a device that is capable of turning on and off + * switches of devices identified by a switch ID. + */ +class PowerSwitchIF : public HasReturnvaluesIF { +public: + /** + * Empty dtor. + */ + virtual ~PowerSwitchIF() { + + } + /** + * The Returnvalues id of this class, required by HasReturnvaluesIF + */ + static const uint8_t INTERFACE_ID = POWER_SWITCH_IF; + static const ReturnValue_t SWITCH_ON = MAKE_RETURN_CODE(1); + static const ReturnValue_t SWITCH_OFF = MAKE_RETURN_CODE(0); + static const ReturnValue_t SWITCH_TIMEOUT = MAKE_RETURN_CODE(2); + static const ReturnValue_t FUSE_ON = MAKE_RETURN_CODE(3); + static const ReturnValue_t FUSE_OFF = MAKE_RETURN_CODE(4); + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PCDU_2; + static const Event SWITCH_WENT_OFF = MAKE_EVENT(0, SEVERITY::LOW); //!< Someone detected that a switch went off which shouldn't. Severity: Low, Parameter1: switchId1, Parameter2: switchId2 TODO: CHANGED EVENT_ID! + /** + * send a direct command to the Power Unit to enable/disable the specified switch. + * + * @param switchNr + * @param onOff on == @c SWITCH_ON; off != @c SWITCH_ON + */ + virtual void sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) const = 0; + /** + * Sends a command to the Power Unit to enable a certain fuse. + */ + virtual void sendFuseOnCommand(uint8_t fuseNr) const = 0; + + /** + * get the state of the Switches. + * @param switchNr + * @return + * - @c SWITCH_ON if the specified switch is on. + * - @c SWITCH_OFF if the specified switch is off. + * - @c RETURN_FAILED if an error occured + */ + virtual ReturnValue_t getSwitchState( uint8_t switchNr ) const = 0; + /** + * get state of a fuse. + * @param fuseNr + * @return + * - @c FUSE_ON if the specified fuse is on. + * - @c FUSE_OFF if the specified fuse is off. + * - @c RETURN_FAILED if an error occured + */ + virtual ReturnValue_t getFuseState( uint8_t fuseNr ) const = 0; + /** + * The maximum delay that it will take to change a switch + * + * This may take into account the time to send a command, wait for it to be executed and see the switch changed. + */ + virtual uint32_t getSwitchDelayMs(void) const = 0; +}; + + +#endif /* POWERSWITCHIF_H_ */ diff --git a/power/PowerSwitcher.cpp b/power/PowerSwitcher.cpp new file mode 100644 index 00000000..b3110e59 --- /dev/null +++ b/power/PowerSwitcher.cpp @@ -0,0 +1,134 @@ +/* + * PowerSwitcher.cpp + * + * Created on: 21.01.2014 + * Author: baetz + */ + +#include +#include +#include + +PowerSwitcher::PowerSwitcher(uint8_t setSwitch1, uint8_t setSwitch2, + PowerSwitcher::State_t setStartState) : + state(setStartState), firstSwitch(setSwitch1), secondSwitch(setSwitch2), power(NULL) { +} + +ReturnValue_t PowerSwitcher::initialize() { + power = objectManager->get(objects::PCDU_HANDLER); + if (power == NULL) { + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t PowerSwitcher::getStateOfSwitches() { + SwitchReturn_t result = howManySwitches(); + switch (result) { + case ONE_SWITCH: + return power->getSwitchState(firstSwitch); + case TWO_SWITCHES: + if ((power->getSwitchState(firstSwitch) == PowerSwitchIF::SWITCH_ON) + && (power->getSwitchState(secondSwitch) == PowerSwitchIF::SWITCH_ON)) { + return PowerSwitchIF::SWITCH_ON; + } else if ((power->getSwitchState(firstSwitch) == PowerSwitchIF::SWITCH_OFF) + && (power->getSwitchState(secondSwitch) == PowerSwitchIF::SWITCH_OFF)) { + return PowerSwitchIF::SWITCH_OFF; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } + default: + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +void PowerSwitcher::commandSwitches(ReturnValue_t onOff) { + SwitchReturn_t result = howManySwitches(); + switch (result) { + case TWO_SWITCHES: + power->sendSwitchCommand(secondSwitch, onOff); + /*NO BREAK*/ + case ONE_SWITCH: + power->sendSwitchCommand(firstSwitch, onOff); + break; + } + return; + +} + +void PowerSwitcher::turnOn() { + commandSwitches(PowerSwitchIF::SWITCH_ON); + state = WAIT_ON; +} + +void PowerSwitcher::turnOff() { + commandSwitches(PowerSwitchIF::SWITCH_OFF); + state = WAIT_OFF; +} + +PowerSwitcher::SwitchReturn_t PowerSwitcher::howManySwitches() { + if (secondSwitch == NO_SWITCH) { + return ONE_SWITCH; + } else { + return TWO_SWITCHES; + } +} + +void PowerSwitcher::doStateMachine() { + switch (state) { + case SWITCH_IS_OFF: + case SWITCH_IS_ON: + //Do nothing. + break; + case WAIT_OFF: + if (getStateOfSwitches() == PowerSwitchIF::SWITCH_OFF) { + state = SWITCH_IS_OFF; + } + break; + case WAIT_ON: + if (getStateOfSwitches() == PowerSwitchIF::SWITCH_ON) { + state = SWITCH_IS_ON; + } + break; + default: + //Should never happen. + break; + } +} + +ReturnValue_t PowerSwitcher::checkSwitchState() { + switch (state) { + case WAIT_OFF: + case WAIT_ON: + return IN_POWER_TRANSITION; + case SWITCH_IS_OFF: + if (getStateOfSwitches() == PowerSwitchIF::SWITCH_OFF) { + return RETURN_OK; + } else { + return SWITCH_STATE_MISMATCH; + } + case SWITCH_IS_ON: + if (getStateOfSwitches() == PowerSwitchIF::SWITCH_ON) { + return RETURN_OK; + } else { + return SWITCH_STATE_MISMATCH; + } + } + return RETURN_FAILED; +} + +PowerSwitcher::State_t PowerSwitcher::getState() { + return state; +} + +uint32_t PowerSwitcher::getSwitchDelay() { + return power->getSwitchDelayMs(); +} + +uint8_t PowerSwitcher::getFirstSwitch() const { + return firstSwitch; +} + +uint8_t PowerSwitcher::getSecondSwitch() const { + return secondSwitch; +} diff --git a/power/PowerSwitcher.h b/power/PowerSwitcher.h new file mode 100644 index 00000000..115daf7a --- /dev/null +++ b/power/PowerSwitcher.h @@ -0,0 +1,52 @@ +/* + * PowerSwitcher.h + * + * Created on: 21.01.2014 + * Author: baetz + */ + +#ifndef POWERSWITCHER_H_ +#define POWERSWITCHER_H_ +#include +#include +#include + +class PowerSwitcher : public HasReturnvaluesIF { +public: + enum State_t { + WAIT_OFF, + WAIT_ON, + SWITCH_IS_OFF, + SWITCH_IS_ON, + }; + State_t state; + static const uint8_t INTERFACE_ID = POWER_SWITCHER; + static const ReturnValue_t IN_POWER_TRANSITION = MAKE_RETURN_CODE(1); + static const ReturnValue_t SWITCH_STATE_MISMATCH = MAKE_RETURN_CODE(2); + PowerSwitcher( uint8_t setSwitch1, uint8_t setSwitch2 = NO_SWITCH, State_t setStartState = SWITCH_IS_OFF ); + ReturnValue_t initialize(); + void turnOn(); + void turnOff(); + void doStateMachine(); + State_t getState(); + ReturnValue_t checkSwitchState(); + uint32_t getSwitchDelay(); + uint8_t getFirstSwitch() const; + uint8_t getSecondSwitch() const; +private: + uint8_t firstSwitch; + uint8_t secondSwitch; + PowerSwitchIF* power; + static const uint8_t NO_SWITCH = 0xFF; + enum SwitchReturn_t { + ONE_SWITCH = 1, + TWO_SWITCHES = 2 + }; + ReturnValue_t getStateOfSwitches(); + void commandSwitches( ReturnValue_t onOff ); + SwitchReturn_t howManySwitches(); +}; + + + +#endif /* POWERSWITCHER_H_ */ diff --git a/privatepool/PrivatePoolEntry.h b/privatepool/PrivatePoolEntry.h new file mode 100644 index 00000000..65a4abc6 --- /dev/null +++ b/privatepool/PrivatePoolEntry.h @@ -0,0 +1,85 @@ +/* + * PrivatePoolEntry.h + * + * Created on: 13.03.2014 + * Author: baetz + */ + +#ifndef PRIVATEPOOLENTRY_H_ +#define PRIVATEPOOLENTRY_H_ + +#include +#include +#include +#include +#include + +template +class PrivatePoolEntry: public PrivatePoolIF, public LinkedElement { +public: + PrivatePoolEntry() : LinkedElement(this), + address(0), element(0) { + } + PrivatePoolEntry(uint32_t address, T value) : LinkedElement(this), + address(address), element(value) { + } + + uint32_t address; + + T element; + + operator T() { + return element; + } + + PrivatePoolEntry &operator =(T value) { + element = value; + return *this; + } + ReturnValue_t handleMemoryLoad(uint32_t address, const uint8_t* data, uint32_t size, uint8_t** dataPointer) { + return handleMemoryDump(address, size, dataPointer, NULL); + } + ReturnValue_t handleMemoryDump(uint32_t address, uint32_t size, uint8_t** dataPointer, uint8_t* dumpTarget ) { + if (this->address != address) { + return INVALID_ADDRESS; + } + if (size != sizeof(element)) { + return INVALID_SIZE; + } + *dataPointer = (uint8_t*)&element; + return POINTS_TO_VARIABLE; + } + ReturnValue_t setAddress(uint32_t* setAddress ) { + address = *setAddress; + return HasReturnvaluesIF::RETURN_OK; + } + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + return SerializeAdapter::serialize(&element, buffer, size, max_size, bigEndian); + } + + uint32_t getSerializedSize() const { + return SerializeAdapter::getSerializedSize(&element); + } + + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return SerializeAdapter::deSerialize(&element, buffer, size, bigEndian); + + } +}; + +template +class PrivatePoolEntry::Is>::type> : public LinkedElement, public T { +public: + //TODO: Have a look on how std::forward works. + template + PrivatePoolEntry(Args... args) : LinkedElement(this), T(std::forward(args)...) { + + } + virtual ~PrivatePoolEntry() { + + } +}; + +#endif /* PRIVATEPOOLENTRY_H_ */ diff --git a/privatepool/PrivatePoolIF.h b/privatepool/PrivatePoolIF.h new file mode 100644 index 00000000..7b7e43d2 --- /dev/null +++ b/privatepool/PrivatePoolIF.h @@ -0,0 +1,22 @@ +/* + * PrivatePoolIF.h + * + * Created on: 20.03.2014 + * Author: baetz + */ + +#ifndef PRIVATEPOOLIF_H_ +#define PRIVATEPOOLIF_H_ +#include +#include + +class PrivatePoolIF : public HasMemoryIF, public SerializeIF { +public: + virtual ~PrivatePoolIF() { + + } +}; + + + +#endif /* PRIVATEPOOLIF_H_ */ diff --git a/privatepool/SetAddressListAdapter.h b/privatepool/SetAddressListAdapter.h new file mode 100644 index 00000000..ea85c0f8 --- /dev/null +++ b/privatepool/SetAddressListAdapter.h @@ -0,0 +1,29 @@ +/* + * SetAddressListAdapter.h + * + * Created on: 24.04.2014 + * Author: baetz + */ + +#ifndef SETADDRESSLISTADAPTER_H_ +#define SETADDRESSLISTADAPTER_H_ +#include +#include + +template +class SetAddressListAdapter { +public: + static ReturnValue_t setAddresses(const LinkedElement* element, uint32_t startFrom) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + while ((result == HasReturnvaluesIF::RETURN_OK) && (element != NULL)) { + result = element->value->setAddress(&startFrom); + element = element->getNext(); + startFrom++; + } + return result; + } +}; + + + +#endif /* SETADDRESSLISTADAPTER_H_ */ diff --git a/returnvalues/HasReturnvaluesIF.h b/returnvalues/HasReturnvaluesIF.h new file mode 100644 index 00000000..1ce13557 --- /dev/null +++ b/returnvalues/HasReturnvaluesIF.h @@ -0,0 +1,107 @@ +/* + * HasReturnvaluesIF.h + * + * Created on: 05.11.2012 + * Author: mohr + */ + +#ifndef HASRETURNVALUESIF_H_ +#define HASRETURNVALUESIF_H_ + +#include + +#define MAKE_RETURN_CODE( number ) ((INTERFACE_ID << 8) + (number)) +typedef uint16_t ReturnValue_t; + +//TODO: make one storageIF containing all possible returnvalues for storing things + +//A list of current interface id's. May be an enum. May be put in a separate header file. +#define OPERATING_SYSTEM_ABSTRACTION 0x01 //OS +#define OBJECT_MANAGER_IF 0x02 //OM +#define DEVICE_HANDLER_BASE 0x03 //DHB +#define RMAP_CHANNEL 0x04 //RMP +#define POWER_SWITCH_IF 0x05 //PS +#define HAS_MEMORY_IF 0x06 //PP +#define DEVICE_STATE_MACHINE_BASE 0x07 //DSMB +#define DATA_SET_CLASS 0x08 //DPS +#define POOL_RAW_ACCESS_CLASS 0x09 //DPR +#define CONTROLLER_BASE 0x0A //CTR +#define SUBSYSTEM_BASE 0x0B //SB +#define MODE_STORE_IF 0x0C //MS +#define SUBSYSTEM 0x0D //SS +#define HAS_MODES_IF 0x0E //HM +#define COMMAND_MESSAGE 0x0F //CM +#define CCSDS_TIME_HELPER_CLASS 0x10 //TIM +#define STAR_TRACKER_HANDLER 0x11 //STR +#define ARRAY_LIST 0x12 //AL +#define ASSEMBLY_BASE 0x13 //AB +#define MEMORY_HELPER 0x14 //MH +#define PCDU_HANDLER_CLASS 0x15 //PHC +#define RW_HANDLER_CLASS 0x16 //RWC +#define PLOC_CC_CLASS 0x17 //PCC +#define SERIALIZE_IF 0x18 //SE +#define FIXED_MAP 0x19 //FM +#define HAS_HEALTH_IF 0x1A //HHI +#define FRAME_FINDER_IF 0x1B //FFI +#define TM_VG_GENERATOR_IF 0x1C //VCG +#define FIFO_CLASS 0x1D //FF +#define MESSAGE_PROXY 0x1E //MQP +#define TRIPLE_REDUNDACY_CHECK 0x1F //TRC +#define TC_PACKET_CHECK 0x20 //TCC +#define PACKET_DISTRIBUTION 0x21 //TCD +#define ACCEPTS_TELECOMMANDS_IF 0x22 //PUS +#define DEVICE_SERVICE_BASE 0x23 //DSB +#define FUNCTION_MANAGEMENT_SERVICE 0x24 //FM +#define HOUSEKEEPING_SERVICE 0x25 //HK +#define MEMORY_MANAGEMENT_SERVICE 0x26 //MM +#define COMMAND_SERVICE_BASE 0x27 //CSB +#define OPERATIONS_SCHEDULING_SERVICE 0x28 //SCH +#define TM_STORE_BACKEND_IF 0x29 //TMB +#define TM_STORE_FRONTEND_IF 0x2A //TMF +#define STORAGE_AND_RETRIEVAL_SERVICE 0x2B //SR +#define MATCH_TREE_CLASS 0x2C //MT +#define EVENT_MANAGER_IF 0x2D //EV +#define HANDLES_FAILURES_IF 0x2E //FDI +#define DEVICE_HANDLER_IF 0x2F //DHI +#define STORAGE_MANAGER_IF 0x30 //SM +#define THERMAL_COMPONENT_IF 0x31 //TC +#define THERMAL_CONTROLLER_CLASS 0x33 //TCC +#define FOG_HANDLER_CLASS 0x34 //FHC +#define INTERNAL_ERROR_CODES 0x35 //IEC +#define TRAP 0x36 //TRP +#define PAYLOAD_HANDLING 0x37 //PLH +#define GPS_MONITORING 0x3A //GPSM +#define SUS_MONITORING 0x3B //SUSM +#define MGMMGT_MONITORING 0x3C //MGMT +#define FOG_MONITORING 0x3D //FOGM +#define STR_MONITORING 0x3E //STRM +#define DEPLOYMENT_CONTROLLER_CLASS 0x3F //DPLC +#define CCSDS_HANDLER_IF 0x40 //CCS +#define ACS_CONTROLLER_CLASS 0x41 //ACS +#define SGP4PROPAGATOR_CLASS 0x42 //SGP +#define PARAMETER_WRAPPER 0x43 //PAW +#define HAS_PARAMETERS_IF 0x44 //HPA +#define ASCII_CONVERTER 0x50 //ASC +#define POWER_SWITCHER 0x51 //POS +#define CCSDS_BOARD_HANDLER 0x52 //CBH +#define LIMITS_IF 0x60 //LIM +#define COMMANDS_ACTIONS_IF 0x80 //CF +#define HAS_ACTIONS_IF 0x81 //HF +#define DEVICE_COMMUNICATION_IF 0x90 //DC +#define BSP 0xF0 //BSP + + + + +class HasReturnvaluesIF { +public: + static const ReturnValue_t RETURN_OK = 0; + static const ReturnValue_t RETURN_FAILED = 1; + virtual ~HasReturnvaluesIF() { + } + +}; + + + +#endif /* HASRETURNVALUESIF_H_ */ diff --git a/rmap/RMAP.cpp b/rmap/RMAP.cpp new file mode 100644 index 00000000..23fe1eae --- /dev/null +++ b/rmap/RMAP.cpp @@ -0,0 +1,91 @@ +#include +#include +#include +#include +#include + +ReturnValue_t RMAP::reset(RMAPCookie* cookie) { + return cookie->channel->reset(); +} + +//TODO Check for channel == NULL +//Done. BB +RMAP::RMAP(){ + +} + +ReturnValue_t RMAP::sendWriteCommand(RMAPCookie *cookie, uint8_t* buffer, + uint32_t length) { + uint8_t instruction; + + if ((buffer == NULL) && (length != 0)) { + return DeviceCommunicationIF::NULLPOINTER; + } + if (cookie->getChannel() == NULL) { + return COMMAND_NO_CHANNEL; + } + instruction = RMAP_COMMAND_WRITE | cookie->command_mask; + return cookie->getChannel()->sendCommand(cookie, instruction, buffer, + length); + +} + +ReturnValue_t RMAP::getWriteReply(RMAPCookie *cookie) { + if (cookie->getChannel() == NULL) { + return COMMAND_NO_CHANNEL; + } + if (cookie->header.instruction & (1 << RMAP_COMMAND_BIT_WRITE)) { + return cookie->getChannel()->getReply(cookie, NULL, NULL); + } else { + return REPLY_MISSMATCH; + } +} + +ReturnValue_t RMAP::writeBlocking(RMAPCookie *cookie, uint8_t* buffer, + uint32_t length, uint32_t timeout_us) { + if (cookie->getChannel() == NULL) { + return COMMAND_NO_CHANNEL; + } + return cookie->getChannel()->sendCommandBlocking(cookie, buffer, length, + NULL, NULL, timeout_us); + +} +ReturnValue_t RMAP::sendReadCommand(RMAPCookie *cookie, uint32_t expLength) { + uint8_t command; + if (cookie->getChannel() == NULL) { + return COMMAND_NO_CHANNEL; + } + command = RMAP_COMMAND_READ + | (cookie->command_mask & ~(1 << RMAP_COMMAND_BIT_VERIFY)); + + return cookie->getChannel()->sendCommand(cookie, command, NULL, expLength); + +} + +ReturnValue_t RMAP::getReadReply(RMAPCookie *cookie, uint8_t **buffer, + uint32_t *size) { + if (cookie->getChannel() == NULL) { + return COMMAND_NO_CHANNEL; + } + if (buffer == NULL || size == NULL) { + return DeviceCommunicationIF::NULLPOINTER; + } + if (cookie->header.instruction & (1 << RMAP_COMMAND_BIT_WRITE)) { + return REPLY_MISSMATCH; + } else { + return cookie->getChannel()->getReply(cookie, buffer, size); + } + +} + +ReturnValue_t RMAP::readBlocking(RMAPCookie *cookie, uint32_t expLength, + uint8_t** buffer, uint32_t *size, uint32_t timeout_us) { + if (cookie->getChannel() == NULL) { + return COMMAND_NO_CHANNEL; + } + if (buffer == NULL || size == NULL) { + return DeviceCommunicationIF::NULLPOINTER; + } + return cookie->getChannel()->sendCommandBlocking(cookie, NULL, expLength, + buffer, size, timeout_us); +} diff --git a/rmap/RMAP.h b/rmap/RMAP.h new file mode 100644 index 00000000..15d4aa64 --- /dev/null +++ b/rmap/RMAP.h @@ -0,0 +1,226 @@ +#ifndef RMAPpp_H_ +#define RMAPpp_H_ + +#include +#include + +//TODO: clean up includes for RMAP, should be enough to include RMAP.h but right now it's quite chaotic... + +/** + * API for a Cookie/Channel based RMAP implementation. + * + * The API offers the four basic RMAP actions: sending a Read or Write command and getting the reply to each. + * As RMAP is an asynchronous protocol, these are implemented as four seperate calls. There are blocking + * calls which combine a send and get call using a timeout, but these are "real" blocking, looping in between + * the calls. + * + * Cookies are used to contain Information between a send[Read,Write]Command and a get[Read,Write]Reply call, + * one can think of them like *nix file descriptors. A cookie is valid from a sendX call until the related getX + * call. That means if a cookie is used in a getX call, the reply to the sendX call the cookie was used with + * previously is returned. + * Depending on the underlying RMAPChannel implementation, a cookie can also be valid for more than one getX + * call, but this should not be assumed generally. + * A cookie encapsulates information like the RMAP Channel to use, as well as the RMAP Address + * and a Command Mask used for specifying which RMAP capabilities are used. + * Cookies are created without interaction with this API, there is no open() call. The RMAP implementation + * will initialize all fields which are not set by the cookie's constructor. + * + * The RMAP implementation relies on Channels. A channel is used to construct the RMAP Headers and handle the + * protocol. It is access via the RMAPChannelIF. Thus it is possible to use different Implementations which only + * need to implement the RMAPChannelIF. A channel is also responsible for accessing the lower layers, for example + * a SPaceWire transport layer. + * + * There is one RMAP Channel per physical device. The cookie-channel as well as the device-channel assignment + * can be changed at runtime to allow for example redundancy switching. This API is static as the information which + * channel to use is contained within the cookie. + */ +class RMAP: public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = RMAP_CHANNEL; + + //static const ReturnValue_t COMMAND_OK = MAKE_RETURN_CODE(0x00); + static const ReturnValue_t COMMAND_NO_DESCRIPTORS_AVAILABLE = + MAKE_RETURN_CODE(0xE1); //no descriptors available for sending command; command was not sent + static const ReturnValue_t COMMAND_BUFFER_FULL = MAKE_RETURN_CODE(0xE2); //no receiver buffer available for expected len; command was not sent + static const ReturnValue_t COMMAND_CHANNEL_OUT_OF_RANGE = MAKE_RETURN_CODE( + 0xE3); //The cookie points to an invalid channel; command was not sent +//Replaced by DeviceCommunicationIF::TOO_MUCH_DATA static const ReturnValue_t COMMAND_TOO_BIG = MAKE_RETURN_CODE(0xE4); //the data that was to be sent was too long for the hw to handle (write command) or the expected len was bigger than maximal expected len (read command) + //command was not sent +//replaced by DeviceCommunicationIF::NULLPOINTER static const ReturnValue_t COMMAND_NULLPOINTER = MAKE_RETURN_CODE(0xE5); //datalen was != 0 but data was == NULL in write command, or nullpointer in read command + static const ReturnValue_t COMMAND_CHANNEL_DEACTIVATED = MAKE_RETURN_CODE( + 0xE6); //the channel has no port set + static const ReturnValue_t COMMAND_PORT_OUT_OF_RANGE = MAKE_RETURN_CODE( + 0xE7); //The specified port is not valid + static const ReturnValue_t COMMAND_PORT_IN_USE = MAKE_RETURN_CODE(0xE8);//The specified port is already in use + static const ReturnValue_t COMMAND_NO_CHANNEL = MAKE_RETURN_CODE(0xE9);//The cookie to work with has no channel assigned. + //return values for both get_write_reply and get_read_reply + static const ReturnValue_t REPLY_NO_REPLY = MAKE_RETURN_CODE(0xD0); //no reply was received + static const ReturnValue_t REPLY_NOT_SENT = MAKE_RETURN_CODE(0xD1); //command was not sent, implies no reply + static const ReturnValue_t REPLY_NOT_YET_SENT = MAKE_RETURN_CODE(0xD2); //command is still waiting to be sent + static const ReturnValue_t REPLY_MISSMATCH = MAKE_RETURN_CODE(0xD3);//a read command was issued, but get_write_rply called, or other way round + static const ReturnValue_t REPLY_TIMEOUT = MAKE_RETURN_CODE(0xD4);//timeout +//replaced by DeviceCommunicationIF::NULLPOINTER static const ReturnValue_t REPLY_NULLPOINTER = MAKE_RETURN_CODE(0xD5);//one of the arguments in a read reply was NULL + //return values for get_reply + static const ReturnValue_t REPLY_INTERFACE_BUSY = MAKE_RETURN_CODE( + 0xC0);//Interface is busy (transmission buffer still being processed) + static const ReturnValue_t REPLY_TRANSMISSION_ERROR = + MAKE_RETURN_CODE(0xC1); //Interface encountered errors during last operation, data could not be processed. (transmission error) + static const ReturnValue_t REPLY_INVALID_DATA = MAKE_RETURN_CODE( + 0xC2); //Invalid data (amount / value) + static const ReturnValue_t REPLY_NOT_SUPPORTED = MAKE_RETURN_CODE( + 0xC3); + + //return values for reset + static const ReturnValue_t LINK_DOWN = MAKE_RETURN_CODE(0xF0);//The spw link is down + //Other SpW codes: + static const ReturnValue_t SPW_CREDIT = MAKE_RETURN_CODE(0xF1); + static const ReturnValue_t SPW_ESCAPE = MAKE_RETURN_CODE(0xF2); + static const ReturnValue_t SPW_DISCONNECT = MAKE_RETURN_CODE(0xF3); + static const ReturnValue_t SPW_PARITY = MAKE_RETURN_CODE(0xF4); + static const ReturnValue_t SPW_WRITE_SYNC = MAKE_RETURN_CODE(0xF5); + static const ReturnValue_t SPW_INVALID_ADDRESS = MAKE_RETURN_CODE(0xF6); + static const ReturnValue_t SPW_EARLY_EOP = MAKE_RETURN_CODE(0xF7); + static const ReturnValue_t SPW_DMA = MAKE_RETURN_CODE(0xF8); + static const ReturnValue_t SPW_LINK_ERROR = MAKE_RETURN_CODE(0xF9); + + + //RMAP standard replies + static const ReturnValue_t REPLY_OK = MAKE_RETURN_CODE(0); + static const ReturnValue_t REPLY_GENERAL_ERROR_CODE = MAKE_RETURN_CODE(1);// The detected error does not fit into the other + // error cases or the node does not support + // further distinction between the errors + static const ReturnValue_t REPLY_UNUSED_PACKET_TYPE_OR_COMMAND_CODE = + MAKE_RETURN_CODE(2); // The Header CRC was decoded correctly but + // the packet type is reserved or the command + // is not used by the RMAP protocol. + static const ReturnValue_t REPLY_INVALID_KEY = MAKE_RETURN_CODE(3); // The Header CRC was decoded correctly but + // the device key did not match that expected + // by the target user application + static const ReturnValue_t REPLY_INVALID_DATA_CRC = MAKE_RETURN_CODE(4);// Error in the CRC of the data field + static const ReturnValue_t REPLY_EARLY_EOP = MAKE_RETURN_CODE(5);// EOP marker detected before the end of the data + static const ReturnValue_t REPLY_TOO_MUCH_DATA = MAKE_RETURN_CODE(6);// More than the expected amount of data in a + // command has been received + static const ReturnValue_t REPLY_EEP = MAKE_RETURN_CODE(7); // EEP marker detected immediately after the + // header CRC or during the transfer of data + // and Data CRC or immediately thereafter. + // Indicates that there was a communication + // failure of some sort on the network + static const ReturnValue_t REPLY_RESERVED = MAKE_RETURN_CODE(8);// Reserved + static const ReturnValue_t REPLY_VERIFY_BUFFER_OVERRUN = MAKE_RETURN_CODE( + 9); // The verify before write bit of the command + // was set so that the data field was buffered in + // order to verify the Data CRC before + // transferring the data to target memory. The + // data field was longer than able to fit inside + // the verify buffer resulting in a buffer overrun + // Note that the command is not executed in + // this case + static const ReturnValue_t REPLY_COMMAND_NOT_IMPLEMENTED_OR_NOT_AUTHORISED = + MAKE_RETURN_CODE(10);// The target user application did not authorise + // the requested operation. This may be because + // the command requested has not been + // implemented + static const ReturnValue_t REPLY_RMW_DATA_LENGTH_ERROR = MAKE_RETURN_CODE( + 11); // The amount of data in a RMW command is + // invalid (0x01 0x03 0x05 0x07 or greater + // than 0x08) + static const ReturnValue_t REPLY_INVALID_TARGET_LOGICAL_ADDRESS = + MAKE_RETURN_CODE(12); // The Header CRC was decoded correctly but + // the Target Logical Address was not the value + // expected by the target + + /** + * Resets the underlying channel. + * + * @param cookie The cookie which points to the channel to reset + * @return + */ + static ReturnValue_t reset(RMAPCookie *cookie); + + /** + * send an RMAP write command + * + * datalen is only 24bit wide, rest will be ignored + * IMPORTANT: the data buffer must be datalen+1 large, as the driver might + * write a CRC sum at data[datalen] + * if you want to send an empty packet, just do datalen = 0 and data = NULL + * + * @param cookie The cookie to write to + * @param buffer the data to write + * @param length length of data + * @return + * - @c COMMAND_NULLPOINTER datalen was != 0 but data was == NULL in write command + * - return codes of RMAPChannelIF::sendCommand() + */ + static ReturnValue_t sendWriteCommand(RMAPCookie *cookie, uint8_t* buffer, + uint32_t length); + + /** + * get the reply to a write command + * + * @param cookie the cookie the command was sent with + * @return + * - @c REPLY_MISSMATCH a read command was issued, but getWriteReply called + * - return codes of RMAPChannelIF::getReply() + */ + static ReturnValue_t getWriteReply(RMAPCookie *cookie); + + /** + * @see sendWriteCommand() + * @see getWriteReply() + * + * @param timeout_us the time after the function returns, if no reply was received + * + * @return + * - All of sendWriteCommand() + * - All of getWriteReply() + * - @c REPLY_TIMEOUT timeout + */ + static ReturnValue_t writeBlocking(RMAPCookie *cookie, uint8_t* buffer, + uint32_t length, uint32_t timeout_us); + + /** + * send an RMAP read command + * + * @param cookie to cookie to read from + * @param expLength the expected maximum length of the reply + * @return + * - @c COMMAND_NULLPOINTER datalen was != 0 but data was == NULL in write command, or nullpointer in read command + * - return codes of RMAPChannelIF::sendCommand() + */ + static ReturnValue_t sendReadCommand(RMAPCookie *cookie, + uint32_t expLength); + + /** + * get a reply to an RMAP read command + * + * @param cookie the cookie that was read from + * @param[out] buffer the location of the data + * @param[out] size size of the data + * @return + * - @c COMMAND_NULLPOINTER buffer or size was NULL + * - @c REPLY_MISSMATCH a write command was issued, but getReadReply called + * - return codes of RMAPChannelIF::getReply() + */ + static ReturnValue_t getReadReply(RMAPCookie *cookie, uint8_t **buffer, + uint32_t *size); + + /** + * @see sendReadCommand() + * @see getReadReply() + * + * @param timeout_us the time after the function returns, if no reply was received + * + * @return + * - All of sendReadCommand() + * - All of getReadReply() + * - @c REPLY_TIMEOUT timeout + */ + static ReturnValue_t readBlocking(RMAPCookie *cookie, uint32_t expLength, + uint8_t** buffer, uint32_t *size, uint32_t timeout_us); + +protected: + RMAP(); +}; + +#endif /* RMAPpp_H_ */ diff --git a/rmap/RMAPChannelIF.h b/rmap/RMAPChannelIF.h new file mode 100644 index 00000000..17e98ea5 --- /dev/null +++ b/rmap/RMAPChannelIF.h @@ -0,0 +1,120 @@ +/* + * RMAPChannelIF.h + * + * Created on: 31.05.2013 + * Author: tod + */ + +#ifndef RMAPCHANNELIF_H_ +#define RMAPCHANNELIF_H_ + +#include + +class RMAPChannelIF: public RMAP { +public: + /** + * Reset an RMAP channel + * + * Clears the receive buffer (all received messages are deleted) and resets the descriptor table. + * Also checks for errors in the descriptors and submits them to FDIR (aka stdout) + * + * @param channel to reset + * + * @return + * - @c LINK_DOWN when the link is down and all replies were missed + * - @c COMMAND_CHANNEL_DEACTIVATED if the channel's port is NULL + * - @c RETURN_OK else + */ + virtual ReturnValue_t reset()=0; + + /** + * Check if a channel is active (ie has a port) + * + * @param channel_nr + * @return + * - @c COMMAND_OK if channel is active + * - @c COMMAND_CHANNEL_DEACTIVATED if channel is deactivated + */ + virtual ReturnValue_t isActive()=0; + + /** + * Assign a SpaceWire port to the Channel + * + * @param port Number of the port. SpaceWire devices are mapped to port numbers to allow checking of the validity + * @param dest_addr the destination address used by all packets sent from this channel + * @param src_addr the source address used by all packets sent from this channel and used when checking incoming packets + * @return + * - @c COMMAND_OK if port was changed + * - @c COMMAND_PORT_OUT_OF_RANGE if the port is invalid + */ + virtual ReturnValue_t setPort(int8_t port, uint8_t dest_addr, + uint8_t src_addr)=0; + + /** + * Assign a SpaceWire port to the Channel + * + * same as setPort(int8_t port, uint8_t dest_addr, uint8_t src_addr), only the addresses are left unchanged + * + * @param port Number of the port. SpaceWire devices are mapped to port numbers to allow checking of the validity + * @return + * - @c COMMAND_OK if port was changed + * - @c COMMAND_PORT_OUT_OF_RANGE if the port is invalid + */ + virtual ReturnValue_t setPort(int8_t port)=0; + + /** + * Send an RMAP command + * + * @param cookie the cookie used with this call + * @param instruction the instruction byte that will be sent (this defines if it is a read or write command) + * @param data data to be sent + * @param datalen length of data + * @return + * - @c RETURN_OK + * - @c COMMAND_NO_DESCRIPTORS_AVAILABLE no descriptors available for sending command; command was not sent + * - @c COMMAND_BUFFER_FULL no receiver buffer available for expected len; command was not sent + * - @c COMMAND_TOO_BIG the data that was to be sent was too long for the hw to handle (write command) or the expected len was bigger than maximal expected len (read command) command was not sent + * - @c COMMAND_CHANNEL_DEACTIVATED the channel has no port set + * - @c NOT_SUPPORTED if you dont feel like implementing something... + */ + virtual ReturnValue_t sendCommand(RMAPCookie *cookie, uint8_t instruction, + uint8_t *data, uint32_t datalen)=0; + + /** + * get the reply to an rmap command + * + * @param cookie the cookie the command was sent with + * @param databuffer a pointer to a pointer the location of the reply will be written to + * @param len a pointer to the variable the length of the reply will be written to + * @return + * - @c REPLY_NO_REPLY no reply was received + * - @c REPLY_NOT_SENT command was not sent, implies no reply + * - @c REPLY_NOT_YET_SENT command is still waiting to be sent + * - @c WRITE_REPLY_INTERFACE_BUSY Interface is busy (transmission buffer still being processed) + * - @c WRITE_REPLY_TRANSMISSION_ERROR Interface encountered errors during last operation, data could not be processed. (transmission error) + * - @c WRITE_REPLY_INVALID_DATA Invalid data (amount / value) + * - @c WRITE_REPLY_NOT_SUPPORTED + * - all RMAP standard replies + */ + virtual ReturnValue_t getReply(RMAPCookie *cookie, uint8_t **databuffer, + uint32_t *len)=0; + + /** + * + * @param cookie + * @param data + * @param datalen + * @param databuffer + * @param len + * @param timeout_us + * @return + * - all replies of sendCommand() and getReply() + * - @c REPLY_TIMEOUT timeout + */ + virtual ReturnValue_t sendCommandBlocking(RMAPCookie *cookie, uint8_t *data, + uint32_t datalen, uint8_t **databuffer, uint32_t *len, + uint32_t timeout_us)=0; + +}; + +#endif /* RMAPCHANNELIF_H_ */ diff --git a/rmap/RMAPCookie.cpp b/rmap/RMAPCookie.cpp new file mode 100644 index 00000000..3e857e52 --- /dev/null +++ b/rmap/RMAPCookie.cpp @@ -0,0 +1,113 @@ +/* + * RMAPcpp + * + * Created on: 07.11.2012 + * Author: mohr + */ + +#include +#include +#include + +//TODO use ctor initialization list +RMAPCookie::RMAPCookie() { + this->header.dest_address = 0; + this->header.protocol = 0x01; + this->header.instruction = 0; + this->header.dest_key = 0; + this->header.source_address = 0; + this->header.tid_h = 0; + this->header.tid_l = 0; + this->header.extended_address = 0; + this->header.address_hh = 0; + this->header.address_h = 0; + this->header.address_l = 0; + this->header.address_ll = 0; + this->header.datalen_h = 0; + this->header.datalen_m = 0; + this->header.datalen_l = 0; + this->header.header_crc = 0; + + + this->txdesc = NULL; + this->rxdesc_index = 0; + this->channel = NULL; + this->command_mask = 0; + + this->maxReplyLen = 0; +} + + + +RMAPCookie::RMAPCookie(uint32_t set_address, uint8_t set_extended_address, + RMAPChannelIF *set_channel, uint8_t set_command_mask, uint32_t maxReplyLen) { + this->header.dest_address = 0; + this->header.protocol = 0x01; + this->header.instruction = 0; + this->header.dest_key = 0; + this->header.source_address = 0; + this->header.tid_h = 0; + this->header.tid_l = 0; + this->header.extended_address = set_extended_address; + setAddress(set_address); + this->header.datalen_h = 0; + this->header.datalen_m = 0; + this->header.datalen_l = 0; + this->header.header_crc = 0; + + this->txdesc = NULL; + this->rxdesc_index = 0; + this->channel = set_channel; + this->command_mask = set_command_mask; + + this->maxReplyLen = maxReplyLen; +} + + +void RMAPCookie::setAddress(uint32_t address) { + this->header.address_hh = (address & 0xFF000000) >> 24; + this->header.address_h = (address & 0x00FF0000) >> 16; + this->header.address_l = (address & 0x0000FF00) >> 8; + this->header.address_ll = address & 0x000000FF; +} + +void RMAPCookie::setExtendedAddress(uint8_t extendedAddress) { + this->header.extended_address = extendedAddress; +} + +void RMAPCookie::setChannel(RMAPChannelIF *channel) { + this->channel = channel; +} + +void RMAPCookie::setCommandMask(uint8_t commandMask) { + this->command_mask = commandMask; +} + +uint32_t RMAPCookie::getAddress() { + return (header.address_hh << 24) + (header.address_h << 16) + + (header.address_l << 8) + (header.address_ll); +} + +uint8_t RMAPCookie::getExtendedAddress() { + return header.extended_address; +} + +RMAPChannelIF *RMAPCookie::getChannel() { + return channel; +} + +uint8_t RMAPCookie::getCommandMask() { + return command_mask; +} + +RMAPCookie::~RMAPCookie() { + +} + +uint32_t RMAPCookie::getMaxReplyLen() const { + return maxReplyLen; +} + +void RMAPCookie::setMaxReplyLen(uint32_t maxReplyLen) { + this->maxReplyLen = maxReplyLen; +} diff --git a/rmap/RMAPCookie.h b/rmap/RMAPCookie.h new file mode 100644 index 00000000..09ac7030 --- /dev/null +++ b/rmap/RMAPCookie.h @@ -0,0 +1,50 @@ +/* + * RMAPCookie.h + * + * Created on: 07.11.2012 + * Author: mohr + */ + +#ifndef RMAPCOOKIE_H_ +#define RMAPCOOKIE_H_ + +#include +#include + +class RMAPChannelIF; + +class RMAPCookie : public Cookie{ + friend class RMAP; + friend class RmapSPWChannel; +public: + //To Uli: Sorry, I need an empty ctor to initialize an array of cookies. + RMAPCookie(); + + RMAPCookie(uint32_t set_address, uint8_t set_extended_address, + RMAPChannelIF *set_channel, uint8_t set_command_mask, uint32_t maxReplyLen = 0); + virtual ~RMAPCookie(); + + + void setAddress(uint32_t address); + uint32_t getAddress(); + void setExtendedAddress(uint8_t); + uint8_t getExtendedAddress(); + void setChannel(RMAPChannelIF *channel); + RMAPChannelIF *getChannel(); + void setCommandMask(uint8_t commandMask); + uint8_t getCommandMask(); + uint32_t getMaxReplyLen() const; + void setMaxReplyLen(uint32_t maxReplyLen); + + //rmap_cookie* getDeviceDescriptor(); + +protected: + RMAPStructs::rmap_cmd_header header; + void *txdesc; + uint8_t rxdesc_index; + RMAPChannelIF *channel; + uint8_t command_mask; + uint32_t maxReplyLen; +}; + +#endif /* RMAPCOOKIE_H_ */ diff --git a/rmap/RmapSPWChannel.cpp b/rmap/RmapSPWChannel.cpp new file mode 100644 index 00000000..9364bd93 --- /dev/null +++ b/rmap/RmapSPWChannel.cpp @@ -0,0 +1,774 @@ +/* + * RMAPStack.cpp + * + * Created on: 30.05.2013 + * Author: tod + */ + +#include +extern "C" { +#include +#include +} + +#include +#include + +////////////////////////////////////////////////////////////////////////////////// +//bits for the failure table +#define RMAP_RX_FAIL_CHECKED 0 +#define RMAP_RX_FAIL_EEP 1 +#define RMAP_RX_FAIL_TRUNC 2 +#define RMAP_RX_FAIL_LEN 3 +#define RMAP_RX_FAIL_CRC 4 +#define RMAP_RX_FAIL_DST_ADDR 5 +#define RMAP_RX_FAIL_PROTO 6 +#define RMAP_RX_FAIL_INSTR_TYPE 7 //this is when the instruction byte is really wrong (must be caused by device) +#define RMAP_RX_FAIL_INSTR 8 //this is when a wrong instruction byte was caused by a wrong instruction sent by the host and detected by the device +#define RMAP_RX_FAIL_SRC_ADDR 9 +#define RMAP_RX_FAIL_LEN_MISSMATCH 10 +#define RMAP_RX_FAIL_WRONG_REPLY 11 //a read command was answered by a write reply or the other way round +//pad a number so it is word aligned (32bit) +#define BYTES2WORDS(number) (((number) +3)/4) + +RmapSPWChannel::RmapSPWChannel(object_id_t setObjectId, + uint16_t buffersize_words, uint32_t maxPacketSize, int8_t portNr, + uint8_t dest_addr, uint8_t src_addr, + datapool::opus_variable_id portVariable) : + SystemObject(setObjectId), port(NULL), port_has_crc(0), rx_index(0), max_rx( + 128), tx_index(0), max_tx(64), src_addr(src_addr), dest_addr( + dest_addr), portPoolId(portVariable) { + buffer = new uint32_t[buffersize_words]; + buffer_pointer = buffer; + end_of_buffer = &buffer[buffersize_words]; + max_packet_len = maxPacketSize & 0x1FFFFFC; + tid = 0; + failure_table = new uint16_t[128]; + setPort(portNr, dest_addr, src_addr); +} + +RmapSPWChannel::~RmapSPWChannel() { + delete[] buffer; + delete[] failure_table; +} + +ReturnValue_t RmapSPWChannel::reset() { + uint8_t link_down = 0; + uint8_t missed, i; + uint32_t state; + + //printf_sif_rmap("reset channel %i\n", channel_nr); + + //check if channel has a port + if (port == NULL) { + return COMMAND_CHANNEL_DEACTIVATED; + } + + //check state of SPW + state = spw_get_state(port); + if (state != SPW_STATE_RUN) { + triggerEvent(SPW_LINK_DOWN, getCurrentPortNr()); + link_down++; + } + //SPW link errors + if ((state = (port->SPWSTR & 0x1de)) != 0) { + reportSpwstr(state); + } + //check DMA errors + if ((state = (port->SPWCHN & 0x180)) != 0) { + triggerEvent(SPW_ERROR, SPW_DMA, state); + } + + //hard reset spw + //only safe way to reset the descriptors + + spw_reset(port); + + spw_reset_rxdesc_table(port); + spw_reset_txdesc_table(port); + + //reset internal buffer + buffer_pointer = buffer; + +#ifdef LEON + asm("flush"); +#endif + + //check how many packets were received + missed = 0; + + if ((rx_index != 0) + && (rx_descriptor_table[rx_index - 1].word0 + & (1 << SPW_DESC_RX_ENABLE))) { + // printf("word 0 = %08lx\n", + // rx_descriptor_table[rx_index - 1].word0); + for (missed = 1; missed < rx_index; missed++) { + if (!(rx_descriptor_table[rx_index - 1 - missed].word0 + & (1 << SPW_DESC_RX_ENABLE))) + break; + } + triggerEvent(RMAP_MISSED_REPLIES, missed); + //missed is number of missed replies + + } + if (missed == rx_index) { + link_down++; + } + //TODO: checkme + for (i = rx_index - missed; i > 0; i--) { + if (!(failure_table[i - 1] & 1)) { + checkRxDescPacket(&(rx_descriptor_table[i - 1]), + &(failure_table[i - 1])); + } + if (failure_table[i - 1] & 0xFFFE) { + reportFailures(failure_table[i - 1], i - 1); + } + } + + //reset descriptor counter + rx_index = 0; + tx_index = 0; + + //clear all descriptors to get rid of unused ones + spw_rx_desc *rx_desc; + i = 0; + for (rx_desc = rx_descriptor_table; rx_desc < &rx_descriptor_table[max_rx]; + rx_desc++) { + rx_desc->word0 = 0; + failure_table[i++] = 0; + } + spw_tx_desc *tx_desc; + for (tx_desc = tx_descriptor_table; tx_desc < &tx_descriptor_table[max_tx]; + tx_desc++) { + tx_desc->word0 = 0; + } + + //restart spw + spw_start(port); + + if (link_down > 1) { + return LINK_DOWN; + } + return RETURN_OK; + +} + +ReturnValue_t RmapSPWChannel::isActive() { + if (port == NULL) { + return COMMAND_CHANNEL_DEACTIVATED; + } + uint32_t state = spw_get_state(port); + if (state != SPW_STATE_RUN) { + return LINK_DOWN; + } + return RETURN_OK; + +} + +ReturnValue_t RmapSPWChannel::setPort(int8_t portNr, uint8_t dest_addr, + uint8_t src_addr) { + + SPW_dev *new_port; + + //only accept ports from a list or -1, which disables channel + if (portNr >= SPW_devices.len && portNr < -1) { + return COMMAND_PORT_OUT_OF_RANGE; + } + + if (portNr != -1) { + new_port = SPW_devices.devices[portNr]; + } else { + new_port = NULL; + } + + //anounce change + triggerEvent(RMAP_SWITCHED_PORT, portNr, getCurrentPortNr()); + if (portPoolId != 0) { + DataSet mySet; + PoolVariable channel(portPoolId, &mySet, + PoolVariableIF::VAR_WRITE); + mySet.read(); + channel = portNr; + mySet.commit(PoolVariableIF::VALID); + } + + //reset the old port to clear pending errors etc + reset(); + //assign new port + this->port = new_port; + + if (new_port == NULL) { + return RETURN_OK; + } + +//stop the new port + spw_stop(new_port); +//check if crc is enabled + port_has_crc = spw_crc_enabled(new_port); +//make sure the new port has the max len set + new_port->SPWRXL = max_packet_len; +//set the addresses + this->dest_addr = dest_addr; + this->src_addr = src_addr; + new_port->SPWNDR = src_addr; +//set descriptor table + tx_descriptor_table = spw_get_tx_descriptor_table(new_port); + rx_descriptor_table = spw_get_rx_descriptor_table(new_port); + spw_reset_rxdesc_table(new_port); + spw_reset_txdesc_table(new_port); +//reset the channel + spw_start(new_port); + OSAL::sleepFor(10); + reset(); + return RETURN_OK; +} + +ReturnValue_t RmapSPWChannel::setPort(int8_t port) { + return setPort(port, this->dest_addr, this->src_addr); +} + +spw_rx_desc* RmapSPWChannel::findReply(RMAPCookie* cookie) { + spw_rx_desc *rxdesc; + uint8_t i; + +//look downwards +//TODO: checkme + for (i = cookie->rxdesc_index; i < 200; i--) { + if ((rxdesc = checkRxDesc(cookie, i)) != NULL) { + return rxdesc; + } + } +//look upwards + for (i = cookie->rxdesc_index + 1; i < max_rx; i++) { + if ((rxdesc = checkRxDesc(cookie, i)) != NULL) { + return rxdesc; + } + } + + return NULL; + +} + +ReturnValue_t RmapSPWChannel::sendCommand(RMAPCookie* cookie, + uint8_t instruction, uint8_t* data, uint32_t datalen) { + + uint8_t headerlen = RMAP_COMMAND_HEADER_LEN - 1; + +//check if channel has a port + if (port == NULL) { + return COMMAND_CHANNEL_DEACTIVATED; + } + +//max send limit + if ((instruction & (1 << RMAP_COMMAND_BIT_WRITE)) + && (datalen & ~0xFFFFFF)) { + printf("application wants to send to much data with cookie at %p\n", + cookie); + return TOO_MUCH_DATA; + } +//max receive limit + if ((!(instruction & (1 << RMAP_COMMAND_BIT_WRITE))) + && (datalen + 1 + RMAP_READ_REPLY_HEADER_LEN > max_packet_len)) { + printf("application wants to read too much data with cookie at %p\n", + cookie); + return TOO_MUCH_DATA; + } + +//check if enough space is left in the buffer +//must be checked against max_packet_len, as you can not tell what hw will do, +//except that it will write this number at most + if ((buffer_pointer + BYTES2WORDS(max_packet_len)) + > end_of_buffer) { + printf("buffer is full, cookie is %p\n", cookie); + return COMMAND_BUFFER_FULL; + } + +//we allow tx descriptor wrap at user discretion, but check we have rx descriptors available +//as we dont wrap them +//note: [tr]x_index will be incremented when done + if (rx_index >= max_rx) { + triggerEvent(RMAP_NO_RX_DESCRIPTORS, 0); + return COMMAND_NO_DESCRIPTORS_AVAILABLE; + } + if (tx_index >= max_tx) { + if (max_tx == 64) { + tx_index = 0; + } else { + triggerEvent(RMAP_NO_TX_DESCRIPTORS, 0); + return COMMAND_NO_DESCRIPTORS_AVAILABLE; + } + } + +//check if tx descriptor is activated, which means we wrapped the table +//and came to a descriptor not yet sent +#ifdef LEON + asm("flush"); +#endif + if (tx_descriptor_table[tx_index].word0 & (1 << SPW_DESC_TX_ENABLE)) { + return COMMAND_NO_DESCRIPTORS_AVAILABLE; + } + +//prepare header +//set addresses + cookie->header.source_address = src_addr; + cookie->header.dest_address = dest_addr; +//set instruction + cookie->header.instruction = instruction; +//set the tid + cookie->header.tid_l = tid & 0xff; + cookie->header.tid_h = (tid >> 8) & 0xff; + tid++; +//set datalen + cookie->header.datalen_l = datalen & 0xff; + cookie->header.datalen_m = (datalen >> 8) & 0xff; + cookie->header.datalen_h = (datalen >> 16) & 0xff; + +//configure rx_descriptor + rx_descriptor_table[rx_index].word1 = (uint32_t) buffer_pointer; + rx_descriptor_table[rx_index].word0 = (1 << SPW_DESC_RX_ENABLE); + +//inc buffer + if (instruction & (1 << RMAP_COMMAND_BIT_WRITE)) { + buffer_pointer += BYTES2WORDS(RMAP_WRITE_REPLY_HEADER_LEN); + } else { + buffer_pointer += BYTES2WORDS(RMAP_READ_REPLY_HEADER_LEN + datalen + 1); //+1 for crc + } + +//calculate crc if not done in hw + if (!port_has_crc) { + cookie->header.header_crc = crc_calculate((uint8_t *) &cookie->header, + RMAP_COMMAND_HEADER_LEN - 1); + headerlen = RMAP_COMMAND_HEADER_LEN; + if (instruction & (1 << RMAP_COMMAND_BIT_WRITE)) { + if (datalen == 0) { + data = &null_crc; + } else { + data[datalen] = crc_calculate(data, datalen); + } + datalen++; + } + } + +//configure tx_descriptor + tx_descriptor_table[tx_index].word1 = (uint32_t) &cookie->header; + if (instruction & (1 << RMAP_COMMAND_BIT_WRITE)) { + tx_descriptor_table[tx_index].word2 = datalen & 0xFFFFFF; + } else { + tx_descriptor_table[tx_index].word2 = 0; + } + tx_descriptor_table[tx_index].word3 = (uint32_t) data; + if (port_has_crc) { + if (instruction & (1 << RMAP_COMMAND_BIT_WRITE)) { + tx_descriptor_table[tx_index].word0 = headerlen + | (1 << SPW_DESC_TX_ENABLE) | (1 << SPW_DESC_TX_CRC_DATA) + | (1 << SPW_DESC_TX_CRC_HEADER); + } else { + tx_descriptor_table[tx_index].word0 = headerlen + | (1 << SPW_DESC_TX_ENABLE) | (1 << SPW_DESC_TX_CRC_HEADER); + } + } else { + tx_descriptor_table[tx_index].word0 = headerlen + | (1 << SPW_DESC_TX_ENABLE); + } + +//remember descriptors to find them faster when looking for the reply + cookie->txdesc = &(tx_descriptor_table[tx_index]); + cookie->rxdesc_index = rx_index; + + tx_index++; + rx_index++; + +//inform the hw about the new descriptors + spw_new_rxdesc(port); + spw_new_txdesc(port); + + return RETURN_OK; + +} + +ReturnValue_t RmapSPWChannel::getReply(RMAPCookie* cookie, uint8_t** databuffer, + uint32_t* len) { + spw_rx_desc *rxdesc; + ReturnValue_t result; + uint32_t packetlen; +//was the command sent right + result = checkTxDesc(cookie); + if (result != RETURN_OK) { + return result; + } +//command was sent +//find the rx descriptor + if ((rxdesc = findReply(cookie)) == NULL) { + return REPLY_NO_REPLY; + } +//reply_header = (rmap_write_reply_header *) rxdesc->word1; + if (databuffer != NULL) { + packetlen = (rxdesc->word0 & 0x1FFFFFF); + // printf_sif("Reached critical entry.\n"); + *databuffer = ((uint8_t *) rxdesc->word1) + RMAP_READ_REPLY_HEADER_LEN; + *len = packetlen - RMAP_READ_REPLY_HEADER_LEN - 1; + } + result = ((RMAPStructs::rmap_write_reply_header *) (rxdesc->word1))->status; + //result is an RMAP standard return code, not a ReturnValue_t, thus we check against 0 + //and do MAKE_RETURN_CODE(), or set to RETURN_OK respectively + //if RETURN_OK == 0, this is not neccessary, but we should not make assumptions on the + //value of ReturnValues + if (result != 0) { + result = ((RMAP::INTERFACE_ID << 8) + (result)); + } else { + result = RETURN_OK; + } + return result; + +} + +ReturnValue_t RmapSPWChannel::checkTxDesc(RMAPCookie* cookie) { + RMAPStructs::rmap_cmd_header *header_desc; + uint16_t tid_desc, tid_cookie; + uint32_t status; +#ifdef LEON + asm("flush"); +#endif + if (cookie->txdesc == NULL) { + return REPLY_NOT_SENT; + } + header_desc = + (RMAPStructs::rmap_cmd_header *) ((spw_tx_desc *) cookie->txdesc)->word1; + status = ((spw_tx_desc *) cookie->txdesc)->word0 + & ((1 << SPW_DESC_TX_ENABLE) | (1 << SPW_DESC_TX_LINK_ERROR)); + tid_cookie = (cookie->header.tid_l) | (cookie->header.tid_h << 8); + tid_desc = (header_desc->tid_l) | (header_desc->tid_h << 8); + if (tid_cookie != tid_desc) { + //tx descriptor was reused, we have no info so let's assume the best + return RETURN_OK; + } +//was command sent or an error seen? + if (status == (1 << SPW_DESC_TX_ENABLE)) { + return REPLY_NOT_YET_SENT; + } + if (status & (1 << SPW_DESC_TX_LINK_ERROR)) { + triggerEvent(SPW_ERROR, SPW_LINK_ERROR, 0); + return REPLY_NOT_SENT; + } + return RETURN_OK; +} + +spw_rx_desc* RmapSPWChannel::checkRxDesc(RMAPCookie* cookie, + uint8_t rxdesc_index) { + uint16_t *failureEntry, tid_desc, tid_cookie; + spw_rx_desc *rxdesc; + RMAPStructs::rmap_read_reply_header *reply_header; + + rxdesc = &rx_descriptor_table[rxdesc_index]; + failureEntry = &failure_table[rxdesc_index]; + + if (!(*failureEntry)) { + checkRxDescPacket(rxdesc, failureEntry); + } + if (*failureEntry != 1 << RMAP_RX_FAIL_CHECKED) { + return NULL; + } +//the packet has been checked and is formally correct +//only thing left is to check tid and instruction: + + reply_header = (RMAPStructs::rmap_read_reply_header *) rxdesc->word1; + + tid_cookie = cookie->header.tid_l | (cookie->header.tid_h << 8); + tid_desc = reply_header->tid_l | (reply_header->tid_h << 8); + if (tid_desc != tid_cookie) { + return NULL; + } + + if ((cookie->header.instruction & 0x3F) != reply_header->instruction) { + *failureEntry |= (1 << RMAP_RX_FAIL_WRONG_REPLY); + return NULL; + } + return rxdesc; + +} + +void RmapSPWChannel::checkRxDescPacket(spw_rx_desc* rxdesc, + uint16_t* failureEntry) { + uint32_t statusword, len; + +#ifdef LEON + asm("flush"); +#endif + + statusword = rxdesc->word0; + + if ((statusword & (1 << SPW_DESC_RX_ENABLE)) || (statusword == 0)) { + return; + } + + *failureEntry = 1 << RMAP_RX_FAIL_CHECKED; + + if (statusword & (1 << SPW_DESC_RX_EEP)) { + *failureEntry |= (1 << RMAP_RX_FAIL_EEP); + return; + } + + if (statusword & (1 << SPW_DESC_RX_TRUNCATED)) { + *failureEntry |= (1 << RMAP_RX_FAIL_TRUNC); + return; + } + + len = statusword & 0x1FFFFFF; + if (!(len >= 13 || len == 8)) { + *failureEntry |= (1 << RMAP_RX_FAIL_LEN); + return; + } + if (checkCrc((uint8_t*) rxdesc->word1, len) != RETURN_OK) { + *failureEntry |= (1 << RMAP_RX_FAIL_CRC); + return; + } + + checkRmapHeader((void *) rxdesc->word1, len, failureEntry); + +} + +ReturnValue_t RmapSPWChannel::checkCrc(uint8_t* packet, uint32_t len) { + if (len == 8) { + if (crc_calculate(packet, len) == 0) { + return RETURN_OK; + } else { + return RETURN_FAILED; + } + } else { + if ((crc_calculate(packet, RMAP_READ_REPLY_HEADER_LEN) == 0) + && (crc_calculate(packet, len) == 0)) { + return RETURN_OK; + } else { + return RETURN_FAILED; + } + } +} + +void RmapSPWChannel::checkRmapHeader(void* _header, uint32_t len, + uint16_t* failureEntry) { + RMAPStructs::rmap_read_reply_header *header = + (RMAPStructs::rmap_read_reply_header *) _header; + uint32_t datalen; + + if (header->dest_address != src_addr) { + *failureEntry |= (1 << RMAP_RX_FAIL_DST_ADDR); + } + + if (header->protocol != 0x01) { + *failureEntry |= (1 << RMAP_RX_FAIL_PROTO); + } + + if (header->instruction & 0xC0) { + *failureEntry |= (1 << RMAP_RX_FAIL_INSTR_TYPE); + } else { //hope that works... + if ((!(header->instruction & (1 << RMAP_COMMAND_BIT_REPLY))) + || ((header->instruction & (1 << RMAP_COMMAND_BIT_VERIFY)) + && (!(header->instruction + & (1 << RMAP_COMMAND_BIT_WRITE)))) + || (header->instruction & 3)) { + if (header->status == REPLY_UNUSED_PACKET_TYPE_OR_COMMAND_CODE) { + *failureEntry |= (1 << RMAP_RX_FAIL_INSTR); + } else { + *failureEntry |= (1 << RMAP_RX_FAIL_INSTR_TYPE); + } + } + } + + if (header->instruction & (1 << RMAP_COMMAND_BIT_WRITE)) { + if (len != 8) { + *failureEntry |= (1 << RMAP_RX_FAIL_INSTR_TYPE); + } + } else { + if (len == 8) { + *failureEntry |= (1 << RMAP_RX_FAIL_INSTR_TYPE); + } + } + + if (header->source_address != dest_addr) { + *failureEntry |= (1 << RMAP_RX_FAIL_SRC_ADDR); + } + + if (len != 8) { + datalen = header->datalen_l | (header->datalen_m << 8) + | (header->datalen_h << 16); + if (datalen + RMAP_READ_REPLY_HEADER_LEN + 1 != len) { + *failureEntry |= (1 << RMAP_RX_FAIL_LEN_MISSMATCH); + } + } +} + +ReturnValue_t RmapSPWChannel::sendCommandBlocking(RMAPCookie *cookie, + uint8_t *data, uint32_t datalen, uint8_t **databuffer, uint32_t *len, + uint32_t timeout_us) { + uint32_t time; + ReturnValue_t result; + if (databuffer != NULL) { + result = sendReadCommand(cookie, datalen); + } else { + result = sendWriteCommand(cookie, data, datalen); + } + if (result != RETURN_OK) { + return result; + } + time = hw_timer_get_us(); + while (time - hw_timer_get_us() < timeout_us) { + result = getReply(cookie, databuffer, len); + if (!((result == REPLY_NOT_YET_SENT) || (result == REPLY_NO_REPLY))) { + return result; + } + } + if (result == REPLY_NOT_YET_SENT) { + return result; + } + return REPLY_TIMEOUT; +} + +//TODO find a better way to inject the Extended address +#include +ReturnValue_t RmapSPWChannel::open(Cookie **cookie, uint32_t address, + uint32_t maxReplyLen) { + *cookie = new RMAPCookie(address, IoBoardExtendedAddresses::DEVICE_BUFFER, + this, 0, maxReplyLen); + return RETURN_OK; +} + +ReturnValue_t RmapSPWChannel::reOpen(Cookie* cookie, uint32_t address, + uint32_t maxReplyLen) { + ReturnValue_t result = isActive(); + if (result != RETURN_OK) { + return result; + } + RMAPCookie *rCookie = dynamic_cast(cookie); + if (rCookie == NULL) { + return INVALID_COOKIE_TYPE; + } + rCookie->setAddress(address); + rCookie->setChannel(this); + rCookie->setCommandMask(0); + rCookie->setExtendedAddress(IoBoardExtendedAddresses::DEVICE_BUFFER); + rCookie->setMaxReplyLen(maxReplyLen); + return REPLY_OK; +} + +void RmapSPWChannel::close(Cookie* cookie) { + delete cookie; +} + +ReturnValue_t RmapSPWChannel::sendMessage(Cookie* cookie, uint8_t* data, + uint32_t len) { + return sendWriteCommand((RMAPCookie *) cookie, data, len); +} + +ReturnValue_t RmapSPWChannel::getSendSuccess(Cookie* cookie) { + return getWriteReply((RMAPCookie *) cookie); +} + +ReturnValue_t RmapSPWChannel::requestReceiveMessage(Cookie* cookie) { + return sendReadCommand((RMAPCookie *) cookie, + ((RMAPCookie *) cookie)->getMaxReplyLen()); +} + +ReturnValue_t RmapSPWChannel::readReceivedMessage(Cookie* cookie, + uint8_t** buffer, uint32_t* size) { + return getReadReply((RMAPCookie *) cookie, buffer, size); +} + +ReturnValue_t RmapSPWChannel::setAddress(Cookie* cookie, uint32_t address) { + ((RMAPCookie *) cookie)->setAddress(address); + return HasReturnvaluesIF::RETURN_OK; +} + +uint32_t RmapSPWChannel::getAddress(Cookie* cookie) { + return ((RMAPCookie *) cookie)->getAddress(); +} + +ReturnValue_t RmapSPWChannel::setParameter(Cookie* cookie, uint32_t parameter) { + return RETURN_FAILED; +} + +uint32_t RmapSPWChannel::getParameter(Cookie* cookie) { + return 0; +} + +void RmapSPWChannel::reportFailures(uint16_t failureEntry, + uint8_t descriptorNr) { + //TODO: Communicate to Kai, adjust return codes. + if (failureEntry & (1 << RMAP_RX_FAIL_CRC)) { + triggerEvent(RMAP_PROTOCOL_ERROR, REPLY_INVALID_DATA_CRC, descriptorNr); + } + if (failureEntry & (1 << RMAP_RX_FAIL_DST_ADDR)) { + triggerEvent(RMAP_PROTOCOL_ERROR, REPLY_INVALID_TARGET_LOGICAL_ADDRESS, + descriptorNr); + } + if (failureEntry & (1 << RMAP_RX_FAIL_EEP)) { + triggerEvent(RMAP_PROTOCOL_ERROR, REPLY_EEP, descriptorNr); + } + if (failureEntry & (1 << RMAP_RX_FAIL_INSTR)) { + triggerEvent(RMAP_PROTOCOL_ERROR, + REPLY_COMMAND_NOT_IMPLEMENTED_OR_NOT_AUTHORISED, descriptorNr); + } + if (failureEntry & (1 << RMAP_RX_FAIL_INSTR_TYPE)) { + triggerEvent(RMAP_PROTOCOL_ERROR, REPLY_RESERVED, descriptorNr); + } + if (failureEntry & (1 << RMAP_RX_FAIL_LEN)) { + triggerEvent(RMAP_PROTOCOL_ERROR, REPLY_TOO_MUCH_DATA, descriptorNr); + } + if (failureEntry & (1 << RMAP_RX_FAIL_LEN_MISSMATCH)) { + triggerEvent(RMAP_PROTOCOL_ERROR, REPLY_INVALID_DATA, descriptorNr); + } + if (failureEntry & (1 << RMAP_RX_FAIL_PROTO)) { + triggerEvent(RMAP_PROTOCOL_ERROR, REPLY_GENERAL_ERROR_CODE, + descriptorNr); + } + if (failureEntry & (1 << RMAP_RX_FAIL_SRC_ADDR)) { + triggerEvent(RMAP_PROTOCOL_ERROR, REPLY_GENERAL_ERROR_CODE, + descriptorNr); + } + if (failureEntry & (1 << RMAP_RX_FAIL_TRUNC)) { + triggerEvent(RMAP_PROTOCOL_ERROR, REPLY_TRANSMISSION_ERROR, + descriptorNr); + } + if (failureEntry & (1 << RMAP_RX_FAIL_WRONG_REPLY)) { + triggerEvent(RMAP_PROTOCOL_ERROR, REPLY_INVALID_KEY, descriptorNr); + ; + } + +} + +void RmapSPWChannel::reportSpwstr(uint32_t spwstr) { + //TODO: Communicate to Kai. + if (spwstr & (1 << SPW_SPWSTR_CE)) { + triggerEvent(SPW_ERROR, SPW_CREDIT); + } + if (spwstr & (1 << SPW_SPWSTR_ER)) { + triggerEvent(SPW_ERROR, SPW_ESCAPE); + } + if (spwstr & (1 << SPW_SPWSTR_DE)) { + triggerEvent(SPW_ERROR, SPW_DISCONNECT); + } + if (spwstr & (1 << SPW_SPWSTR_PE)) { + triggerEvent(SPW_ERROR, SPW_PARITY); + } + if (spwstr & (1 << SPW_SPWSTR_WE)) { + triggerEvent(SPW_ERROR, SPW_WRITE_SYNC); + } + if (spwstr & (1 << SPW_SPWSTR_IA)) { + triggerEvent(SPW_ERROR, SPW_INVALID_ADDRESS); + } + if (spwstr & (1 << SPW_SPWSTR_EE)) { + triggerEvent(SPW_ERROR, SPW_EARLY_EOP); + } +} + +uint8_t RmapSPWChannel::null_crc = 0; + +int8_t RmapSPWChannel::getCurrentPortNr() { + uint8_t i; + int8_t oldport = -1; + for (i = 0; i < SPW_devices.len; ++i) { + if (SPW_devices.devices[i] == this->port) { + oldport = i; + } + } + return oldport; +} diff --git a/rmap/RmapSPWChannel.h b/rmap/RmapSPWChannel.h new file mode 100644 index 00000000..4ef82472 --- /dev/null +++ b/rmap/RmapSPWChannel.h @@ -0,0 +1,141 @@ +/* + * RMAPStack.h + * + * Created on: 30.05.2013 + * Author: tod + */ + +#ifndef RMAPCHANNEL_H_ +#define RMAPCHANNEL_H_ + +#include +#include +extern "C" { +#include +} +#include +#include +#include + +class RmapSPWChannel: public SystemObject, + public RMAPChannelIF, + public DeviceCommunicationIF { +public: + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::CDH_1; + static const Event SPW_ERROR = MAKE_EVENT(0, SEVERITY::LOW); + static const Event SPW_LINK_DOWN = MAKE_EVENT(1, SEVERITY::LOW); +// static const Event SPW_CREDIT = MAKE_EVENT(1, SEVERITY::LOW); +// static const Event SPW_ESCAPE = MAKE_EVENT(2, SEVERITY::LOW); +// static const Event SPW_DISCONNECT = MAKE_EVENT(3, SEVERITY::LOW); +// static const Event SPW_PARITY = MAKE_EVENT(4, SEVERITY::LOW); +// static const Event SPW_WRITE_SYNC = MAKE_EVENT(5, SEVERITY::LOW); +// static const Event SPW_INVALID_ADDRESS = MAKE_EVENT(6, SEVERITY::LOW); +// static const Event SPW_EARLY_EOP = MAKE_EVENT(7, SEVERITY::LOW); +// static const Event SPW_DMA = MAKE_EVENT(8, SEVERITY::LOW); +// static const Event SPW_LINK_ERROR = MAKE_EVENT(9, SEVERITY::LOW); + static const Event RMAP_PROTOCOL_ERROR = MAKE_EVENT(10, SEVERITY::LOW); +// static const Event RMAP_CRC = MAKE_EVENT(10, SEVERITY::LOW); +// static const Event RMAP_DST_ADDR = MAKE_EVENT(11, SEVERITY::LOW); +// static const Event RMAP_EEP = MAKE_EVENT(12, SEVERITY::LOW); +// static const Event RMAP_INSTR = MAKE_EVENT(13, SEVERITY::LOW); +// static const Event RMAP_INSTR_TYPE = MAKE_EVENT(14, SEVERITY::LOW); +// static const Event RMAP_LEN = MAKE_EVENT(15, SEVERITY::LOW); +// static const Event RMAP_LEN_MISSMATCH = MAKE_EVENT(16, SEVERITY::LOW); +// static const Event RMAP_PROTO = MAKE_EVENT(17, SEVERITY::LOW); +// static const Event RMAP_SRC_ADDR = MAKE_EVENT(18, SEVERITY::LOW); +// static const Event RMAP_TRUNC = MAKE_EVENT(19, SEVERITY::LOW); +// static const Event RMAP_WRONG_REPLY = MAKE_EVENT(20, SEVERITY::LOW); + static const Event RMAP_MISSED_REPLIES = MAKE_EVENT(21, SEVERITY::LOW); + static const Event RMAP_NO_RX_DESCRIPTORS = MAKE_EVENT(22, SEVERITY::LOW); + static const Event RMAP_NO_TX_DESCRIPTORS = MAKE_EVENT(23, SEVERITY::LOW); + static const Event RMAP_SWITCHED_PORT = MAKE_EVENT(24, SEVERITY::INFO); + + RmapSPWChannel(object_id_t setObjectId, uint16_t buffersize_words, + uint32_t maxPacketSize, int8_t portNr, uint8_t dest_addr, + uint8_t src_addr, datapool::opus_variable_id portVariable = + datapool::NO_PARAMETER); + + virtual ~RmapSPWChannel(); + + virtual ReturnValue_t reset(); + + virtual ReturnValue_t isActive(); + + virtual ReturnValue_t setPort(int8_t port, uint8_t dest_addr, + uint8_t src_addr); + + virtual ReturnValue_t setPort(int8_t port); + + virtual ReturnValue_t sendCommand(RMAPCookie *cookie, uint8_t instruction, + uint8_t *data, uint32_t datalen); + virtual ReturnValue_t getReply(RMAPCookie *cookie, uint8_t **databuffer, + uint32_t *len); + virtual ReturnValue_t sendCommandBlocking(RMAPCookie *cookie, uint8_t *data, + uint32_t datalen, uint8_t **databuffer, uint32_t *len, + uint32_t timeout_us); + + virtual ReturnValue_t open(Cookie **cookie, uint32_t address, + uint32_t maxReplyLen); + + virtual ReturnValue_t reOpen(Cookie *cookie, uint32_t address, + uint32_t maxReplyLen); + + virtual void close(Cookie *cookie); + + virtual ReturnValue_t sendMessage(Cookie *cookie, uint8_t *data, + uint32_t len); + + virtual ReturnValue_t getSendSuccess(Cookie *cookie); + + virtual ReturnValue_t requestReceiveMessage(Cookie *cookie); + + virtual ReturnValue_t readReceivedMessage(Cookie *cookie, uint8_t **buffer, + uint32_t *size); + + virtual ReturnValue_t setAddress(Cookie *cookie, uint32_t address); + + virtual uint32_t getAddress(Cookie *cookie); + + virtual ReturnValue_t setParameter(Cookie *cookie, uint32_t parameter); + + virtual uint32_t getParameter(Cookie *cookie); + +private: + + SPW_dev *port; + uint32_t *buffer; + uint32_t *buffer_pointer; //points to the next free space in the buffer + uint32_t *end_of_buffer; //points to the word AFTER the buffer + spw_tx_desc *tx_descriptor_table; + spw_rx_desc *rx_descriptor_table; + uint16_t *failure_table; + uint32_t max_packet_len; + uint16_t tid; + uint8_t port_has_crc; + uint8_t rx_index; //index of the next unused rx descriptor + uint8_t max_rx; + uint8_t tx_index; //index of the next unused tx descriptor + uint8_t max_tx; + uint8_t src_addr; + uint8_t dest_addr; + + datapool::opus_variable_id portPoolId; + + static uint8_t null_crc; + + spw_rx_desc *findReply(RMAPCookie *cookie); + ReturnValue_t checkTxDesc(RMAPCookie *cookie); + spw_rx_desc *checkRxDesc(RMAPCookie *cookie, uint8_t rxdesc_index); + void checkRxDescPacket(spw_rx_desc *rxdesc, uint16_t *failureEntry); + + ReturnValue_t checkCrc(uint8_t *packet, uint32_t len); + + void checkRmapHeader(void *_header, uint32_t len, uint16_t *failureEntry); + + void reportFailures(uint16_t failureEntry, uint8_t descriptorNr); + void reportSpwstr(uint32_t spwstr); + int8_t getCurrentPortNr(); + +}; + +#endif /* RMAPCHANNEL_H_ */ diff --git a/rmap/rmapStructs.h b/rmap/rmapStructs.h new file mode 100644 index 00000000..978e488c --- /dev/null +++ b/rmap/rmapStructs.h @@ -0,0 +1,88 @@ +/* + * rmapStructs.h + * + * Created on: 30.05.2013 + * Author: tod + */ + +#ifndef RMAPSTRUCTS_H_ +#define RMAPSTRUCTS_H_ + +#include + +//TODO: having the defines within a namespace would be nice. Problem are the defines referencing the previous define, eg RMAP_COMMAND_WRITE + +////////////////////////////////////////////////////////////////////////////////// +// RMAP command bits +#define RMAP_COMMAND_BIT_INCREMENT 2 +#define RMAP_COMMAND_BIT_REPLY 3 +#define RMAP_COMMAND_BIT_WRITE 5 +#define RMAP_COMMAND_BIT_VERIFY 4 +#define RMAP_COMMAND_BIT 6 + +////////////////////////////////////////////////////////////////////////////////// +// RMAP commands + +#define RMAP_COMMAND_WRITE ((1< +//#include //for testing on x86 + + +class EndianSwapper { +private: + EndianSwapper() { + } + ; +public: + template + static T swap(T in) { +#ifndef BYTE_ORDER +#error BYTE_ORDER not defined +#elif BYTE_ORDER == LITTLE_ENDIAN + T tmp; + uint8_t *pointerOut = (uint8_t *) &tmp; + uint8_t *pointerIn = (uint8_t *) ∈ + for (uint8_t count = 0; count < sizeof(T); count++) { + pointerOut[sizeof(T) - count - 1] = pointerIn[count]; + } + return tmp; +#elif BYTE_ORDER == BIG_ENDIAN + return in; +#endif + } + static void swap(uint8_t* out, const uint8_t* in, uint32_t size) { +#ifndef BYTE_ORDER +#error BYTE_ORDER not defined +#elif BYTE_ORDER == LITTLE_ENDIAN + for (uint8_t count = 0; count < size; count++) { + out[size - count - 1] = in[count]; + } + return; +#elif BYTE_ORDER == BIG_ENDIAN + memcpy(out, in, size); + return; +#endif + } +}; + +#endif /* ENDIANSWAPPER_H_ */ diff --git a/serialize/SerialArrayListAdapter.h b/serialize/SerialArrayListAdapter.h new file mode 100644 index 00000000..1384775f --- /dev/null +++ b/serialize/SerialArrayListAdapter.h @@ -0,0 +1,80 @@ +/** + * @file SerialArrayListAdapter.h + * @brief This file defines the SerialArrayListAdapter class. + * @date 22.07.2014 + * @author baetz + */ +#ifndef SERIALARRAYLISTADAPTER_H_ +#define SERIALARRAYLISTADAPTER_H_ + +#include +#include +#include + +template +class SerialArrayListAdapter : public SerializeIF { +public: + SerialArrayListAdapter(ArrayList *adaptee) : adaptee(adaptee) { + } + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + return serialize(adaptee, buffer, size, max_size, bigEndian); + } + + static ReturnValue_t serialize(const ArrayList* list, uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) { + ReturnValue_t result = SerializeAdapter::serialize(&list->size, + buffer, size, max_size, bigEndian); + count_t i = 0; + while ((result == HasReturnvaluesIF::RETURN_OK) && (i < list->size)) { + result = SerializeAdapter::serialize(&list->entries[i], buffer, size, + max_size, bigEndian); + ++i; + } + return result; + } + + virtual uint32_t getSerializedSize() const { + return getSerializedSize(adaptee); + } + + static uint32_t getSerializedSize(const ArrayList* list) { + uint32_t printSize = sizeof(count_t); + count_t i = 0; + + for (i = 0; i < list->size; ++i) { + printSize += SerializeAdapter::getSerializedSize(&list->entries[i]); + } + + return printSize; + } + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return deSerialize(adaptee, buffer, size, bigEndian); + } + + static ReturnValue_t deSerialize(ArrayList* list, const uint8_t** buffer, int32_t* size, + bool bigEndian) { + ReturnValue_t result = SerializeAdapter::deSerialize(&list->size, + buffer, size, bigEndian); + if (list->size > list->maxSize()) { + return SerializeIF::TOO_MANY_ELEMENTS; + } + count_t i = 0; + while ((result == HasReturnvaluesIF::RETURN_OK) && (i < list->size)) { + result = SerializeAdapter::deSerialize( + &list->front()[i], buffer, size, + bigEndian); + ++i; + } + return result; + } +private: + ArrayList *adaptee; +}; + + + +#endif /* SERIALARRAYLISTADAPTER_H_ */ diff --git a/serialize/SerialBufferAdapter.cpp b/serialize/SerialBufferAdapter.cpp new file mode 100644 index 00000000..da27432f --- /dev/null +++ b/serialize/SerialBufferAdapter.cpp @@ -0,0 +1,52 @@ +#include +#include + +SerialBufferAdapter::SerialBufferAdapter(const uint8_t* buffer, + uint32_t bufferLength) : + constBuffer(buffer), buffer(NULL), bufferLength(bufferLength) { +} + +SerialBufferAdapter::SerialBufferAdapter(uint8_t* buffer, uint32_t bufferLength) : + constBuffer(NULL), buffer(buffer), bufferLength(bufferLength) { +} + +SerialBufferAdapter::~SerialBufferAdapter() { +} + +ReturnValue_t SerialBufferAdapter::serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + if (*size + bufferLength > max_size) { + return BUFFER_TOO_SHORT; + } else { + if (this->constBuffer != NULL) { + memcpy(*buffer, this->constBuffer, bufferLength); + } else if (this->buffer != NULL) { + memcpy(*buffer, this->buffer, bufferLength); + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } + *size += bufferLength; + buffer += bufferLength; + return HasReturnvaluesIF::RETURN_OK; + } +} + +uint32_t SerialBufferAdapter::getSerializedSize() const { + return bufferLength; +} + +ReturnValue_t SerialBufferAdapter::deSerialize(const uint8_t** buffer, + int32_t* size, bool bigEndian) { + if (buffer != NULL) { + if (*size - bufferLength >= 0) { + *size -= bufferLength; + memcpy(this->buffer, *buffer, bufferLength); + buffer += bufferLength; + return HasReturnvaluesIF::RETURN_OK; + } else { + return STREAM_TOO_SHORT; + } + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } +} diff --git a/serialize/SerialBufferAdapter.h b/serialize/SerialBufferAdapter.h new file mode 100644 index 00000000..eb763b24 --- /dev/null +++ b/serialize/SerialBufferAdapter.h @@ -0,0 +1,26 @@ +#ifndef SERIALBUFFERADAPTER_H_ +#define SERIALBUFFERADAPTER_H_ + +#include + +class SerialBufferAdapter: public SerializeIF { +public: + SerialBufferAdapter(const uint8_t * buffer, uint32_t bufferLength); + SerialBufferAdapter(uint8_t* buffer, uint32_t bufferLength); + + virtual ~SerialBufferAdapter(); + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const; + + virtual uint32_t getSerializedSize() const; + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian); +private: + const uint8_t *constBuffer; + uint8_t *buffer; + uint32_t bufferLength; +}; + +#endif /* SERIALBUFFERADAPTER_H_ */ diff --git a/serialize/SerialFixedArrayListAdapter.h b/serialize/SerialFixedArrayListAdapter.h new file mode 100644 index 00000000..14739092 --- /dev/null +++ b/serialize/SerialFixedArrayListAdapter.h @@ -0,0 +1,35 @@ +/* + * SerialFixedArrayListAdapter.h + * + * Created on: 22.07.2014 + * Author: baetz + */ + +#ifndef SERIALFIXEDARRAYLISTADAPTER_H_ +#define SERIALFIXEDARRAYLISTADAPTER_H_ + +#include +#include + +template +class SerialFixedArrayListAdapter : public FixedArrayList, public SerializeIF { +public: + template + SerialFixedArrayListAdapter(Args... args) : FixedArrayList(std::forward(args)...) { + } + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + return SerialArrayListAdapter::serialize(this, buffer, size, max_size, bigEndian); + } + uint32_t getSerializedSize() const { + return SerialArrayListAdapter::getSerializedSize(this); + } + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return SerialArrayListAdapter::deSerialize(this, buffer, size, bigEndian); + } +}; + + + +#endif /* SERIALFIXEDARRAYLISTADAPTER_H_ */ diff --git a/serialize/SerialLinkedListAdapter.h b/serialize/SerialLinkedListAdapter.h new file mode 100644 index 00000000..e6c773fb --- /dev/null +++ b/serialize/SerialLinkedListAdapter.h @@ -0,0 +1,91 @@ +/** + * @file SerialLinkedListAdapter.h + * @brief This file defines the SerialLinkedListAdapter class. + * @date 22.07.2014 + * @author baetz + */ +#ifndef SERIALLINKEDLISTADAPTER_H_ +#define SERIALLINKEDLISTADAPTER_H_ + +#include +#include +#include +#include +//This is where we need the SerializeAdapter! +template +class SerialLinkedListAdapter: public SinglyLinkedList, public SerializeIF { +public: + SerialLinkedListAdapter(typename LinkedElement::Iterator start, + bool printCount = false) : + SinglyLinkedList(start), printCount(printCount) { + } + SerialLinkedListAdapter(LinkedElement* first, bool printCount = false) : + SinglyLinkedList(first), printCount(printCount) { + + } + SerialLinkedListAdapter(bool printCount = false) : + SinglyLinkedList(), printCount(printCount) { + } + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + if (printCount) { + count_t mySize = SinglyLinkedList::getSize(); + ReturnValue_t result = SerializeAdapter::serialize(&mySize, + buffer, size, max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + } + return serialize(SinglyLinkedList::start, buffer, size, max_size, + bigEndian); + } + + static ReturnValue_t serialize(const LinkedElement* element, + uint8_t** buffer, uint32_t* size, const uint32_t max_size, + bool bigEndian) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + while ((result == HasReturnvaluesIF::RETURN_OK) && (element != NULL)) { + result = element->value->serialize(buffer, size, max_size, + bigEndian); + element = element->getNext(); + } + return result; + } + virtual uint32_t getSerializedSize() const { + if (printCount) { + return SerialLinkedListAdapter::getSerializedSize() + + sizeof(count_t); + } else { + return getSerializedSize(SinglyLinkedList::start); + } + } + static uint32_t getSerializedSize(const LinkedElement *element) { + uint32_t size = 0; + while (element != NULL) { + size += element->value->getSerializedSize(); + element = element->getNext(); + } + return size; + } + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return deSerialize(SinglyLinkedList::start, buffer, size, bigEndian); + } + + static ReturnValue_t deSerialize(LinkedElement* element, + const uint8_t** buffer, int32_t* size, bool bigEndian) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + while ((result == HasReturnvaluesIF::RETURN_OK) && (element != NULL)) { + result = element->value->deSerialize(buffer, size, bigEndian); + element = element->getNext(); + } + return result; + } + + bool printCount; + +}; + +#endif /* SERIALLINKEDLISTADAPTER_H_ */ diff --git a/serialize/SerializeAdapter.h b/serialize/SerializeAdapter.h new file mode 100644 index 00000000..f1fb2709 --- /dev/null +++ b/serialize/SerializeAdapter.h @@ -0,0 +1,107 @@ +/* + * SerializeAdapter.h + * + * Created on: 19.03.2014 + * Author: baetz + */ + +#ifndef SERIALIZEADAPTER_H_ +#define SERIALIZEADAPTER_H_ + +#include +#include +#include +#include +#include + +template +class SerializeAdapter_ { +public: + static ReturnValue_t serialize(const T* object, uint8_t** buffer, + uint32_t* size, const uint32_t max_size, bool bigEndian) { + uint32_t ignoredSize = 0; + if (size == NULL) { + size = &ignoredSize; + } + if (sizeof(T) + *size <= max_size) { + T tmp; + if (bigEndian) { + tmp = EndianSwapper::swap(*object); + } else { + tmp = *object; + } + memcpy(*buffer, &tmp, sizeof(T)); + *size += sizeof(T); + (*buffer) += sizeof(T); + return HasReturnvaluesIF::RETURN_OK; + } else { + return SerializeIF::BUFFER_TOO_SHORT; + } + } + + ReturnValue_t deSerialize(T* object, const uint8_t** buffer, int32_t* size, + bool bigEndian) { + T tmp; + *size -= sizeof(T); + if (*size >= 0) { + memcpy(&tmp, *buffer, sizeof(T)); + if (bigEndian) { + *object = EndianSwapper::swap(tmp); + } else { + *object = tmp; + } + *buffer += sizeof(T); + return HasReturnvaluesIF::RETURN_OK; + } else { + return SerializeIF::STREAM_TOO_SHORT; + } + } + + uint32_t getSerializedSize(const T * object) { + return sizeof(T); + } + +}; + +template +class SerializeAdapter_ { +public: + ReturnValue_t serialize(const T* object, uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + uint32_t ignoredSize = 0; + if (size == NULL) { + size = &ignoredSize; + } + return object->serialize(buffer, size, max_size, bigEndian); + } + uint32_t getSerializedSize(const T* object) const { + return object->getSerializedSize(); + } + + ReturnValue_t deSerialize(T* object, const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return object->deSerialize(buffer, size, bigEndian); + } +}; + +template +class SerializeAdapter { +public: + static ReturnValue_t serialize(const T* object, uint8_t** buffer, + uint32_t* size, const uint32_t max_size, bool bigEndian) { + SerializeAdapter_::Is> adapter; + return adapter.serialize(object, buffer, size, max_size, bigEndian); + } + static uint32_t getSerializedSize(const T* object) { + SerializeAdapter_::Is> adapter; + return adapter.getSerializedSize(object); + } + + static ReturnValue_t deSerialize(T* object, const uint8_t** buffer, + int32_t* size, bool bigEndian) { + SerializeAdapter_::Is> adapter; + return adapter.deSerialize(object, buffer, size, bigEndian); + } +}; + +#endif /* SERIALIZEADAPTER_H_ */ diff --git a/serialize/SerializeElement.h b/serialize/SerializeElement.h new file mode 100644 index 00000000..4f0fb1ba --- /dev/null +++ b/serialize/SerializeElement.h @@ -0,0 +1,53 @@ +/* + * SerializeElement.h + * + * Created on: 24.03.2014 + * Author: baetz + */ + +#ifndef SERIALIZEELEMENT_H_ +#define SERIALIZEELEMENT_H_ + +#include +#include +#include + +template +class SerializeElement : public SerializeIF, public LinkedElement { +public: + template + SerializeElement(Args... args) : LinkedElement(this), entry(std::forward(args)...) { + + } + SerializeElement() : LinkedElement(this) { + } + T entry; + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + return SerializeAdapter::serialize(&entry, buffer, size, max_size, bigEndian); + } + + uint32_t getSerializedSize() const { + return SerializeAdapter::getSerializedSize(&entry); + } + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return SerializeAdapter::deSerialize(&entry, buffer, size, bigEndian); + } + operator T() { + return entry; + } + + SerializeElement &operator=(T newValue) { + entry = newValue; + return *this; + } + T *operator->() { + return &entry; + } +}; + + + +#endif /* SERIALIZEELEMENT_H_ */ diff --git a/serialize/SerializeIF.h b/serialize/SerializeIF.h new file mode 100644 index 00000000..efc12630 --- /dev/null +++ b/serialize/SerializeIF.h @@ -0,0 +1,33 @@ +/* + * SerializeIF.h + * + * Created on: 19.03.2014 + * Author: baetz + */ + +#ifndef SERIALIZEIF_H_ +#define SERIALIZEIF_H_ + +#include + +class SerializeIF { +public: + static const uint8_t INTERFACE_ID = SERIALIZE_IF; + static const ReturnValue_t BUFFER_TOO_SHORT = MAKE_RETURN_CODE(1); + static const ReturnValue_t STREAM_TOO_SHORT = MAKE_RETURN_CODE(2); + static const ReturnValue_t TOO_MANY_ELEMENTS = MAKE_RETURN_CODE(3); + + virtual ~SerializeIF() { + } + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const = 0; + + virtual uint32_t getSerializedSize() const = 0; + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) = 0; + +}; + +#endif /* SERIALIZEIF_H_ */ diff --git a/serviceinterface/Makefile b/serviceinterface/Makefile new file mode 100755 index 00000000..fb0de152 --- /dev/null +++ b/serviceinterface/Makefile @@ -0,0 +1,22 @@ +#!/bin/bash +# +# OSAL makefile +# +# Created on: Mar 04, 2010 +# Author: ziemke +# Author: Claas Ziemke +# Copyright 2010, Claas Ziemke +# + +BASEDIR=../../ +include $(BASEDIR)options.mk + +OBJ = $(BUILDDIR)/OPUSLogger.o + +all: $(OBJ) + +$(BUILDDIR)/%.o: %.cpp %.h + $(CPP) $(CFLAGS) $(DEFINES) $(CCOPT) ${INCLUDE} -c $< -o $@ + +clean: + $(RM) *.o *.gcno *.gcda diff --git a/serviceinterface/ServiceInterfaceBuffer.cpp b/serviceinterface/ServiceInterfaceBuffer.cpp new file mode 100644 index 00000000..3e13452a --- /dev/null +++ b/serviceinterface/ServiceInterfaceBuffer.cpp @@ -0,0 +1,136 @@ +/* + * ServiceInterfaceBuffer.cpp + * + * Created on: 06.08.2015 + * Author: baetz + */ + +#include +#include + +int ServiceInterfaceBuffer::overflow(int c) { + // Handle output + putChars(pbase(), pptr()); + if (c != Traits::eof()) { + char c2 = c; + // Handle the one character that didn't fit to buffer + putChars(&c2, &c2 + 1); + } + // This tells that buffer is empty again + setp(buf, buf + BUF_SIZE - 1); + // I'm not sure about this return value! + return 0; +} + +int ServiceInterfaceBuffer::sync(void) { + if (this->isActive) { + TimeOfDay_t loggerTime; + OSAL::getDateAndTime(&loggerTime); + char preamble[96] = { 0 }; + sprintf(preamble, "%s: | %lu:%02lu:%02lu.%03lu | ", + this->log_message.c_str(), (unsigned long) loggerTime.hour, + (unsigned long) loggerTime.minute, + (unsigned long) loggerTime.second, + (unsigned long) loggerTime.ticks); + // Write log_message and time + this->putChars(preamble, preamble + sizeof(preamble)); + // Handle output + this->putChars(pbase(), pptr()); + } + // This tells that buffer is empty again + setp(buf, buf + BUF_SIZE - 1); + return 0; +} + +#ifdef UT699 +ServiceInterfaceBuffer::ServiceInterfaceBuffer(std::string set_message, uint16_t port) { + this->log_message = set_message; + this->isActive = true; + setp( buf, buf + BUF_SIZE ); +} + +void ServiceInterfaceBuffer::putChars(char const* begin, char const* end) { + char array[BUF_SIZE]; + uint32_t length = end - begin; + if (length > sizeof(array)) { + length = sizeof(array); + } + memcpy(array, begin, length); + +#ifdef DEBUG + if (!OSAL::isInterruptInProgress()) { + std::cout << array; + } else { + //Uncomment the following line if you need ISR debug output. +// printk(array); + } +#else +#error Non-DEBUG out messages are not implemented. define DEBUG flag! +#endif +} +#endif //UT699 + +#ifdef ML505 +#include +ServiceInterfaceBuffer::ServiceInterfaceBuffer(std::string set_message, + uint16_t port) : + isActive(true), log_message(set_message), udpSocket(0), remoteAddressLength( + sizeof(remoteAddress)) { + setp(buf, buf + BUF_SIZE); + memset((uint8_t*) &remoteAddress, 0, sizeof(remoteAddress)); + remoteAddress.sin_family = AF_INET; + remoteAddress.sin_port = htons(port); + remoteAddress.sin_addr.s_addr = htonl(inet_addr("192.168.250.100")); +} + +void ServiceInterfaceBuffer::putChars(char const* begin, char const* end) { + + char array[BUF_SIZE]; + uint32_t length = end - begin; + if (length > sizeof(array)) { + length = sizeof(array); + } + memcpy(array, begin, length); + + if (udpSocket <= 0) { + initSocket(); + } +#ifdef DEBUG + if (udpSocket > 0) { + sendto(udpSocket, array, length, 0, (sockaddr*) &remoteAddress, + sizeof(remoteAddress)); + } +#else +#error Non-DEBUG out messages are not implemented. define DEBUG flag! +#endif +} + +void ServiceInterfaceBuffer::initSocket() { + sockaddr_in address; + memset((uint8_t*) &address, 0, sizeof(address)); + address.sin_family = AF_INET; + address.sin_port = htons(0); + address.sin_addr.s_addr = htonl(INADDR_ANY); + + udpSocket = socket(PF_INET, SOCK_DGRAM, 0); + if (socket < 0) { + printf("Error opening socket!\n"); + return; + } + timeval timeout = { 0, 20 }; + if (setsockopt(udpSocket, SOL_SOCKET, SO_RCVTIMEO, &timeout, + sizeof(timeout)) < 0) { + printf("Error setting SO_RCVTIMEO socket options!\n"); + return; + } + if (setsockopt(udpSocket, SOL_SOCKET, SO_SNDTIMEO, &timeout, + sizeof(timeout)) < 0) { + printf("Error setting SO_SNDTIMEO socket options!\n"); + return; + } + if (bind(udpSocket, (sockaddr *) &address, sizeof(address)) < 0) { + printf("Error binding socket!\n"); + } +} + +#endif //ML505 diff --git a/serviceinterface/ServiceInterfaceBuffer.h b/serviceinterface/ServiceInterfaceBuffer.h new file mode 100644 index 00000000..7fa1dd54 --- /dev/null +++ b/serviceinterface/ServiceInterfaceBuffer.h @@ -0,0 +1,92 @@ +/* + * ServiceInterfaceBuffer.h + * + * Created on: 06.08.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACEBUFFER_H_ +#define FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACEBUFFER_H_ + +#include +#include +#include +#include + +#ifdef UT699 +class ServiceInterfaceBuffer: public std::basic_streambuf > { + friend class ServiceInterfaceStream; +public: + ServiceInterfaceBuffer(std::string set_message, uint16_t port); +protected: + bool isActive; + // This is called when buffer becomes full. If + // buffer is not used, then this is called every + // time when characters are put to stream. + virtual int overflow(int c = Traits::eof()); + + // This function is called when stream is flushed, + // for example when std::endl is put to stream. + virtual int sync(void); + +private: + // For additional message information + std::string log_message; + // For EOF detection + typedef std::char_traits Traits; + + // Work in buffer mode. It is also possible to work without buffer. + static size_t const BUF_SIZE = 128; + char buf[BUF_SIZE]; + + // In this function, the characters are parsed. + void putChars(char const* begin, char const* end); +}; +#endif //UT699 + +#ifdef ML505 +#include +#include +#include +#include +#include + +class ServiceInterfaceBuffer: public std::basic_streambuf > { + friend class ServiceInterfaceStream; +public: + ServiceInterfaceBuffer(std::string set_message, uint16_t port); +protected: + bool isActive; + // This is called when buffer becomes full. If + // buffer is not used, then this is called every + // time when characters are put to stream. + virtual int overflow(int c = Traits::eof()); + + // This function is called when stream is flushed, + // for example when std::endl is put to stream. + virtual int sync(void); + +private: + // For additional message information + std::string log_message; + // For EOF detection + typedef std::char_traits Traits; + + // Work in buffer mode. It is also possible to work without buffer. + static size_t const BUF_SIZE = 128; + char buf[BUF_SIZE]; + + // In this function, the characters are parsed. + void putChars(char const* begin, char const* end); + + int udpSocket; + sockaddr_in remoteAddress; + socklen_t remoteAddressLength; + void initSocket(); +}; +#endif //ML505 + + +#endif /* FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACEBUFFER_H_ */ diff --git a/serviceinterface/ServiceInterfaceStream.cpp b/serviceinterface/ServiceInterfaceStream.cpp new file mode 100644 index 00000000..7d61dabe --- /dev/null +++ b/serviceinterface/ServiceInterfaceStream.cpp @@ -0,0 +1,19 @@ +/* + * ServiceInterfaceStream.cpp + * + * Created on: 06.08.2015 + * Author: baetz + */ + + +#include + +void ServiceInterfaceStream::setActive( bool myActive) { + this->buf.isActive = myActive; +} + +ServiceInterfaceStream::ServiceInterfaceStream(std::string set_message, + uint16_t port) : + std::basic_ostream >(&buf), buf( + set_message, port) { +} diff --git a/serviceinterface/ServiceInterfaceStream.h b/serviceinterface/ServiceInterfaceStream.h new file mode 100644 index 00000000..a95a67e5 --- /dev/null +++ b/serviceinterface/ServiceInterfaceStream.h @@ -0,0 +1,33 @@ +/* + * ServiceInterfaceStream.h + * + * Created on: 06.08.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACESTREAM_H_ +#define FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACESTREAM_H_ + +#include +#include +#include +#include +#include + +//Unfortunately, there must be a forward declaration of log_fe (MUST be defined in main), to let the system know where to write to. +extern std::ostream debug; +extern std::ostream info; +extern std::ostream warning; +extern std::ostream error; + +class ServiceInterfaceStream : public std::basic_ostream< char, std::char_traits< char > > { +protected: + ServiceInterfaceBuffer buf; +public: + ServiceInterfaceStream( std::string set_message, uint16_t port = 1234 ); + void setActive( bool ); +}; + + + +#endif /* FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACESTREAM_H_ */ diff --git a/storagemanager/LocalPool.h b/storagemanager/LocalPool.h new file mode 100644 index 00000000..253aa8b6 --- /dev/null +++ b/storagemanager/LocalPool.h @@ -0,0 +1,419 @@ +/* + * LocalPool.h + * + * Created on: 11.02.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_STORAGEMANAGER_LOCALPOOL_H_ +#define FRAMEWORK_STORAGEMANAGER_LOCALPOOL_H_ + +/** + * @file LocalPool + * + * @date 02.02.2012 + * @author Bastian Baetz + * + * @brief This file contains the definition of the LocalPool class. + */ + +#include +#include +#include +#include +//TODO: Debugging.. remove! +//#include + +/** + * @brief The LocalPool class provides an intermediate data storage with + * a fixed pool size policy. + * \details The class implements the StorageManagerIF interface. While the + * total number of pools is fixed, the element sizes in one pool and + * the number of pool elements per pool are set on construction. + * The full amount of memory is allocated on construction. + * The overhead is 4 byte per pool element to store the size + * information of each stored element. + * To maintain an "empty" information, the pool size is limited to + * 0xFFFF-1 bytes. + * It is possible to store empty packets in the pool. + * The local pool is NOT thread-safe. + */ + +template +class LocalPool: public SystemObject, public StorageManagerIF { +public: + /** + * @brief This definition generally sets the number of different sized pools. + * @details This must be less than the maximum number of pools (currently 0xff). + */ +// static const uint32_t NUMBER_OF_POOLS; +private: + /** + * Indicates that this element is free. + * This value limits the maximum size of a pool. Change to larger data type if increase is required. + */ + static const uint32_t STORAGE_FREE = 0xFFFFFFFF; + /** + * @brief In this array, the element sizes of each pool is stored. + * @details The sizes are maintained for internal pool management. The sizes + * must be set in ascending order on construction. + */ + uint32_t element_sizes[NUMBER_OF_POOLS]; + /** + * @brief n_elements stores the number of elements per pool. + * @details These numbers are maintained for internal pool management. + */ + uint16_t n_elements[NUMBER_OF_POOLS]; + /** + * @brief store represents the actual memory pool. + * @details It is an array of pointers to memory, which was allocated with + * a \c new call on construction. + */ + uint8_t* store[NUMBER_OF_POOLS]; + /** + * @brief The size_list attribute stores the size values of every pool element. + * @details As the number of elements is determined on construction, the size list + * is also dynamically allocated there. + */ + uint32_t* size_list[NUMBER_OF_POOLS]; + bool spillsToHigherPools; //!< A variable to determine whether higher n pools are used if the store is full. + /** + * @brief This method safely stores the given data in the given packet_id. + * @details It also sets the size in size_list. The method does not perform + * any range checks, these are done in advance. + * @param packet_id The storage identifier in which the data shall be stored. + * @param data The data to be stored. + * @param size The size of the data to be stored. + */ + void write(store_address_t packet_id, const uint8_t* data, uint32_t size); + /** + * @brief A helper method to read the element size of a certain pool. + * @param pool_index The pool in which to look. + * @return Returns the size of an element or 0. + */ + uint32_t getPageSize(uint16_t pool_index); + /** + * @brief This helper method looks up a fitting pool for a given size. + * @details The pools are looked up in ascending order, so the first that + * fits is used. + * @param packet_size The size of the data to be stored. + * @return Returns the pool that fits or StorageManagerIF::INVALID_ADDRESS. + */ + /** + * @brief This helper method looks up a fitting pool for a given size. + * @details The pools are looked up in ascending order, so the first that + * fits is used. + * @param packet_size The size of the data to be stored. + * @param[out] poolIndex The fitting pool index found. + * @return - #RETURN_OK on success, + * - #DATA_TOO_LARGE otherwise. + */ + ReturnValue_t getPoolIndex(uint32_t packet_size, uint16_t* poolIndex, uint16_t startAtIndex = 0); + /** + * @brief This helper method calculates the true array position in store + * of a given packet id. + * @details The method does not perform any range checks, these are done in + * advance. + * @param packet_id The packet id to look up. + * @return Returns the position of the data in store. + */ + uint32_t getRawPosition(store_address_t packet_id); + /** + * With this helper method, a free element of \c size is reserved. + * + * @param size The minimum packet size that shall be reserved. + * @return Returns the storage identifier within the storage or + * StorageManagerIF::INVALID_ADDRESS (in raw). + */ + /** + * With this helper method, a free element of \c size is reserved. + * @param size The minimum packet size that shall be reserved. + * @param[out] address Storage ID of the reserved data. + * @return - #RETURN_OK on success, + * - the return codes of #getPoolIndex or #findEmpty otherwise. + */ + ReturnValue_t reserveSpace(const uint32_t size, store_address_t* address); +protected: + /** + * @brief This is a helper method to find an empty element in a given pool. + * @details The method searches size_list for the first empty element, so + * duration grows with the fill level of the pool. + * @param pool_index The pool in which the search is performed. + * @param[out] element The first found element in the pool. + * @return - #RETURN_OK on success, + * - #DATA_STORAGE_FULL if the store is full + */ + virtual ReturnValue_t findEmpty(uint16_t pool_index, uint16_t* element); +public: + /** + * @brief This is the default constructor for a pool manager instance. + * @details By passing two arrays of size NUMBER_OF_POOLS, the constructor + * allocates memory (with \c new) for store and size_list. These + * regions are all set to zero on start up. + * @param setObjectId The object identifier to be set. This allows for + * multiple instances of LocalPool in the system. + * @param element_sizes An array of size NUMBER_OF_POOLS in which the size + * of a single element in each pool is determined. + * The sizes must be provided in ascending order. + * + * @param n_elements An array of size NUMBER_OF_POOLS in which the + * number of elements for each pool is determined. + * The position of these values correspond to those in + * element_sizes. + * @param registered Register the pool in object manager or not. Default is false (local pool). + */ + LocalPool(object_id_t setObjectId, + const uint16_t element_sizes[NUMBER_OF_POOLS], + const uint16_t n_elements[NUMBER_OF_POOLS], + bool registered = false, + bool spillsToHigherPools = false); + /** + * @brief In the LocalPool's destructor all allocated memory is freed. + */ + virtual ~LocalPool(void); + ReturnValue_t addData(store_address_t* storageId, const uint8_t * data, + uint32_t size); + + ReturnValue_t getFreeElement(store_address_t* storageId, + const uint32_t size, uint8_t** p_data); + ReturnValue_t getData(store_address_t packet_id, const uint8_t** packet_ptr, + uint32_t* size); + ReturnValue_t modifyData(store_address_t packet_id, uint8_t** packet_ptr, + uint32_t* size); + virtual ReturnValue_t deleteData(store_address_t); + virtual ReturnValue_t deleteData(uint8_t* ptr, uint32_t size, + store_address_t* storeId = NULL); + void clearStore(); + ReturnValue_t initialize(); +}; + +template +inline ReturnValue_t LocalPool::findEmpty(uint16_t pool_index, + uint16_t* element) { + ReturnValue_t status = DATA_STORAGE_FULL; + for (uint16_t foundElement = 0; foundElement < n_elements[pool_index]; + foundElement++) { + if (size_list[pool_index][foundElement] == STORAGE_FREE) { + *element = foundElement; + status = RETURN_OK; + break; + } + } + return status; +} + +template +inline void LocalPool::write(store_address_t packet_id, + const uint8_t* data, uint32_t size) { + uint8_t* ptr; + uint32_t packet_position = getRawPosition(packet_id); + + //check size? -> Not necessary, because size is checked before calling this function. + ptr = &store[packet_id.pool_index][packet_position]; + memcpy(ptr, data, size); + size_list[packet_id.pool_index][packet_id.packet_index] = size; +} + +//Returns page size of 0 in case store_index is illegal +template +inline uint32_t LocalPool::getPageSize(uint16_t pool_index) { + if (pool_index < NUMBER_OF_POOLS) { + return element_sizes[pool_index]; + } else { + return 0; + } +} + +template +inline ReturnValue_t LocalPool::getPoolIndex( + uint32_t packet_size, uint16_t* poolIndex, uint16_t startAtIndex) { + for (uint16_t n = startAtIndex; n < NUMBER_OF_POOLS; n++) { +// debug << "LocalPool " << getObjectId() << "::getPoolIndex: Pool: " << n << ", Element Size: " << element_sizes[n] << std::endl; + if (element_sizes[n] >= packet_size) { + *poolIndex = n; + return RETURN_OK; + } + } + return DATA_TOO_LARGE; +} + +template +inline uint32_t LocalPool::getRawPosition( + store_address_t packet_id) { + return packet_id.packet_index * element_sizes[packet_id.pool_index]; +} + +template +inline ReturnValue_t LocalPool::reserveSpace( + const uint32_t size, store_address_t* address) { + ReturnValue_t status = getPoolIndex(size, &address->pool_index); + if (status != RETURN_OK) { + error << "LocalPool( " << std::hex << getObjectId() << std::dec + << " )::reserveSpace: Packet too large." << std::endl; + } + status = findEmpty(address->pool_index, &address->packet_index); + while (status != RETURN_OK && spillsToHigherPools) { + status = getPoolIndex(size, &address->pool_index, address->pool_index + 1); + if (status != RETURN_OK) { + //We don't find any fitting pool anymore. + break; + } + status = findEmpty(address->pool_index, &address->packet_index); + } + if (status == RETURN_OK) { +// debug << "LocalPool( " << translateObject(getObjectId()) << " )::reserveSpace: Empty position found: Position: Pool: " << address->pool_index << " Index: " << address->packet_index << std::endl; + size_list[address->pool_index][address->packet_index] = size; + } else { + error << "LocalPool( " << std::hex << getObjectId() << std::dec + << " )::reserveSpace: Packet store is full." << std::endl; + } + return status; +} + +template +inline LocalPool::LocalPool(object_id_t setObjectId, + const uint16_t element_sizes[NUMBER_OF_POOLS], + const uint16_t n_elements[NUMBER_OF_POOLS], bool registered, bool spillsToHigherPools) : + SystemObject(setObjectId, registered), spillsToHigherPools(spillsToHigherPools) { + for (uint16_t n = 0; n < NUMBER_OF_POOLS; n++) { + this->element_sizes[n] = element_sizes[n]; + this->n_elements[n] = n_elements[n]; + store[n] = new uint8_t[n_elements[n] * element_sizes[n]]; + size_list[n] = new uint32_t[n_elements[n]]; + memset(store[n], 0x00, (n_elements[n] * element_sizes[n])); + memset(size_list[n], STORAGE_FREE, (n_elements[n] * sizeof(**size_list))); //TODO checkme + } +} + +template +inline LocalPool::~LocalPool(void) { + for (uint16_t n = 0; n < NUMBER_OF_POOLS; n++) { + delete[] store[n]; + delete[] size_list[n]; + } +} + +template +inline ReturnValue_t LocalPool::addData( + store_address_t* storageId, const uint8_t* data, uint32_t size) { + ReturnValue_t status = reserveSpace(size, storageId); + if (status == RETURN_OK) { + write(*storageId, data, size); + } + return status; +} + +template +inline ReturnValue_t LocalPool::getFreeElement( + store_address_t* storageId, const uint32_t size, uint8_t** p_data) { + ReturnValue_t status = reserveSpace(size, storageId); + if (status == RETURN_OK) { + *p_data = &store[storageId->pool_index][getRawPosition(*storageId)]; + } else { + *p_data = NULL; + } + return status; +} + +template +inline ReturnValue_t LocalPool::getData( + store_address_t packet_id, const uint8_t** packet_ptr, uint32_t* size) { + uint8_t* tempData = NULL; + ReturnValue_t status = modifyData(packet_id, &tempData, size); + *packet_ptr = tempData; + return status; +} + +template +inline ReturnValue_t LocalPool::modifyData(store_address_t packet_id, + uint8_t** packet_ptr, uint32_t* size) { + ReturnValue_t status = RETURN_FAILED; + if ((packet_id.packet_index < n_elements[packet_id.pool_index]) + && (packet_id.pool_index < NUMBER_OF_POOLS)) { + if (size_list[packet_id.pool_index][packet_id.packet_index] + != STORAGE_FREE) { + uint32_t packet_position = getRawPosition(packet_id); + *packet_ptr = &store[packet_id.pool_index][packet_position]; + *size = size_list[packet_id.pool_index][packet_id.packet_index]; + status = RETURN_OK; + } else { + status = DATA_DOES_NOT_EXIST; + } + } else { + status = ILLEGAL_STORAGE_ID; + } + return status; +} + +template +inline ReturnValue_t LocalPool::deleteData( + store_address_t packet_id) { +// debug << "LocalPool( " << translateObject(getObjectId()) << " )::deleteData from store " << packet_id.pool_index << ". id is " << packet_id.packet_index << std::endl; + ReturnValue_t status = RETURN_OK; + uint32_t page_size = getPageSize(packet_id.pool_index); + if ((page_size != 0) + && (packet_id.packet_index < n_elements[packet_id.pool_index])) { + uint16_t packet_position = getRawPosition(packet_id); + uint8_t* ptr = &store[packet_id.pool_index][packet_position]; + memset(ptr, 0, page_size); + //Set free list + size_list[packet_id.pool_index][packet_id.packet_index] = STORAGE_FREE; + } else { + //packet_index is too large + error << "LocalPool:deleteData failed." << std::endl; + status = ILLEGAL_STORAGE_ID; + } + return status; +} + +template +inline void LocalPool::clearStore() { + for (uint16_t n = 0; n < NUMBER_OF_POOLS; n++) { + memset(size_list[n], STORAGE_FREE, (n_elements[n] * sizeof(**size_list)));//TODO checkme + } +} + +template +inline ReturnValue_t LocalPool::deleteData(uint8_t* ptr, + uint32_t size, store_address_t* storeId) { + store_address_t localId; + ReturnValue_t result = ILLEGAL_ADDRESS; + for (uint16_t n = 0; n < NUMBER_OF_POOLS; n++) { + //Not sure if new allocates all stores in order. so better be careful. + if ((store[n] <= ptr) && (&store[n][n_elements[n]*element_sizes[n]]) > ptr) { + localId.pool_index = n; + uint32_t deltaAddress = (uint32_t) ptr - (uint32_t) store[n]; + //Getting any data from the right "block" is ok. This is necessary, as IF's sometimes don't point to the first element of an object. + localId.packet_index = deltaAddress / element_sizes[n]; + result = deleteData(localId); +// if (deltaAddress % element_sizes[n] != 0) { +// error << "Pool::deleteData: address not aligned!" << std::endl; +// } + break; + } + } + if (storeId != NULL) { + *storeId = localId; + } + return result; +} + +template +inline ReturnValue_t LocalPool::initialize() { + ReturnValue_t result = SystemObject::initialize(); + if (result != RETURN_OK) { + return result; + } + //Check if any pool size is large than the maximum allowed. + for (uint8_t count = 0; count < NUMBER_OF_POOLS; count++) { + if (element_sizes[count] >= STORAGE_FREE) { + error + << "LocalPool::initialize: Pool is too large! Max. allowed size is: " + << (STORAGE_FREE - 1) << std::endl; + return RETURN_FAILED; + } + } + return RETURN_OK; +} + +#endif /* FRAMEWORK_STORAGEMANAGER_LOCALPOOL_H_ */ diff --git a/storagemanager/Makefile b/storagemanager/Makefile new file mode 100755 index 00000000..9ca946ac --- /dev/null +++ b/storagemanager/Makefile @@ -0,0 +1,22 @@ +#!/bin/bash +# +# OSAL makefile +# +# Created on: Mar 04, 2010 +# Author: ziemke +# Author: Claas Ziemke +# Copyright 2010, Claas Ziemke +# + +BASEDIR=../../ +include $(BASEDIR)options.mk + +OBJ = $(BUILDDIR)/TmTcStorage.o + +all: $(OBJ) + +$(BUILDDIR)/%.o: %.cpp %.h + $(CPP) $(CFLAGS) $(DEFINES) $(CCOPT) ${INCLUDE} -c $< -o $@ + +clean: + $(RM) *.o *.gcno *.gcda diff --git a/storagemanager/PoolManager.h b/storagemanager/PoolManager.h new file mode 100644 index 00000000..9be74049 --- /dev/null +++ b/storagemanager/PoolManager.h @@ -0,0 +1,121 @@ +/** + * @file PoolManager + * + * @date 02.02.2012 + * @author Bastian Baetz + * + * @brief This file contains the definition of the PoolManager class. + */ + +#ifndef POOLMANAGER_H_ +#define POOLMANAGER_H_ + + +#include +#include + +/** + * @brief The PoolManager class provides an intermediate data storage with + * a fixed pool size policy for inter-process communication. + * \details Uses local pool, but is thread-safe. + */ + +template +class PoolManager : public LocalPool { +protected: + /** + * Overwritten for thread safety. + * Locks during execution. + */ + ReturnValue_t findEmpty( uint16_t pool_index, uint16_t* element ); + /** + * \brief The mutex is created in the constructor and makes access mutual exclusive. + * \details Locking and unlocking is done during searching for free slots and deleting existing slots. + */ + MutexId_t* mutex; +public: + PoolManager( object_id_t setObjectId, const uint16_t element_sizes[NUMBER_OF_POOLS], const uint16_t n_elements[NUMBER_OF_POOLS] ); + /** + * @brief In the PoolManager's destructor all allocated memory is freed. + */ + virtual ~PoolManager( void ); + /** + * Overwritten for thread safety. + */ + virtual ReturnValue_t deleteData(store_address_t); + virtual ReturnValue_t deleteData(uint8_t* buffer, uint32_t size, store_address_t* storeId = NULL); +}; + +template +inline ReturnValue_t PoolManager::findEmpty(uint16_t pool_index, + uint16_t* element) { + ReturnValue_t mutexStatus = OSAL::lockMutex( mutex, OSAL::NO_TIMEOUT ); + ReturnValue_t status = this->DATA_STORAGE_FULL; + if ( mutexStatus == this->RETURN_OK ) { + status = LocalPool::findEmpty(pool_index, element); + } else { + error << "PoolManager::findEmpty: Mutex could not be acquired. Error code: " << status << std::endl; + } + mutexStatus = OSAL::unlockMutex( mutex ); + if (mutexStatus != this->RETURN_OK) { + return mutexStatus; + } else { + return status; + } +} + +template +inline PoolManager::PoolManager(object_id_t setObjectId, + const uint16_t element_sizes[NUMBER_OF_POOLS], + const uint16_t n_elements[NUMBER_OF_POOLS]) : LocalPool(setObjectId, element_sizes, n_elements, true) { + mutex = new MutexId_t; + ReturnValue_t result = OSAL::createMutex( OSAL::buildName('M','T','X','1'), ( mutex ) ); + if (result != this->RETURN_OK) { + error << "PoolManager( " << std::hex << this->getObjectId() << std::dec << " )::ctor: Creating mutex failed." << std::endl; + } +} + +template +inline PoolManager::~PoolManager(void) { + OSAL::deleteMutex( mutex ); + delete mutex; +} + +template +inline ReturnValue_t PoolManager::deleteData( + store_address_t packet_id) { + // debug << "PoolManager( " << translateObject(getObjectId()) << " )::deleteData from store " << packet_id.pool_index << ". id is " << packet_id.packet_index << std::endl; + ReturnValue_t mutexStatus = OSAL::lockMutex( mutex, OSAL::NO_TIMEOUT ); + ReturnValue_t status = this->RETURN_OK; + if ( mutexStatus == this->RETURN_OK ) { + LocalPool::deleteData(packet_id); + } else { + error << "PoolManager:deleteData: Mutex could not be acquired. Error code: " << status << std::endl; + } + mutexStatus = OSAL::unlockMutex( mutex ); + if (mutexStatus != this->RETURN_OK) { + return mutexStatus; + } else { + return status; + } +} + +template +inline ReturnValue_t PoolManager::deleteData(uint8_t* buffer, uint32_t size, + store_address_t* storeId) { + ReturnValue_t mutexStatus = OSAL::lockMutex( mutex, OSAL::NO_TIMEOUT ); + ReturnValue_t status = this->RETURN_OK; + if ( mutexStatus == this->RETURN_OK ) { + LocalPool::deleteData(buffer, size, storeId); + } else { + error << "PoolManager:deleteData: Mutex could not be acquired. Error code: " << status << std::endl; + } + mutexStatus = OSAL::unlockMutex( mutex ); + if (mutexStatus != this->RETURN_OK) { + return mutexStatus; + } else { + return status; + } +} + +#endif /* POOLMANAGER_H_ */ diff --git a/storagemanager/StorageManagerIF.h b/storagemanager/StorageManagerIF.h new file mode 100644 index 00000000..40afb093 --- /dev/null +++ b/storagemanager/StorageManagerIF.h @@ -0,0 +1,158 @@ +#ifndef STORAGEMANAGERIF_H_H +#define STORAGEMANAGERIF_H_H + +#include +#include +#include + +//TODO: Check setting and returning of store sizes. + +/** + * This union defines the type that identifies where a data packet is stored in the store. + * It comprises of a raw part to read it as raw value and a structured part to use it in + * pool-like stores. + */ +union store_address_t { + /** + * Default Constructor, initializing to INVALID_ADDRESS + */ + store_address_t():raw(0xFFFFFFFF){} + /** + * Constructor to create an address object using the raw address + * + * @param rawAddress + */ + store_address_t(uint32_t rawAddress):raw(rawAddress){} + + /** + * Constructor to create an address object using pool + * and packet indices + * + * @param poolIndex + * @param packetIndex + */ + store_address_t(uint16_t poolIndex, uint16_t packetIndex): + pool_index(poolIndex),packet_index(packetIndex){} + /** + * A structure with two elements to access the store address pool-like. + */ + struct { + /** + * The index in which pool the packet lies. + */ + uint16_t pool_index; + /** + * The position in the chosen pool. + */ + uint16_t packet_index; + }; + /** + * Alternative access to the raw value. + */ + uint32_t raw; +}; + +/** + * @brief This class provides an interface for intermediate data storage. + * @details The Storage manager classes shall be used to store larger chunks of + * data in RAM for exchange between tasks. This interface expects the + * data to be stored in one consecutive block of memory, so tasks can + * write directly to the destination pointer. + * For interprocess communication, the stores must be locked during + * insertion and deletion. If the receiving storage identifier is + * passed token-like between tasks, a lock during read and write + * operations is not necessary. + * @author Bastian Baetz + * @date 18.09.2012 + */ +class StorageManagerIF : public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = STORAGE_MANAGER_IF; //!< The unique ID for return codes for this interface. + static const ReturnValue_t DATA_TOO_LARGE = MAKE_RETURN_CODE(1); //!< This return code indicates that the data to be stored is too large for the store. + static const ReturnValue_t DATA_STORAGE_FULL = MAKE_RETURN_CODE(2); //!< This return code indicates that a data storage is full. + static const ReturnValue_t ILLEGAL_STORAGE_ID = MAKE_RETURN_CODE(3); //!< This return code indicates that data was requested with an illegal storage ID. + static const ReturnValue_t DATA_DOES_NOT_EXIST = MAKE_RETURN_CODE(4); //!< This return code indicates that the requested ID was valid, but no data is stored there. + static const ReturnValue_t ILLEGAL_ADDRESS = MAKE_RETURN_CODE(5); + + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::OBSW; + static const Event GET_DATA_FAILED = MAKE_EVENT(0, SEVERITY::LOW); + static const Event STORE_DATA_FAILED = MAKE_EVENT(1, SEVERITY::LOW); + + static const uint32_t INVALID_ADDRESS = 0xFFFFFFFF; //!< Indicates an invalid (i.e unused) storage address. + /** + * @brief This is the empty virtual destructor as required for C++ interfaces. + */ + virtual ~StorageManagerIF() { + } + ; + /** + * @brief With addData, a free storage position is allocated and data + * stored there. + * @details During the allocation, the StorageManager is blocked. + * @param storageId A pointer to the storageId to retrieve. + * @param data The data to be stored in the StorageManager. + * @param size The amount of data to be stored. + * @return Returns @li RETURN_OK if data was added. + * @li RETURN_FAILED if data could not be added. + * storageId is unchanged then. + */ + virtual ReturnValue_t addData(store_address_t* storageId, const uint8_t * data, uint32_t size) = 0; + /** + * @brief With deleteData, the storageManager frees the memory region + * identified by packet_id. + * @param packet_id The identifier of the memory region to be freed. + * @return @li RETURN_OK on success. + * @li RETURN_FAILED if deletion did not work + * (e.g. an illegal packet_id was passed). + */ + virtual ReturnValue_t deleteData(store_address_t packet_id) = 0; + /** + * @brief Another deleteData which uses the pointer and size of the stored data to delete the content. + * @param buffer Pointer to the data. + * @param size Size of data to be stored. + * @param storeId Store id of the deleted element (optional) + * @return @li RETURN_OK on success. + * @li failure code if deletion did not work + */ + virtual ReturnValue_t deleteData(uint8_t* buffer, uint32_t size, store_address_t* storeId = NULL) = 0; + /** + * @brief getData returns an address to data and the size of the data + * for a given packet_id. + * @param packet_id The id of the data to be returned + * @param packet_ptr The passed pointer address is set to the the memory + * position + * @param size The exact size of the stored data is returned here. + * @return @li RETURN_OK on success. + * @li RETURN_FAILED if fetching data did not work + * (e.g. an illegal packet_id was passed). + */ + virtual ReturnValue_t getData(store_address_t packet_id, + const uint8_t** packet_ptr, uint32_t* size) = 0; + /** + * Same as above, but not const and therefore modifiable. + */ + virtual ReturnValue_t modifyData(store_address_t packet_id, + uint8_t** packet_ptr, uint32_t* size) = 0; + /** + * This method reserves an element of \c size. + * + * It returns the packet id of this element as well as a direct pointer to the + * data of the element. It must be assured that exactly \c size data is + * written to p_data! + * @param storageId A pointer to the storageId to retrieve. + * @param size The size of the space to be reserved. + * @param p_data A pointer to the element data is returned here. + * @return Returns @li RETURN_OK if data was added. + * @li RETURN_FAILED if data could not be added. + * storageId is unchanged then. + */ + virtual ReturnValue_t getFreeElement(store_address_t* storageId, const uint32_t size, uint8_t** p_data ) = 0; + /** + * Clears the whole store. + * Use with care! + */ + virtual void clearStore() = 0; + +}; + +#endif /* STORAGEMANAGERIF_H_ */ diff --git a/subsystem/Subsystem.cpp b/subsystem/Subsystem.cpp new file mode 100644 index 00000000..4905b6aa --- /dev/null +++ b/subsystem/Subsystem.cpp @@ -0,0 +1,636 @@ +/* + * Subsystem.cpp + * + * Created on: 12.07.2013 + * Author: tod + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +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), modeStore(NULL) { + +} + +Subsystem::~Subsystem() { + //Auto-generated destructor stub +} + +ReturnValue_t Subsystem::checkSequence(HybridIterator 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 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 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) == RETURN_OK) { + setMode(targetMode, targetSubmode); + isInTransition = false; + return; + } else { + transitionFailed(TARGET_TABLE_NOT_REACHED, + getSequence(targetMode)->getTableId()); + return; + } + } + if (currentSequenceIterator->checkSuccess()) { + if (checkStateAgainstTable(getCurrentTable()) != RETURN_OK) { + transitionFailed(TABLE_CHECK_FAILED, + currentSequenceIterator->getTableId()); + return; + } + } + if (currentSequenceIterator->getWaitSeconds() != 0) { + if (uptimeStartTable != 0) { + OSAL::getUptime(&uptimeStartTable); + return; + } else { + uint32_t uptimeNow; + OSAL::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()); + } + } + } else { + if (childrenChangedHealth) { + triggerEvent(CHILD_CHANGED_HEALTH, 0, 0); + childrenChangedHealth = false; + startTransition(mode, submode); + } else if (childrenChangedMode) { + if (checkStateAgainstTable(currentTargetTable) != RETURN_OK) { + triggerEvent(CANT_KEEP_MODE, mode, submode); + cantKeepMode(); + } + } + } +} + +HybridIterator Subsystem::getSequence(Mode_t id) { + SequenceInfo *sequenceInfo = modeSequences.findValue(id); + if (sequenceInfo->entries.islinked) { + return HybridIterator( + sequenceInfo->entries.firstLinkedElement); + } else { + return HybridIterator( + sequenceInfo->entries.array->front(), + sequenceInfo->entries.array->back()); + } +} + +HybridIterator Subsystem::getTable(Mode_t id) { + EntryPointer *entry = modeTables.findValue(id); + if (entry->islinked) { + return HybridIterator(entry->firstLinkedElement); + } else { + return HybridIterator(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 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::deSerialize(&fallbackId, + &pointer, &size, true); + if (result == RETURN_OK) { + result = SerialArrayListAdapter::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 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::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 sequences; + FixedMap::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 tables; + FixedMap::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 sequenceId(sequence); + SerializeElement fallbackSequenceId( + getFallbackSequence(sequence)); + + elements[0] = &sequenceId; + elements[1] = &fallbackSequenceId; + + if (sequenceInfo->entries.islinked) { + SerialLinkedListAdapter list( + sequenceInfo->entries.firstLinkedElement, true); + elements[2] = &list; + sendSerializablesAsCommandMessage(ModeSequenceMessage::SEQUENCE, + elements, 3); + } else { + SerialArrayListAdapter 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 tableId(table); + + elements[0] = &tableId; + + if (entry->islinked) { + SerialLinkedListAdapter list( + entry->firstLinkedElement, true); + elements[1] = &list; + sendSerializablesAsCommandMessage(ModeSequenceMessage::TABLE, + elements, 2); + } else { + SerialArrayListAdapter 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* 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(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) { + result = modeStore->storeArray(sequence, + &(modeSequences.find(id)->entries.firstLinkedElement)); + if (result != RETURN_OK) { + modeSequences.erase(id); + } + } + return result; +} + +ReturnValue_t Subsystem::addTable(ArrayList *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(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) { + result = modeStore->storeArray(table, + &(modeTables.find(id)->firstLinkedElement)); + if (result != RETURN_OK) { + modeTables.erase(id); + } + } + 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; + } + + modeStore->deleteList(sequenceInfo->entries.firstLinkedElement); + 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; + } + modeStore->deleteList(pointer->firstLinkedElement); + modeSequences.erase(id); + return RETURN_OK; +} + +ReturnValue_t Subsystem::initialize() { + ReturnValue_t result = SubsystemBase::initialize(); + + if (result != RETURN_OK) { + return result; + } + + IPCStore = objectManager->get(objects::IPC_STORE); + modeStore = objectManager->get(objects::MODE_STORE); + + if ((IPCStore == NULL) || (modeStore == NULL)) { + return RETURN_FAILED; + } + + 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) { + 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()); + } +} + +Mode_t Subsystem::getFallbackSequence(Mode_t sequence) { + for (FixedMap::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::Iterator iter = modeSequences.begin(); + iter != modeSequences.end(); iter++) { + if (iter->fallbackSequence == SequenceId) { + return true; + } + } + return false; +} + +bool Subsystem::isTableUsed(Mode_t tableId) { + for (FixedMap::Iterator sequence = + modeSequences.begin(); sequence != modeSequences.end(); + sequence++) { + HybridIterator 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::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); +} diff --git a/subsystem/Subsystem.h b/subsystem/Subsystem.h new file mode 100644 index 00000000..5e924a39 --- /dev/null +++ b/subsystem/Subsystem.h @@ -0,0 +1,169 @@ +/* + * Subsystem.h + * + * Created on: 12.07.2013 + * Author: tod + */ + +#ifndef SUBSYSTEM_H_ +#define SUBSYSTEM_H_ + +#include +#include +#include +#include +#include +#include +#include + +class Subsystem: public SubsystemBase, public HasModeSequenceIF { +public: + static const uint8_t INTERFACE_ID = SUBSYSTEM; + static const ReturnValue_t SEQUENCE_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t TABLE_ALREADY_EXISTS = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t TABLE_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x03); + static const ReturnValue_t TABLE_OR_SEQUENCE_LENGTH_INVALID = MAKE_RETURN_CODE(0x04); + static const ReturnValue_t SEQUENCE_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x05); + static const ReturnValue_t TABLE_CONTAINS_INVALID_OBJECT_ID = + MAKE_RETURN_CODE(0x06); + static const ReturnValue_t FALLBACK_SEQUENCE_DOES_NOT_EXIST = + MAKE_RETURN_CODE(0x07); + static const ReturnValue_t NO_TARGET_TABLE = MAKE_RETURN_CODE(0x08); + static const ReturnValue_t SEQUENCE_OR_TABLE_TOO_LONG = MAKE_RETURN_CODE(0x09); + static const ReturnValue_t IS_FALLBACK_SEQUENCE = MAKE_RETURN_CODE(0x0B); + static const ReturnValue_t ACCESS_DENIED = MAKE_RETURN_CODE(0x0C); + static const ReturnValue_t TABLE_IN_USE = MAKE_RETURN_CODE(0x0E); + + static const ReturnValue_t TARGET_TABLE_NOT_REACHED = MAKE_RETURN_CODE(0xA1); + static const ReturnValue_t TABLE_CHECK_FAILED = MAKE_RETURN_CODE(0xA2); + + + + Subsystem(object_id_t setObjectId, object_id_t parent, + uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables); + virtual ~Subsystem(); + + ReturnValue_t addSequence(ArrayList* sequence, Mode_t id, + Mode_t fallbackSequence, bool inStore = true, bool preInit = true); + + ReturnValue_t addTable(ArrayList *table, Mode_t id, + bool inStore = true, bool preInit = true); + + void setInitialMode(Mode_t mode); + + virtual ReturnValue_t initialize(); + + virtual ReturnValue_t checkObjectConnections(); + + virtual MessageQueueId_t getSequenceCommandQueue() const; + + /** + * + * + * IMPORTANT: Do not call on non existing sequence! Use existsSequence() first + * + * @param sequence + * @return + */ + ReturnValue_t checkSequence(Mode_t sequence); + + /** + * + * + * IMPORTANT: Do not call on non existing sequence! Use existsSequence() first + * + * @param iter + * @return + */ + ReturnValue_t checkSequence(HybridIterator iter, Mode_t fallbackSequence); +protected: + + struct EntryPointer { + bool islinked; + union { + ModeListEntry *firstLinkedElement; + ArrayList *array; + }; + }; + + struct SequenceInfo { + Mode_t fallbackSequence; + EntryPointer entries; + }; + + static const uint8_t MAX_NUMBER_OF_TABLES_OR_SEQUENCES = 60; + + static const uint8_t MAX_LENGTH_OF_TABLE_OR_SEQUENCE = 20; + + bool isInTransition; + + bool childrenChangedHealth; + + uint32_t uptimeStartTable; + + HybridIterator currentTargetTable; + + Mode_t targetMode; + + Submode_t targetSubmode; + + Mode_t initialMode; + + HybridIterator currentSequenceIterator; + + FixedMap modeTables; + + FixedMap modeSequences; + + StorageManagerIF *IPCStore; + + ModeStoreIF *modeStore; + + bool existsModeSequence(Mode_t id); + + HybridIterator getSequence(Mode_t id); + + bool existsModeTable(Mode_t id); + + HybridIterator getTable(Mode_t id); + + HybridIterator getCurrentTable(); + +// void startSequence(Mode_t sequence); + + /** + * DO NOT USE ON NON EXISTING SEQUENCE + * + * @param a sequence + * @return the fallback sequence's Id + */ + Mode_t getFallbackSequence(Mode_t sequence); + + void replyToCommand(ReturnValue_t status, uint32_t parameter); + + ReturnValue_t deleteSequence(Mode_t id); + + ReturnValue_t deleteTable(Mode_t id); + + virtual void performChildOperation(); + + virtual ReturnValue_t handleCommandMessage(CommandMessage *message); + + bool isFallbackSequence(Mode_t SequenceId); + + bool isTableUsed(Mode_t tableId); + + virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode); + + virtual void startTransition(Mode_t mode, Submode_t submode); + + void sendSerializablesAsCommandMessage(Command_t command, SerializeIF **elements, uint8_t count); + + void transitionFailed(ReturnValue_t failureCode, uint32_t parameter); + + void cantKeepMode(); + +}; + +#endif /* SUBSYSTEM_H_ */ diff --git a/subsystem/SubsystemBase.cpp b/subsystem/SubsystemBase.cpp new file mode 100644 index 00000000..03c3d081 --- /dev/null +++ b/subsystem/SubsystemBase.cpp @@ -0,0 +1,336 @@ +#include +#include +#include + +SubsystemBase::~SubsystemBase() { + +} + +ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) { + ChildInfo info; + + HasModesIF *child = objectManager->get(objectId); + //This is a rather ugly hack to have the changedHealth info for all children available. (needed for FOGs). + HasHealthIF* healthChild = objectManager->get(objectId); + if (child == NULL) { + if (healthChild == NULL) { + 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; + + std::pair::iterator, bool> returnValue = + childrenMap.insert( + std::pair(objectId, info)); + if (!(returnValue.second)) { + return COULD_NOT_INSERT_CHILD; + } else { + return RETURN_OK; + } +} + +ReturnValue_t SubsystemBase::checkStateAgainstTable( + HybridIterator tableIter) { + + std::map::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; + } + if (childIter->second.submode != tableIter.value->getSubmode()) { + return RETURN_FAILED; + } + } + return RETURN_OK; +} + +void SubsystemBase::executeTable(HybridIterator tableIter) { + + CommandMessage message; + + std::map::iterator iter; + + commandsOutstanding = 0; + + for (; tableIter.value != NULL; ++tableIter) { + object_id_t object = tableIter.value->getObject(); + if ((iter = childrenMap.find(object)) == childrenMap.end()) { + //illegal table entry + //TODO: software error + continue; + } + + if (healthHelper.healthTable->hasHealth(object)) { + if (healthHelper.healthTable->isFaulty(object)) { + ModeMessage::setModeMessage(&message, + ModeMessage::CMD_MODE_COMMAND, HasModesIF::MODE_OFF, + SUBMODE_NONE); + } else { + if (modeHelper.isForced()) { + ModeMessage::setModeMessage(&message, + ModeMessage::CMD_MODE_COMMAND_FORCED, + tableIter.value->getMode(), + tableIter.value->getSubmode()); + } else { + if (healthHelper.healthTable->isCommandable(object)) { + ModeMessage::setModeMessage(&message, + ModeMessage::CMD_MODE_COMMAND, + tableIter.value->getMode(), + tableIter.value->getSubmode()); + } else { + continue; + } + } + } + } else { + ModeMessage::setModeMessage(&message, ModeMessage::CMD_MODE_COMMAND, + tableIter.value->getMode(), tableIter.value->getSubmode()); + } + //TODO: This may causes trouble with more than two layers, sys commands subsys off, which is already off, but children are on (external). + // So, they stay on. Might only be an issue if mode is forced, so we do + if ((iter->second.mode == ModeMessage::getMode(&message)) + && (iter->second.submode == ModeMessage::getSubmode(&message)) && !modeHelper.isForced()) { + continue; //don't send redundant mode commands (produces event spam) + } + ReturnValue_t result = commandQueue.sendMessage( + iter->second.commandQueue, &message); + if (result != RETURN_OK) { + //TODO OBSW internal error + } + ++commandsOutstanding; + } + +} + +ReturnValue_t SubsystemBase::updateChildMode(MessageQueueId_t queue, + Mode_t mode, Submode_t submode) { + std::map::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; +} + +SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, + Mode_t initialMode, uint16_t commandQueueDepth) : + SystemObject(setObjectId), mode(initialMode), submode(SUBMODE_NONE), childrenChangedMode( + false), commandsOutstanding(0), commandQueue(commandQueueDepth, + CommandMessage::MAX_MESSAGE_SIZE), healthHelper(this, setObjectId), modeHelper(this), parentId(parent) { +} + +MessageQueueId_t SubsystemBase::getCommandQueue() const { + return commandQueue.getId(); +} + +ReturnValue_t SubsystemBase::initialize() { + MessageQueueId_t parentQueue = 0; + ReturnValue_t result = SystemObject::initialize(); + + if (result != RETURN_OK) { + return result; + } + + if (parentId != 0) { + SubsystemBase *parent = objectManager->get(parentId); + if (parent == NULL) { + 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() { + + 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 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; + announceMode(false); +} + +void SubsystemBase::setMode(Mode_t newMode) { + setMode(newMode, submode); +} + +void SubsystemBase::commandAllChildren(CommandMessage* message) { + std::map::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 message; + + for (result = commandQueue.receiveMessage(&message); result == RETURN_OK; + result = commandQueue.receiveMessage(&message)) { + + result = healthHelper.handleHealthCommand(&message); + if (result == RETURN_OK) { + continue; + } + + result = modeHelper.handleModeCommand(&message); + if (result == RETURN_OK) { + continue; + } + + result = handleModeReply(&message); + if (result == RETURN_OK) { + continue; + } + + result = handleCommandMessage(&message); + if (result != RETURN_OK) { + CommandMessage reply; + reply.setReplyRejected(CommandMessage::UNKNOW_COMMAND, message.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(); +} diff --git a/subsystem/SubsystemBase.h b/subsystem/SubsystemBase.h new file mode 100644 index 00000000..e44a9e88 --- /dev/null +++ b/subsystem/SubsystemBase.h @@ -0,0 +1,116 @@ +#ifndef SUBSYSTEMBASE_H_ +#define SUBSYSTEMBASE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +class SubsystemBase: public SystemObject, + public HasModesIF, + public HasHealthIF, + public HasReturnvaluesIF, + public ExecutableObjectIF { +public: + static const uint8_t INTERFACE_ID = SUBSYSTEM_BASE; + static const ReturnValue_t CHILD_NOT_FOUND = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t CHILD_INFO_UPDATED = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t CHILD_DOESNT_HAVE_MODES = MAKE_RETURN_CODE(0x03); + static const ReturnValue_t COULD_NOT_INSERT_CHILD = MAKE_RETURN_CODE(0x04); + static const ReturnValue_t TABLE_CONTAINS_INVALID_OBJECT_ID = + MAKE_RETURN_CODE(0x05); + + SubsystemBase(object_id_t setObjectId, object_id_t parent, + Mode_t initialMode = 0, uint16_t commandQueueDepth = 8); + virtual ~SubsystemBase(); + + virtual MessageQueueId_t getCommandQueue() const; + + ReturnValue_t registerChild(object_id_t objectId); + + virtual ReturnValue_t initialize(); + + virtual ReturnValue_t performOperation(); + + virtual ReturnValue_t setHealth(HealthState health); + + virtual HasHealthIF::HealthState getHealth(); + +protected: + struct ChildInfo { + MessageQueueId_t commandQueue; + Mode_t mode; + Submode_t submode; + bool healthChanged; + }; + + Mode_t mode; + + Submode_t submode; + + bool childrenChangedMode; + + /** + * Always check this against <=0, so you are robust against too many replies + */ + int32_t commandsOutstanding; + + MessageQueue commandQueue; + + HealthHelper healthHelper; + + ModeHelper modeHelper; + + const object_id_t parentId; + + typedef std::map ChildrenMap; + ChildrenMap childrenMap; + + void checkCommandQueue(); + + ReturnValue_t checkStateAgainstTable( + HybridIterator tableIter); + + void executeTable(HybridIterator tableIter); + + ReturnValue_t updateChildMode(MessageQueueId_t queue, Mode_t mode, + Submode_t submode); + + ReturnValue_t updateChildChangedHealth(MessageQueueId_t queue, bool changedHealth = true); + + virtual ReturnValue_t handleModeReply(CommandMessage *message); + + void commandAllChildren(CommandMessage *message); + + ReturnValue_t checkTable(HybridIterator tableIter); + + void replyToCommand(CommandMessage *message); + + void setMode(Mode_t newMode, Submode_t newSubmode); + + void setMode(Mode_t newMode); + + virtual ReturnValue_t handleCommandMessage(CommandMessage *message) = 0; + + virtual void performChildOperation() = 0; + + virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode) = 0; + + virtual void startTransition(Mode_t mode, Submode_t submode) = 0; + + virtual void getMode(Mode_t *mode, Submode_t *submode); + + virtual void setToExternalControl(); + + virtual void announceMode(bool recursive); +}; + +#endif /* SUBSYSTEMBASE_H_ */ diff --git a/subsystem/modes/HasModeSequenceIF.h b/subsystem/modes/HasModeSequenceIF.h new file mode 100644 index 00000000..e03de410 --- /dev/null +++ b/subsystem/modes/HasModeSequenceIF.h @@ -0,0 +1,20 @@ +#ifndef HASMODESEQUENCEIF_H_ +#define HASMODESEQUENCEIF_H_ + +#include +#include +#include + + +class HasModeSequenceIF { +public: + virtual ~HasModeSequenceIF() { + + } + + virtual MessageQueueId_t getSequenceCommandQueue() const = 0; + +}; + + +#endif /* HASMODESEQUENCEIF_H_ */ diff --git a/subsystem/modes/ModeDefinitions.h b/subsystem/modes/ModeDefinitions.h new file mode 100644 index 00000000..4a122ef2 --- /dev/null +++ b/subsystem/modes/ModeDefinitions.h @@ -0,0 +1,134 @@ +/* + * ModeTable.h + * + * Created on: 12.07.2013 + * Author: tod + */ + +#ifndef MODEDEFINITIONS_H_ +#define MODEDEFINITIONS_H_ + +#include +#include +#include +#include +class ModeListEntry: public SerializeIF, public LinkedElement { +public: + ModeListEntry() : + LinkedElement(this), value1(0), value2(0), value3(0) { + + } + + uint32_t value1; + uint32_t value2; + uint8_t value3; + + virtual ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + + ReturnValue_t result; + + result = SerializeAdapter::serialize(&value1, buffer, size, + max_size, bigEndian); + + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = SerializeAdapter::serialize(&value2, buffer, size, + max_size, bigEndian); + + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = SerializeAdapter::serialize(&value3, buffer, size, + max_size, bigEndian); + + return result; + + } + + virtual uint32_t getSerializedSize() const { + return sizeof(value1) + sizeof(value2) + sizeof(value3); + } + + virtual ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + ReturnValue_t result; + + result = SerializeAdapter::deSerialize(&value1, buffer, size, + bigEndian); + + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = SerializeAdapter::deSerialize(&value2, buffer, size, + bigEndian); + + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = SerializeAdapter::deSerialize(&value3, buffer, size, + bigEndian); + + return result; + } + + //for Sequences + Mode_t getTableId() const { + return value1; + } + + void setTableId(Mode_t tableId) { + this->value1 = tableId; + } + + uint8_t getWaitSeconds() const { + return value2; + } + + void setWaitSeconds(uint8_t waitSeconds) { + this->value2 = waitSeconds; + } + + bool checkSuccess() const { + return value3 == 1; + } + + void setCheckSuccess(bool checkSuccess) { + this->value3 = checkSuccess; + } + + + //for Tables + object_id_t getObject() const { + return value1; + } + + void setObject(object_id_t object) { + this->value1 = object; + } + + //TODO no cast! + Mode_t getMode() const { + return value2; + } + + void setMode(Mode_t mode) { + this->value2 = mode; + } + + Submode_t getSubmode() const { + return value3; + } + + void setSubmode(Submode_t submode) { + this->value3 = submode; + } + + bool operator==(ModeListEntry other) { + return ((value1 == other.value1) && (value2 == other.value2) + && (value3 == other.value3)); + } +}; + +#endif //MODEDEFINITIONS_H_ diff --git a/subsystem/modes/ModeSequenceMessage.cpp b/subsystem/modes/ModeSequenceMessage.cpp new file mode 100644 index 00000000..6b6f9eb5 --- /dev/null +++ b/subsystem/modes/ModeSequenceMessage.cpp @@ -0,0 +1,89 @@ +#include +#include +#include +#include +#include + +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, ModeTableId_t table, store_address_t storeAddress) { +// message->setCommand(command); +// message->setParameter(storeAddress.raw); +// message->setParameter2(table); +//} + +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, ModeTableId_t table) { +// message->setCommand(command); +// message->setParameter2(table); +//} + +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->get(objects::IPC_STORE); + if (ipcStore != NULL){ + ipcStore->deleteData(ModeSequenceMessage::getStoreAddress(message)); + } + } + /*NO BREAK*/ + 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; + } +} diff --git a/subsystem/modes/ModeSequenceMessage.h b/subsystem/modes/ModeSequenceMessage.h new file mode 100644 index 00000000..f76bbc9a --- /dev/null +++ b/subsystem/modes/ModeSequenceMessage.h @@ -0,0 +1,48 @@ +#ifndef MODESEQUENCEMESSAGE_H_ +#define MODESEQUENCEMESSAGE_H_ + +#include +#include +#include + +class ModeSequenceMessage { +public: + static const uint8_t MESSAGE_ID = MODE_SEQUENCE_MESSAGE_ID; + + static const Command_t ADD_SEQUENCE = MAKE_COMMAND_ID(0x01); + static const Command_t ADD_TABLE = MAKE_COMMAND_ID(0x02); + static const Command_t DELETE_SEQUENCE = MAKE_COMMAND_ID(0x03); + static const Command_t DELETE_TABLE = MAKE_COMMAND_ID(0x04); + static const Command_t READ_SEQUENCE = MAKE_COMMAND_ID(0x05); + static const Command_t READ_TABLE = MAKE_COMMAND_ID(0x06); + static const Command_t LIST_SEQUENCES = MAKE_COMMAND_ID(0x07); + static const Command_t LIST_TABLES = MAKE_COMMAND_ID(0x08); + static const Command_t SEQUENCE_LIST = MAKE_COMMAND_ID(0x09); + static const Command_t TABLE_LIST = MAKE_COMMAND_ID(0x0A); + static const Command_t TABLE = MAKE_COMMAND_ID(0x0B); + static const Command_t SEQUENCE = MAKE_COMMAND_ID(0x0C); + static const Command_t READ_FREE_SEQUENCE_SLOTS = MAKE_COMMAND_ID(0x0D); + static const Command_t FREE_SEQUENCE_SLOTS = MAKE_COMMAND_ID(0x0E); + static const Command_t READ_FREE_TABLE_SLOTS = MAKE_COMMAND_ID(0x0F); + static const Command_t FREE_TABLE_SLOTS = MAKE_COMMAND_ID(0x10); + + static void setModeSequenceMessage(CommandMessage *message, + Command_t command, Mode_t sequenceOrTable, + store_address_t storeAddress); + static void setModeSequenceMessage(CommandMessage *message, + Command_t command, Mode_t sequenceOrTable); + static void setModeSequenceMessage(CommandMessage *message, + Command_t command, store_address_t storeAddress); + + static store_address_t getStoreAddress(const CommandMessage *message); + static Mode_t getSequenceId(const CommandMessage *message); + static Mode_t getTableId(const CommandMessage *message); + static uint32_t getNumber(const CommandMessage *message); + + static void clear(CommandMessage *message); + +private: + ModeSequenceMessage(); +}; + +#endif /* MODESEQUENCEMESSAGE_H_ */ diff --git a/subsystem/modes/ModeStore.cpp b/subsystem/modes/ModeStore.cpp new file mode 100644 index 00000000..017b7202 --- /dev/null +++ b/subsystem/modes/ModeStore.cpp @@ -0,0 +1,122 @@ +#include + +ModeStore::ModeStore(object_id_t objectId, uint32_t slots) : + SystemObject(objectId), store(slots), emptySlot(store.front()) { + mutex = new MutexId_t; + 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::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* 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::Iterator iter; + for (iter = sequence->begin(); iter != sequence->end(); ++iter) { + //TODO: 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* 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::Iterator iter; + for (iter = store.begin(); iter != store.end(); ++iter) { + iter->setNext(emptySlot); + } + OSAL::unlockMutex(mutex); +} + +ModeListEntry* ModeStore::findEmptySlotNoLock(ModeListEntry* startFrom) { + ArrayList::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; +} diff --git a/subsystem/modes/ModeStore.h b/subsystem/modes/ModeStore.h new file mode 100644 index 00000000..8295fd8c --- /dev/null +++ b/subsystem/modes/ModeStore.h @@ -0,0 +1,41 @@ +#ifndef MODESTORE_H_ +#define MODESTORE_H_ + +#include +#include +#include +#include + +class ModeStore: public ModeStoreIF, public SystemObject { +public: + ModeStore(object_id_t objectId, uint32_t slots); + virtual ~ModeStore(); + + virtual ReturnValue_t storeArray(ArrayList *sequence, + ModeListEntry **storedFirstEntry); + + virtual ReturnValue_t deleteList( + ModeListEntry *sequence); + + virtual ReturnValue_t readList( + ModeListEntry *sequence, + ArrayList *into); + + virtual uint32_t getFreeSlots(); + +private: + MutexId_t* mutex; + ArrayList store; + ModeListEntry *emptySlot; + + void clear(); + ModeListEntry* findEmptySlotNoLock( + ModeListEntry* startFrom); + void deleteListNoLock( + ModeListEntry *sequence); + + ReturnValue_t isValidEntry(ModeListEntry *sequence); +}; + +#endif /* MODESTORE_H_ */ + diff --git a/subsystem/modes/ModeStoreIF.h b/subsystem/modes/ModeStoreIF.h new file mode 100644 index 00000000..3d8efbc8 --- /dev/null +++ b/subsystem/modes/ModeStoreIF.h @@ -0,0 +1,33 @@ +#ifndef MODESTOREIF_H_ +#define MODESTOREIF_H_ + +#include +#include +#include +#include + +class ModeStoreIF { +public: + static const uint8_t INTERFACE_ID = MODE_STORE_IF; + static const ReturnValue_t INVALID_ENTRY = MAKE_RETURN_CODE(0x02); + static const ReturnValue_t TOO_MANY_ELEMENTS = MAKE_RETURN_CODE(0x03); + static const ReturnValue_t CANT_STORE_EMPTY = MAKE_RETURN_CODE(0x04); + + virtual ~ModeStoreIF() { + + } + + virtual ReturnValue_t storeArray(ArrayList *sequence, + ModeListEntry **storedFirstEntry) = 0; + + virtual ReturnValue_t deleteList( + ModeListEntry *sequence) = 0; + + virtual ReturnValue_t readList( + ModeListEntry *sequence, + ArrayList *into) = 0; + + virtual uint32_t getFreeSlots() = 0; +}; + +#endif /* MODESTOREIF_H_ */ diff --git a/tasks/ExecutableObjectIF.h b/tasks/ExecutableObjectIF.h new file mode 100644 index 00000000..9ad4fa8d --- /dev/null +++ b/tasks/ExecutableObjectIF.h @@ -0,0 +1,37 @@ +/** + * @file ExecutableObjectIF.h + * + * @brief This file contains the definition for the ExecutableObjectIF interface. + * + * @author Bastian Baetz + * + * @date 12.03.2012 + */ + +#ifndef EXECUTABLEOBJECTIF_H_ +#define EXECUTABLEOBJECTIF_H_ + + +#include + +/** + * @brief The interface provides a method to execute objects within a task. + * @details The performOperation method, that is required by the interface is + * executed cyclically within the ObjectTask's context. + */ +class ExecutableObjectIF { +public: + /** + * @brief This is the empty virtual destructor as required for C++ interfaces. + */ + virtual ~ExecutableObjectIF() { } + /** + * @brief The performOperation method is executed in the ObjectTask context. + * @details There are no restrictions for calls within this method, so any + * other member of the class can be used. + * @return Currently, the return value is ignored. + */ + virtual ReturnValue_t performOperation() = 0; +}; + +#endif /* EXECUTABLEOBJECTIF_H_ */ diff --git a/tasks/Makefile b/tasks/Makefile new file mode 100755 index 00000000..bafd59a9 --- /dev/null +++ b/tasks/Makefile @@ -0,0 +1,24 @@ +#!/bin/bash +# +# OSAL makefile +# +# Created on: Mar 04, 2010 +# Author: ziemke +# Author: Claas Ziemke +# Copyright 2010, Claas Ziemke +# + +BASEDIR=../../ +include $(BASEDIR)options.mk + +OBJ = $(BUILDDIR)/PeriodicTask.o \ + $(BUILDDIR)/ObjectTask.o \ + $(BUILDDIR)/TaskBase.o + +all: $(OBJ) + +$(BUILDDIR)/%.o: %.cpp %.h + $(CPP) $(CFLAGS) $(DEFINES) $(CCOPT) ${INCLUDE} -c $< -o $@ + +clean: + $(RM) *.o *.gcno *.gcda diff --git a/tasks/MultiObjectTask.cpp b/tasks/MultiObjectTask.cpp new file mode 100644 index 00000000..65fecaab --- /dev/null +++ b/tasks/MultiObjectTask.cpp @@ -0,0 +1,99 @@ +/** + * @file MultiObjectTask.cpp + * @brief This file defines the MultiObjectTask class. + * @date 30.01.2014 + * @author baetz + */ + +#include +#include + +MultiObjectTask::MultiObjectTask(const char *name, TaskPriority_t setPriority, + size_t setStack, Interval_t setPeriod, void (*setDeadlineMissedFunc)()) : + TaskBase(setPriority, setStack, name), period(setPeriod), periodId(0), deadlineMissedFunc( + setDeadlineMissedFunc) { + +} + +MultiObjectTask::~MultiObjectTask(void) { + //Do not delete objects! +} +TaskReturn_t MultiObjectTask::taskEntryPoint(TaskArgument_t argument) { + //The argument is re-interpreted as ObjectTask. The Task object is global, so it is found from any place. + MultiObjectTask *originalTask(reinterpret_cast(argument)); + originalTask->taskFunctionality(); + debug << "MultiObjectTask " << originalTask->getId() + << " returned from taskFunctionality. Deleting task." << std::endl; +} + +ReturnValue_t MultiObjectTask::startTask() { + this->setRunning( true); + ReturnValue_t status; + status = OSAL::startTask(&(this->id), MultiObjectTask::taskEntryPoint, + TaskArgument_t((void *) this)); + if (status != RETURN_OK) { + //TODO: Call any FDIR routine? + error << "MultiObjectTask::startTask for " << std::hex << this->getId() + << std::dec << " failed." << std::endl; + } else { +// debug << "ObjectTask::startTask for " << std::hex << this->getId() << std::dec << " successful" << std::endl; + } + return status; +} + +void MultiObjectTask::taskFunctionality() { + ReturnValue_t status = OSAL::TIMEOUT; + //The period is set up and started with the system call. + if (objectList.begin() != objectList.end()) { + status = OSAL::setAndStartPeriod(this->period, &(this->periodId)); + if (status == RETURN_OK) { + //The task's "infinite" inner loop is entered. + while (this->isRunning) { + for (ObjectList::iterator it = objectList.begin(); + it != objectList.end(); ++it) { + (*it)->performOperation(); + } + if (OSAL::checkAndRestartPeriod(this->periodId, this->period) + == OSAL::TIMEOUT) { + char nameSpace[8] = { 0 }; + char* ptr = rtems_object_get_name(getId(), + sizeof(nameSpace), nameSpace); + error << "MultiObjectTask: " << ptr << " Deadline missed." + << std::endl; + if (this->deadlineMissedFunc != NULL) { + this->deadlineMissedFunc(); + } + } + } + debug << "MultiObjectTask: Returned from taskFunctionality()-Loop." + << std::endl; + } else { + error << "MultiObjectTask::setAndStartPeriod failed with status " + << status << std::endl; + } + } else { + error << "MultiObjectTask::taskFunctionality. No object assigned." + << std::endl; + } + //Any operating system object for periodic execution is deleted. + debug << "Deleting the ObjectTask's period." << std::endl; + OSAL::deletePeriod(&(this->id)); +} + +ReturnValue_t MultiObjectTask::addObject(object_id_t object) { + ExecutableObjectIF* newObject = objectManager->get( + object); + if (newObject == NULL) { + return RETURN_FAILED; + } + objectList.push_back(newObject); + return RETURN_OK; +} + +ReturnValue_t MultiObjectTask::addObject(ExecutableObjectIF* object) { + if (object == NULL) { + return RETURN_FAILED; + } + objectList.push_back(object); + return RETURN_OK; +} diff --git a/tasks/MultiObjectTask.h b/tasks/MultiObjectTask.h new file mode 100644 index 00000000..d785cefa --- /dev/null +++ b/tasks/MultiObjectTask.h @@ -0,0 +1,113 @@ +/** + * @file MultiObjectTask.h + * @brief This file defines the MultiObjectTask class. + * @date 30.01.2014 + * @author baetz + */ +#ifndef MULTIOBJECTTASK_H_ +#define MULTIOBJECTTASK_H_ + +#include +#include +#include +#include +/** + * @brief This class represents a specialized task for periodic activities of multiple objects. + * + * @details MultiObjectTask is an extension to ObjectTask in the way that it is able to execute + * multiple objects that implement the ExecutableObjectIF interface. The objects must be + * added prior to starting the task. + * + * @ingroup task_handling + */ +class MultiObjectTask: public TaskBase { +protected: + typedef std::list ObjectList; //!< Typedef for the List of objects. + /** + * @brief This attribute holds a list of objects to be executed. + */ + ObjectList objectList; + /** + * @brief The period of the task. + * @details The period determines the frequency of the task's execution. It is expressed in clock ticks. + */ + Interval_t period; + /** + * @brief id of the associated OS period + */ + PeriodId_t periodId; + /** + * @brief The pointer to the deadline-missed function. + * @details This pointer stores the function that is executed if the task's deadline is missed. + * So, each may react individually on a timing failure. The pointer may be NULL, + * then nothing happens on missing the deadline. The deadline is equal to the next execution + * of the periodic task. + */ + void (*deadlineMissedFunc)(void); + /** + * @brief This is the function executed in the new task's context. + * @details It converts the argument back to the thread object type and copies the class instance + * to the task context. The taskFunctionality method is called afterwards. + * @param A pointer to the task object itself is passed as argument. + */ + static TaskReturn_t taskEntryPoint(TaskArgument_t argument); + /** + * @brief The function containing the actual functionality of the task. + * @details The method sets and starts + * the task's period, then enters a loop that is repeated as long as the isRunning + * attribute is true. Within the loop, all performOperation methods of the added + * objects are called. Afterwards the checkAndRestartPeriod system call blocks the task + * until the next period. + * On missing the deadline, the deadlineMissedFunction is executed. + */ + void taskFunctionality(void); + +public: + /** + * @brief Standard constructor of the class. + * @details The class is initialized without allocated objects. These need to be added + * with #addObject. + * In the underlying TaskBase class, a new operating system task is created. + * In addition to the TaskBase parameters, the period, the pointer to the + * aforementioned initialization function and an optional "deadline-missed" + * function pointer is passed. + * @param priority Sets the priority of a task. Values range from a low 0 to a high 99. + * @param stack_size The stack size reserved by the operating system for the task. + * @param setPeriod The length of the period with which the task's functionality will be + * executed. It is expressed in clock ticks. + * @param setDeadlineMissedFunc The function pointer to the deadline missed function + * that shall be assigned. + */ + MultiObjectTask(const char *name, TaskPriority_t setPriority, size_t setStack, Interval_t setPeriod, + void (*setDeadlineMissedFunc)()); + /** + * @brief Currently, the executed object's lifetime is not coupled with the task object's + * lifetime, so the destructor is empty. + */ + virtual ~MultiObjectTask(void); + + /** + * @brief The method to start the task. + * @details The method starts the task with the respective system call. + * Entry point is the taskEntryPoint method described below. + * The address of the task object is passed as an argument + * to the system call. + */ + ReturnValue_t startTask(void); + /** + * Adds an object to the list of objects to be executed. + * The objects are executed in the order added. + * @param object Id of the object to add. + * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. + */ + ReturnValue_t addObject(object_id_t object); + /** + * Variant to add objects with known location directly. + * @param object Reference to a persistant executable object. + * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. + */ + ReturnValue_t addObject(ExecutableObjectIF* object); + +}; + +#endif /* MULTIOBJECTTASK_H_ */ diff --git a/tasks/ObjectTask.cpp b/tasks/ObjectTask.cpp new file mode 100644 index 00000000..83d28745 --- /dev/null +++ b/tasks/ObjectTask.cpp @@ -0,0 +1,78 @@ +/* + * ObjectTask.cpp + * + * Created on: 03.02.2012 + * Author: baetz + */ + +#include +#include + +ObjectTask::ObjectTask( const char *name, TaskPriority_t setPriority, size_t setStack, Interval_t setPeriod, void (*setDeadlineMissedFunc)(), ExecutableObjectIF* (*initFunction)() ) : + TaskBase( setPriority, setStack, name ), period(setPeriod), periodId(0), deadlineMissedFunc(setDeadlineMissedFunc) { + // All additional attributes are applied to the object. + this->executingObject = initFunction(); + +} + +ObjectTask::ObjectTask(const char *name, TaskPriority_t setPriority, size_t setStack, + Interval_t setPeriod, void (*setDeadlineMissedFunc)(), + object_id_t object_id) : TaskBase( setPriority, setStack, name ), period(setPeriod), periodId(0), deadlineMissedFunc(setDeadlineMissedFunc) { + this->executingObject = objectManager->get( object_id ); +} + +ObjectTask::~ObjectTask() { +} + +TaskReturn_t ObjectTask::taskEntryPoint( TaskArgument_t argument ) { + //The argument is re-interpreted as ObjectTask. The Task object is global, so it is found from any place. + ObjectTask *originalTask( reinterpret_cast( argument ) ); + originalTask->taskFunctionality(); + debug << "Object task " << originalTask->getId() << " returned from taskFunctionality. Deleting task." << std::endl; + //TODO: destroy the task object? +} + +ReturnValue_t ObjectTask::startTask() { + this->setRunning( true ); + ReturnValue_t status; + status = OSAL::startTask( &( this->id ), ObjectTask::taskEntryPoint, TaskArgument_t( ( void *)this ) ); + if( status != RETURN_OK ) { + //TODO: Call any FDIR routine? + error << "ObjectTask::startTask for " << std::hex << this->getId() << std::dec << " failed." << std::endl; + } else { +// debug << "ObjectTask::startTask for " << std::hex << this->getId() << std::dec << " successful" << std::endl; + } + return status; +} + +void ObjectTask::taskFunctionality() { + ReturnValue_t status = OSAL::TIMEOUT; + //The period is set up and started with the system call. + if ( this->executingObject != NULL ) { + status = OSAL::setAndStartPeriod( this->period, &(this->periodId) ); + if( status == RETURN_OK ) { + //The task's "infinite" inner loop is entered. + while( this->isRunning ) { + + this->executingObject->performOperation(); + + if( OSAL::checkAndRestartPeriod( this->periodId, this->period ) == OSAL::TIMEOUT ) { + char nameSpace[8] = {0}; + char* ptr = rtems_object_get_name(getId(), sizeof(nameSpace), nameSpace); + error << "ObjectTask: " << ptr << " Deadline missed." << std::endl; + if( this->deadlineMissedFunc != NULL ) { + this->deadlineMissedFunc(); + } + } + } + debug << "Returned from taskFunctionality()-Loop." << std::endl; + } else { + error << "ObjectTask::setAndStartPeriod failed with status " << status << std::endl; + } + } else { + error << "ObjectTask::taskFunctionality. Object not found." << std::endl; + } + //Any operating system object for periodic execution is deleted. + debug << "Deleting the ObjectTask's period." << std::endl; + OSAL::deletePeriod( &(this->id) ); +} diff --git a/tasks/ObjectTask.h b/tasks/ObjectTask.h new file mode 100644 index 00000000..eab8d30c --- /dev/null +++ b/tasks/ObjectTask.h @@ -0,0 +1,129 @@ + +/** + * @file ObjectTask.h + * + * @brief This file contains the definition for the ObjectTask class. + * + * @author Bastian Baetz + * + * @date 02/03/2012 + * + */ +#ifndef OBJECTTASK_H_ +#define OBJECTTASK_H_ + +#include +#include +#include + +/** + * @brief This class represents a specialized task for periodic activities of objects. + * + * @details For object-oriented embedded systems, simple periodic tasks executing a single function + * are not very useful. An object method, representing some kind of activity to be performed, + * would either be instantiated in each execution, thus loosing all attribute information, or + * must be known globally. To address this issue, the ObjectTask class is used. + * It implements the TaskBase prototype and provides additional means to make use of any + * object that implements the ExecutableObjectIF interface. The required performOperation + * method is then called periodically. Attributes of the executed object are persistent + * during the task's lifetime. + * Functionally, the class works similar to PeriodicTask. + * + * @author Bastian Baetz + * + * @date 02/03/2012 + * + * @ingroup task_handling + */ +class ObjectTask: public TaskBase { +protected: + /** + * @brief This attribute holds a pointer to the object to be executed. + */ + ExecutableObjectIF* executingObject; + /** + * @brief The period of the task. + * @details The period determines the frequency of the task's execution. It is expressed in clock ticks. + */ + Interval_t period; + /** + * @brief id of the associated OS period + */ + PeriodId_t periodId; + /** + * @brief The pointer to the deadline-missed function. + * @details This pointer stores the function that is executed if the task's deadline is missed. + * So, each may react individually on a timing failure. The pointer may be NULL, + * then nothing happens on missing the deadline. The deadline is equal to the next execution + * of the periodic task. + */ + void ( *deadlineMissedFunc )( void ); + /** + * @brief This is the function executed in the new task's context. + * @details It converts the argument back to the thread object type and copies the class instance + * to the task context. The taskFunctionality method is called afterwards. + * @param A pointer to the task object itself is passed as argument. + */ + static TaskReturn_t taskEntryPoint( TaskArgument_t argument ); + /** + * @brief The function containing the actual functionality of the task. + * @details The method sets and starts + * the task's period, then enters a loop that is repeated as long as the isRunning + * attribute is true. Within the loop, the object's performFunction method is called, and + * afterwards the checkAndRestartPeriod system call to block the task until the next + * period. On missing the deadline, the deadlineMissedFunction is executed. + */ + void taskFunctionality( void ); + +public: + /** + * @brief The first of the two standard constructors of the class. + * @details This constructor initializes the object to be executed with the aid of an + * initialization function that returns the pointer to the object. + * In the underlying TaskBase class, a new operating system task is created. + * In addition to the TaskBase parameters, the period, the pointer to the + * aforementioned initialization function and an optional "deadline-missed" + * function pointer is passed. + * @param priority Sets the priority of a task. Values range from a low 0 to a high 99. + * @param stack_size The stack size reserved by the operating system for the task. + * @param setPeriod The length of the period with which the task's functionality will be + * executed. It is expressed in clock ticks. + * @param setDeadlineMissedFunc The function pointer to the deadline missed function + * that shall be assigned. + * @param initFunction A pointer to the initialization function that returns the object to be executed. + */ + ObjectTask( const char *name, TaskPriority_t setPriority, size_t setStack, Interval_t setPeriod, void (*setDeadlineMissedFunc)(), ExecutableObjectIF* (*initFunction)() ); + /** + * @brief The second of the two standard constructors of the class. + * @details This constructor initializes the object to be executed with the aid of an + * object id and the global ObjectManager class. + * In the underlying TaskBase class, a new operating system task is created. + * In addition to the TaskBase parameters, the period, the object_id of the + * object to be executed and an optional "deadline-missed" function pointer is passed. + * @param priority Sets the priority of a task. Values range from a low 0 to a high 99. + * @param stack_size The stack size reserved by the operating system for the task. + * @param setPeriod The length of the period with which the task's functionality will be + * executed. It is expressed in clock ticks. + * @param setDeadlineMissedFunc The function pointer to the deadline missed function + * that shall be assigned. + * @param object_id The object id of the object to be executed. + */ + ObjectTask( const char *name, TaskPriority_t setPriority, size_t setStack, Interval_t setPeriod, void (*setDeadlineMissedFunc)(), object_id_t object_id ); + /** + * @brief Currently, the executed object's lifetime is not coupled with the task object's + * lifetime, so the destructor is empty. + */ + virtual ~ObjectTask( void ); + + /** + * @brief The method to start the task. + * @details The method starts the task with the respective system call. + * Entry point is the taskEntryPoint method described below. + * The address of the task object is passed as an argument + * to the system call. + */ + ReturnValue_t startTask( void ); + +}; + +#endif /* OBJECTTASK_H_ */ diff --git a/tasks/PeriodicTask.cpp b/tasks/PeriodicTask.cpp new file mode 100644 index 00000000..dfc31b1e --- /dev/null +++ b/tasks/PeriodicTask.cpp @@ -0,0 +1,81 @@ +/** + * \file PeriodicTask.cpp + * + * \brief This file contains the implementation for the PeriodicTask class. + * + * \author Bastian Baetz + * + * \date 21.07.2010 + * + * Copyright 2010, Bastian Baetz + * All rights reserved + * + */ + +//#include +#include +#include + +//TODO: Check if isRunning flag is useful. Shall tasks be restartable? +TaskReturn_t PeriodicTask::taskEntryPoint( TaskArgument_t argument ) { + + //The argument is re-interpreted as PeriodicTask + PeriodicTask *originalTask( reinterpret_cast(argument) ); + originalTask->taskFunctionality(); + debug << "Object task " << originalTask->getId() << " returned from taskFunctionality." << std::endl; + //TODO: destroy the task object? +} + +PeriodicTask::PeriodicTask( const char *name, TaskPriority_t setPriority, size_t setStack, Interval_t setPeriod, + void (*setDeadlineMissedFunc)(), ReturnValue_t ( *setTaskFunction )( TaskBase* ) ) : + TaskBase( setPriority, setStack, name ), period(setPeriod), periodId(0), deadlineMissedFunc(setDeadlineMissedFunc), + taskFunction(setTaskFunction) { +} + +PeriodicTask::~PeriodicTask() { +} + +ReturnValue_t PeriodicTask::startTask() { + debug << "PeriodicTask::startTask. TaskId: " << this->getId() << std::endl; + this->setRunning( true ); + ReturnValue_t status; + status = OSAL::startTask( &( this->id ), PeriodicTask::taskEntryPoint, TaskArgument_t( ( void *)this ) ); + if( status != RETURN_OK ) { + //TODO: Call any FDIR routine? + error << "PeriodicTask::startTask for " << this->getId() << " failed." << std::endl; + } else { + debug << "PeriodicTask::startTask for " << this->getId() << " successful" << std::endl; + } + return status; +} + +void PeriodicTask::taskFunctionality() { + + ReturnValue_t status = RETURN_OK; + + //The period is set up and started with the system call. + status = OSAL::setAndStartPeriod( this->period, &(this->periodId) ); + if( status == RETURN_OK ) { + //The task's "infinite" inner loop is entered. + while( this->isRunning ) { + //If a functionality is announced, it is started. + if( this->taskFunction != NULL ) { + this->taskFunction( this ); + } + //The period is checked and restarted. + //If the deadline was missed, the deadlineMissedFunc is called. + if( OSAL::checkAndRestartPeriod( this->periodId, this->period ) == OSAL::TIMEOUT ) { + error << "PeriodicTask: " << std::hex << this->getId() << " Deadline missed." << std::endl; + if( this->deadlineMissedFunc != NULL ) { + this->deadlineMissedFunc(); + } + } + } + debug << "Returned from taskFunctionality()-Loop." << std::endl; + } else { + error << "PeriodicTask::setAndStartPeriod failed with status " << status << std::endl; + } + //Any operating system object for periodic execution is deleted. + debug << "Deleting the PeriodicThread's period." << std::endl; + OSAL::deletePeriod( &(this->id) ); +} diff --git a/tasks/PeriodicTask.h b/tasks/PeriodicTask.h new file mode 100644 index 00000000..fc36ab04 --- /dev/null +++ b/tasks/PeriodicTask.h @@ -0,0 +1,113 @@ +/** + * @file PeriodicTask.h + * + * @brief This file contains the definition for the PeriodicTask class. + * + * @author Bastian Baetz + * + * @date 07/21/2010 + * + */ +#ifndef OPUSPERIODICTASK_H_ +#define OPUSPERIODICTASK_H_ + +#include + +/** + * + * @brief This class represents a specialized task for periodic activities. + * + * @details A simple, but very important task type is the periodic task. Each task of this type has + * a certain period assigned. When started, the task's functionality (a simple function) + * is executed. On finishing, the task is blocked for the rest of the period and restarted + * afterwards. A missed deadline is detected and a function to perform necessary failure + * detection may be called. + * + * @author Bastian Baetz + * + * @date 07/21/2010 + * + * @ingroup task_handling + */ +class PeriodicTask: public TaskBase { +protected: + + /** + * @brief The period of the task. + * @details The period determines the frequency of the task's execution. It is expressed in clock ticks. + */ + Interval_t period; + + /** + * @brief id of the associated OS period + */ + PeriodId_t periodId; + + /** + * @brief This is the function executed in the new task's context. + * @details It converts the argument back to the thread object type and copies the class instance + * to the task context. The taskFunctionality method is called afterwards. + * @param A pointer to the task object itself is passed as argument. + */ + static TaskReturn_t taskEntryPoint( TaskArgument_t argument ); + + /** + * @brief The pointer to the deadline-missed function. + * @details This pointer stores the function that is executed if the task's deadline is missed. + * So, each may react individually on a timing failure. The pointer may be NULL, + * then nothing happens on missing the deadline. The deadline is equal to the next execution + * of the periodic task. + */ + void ( *deadlineMissedFunc )( void ); + + /** + * @brief The function containing the actual functionality of the task. + * @image latex act_OPUSPeriodicThread.eps "Activity diagram of the PeriodicThread functionality." width=0.6@textwidth + * @image html act_OPUSPeriodicThread.png "Activity diagram of the PeriodicThread functionality." + * @details The figure above shows the functional execution of this method. It sets and starts + * the task's period, then enters a loop that is repeated as long as the isRunning + * attribute is true. Within the loop, the taskFunction is called, and + * afterwards the checkAndRestartPeriod system call to block the task until the next + * period. On missing the deadline, the deadlineMissedFunction is executed. + */ + void taskFunctionality( void ); + /** + * @brief In this attribute the pointer to the function which shall be executed periodically + * is stored. + */ + ReturnValue_t ( *taskFunction )( TaskBase* ); +public: + /** + * @brief The standard constructor of the class. + * @details This is the general constructor of the class. In the underlying TaskBase class, + * a new operating system task is created. In addition to the TaskBase parameters, + * the period, the actual function to be executed and an optional "deadline-missed" + * function pointer is passed. + * @param priority Sets the priority of a task. Values range from a low 0 to a high 99. + * @param stack_size The stack size reserved by the operating system for the task. + * @param setPeriod The length of the period with which the task's functionality will be + * executed. It is expressed in clock ticks. + * @param (*setDeadlineMissedFunc)() The function pointer to the deadline missed function + * that shall be assigned. + * @param ( *setTaskFunction )( TaskBase* ) A pointer to the actual function to be executed. + */ + PeriodicTask( const char *name, TaskPriority_t setPriority, size_t setStack, Interval_t setPeriod, void ( *setDeadlineMissedFunc )(), ReturnValue_t ( *setTaskFunction )( TaskBase* ) ); + + /** + * @brief The destructor of the class. + * @details Similar to the destructor in the parent class, no memory clean-ups are necessary. + * Thus, the destructor is empty. + */ + virtual ~PeriodicTask( void ); + + /** + * @brief The method to start the task. + * @details The method starts the task with the respective system call. + * Entry point is the taskEntryPoint method described below. + * The address of the task object is passed as an argument + * to the system call. + */ + virtual ReturnValue_t startTask( void ); +}; + +#endif /* OPUSPERIODICTASK_H_ */ diff --git a/tasks/TaskBase.cpp b/tasks/TaskBase.cpp new file mode 100644 index 00000000..9279bad1 --- /dev/null +++ b/tasks/TaskBase.cpp @@ -0,0 +1,50 @@ +/* + * TaskBase.cpp + * + * Created on: Nov 5, 2012 + * Author: baetz + */ + + + + +#include +#include + +TaskBase::TaskBase( TaskPriority_t set_priority, size_t stack_size , const char *name) : isRunning(false) { + Name_t osalName = 0; + for (uint8_t i = 0; i < 4; i++){ + if (name[i] == 0){ + break; + } + osalName += name[i] << (8*(3-i)); + } + //The task is created with the operating system's system call. + ReturnValue_t status = OSAL::createTask( + osalName, set_priority, + stack_size, + OSAL::PREEMPT | OSAL::NO_TIMESLICE | OSAL::NO_ASR, OSAL::FLOATING_POINT, + &( this->id ) + ); + //TODO: Safe system halt or FDIR call on failed task creation? + if( status != RETURN_OK ) { + error << "TaskBase::TaskBase: createTask with name " << std::hex << osalName << std::dec << " failed with return code " << (uint32_t)status << std::endl; + this->id = 0; + } else { +// debug << "TaskBase::TaskBase: createTask successful. Name: " << std::hex << new_name << ", ID: " << this->id << std::dec << std::endl; + } +} + +TaskBase::~TaskBase() { + OSAL::deleteTask( &(this->id) ); +} + +TaskId_t TaskBase::getId() { + return this->id; +} + +void TaskBase::setRunning( bool set ) { + this->isRunning = set; +} + +uint8_t TaskBase::taskCounter = 0; diff --git a/tasks/TaskBase.h b/tasks/TaskBase.h new file mode 100644 index 00000000..2aa66d07 --- /dev/null +++ b/tasks/TaskBase.h @@ -0,0 +1,86 @@ +/** + * @file TaskBase.h + * + * @date 11/05/2012 + * @author Bastian Baetz + * + * @brief This file contains the definition of the TaskBase class. + * It is a reviewed and updated version of a file originally created + * by Claas Ziemke in 2010. + */ + +#ifndef TASKBASE_H_ +#define TASKBASE_H_ + + + +#include +#include +#include +/** + * @brief This is the basic task handling class. + * + * @details The virtual parent class contains attributes and methods to perform basic task operations. + * The task is created in the constructor. Next to the main methods to set the functionality + * and start the task, it also contains methods to stop execution safely and to return its + * identifier. + * The whole class was undergoing a major redesign by Bastian Baetz in November 2012 where + * unnecessary attributes were removed and task creation was simplified. Also, the class + * implements the TaskIF now. + * + * @author Claas Ziemke + * + * @date 07/23/2010 + * + * @ingroup task_handling + */ +class TaskBase : public TaskIF, public HasReturnvaluesIF { +protected: + /** + * @brief The task's name -a user specific information for the operating system- is + * generated automatically with the help of this static counter. + */ + static uint8_t taskCounter; + /** + * @brief The class stores the task id it got assigned from the operating system in this attribute. + * If initialization fails, the id is set to zero. + */ + TaskId_t id; + /** + * @brief The isRunning information can be used by child classes to change its operational behavior. + * @details It is not used in the TaskBase class itself, but for example in periodic tasks to leave + * the periodic activity. + */ + bool isRunning; +public: + /** + * @brief The constructor creates and initializes a task. + * @details This is accomplished by using the operating system call to create a task. The name is + * created automatically with the help od taskCounter. Priority and stack size are + * adjustable, all other attributes are set with default values. + * @param priority Sets the priority of a task. Values range from a low 0 to a high 99. + * @param stack_size The stack size reserved by the operating system for the task. + * @param nam The name of the Task, as a null-terminated String. Currently max 4 chars supported (excluding Null-terminator), rest will be truncated + */ + TaskBase( TaskPriority_t priority, size_t stack_size, const char *name); + /** + * @brief In the destructor, the created task is deleted. + */ + virtual ~TaskBase(); + /** + * @brief This abstract method must be implemented by child classes to successfully start a task. + */ + virtual ReturnValue_t startTask() = 0; + /** + * @brief This method returns the task id of this class. + */ + TaskId_t getId(); + /** + * @brief With this method, the task "running" state can be set. + * @details This typically involves leaving any kind of periodic activity. + */ + void setRunning( bool set ); +}; + + +#endif /* TASKBASE_H_ */ diff --git a/tasks/TaskIF.h b/tasks/TaskIF.h new file mode 100644 index 00000000..4b383c5e --- /dev/null +++ b/tasks/TaskIF.h @@ -0,0 +1,42 @@ +#ifndef TASKIF_H_ +#define TASKIF_H_ + +/** + * \defgroup task_handling Task Handling + * This is the group, where all classes associated with Task Handling belong to. + * Task handling is based on the task handling methods which the operating system + * provides (currently RTEMS). + */ + +/** + * @brief This interface provides all basic methods to handle task operations. + * @details To uniformly operate different types of tasks, this interface provides methods to + * start and stop created tasks and to return the operating system's identifier. + * + * @date 11/05/2012 + * @ingroup task_handling + * author Bastian Baetz + */ +class TaskIF { +public: + /** + * @brief A virtual destructor as it is mandatory for interfaces. + */ + virtual ~TaskIF() { } + /** + * @brief With the startTask method, a created task can be started for the first time. + */ + virtual ReturnValue_t startTask() = 0; + /** + * @brief The getId method returns the task's operating system identifier. + */ + virtual TaskId_t getId() = 0; + /** + * @brief With this method, the task "running" state can be set. + * @details This typically involves leaving any kind of periodic activity. + */ + virtual void setRunning( bool set ) = 0; +}; + + +#endif /* TASKIF_H_ */ diff --git a/tcdistribution/CCSDSDistributor.cpp b/tcdistribution/CCSDSDistributor.cpp new file mode 100644 index 00000000..952f4fbd --- /dev/null +++ b/tcdistribution/CCSDSDistributor.cpp @@ -0,0 +1,81 @@ +/* + * CCSDSDistributor.cpp + * + * Created on: 18.06.2012 + * Author: baetz + */ + +#include +#include +#include + +CCSDSDistributor::CCSDSDistributor( uint16_t set_default_apid ) : + TcDistributor( objects::CCSDS_PACKET_DISTRIBUTOR ), default_apid( set_default_apid ), tcStore(NULL) { +} + +CCSDSDistributor::~CCSDSDistributor() { + +} + +iterator_t CCSDSDistributor::selectDestination() { +// debug << "CCSDSDistributor::selectDestination received: " << this->currentMessage.getStorageId().pool_index << ", " << this->currentMessage.getStorageId().packet_index << std::endl; + const uint8_t* p_packet = NULL; + uint32_t size = 0; + this->tcStore->getData( this->currentMessage.getStorageId(), &p_packet, &size ); + SpacePacketBase current_packet( p_packet ); +// info << "CCSDSDistributor::selectDestination has packet with APID " << std::hex << current_packet.getAPID() << std::dec << std::endl; + iterator_t position = this->queueMap.find( current_packet.getAPID() ); + if ( position != this->queueMap.end() ) { + return position; + } else { + //The APID was not found. Forward packet to main SW-APID anyway to create acceptance failure report. + return this->queueMap.find( this->default_apid ); + } + +} + +MessageQueueId_t CCSDSDistributor::getRequestQueue() { + return this->tcQueue.getId(); +} + +ReturnValue_t CCSDSDistributor::registerApplication( + AcceptsTelecommandsIF* application) { + ReturnValue_t returnValue = RETURN_OK; + bool errorCode = true; + errorCode = this->queueMap.insert( std::pair( application->getIdentifier(), application->getRequestQueue() ) ).second; + if( errorCode == false ) { + returnValue = RETURN_FAILED; + } + return returnValue; +} + +ReturnValue_t CCSDSDistributor::registerApplication(uint16_t apid, + MessageQueueId_t id) { + ReturnValue_t returnValue = RETURN_OK; + bool errorCode = true; + errorCode = this->queueMap.insert( std::pair( apid, id ) ).second; + if( errorCode == false ) { + returnValue = RETURN_FAILED; + } + return returnValue; + +} + +uint16_t CCSDSDistributor::getIdentifier() { + return 0; +} + +ReturnValue_t CCSDSDistributor::initialize() { + ReturnValue_t status = this->TcDistributor::initialize(); + this->tcStore = objectManager->get( objects::TC_STORE ); + if (this->tcStore == NULL) status = RETURN_FAILED; + return status; +} + +ReturnValue_t CCSDSDistributor::callbackAfterSending( + ReturnValue_t queueStatus) { + if (queueStatus != RETURN_OK) { + tcStore->deleteData(currentMessage.getStorageId()); + } + return RETURN_OK; +} diff --git a/tcdistribution/CCSDSDistributor.h b/tcdistribution/CCSDSDistributor.h new file mode 100644 index 00000000..5b22c710 --- /dev/null +++ b/tcdistribution/CCSDSDistributor.h @@ -0,0 +1,65 @@ +/* + * CCSDSDistributor.h + * + * Created on: 18.06.2012 + * Author: baetz + */ + +#ifndef CCSDSDISTRIBUTOR_H_ +#define CCSDSDISTRIBUTOR_H_ + +#include +#include +#include +#include +#include +/** + * An instantiation of the CCSDSDistributorIF. + * It receives Space Packets, and selects a destination depending on the APID of the telecommands. + * The Secondary Header (with Service/Subservice) is ignored. + * \ingroup tc_distribution + */ +class CCSDSDistributor : public TcDistributor, public CCSDSDistributorIF, public AcceptsTelecommandsIF { +protected: + /** + * This implementation checks if an Application with fitting APID has registered and forwards the + * packet to the according message queue. + * If the packet is not found, it returns the queue to \c default_apid, where a Acceptance Failure + * message should be generated. + * @return Iterator to map entry of found APID or iterator to default APID. + */ + iterator_t selectDestination(); + /** + * The default APID, where packets with unknown APID are sent to. + */ + uint16_t default_apid; + /** + * A reference to the TC storage must be maintained, as this class handles pure Space Packets and there + * exists no SpacePacketStored class. + */ + StorageManagerIF* tcStore; + /** + * The callback here handles the generation of acceptance success/failure messages. + */ + ReturnValue_t callbackAfterSending( ReturnValue_t queueStatus ); +public: + /** + * The constructor sets the default APID and calls the TcDistributor ctor with a certain object id. + * \c tcStore is set in the \c initialize method. + * @param set_default_apid The default APID, where packets with unknown destination are sent to. + */ + CCSDSDistributor( uint16_t set_default_apid = 0x35 ); + /** + * The destructor is empty. + */ + ~CCSDSDistributor(); + MessageQueueId_t getRequestQueue(); + ReturnValue_t registerApplication( uint16_t apid, MessageQueueId_t id ); + ReturnValue_t registerApplication( AcceptsTelecommandsIF* application ); + uint16_t getIdentifier(); + ReturnValue_t initialize(); +}; + + + +#endif /* CCSDSDISTRIBUTOR_H_ */ diff --git a/tcdistribution/CCSDSDistributorIF.h b/tcdistribution/CCSDSDistributorIF.h new file mode 100644 index 00000000..9843c8a8 --- /dev/null +++ b/tcdistribution/CCSDSDistributorIF.h @@ -0,0 +1,47 @@ +/* + * CCSDSDistributorIF.h + * + * Created on: 21.11.2012 + * Author: baetz + */ + +#ifndef CCSDSDISTRIBUTORIF_H_ +#define CCSDSDISTRIBUTORIF_H_ + +#include +#include +/** + * This is the Interface to a CCSDS Distributor. + * On a CCSDS Distributor, Applications (in terms of CCSDS) may register themselves, + * either by passing a pointer to themselves (and implementing the CCSDSApplicationIF, + * or by explicitly passing an APID and a MessageQueueId to route the TC's to. + * \ingroup tc_distribution + */ +class CCSDSDistributorIF { +public: + /** + * With this call, a class implementing the CCSDSApplicationIF can register at the + * distributor. + * @param application A pointer to the Application to register. + * @return - \c RETURN_OK on success, + * - \c RETURN_FAILED on failure. + */ + virtual ReturnValue_t registerApplication( AcceptsTelecommandsIF* application ) = 0; + /** + * With this call, other Applications can register to the CCSDS distributor. + * This is done by passing an APID and a MessageQueueId to the method. + * @param apid The APID to register. + * @param id The MessageQueueId of the message queue to send the TC Packets to. + * @return - \c RETURN_OK on success, + * - \c RETURN_FAILED on failure. + */ + virtual ReturnValue_t registerApplication( uint16_t apid, MessageQueueId_t id ) = 0; + /** + * The empty virtual destructor. + */ + virtual ~CCSDSDistributorIF() { + } +}; + + +#endif /* CCSDSDISTRIBUTORIF_H_ */ diff --git a/tcdistribution/Makefile b/tcdistribution/Makefile new file mode 100755 index 00000000..03c13e75 --- /dev/null +++ b/tcdistribution/Makefile @@ -0,0 +1,23 @@ +#!/bin/bash +# +# OSAL makefile +# +# Created on: Mar 04, 2010 +# Author: ziemke +# Author: Claas Ziemke +# Copyright 2010, Claas Ziemke +# + +BASEDIR=../../ +include $(BASEDIR)options.mk + +OBJ = $(BUILDDIR)/TcDistributor.o\ + $(BUILDDIR)/CCSDSDistributor.o + +all: $(OBJ) + +$(BUILDDIR)/%.o: %.cpp %.h + $(CPP) $(CFLAGS) $(DEFINES) $(CCOPT) ${INCLUDE} -c $< -o $@ + +clean: + $(RM) *.o *.gcno *.gcda diff --git a/tcdistribution/PUSDistributor.cpp b/tcdistribution/PUSDistributor.cpp new file mode 100644 index 00000000..46f2ab37 --- /dev/null +++ b/tcdistribution/PUSDistributor.cpp @@ -0,0 +1,97 @@ +/* + * PUSDistributor.cpp + * + * Created on: 18.06.2012 + * Author: baetz + */ + + + +#include +#include +#include +#include +#include + +PUSDistributor::PUSDistributor(uint16_t set_apid ) : TcDistributor(objects::PUS_PACKET_DISTRIBUTOR), checker(set_apid), verify_channel(), + current_packet(), tc_status(RETURN_FAILED) { + +} + +PUSDistributor::~PUSDistributor() { + +} + +iterator_t PUSDistributor::selectDestination() { +// debug << "PUSDistributor::handlePacket received: " << this->current_packet_id.store_index << ", " << this->current_packet_id.packet_index << std::endl; + iterator_t queueMapIt = this->queueMap.end(); + this->current_packet.setStoreAddress( this->currentMessage.getStorageId() ); + if ( current_packet.getWholeData() != NULL ) { + tc_status = checker.checkPacket( ¤t_packet ); +// info << "PUSDistributor::handlePacket: packetCheck returned with " << (int)tc_status << std::endl; + uint32_t queue_id = current_packet.getService(); + queueMapIt = this->queueMap.find( queue_id ); + } else { + tc_status = PACKET_LOST; + } + if ( queueMapIt == this->queueMap.end() ) { + tc_status = DESTINATION_NOT_FOUND; + } + + if ( tc_status != RETURN_OK ) { + debug << "PUSDistributor::handlePacket: error with " << (int)tc_status << std::endl; + return this->queueMap.end(); + } else { + return queueMapIt; + } + +} + + +//uint16_t PUSDistributor::createDestination( uint8_t service_id, uint8_t subservice_id ) { +// return ( service_id << 8 ) + subservice_id; +//} + +ReturnValue_t PUSDistributor::registerService(AcceptsTelecommandsIF* service) { + ReturnValue_t returnValue = RETURN_OK; + bool errorCode = true; + uint16_t serviceId = service->getIdentifier(); + MessageQueueId_t queue = service->getRequestQueue(); + errorCode = this->queueMap.insert( std::pair( serviceId, queue) ).second; + if( errorCode == false ) { + returnValue = OSAL::RESOURCE_IN_USE; + } + return returnValue; +} + +MessageQueueId_t PUSDistributor::getRequestQueue() { + return this->tcQueue.getId(); +} + +ReturnValue_t PUSDistributor::callbackAfterSending(ReturnValue_t queueStatus) { + if ( queueStatus != RETURN_OK ) { + tc_status = queueStatus; + } + if ( tc_status != RETURN_OK ) { + this->verify_channel.sendFailureReport( TC_VERIFY::ACCEPTANCE_FAILURE, ¤t_packet, tc_status ); + //A failed packet is deleted immediately after reporting, otherwise it will block memory. + current_packet.deletePacket(); + return RETURN_FAILED; + } else { + this->verify_channel.sendSuccessReport( TC_VERIFY::ACCEPTANCE_SUCCESS, ¤t_packet ); + return RETURN_OK; + } +} + +uint16_t PUSDistributor::getIdentifier() { + return checker.getApid(); +} + +ReturnValue_t PUSDistributor::initialize() { + CCSDSDistributorIF* ccsdsDistributor = objectManager->get(objects::CCSDS_PACKET_DISTRIBUTOR); + if (ccsdsDistributor == NULL) { + return RETURN_FAILED; + } else { + return ccsdsDistributor->registerApplication(this); + } +} diff --git a/tcdistribution/PUSDistributor.h b/tcdistribution/PUSDistributor.h new file mode 100644 index 00000000..620f42bd --- /dev/null +++ b/tcdistribution/PUSDistributor.h @@ -0,0 +1,71 @@ +/* + * PUSDistributor.h + * + * Created on: 18.06.2012 + * Author: baetz + */ + +#ifndef PUSDISTRIBUTOR_H_ +#define PUSDISTRIBUTOR_H_ + +#include +#include +#include +#include +#include +#include + +/** + * This class accepts PUS Telecommands and forwards them to Application services. + * In addition, the class performs a formal packet check and sends acceptance success + * or failure messages. + * \ingroup tc_distribution + */ +class PUSDistributor : public TcDistributor, public PUSDistributorIF, public AcceptsTelecommandsIF { +protected: + /** + * This attribute contains the class, that performs a formal packet check. + */ + TcPacketCheck checker; + /** + * With this class, verification messages are sent to the TC Verification service. + */ + VerificationReporter verify_channel; + /** + * The currently handled packet is stored here. + */ + TcPacketStored current_packet; + /** + * With this variable, the current check status is stored to generate acceptance messages later. + */ + ReturnValue_t tc_status; + /** + * This method reads the packet service, checks if such a service is registered and forwards the packet to the destination. + * It also initiates the formal packet check and sending of verification messages. + * @return Iterator to map entry of found service id or iterator to \c map.end(). + */ + iterator_t selectDestination(); + /** + * The callback here handles the generation of acceptance success/failure messages. + */ + ReturnValue_t callbackAfterSending( ReturnValue_t queueStatus ); +public: + /** + * The ctor passes \c set_apid to the checker class and calls the TcDistribution ctor with a certain object id. + * @param set_apid The APID of this receiving Application. + */ + PUSDistributor( uint16_t set_apid ); + /** + * The destructor is empty. + */ + virtual ~PUSDistributor(); + ReturnValue_t registerService( AcceptsTelecommandsIF* service ); + MessageQueueId_t getRequestQueue(); + uint16_t getIdentifier(); + ReturnValue_t initialize(); +}; + + + + +#endif /* PUSDISTRIBUTOR_H_ */ diff --git a/tcdistribution/PUSDistributorIF.h b/tcdistribution/PUSDistributorIF.h new file mode 100644 index 00000000..f40e6f9f --- /dev/null +++ b/tcdistribution/PUSDistributorIF.h @@ -0,0 +1,33 @@ +/* + * PUSDistributorIF.h + * + * Created on: 21.11.2012 + * Author: baetz + */ + +#ifndef PUSDISTRIBUTORIF_H_ +#define PUSDISTRIBUTORIF_H_ + +#include +#include +/** + * This interface allows PUS Services to register themselves at a PUS Distributor. + * \ingroup tc_distribution + */ +class PUSDistributorIF { +public: + /** + * The empty virtual destructor. + */ + virtual ~PUSDistributorIF() { + } +/** + * With this method, Services can register themselves at the PUS Distributor. + * @param service A pointer to the registering Service. + * @return - \c RETURN_OK on success, + * - \c RETURN_FAILED on failure. + */ + virtual ReturnValue_t registerService( AcceptsTelecommandsIF* service ) = 0; +}; + +#endif /* PUSDISTRIBUTORIF_H_ */ diff --git a/tcdistribution/TcDistributor.cpp b/tcdistribution/TcDistributor.cpp new file mode 100644 index 00000000..4c7a67a9 --- /dev/null +++ b/tcdistribution/TcDistributor.cpp @@ -0,0 +1,55 @@ +#include +#include +#include +#include +#include + +TcDistributor::TcDistributor(object_id_t set_object_id) : + SystemObject(set_object_id), tcQueue(DISTRIBUTER_MAX_PACKETS) { + +} + +TcDistributor::~TcDistributor() { + //Nothing to do in the destructor, MQ's are destroyed elsewhere. +} + +ReturnValue_t TcDistributor::performOperation(void) { + ReturnValue_t status = RETURN_OK; +// debug << "TcDistributor: performing Operation." << std::endl; + for (status = tcQueue.receiveMessage(¤tMessage); status == RETURN_OK; + status = tcQueue.receiveMessage(¤tMessage)) { + status = handlePacket(); + } + if (status == OSAL::QUEUE_EMPTY) { + return RETURN_OK; + } else { + return status; + } +} + +ReturnValue_t TcDistributor::handlePacket() { + + iterator_t queueMapIt = this->selectDestination(); + ReturnValue_t returnValue = RETURN_FAILED; + if (queueMapIt != this->queueMap.end()) { + returnValue = this->tcQueue.sendMessage(queueMapIt->second, + &this->currentMessage); + } + return this->callbackAfterSending(returnValue); +} + +void TcDistributor::print() { + debug << "Distributor content is: " << std::endl << "ID\t| message queue id" + << std::endl; + for (iterator_t it = this->queueMap.begin(); it != this->queueMap.end(); + it++) { + debug << it->first << "\t| 0x" << std::hex << it->second << std::dec + << std::endl; + } + debug << std::dec; + +} + +ReturnValue_t TcDistributor::callbackAfterSending(ReturnValue_t queueStatus) { + return RETURN_OK; +} diff --git a/tcdistribution/TcDistributor.h b/tcdistribution/TcDistributor.h new file mode 100644 index 00000000..792762ca --- /dev/null +++ b/tcdistribution/TcDistributor.h @@ -0,0 +1,117 @@ +#ifndef TCDISTRIBUTOR_H_ +#define TCDISTRIBUTOR_H_ +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/** + * \defgroup tc_distribution Telecommand Distribution + * All classes associated with Routing and Distribution of Telecommands belong to this group. + */ + +/** + * This typedef simplifies writing down the \c map iterator. + */ +typedef std::map::iterator iterator_t; + +/** + * This is the base class to implement distributors for Space Packets. + * Typically, the distribution is required to forward Telecommand packets + * over the satellite applications and services. The class receives + * Space Packets over a message queue and holds a map that links other + * message queue ids to some identifier. The process of unpacking the + * destination information from the packet is handled by the child class + * implementations. + * \ingroup tc_distribution + */ +class TcDistributor : public SystemObject, public ExecutableObjectIF, public HasReturnvaluesIF { +private: + /** + * This constant sets the maximum number of packets distributed per call. + */ + static const uint8_t DISTRIBUTER_MAX_PACKETS = 128; +protected: + /** + * This is the receiving queue for incoming Telecommands. + * The child classes must make its queue id public. + */ + MessageQueue tcQueue; + /** + * The last received incoming packet information is stored in this + * member. + * As different child classes unpack the incoming packet differently + * (i.e. as a CCSDS Space Packet or as a PUS Telecommand Packet), it + * is not tried to unpack the packet information within this class. + */ + TmTcMessage currentMessage; + /** + * The map that links certain packet information to a destination. + * The packet information may be the APID of the packet or the service + * identifier. Filling of the map is under control of the different child + * classes. + */ + std::map queueMap; + /** + * This method shall unpack the routing information from the incoming + * packet and select the map entry which represents the packet's target. + * @return An iterator to the map element to forward to or queuMap.end(). + */ + virtual iterator_t selectDestination() = 0; + /** + * The handlePacket method calls the child class's selectDestination method + * and forwards the packet to its destination, if found. + * @return The message queue return value or \c RETURN_FAILED, in case no + * destination was found. + */ + ReturnValue_t handlePacket(); + /** + * This method gives the child class a chance to perform some kind of operation + * after the parent tried to forward the message. + * A typically application would be sending success/failure messages. + * The default implementation just returns \c RETURN_OK. + * @param queueStatus The status of the message queue after an attempt to send the TC. + * @return - \c RETURN_OK on success + * - \c RETURN_FAILED on failure + */ + virtual ReturnValue_t callbackAfterSending( ReturnValue_t queueStatus ); +public: + static const uint8_t INTERFACE_ID = PACKET_DISTRIBUTION; + static const ReturnValue_t PACKET_LOST = MAKE_RETURN_CODE( 1 ); + static const ReturnValue_t DESTINATION_NOT_FOUND = MAKE_RETURN_CODE( 2 ); + /** + * Within the default constructor, the SystemObject id is set and the + * message queue is initialized. + * Filling the map is under control of the child classes. + * @param set_object_id This id is assigned to the distributor + * implementation. + */ + TcDistributor( object_id_t set_object_id ); + /** + * The destructor is empty, the message queues are not in the vicinity of + * this class. + */ + virtual ~TcDistributor(); + /** + * The method is called cyclically and fetches new incoming packets from + * the message queue. + * In case a new packet is found, it calls the handlePacket method to deal + * with distribution. + * @return The error code of the message queue call. + */ + ReturnValue_t performOperation(); + /** + * A simple debug print, that prints all distribution information stored in + * queueMap. + */ + void print(); +}; + + +#endif /* TCDISTRIBUTOR_H_ */ diff --git a/tcdistribution/TcPacketCheck.cpp b/tcdistribution/TcPacketCheck.cpp new file mode 100644 index 00000000..e82ee564 --- /dev/null +++ b/tcdistribution/TcPacketCheck.cpp @@ -0,0 +1,45 @@ +/* + * TcPacketCheck.cpp + * + * Created on: 19.06.2012 + * Author: baetz + */ + + +#include +#include +#include +#include +#include + +TcPacketCheck::TcPacketCheck( uint16_t set_apid ) : apid(set_apid) { +} + +ReturnValue_t TcPacketCheck::checkPacket( TcPacketStored* current_packet ) { + uint16_t calculated_crc = ::Calculate_CRC ( current_packet->getWholeData(), current_packet->getFullSize() ); + if ( calculated_crc != 0 ) { + return INCORRECT_CHECKSUM; + } + bool condition = !(current_packet->hasSecondaryHeader()) || + current_packet->getPacketVersionNumber() != CCSDS_VERSION_NUMBER || + !(current_packet->isTelecommand()); + if ( condition ) { + return INCORRECT_PRIMARY_HEADER; + } + if ( current_packet->getAPID() != this->apid ) + return ILLEGAL_APID; + + if ( !current_packet->isSizeCorrect() ) { + return INCOMPLETE_PACKET; + } + condition = (current_packet->getSecondaryHeaderFlag() != CCSDS_SECONDARY_HEADER_FLAG) || + (current_packet->getPusVersionNumber() != PUS_VERSION_NUMBER); + if ( condition ) { + return INCORRECT_SECONDARY_HEADER; + } + return RETURN_OK; +} + +uint16_t TcPacketCheck::getApid() const { + return apid; +} diff --git a/tcdistribution/TcPacketCheck.h b/tcdistribution/TcPacketCheck.h new file mode 100644 index 00000000..b635b949 --- /dev/null +++ b/tcdistribution/TcPacketCheck.h @@ -0,0 +1,66 @@ +/* + * TcPacketCheck.h + * + * Created on: 19.06.2012 + * Author: baetz + */ + +#ifndef TCPACKETCHECK_H_ +#define TCPACKETCHECK_H_ + +#include +#include +#include +/** + * This class performs a formal packet check for incoming PUS Telecommand Packets. + * Currently, it only checks if the APID and CRC are correct. + * \ingroup tc_distribution + */ +class TcPacketCheck : public HasReturnvaluesIF { +protected: + /** + * Describes the version number a packet must have to pass. + */ + static const uint8_t CCSDS_VERSION_NUMBER = 0; + /** + * Describes the secondary header a packet must have to pass. + */ + static const uint8_t CCSDS_SECONDARY_HEADER_FLAG = 0; + /** + * Describes the TC Packet PUS Version Number a packet must have to pass. + */ + static const uint8_t PUS_VERSION_NUMBER = 1; + /** + * The packet id each correct packet should have. + * It is composed of the APID and some static fields. + */ + uint16_t apid; +public: + static const uint8_t INTERFACE_ID = TC_PACKET_CHECK; + static const ReturnValue_t ILLEGAL_APID = MAKE_RETURN_CODE( 0 ); + static const ReturnValue_t INCOMPLETE_PACKET = MAKE_RETURN_CODE( 1 ); + static const ReturnValue_t INCORRECT_CHECKSUM = MAKE_RETURN_CODE( 2 ); + static const ReturnValue_t ILLEGAL_PACKET_TYPE = MAKE_RETURN_CODE( 3 ); + static const ReturnValue_t ILLEGAL_PACKET_SUBTYPE = MAKE_RETURN_CODE( 4 ); + static const ReturnValue_t INCORRECT_PRIMARY_HEADER = MAKE_RETURN_CODE( 5 ); + static const ReturnValue_t INCORRECT_SECONDARY_HEADER = MAKE_RETURN_CODE( 6 ); + /** + * The constructor only sets the APID attribute. + * @param set_apid The APID to set. + */ + TcPacketCheck( uint16_t set_apid ); + /** + * This is the actual method to formally check a certain Telecommand Packet. + * The packet's Application Data can not be checked here. + * @param current_packet The packt to check + * @return - \c RETURN_OK on success. + * - \c INCORRECT_CHECKSUM if checksum is invalid. + * - \c ILLEGAL_APID if APID does not match. + */ + ReturnValue_t checkPacket( TcPacketStored* current_packet ); + + uint16_t getApid() const; +}; + + +#endif /* TCPACKETCHECK_H_ */ diff --git a/timemanager/CCSDSTime.cpp b/timemanager/CCSDSTime.cpp new file mode 100644 index 00000000..29d68e34 --- /dev/null +++ b/timemanager/CCSDSTime.cpp @@ -0,0 +1,471 @@ +/* + * CCSDSTime.cpp + * + * Created on: 22.03.2013 + * Author: tod + */ + +#include +#include +#include +#include + +CCSDSTime::CCSDSTime() { +} + +CCSDSTime::~CCSDSTime() { +} + +ReturnValue_t CCSDSTime::convertToCcsds(Ccs_seconds* to, + const TimeOfDay_t* from) { + to->pField = (CCS << 4); + + to->yearMSB = (from->year >> 8); + to->yearLSB = from->year & 0xff; + to->month = from->month; + to->day = from->day; + to->hour = from->hour; + to->minute = from->minute; + to->second = from->second; + + return RETURN_OK; +} + +ReturnValue_t CCSDSTime::convertToCcsds(Ccs_mseconds* to, + const TimeOfDay_t* from) { + to->pField = (CCS << 4) + 2; + + to->yearMSB = (from->year >> 8); + to->yearLSB = from->year & 0xff; + to->month = from->month; + to->day = from->day; + to->hour = from->hour; + to->minute = from->minute; + to->second = from->second; + to->secondEminus2 = from->ticks / 10; + to->secondEminus4 = (from->ticks % 10) * 10; + + return RETURN_OK; +} + +ReturnValue_t CCSDSTime::convertFromCcsds(TimeOfDay_t* to, const uint8_t* from, + uint32_t length) { + ReturnValue_t result; + if (length > 0xFF) { + return LENGTH_MISMATCH; + } + result = convertFromASCII(to, from, length); //Try to parse it as ASCII + if (result == RETURN_OK) { + return RETURN_OK; + } + + //Seems to be no ascii, try the other formats + uint8_t codeIdentification = (*from >> 4); + switch (codeIdentification) { + case CUC_LEVEL1: //CUC_LEVEL2 can not be converted to TimeOfDay (ToD is Level 1) <- Well, if we know the epoch, we can... + return convertFromCUC(to, from, length); + case CDS: + return convertFromCDS(to, from, length); + case CCS: { + //SHOULDDO: Returning the actual found length is the right way to go, but for now, keep it. + uint32_t temp = 0; + return convertFromCCS(to, from, &temp, length); + } + + default: + return UNSUPPORTED_TIME_FORMAT; + } +} + +ReturnValue_t CCSDSTime::convertFromCUC(TimeOfDay_t* to, const uint8_t* from, + uint8_t length) { + return UNSUPPORTED_TIME_FORMAT; +} + +ReturnValue_t CCSDSTime::convertFromCDS(TimeOfDay_t* to, const uint8_t* from, + uint8_t length) { + timeval time; + ReturnValue_t result = convertFromCDS(&time, from, NULL, length); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return convertTimevalToTimeOfDay(to, &time); +} + +ReturnValue_t CCSDSTime::convertFromCCS(TimeOfDay_t* to, const uint8_t* from, + uint32_t* foundLength, uint32_t maxLength) { + uint8_t subsecondsLength = *from & 0b111; + uint32_t totalLength = subsecondsLength + 8; + if (maxLength < totalLength) { + return LENGTH_MISMATCH; + } + //TODO: Check this! + Ccs_mseconds *temp = (Ccs_mseconds *) from; + + if (checkCcs((Ccs_seconds *) from) != RETURN_OK) { + return INVALID_TIME_FORMAT; + } + + to->year = (temp->yearMSB << 8) + temp->yearLSB; + to->hour = temp->hour; + to->minute = temp->minute; + to->second = temp->second; + + if (*from & (1 << 3)) { //day of year variation + uint16_t tempDay = (temp->month << 8) + temp->day; + ReturnValue_t result = convertDaysOfYear(tempDay, to->year, + &(temp->month), &(temp->day)); + if (result != RETURN_OK) { + return result; + } + } + + to->month = temp->month; + to->day = temp->day; + + to->ticks = 0; + *foundLength = sizeof(Ccs_seconds); + if (subsecondsLength > 0) { + *foundLength += 1; + if (temp->secondEminus2 >= 100) { + return INVALID_TIME_FORMAT; + } + to->ticks = temp->secondEminus2 * 10; + } + + if (subsecondsLength > 1) { + *foundLength += 1; + if (temp->secondEminus4 >= 100) { + return INVALID_TIME_FORMAT; + } + to->ticks += temp->secondEminus4 / 10; + } + + return RETURN_OK; + +} + +ReturnValue_t CCSDSTime::convertFromASCII(TimeOfDay_t* to, const uint8_t* from, + uint8_t length) { + if (length < 19) { + return RETURN_FAILED; + } + uint16_t year; + uint8_t month; + uint16_t day; + uint8_t hour; + uint8_t minute; + float second; +//try Code A (yyyy-mm-dd) + int count = sscanf((char *) from, "%4hi-%2hhi-%2hiT%2hhi:%2hhi:%fZ", &year, + &month, &day, &hour, &minute, &second); + if (count == 6) { + to->year = year; + to->month = month; + to->day = day; + to->hour = hour; + to->minute = minute; + to->second = second; + to->ticks = (second - floor(second)) * 1000; + return RETURN_OK; + } + + //try Code B (yyyy-ddd) + count = sscanf((char *) from, "%4hi-%3hiT%2hhi:%2hhi:%fZ", &year, &day, + &hour, &minute, &second); + if (count == 5) { + uint8_t tempDay; + ReturnValue_t result = CCSDSTime::convertDaysOfYear(day, year, + &month, &tempDay); + if (result != RETURN_OK) { + return RETURN_FAILED; + } + to->year = year; + to->month = month; + to->day = tempDay; + to->hour = hour; + to->minute = minute; + to->second = second; + to->ticks = (second - floor(second)) * 1000; + return RETURN_OK; + } + + return UNSUPPORTED_TIME_FORMAT; +} + +ReturnValue_t CCSDSTime::checkCcs(Ccs_seconds* time) { + if (time->pField & (1 << 3)) { //day of year variation + uint16_t day = (time->month << 8) + time->day; + if (day > 366) { + return INVALID_TIME_FORMAT; + } + } else { + if (time->month > 12) { + return INVALID_TIME_FORMAT; + } + if (time->day > 31) { + return INVALID_TIME_FORMAT; + } + } + if (time->hour > 23) { + return INVALID_TIME_FORMAT; + } + if (time->minute > 59) { + return INVALID_TIME_FORMAT; + } + if (time->second > 59) { + return INVALID_TIME_FORMAT; + } + return RETURN_OK; +} + +ReturnValue_t CCSDSTime::convertDaysOfYear(uint16_t dayofYear, uint16_t year, + uint8_t* month, uint8_t* day) { + if (isLeapYear(year)) { + if (dayofYear > 366) { + return INVALID_DAY_OF_YEAR; + } + } else { + if (dayofYear > 365) { + return INVALID_DAY_OF_YEAR; + } + } + *month = 1; + if (dayofYear <= 31) { + *day = dayofYear; + return RETURN_OK; + } + *month += 1; + dayofYear -= 31; + if (isLeapYear(year)) { + if (dayofYear <= 29) { + *day = dayofYear; + return RETURN_OK; + } + *month += 1; + dayofYear -= 29; + } else { + if (dayofYear <= 28) { + *day = dayofYear; + return RETURN_OK; + } + *month += 1; + dayofYear -= 28; + } + while (*month <= 12) { + if (dayofYear <= 31) { + *day = dayofYear; + return RETURN_OK; + } + *month += 1; + dayofYear -= 31; + + if (dayofYear <= 30) { + *day = dayofYear; + return RETURN_OK; + } + *month += 1; + dayofYear -= 30; + } + return INVALID_DAY_OF_YEAR; +} + +bool CCSDSTime::isLeapYear(uint16_t year) { + if ((year % 400) == 0) { + return true; + } + if ((year % 100) == 0) { + return false; + } + if ((year % 4) == 0) { + return true; + } + return false; +} + +ReturnValue_t CCSDSTime::convertToCcsds(CDS_short* to, const timeval* from) { + to->pField = (CDS << 4) + 0; + uint32_t days = ((from->tv_sec) / SECONDS_PER_DAY) + + DAYS_CCSDS_TO_UNIX_EPOCH; + if (days > (1 << 16)) { + //Date is beyond year 2137 + return TIME_DOES_NOT_FIT_FORMAT; + } + to->dayMSB = (days & 0xFF00) >> 8; + to->dayLSB = (days & 0xFF); + uint32_t msDay = ((from->tv_sec % SECONDS_PER_DAY) * 1000) + + (from->tv_usec / 1000); + to->msDay_hh = (msDay & 0xFF000000) >> 24; + to->msDay_h = (msDay & 0xFF0000) >> 16; + to->msDay_l = (msDay & 0xFF00) >> 8; + to->msDay_ll = (msDay & 0xFF); + return RETURN_OK; +} + +ReturnValue_t CCSDSTime::convertToCcsds(OBT_FLP* to, const timeval* from) { + to->pFiled = (AGENCY_DEFINED << 4) + 5; + to->seconds_hh = (from->tv_sec >> 24) & 0xff; + to->seconds_h = (from->tv_sec >> 16) & 0xff; + to->seconds_l = (from->tv_sec >> 8) & 0xff; + to->seconds_ll = (from->tv_sec >> 0) & 0xff; + + //convert the µs to 2E-16 seconds + uint64_t temp = from->tv_usec; + temp = temp << 16; + temp = temp / 1E6; + + to->subsecondsMSB = (temp >> 8) & 0xff; + to->subsecondsLSB = temp & 0xff; + + return RETURN_OK; +} + +ReturnValue_t CCSDSTime::convertFromCcsds(timeval* to, const uint8_t* from, + uint32_t* foundLength, uint32_t maxLength) { + //We don't expect ascii here. + uint8_t codeIdentification = (*from >> 4); + switch (codeIdentification) { + case CUC_LEVEL1: + return convertFromCUC(to, from, foundLength, maxLength); + case CDS: + return convertFromCDS(to, from, foundLength, maxLength); + case CCS: + return convertFromCCS(to, from, foundLength, maxLength); + default: + return UNSUPPORTED_TIME_FORMAT; + } + + +} + +ReturnValue_t CCSDSTime::convertFromCUC(timeval* to, const uint8_t* from, + uint32_t* foundLength, uint32_t maxLength) { + if (maxLength < 1) { + return INVALID_TIME_FORMAT; + } + uint8_t pField = *from; + from++; + ReturnValue_t result = convertFromCUC(to, pField, from, foundLength, maxLength-1 ); + if (result == HasReturnvaluesIF::RETURN_OK) { + if (foundLength != NULL) { + *foundLength += 1; + } + } + return result; +} + +ReturnValue_t CCSDSTime::convertTimevalToTimeOfDay(TimeOfDay_t* to, + timeval* from) { + //TODO: This is rather tricky. Implement only if needed. Also, if so, move to OSAL. + return UNSUPPORTED_TIME_FORMAT; +} + +ReturnValue_t CCSDSTime::convertFromCDS(timeval* to, const uint8_t* from, + uint32_t* foundLength, uint32_t maxLength) { + uint8_t pField = *from; + from++; + //Check epoch + if (pField & 0b1000) { + return NOT_ENOUGH_INFORMATION_FOR_TARGET_FORMAT; + } + //Check length + uint8_t expectedLength = 7; //Including p-Field. + bool extendedDays = pField & 0b100; + if (extendedDays) { + expectedLength += 1; + } + if ((pField & 0b11) == 0b01) { + expectedLength += 2; + } else if ((pField & 0b11) == 0b10) { + expectedLength += 4; + } + if (foundLength != NULL) { + *foundLength = expectedLength; + } + if (expectedLength > maxLength) { + return LENGTH_MISMATCH; + } + //Check and count days + uint32_t days = 0; + if (extendedDays) { + days = (from[0] << 16) + (from[1] << 8) + from[2]; + from +=3; + } else { + days = (from[0] << 8) + from[1]; + from +=2; + } + //Move to POSIX epoch. + if (days <= DAYS_CCSDS_TO_UNIX_EPOCH) { + return INVALID_TIME_FORMAT; + } + days -= DAYS_CCSDS_TO_UNIX_EPOCH; + to->tv_sec = days * SECONDS_PER_DAY; + uint32_t msDay = (from[0] << 24) + (from[1] << 16) + (from[2] << 8) + from[3]; + from +=4; + to->tv_sec += (msDay / 1000); + to->tv_usec = (msDay%1000)*1000; + if ((pField & 0b11) == 0b01 ) { + uint16_t usecs = (from[0] << 16) + from[1]; + from += 2; + if (usecs > 999) { + return INVALID_TIME_FORMAT; + } + to->tv_usec += usecs; + } else if ((pField & 0b11) == 0b10 ) { + uint32_t picosecs = (from[0] << 24) + (from[1] << 16) + (from[2] << 8) + from[3]; + from += 4; + if (picosecs > 999999) { + return INVALID_TIME_FORMAT; + } + //Not very useful. + to->tv_usec += (picosecs / 1000); + } + return RETURN_OK; +} + +ReturnValue_t CCSDSTime::convertFromCUC(timeval* to, uint8_t pField, + const uint8_t* from, uint32_t* foundLength, uint32_t maxLength) { + uint32_t secs = 0; + uint32_t subSeconds = 0; + uint8_t nCoarse = ((pField & 0b1100) >> 2) + 1; + uint8_t nFine = (pField & 0b11); + uint32_t totalLength = nCoarse+nFine; + if (foundLength != NULL) { + *foundLength = totalLength; + } + if (totalLength > maxLength) { + return LENGTH_MISMATCH; + } + for (int count = 0; count < nCoarse; count++) { + secs += *from << ((nCoarse*8-8)*(1+count)); + from++; + } + for (int count = 0; count < nFine; count++) { + subSeconds += *from << ((nFine*8-8)*(1+count)); + from++; + } + //Move to POSIX epoch. + to->tv_sec = secs; + if (pField & 0b10000) { + //CCSDS-Epoch + to->tv_sec -= (DAYS_CCSDS_TO_UNIX_EPOCH * SECONDS_PER_DAY); + } + to->tv_usec = subsecondsToMicroseconds(subSeconds); + return RETURN_OK; +} + +uint32_t CCSDSTime::subsecondsToMicroseconds(uint16_t subseconds) { + uint64_t temp = (uint64_t)subseconds * 1000000 / (1 << (sizeof(subseconds)*8)); + return temp; +} + +ReturnValue_t CCSDSTime::convertFromCCS(timeval* to, const uint8_t* from, + uint32_t* foundLength, uint32_t maxLength) { + TimeOfDay_t tempTime; + ReturnValue_t result = convertFromCCS(&tempTime,from, foundLength, maxLength); + if (result != RETURN_OK) { + return result; + } + return OSAL::convertTimeOfDayToTimeval(&tempTime, to); + +} diff --git a/timemanager/CCSDSTime.h b/timemanager/CCSDSTime.h new file mode 100644 index 00000000..5386a2f8 --- /dev/null +++ b/timemanager/CCSDSTime.h @@ -0,0 +1,234 @@ +/* + * CCSDSTimeHelper.h + * + * Created on: 22.03.2013 + * Author: tod + */ + +#ifndef CCSDSTIME_H_ +#define CCSDSTIME_H_ + +#include +#include +#include + +bool operator<(const timeval& lhs, const timeval& rhs); +bool operator==(const timeval& lhs, const timeval& rhs); +/** + * static helper class for CCSDS Time Code Formats + * + * as described in CCSDS 301.0-B-3 + * + * Still work in progress thus TODO finishme + */ +class CCSDSTime: public HasReturnvaluesIF { +public: + /** + * The Time code identifications, bits 4-6 in the P-Field + */ + enum TimeCodeIdentification { + CCS = 0b101, + CUC_LEVEL1 = 0b001, + CUC_LEVEL2 = 0b010, + CDS = 0b100, + AGENCY_DEFINED = 0b110 + }; + static const uint8_t P_FIELD_CUC_6B_CCSDS = (CUC_LEVEL1 << 4) + (3 << 2) + + 2; + static const uint8_t P_FIELD_CUC_6B_AGENCY = (CUC_LEVEL2 << 4) + (3 << 2) + + 2; + /** + * Struct for CDS day-segmented format. + */ + struct CDS_short { + uint8_t pField; + uint8_t dayMSB; + uint8_t dayLSB; + uint8_t msDay_hh; + uint8_t msDay_h; + uint8_t msDay_l; + uint8_t msDay_ll; + }; + /** + * Struct for the CCS fromat in day of month variation with seconds resolution + */ + struct Ccs_seconds { + uint8_t pField; + uint8_t yearMSB; + uint8_t yearLSB; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint8_t second; + }; + + /** + * Struct for the CCS fromat in day of month variation with 10E-4 seconds resolution + */ + struct Ccs_mseconds { + uint8_t pField; + uint8_t yearMSB; + uint8_t yearLSB; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint8_t second; + uint8_t secondEminus2; + uint8_t secondEminus4; + }; + + struct OBT_FLP { + uint8_t pFiled; + uint8_t seconds_hh; + uint8_t seconds_h; + uint8_t seconds_l; + uint8_t seconds_ll; + uint8_t subsecondsMSB; + uint8_t subsecondsLSB; + }; + + struct TimevalLess { + bool operator()(const timeval& lhs, const timeval& rhs) const { + return (lhs < rhs); + } + }; + + static const uint8_t INTERFACE_ID = CCSDS_TIME_HELPER_CLASS; + static const ReturnValue_t UNSUPPORTED_TIME_FORMAT = MAKE_RETURN_CODE(0); + static const ReturnValue_t NOT_ENOUGH_INFORMATION_FOR_TARGET_FORMAT = + MAKE_RETURN_CODE(1); + static const ReturnValue_t LENGTH_MISMATCH = MAKE_RETURN_CODE(2); + static const ReturnValue_t INVALID_TIME_FORMAT = MAKE_RETURN_CODE(3); + static const ReturnValue_t INVALID_DAY_OF_YEAR = MAKE_RETURN_CODE(4); + static const ReturnValue_t TIME_DOES_NOT_FIT_FORMAT = MAKE_RETURN_CODE(5); + + /** + * convert a TimeofDay struct to ccs with seconds resolution + * + * Assumes a valid TimeOfDay. TODO: maybe check it anyway? + * + * @param to pointer to a CCS struct + * @param from pointer to a TimeOfDay Struct + * @return + * - @c RETURN_OK as it assumes a valid TimeOfDay + */ + static ReturnValue_t convertToCcsds(Ccs_seconds *to, + TimeOfDay_t const *from); + + /** + * Converts to CDS format from timeval. + * @param to pointer to the CDS struct to generate + * @param from pointer to a timeval struct which comprises a time of day since UNIX epoch. + * @return + * - @c RETURN_OK as it assumes a valid timeval. + */ + static ReturnValue_t convertToCcsds(CDS_short* to, timeval const *from); + + static ReturnValue_t convertToCcsds(OBT_FLP* to, timeval const *from); + /** + * convert a TimeofDay struct to ccs with 10E-3 seconds resolution + * + * Assumes a valid TimeOfDay. TODO: maybe check it anyway? + * + * The 10E-4 seconds in the CCS Struct are 0 as the TimeOfDay only has ms resolution + * + * @param to pointer to a CCS struct + * @param from pointer to a TimeOfDay Struct + * @return + * - @c RETURN_OK as it assumes a valid TimeOfDay + */ + static ReturnValue_t convertToCcsds(Ccs_mseconds *to, + TimeOfDay_t const *from); + + /** + * TODO: can this be modified to recognize padding? + * Tries to interpret a Level 1 CCSDS time code + * + * It assumes binary formats contain a valid P Field and recognizes the ASCII format + * by the lack of one. + * + * @param to an empty TimeOfDay struct + * @param from pointer to an CCSDS Time code + * @param length length of the Time code + * @return + * - @c RETURN_OK if successful + * - @c UNSUPPORTED_TIME_FORMAT if a (possibly valid) time code is not supported TODO: the missing codes should be implemented... + * - @c LENGTH_MISMATCH if the length does not match the P Field + * - @c INVALID_TIME_FORMAT if the format or a value is invalid + */ + static ReturnValue_t convertFromCcsds(TimeOfDay_t *to, uint8_t const *from, + uint32_t length); + + /** + * not implemented yet + * + * @param to + * @param from + * @return + */ + static ReturnValue_t convertFromCcsds(timeval *to, uint8_t const *from, + uint32_t* foundLength, uint32_t maxLength); + + static ReturnValue_t convertFromCUC(TimeOfDay_t *to, uint8_t const *from, + uint8_t length); + + static ReturnValue_t convertFromCUC(timeval *to, uint8_t const *from, + uint32_t* foundLength, uint32_t maxLength); + + static ReturnValue_t convertFromCUC(timeval *to, uint8_t pField, + uint8_t const *from, uint32_t* foundLength, uint32_t maxLength); + + static ReturnValue_t convertFromCCS(timeval *to, uint8_t const *from, + uint32_t* foundLength, uint32_t maxLength); + + static ReturnValue_t convertFromCCS(timeval *to, uint8_t pField, + uint8_t const *from, uint32_t* foundLength, uint32_t maxLength); + + static ReturnValue_t convertFromCDS(TimeOfDay_t *to, uint8_t const *from, + uint8_t length); + + static ReturnValue_t convertFromCDS(timeval *to, uint8_t const *from, + uint32_t* foundLength, uint32_t maxLength); + + static ReturnValue_t convertFromCCS(TimeOfDay_t *to, uint8_t const *from, + uint32_t* foundLength, uint32_t maxLength); + + static ReturnValue_t convertFromASCII(TimeOfDay_t *to, uint8_t const *from, + uint8_t length); + + static uint32_t subsecondsToMicroseconds(uint16_t subseconds); +private: + CCSDSTime(); + virtual ~CCSDSTime(); + /** + * checks a ccs time struct for validity + * + * only checks year to second, subseconds must be checked elsewhere + * + * @param time pointer to an Ccs struct (should be cast to Ccs_seconds as subseconds are not checked. Cast is save as subseconds are at the end of the struct) + * @return + */ + static ReturnValue_t checkCcs(Ccs_seconds *time); + + static const uint32_t SECONDS_PER_DAY = 24 * 60 * 60; + static const uint32_t SECONDS_PER_NON_LEAP_YEAR = SECONDS_PER_DAY * 365; + static const uint32_t DAYS_CCSDS_TO_UNIX_EPOCH = 4383; //!< Time difference between CCSDS and POSIX epoch. This is exact, because leap-seconds where not introduced before 1972. + static const uint32_t SECONDS_CCSDS_TO_UNIX_EPOCH = DAYS_CCSDS_TO_UNIX_EPOCH + * SECONDS_PER_DAY; + /** + * @param dayofYear + * @param year + * @param month + * @param day + */ + static ReturnValue_t convertDaysOfYear(uint16_t dayofYear, uint16_t year, + uint8_t *month, uint8_t *day); + + static bool isLeapYear(uint16_t year); + static ReturnValue_t convertTimevalToTimeOfDay(TimeOfDay_t* to, + timeval* from); +}; + +#endif /* CCSDSTIME_H_ */ diff --git a/timemanager/Countdown.cpp b/timemanager/Countdown.cpp new file mode 100644 index 00000000..94e1007e --- /dev/null +++ b/timemanager/Countdown.cpp @@ -0,0 +1,39 @@ +/** + * @file Countdown.cpp + * @brief This file defines the Countdown class. + * @date 21.03.2013 + * @author baetz + */ + + +#include + +Countdown::Countdown(uint32_t initialTimeout) : startTime(0), timeout(initialTimeout) { +} + +Countdown::~Countdown() { +} + +ReturnValue_t Countdown::setTimeout(uint32_t miliseconds) { + ReturnValue_t return_value = OSAL::getUptime( &startTime ); + timeout = miliseconds; + return return_value; +} + +bool Countdown::hasTimedOut() const { + uint32_t current_time; + OSAL::getUptime( ¤t_time ); + if ( uint32_t(current_time - startTime) >= timeout) { + return true; + } else { + return false; + } +} + +bool Countdown::isBusy() const { + return !hasTimedOut(); +} + +ReturnValue_t Countdown::resetTimer() { + return setTimeout(timeout); +} diff --git a/timemanager/Countdown.h b/timemanager/Countdown.h new file mode 100644 index 00000000..4c9ac43a --- /dev/null +++ b/timemanager/Countdown.h @@ -0,0 +1,29 @@ +/** + * @file Countdown.h + * @brief This file defines the Countdown class. + * @date 21.03.2013 + * @author baetz + */ + +#ifndef COUNTDOWN_H_ +#define COUNTDOWN_H_ + +#include + +class Countdown { +private: + uint32_t startTime; +public: + uint32_t timeout; + Countdown(uint32_t initialTimeout = 0); + ~Countdown(); + ReturnValue_t setTimeout(uint32_t miliseconds); + + bool hasTimedOut() const; + + bool isBusy() const; + + ReturnValue_t resetTimer(); //!< Use last set timeout value and restart timer. +}; + +#endif /* COUNTDOWN_H_ */ diff --git a/timemanager/ReceivesTimeInfoIF.h b/timemanager/ReceivesTimeInfoIF.h new file mode 100644 index 00000000..813c30b2 --- /dev/null +++ b/timemanager/ReceivesTimeInfoIF.h @@ -0,0 +1,32 @@ +/** + * @file ReceivesTimeInfoIF.h + * @brief This file defines the ReceivesTimeInfoIF class. + * @date 26.02.2013 + * @author baetz + */ + +#ifndef RECEIVESTIMEINFOIF_H_ +#define RECEIVESTIMEINFOIF_H_ + +#include +/** + * This is a Interface for classes that receive timing information + * with the help of a dedicated message queue. + */ +class ReceivesTimeInfoIF { +public: + /** + * Returns the id of the queue which receives the timing information. + * @return Queue id of the timing queue. + */ + virtual MessageQueueId_t getTimeReceptionQueue() = 0; + /** + * Empty virtual destructor. + */ + virtual ~ReceivesTimeInfoIF() { + } + +}; + + +#endif /* RECEIVESTIMEINFOIF_H_ */ diff --git a/timemanager/TimeMessage.cpp b/timemanager/TimeMessage.cpp new file mode 100644 index 00000000..d2fe02b7 --- /dev/null +++ b/timemanager/TimeMessage.cpp @@ -0,0 +1,30 @@ +/** + * @file TimeMessage.cpp + * @brief This file defines the TimeMessage class. + * @date 26.02.2013 + * @author baetz + */ + +#include + +TimeMessage::TimeMessage() { + this->messageSize += sizeof(timeval); +} + +TimeMessage::TimeMessage(timeval setTime) { + memcpy (this->getData(), &setTime, sizeof(timeval)); + this->messageSize += sizeof(timeval); +} + +TimeMessage::~TimeMessage() { +} + +timeval TimeMessage::getTime() { + timeval temp; + memcpy( &temp, this->getData(), sizeof(timeval)); + return temp; +} + +size_t TimeMessage::getMinimumMessageSize() { + return this->MAX_SIZE; +} diff --git a/timemanager/TimeMessage.h b/timemanager/TimeMessage.h new file mode 100644 index 00000000..2e7397d5 --- /dev/null +++ b/timemanager/TimeMessage.h @@ -0,0 +1,48 @@ +/** + * @file TimeMessage.h + * @brief This file defines the TimeMessage class. + * @date 26.02.2013 + * @author baetz + */ + +#ifndef TIMEMESSAGE_H_ +#define TIMEMESSAGE_H_ + +#include + +class TimeMessage : public MessageQueueMessage { +protected: + /** + * @brief This call always returns the same fixed size of the message. + * @return Returns HEADER_SIZE + \c sizeof(timeval). + */ + size_t getMinimumMessageSize(); +public: + + /** + * @ brief the size of a TimeMessage + */ + static const uint32_t MAX_SIZE = HEADER_SIZE + sizeof(timeval); + + /** + * @brief In the default constructor, only the message_size is set. + */ + TimeMessage(); + /** + * @brief With this constructor, the passed time information is directly put + * into the message. + * @param setTime The time information to put into the message. + */ + TimeMessage( timeval setTime ); + /** + * @brief The class's destructor is empty. + */ + ~TimeMessage(); + /** + * @brief This getter returns the time information in timeval format. + * @return Returns the time stored in this packet. + */ + timeval getTime(); +}; + +#endif /* TIMEMESSAGE_H_ */ diff --git a/timemanager/TimeStamperIF.h b/timemanager/TimeStamperIF.h new file mode 100644 index 00000000..b4f1b121 --- /dev/null +++ b/timemanager/TimeStamperIF.h @@ -0,0 +1,22 @@ +/* + * TimeStamperIF.h + * + * Created on: 31.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_TIMEMANAGER_TIMESTAMPERIF_H_ +#define FRAMEWORK_TIMEMANAGER_TIMESTAMPERIF_H_ + +#include + +class TimeStamperIF { +public: + static const uint8_t MISSION_TIMESTAMP_SIZE = 8; //!< This is a mission-specific constant and determines the total size reserved for timestamps. + virtual ReturnValue_t addTimeStamp(uint8_t* buffer, const uint8_t maxSize) = 0; + virtual ~TimeStamperIF() {} +}; + + + +#endif /* FRAMEWORK_TIMEMANAGER_TIMESTAMPERIF_H_ */ diff --git a/tmstorage/TmStore.cpp b/tmstorage/TmStore.cpp new file mode 100644 index 00000000..e771cba8 --- /dev/null +++ b/tmstorage/TmStore.cpp @@ -0,0 +1,704 @@ +/* + * TmStore.cpp + * + * Created on: 05.02.2015 + * Author: baetz + */ + +#include +#include +#include +#include +#include +#include + + +TmStore::TmStore(TmStoreFrontendIF* owner, object_id_t memoryObject, + uint32_t storeAddress, uint32_t size, bool overwriteOld, + uint32_t maxDumpPacketsPerCylce, uint32_t maxDumpedBytesPerCylce, + uint32_t updateRemotePtrsMs, uint32_t chunkSize) : + owner(owner), eventProxy(NULL), info(&oldestPacket, &newestPacket), infoSize( + info.getSerializedSize()), ring(storeAddress + infoSize, + size - infoSize, overwriteOld), state(OFF), writeState( + WRITE_IDLE), readState(READ_IDLE), memoryObject(memoryObject), memoryQueue(), ipcStore( + NULL), timer(), pendingDataToWrite(0), maximumAmountToRead(0), storedPacketCounter(0), pendingStoredPackets(0), splitWrite( + false), splitRead(NO_SPLIT), splitReadTotalSize(0), tmBuffer(chunkSize), dumpBuffer( + NULL), pointerAddress(storeAddress), updateRemotePtrsMs( + updateRemotePtrsMs), maxDumpPacketsPerCycle( + maxDumpPacketsPerCylce), maxDumpedBytesPerCycle(maxDumpedBytesPerCylce), tryToSetStoreInfo(false) { + dumpBuffer = new uint8_t[chunkSize]; +} + +TmStore::~TmStore() { + delete[] dumpBuffer; +} + +ReturnValue_t TmStore::performOperation() { + doStateMachine(); + checkMemoryQueue(); + if (localBufferTimer.hasTimedOut() && (state == READY)) { + sendTmBufferToStore(); + localBufferTimer.setTimeout(LOCAL_BUFFER_TIMEOUT_MS); + } + return RETURN_OK; +} + +ReturnValue_t TmStore::storePacket(TmPacketMinimal* tmPacket) { + if (tmPacket->getFullSize() > chunkSize()) { + return TOO_LARGE; + } + ReturnValue_t result = RETURN_FAILED; + if (hasEnoughSpaceFor(tmPacket->getFullSize())) { + localBufferTimer.setTimeout(LOCAL_BUFFER_TIMEOUT_MS); + result = storeOrForwardPacket(tmPacket->getWholeData(), + tmPacket->getFullSize()); + } else { + result = handleFullStore(tmPacket->getWholeData(), + tmPacket->getFullSize()); + } + if (result == RETURN_OK) { + pendingNewestPacket.setContent(tmPacket); + if (!pendingOldestPacket.isValid()) { + pendingOldestPacket.setContent(tmPacket); + } + pendingStoredPackets++; + } + return result; +} + +ReturnValue_t TmStore::fetchPackets(bool useAddress, uint32_t startAtAddress) { + if (!isReady()) { + return NOT_READY; + } + if (ring.isEmpty()) { + return EMPTY; + } + if (readState != READ_IDLE) { + return BUSY; + } + // Never download more than the currently available data. + maximumAmountToRead = ring.availableReadData(); + if (useAddress) { + ring.setRead(startAtAddress, TEMP_READ_PTR); + } else { + ring.setRead(ring.getRead(READ_PTR), TEMP_READ_PTR); + } + ReturnValue_t result = requestChunkOfData(); + if (result != RETURN_OK) { + return result; + } + setReadState(DUMPING_PACKETS); + return RETURN_OK; +} + +ReturnValue_t TmStore::requestStoreInfo() { + CommandMessage message; + MemoryMessage::setMemoryDumpCommand(&message, pointerAddress, infoSize); + ReturnValue_t result = memoryQueue.sendToDefault(&message); + if (result != RETURN_OK) { + return result; + } + return RETURN_OK; +} + +ReturnValue_t TmStore::setStoreInfo() { + store_address_t storeId; + uint8_t* data = NULL; + uint32_t size = 0; + ReturnValue_t result = ipcStore->getFreeElement(&storeId, infoSize, &data); + if (result != RETURN_OK) { + return result; + } + info.setContent(ring.getRead(), ring.getWrite(), storedPacketCounter); + result = info.serialize(&data, &size, infoSize, true); + if (result != RETURN_OK) { + ipcStore->deleteData(storeId); + return result; + } + CommandMessage message; + MemoryMessage::setMemoryLoadCommand(&message, pointerAddress, storeId); + result = memoryQueue.sendToDefault(&message); + if (result != RETURN_OK) { + ipcStore->deleteData(storeId); + return result; + } + return RETURN_OK; +} + +void TmStore::doStateMachine() { + switch (state) { + case OFF: + break; + case STARTUP: { + ReturnValue_t result = requestStoreInfo(); + if (result == RETURN_OK) { + setState(FETCH_STORE_INFORMATION); + setReadState(FETCHING_STORE_INFO); + timer.setTimeout(DEFAULT_TIMEOUT_MS); + } else { + eventProxy->forwardEvent(STORE_INIT_FAILED, result, 0); + setState(OFF); + } + } + break; + case FETCH_STORE_INFORMATION: + if (timer.hasTimedOut()) { + eventProxy->forwardEvent(STORE_INIT_FAILED, TIMEOUT, 1); + setState(OFF); + } + break; + case READY: + if (tryToSetStoreInfo) { + if ((writeState == WRITE_IDLE) && (readState == READ_IDLE)) { + if (setStoreInfo() == RETURN_OK) { + setWriteState(SETTING_STORE_INFO); + } + tryToSetStoreInfo = false; + } + } + break; + default: + //do nothing. + break; + } +} + +void TmStore::checkMemoryQueue() { + CommandMessage message; + for (ReturnValue_t result = memoryQueue.receiveMessage(&message); + result == RETURN_OK; + result = memoryQueue.receiveMessage(&message)) { + switch (message.getCommand()) { + case CommandMessage::REPLY_COMMAND_OK: + handleLoadSuccess(); + break; + case MemoryMessage::REPLY_MEMORY_DUMP: + handleDump(MemoryMessage::getStoreID(&message)); + break; + case MemoryMessage::REPLY_MEMORY_FAILED: + case CommandMessage::REPLY_REJECTED: // REPLY_REJECTED uses the same protocol. + switch (MemoryMessage::getInitialCommand(&message)) { + case MemoryMessage::CMD_MEMORY_LOAD: + handleLoadFailed(MemoryMessage::getErrorCode(&message)); + break; + case MemoryMessage::CMD_MEMORY_DUMP: + handleDumpFailed(MemoryMessage::getErrorCode(&message)); + break; + default: + debug << "TmStore: Unknown InitialCommand: " + << MemoryMessage::getInitialCommand(&message) + << std::endl; + break; + } + break; + default: + eventProxy->forwardEvent(UNEXPECTED_MSG, 0, 0); + break; + } + } +} + +void TmStore::handleLoadSuccess() { + switch (writeState) { + case SETTING_STORE_INFO: + setWriteState(WRITE_IDLE); + break; + case SENDING_PACKETS: + if (splitWrite) { + ReturnValue_t result = sendRemainingTmPiece(); + if (result != RETURN_OK) { + eventProxy->forwardEvent(STORE_SEND_WRITE_FAILED, result, 0); + resetStore(true); + pendingDataToWrite = 0; + } + splitWrite = false; + } else { + ring.writeData(pendingDataToWrite); + pendingDataToWrite = 0; + storedPacketCounter += pendingStoredPackets; + newestPacket.setContent(&pendingNewestPacket); + if (!oldestPacket.isValid()) { + oldestPacket.setContent(&pendingOldestPacket); + } + pendingStoredPackets = 0; + setWriteState(WRITE_IDLE); + tryToSetStoreInfo = true; + } + break; + default: + eventProxy->forwardEvent(UNEXPECTED_MSG, 0, 1); + } +} + +void TmStore::handleDump(store_address_t storeId) { + const uint8_t* buffer = NULL; + uint32_t size = 0; + ReturnValue_t result = ipcStore->getData(storeId, &buffer, &size); + if (result != RETURN_OK) { + return; + } + switch (readState) { + case FETCHING_STORE_INFO: + readStoreInfo(buffer, size); + break; + case DUMPING_PACKETS: + case DELETING_OLD: + case DELETING_MORE: + handleSplitRead(buffer, size); + break; + default: + setReadState(READ_IDLE); + //No break. + case READ_IDLE: + eventProxy->forwardEvent(UNEXPECTED_MSG, 0, 2); + break; + } + ipcStore->deleteData(storeId); +} + +void TmStore::handleSplitRead(const uint8_t* buffer, uint32_t size) { + switch (splitRead) { + case NO_SPLIT: + if (readState == DELETING_OLD || readState == DELETING_MORE) { + deleteOld(buffer, size); + } else { + dumpPackets(buffer, size); + } + break; + case SPLIT_STARTED: + if (size <= chunkSize()) { + ReturnValue_t result = requestRemainingDumpPiece(size); + if (result == RETURN_OK) { + memcpy(dumpBuffer, buffer, size); + splitRead = ONE_RECEIVED; + } else { + eventProxy->forwardEvent(STORE_SEND_READ_FAILED, result, 0); + splitRead = NO_SPLIT; + maximumAmountToRead = 0; + } + } else { + eventProxy->forwardEvent(STORE_SEND_READ_FAILED, TOO_LARGE, 0); + splitRead = NO_SPLIT; + maximumAmountToRead = 0; + } + break; + case ONE_RECEIVED: + memcpy(&dumpBuffer[splitReadTotalSize - size], buffer, size); + if (readState == DELETING_OLD || readState == DELETING_MORE) { + deleteOld(dumpBuffer, splitReadTotalSize); + } else { + dumpPackets(dumpBuffer, splitReadTotalSize); + } + splitRead = NO_SPLIT; + break; + } +} + +void TmStore::handleLoadFailed(ReturnValue_t errorCode) { + eventProxy->forwardEvent(STORE_WRITE_FAILED, errorCode, 0); + setWriteState(WRITE_IDLE); + setState(READY); + splitWrite = false; + pendingDataToWrite = 0; +} + +void TmStore::handleDumpFailed(ReturnValue_t errorCode) { + switch (readState) { + case FETCHING_STORE_INFO: + eventProxy->forwardEvent(STORE_INIT_FAILED, errorCode, 2); + setState(OFF); + break; + case DUMPING_PACKETS: + owner->handleRetrievalFailed(errorCode); + //No break + default: + case READ_IDLE: + case DELETING_OLD: + case DELETING_MORE: + eventProxy->forwardEvent(STORE_READ_FAILED, errorCode, 0); + setState(READY); + break; + } + setReadState(READ_IDLE); + splitRead = NO_SPLIT; + maximumAmountToRead = 0; +} + +void TmStore::readStoreInfo(const uint8_t* buffer, uint32_t size) { + setReadState(READ_IDLE); + if (::Calculate_CRC(buffer, infoSize) != 0) { + eventProxy->forwardEvent(STORE_INIT_EMPTY, 0, 0); + setState(READY); + return; + } + int32_t deSize = size; + ReturnValue_t result = info.deSerialize(&buffer, &deSize, true); + if (result != RETURN_OK) { + //An error here is extremely unlikely. + eventProxy->forwardEvent(STORE_INIT_FAILED, result, 3); + setState(OFF); + return; + } + eventProxy->forwardEvent(STORE_INIT_DONE); + ring.setWrite(info.writeP); + ring.setRead(info.readP); + storedPacketCounter = info.storedPacketCount; + setState(READY); +} + +void TmStore::dumpPackets(const uint8_t* buffer, uint32_t size) { + ReturnValue_t result = RETURN_OK; + uint32_t tempSize = 0; + uint32_t dumpedPacketCounter = 0; + uint32_t dumpedBytesCounter = 0; + while (size >= TmPacketMinimal::MINIMUM_SIZE + && dumpedPacketCounter < maxDumpPacketsPerCycle + && dumpedBytesCounter < maxDumpedBytesPerCycle + && result == RETURN_OK) { + TmPacketMinimal packet(buffer); + tempSize = packet.getFullSize(); + if (tempSize > size) { + if (tempSize > ring.availableReadData(READ_PTR) || tempSize > chunkSize()) { + owner->handleRetrievalFailed(DUMP_ERROR); + eventProxy->forwardEvent(STORE_CONTENT_CORRUPTED, 0, tempSize); + result = DUMP_ERROR; + } + break; + } + size -= tempSize; + buffer += tempSize; + dumpedBytesCounter += tempSize; + if (maximumAmountToRead > tempSize) { + result = owner->packetRetrieved(&packet, + ring.getRead(TEMP_READ_PTR)); + maximumAmountToRead -= tempSize; + } else { + //packet is complete and last. + result = owner->packetRetrieved(&packet, + ring.getRead(TEMP_READ_PTR), true); + maximumAmountToRead = 0; + break; + } + if (result == RETURN_OK) { + ring.readData(tempSize, TEMP_READ_PTR); + dumpedPacketCounter++; + } + } + if (result == RETURN_OK && maximumAmountToRead != 0) { + result = requestChunkOfData(); + if (result != HasReturnvaluesIF::RETURN_OK) { + owner->handleRetrievalFailed(result); + } + } else { + setReadState(READ_IDLE); + } +} + +ReturnValue_t TmStore::sendDataToTmStore(const uint8_t* data, uint32_t size) { + //Unfortunately, cleanup is quite complex. + store_address_t storeId; + ReturnValue_t result = RETURN_FAILED; + if (size > ring.writeTillWrap()) { + //Two-folded write + result = ipcStore->addData(&storeId, data, ring.writeTillWrap()); + if (result != RETURN_OK) { + return result; + } + splitWrite = true; + result = ipcStore->addData(&splitWriteStoreId, + data + ring.writeTillWrap(), size - ring.writeTillWrap()); + if (result != RETURN_OK) { + ipcStore->deleteData(storeId); + } + } else { + //Normal write + splitWrite = false; + result = ipcStore->addData(&storeId, data, size); + } + if (result != RETURN_OK) { + splitWrite = false; + return result; + } + CommandMessage message; + MemoryMessage::setMemoryLoadCommand(&message, ring.getWrite(), storeId); + if ((result = memoryQueue.sendToDefault(&message)) == RETURN_OK) { + setWriteState(SENDING_PACKETS); + pendingDataToWrite = size; + } else { + if (splitWrite) { + ipcStore->deleteData(splitWriteStoreId); + } + splitWrite = false; + ipcStore->deleteData(storeId); + } + return result; +} + +ReturnValue_t TmStore::requestChunkOfData() { + uint32_t readSize = + (maximumAmountToRead > chunkSize()) ? + chunkSize() : maximumAmountToRead; + CommandMessage message; + if (readSize > ring.readTillWrap(TEMP_READ_PTR)) { + //Wrap + splitReadTotalSize = readSize; + splitRead = SPLIT_STARTED; + MemoryMessage::setMemoryDumpCommand(&message, + ring.getRead(TEMP_READ_PTR), ring.readTillWrap(TEMP_READ_PTR)); + } else { + splitRead = NO_SPLIT; + MemoryMessage::setMemoryDumpCommand(&message, + ring.getRead(TEMP_READ_PTR), readSize); + } + ReturnValue_t result = memoryQueue.sendToDefault(&message); + if (result != RETURN_OK) { + splitRead = NO_SPLIT; + maximumAmountToRead = 0; + } + return result; +} + +ReturnValue_t TmStore::initialize() { + AcceptsMemoryMessagesIF* memoryTarget = objectManager->get< + AcceptsMemoryMessagesIF>(memoryObject); + if (memoryTarget == NULL) { + return RETURN_FAILED; + } + memoryQueue.setDefaultDestination(memoryTarget->getCommandQueue()); + ipcStore = objectManager->get(objects::IPC_STORE); + if (ipcStore == NULL) { + return RETURN_FAILED; + } + //assert that Store is larger than local store and potential data on the way. + if (ring.availableWriteSpace(READ_PTR) < (2 * tmBuffer.maxSize())) { + //Ring buffer is too small. + return RETURN_FAILED; + } + //Ugly cast to have the correct link to eventProxy: + eventProxy = dynamic_cast(owner); + if (eventProxy == NULL) { + return RETURN_FAILED; + } + return RETURN_OK; +} + +ReturnValue_t TmStore::initializeStore() { + setState(STARTUP); + eventProxy->forwardEvent(STORE_INITIALIZE, 0, 0); + return RETURN_OK; +} + +bool TmStore::isReady() { + return (state == READY); +} + +ReturnValue_t TmStore::sendRemainingTmPiece() { + if (writeState != SENDING_PACKETS) { + return RETURN_FAILED; + } + CommandMessage message; + MemoryMessage::setMemoryLoadCommand(&message, ring.getStart(), + splitWriteStoreId); + return memoryQueue.sendToDefault(&message); +} + +ReturnValue_t TmStore::requestRemainingDumpPiece(uint32_t firstPartSize) { + CommandMessage message; + MemoryMessage::setMemoryDumpCommand(&message, ring.getStart(), + splitReadTotalSize - firstPartSize); + return memoryQueue.sendToDefault(&message); +} + +uint32_t TmStore::availableData() { + return ring.availableReadData(); +} + +ReturnValue_t TmStore::storeOrForwardPacket(const uint8_t* data, + uint32_t size) { + if (tmBuffer.remaining() >= size) { + memcpy(&tmBuffer[tmBuffer.size], data, size); + tmBuffer.size += size; + return RETURN_OK; + } else { + if (writeState != WRITE_IDLE) { + return BUSY; + } + if (!isReady()) { + return NOT_READY; + } + sendTmBufferToStore(); + memcpy(tmBuffer.front(), data, size); + tmBuffer.size += size; + return RETURN_OK; + } +} + +bool TmStore::hasEnoughSpaceFor(uint32_t size) { + //Correct size configuration is asserted in initialize(). + if ((ring.availableWriteSpace(READ_PTR) - tmBuffer.size - pendingDataToWrite) + > size) { + return true; + } else { + return false; + } +} + +ReturnValue_t TmStore::deleteContent(bool deletePart, uint32_t upToAddress, + uint32_t nDeletedPackets, TmPacketMinimal* newOldestPacket) { + if (deletePart) { + ring.setRead(upToAddress, READ_PTR); + storedPacketCounter -= nDeletedPackets; + if (newOldestPacket != NULL) { + oldestPacket.setContent(newOldestPacket); + } + } else { + resetStore(); + } + tryToSetStoreInfo = true; + return RETURN_OK; +} + +ReturnValue_t TmStore::handleFullStore(const uint8_t* data, uint32_t size) { + if (!isReady()) { + return NOT_READY; + } + if (!ring.overwritesOld()) { + sendTmBufferToStore(); + return FULL; + } + ReturnValue_t result = FULL; + if ((writeState == WRITE_IDLE) && (readState == READ_IDLE)) { + sendTmBufferToStore(); + maximumAmountToRead = chunkSize(); + ring.setRead(ring.getRead(READ_PTR), TEMP_READ_PTR); + result = requestChunkOfData(); + if (result != RETURN_OK) { + return result; + } + setReadState(DELETING_OLD); + //Store data in cleared store. + memcpy(tmBuffer.front(), data, size); + tmBuffer.size += size; + result = RETURN_OK; + } else if (readState == DELETING_OLD) { + //Try to store local as long as possible + if (tmBuffer.remaining() >= size) { + memcpy(&tmBuffer[tmBuffer.size], data, size); + tmBuffer.size += size; + result = RETURN_OK; + } + } + return result; +} + +void TmStore::deleteOld(const uint8_t* buffer, uint32_t size) { + ReturnValue_t result = RETURN_OK; + uint32_t tempSize = 0; + while (size >= TmPacketMinimal::MINIMUM_SIZE && result == RETURN_OK) { + TmPacketMinimal packet(buffer); + tempSize = packet.getFullSize(); + if (readState == DELETING_MORE) { + if (size < (TmPacketMinimal::MINIMUM_SIZE + tempSize)) { + oldestPacket.setContent(&packet); + break; + } + } else { + if (tempSize > size) { + //Don't delete the last packet half in store... + break; + } + } + result = ring.readData(tempSize, READ_PTR); + ring.readData(tempSize, TEMP_READ_PTR); + size -= tempSize; + buffer += tempSize; + storedPacketCounter--; + } + if (readState == DELETING_OLD) { + result = requestChunkOfData(); + if (result != HasReturnvaluesIF::RETURN_OK) { + oldestPacket.reset(); + eventProxy->forwardEvent(STORE_SEND_WRITE_FAILED, result, 1); + setReadState(READ_IDLE); + } else { + setReadState(DELETING_MORE); + } + } else { + setReadState(READ_IDLE); + owner->restDownlinkedPacketCount(); + tryToSetStoreInfo = true; + } +} + +void TmStore::sendTmBufferToStore() { + //No need to send empty data packet. + if (tmBuffer.size > 0) { + ReturnValue_t result = sendDataToTmStore(tmBuffer.front(), tmBuffer.size); + if (result != HasReturnvaluesIF::RETURN_OK) { + eventProxy->forwardEvent(STORE_SEND_WRITE_FAILED, result, 2); + } + tmBuffer.clear(); + } +} + +void TmStore::resetStore(bool resetWrite, bool resetRead) { + ring.clear(); + setState(READY); + storedPacketCounter = 0; + clearPending(); + tmBuffer.clear(); + oldestPacket.reset(); + newestPacket.reset(); + tryToSetStoreInfo = true; + if (resetWrite) { + setWriteState(WRITE_IDLE); + splitWrite = false; + pendingDataToWrite = 0; + } + if (resetRead) { + maximumAmountToRead = 0; + splitRead = NO_SPLIT; + setReadState(READ_IDLE); + } +} + +void TmStore::setState(State state) { +// debug << "TmStore::setState: " << state << std::endl; + this->state = state; +} + +void TmStore::setWriteState(WriteState writeState) { +// debug << "TmStore::setWriteState: " << writeState << std::endl; + this->writeState = writeState; +} + +void TmStore::setReadState(ReadState readState) { +// debug << "TmStore::setReadState: " << readState << std::endl; + this->readState = readState; +} + +float TmStore::getPercentageFilled() const { + return ring.availableReadData(READ_PTR) / float(ring.maxSize()); +} + +uint32_t TmStore::getStoredPacketsCount() const { + return storedPacketCounter; +} + +TmPacketInformation* TmStore::getOldestPacket() { + return &oldestPacket; +} + +TmPacketInformation* TmStore::getYoungestPacket() { + return &newestPacket; +} + +uint32_t TmStore::chunkSize() { + return tmBuffer.maxSize(); +} + +void TmStore::clearPending() { + pendingOldestPacket.reset(); + pendingNewestPacket.reset(); + pendingStoredPackets = 0; +} diff --git a/tmstorage/TmStore.h b/tmstorage/TmStore.h new file mode 100644 index 00000000..b56ad67d --- /dev/null +++ b/tmstorage/TmStore.h @@ -0,0 +1,147 @@ +/* + * TmStore.h + * + * Created on: 05.02.2015 + * Author: baetz + */ + +#ifndef PLATFORM_TMTCSERVICES_TMSTORE_H_ +#define PLATFORM_TMTCSERVICES_TMSTORE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class TmStore: public HasReturnvaluesIF, public TmStoreBackendIF { +public: + static const uint32_t DEFAULT_TIMEOUT_MS = 3000; + static const uint32_t UPDATE_REMOTE_PTRS_DEFAULT_MS = 20000; + static const uint32_t MAX_DUMP_PACKETS_PER_CYCLE_DEFAULT = 16; + static const uint32_t MAX_DUMPED_BYTES_PER_CYCLE_DEFAULT = 1024; + TmStore(TmStoreFrontendIF* owner, object_id_t memoryObject, + uint32_t storeAddress, uint32_t size, bool overwriteOld = false, + uint32_t maxDumpPacketsPerCylce = MAX_DUMP_PACKETS_PER_CYCLE_DEFAULT, + uint32_t maxDumpedBytesPerCycle = MAX_DUMPED_BYTES_PER_CYCLE_DEFAULT, + uint32_t updateRemotePtrsMs = UPDATE_REMOTE_PTRS_DEFAULT_MS, + uint32_t chunkSize = CHUNK_SIZE_DEFAULT); + virtual ~TmStore(); + ReturnValue_t performOperation(); + ReturnValue_t storePacket(TmPacketMinimal* tmPacket); + ReturnValue_t fetchPackets(bool useAddress = false, + uint32_t startAtAddress = 0); + uint32_t availableData(); + ReturnValue_t deleteContent(bool deletePart = false, uint32_t upToAddress = + 0, uint32_t nDeletedPackets = 0, TmPacketMinimal* newOldestPacket = + NULL); + ReturnValue_t initialize(); + ReturnValue_t initializeStore(); + bool isReady(); + void resetStore(bool resetWrite = false, bool resetRead = false); + float getPercentageFilled() const; + uint32_t getStoredPacketsCount() const; + TmPacketInformation* getOldestPacket(); + TmPacketInformation* getYoungestPacket(); +private: + TmStoreFrontendIF* owner; + EventReportingProxyIF* eventProxy; + TmPacketInformation pendingOldestPacket; + TmPacketInformation pendingNewestPacket; + TmPacketInformation oldestPacket; + TmPacketInformation newestPacket; + TmStoreInfo info; + const uint32_t infoSize; + RingBufferBase<2> ring; + enum State { + OFF = 0, + STARTUP = 1, + FETCH_STORE_INFORMATION = 2, + STORE_INFORMATION_RECEIVED = 3, + READY = 4 + }; + State state; + enum WriteState { + WRITE_IDLE = 0, SETTING_STORE_INFO = 1, SENDING_PACKETS = 2, + }; + WriteState writeState; + enum ReadState { + READ_IDLE = 0, + DELETING_OLD = 1, + DUMPING_PACKETS = 2, + FETCHING_STORE_INFO = 3, + DELETING_MORE = 4 + }; + ReadState readState; + object_id_t memoryObject; + MessageQueue memoryQueue; + StorageManagerIF* ipcStore; + Countdown timer; + uint32_t pendingDataToWrite; + uint32_t maximumAmountToRead; + uint32_t storedPacketCounter; + uint32_t pendingStoredPackets; + bool splitWrite; + store_address_t splitWriteStoreId; + enum SplitReadState { + NO_SPLIT, SPLIT_STARTED, ONE_RECEIVED + }; + SplitReadState splitRead; + uint32_t splitReadTotalSize; + ArrayList tmBuffer; + uint8_t* dumpBuffer; + const uint32_t pointerAddress; + const uint32_t updateRemotePtrsMs; + const uint32_t maxDumpPacketsPerCycle; + const uint32_t maxDumpedBytesPerCycle; + Countdown localBufferTimer; + bool tryToSetStoreInfo; + static const uint8_t READ_PTR = 0; + static const uint8_t TEMP_READ_PTR = 1; + static const uint32_t LOCAL_BUFFER_TIMEOUT_MS = 5000; + static const uint32_t CHUNK_SIZE_DEFAULT = 2048; + void checkMemoryQueue(); + ReturnValue_t requestStoreInfo(); + ReturnValue_t setStoreInfo(); + void readStoreInfo(const uint8_t* buffer, uint32_t size); + void dumpPackets(const uint8_t* buffer, uint32_t size); + /** + * Deletes old packets to free space in the store for new packets. + * Two chunks of data are requested to definitely delete mored than one + * chunk. Deletion is stopped when the last full packet is found, to + * be able to remember the oldest packet in store. + * @param buffer Data containing the old packets. + * @param size Size of the data chunk. + */ + void deleteOld(const uint8_t* buffer, uint32_t size); + void doStateMachine(); + void handleLoadSuccess(); + void handleDump(store_address_t storeId); + void handleLoadFailed(ReturnValue_t errorCode); + void handleDumpFailed(ReturnValue_t errorCode); + ReturnValue_t sendDataToTmStore(const uint8_t* data, uint32_t size); + ReturnValue_t sendRemainingTmPiece(); + ReturnValue_t requestChunkOfData(); + ReturnValue_t requestRemainingDumpPiece(uint32_t firstPartSize); + ReturnValue_t storeOrForwardPacket(const uint8_t* data, uint32_t size); + ReturnValue_t handleFullStore(const uint8_t* data, uint32_t size); + bool hasEnoughSpaceFor(uint32_t size); + void sendTmBufferToStore(); + void handleSplitRead(const uint8_t* buffer, uint32_t size); + void setState(State state); + void setReadState(ReadState readState); + void setWriteState(WriteState writeState); + uint32_t chunkSize(); + void clearPending(); +}; + +#endif /* PLATFORM_TMTCSERVICES_TMSTORE_H_ */ diff --git a/tmstorage/TmStoreBackendIF.h b/tmstorage/TmStoreBackendIF.h new file mode 100644 index 00000000..8c1c1ef0 --- /dev/null +++ b/tmstorage/TmStoreBackendIF.h @@ -0,0 +1,60 @@ +/* + * TmStoreBackendIF.h + * + * Created on: 18.02.2015 + * Author: baetz + */ + +#ifndef PLATFORM_TMTCSERVICES_TMSTOREBACKENDIF_H_ +#define PLATFORM_TMTCSERVICES_TMSTOREBACKENDIF_H_ + +#include +class TmPacketInformation; +class TmPacketMinimal; + +class TmStoreBackendIF { +public: + static const uint8_t INTERFACE_ID = TM_STORE_BACKEND_IF; + static const ReturnValue_t BUSY = MAKE_RETURN_CODE(1); + static const ReturnValue_t FULL = MAKE_RETURN_CODE(2); + static const ReturnValue_t EMPTY = MAKE_RETURN_CODE(3); + static const ReturnValue_t NULL_REQUESTED = MAKE_RETURN_CODE(4); + static const ReturnValue_t TOO_LARGE = MAKE_RETURN_CODE(5); + static const ReturnValue_t NOT_READY = MAKE_RETURN_CODE(6); + static const ReturnValue_t DUMP_ERROR = MAKE_RETURN_CODE(7); + static const ReturnValue_t CRC_ERROR = MAKE_RETURN_CODE(8); + static const ReturnValue_t TIMEOUT = MAKE_RETURN_CODE(9); + + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MEMORY; + static const Event STORE_SEND_WRITE_FAILED = MAKE_EVENT(0, SEVERITY::LOW); //!< Initiating sending data to store failed. Low, par1: returnCode, par2: integer (debug info) + static const Event STORE_WRITE_FAILED = MAKE_EVENT(1, SEVERITY::LOW); //!< Data was sent, but writing failed. Low, par1: returnCode, par2: 0 + static const Event STORE_SEND_READ_FAILED = MAKE_EVENT(2, SEVERITY::LOW); //!< Initiating reading data from store failed. Low, par1: returnCode, par2: 0 + static const Event STORE_READ_FAILED = MAKE_EVENT(3, SEVERITY::LOW); //!< Data was requested, but access failed. Low, par1: returnCode, par2: 0 + static const Event UNEXPECTED_MSG = MAKE_EVENT(4, SEVERITY::INFO); //!< An unexpected TM packet or data message occurred. Info, par1: 0, par2: integer (debug info) + static const Event STORING_FAILED = MAKE_EVENT(5, SEVERITY::LOW); //!< Storing data failed. May simply be a full store. Low, par1: returnCode, par2: integer (sequence count of failed packet). + static const Event TM_DUMP_FAILED = MAKE_EVENT(6, SEVERITY::LOW); //!< Dumping retrieved data failed. Low, par1: returnCode, par2: integer (sequence count of failed packet). + static const Event STORE_INIT_FAILED = MAKE_EVENT(7, SEVERITY::LOW); //!< Corrupted init data or read error. Low, par1: returnCode, par2: integer (debug info) + static const Event STORE_INIT_EMPTY = MAKE_EVENT(8, SEVERITY::INFO); //!< Store was not initialized. Starts empty. Info, parameters both zero. + static const Event STORE_CONTENT_CORRUPTED = MAKE_EVENT(9, SEVERITY::LOW); //!< Data was read out, but it is inconsistent. Low par1: 0, par2: integer (debug info) + static const Event STORE_INITIALIZE = MAKE_EVENT(10, SEVERITY::INFO); //!< Info event indicating the store will be initialized, either at boot or after IOB switch. Info. pars: 0 + static const Event STORE_INIT_DONE = MAKE_EVENT(11, SEVERITY::INFO); //!< Info event indicating the store was successfully initialized, either at boot or after IOB switch. Info. pars: 0 + + virtual ~TmStoreBackendIF() {} + virtual ReturnValue_t performOperation() = 0; + virtual ReturnValue_t initialize() = 0; + virtual ReturnValue_t storePacket(TmPacketMinimal* tmPacket) = 0; + virtual ReturnValue_t fetchPackets(bool useAddress = false, uint32_t startAtAddress = 0) = 0; + virtual ReturnValue_t deleteContent(bool deletePart = false, uint32_t upToAddress = 0, uint32_t nDeletedPackets = 0, TmPacketMinimal* newOldestPacket = NULL) = 0; + virtual ReturnValue_t initializeStore() = 0; + virtual void resetStore(bool resetWrite = false, bool resetRead = false) = 0; + virtual bool isReady() = 0; + virtual uint32_t availableData() = 0; + virtual float getPercentageFilled() const = 0; + virtual uint32_t getStoredPacketsCount() const = 0; + virtual TmPacketInformation* getOldestPacket() = 0; + virtual TmPacketInformation* getYoungestPacket() = 0; +}; + + + +#endif /* PLATFORM_TMTCSERVICES_TMSTOREBACKENDIF_H_ */ diff --git a/tmstorage/TmStoreFrontendIF.h b/tmstorage/TmStoreFrontendIF.h new file mode 100644 index 00000000..cd0f606a --- /dev/null +++ b/tmstorage/TmStoreFrontendIF.h @@ -0,0 +1,58 @@ +/* + * TmStoreFrontendIF.h + * + * Created on: 19.02.2015 + * Author: baetz + */ + +#ifndef PLATFORM_TMTCSERVICES_TMSTOREFRONTENDIF_H_ +#define PLATFORM_TMTCSERVICES_TMSTOREFRONTENDIF_H_ + +#include +#include +class TmPacketMinimal; +class SpacePacketBase; +class TmStoreBackendIF; + +class TmStoreFrontendIF { +public: + struct ApidSsc { + uint16_t apid; + uint16_t ssc; + }; + virtual TmStoreBackendIF* getBackend() const = 0; + virtual ReturnValue_t performOperation() = 0; + /** + * Callback from the back-end to indicate a certain packet was received. + * front-end takes care of discarding/downloading the packet. + * @param packet Pointer to the newly received Space Packet. + * @param address Start address of the packet found + * @param isLastPacket Indicates if no more packets can be fetched. + * @return If more packets shall be fetched, RETURN_OK must be returned. + * Any other code stops fetching packets. + */ + virtual ReturnValue_t packetRetrieved(TmPacketMinimal* packet, uint32_t address, bool isLastPacket = false) = 0; + virtual void handleRetrievalFailed(ReturnValue_t errorCode, uint32_t parameter1 = 0, uint32_t parameter2 = 0) = 0; + /** + * To get the queue where commands shall be sent. + * @return Id of command queue. + */ + virtual MessageQueueId_t getCommandQueue() = 0; + virtual ReturnValue_t fetchPackets(ApidSsc start, ApidSsc end) = 0; + virtual ReturnValue_t deletePackets(ApidSsc upTo) = 0; + virtual ReturnValue_t checkPacket(SpacePacketBase* tmPacket) = 0; + virtual bool isEnabled() const = 0; + virtual void setEnabled(bool enabled) = 0; + virtual void restDownlinkedPacketCount() = 0; + static const uint8_t INTERFACE_ID = TM_STORE_FRONTEND_IF; + static const ReturnValue_t BUSY = MAKE_RETURN_CODE(1); + static const ReturnValue_t LAST_PACKET_FOUND = MAKE_RETURN_CODE(2); + static const ReturnValue_t STOP_FETCH = MAKE_RETURN_CODE(3); + static const ReturnValue_t TIMEOUT = MAKE_RETURN_CODE(4); + virtual ~TmStoreFrontendIF() { + } +}; + + + +#endif /* PLATFORM_TMTCSERVICES_TMSTOREFRONTENDIF_H_ */ diff --git a/tmstorage/TmStoreInfo.h b/tmstorage/TmStoreInfo.h new file mode 100644 index 00000000..5df7d29b --- /dev/null +++ b/tmstorage/TmStoreInfo.h @@ -0,0 +1,67 @@ +/* + * TmStoreInfo.h + * + * Created on: 19.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_TMSTORAGE_TMSTOREINFO_H_ +#define FRAMEWORK_TMSTORAGE_TMSTOREINFO_H_ + +#include +#include + +class TmStoreInfo: public SerializeIF { +public: + SerializeElement readP; + SerializeElement writeP; + SerializeElement storedPacketCount; + LinkedElement linkedOldestPacket; + LinkedElement linkedYoungestPacket; + SerializeElement crc; + TmStoreInfo(SerializeIF* oldestPacket, SerializeIF* youngestPacket) : + readP(0), writeP(0), linkedOldestPacket(oldestPacket), linkedYoungestPacket( + youngestPacket) { + this->readP.setNext(&this->writeP); + this->writeP.setNext(&this->storedPacketCount); + storedPacketCount.setNext(&linkedOldestPacket); + linkedOldestPacket.setNext(&linkedYoungestPacket); + } + void setContent(uint32_t readP, uint32_t writeP, uint32_t storedPackets) { + this->readP = readP; + this->writeP = writeP; + this->storedPacketCount = storedPackets; + } + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + uint8_t* startBuffer = *buffer; + ReturnValue_t result = SerialLinkedListAdapter::serialize( + &readP, buffer, size, max_size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + uint32_t elementSize = + SerialLinkedListAdapter::getSerializedSize(&readP); + SerializeElement localCrc; + localCrc.entry = ::Calculate_CRC(startBuffer, elementSize); + return localCrc.serialize(buffer, size, max_size, bigEndian); + } + + uint32_t getSerializedSize() const { + return (SerialLinkedListAdapter::getSerializedSize(&readP) + + crc.getSerializedSize()); + } + + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + ReturnValue_t result = + SerialLinkedListAdapter::deSerialize(&readP, + buffer, size, bigEndian); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return crc.deSerialize(buffer, size, bigEndian); + } +}; + +#endif /* FRAMEWORK_TMSTORAGE_TMSTOREINFO_H_ */ diff --git a/tmstorage/TmStoreManager.cpp b/tmstorage/TmStoreManager.cpp new file mode 100644 index 00000000..61891fa6 --- /dev/null +++ b/tmstorage/TmStoreManager.cpp @@ -0,0 +1,511 @@ +/* + * TmStoreManager.cpp + * + * Created on: 18.02.2015 + * Author: baetz + */ + +#include +#include +#include +#include +#include + +TmStoreManager::TmStoreManager(object_id_t objectId, object_id_t setDumpTarget, + uint8_t setVC, uint32_t setTimeoutMs) : + SystemObject(objectId), backend(NULL), tmForwardStore(NULL), dumpTarget( + setDumpTarget), virtualChannel(setVC), fetchState( + NOTHING_FETCHED), addressOfFetchCandidate(0), deletionStarted( + false), lastAddressToDelete(0), pendingPacketsToDelete(0), state(IDLE), storingEnabled( + false), timeoutMs(setTimeoutMs), ipcStore(NULL), downlinkedPacketsCount( + 0), fullEventThrown(false) { +} + +TmStoreManager::~TmStoreManager() { +} + +ReturnValue_t TmStoreManager::initialize() { + ReturnValue_t result = SystemObject::initialize(); + if (result != RETURN_OK) { + return result; + } + AcceptsTelemetryIF* telemetryDestimation = objectManager->get< + AcceptsTelemetryIF>(dumpTarget); + if (telemetryDestimation == NULL) { + return RETURN_FAILED; + } + tmQueue.setDefaultDestination( + telemetryDestimation->getReportReceptionQueue(virtualChannel)); + tmForwardStore = objectManager->get(objects::TM_STORE); + if (tmForwardStore == NULL) { + return RETURN_FAILED; + } + result = backend->initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + ipcStore = objectManager->get(objects::IPC_STORE); + if (ipcStore == NULL) { + return RETURN_FAILED; + } + return matcher.initialize(); +} + +ReturnValue_t TmStoreManager::fetchPackets(ApidSsc start, ApidSsc end) { + //Check mode, store ids, start fetching. + if (state != IDLE) { + return BUSY; + } + if (start.apid < SpacePacketBase::LIMIT_APID) { + firstPacketToFetch = start; + fetchCandidate.apid = start.apid; + fetchState = NOTHING_FETCHED; + } else { + firstPacketToFetch.apid = SpacePacketBase::LIMIT_APID; + firstPacketToFetch.ssc = 0; + //There's no start defined, so we're in range automatically. + fetchState = IN_RANGE; + } + if (end.apid < SpacePacketBase::LIMIT_APID) { + lastPacketToFetch = end; + } else { + lastPacketToFetch.apid = SpacePacketBase::LIMIT_APID; + lastPacketToFetch.ssc = SpacePacketBase::LIMIT_SEQUENCE_COUNT; + } + //Advanced: start fetching at a certain position + ReturnValue_t result = backend->fetchPackets(); + if (result != RETURN_OK) { + return result; + } + downlinkedPacketsCount = 0; + state = RETRIEVING_PACKETS; + return result; +} + +ReturnValue_t TmStoreManager::performOperation() { + backend->performOperation(); + checkCommandQueue(); + return RETURN_OK; +} + +ReturnValue_t TmStoreManager::packetRetrieved(TmPacketMinimal* packet, + uint32_t address, bool isLastPacket) { + ReturnValue_t result = RETURN_FAILED; + switch (state) { + case DELETING_PACKETS: + result = checkDeletionLimit(packet, address); + break; + case RETRIEVING_PACKETS: + result = checkRetrievalLimit(packet, address); + break; + case GET_OLDEST_PACKET_INFO: { + backend->deleteContent(true, lastAddressToDelete, + pendingPacketsToDelete, packet); + result = LAST_PACKET_FOUND; + } + break; + default: + triggerEvent(TmStoreBackendIF::UNEXPECTED_MSG, 0, packet->getPacketSequenceCount()); + break; + } + switch (result) { + case RETURN_OK: + //We go on. But if lastPacket... + if (isLastPacket) { + //... we'll stop successfully. + replySuccess(); + if (state == DELETING_PACKETS) { + //Store is completely deleted, so we can reset oldest and newest packet. + restDownlinkedPacketCount(); + backend->resetStore(); + } + state = IDLE; + } + break; + case LAST_PACKET_FOUND: + //All ok + replySuccess(); + state = IDLE; + break; + case STOP_FETCH: + //This means, we started a new fetch. Returning STOP_FETCH will stop current scan. Stay in state. + break; + default: + //Some error occurred. + replyFailure(result); + state = IDLE; + break; + } + return result; +} + +ReturnValue_t TmStoreManager::checkRetrievalLimit(TmPacketMinimal* packet, + uint32_t address) { + switch (fetchState) { + case NOTHING_FETCHED: + case BEFORE_RANGE: + if (packet->getAPID() != firstPacketToFetch.apid) { + //Dump all packets of unknown APID. + dumpPacket(packet); + return RETURN_OK; + } + if (packet->getPacketSequenceCount() < firstPacketToFetch.ssc) { + addressOfFetchCandidate = address; + fetchCandidate.ssc = packet->getPacketSequenceCount(); + fetchState = BEFORE_RANGE; + return RETURN_OK; + } else if ((packet->getPacketSequenceCount() == firstPacketToFetch.ssc) + || (fetchState == NOTHING_FETCHED)) { + //We have either found the right packet or one with higher count without having an older packet. So dump. + fetchState = IN_RANGE; + dumpPacket(packet); + if (packet->getPacketSequenceCount() < lastPacketToFetch.ssc) { + return RETURN_OK; + } else { + return LAST_PACKET_FOUND; + } + } else { + //We're in range, but the expected SSC is not in store. So we shall restart from the first older packet. + ReturnValue_t result = backend->fetchPackets(true, + addressOfFetchCandidate); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + fetchState = NOTHING_FETCHED; + firstPacketToFetch = fetchCandidate; + return STOP_FETCH; + } + case IN_RANGE: + dumpPacket(packet); + if ((packet->getAPID() == lastPacketToFetch.apid) + && (packet->getPacketSequenceCount() >= lastPacketToFetch.ssc)) { + return LAST_PACKET_FOUND; + } else { + return RETURN_OK; + } + default: + break; + } + return RETURN_FAILED; +} + +ReturnValue_t TmStoreManager::deletePackets(ApidSsc upTo) { + if (state != IDLE) { + return BUSY; + } + if (upTo.apid >= SpacePacketBase::LIMIT_APID) { + ReturnValue_t result = backend->deleteContent(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + restDownlinkedPacketCount(); + return result; + } else { + state = DELETING_PACKETS; + restDownlinkedPacketCount(); + lastPacketToDelete = upTo; + deletionStarted = false; + pendingPacketsToDelete = 0; + return backend->fetchPackets(); + } +} + +ReturnValue_t TmStoreManager::checkDeletionLimit(TmPacketMinimal* packet, + uint32_t address) { + uint32_t addressOfLastByte = address + packet->getFullSize(); + if (packet->getAPID() == lastPacketToDelete.apid) { + if (packet->getPacketSequenceCount() < lastPacketToDelete.ssc) { + if (deletionStarted) { + backend->deleteContent(true, lastAddressToDelete, + pendingPacketsToDelete, packet); + pendingPacketsToDelete = 0; + } else { + deletionStarted = true; + } + pendingPacketsToDelete++; + lastAddressToDelete = addressOfLastByte; + return RETURN_OK; + } else if (packet->getPacketSequenceCount() == lastPacketToDelete.ssc) { + lastAddressToDelete = addressOfLastByte; + pendingPacketsToDelete++; + state = GET_OLDEST_PACKET_INFO; + return RETURN_OK; + } else { + return LAST_PACKET_FOUND; + } + } else { + lastAddressToDelete = addressOfLastByte; + pendingPacketsToDelete++; + return RETURN_OK; + } +} + +void TmStoreManager::setBackend(TmStoreBackendIF* backend) { + this->backend = backend; +} + +TmStoreBackendIF* TmStoreManager::getBackend() const { + return backend; +} + +ReturnValue_t TmStoreManager::checkPacket(SpacePacketBase* tmPacket) { + if (!storingEnabled) { + return RETURN_OK; + } + TmPacketMinimal testPacket(tmPacket->getWholeData()); + if (matcher.match(&testPacket)) { + ReturnValue_t result = backend->storePacket(&testPacket); + if (result != HasReturnvaluesIF::RETURN_OK) { + //Generate only one event to avoid full event store loop. + if (!fullEventThrown) { + triggerEvent(TmStoreBackendIF::STORING_FAILED, result, testPacket.getPacketSequenceCount()); + fullEventThrown = true; + } + } else { + fullEventThrown = false; + } + return result; + } else { + return RETURN_OK; + } +} + +void TmStoreManager::dumpPacket(SpacePacketBase* packet) { + store_address_t forwardStoreId; + TmTcMessage message; + ReturnValue_t result = tmForwardStore->addData(&forwardStoreId, + packet->getWholeData(), packet->getFullSize()); + if (result != RETURN_OK) { + triggerEvent(TmStoreBackendIF::TM_DUMP_FAILED, result, packet->getPacketSequenceCount()); + return; + } + message.setStorageId(forwardStoreId); + result = tmQueue.sendToDefault(&message); + if (result != RETURN_OK) { + triggerEvent(TmStoreBackendIF::TM_DUMP_FAILED, result, packet->getPacketSequenceCount()); + tmForwardStore->deleteData(forwardStoreId); + return; + } + downlinkedPacketsCount.entry++; +} + +bool TmStoreManager::isEnabled() const { + return storingEnabled; +} + +void TmStoreManager::setEnabled(bool enabled) { + storingEnabled = enabled; +} + +void TmStoreManager::checkCommandQueue() { + if (state != IDLE) { + if (timer.hasTimedOut()) { + replyFailure(TIMEOUT); + state = IDLE; + } + return; + } + CommandMessage message; + for (ReturnValue_t result = commandQueue.receiveMessage(&message); + result == RETURN_OK; + result = commandQueue.receiveMessage(&message)) { + switch (message.getCommand()) { + case TmStoreMessage::ENABLE_STORING: + setEnabled(TmStoreMessage::getEnableStoring(&message)); + replySuccess(); + break; + case TmStoreMessage::CHANGE_SELECTION_DEFINITION: { + uint32_t errorCount = 0; + result = changeSelectionDefinition( + TmStoreMessage::getAddToSelection(&message), + TmStoreMessage::getStoreId(&message), &errorCount); + if (result != RETURN_OK) { + replyFailure(result, errorCount); + } else { + replySuccess(); + } + break; + } + case TmStoreMessage::DOWNLINK_STORE_CONTENT: + result = fetchPackets(TmStoreMessage::getPacketId1(&message), + TmStoreMessage::getPacketId2(&message)); + if (result != RETURN_OK) { + if (result == TmStoreBackendIF::EMPTY) { + replySuccess(); + } else { + replyFailure(result); + } + } else { + timer.setTimeout(timeoutMs); + } + break; + case TmStoreMessage::DELETE_STORE_CONTENT: + result = deletePackets(TmStoreMessage::getPacketId1(&message)); + if (result == RETURN_OK) { + if (state == IDLE) { + //We're finished + replySuccess(); + } else { + timer.setTimeout(timeoutMs); + } + } else { + replyFailure(result); + } + break; + case TmStoreMessage::REPORT_SELECTION_DEFINITION: + result = sendMatchTree(); + if (result != RETURN_OK) { + replyFailure(result); + } + break; + case TmStoreMessage::REPORT_STORE_CATALOGUE: + result = sendStatistics(); + if (result != RETURN_OK) { + replyFailure(result); + } + break; + default: + replyFailure(CommandMessage::UNKNOW_COMMAND, message.getCommand()); + break; + } + } +} + +void TmStoreManager::replySuccess() { + CommandMessage reply(CommandMessage::REPLY_COMMAND_OK, 0, 0); + commandQueue.reply(&reply); + +} + +MessageQueueId_t TmStoreManager::getCommandQueue() { + return commandQueue.getId(); +} + +void TmStoreManager::replyFailure(ReturnValue_t errorCode, uint32_t parameter) { + CommandMessage reply(CommandMessage::REPLY_REJECTED, errorCode, parameter); + commandQueue.reply(&reply); +} + +ReturnValue_t TmStoreManager::changeSelectionDefinition(bool addSelection, + store_address_t storeId, uint32_t* errorCount) { + const uint8_t* pData; + uint32_t size = 0; + ReturnValue_t result = ipcStore->getData(storeId, &pData, &size); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + int32_t iSize = size; + ChangeSelectionDefinition content; + result = content.deSerialize(&pData, &iSize, true); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + ReturnValue_t totalResult = RETURN_OK; + if (content.serviceList.size == 0) { + return updateMatch(addSelection, content.apid, NO_SERVICE, + NO_SUBSERVICE); + } + for (auto iter = content.serviceList.begin(); + iter != content.serviceList.end(); iter++) { + if (iter->subservices.size == 0) { + result = updateMatch(addSelection, content.apid, + iter->service, NO_SUBSERVICE); + if (result != HasReturnvaluesIF::RETURN_OK) { + totalResult = result; + (*errorCount)++; + } + } else { + for (auto iter2 = iter->subservices.begin(); + iter2 != iter->subservices.end(); iter2++) { + result = updateMatch(addSelection, content.apid, + iter->service, *(iter2.value)); + if (result != HasReturnvaluesIF::RETURN_OK) { + totalResult = result; + (*errorCount)++; + } + } + } + } + return totalResult; +} + +ReturnValue_t TmStoreManager::sendMatchTree() { + store_address_t storeId; + uint8_t* buffer; + uint32_t maxSize = matcher.getSerializedSize(); + ReturnValue_t result = ipcStore->getFreeElement(&storeId, maxSize, &buffer); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + uint32_t size = 0; + result = matcher.serialize(&buffer, &size, maxSize, true); + if (result != HasReturnvaluesIF::RETURN_OK) { + ipcStore->deleteData(storeId); + return result; + } + CommandMessage reply; + TmStoreMessage::setSelectionDefinitionReportMessage(&reply, storeId); + result = commandQueue.reply(&reply); + if (result != RETURN_OK) { + ipcStore->deleteData(storeId); + } + return result; +} + +void TmStoreManager::restDownlinkedPacketCount() { + downlinkedPacketsCount = 0; +} + +ReturnValue_t TmStoreManager::updateMatch(bool addSelection, uint16_t apid, uint8_t serviceType, + uint8_t serviceSubtype) { + return matcher.changeMatch(addSelection, apid, serviceType, serviceSubtype); +} + +void TmStoreManager::handleRetrievalFailed(ReturnValue_t errorCode, + uint32_t parameter1, uint32_t parameter2) { + restDownlinkedPacketCount(); + state = IDLE; + replyFailure(errorCode, parameter1); +} + +ReturnValue_t TmStoreManager::sendStatistics() { + LinkedElement linkedOldestPacket(backend->getOldestPacket()); + LinkedElement linkedYoungestPacket( + backend->getYoungestPacket()); + SerializeElement percentageFilled(backend->getPercentageFilled()); + float pDownlinked = + (backend->getStoredPacketsCount() != 0) ? + (float) downlinkedPacketsCount + / (float) backend->getStoredPacketsCount() : + 0.0; + SerializeElement percentageDownlinked(pDownlinked); + SerializeElement storedPacketsCount( + backend->getStoredPacketsCount()); + linkedOldestPacket.setNext(&linkedYoungestPacket); + linkedYoungestPacket.setNext(&percentageFilled); + percentageFilled.setNext(&percentageDownlinked); + percentageDownlinked.setNext(&storedPacketsCount); + storedPacketsCount.setNext(&downlinkedPacketsCount); + store_address_t storeId; + uint8_t* buffer; + uint32_t maxSize = SerialLinkedListAdapter::getSerializedSize( + &linkedOldestPacket); + ReturnValue_t result = ipcStore->getFreeElement(&storeId, maxSize, &buffer); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + uint32_t size = 0; + result = SerialLinkedListAdapter::serialize( + &linkedOldestPacket, &buffer, &size, maxSize, true); + if (result != HasReturnvaluesIF::RETURN_OK) { + ipcStore->deleteData(storeId); + return result; + } + CommandMessage reply; + TmStoreMessage::setStoreCatalogueReportMessage(&reply, storeId); + result = commandQueue.reply(&reply); + if (result != RETURN_OK) { + ipcStore->deleteData(storeId); + } + return result; +} diff --git a/tmstorage/TmStoreManager.h b/tmstorage/TmStoreManager.h new file mode 100644 index 00000000..7f1ffd62 --- /dev/null +++ b/tmstorage/TmStoreManager.h @@ -0,0 +1,90 @@ +/* + * TmStoreManager.h + * + * Created on: 18.02.2015 + * Author: baetz + */ + +#ifndef PLATFORM_TMTCSERVICES_TMSTOREMANAGER_H_ +#define PLATFORM_TMTCSERVICES_TMSTOREMANAGER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class TmStoreManager : public SystemObject, public HasReturnvaluesIF, public TmStoreFrontendIF { +public: + TmStoreManager(object_id_t objectId, object_id_t setDumpTarget, uint8_t setVC, uint32_t setTimeoutMs); + virtual ~TmStoreManager(); + ReturnValue_t fetchPackets(ApidSsc start, ApidSsc end); + ReturnValue_t deletePackets(ApidSsc upTo); + ReturnValue_t checkPacket(SpacePacketBase* tmPacket); + ReturnValue_t performOperation(); + ReturnValue_t initialize(); + ReturnValue_t packetRetrieved(TmPacketMinimal* packet, uint32_t address, bool isLastPacket = false); + void handleRetrievalFailed(ReturnValue_t errorCode, uint32_t parameter1 = 0, uint32_t parameter2 = 0); + bool isEnabled() const; + void setEnabled(bool enabled); + void setBackend(TmStoreBackendIF* backend); + TmStoreBackendIF* getBackend() const; + MessageQueueId_t getCommandQueue(); + ReturnValue_t updateMatch(bool addSelection, uint16_t apid, uint8_t serviceType, uint8_t serviceSubtype); +private: + MessageQueue commandQueue; + TmStoreBackendIF* backend; + StorageManagerIF* tmForwardStore; + MessageQueueSender tmQueue; + const object_id_t dumpTarget; + const uint8_t virtualChannel; + enum FetchState { + NOTHING_FETCHED, + BEFORE_RANGE, + IN_RANGE + }; + FetchState fetchState; + ApidSsc firstPacketToFetch; + ApidSsc lastPacketToFetch; + uint32_t addressOfFetchCandidate; + ApidSsc fetchCandidate; + bool deletionStarted; + ApidSsc lastPacketToDelete; + uint32_t lastAddressToDelete; + uint32_t pendingPacketsToDelete; + TmPacketInformation pendingOldestPacket; + enum State { + IDLE, + DELETING_PACKETS, + RETRIEVING_PACKETS, + GET_OLDEST_PACKET_INFO + }; + State state; + bool storingEnabled; + const uint32_t timeoutMs; + Countdown timer; + PacketMatchTree matcher; + StorageManagerIF* ipcStore; + SerializeElement downlinkedPacketsCount; + bool fullEventThrown; + ReturnValue_t checkRetrievalLimit(TmPacketMinimal* packet, uint32_t address); + ReturnValue_t checkDeletionLimit(TmPacketMinimal* packet, uint32_t address); + void dumpPacket(SpacePacketBase* packet); + void checkCommandQueue(); + void replySuccess(); + void replyFailure(ReturnValue_t errorCode, uint32_t parameter = 0); + ReturnValue_t changeSelectionDefinition(bool addSelection, store_address_t storeId, uint32_t* errorCount); + ReturnValue_t sendMatchTree(); + void restDownlinkedPacketCount(); + ReturnValue_t sendStatistics(); + static const uint8_t NO_SERVICE = 0; + static const uint8_t NO_SUBSERVICE = 0; +}; + +#endif /* PLATFORM_TMTCSERVICES_TMSTOREMANAGER_H_ */ diff --git a/tmstorage/TmStoreMessage.cpp b/tmstorage/TmStoreMessage.cpp new file mode 100644 index 00000000..edf37bf8 --- /dev/null +++ b/tmstorage/TmStoreMessage.cpp @@ -0,0 +1,115 @@ +/* + * TmStoreMessage.cpp + * + * Created on: 23.02.2015 + * Author: baetz + */ + +#include +#include + +TmStoreMessage::~TmStoreMessage() { + +} + +TmStoreMessage::TmStoreMessage() { +} + +ReturnValue_t TmStoreMessage::setEnableStoringMessage(CommandMessage* cmd, + bool setEnabled) { + cmd->setCommand(ENABLE_STORING); + cmd->setParameter(setEnabled); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t TmStoreMessage::setDeleteContentMessage(CommandMessage* cmd, + TmStoreFrontendIF::ApidSsc upTo) { + cmd->setCommand(DELETE_STORE_CONTENT); + cmd->setParameter((upTo.apid<<16) + upTo.ssc); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t TmStoreMessage::setDownlinkContentMessage(CommandMessage* cmd, + TmStoreFrontendIF::ApidSsc fromPacket, + TmStoreFrontendIF::ApidSsc toPacket) { + cmd->setCommand(DOWNLINK_STORE_CONTENT); + cmd->setParameter((fromPacket.apid<<16) + fromPacket.ssc); + cmd->setParameter2((toPacket.apid<<16) + toPacket.ssc); + return HasReturnvaluesIF::RETURN_OK; +} + +TmStoreFrontendIF::ApidSsc TmStoreMessage::getPacketId1(CommandMessage* cmd) { + TmStoreFrontendIF::ApidSsc temp; + temp.apid = (cmd->getParameter() >> 16) & 0xFFFF; + temp.ssc = cmd->getParameter() & 0xFFFF; + return temp; +} + +TmStoreFrontendIF::ApidSsc TmStoreMessage::getPacketId2(CommandMessage* cmd) { + TmStoreFrontendIF::ApidSsc temp; + temp.apid = (cmd->getParameter2() >> 16) & 0xFFFF; + temp.ssc = cmd->getParameter2() & 0xFFFF; + return temp; +} + +bool TmStoreMessage::getEnableStoring(CommandMessage* cmd) { + return (bool)cmd->getParameter(); +} + +void TmStoreMessage::setChangeSelectionDefinitionMessage( + CommandMessage* cmd, bool addDefinition, store_address_t store_id) { + cmd->setCommand(CHANGE_SELECTION_DEFINITION); + cmd->setParameter(addDefinition); + cmd->setParameter2(store_id.raw); +} + +void TmStoreMessage::clear(CommandMessage* cmd) { + switch(cmd->getCommand()) { + case SELECTION_DEFINITION_REPORT: + case STORE_CATALOGUE_REPORT: + case CHANGE_SELECTION_DEFINITION: { + StorageManagerIF *ipcStore = objectManager->get( + objects::IPC_STORE); + if (ipcStore != NULL) { + ipcStore->deleteData(getStoreId(cmd)); + } + } + break; + default: + break; + } +} + +store_address_t TmStoreMessage::getStoreId(const CommandMessage* cmd) { + store_address_t temp; + temp.raw = cmd->getParameter2(); + return temp; +} + +bool TmStoreMessage::getAddToSelection(CommandMessage* cmd) { + return (bool)cmd->getParameter(); +} + +ReturnValue_t TmStoreMessage::setReportSelectionDefinitionMessage( + CommandMessage* cmd) { + cmd->setCommand(REPORT_SELECTION_DEFINITION); + return HasReturnvaluesIF::RETURN_OK; +} + +void TmStoreMessage::setSelectionDefinitionReportMessage( + CommandMessage* cmd, store_address_t storeId) { + cmd->setCommand(SELECTION_DEFINITION_REPORT); + cmd->setParameter2(storeId.raw); +} + +ReturnValue_t TmStoreMessage::setReportStoreCatalogueMessage( + CommandMessage* cmd) { + cmd->setCommand(REPORT_STORE_CATALOGUE); + return HasReturnvaluesIF::RETURN_OK; +} + +void TmStoreMessage::setStoreCatalogueReportMessage(CommandMessage* cmd, + store_address_t storeId) { + cmd->setCommand(STORE_CATALOGUE_REPORT); + cmd->setParameter2(storeId.raw); +} diff --git a/tmstorage/TmStoreMessage.h b/tmstorage/TmStoreMessage.h new file mode 100644 index 00000000..436895c5 --- /dev/null +++ b/tmstorage/TmStoreMessage.h @@ -0,0 +1,45 @@ +/* + * TmStoreMessage.h + * + * Created on: 23.02.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_TMSTORAGE_TMSTOREMESSAGE_H_ +#define FRAMEWORK_TMSTORAGE_TMSTOREMESSAGE_H_ + +#include +#include +#include + +class TmStoreMessage: public CommandMessage { +public: + static ReturnValue_t setEnableStoringMessage(CommandMessage* cmd, bool setEnabled); + static ReturnValue_t setDeleteContentMessage(CommandMessage* cmd, TmStoreFrontendIF::ApidSsc upTo); + static ReturnValue_t setDownlinkContentMessage(CommandMessage* cmd, TmStoreFrontendIF::ApidSsc fromPacket, TmStoreFrontendIF::ApidSsc toPacket ); + static void setChangeSelectionDefinitionMessage(CommandMessage* cmd, bool addDefinition, store_address_t store_id); + static ReturnValue_t setReportSelectionDefinitionMessage(CommandMessage* cmd); + static void setSelectionDefinitionReportMessage(CommandMessage* cmd, store_address_t storeId); + static ReturnValue_t setReportStoreCatalogueMessage(CommandMessage* cmd); + static void setStoreCatalogueReportMessage(CommandMessage* cmd, store_address_t storeId); + static void clear(CommandMessage* cmd); + static TmStoreFrontendIF::ApidSsc getPacketId1(CommandMessage* cmd); + static TmStoreFrontendIF::ApidSsc getPacketId2(CommandMessage* cmd); + static bool getEnableStoring(CommandMessage* cmd); + static bool getAddToSelection(CommandMessage* cmd); + static store_address_t getStoreId(const CommandMessage* cmd); + virtual ~TmStoreMessage(); + static const uint8_t MESSAGE_ID = TM_STORE_MESSAGE_ID; + static const Command_t ENABLE_STORING = MAKE_COMMAND_ID(1); + static const Command_t DELETE_STORE_CONTENT = MAKE_COMMAND_ID(2); + static const Command_t DOWNLINK_STORE_CONTENT = MAKE_COMMAND_ID(3); + static const Command_t CHANGE_SELECTION_DEFINITION = MAKE_COMMAND_ID(4); + static const Command_t REPORT_SELECTION_DEFINITION = MAKE_COMMAND_ID(5); + static const Command_t SELECTION_DEFINITION_REPORT = MAKE_COMMAND_ID(6); + static const Command_t REPORT_STORE_CATALOGUE = MAKE_COMMAND_ID(7); + static const Command_t STORE_CATALOGUE_REPORT = MAKE_COMMAND_ID(8); +private: + TmStoreMessage(); +}; + +#endif /* FRAMEWORK_TMSTORAGE_TMSTOREMESSAGE_H_ */ diff --git a/tmstorage/TmStorePackets.h b/tmstorage/TmStorePackets.h new file mode 100644 index 00000000..c89d0762 --- /dev/null +++ b/tmstorage/TmStorePackets.h @@ -0,0 +1,91 @@ +/* + * TmStorePackets.h + * + * Created on: 11.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_TMSTORAGE_TMSTOREPACKETS_H_ +#define FRAMEWORK_TMSTORAGE_TMSTOREPACKETS_H_ + +#include +#include +#include +#include + +class ServiceSubservice: public SerialLinkedListAdapter { +public: + SerializeElement service; + SerialFixedArrayListAdapter subservices; + LinkedElement linkedSubservices; + ServiceSubservice() : + SerialLinkedListAdapter(&service), linkedSubservices( + &subservices) { + service.setNext(&linkedSubservices); + } +}; + +class ChangeSelectionDefinition: public SerialLinkedListAdapter { +public: + SerializeElement apid; + SerialFixedArrayListAdapter serviceList; + LinkedElement linkedServiceList; + ChangeSelectionDefinition() : + SerialLinkedListAdapter(&apid), linkedServiceList( + &serviceList) { + apid.setNext(&linkedServiceList); + } +}; + +class TmPacketInformation: public SerialLinkedListAdapter { +public: + TmPacketInformation(TmPacketMinimal* packet) : + SerialLinkedListAdapter(&apid) { + setAllNext(); + setContent(packet); + } + TmPacketInformation() : + SerialLinkedListAdapter(&apid), apid( + SpacePacketBase::LIMIT_APID), sourceSequenceCount(0), serviceType( + 0), serviceSubtype(0), subCounter(0) { + setAllNext(); + } + void reset() { + apid = SpacePacketBase::LIMIT_APID; + sourceSequenceCount = 0; + serviceType = 0; + serviceSubtype = 0; + subCounter = 0; + } + void setContent(TmPacketMinimal* packet) { + apid = packet->getAPID(); + sourceSequenceCount = packet->getPacketSequenceCount(); + serviceType = packet->getService(); + serviceSubtype = packet->getSubService(); + subCounter = packet->getPacketSubcounter(); + } + void setContent(TmPacketInformation* content) { + apid.entry = content->apid.entry; + sourceSequenceCount.entry = content->sourceSequenceCount.entry; + serviceType.entry = content->serviceType.entry; + serviceSubtype.entry = content->serviceSubtype.entry; + subCounter.entry = content->subCounter.entry; + } + bool isValid() { + return (apid < SpacePacketBase::LIMIT_APID) ? true : false; + } +private: + SerializeElement apid; + SerializeElement sourceSequenceCount; + SerializeElement serviceType; + SerializeElement serviceSubtype; + SerializeElement subCounter; + void setAllNext() { + apid.setNext(&sourceSequenceCount); + sourceSequenceCount.setNext(&serviceType); + serviceType.setNext(&serviceSubtype); + serviceSubtype.setNext(&subCounter); + } +}; + +#endif /* FRAMEWORK_TMSTORAGE_TMSTOREPACKETS_H_ */ diff --git a/tmtcpacket/Makefile b/tmtcpacket/Makefile new file mode 100755 index 00000000..5559931d --- /dev/null +++ b/tmtcpacket/Makefile @@ -0,0 +1,24 @@ +#!/bin/bash +# +# OSAL makefile +# +# Created on: Mar 04, 2010 +# Author: ziemke +# Author: Claas Ziemke +# Copyright 2010, Claas Ziemke +# + +BASEDIR=../../ +include $(BASEDIR)options.mk + +OBJ = $(BUILDDIR)/SpacePacketBase.o \ + $(BUILDDIR)/SpacePacket.o + + +all: $(OBJ) + +$(BUILDDIR)/%.o: %.cpp %.h + $(CPP) $(CFLAGS) $(DEFINES) $(CCOPT) ${INCLUDE} -c $< -o $@ + +clean: + $(RM) *.o *.gcno *.gcda diff --git a/tmtcpacket/SpacePacket.cpp b/tmtcpacket/SpacePacket.cpp new file mode 100644 index 00000000..3479bddf --- /dev/null +++ b/tmtcpacket/SpacePacket.cpp @@ -0,0 +1,39 @@ +/* + * SpacePacket.cpp + * + * Created on: Mar 23, 2012 + * Author: baetz + */ + +#include +#include +#include +#include + +SpacePacket::SpacePacket( uint16_t set_packet_data_length, uint8_t set_type, uint16_t new_apid, uint16_t set_count ): +SpacePacketBase( (uint8_t*)&this->local_data ) { + //reset everything to zero: + memset(this->local_data.byteStream,0 , sizeof(this->local_data.byteStream) ); + //Primary header: + this->local_data.fields.header.packet_id_h = 0b00000000 + ( (set_type & 0b1) << 4 ); + this->setAPID( new_apid ); + this->local_data.fields.header.sequence_control_h = 0b11000000; + this->setPacketSequenceCount(set_count); + if ( set_packet_data_length <= sizeof(this->local_data.fields.buffer) ) { + this->setPacketDataLength(set_packet_data_length); + } else { + this->setPacketDataLength( sizeof(this->local_data.fields.buffer) ); + } +} + +SpacePacket::~SpacePacket( void ) { +} + +bool SpacePacket::addWholeData( const uint8_t* p_Data, uint32_t packet_size ) { + if ( packet_size <= sizeof(this->data) ) { + memcpy( &this->local_data.byteStream, p_Data, packet_size ); + return true; + } else { + return false; + } +} diff --git a/tmtcpacket/SpacePacket.h b/tmtcpacket/SpacePacket.h new file mode 100644 index 00000000..4dd0630c --- /dev/null +++ b/tmtcpacket/SpacePacket.h @@ -0,0 +1,72 @@ +#ifndef SPACEPACKET_H_ +#define SPACEPACKET_H_ + +#include + +/** + * The SpacePacket class is a representation of a simple CCSDS Space Packet + * without (control over) a secondary header. + * It can be instantiated with a size smaller than \c PACKET_MAX_SIZE. Its + * main use is to serve as an idle packet in case no other packets are sent. + * For the ECSS PUS part the TcPacket and TmPacket classes are used. + * A pointer to \c local_data is passed to the \c SpacePacketBase parent class, + * so the parent's methods are reachable. + * @ingroup tmtcpackets + */ +class SpacePacket : public SpacePacketBase { + public: + static const uint16_t PACKET_MAX_SIZE = 1024; + /** + * The constructor initializes the packet and sets all header information + * according to the passed parameters. + * @param set_packet_data_length Sets the packet data length field and + * therefore specifies the size of the packet. + * @param set_type Sets the packet type field to either 0 (TM) or 1 (TC). + * @param set_apid Sets the packet's APID field. The default value + * describes an idle packet. + * @param set_sequence_count Sets the packet's Source Sequence Count + * field. + */ + SpacePacket( uint16_t set_packet_data_length, uint8_t set_type = 0, uint16_t set_apid = APID_IDLE_PACKET, uint16_t set_sequence_count = 0 ); + /** + * The class's default destructor. + */ + virtual ~SpacePacket(); + /** + * With this call, the complete data content (including the CCSDS Primary + * Header) is overwritten with the byte stream given. + * @param p_data Pointer to data to overwrite the content with + * @param packet_size Size of the data + * @return @li \c true if packet_size is smaller than \c MAX_PACKET_SIZE. + * @li \c false else. + */ + bool addWholeData( const uint8_t* p_data, uint32_t packet_size ); +protected: + /** + * This structure defines the data structure of a Space Packet as local data. + * There's a buffer which corresponds to the Space Packet Data Field with a + * maximum size of \c PACKET_MAX_SIZE. + */ + struct PacketStructured { + CCSDSPrimaryHeader header; + uint8_t buffer[PACKET_MAX_SIZE]; + }; + /** + * This union simplifies accessing the full data content of the Space Packet. + * This is achieved by putting the \c PacketStructured struct in a union with + * a plain buffer. + */ + union SpacePacketData { + PacketStructured fields; + uint8_t byteStream[PACKET_MAX_SIZE + sizeof(CCSDSPrimaryHeader)]; + }; + /** + * This is the data representation of the class. + * It is a struct of CCSDS Primary Header and a data field, which again is + * packed in an union, so the data can be accessed as a byte stream without + * a cast. + */ + SpacePacketData local_data; +}; + +#endif /* SPACEPACKET_H_ */ diff --git a/tmtcpacket/SpacePacketBase.cpp b/tmtcpacket/SpacePacketBase.cpp new file mode 100644 index 00000000..c148bcc2 --- /dev/null +++ b/tmtcpacket/SpacePacketBase.cpp @@ -0,0 +1,85 @@ +/* + * SpacePacketBase.cpp + * + * Created on: 21.03.2012 + * Author: baetz + */ +#include +#include +SpacePacketBase::SpacePacketBase( const uint8_t* set_address ) { + this->data = (SpacePacketPointer*) set_address; +} + +SpacePacketBase::~SpacePacketBase() { +}; + +//CCSDS Methods: +uint8_t SpacePacketBase::getPacketVersionNumber( void ) { + return (this->data->header.packet_id_h & 0b11100000) >> 5; +} + +bool SpacePacketBase::isTelecommand( void ) { + return (this->data->header.packet_id_h & 0b00010000) >> 4; +} + +bool SpacePacketBase::hasSecondaryHeader( void ) { + return (this->data->header.packet_id_h & 0b00001000) >> 3; +} + +uint16_t SpacePacketBase::getPacketId() { + return ( (this->data->header.packet_id_h) << 8 ) + + this->data->header.packet_id_l; +} + +uint16_t SpacePacketBase::getAPID( void ) { + return ( (this->data->header.packet_id_h & 0b00000111) << 8 ) + + this->data->header.packet_id_l; +} + +void SpacePacketBase::setAPID( uint16_t new_apid ) { + //Use first three bits of new APID, but keep rest of packet id as it was (see specification). + this->data->header.packet_id_h = (this->data->header.packet_id_h & 0b11111000) | ( ( new_apid & 0x0700 ) >> 8 ); + this->data->header.packet_id_l = ( new_apid & 0x00FF ); +} + +uint16_t SpacePacketBase::getPacketSequenceControl( void ) { + return ( (this->data->header.sequence_control_h) << 8 ) + + this->data->header.sequence_control_l; +} + +uint8_t SpacePacketBase::getSequenceFlags( void ) { + return (this->data->header.sequence_control_h & 0b11000000) >> 6 ; +} + +uint16_t SpacePacketBase::getPacketSequenceCount( void ) { + return ( (this->data->header.sequence_control_h & 0b00111111) << 8 ) + + this->data->header.sequence_control_l; +} + +void SpacePacketBase::setPacketSequenceCount( uint16_t new_count) { + this->data->header.sequence_control_h = ( this->data->header.sequence_control_h & 0b11000000 ) | ( ( (new_count%LIMIT_SEQUENCE_COUNT) & 0x3F00 ) >> 8 ); + this->data->header.sequence_control_l = ( (new_count%LIMIT_SEQUENCE_COUNT) & 0x00FF ); +} + +uint16_t SpacePacketBase::getPacketDataLength( void ) { + return ( (this->data->header.packet_length_h) << 8 ) + + this->data->header.packet_length_l; +} + +void SpacePacketBase::setPacketDataLength( uint16_t new_length) { + this->data->header.packet_length_h = ( ( new_length & 0xFF00 ) >> 8 ); + this->data->header.packet_length_l = ( new_length & 0x00FF ); +} + +uint32_t SpacePacketBase::getFullSize() { + //+1 is done because size in packet data length field is: size of data field -1 + return this->getPacketDataLength() + sizeof(this->data->header) + 1; +} + +uint8_t* SpacePacketBase::getWholeData() { + return (uint8_t*)this->data; +} + +void SpacePacketBase::setData( const uint8_t* p_Data ) { + this->data = (SpacePacketPointer*)p_Data; +} diff --git a/tmtcpacket/SpacePacketBase.h b/tmtcpacket/SpacePacketBase.h new file mode 100644 index 00000000..5d506a3a --- /dev/null +++ b/tmtcpacket/SpacePacketBase.h @@ -0,0 +1,170 @@ +#ifndef SPACEPACKETBASE_H_ +#define SPACEPACKETBASE_H_ + +#include + +/** + * \defgroup tmtcpackets Space Packets + * This is the group, where all classes associated with Telecommand and + * Telemetry packets belong to. + * The class hierarchy resembles the dependency between the different standards + * applied, namely the CCSDS Space Packet standard and the ECCSS Packet + * Utilization Standard. Most field and structure names are taken from these + * standards. + */ + +/** + * This struct defines the data structure of a Space Packet when accessed + * via a pointer. + * @ingroup tmtcpackets + */ +struct SpacePacketPointer { + CCSDSPrimaryHeader header; + uint8_t packet_data; +}; + +/** + * This class is the basic data handler for any CCSDS Space Packet + * compatible Telecommand and Telemetry packet. + * It does not contain the packet data itself but a pointer to the + * data must be set on instantiation. An invalid pointer may cause + * damage, as no getter method checks data validity. Anyway, a NULL + * check can be performed by making use of the getWholeData method. + * Remark: All bit numbers in this documentation are counted from + * the most significant bit (from left). + * @ingroup tmtcpackets + */ +class SpacePacketBase { +protected: + /** + * A pointer to a structure which defines the data structure of + * the packet header. + * To be hardware-safe, all elements are of byte size. + */ + SpacePacketPointer* data; +public: + static const uint16_t LIMIT_APID = 2048; //2^1 + static const uint16_t LIMIT_SEQUENCE_COUNT = 16384; // 2^14 + static const uint16_t APID_IDLE_PACKET = 0x7FF; + static const uint8_t TELECOMMAND_PACKET = 1; + static const uint8_t TELEMETRY_PACKET = 0; + /** + * This definition defines the CRC size in byte. + */ + static const uint8_t CRC_SIZE = 2; + /** + * This is the minimum size of a SpacePacket. + */ + static const uint16_t MINIMUM_SIZE = sizeof(CCSDSPrimaryHeader) + CRC_SIZE; + /** + * This is the default constructor. + * It sets its internal data pointer to the address passed. + * @param set_address The position where the packet data lies. + */ + SpacePacketBase( const uint8_t* set_address ); + /** + * No data is allocated, so the destructor is empty. + */ + virtual ~SpacePacketBase(); + + //CCSDS Methods: + /** + * Getter for the packet version number field. + * @return Returns the highest three bit of the packet in one byte. + */ + uint8_t getPacketVersionNumber( void ); + /** + * This method checks the type field in the header. + * This bit specifies, if the command is interpreted as Telecommand of + * as Telemetry. For a Telecommand, the bit is set. + * @return Returns true if the bit is set and false if not. + */ + bool isTelecommand( void ); + /** + * The CCSDS header provides a secondary header flag (the fifth-highest bit), + * which is checked with this method. + * @return Returns true if the bit is set and false if not. + */ + bool hasSecondaryHeader( void ); + /** + * Returns the complete first two bytes of the packet, which together form + * the CCSDS packet id. + * @return The CCSDS packet id. + */ + uint16_t getPacketId( void ); + /** + * Returns the APID of a packet, which are the lowest 11 bit of the packet + * id. + * @return The CCSDS APID. + */ + uint16_t getAPID( void ); + /** + * Sets the APID of a packet, which are the lowest 11 bit of the packet + * id. + * @param The APID to set. The highest five bits of the parameter are + * ignored. + */ + void setAPID( uint16_t setAPID ); + /** + * Returns the CCSDS packet sequence control field, which are the third and + * the fourth byte of the CCSDS primary header. + * @return The CCSDS packet sequence control field. + */ + uint16_t getPacketSequenceControl( void ); + /** + * Returns the SequenceFlags, which are the highest two bit of the packet + * sequence control field. + * @return The CCSDS sequence flags. + */ + uint8_t getSequenceFlags( void ); + /** + * Returns the packet sequence count, which are the lowest 14 bit of the + * packet sequence control field. + * @return The CCSDS sequence count. + */ + uint16_t getPacketSequenceCount( void ); + /** + * Sets the packet sequence count, which are the lowest 14 bit of the + * packet sequence control field. + * setCount is modulo-divided by \c LIMIT_SEQUENCE_COUNT to avoid overflows. + * @param setCount The value to set the count to. + */ + void setPacketSequenceCount( uint16_t setCount ); + /** + * Returns the packet data length, which is the fifth and sixth byte of the + * CCSDS Primary Header. The packet data length is the size of every kind + * of data \b after the CCSDS Primary Header \b -1. + * @return The CCSDS packet data length. + */ + uint16_t getPacketDataLength( void ); //uint16_t is sufficient, because this is limit in CCSDS standard + /** + * Sets the packet data length, which is the fifth and sixth byte of the + * CCSDS Primary Header. + * @param setLength The value of the length to set. It must fit the true + * CCSDS packet data length . The packet data length is + * the size of every kind of data \b after the CCSDS + * Primary Header \b -1. + */ + void setPacketDataLength( uint16_t setLength ); + + //Helper methods: + /** + * This method returns a raw uint8_t pointer to the packet. + * @return A \c uint8_t pointer to the first byte of the CCSDS primary header. + */ + virtual uint8_t* getWholeData( void ); + /** + * With this method, the packet data pointer can be redirected to another + * location. + * @param p_Data A pointer to another raw Space Packet. + */ + virtual void setData( const uint8_t* p_Data ); + /** + * This method returns the full raw packet size. + * @return The full size of the packet in bytes. + */ + uint32_t getFullSize(); + +}; + +#endif /* SPACEPACKETBASE_H_ */ diff --git a/tmtcpacket/ccsds_header.h b/tmtcpacket/ccsds_header.h new file mode 100644 index 00000000..40d6829f --- /dev/null +++ b/tmtcpacket/ccsds_header.h @@ -0,0 +1,15 @@ +#ifndef CCSDS_HEADER_H_ +#define CCSDS_HEADER_H_ + +#include + +struct CCSDSPrimaryHeader { + uint8_t packet_id_h; + uint8_t packet_id_l; + uint8_t sequence_control_h; + uint8_t sequence_control_l; + uint8_t packet_length_h; + uint8_t packet_length_l; +}; + +#endif /* CCSDS_HEADER_H_ */ diff --git a/tmtcpacket/packetmatcher/ApidMatcher.h b/tmtcpacket/packetmatcher/ApidMatcher.h new file mode 100644 index 00000000..9199b7ef --- /dev/null +++ b/tmtcpacket/packetmatcher/ApidMatcher.h @@ -0,0 +1,44 @@ +/* + * ApidMatcher.h + * + * Created on: 09.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_ +#define FRAMEWORK_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_ + +#include +#include +#include + +class ApidMatcher: public SerializeableMatcherIF { +private: + uint16_t apid; +public: + ApidMatcher(uint16_t setApid) : + apid(setApid) { + } + bool match(TmPacketMinimal* packet) { + if (packet->getAPID() == apid) { + return true; + } else { + return false; + } + } + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + return SerializeAdapter::serialize(&apid, buffer, size, max_size, bigEndian); + } + uint32_t getSerializedSize() const { + return SerializeAdapter::getSerializedSize(&apid); + } + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return SerializeAdapter::deSerialize(&apid, buffer, size, bigEndian); + } +}; + + + +#endif /* FRAMEWORK_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_ */ diff --git a/tmtcpacket/packetmatcher/PacketMatchTree.cpp b/tmtcpacket/packetmatcher/PacketMatchTree.cpp new file mode 100644 index 00000000..859070f2 --- /dev/null +++ b/tmtcpacket/packetmatcher/PacketMatchTree.cpp @@ -0,0 +1,169 @@ +/* + * PacketMatchTree.cpp + * + * Created on: 10.03.2015 + * Author: baetz + */ + +#include +#include +#include +#include + +PacketMatchTree::PacketMatchTree(Node* root) : + MatchTree(root, 2), factoryBackend(0, POOL_SIZES, + N_ELEMENTS, false, true), factory(&factoryBackend) { +} + +PacketMatchTree::PacketMatchTree(iterator root) : + MatchTree(root.element, 2), factoryBackend(0, + POOL_SIZES, N_ELEMENTS, false, true), factory(&factoryBackend) { +} + +PacketMatchTree::PacketMatchTree() : + MatchTree((Node*) NULL, 2), factoryBackend(0, + POOL_SIZES, N_ELEMENTS, false, true), factory(&factoryBackend) { +} + +PacketMatchTree::~PacketMatchTree() { +} + +ReturnValue_t PacketMatchTree::addMatch(uint16_t apid, uint8_t type, + uint8_t subtype) { + //We assume adding APID is always requested. + uint8_t expectedHierarchy = 0; + if (type != 0) { + expectedHierarchy++; + if (subtype != 0) { + expectedHierarchy++; + } + } + TmPacketMinimal::TmPacketMinimalPointer data; + data.data_field.service_type = type; + data.data_field.service_subtype = subtype; + TmPacketMinimal testPacket((uint8_t*)&data); + testPacket.setAPID(apid); + uint8_t realHierarchy = 0; + iterator lastMatch; + bool isInTree = matchesTree(&testPacket, &lastMatch, &realHierarchy); + if (isInTree) { + //Matches somehow. + //TODO: Are we interested in details? + return RETURN_OK; + } + if (expectedHierarchy == realHierarchy) { + //Add another element (of correct type) at the OR branch. + lastMatch = addElement(OR, realHierarchy, lastMatch, &testPacket); + if (lastMatch == this->end()) { + return FULL; + } + } else if (expectedHierarchy > realHierarchy) { + //A certain amount of downward structure does not exist. Add first element as OR, rest as AND. + + lastMatch = addElement(OR, realHierarchy, lastMatch, &testPacket); + if (lastMatch == end()) { + return FULL; + } + iterator firstOfList = lastMatch; + while (lastMatch != end() && realHierarchy < expectedHierarchy) { + realHierarchy++; + lastMatch = addElement(AND, realHierarchy, lastMatch, &testPacket); + } + if (lastMatch == end()) { + //At least one element could not be inserted. So delete everything. + removeElementAndAllChildren(firstOfList); + return FULL; + } + } else { + //Might work like that. + //Too detailed match, delete the last element and all its children. + while (lastMatch.up() != end() && lastMatch.up().left() != lastMatch) { + lastMatch = lastMatch.up(); + } + removeElementAndAllChildren(lastMatch); + } + return RETURN_OK; +} + +ReturnValue_t PacketMatchTree::removeMatch(uint16_t apid, uint8_t type, + uint8_t subtype) { + //We assume APID is always in request. + uint8_t expectedHierarchy = 1; + if (type != 0) { + expectedHierarchy++; + if (subtype != 0) { + expectedHierarchy++; + } + } + TmPacketMinimal::TmPacketMinimalPointer data; + data.data_field.service_type = type; + data.data_field.service_subtype = subtype; + TmPacketMinimal testPacket((uint8_t*)&data); + testPacket.setAPID(apid); + uint8_t realHierarchy = 0; + iterator lastMatch; + bool isInTree = matchesTree(&testPacket, &lastMatch, &realHierarchy); + if (isInTree) { + if (expectedHierarchy == realHierarchy) { + return removeElementAndReconnectChildren(lastMatch); + } else { + return TOO_DETAILED_REQUEST; + } + } else { + //TODO: Maybe refine this a bit. + return NO_MATCH; + } +} + +PacketMatchTree::iterator PacketMatchTree::addElement(bool andBranch, + uint8_t level, PacketMatchTree::iterator position, + TmPacketMinimal* content) { + SerializeableMatcherIF* newContent = NULL; + switch (level) { + case 0: + newContent = factory.generate(content->getAPID()); + break; + case 1: + newContent = factory.generate(content->getService()); + break; + case 2: + newContent = factory.generate( + content->getSubService()); + break; + default: + break; + } + if (newContent == NULL) { + return end(); + } + Node* newNode = factory.generate(newContent); + if (newNode == NULL) { + return end(); + } + return insert(andBranch, position, newNode); +} + +ReturnValue_t PacketMatchTree::initialize() { + return factoryBackend.initialize(); +} + +const uint16_t PacketMatchTree::POOL_SIZES[N_POOLS] = { sizeof(ServiceMatcher), + sizeof(SubServiceMatcher), sizeof(ApidMatcher), + sizeof(PacketMatchTree::Node) }; +//TODO: Rather arbitrary. Adjust! +const uint16_t PacketMatchTree::N_ELEMENTS[N_POOLS] = { 10, 20, 2, 40 }; + +ReturnValue_t PacketMatchTree::changeMatch(bool addToMatch, uint16_t apid, + uint8_t type, uint8_t subtype) { + if (addToMatch) { + return addMatch(apid, type, subtype); + } else { + return removeMatch(apid, type, subtype); + } +} + +ReturnValue_t PacketMatchTree::cleanUpElement(iterator position) { + //TODO: What if first deletion fails? + factory.destroy(position.element->value); + return factory.destroy(position.element); +} diff --git a/tmtcpacket/packetmatcher/PacketMatchTree.h b/tmtcpacket/packetmatcher/PacketMatchTree.h new file mode 100644 index 00000000..81f1aafe --- /dev/null +++ b/tmtcpacket/packetmatcher/PacketMatchTree.h @@ -0,0 +1,41 @@ +/* + * PacketMatchTree.h + * + * Created on: 10.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_ +#define FRAMEWORK_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_ + +#include +#include +#include +#include + +class PacketMatchTree: public MatchTree, public HasReturnvaluesIF { +public: + PacketMatchTree(Node* root); + PacketMatchTree(iterator root); + PacketMatchTree(); + virtual ~PacketMatchTree(); + ReturnValue_t changeMatch(bool addToMatch, uint16_t apid, uint8_t type = 0, + uint8_t subtype = 0); + ReturnValue_t addMatch(uint16_t apid, uint8_t type = 0, + uint8_t subtype = 0); + ReturnValue_t removeMatch(uint16_t apid, uint8_t type = 0, + uint8_t subtype = 0); + ReturnValue_t initialize(); +protected: + ReturnValue_t cleanUpElement(iterator position); +private: + static const uint8_t N_POOLS = 4; + LocalPool factoryBackend; + PlacementFactory factory; + static const uint16_t POOL_SIZES[N_POOLS]; + static const uint16_t N_ELEMENTS[N_POOLS]; + iterator addElement(bool andBranch, uint8_t level, iterator position, TmPacketMinimal* content); +}; + +#endif /* FRAMEWORK_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_ */ + diff --git a/tmtcpacket/packetmatcher/ServiceMatcher.h b/tmtcpacket/packetmatcher/ServiceMatcher.h new file mode 100644 index 00000000..90a1d6e1 --- /dev/null +++ b/tmtcpacket/packetmatcher/ServiceMatcher.h @@ -0,0 +1,43 @@ +/* + * ServiceMatcher.h + * + * Created on: 09.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_TMTCPACKET_PACKETMATCHER_SERVICEMATCHER_H_ +#define FRAMEWORK_TMTCPACKET_PACKETMATCHER_SERVICEMATCHER_H_ + +#include +#include +#include + +class ServiceMatcher: public SerializeableMatcherIF { +private: + uint8_t service; +public: + ServiceMatcher(uint8_t setService) : + service(setService) { + } + bool match(TmPacketMinimal* packet) { + if (packet->getService() == service) { + return true; + } else { + return false; + } + } + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + return SerializeAdapter::serialize(&service, buffer, size, max_size, bigEndian); + } + uint32_t getSerializedSize() const { + return SerializeAdapter::getSerializedSize(&service); + } + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return SerializeAdapter::deSerialize(&service, buffer, size, bigEndian); + } +}; + + +#endif /* FRAMEWORK_TMTCPACKET_PACKETMATCHER_SERVICEMATCHER_H_ */ diff --git a/tmtcpacket/packetmatcher/SubserviceMatcher.h b/tmtcpacket/packetmatcher/SubserviceMatcher.h new file mode 100644 index 00000000..51c00baf --- /dev/null +++ b/tmtcpacket/packetmatcher/SubserviceMatcher.h @@ -0,0 +1,44 @@ +/* + * SubserviceMatcher.h + * + * Created on: 09.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_ +#define FRAMEWORK_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_ + +#include +#include +#include + +class SubServiceMatcher: public SerializeableMatcherIF { +public: + SubServiceMatcher(uint8_t subService) : + subService(subService) { + } + bool match(TmPacketMinimal* packet) { + if (packet->getSubService() == subService) { + return true; + } else { + return false; + } + } + ReturnValue_t serialize(uint8_t** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + return SerializeAdapter::serialize(&subService, buffer, size, max_size, bigEndian); + } + uint32_t getSerializedSize() const { + return SerializeAdapter::getSerializedSize(&subService); + } + ReturnValue_t deSerialize(const uint8_t** buffer, int32_t* size, + bool bigEndian) { + return SerializeAdapter::deSerialize(&subService, buffer, size, bigEndian); + } +private: + uint8_t subService; +}; + + + +#endif /* FRAMEWORK_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_ */ diff --git a/tmtcpacket/pus/TcPacket.cpp b/tmtcpacket/pus/TcPacket.cpp new file mode 100644 index 00000000..29f67443 --- /dev/null +++ b/tmtcpacket/pus/TcPacket.cpp @@ -0,0 +1,74 @@ +/* + * TcPacket.cpp + * + * Created on: 18.06.2012 + * Author: baetz + */ + +#include +#include +#include +template +TcPacket::TcPacket() : TcPacketBase( (uint8_t*)&this->local_data ) { + memset( &this->local_data, 0, sizeof(this->local_data) ); + //Set all constant elements in header according to ECSS E-70-41A (2003) + //Primary header: + //Set APID to idle packet + this->local_data.primary.packet_id_h = 0b00011000; + this->setAPID( APID_IDLE_PACKET ); + //Set Sequence Flags to "stand-alone packet" + this->local_data.primary.sequence_control_h = 0b11000000; + //Set packet size to size of data field header + CRC + this->setPacketDataLength( sizeof(this->local_data.data_field) + CRC_SIZE ); + + //Data Field Header: + //Set CCSDS_secondary_header_flag to 0, version number to 001 and ack to 0000 + this->local_data.data_field.version_type_ack = 0b00010000; + +} + +template +TcPacket::TcPacket( uint16_t new_apid, uint8_t new_ack, uint8_t new_service, uint8_t new_subservice, uint16_t new_sequence_count) : TcPacketBase( (uint8_t*)&this->local_data ) { + memset( &this->local_data, 0, sizeof(this->local_data) ); + //Set all constant elements in header according to ECSS E-70-41A (2003) + //Primary header: + //Set APID to idle packet + this->local_data.primary.packet_id_h = 0b00011000; + this->setAPID( new_apid ); + //Set Sequence Flags to "stand-alone packet" + this->local_data.primary.sequence_control_h = 0b11000000; + this->setPacketSequenceCount( new_sequence_count ); + this->setPacketDataLength( sizeof(this->local_data.data_field) + CRC_SIZE ); + + //Data Field Header: + //Set CCSDS_secondary_header_flag to 0, version number to 001 and ack to 0000 + this->local_data.data_field.version_type_ack = 0b00010000; + this->local_data.data_field.version_type_ack |= ( new_ack & 0x0F); + this->local_data.data_field.service_type = new_service; + this->local_data.data_field.service_subtype = new_subservice; + this->setErrorControl(); + +} + +template +TcPacket< byte_size >::~TcPacket() { +} + +template +bool TcPacket::addApplicationData( uint8_t* newData, uint32_t amount ) { + if ( amount <= ( sizeof(this->local_data.application_data) - CRC_SIZE ) ) { + memcpy( this->local_data.application_data, newData, amount ); + this->setPacketDataLength( amount + sizeof(this->local_data.data_field) + CRC_SIZE - 1 ); + this->setErrorControl(); + return true; + } else { + return false; + } +} + +template class TcPacket; +template class TcPacket<32>; +template class TcPacket<64>; +template class TcPacket<128>; +template class TcPacket<256>; +template class TcPacket<1024>; diff --git a/tmtcpacket/pus/TcPacket.h b/tmtcpacket/pus/TcPacket.h new file mode 100644 index 00000000..a3cfb757 --- /dev/null +++ b/tmtcpacket/pus/TcPacket.h @@ -0,0 +1,83 @@ +#ifndef TCPACKET_H_ +#define TCPACKET_H_ + +#include + +/** + * This constant defines the minimum size of a valid PUS Telecommand Packet. + */ +#define TC_PACKET_MIN_SIZE (sizeof(CCSDSPrimaryHeader) + sizeof(PUSTcDataFieldHeader) + 2) + +/** + * The TcPacket class is a representation of a ECSS PUS Telecommand packet. + + * The template parameter is used to instantiate the class with different + * maximum Application Data sizes (to avoid wasting stack size). + * Telecommand packets are mainly created on ground, which are accessed with + * the TcPacketBase class. However, there are occasions when command packets + * are created on-board, for example when commanding another PUS compatible + * device such as the star trackers. + * These packets can be created with the TcPacket class. + * A pointer to \c local_data is passed to the \c TcPacketBase parent class, + * so the parent's methods are reachable. + * @t_param byte_size The maximum size of the complete packet (including CRC + * and headers) + * @ingroup tmtcpackets + */ +template +class TcPacket : public TcPacketBase { +private: + /** + * This structure defines the data structure of a Telecommand Packet as + * local data. + * + * There's a buffer which corresponds to the Telecommand Application Data + * Field with a maximum size of \c byte_size. + */ + struct TcPacketData { + CCSDSPrimaryHeader primary; + PUSTcDataFieldHeader data_field; + uint8_t application_data[byte_size - sizeof(CCSDSPrimaryHeader) - sizeof(PUSTcDataFieldHeader)]; + }; + /** + * This is the data representation of the class. + */ + TcPacketData local_data; +public: + /** + * This is the default constructor of the class. + * + * It sets all values to default for a CCSDS Idle Packet (i.e. APID is 2047). + */ + TcPacket( void ); + /** + * A constructor which directly sets all relevant header information. + * @param apid Sets the packet's APID field. + * @param ack Set's the packet's Ack field, + * which specifies number and size of verification packets returned + * for this command. + * @param service Sets the packet's Service ID field. + * This specifies the destination service. + * @param subservice Sets the packet's Service Subtype field. + * This specifies the destination sub-service. + * @param sequence_count Sets the packet's Source Sequence Count field. + */ + TcPacket( uint16_t apid, uint8_t ack, uint8_t service, uint8_t subservice, uint16_t sequence_count = 0 ); + /** + * This is the empty default destructor of the class. + */ + ~TcPacket( void ); + /** + * With this call, application data can be added to the Application Data + * buffer. + * The Error Control Field is updated automatically. + * @param data The data to add to the Application Data field. + * @param size The size of the data to add. + * @return @li \c true if \c size is smaller than the Application Data + * buffer. + * @li \c false else. + */ + bool addApplicationData( uint8_t* data, uint32_t size ); +}; + +#endif /* TCPACKET_H_ */ diff --git a/tmtcpacket/pus/TcPacketBase.cpp b/tmtcpacket/pus/TcPacketBase.cpp new file mode 100644 index 00000000..593e5937 --- /dev/null +++ b/tmtcpacket/pus/TcPacketBase.cpp @@ -0,0 +1,74 @@ +/* + * TcPacketBase.cpp + * + * Created on: 18.06.2012 + * Author: baetz + */ + +#include +#include +#include + +TcPacketBase::TcPacketBase( const uint8_t* set_data ) : SpacePacketBase( set_data ) { + this->tc_data = (TcPacketPointer*)set_data; +} + +TcPacketBase::~TcPacketBase() { + //Nothing to do. +} + +uint8_t TcPacketBase::getService() { + return this->tc_data->data_field.service_type; +} + +uint8_t TcPacketBase::getSubService() { + return this->tc_data->data_field.service_subtype; +} + +uint8_t TcPacketBase::getAcknowledgeFlags() { + return this->tc_data->data_field.version_type_ack & 0b00001111; +} + +const uint8_t* TcPacketBase::getApplicationData() const { + return &this->tc_data->data; +} + +uint16_t TcPacketBase::getApplicationDataSize() { + return this->getPacketDataLength() - sizeof(this->tc_data->data_field) - CRC_SIZE + 1; +} + +uint16_t TcPacketBase::getErrorControl() { + uint16_t size = this->getApplicationDataSize() + CRC_SIZE; + uint8_t* p_to_buffer = &this->tc_data->data; + return ( p_to_buffer[size - 2] << 8 ) + p_to_buffer[size - 1]; +} + +void TcPacketBase::setErrorControl() { + uint32_t full_size = this->getFullSize(); + uint16_t crc = ::Calculate_CRC ( this->getWholeData(), full_size - CRC_SIZE ); + uint32_t size = this->getApplicationDataSize(); + (&tc_data->data)[ size ] = ( crc & 0XFF00) >> 8; // CRCH + (&tc_data->data)[ size + 1 ] = ( crc ) & 0X00FF; // CRCL +} + +void TcPacketBase::setData(const uint8_t* p_Data) { + this->SpacePacketBase::setData( p_Data ); + this->tc_data = (TcPacketPointer*)p_Data; +} + +uint8_t TcPacketBase::getSecondaryHeaderFlag() { + return (this->tc_data->data_field.version_type_ack & 0b10000000) >> 7; +} + +uint8_t TcPacketBase::getPusVersionNumber() { + return (this->tc_data->data_field.version_type_ack & 0b01110000) >> 4; +} + +void TcPacketBase::print() { + uint8_t * wholeData = this->getWholeData(); + debug << "TcPacket contains: " << std::endl; + for (uint8_t count = 0; count < this->getFullSize(); ++count ) { + debug << std::hex << (uint16_t)wholeData[count] << " "; + } + debug << std::dec << std::endl; +} diff --git a/tmtcpacket/pus/TcPacketBase.h b/tmtcpacket/pus/TcPacketBase.h new file mode 100644 index 00000000..eaeddcd8 --- /dev/null +++ b/tmtcpacket/pus/TcPacketBase.h @@ -0,0 +1,170 @@ +#ifndef TCPACKETBASE_H_ +#define TCPACKETBASE_H_ + +#include + +/** + * This struct defines a byte-wise structured PUS TC Data Field Header. + * Any optional fields in the header must be added or removed here. + * Currently, the Source Id field is present with one byte. + * @ingroup tmtcpackets + */ +struct PUSTcDataFieldHeader { + uint8_t version_type_ack; + uint8_t service_type; + uint8_t service_subtype; + uint8_t source_id; +}; + +/** + * This struct defines the data structure of a PUS Telecommand Packet when + * accessed via a pointer. + * @ingroup tmtcpackets + */ +struct TcPacketPointer { + CCSDSPrimaryHeader primary; + PUSTcDataFieldHeader data_field; + uint8_t data; +}; + +/** + * This class is the basic data handler for any ECSS PUS Telecommand packet. + * + * In addition to \SpacePacketBase, the class provides methods to handle + * the standardized entries of the PUS TC Packet Data Field Header. + * It does not contain the packet data itself but a pointer to the + * data must be set on instantiation. An invalid pointer may cause + * damage, as no getter method checks data validity. Anyway, a NULL + * check can be performed by making use of the getWholeData method. + * @ingroup tmtcpackets + */ +class TcPacketBase : public SpacePacketBase { +protected: + /** + * A pointer to a structure which defines the data structure of + * the packet's data. + * + * To be hardware-safe, all elements are of byte size. + */ + TcPacketPointer* tc_data; +public: + /** + * With this constant for the acknowledge field responses on all levels are expected. + */ + static const uint8_t ACK_ALL = 0b1111; + /** + * With this constant for the acknowledge field a response on acceptance is expected. + */ + static const uint8_t ACK_ACCEPTANCE = 0b0001; + /** + * With this constant for the acknowledge field a response on start of execution is expected. + */ + static const uint8_t ACK_START = 0b0010; + /** + * With this constant for the acknowledge field responses on execution steps are expected. + */ + static const uint8_t ACK_STEP = 0b0100; + /** + * With this constant for the acknowledge field a response on completion is expected. + */ + static const uint8_t ACK_COMPLETION = 0b1000; + /** + * With this constant for the acknowledge field no responses are expected. + */ + static const uint8_t ACK_NONE = 0b000; + /** + * This is the default constructor. + * It sets its internal data pointer to the address passed and also + * forwards the data pointer to the parent SpacePacketBase class. + * @param set_address The position where the packet data lies. + */ + TcPacketBase( const uint8_t* set_data ); + /** + * This is the empty default destructor. + */ + virtual ~TcPacketBase(); + /** + * This command returns the CCSDS Secondary Header Flag. + * It shall always be zero for PUS Packets. This is the + * highest bit of the first byte of the Data Field Header. + * @return the CCSDS Secondary Header Flag + */ + uint8_t getSecondaryHeaderFlag(); + /** + * This command returns the TC Packet PUS Version Number. + * The version number of ECSS PUS 2003 is 1. + * It consists of the second to fourth highest bits of the + * first byte. + * @return + */ + uint8_t getPusVersionNumber(); + /** + * This is a getter for the packet's Ack field, which are the lowest four + * bits of the first byte of the Data Field Header. + * + * It is packed in a uint8_t variable. + * @return The packet's PUS Ack field. + */ + uint8_t getAcknowledgeFlags(); + /** + * This is a getter for the packet's PUS Service ID, which is the second + * byte of the Data Field Header. + * @return The packet's PUS Service ID. + */ + uint8_t getService(); + /** + * This is a getter for the packet's PUS Service Subtype, which is the + * third byte of the Data Field Header. + * @return The packet's PUS Service Subtype. + */ + uint8_t getSubService(); + /** + * This is a getter for a pointer to the packet's Application data. + * + * These are the bytes that follow after the Data Field Header. They form + * the packet's application data. + * @return A pointer to the PUS Application Data. + */ + const uint8_t* getApplicationData() const; + /** + * This method calculates the size of the PUS Application data field. + * + * It takes the information stored in the CCSDS Packet Data Length field + * and subtracts the Data Field Header size and the CRC size. + * @return The size of the PUS Application Data (without Error Control + * field) + */ + uint16_t getApplicationDataSize(); + /** + * This getter returns the Error Control Field of the packet. + * + * The field is placed after any possible Application Data. If no + * Application Data is present there's still an Error Control field. It is + * supposed to be a 16bit-CRC. + * @return The PUS Error Control + */ + uint16_t getErrorControl(); + /** + * With this method, the Error Control Field is updated to match the + * current content of the packet. + */ + void setErrorControl(); + /** + * With this method, the packet data pointer can be redirected to another + * location. + * + * This call overwrites the parent's setData method to set both its + * \c tc_data pointer and the parent's \c data pointer. + * + * @param p_data A pointer to another PUS Telecommand Packet. + */ + void setData( const uint8_t* p_data ); + /** + * This is a debugging helper method that prints the whole packet content + * to the screen. + */ + void print(); +}; + + +#endif /* TCPACKETBASE_H_ */ diff --git a/tmtcpacket/pus/TcPacketStored.cpp b/tmtcpacket/pus/TcPacketStored.cpp new file mode 100644 index 00000000..67542b79 --- /dev/null +++ b/tmtcpacket/pus/TcPacketStored.cpp @@ -0,0 +1,104 @@ + +/* + * TcPacketStored.cpp + * + * Created on: 20.11.2012 + * Author: baetz + */ + +#include +#include +#include +#include +#include + +TcPacketStored::TcPacketStored( store_address_t setAddress) : TcPacketBase(NULL), storeAddress(setAddress) { + this->setStoreAddress( this->storeAddress ); +} + +TcPacketStored::TcPacketStored( uint16_t apid, uint8_t ack, uint8_t service, uint8_t subservice, + uint8_t sequence_count, const uint8_t* data, uint32_t size) : TcPacketBase(NULL) { + this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; + if ( this->checkAndSetStore() ) { + TcPacket temp_packet( apid, ack, service, subservice, sequence_count); + uint8_t* p_data = NULL; + ReturnValue_t returnValue = this->store->getFreeElement( &this->storeAddress, (TC_PACKET_MIN_SIZE + size), &p_data ); + if ( returnValue == this->store->RETURN_OK ) { + memcpy(p_data, temp_packet.getWholeData(), temp_packet.getFullSize() ); + this->setData( p_data ); + memcpy( &tc_data->data, data, size ); + this->setPacketDataLength( size + sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1 ); + this->setErrorControl(); + } + } +} + +TcPacketStored::TcPacketStored() : TcPacketBase(NULL) { + this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; + this->checkAndSetStore(); + +} + +//TODO: return code? +void TcPacketStored::deletePacket() { + this->store->deleteData( this->storeAddress ); + this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; + this->setData( NULL ); +} + +bool TcPacketStored::checkAndSetStore() { + if (this->store == NULL) { + this->store = objectManager->get( objects::TC_STORE ); + if ( this->store == NULL ) { + error << "TcPacketStored::TcPacketStored: TC Store not found!" << std::endl; + return false; + } + } + return true; +} + +void TcPacketStored::setStoreAddress(store_address_t setAddress) { + this->storeAddress = setAddress; + const uint8_t* temp_data = NULL; + uint32_t temp_size; + ReturnValue_t status = StorageManagerIF::RETURN_FAILED; + if ( this->checkAndSetStore() ) { + status = this->store->getData(this->storeAddress, &temp_data, &temp_size ); + } + if (status == StorageManagerIF::RETURN_OK) { + this->setData(temp_data); + } else { + this->setData(NULL); + this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; + } +} + +store_address_t TcPacketStored::getStoreAddress() { + return this->storeAddress; +} + +bool TcPacketStored::isSizeCorrect() { + const uint8_t* temp_data = NULL; + uint32_t temp_size; + ReturnValue_t status = this->store->getData(this->storeAddress, &temp_data, &temp_size ); + if (status == StorageManagerIF::RETURN_OK) { + if (this->getFullSize() == temp_size ) { + return true; + } + } + return false; +} + +StorageManagerIF* TcPacketStored::store = NULL; + +TcPacketStored::TcPacketStored(const uint8_t* data, uint32_t size) : TcPacketBase(data) { + if (getFullSize() != size) { + return; + } + if ( this->checkAndSetStore() ) { + ReturnValue_t status = store->addData(&storeAddress, data, size); + if (status != HasReturnvaluesIF::RETURN_OK) { + this->setData(NULL); + } + } +} diff --git a/tmtcpacket/pus/TcPacketStored.h b/tmtcpacket/pus/TcPacketStored.h new file mode 100644 index 00000000..58371cbc --- /dev/null +++ b/tmtcpacket/pus/TcPacketStored.h @@ -0,0 +1,104 @@ +#ifndef TCPACKETSTORED_H_ +#define TCPACKETSTORED_H_ + +#include +#include + +/** + * This class generates a ECSS PUS Telecommand packet within a given + * intermediate storage. + * As most packets are passed between tasks with the help of a storage + * anyway, it seems logical to create a Packet-In-Storage access class + * which saves the user almost all storage handling operation. + * Packets can both be newly created with the class and be "linked" to + * packets in a store with the help of a storeAddress. + * @ingroup tmtcpackets + */ +class TcPacketStored : public TcPacketBase { +private: + /** + * This is a pointer to the store all instances of the class use. + * If the store is not yet set (i.e. \c store is NULL), every constructor + * call tries to set it and throws an error message in case of failures. + * The default store is objects::TC_STORE. + */ + static StorageManagerIF* store; + /** + * The address where the packet data of the object instance is stored. + */ + store_address_t storeAddress; + /** + * A helper method to check if a store is assigned to the class. + * If not, the method tries to retrieve the store from the global + * ObjectManager. + * @return @li \c true if the store is linked or could be created. + * @li \c false otherwise. + */ + bool checkAndSetStore(); +public: + /** + * This is a default constructor which does not set the data pointer. + * However, it does try to set the packet store. + */ + TcPacketStored(); + /** + * With this constructor, the class instance is linked to an existing + * packet in the packet store. + * The packet content is neither checked nor changed with this call. If + * the packet could not be found, the data pointer is set to NULL. + */ + TcPacketStored( store_address_t setAddress ); + /** + * With this constructor, new space is allocated in the packet store and + * a new PUS Telecommand Packet is created there. + * Packet Application Data passed in data is copied into the packet. + * @param apid Sets the packet's APID field. + * @param ack Set's the packet's Ack field, + * which specifies number and size of verification packets returned + * for this command. + * @param service Sets the packet's Service ID field. + * This specifies the destination service. + * @param subservice Sets the packet's Service Subtype field. + * This specifies the destination sub-service. + * @param sequence_count Sets the packet's Source Sequence Count field. + * @param data The data to be copied to the Application Data Field. + * @param size The amount of data to be copied. + */ + TcPacketStored( uint16_t apid, uint8_t ack, uint8_t service, uint8_t subservice, uint8_t sequence_count = 0, const uint8_t* data = NULL, uint32_t size = 0 ); + /** + * Another constructor to create a TcPacket from a raw packet stream. + * Takes the data and adds it unchecked to the TcStore. + * @param data Pointer to the complete TC Space Packet. + * @param Size size of the packet. + */ + TcPacketStored( const uint8_t* data, uint32_t size); + /** + * This is a getter for the current store address of the packet. + * @return The current store address. The (raw) value is \c StorageManagerIF::INVALID_ADDRESS if + * the packet is not linked. + */ + store_address_t getStoreAddress(); + /** + * With this call, the packet is deleted. + * It removes itself from the store and sets its data pointer to NULL. + */ + void deletePacket(); + /** + * With this call, a packet can be linked to another store. This is useful + * if the packet is a class member and used for more than one packet. + * @param setAddress The new packet id to link to. + */ + void setStoreAddress( store_address_t setAddress ); + /** + * This method performs a size check. + * It reads the stored size and compares it with the size entered in the + * packet header. This class is the optimal place for such a check as it + * has access to both the header data and the store. + * @return true if size is correct, false if packet is not registered in + * store or size is incorrect. + */ + bool isSizeCorrect(); +}; + + +#endif /* TCPACKETSTORED_H_ */ diff --git a/tmtcpacket/pus/TmPacket.cpp b/tmtcpacket/pus/TmPacket.cpp new file mode 100644 index 00000000..5855ef8a --- /dev/null +++ b/tmtcpacket/pus/TmPacket.cpp @@ -0,0 +1,80 @@ +/* + * TmPacket.cpp + * + * Created on: 18.06.2012 + * Author: baetz + */ + +#include +#include +template +TmPacket::TmPacket() : TmPacketBase( (uint8_t*)&this->local_data ) { + memset( &this->local_data, 0, sizeof(this->local_data) ); + //Set all constant elements in header according to ECSS E-70-41A (2003) + //Primary header: + //Set APID to idle packet + local_data.primary.packet_id_h = 0b00001000; + setAPID( APID_IDLE_PACKET ); + //Set Sequence Flags to "stand-alone packet" + local_data.primary.sequence_control_h = 0b11000000; + //Set packet size to size of data field header + CRC + setPacketDataLength( sizeof(this->local_data.data_field) + CRC_SIZE ); + if (checkAndSetStamper()) { + timeStamper->addTimeStamp(local_data.data_field.time, sizeof(local_data.data_field.time)); + } + //Data Field Header: + //Set CCSDS_secondary_header_flag to 0, version number to 001 and ack to 0000 + this->local_data.data_field.version_type_ack = 0b00010000; + +} + +template +TmPacket::TmPacket( uint16_t new_apid, uint8_t new_service, uint8_t new_subservice, uint8_t new_packet_counter ) : TmPacketBase( (uint8_t*)&this->local_data ) { + memset( &this->local_data, 0, sizeof(this->local_data) ); + //Set all constant elements in header according to ECSS E-70-41A (2003) + //Primary header: + //Set APID to idle packet + this->local_data.primary.packet_id_h = 0b00001000; + this->setAPID( new_apid ); + //Set Sequence Flags to "stand-alone packet" + this->local_data.primary.sequence_control_h = 0b11000000; + //Set Data Length to minimum size (Data Field Header + CRC size) + this->setPacketDataLength( sizeof(this->local_data.data_field) + CRC_SIZE ); + + //Data Field Header: + //Set CCSDS_secondary_header_flag to 0, version number to 001 and ack to 0000 + this->local_data.data_field.version_type_ack = 0b00010000; + this->local_data.data_field.service_type = new_service; + this->local_data.data_field.service_subtype = new_subservice; + this->local_data.data_field.subcounter = new_packet_counter; + + if (checkAndSetStamper()) { + timeStamper->addTimeStamp(local_data.data_field.time, sizeof(local_data.data_field.time)); + } + +// this->local_data.data_field.destination = new_destination; + this->setErrorControl(); +} + +template +TmPacket< byte_size >::~TmPacket() { +} + +template +bool TmPacket::addSourceData( uint8_t* newData, uint32_t amount ) { + if ( amount <= ( sizeof(this->local_data.source_data) - CRC_SIZE ) ) { + memcpy( this->local_data.source_data, newData, amount ); + this->setPacketDataLength( amount + sizeof(this->local_data.data_field) + CRC_SIZE - 1 ); + this->setErrorControl(); + return true; + } else { + return false; + } +} + +template class TmPacket; +template class TmPacket<64>; +template class TmPacket<128>; +template class TmPacket<256>; +template class TmPacket<512>; +template class TmPacket<1024>; diff --git a/tmtcpacket/pus/TmPacket.h b/tmtcpacket/pus/TmPacket.h new file mode 100644 index 00000000..b6d7c412 --- /dev/null +++ b/tmtcpacket/pus/TmPacket.h @@ -0,0 +1,72 @@ +#ifndef TMPACKET_H_ +#define TMPACKET_H_ + +#include + +/** + * The TmPacket class is a representation of a ECSS PUS Telemetry packet. + + * The template parameter is used to instantiate the class with different + * maximum Application Data sizes (to avoid wasting stack size). Almost every + * on-board service is a source of telemetry packets. These packets are created + * with the TmPacket class, which is instantiated similar to the TcPacket + * class. + * A pointer to \c local_data is passed to the \c TmPacketBase parent class, + * so the parent's methods are reachable. + * @t_param byte_size The maximum size of the complete packet (including CRC + * and headers) + * @ingroup tmtcpackets + */ +template +class TmPacket : public TmPacketBase { +public: + /** + * This is the default constructor of the class. + * + * It sets all values to default for a CCSDS Idle Packet (i.e. APID is 2047). + */ + TmPacket( void ); + /** + * A constructor which directly sets all relevant header information. + * @param apid Sets the packet's APID field. + * @param service Sets the packet's Service ID field. + * This specifies the source service. + * @param subservice Sets the packet's Service Subtype field. + * This specifies the source sub-service. + * @param packet_counter Sets the Packet counter field of this packet. + */ + TmPacket( uint16_t apid, uint8_t service, uint8_t subservice, uint8_t packet_counter = 0 ); + /** + * This is the empty default destructor of the class. + */ + ~TmPacket( void ); + /** + * With this call, application data can be added to the Source Data + * buffer. + * The Error Control Field is updated automatically. + * @param data The data to add to the Source Data field. + * @param size The size of the data to add. + * @return @li \c true if \c size is smaller than the Source Data buffer. + * @li \c false else. + */ + bool addSourceData( uint8_t*, uint32_t ); +private: + /** + * This structure defines the data structure of a Telemetry Packet as + * local data. + * + * There's a buffer which corresponds to the Telemetry Source Data Field + * with a maximum size of \c byte_size. + */ + struct TmPacketData { + CCSDSPrimaryHeader primary; + PUSTmDataFieldHeader data_field; + uint8_t source_data[byte_size - sizeof(CCSDSPrimaryHeader) - sizeof(PUSTmDataFieldHeader)]; + }; + /** + * This is the data representation of the class. + */ + TmPacketData local_data; +}; + +#endif /* TMPACKET_H_ */ diff --git a/tmtcpacket/pus/TmPacketBase.cpp b/tmtcpacket/pus/TmPacketBase.cpp new file mode 100644 index 00000000..3bd18283 --- /dev/null +++ b/tmtcpacket/pus/TmPacketBase.cpp @@ -0,0 +1,78 @@ +/* + * TmPacketBase.cpp + * + * Created on: 18.06.2012 + * Author: baetz + */ + +#include +#include +#include +#include + +TmPacketBase::TmPacketBase( uint8_t* set_data ) : SpacePacketBase( set_data ) { + this->tm_data = (TmPacketPointer*)set_data; +} + +TmPacketBase::~TmPacketBase() { + //Nothing to do. +} + +uint8_t TmPacketBase::getService() { + return this->tm_data->data_field.service_type; +} + +uint8_t TmPacketBase::getSubService() { + return this->tm_data->data_field.service_subtype; +} + +uint8_t* TmPacketBase::getSourceData() { + return &this->tm_data->data; +} + +uint16_t TmPacketBase::getSourceDataSize() { + return this->getPacketDataLength() - sizeof(this->tm_data->data_field) - CRC_SIZE + 1; +} + +uint16_t TmPacketBase::getErrorControl() { + uint32_t size = this->getSourceDataSize() + CRC_SIZE; + uint8_t* p_to_buffer = &this->tm_data->data; + return ( p_to_buffer[size - 2] << 8 ) + p_to_buffer[size - 1]; +} + + +void TmPacketBase::setErrorControl() { + uint32_t full_size = this->getFullSize(); + uint16_t crc = ::Calculate_CRC ( this->getWholeData(), full_size - CRC_SIZE ); + uint32_t size = this->getSourceDataSize(); + this->getSourceData()[ size ] = ( crc & 0XFF00) >> 8; // CRCH + this->getSourceData()[ size + 1 ] = ( crc ) & 0X00FF; // CRCL +} + +void TmPacketBase::setData(const uint8_t* p_Data) { + this->SpacePacketBase::setData( p_Data ); + this->tm_data = (TmPacketPointer*)p_Data; +} + +void TmPacketBase::print() { + /*uint8_t * wholeData = this->getWholeData(); + debug << "TmPacket contains: " << std::endl; + for (uint8_t count = 0; count < this->getFullSize(); ++count ) { + debug << std::hex << (uint16_t)wholeData[count] << " "; + } + debug << std::dec << std::endl;*/ +} + +bool TmPacketBase::checkAndSetStamper() { + if (timeStamper == NULL) { + //TODO: Adjust name? + timeStamper = objectManager->get( objects::TIME_MANAGER ); + if ( timeStamper == NULL ) { + error << "TmPacketBase::checkAndSetStamper: Stamper not found!" << std::endl; + return false; + } + } + return true; +} + +TimeStamperIF* TmPacketBase::timeStamper = NULL; diff --git a/tmtcpacket/pus/TmPacketBase.h b/tmtcpacket/pus/TmPacketBase.h new file mode 100644 index 00000000..beda760f --- /dev/null +++ b/tmtcpacket/pus/TmPacketBase.h @@ -0,0 +1,143 @@ +#ifndef TMPACKETBASE_H_ +#define TMPACKETBASE_H_ + +#include +#include + +/** + * This struct defines a byte-wise structured PUS TM Data Field Header. + * Any optional fields in the header must be added or removed here. + * Currently, no Destination field is present, but an eigth-byte representation + * for a time tag [TBD]. + * @ingroup tmtcpackets + */ +struct PUSTmDataFieldHeader { + uint8_t version_type_ack; + uint8_t service_type; + uint8_t service_subtype; + uint8_t subcounter; +// uint8_t destination; + uint8_t time[TimeStamperIF::MISSION_TIMESTAMP_SIZE]; +}; + +/** + * This struct defines the data structure of a PUS Telecommand Packet when + * accessed via a pointer. + * @ingroup tmtcpackets + */ +struct TmPacketPointer { + CCSDSPrimaryHeader primary; + PUSTmDataFieldHeader data_field; + uint8_t data; +}; + +//TODO: add getTime, getSubcounter, getDestionation (if required) + +/** + * This class is the basic data handler for any ECSS PUS Telemetry packet. + * + * In addition to \SpacePacketBase, the class provides methods to handle + * the standardized entries of the PUS TM Packet Data Field Header. + * It does not contain the packet data itself but a pointer to the + * data must be set on instantiation. An invalid pointer may cause + * damage, as no getter method checks data validity. Anyway, a NULL + * check can be performed by making use of the getWholeData method. + * @ingroup tmtcpackets + */ +class TmPacketBase : public SpacePacketBase { +public: + /** + * This is the default constructor. + * It sets its internal data pointer to the address passed and also + * forwards the data pointer to the parent SpacePacketBase class. + * @param set_address The position where the packet data lies. + */ + TmPacketBase( uint8_t* set_data ); + /** + * This is the empty default destructor. + */ + virtual ~TmPacketBase(); + /** + * This is a getter for the packet's PUS Service ID, which is the second + * byte of the Data Field Header. + * @return The packet's PUS Service ID. + */ + uint8_t getService(); + /** + * This is a getter for the packet's PUS Service Subtype, which is the + * third byte of the Data Field Header. + * @return The packet's PUS Service Subtype. + */ + uint8_t getSubService(); + /** + * This is a getter for a pointer to the packet's Source data. + * + * These are the bytes that follow after the Data Field Header. They form + * the packet's source data. + * @return A pointer to the PUS Source Data. + */ + uint8_t* getSourceData(); + /** + * This method calculates the size of the PUS Source data field. + * + * It takes the information stored in the CCSDS Packet Data Length field + * and subtracts the Data Field Header size and the CRC size. + * @return The size of the PUS Source Data (without Error Control field) + */ + uint16_t getSourceDataSize(); + /** + * This getter returns the Error Control Field of the packet. + * + * The field is placed after any possible Source Data. If no + * Source Data is present there's still an Error Control field. It is + * supposed to be a 16bit-CRC. + * @return The PUS Error Control + */ + uint16_t getErrorControl(); + /** + * With this method, the Error Control Field is updated to match the + * current content of the packet. + */ + void setErrorControl(); + /** + * With this method, the packet data pointer can be redirected to another + * location. + * + * This call overwrites the parent's setData method to set both its + * \c tc_data pointer and the parent's \c data pointer. + * + * @param p_data A pointer to another PUS Telemetry Packet. + */ + void setData( const uint8_t* p_Data ); + /** + * This is a debugging helper method that prints the whole packet content + * to the screen. + */ + void print(); + /** + * This constant defines the minimum size of a valid PUS Telemetry Packet. + */ + static const uint32_t TM_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) + sizeof(PUSTmDataFieldHeader) + 2); + static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048; //!< Maximum size of a TM Packet in this mission. +protected: + /** + * A pointer to a structure which defines the data structure of + * the packet's data. + * + * To be hardware-safe, all elements are of byte size. + */ + TmPacketPointer* tm_data; + /** + * The timeStamper is responsible for adding a timestamp to the packet. + * It is initialized lazy. + */ + static TimeStamperIF* timeStamper; + /** + * Checks if a time stamper is available and tries to set it if not. + * @return Returns false if setting failed. + */ + bool checkAndSetStamper(); +}; + + +#endif /* TMPACKETBASE_H_ */ diff --git a/tmtcpacket/pus/TmPacketMinimal.cpp b/tmtcpacket/pus/TmPacketMinimal.cpp new file mode 100644 index 00000000..12a9e6e3 --- /dev/null +++ b/tmtcpacket/pus/TmPacketMinimal.cpp @@ -0,0 +1,21 @@ + +#include + +TmPacketMinimal::TmPacketMinimal(const uint8_t* set_data) : SpacePacketBase( set_data ) { + this->tm_data = (TmPacketMinimalPointer*)set_data; +} + +TmPacketMinimal::~TmPacketMinimal() { +} + +uint8_t TmPacketMinimal::getService() { + return tm_data->data_field.service_type; +} + +uint8_t TmPacketMinimal::getSubService() { + return tm_data->data_field.service_subtype; +} + +uint8_t TmPacketMinimal::getPacketSubcounter() { + return tm_data->data_field.subcounter; +} diff --git a/tmtcpacket/pus/TmPacketMinimal.h b/tmtcpacket/pus/TmPacketMinimal.h new file mode 100644 index 00000000..be8f1423 --- /dev/null +++ b/tmtcpacket/pus/TmPacketMinimal.h @@ -0,0 +1,80 @@ +/* + * TmPacketMinimal.h + * + * Created on: 09.03.2015 + * Author: baetz + */ + +#ifndef FRAMEWORK_TMTCPACKET_PUS_TMPACKETMINIMAL_H_ +#define FRAMEWORK_TMTCPACKET_PUS_TMPACKETMINIMAL_H_ + + +#include + +/** + * This is a minimal version of a PUS TmPacket without any variable field, or, + * in other words with Service Type, Subtype and subcounter only. + * This is required for handling TM packets with different APIDs with different + * secondary headers. + */ +class TmPacketMinimal : public SpacePacketBase { +public: + /** + * This is the default constructor. + * It sets its internal data pointer to the address passed and also + * forwards the data pointer to the parent SpacePacketBase class. + * @param set_address The position where the packet data lies. + */ + TmPacketMinimal( const uint8_t* set_data ); + /** + * This is the empty default destructor. + */ + virtual ~TmPacketMinimal(); + /** + * This is a getter for the packet's PUS Service ID, which is the second + * byte of the Data Field Header. + * @return The packet's PUS Service ID. + */ + uint8_t getService(); + /** + * This is a getter for the packet's PUS Service Subtype, which is the + * third byte of the Data Field Header. + * @return The packet's PUS Service Subtype. + */ + uint8_t getSubService(); + /** + * Returns the subcounter. + * @return the subcounter of the Data Field Header. + */ + uint8_t getPacketSubcounter(); + struct PUSTmMinimalHeader { + uint8_t version_type_ack; + uint8_t service_type; + uint8_t service_subtype; + uint8_t subcounter; + }; + + /** + * This struct defines the data structure of a PUS Telecommand Packet when + * accessed via a pointer. + * @ingroup tmtcpackets + */ + struct TmPacketMinimalPointer { + CCSDSPrimaryHeader primary; + PUSTmMinimalHeader data_field; + uint8_t rest; + }; + static const uint16_t MINIMUM_SIZE = sizeof(TmPacketMinimalPointer) -1; +protected: + /** + * A pointer to a structure which defines the data structure of + * the packet's data. + * + * To be hardware-safe, all elements are of byte size. + */ + TmPacketMinimalPointer* tm_data; +}; + + + +#endif /* FRAMEWORK_TMTCPACKET_PUS_TMPACKETMINIMAL_H_ */ diff --git a/tmtcpacket/pus/TmPacketStored.cpp b/tmtcpacket/pus/TmPacketStored.cpp new file mode 100644 index 00000000..a61faa5b --- /dev/null +++ b/tmtcpacket/pus/TmPacketStored.cpp @@ -0,0 +1,105 @@ +/* + * TmPacketStored.cpp + * + * Created on: 19.11.2012 + * Author: baetz + */ + +#include +#include +#include +#include +#include + +TmPacketStored::TmPacketStored( store_address_t setAddress ) : TmPacketBase(NULL), storeAddress(setAddress) { + this->setStoreAddress( this->storeAddress ); +} + +TmPacketStored::TmPacketStored( uint16_t apid, uint8_t service, + uint8_t subservice, uint8_t packet_counter, const uint8_t* data, uint32_t size,const uint8_t* headerData, uint32_t headerSize) : TmPacketBase(NULL) { + this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; + if ( this->checkAndSetStore() ) { + TmPacket temp_packet( apid, service, subservice, packet_counter ); + uint8_t* p_data = NULL; + ReturnValue_t returnValue = this->store->getFreeElement( &this->storeAddress, (TmPacketBase::TM_PACKET_MIN_SIZE + size + headerSize), &p_data ); + if ( returnValue == this->store->RETURN_OK ) { + memcpy(p_data, temp_packet.getWholeData(), temp_packet.getFullSize() ); + this->setData( p_data ); + memcpy( this->getSourceData(), headerData, headerSize ); + memcpy( this->getSourceData() + headerSize, data, size ); + this->setPacketDataLength( size + headerSize + sizeof(PUSTmDataFieldHeader) + CRC_SIZE - 1 ); + this->setErrorControl(); + } + } +} + +TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service, + uint8_t subservice, uint8_t packet_counter, SerializeIF* content, + SerializeIF* header) : TmPacketBase(NULL) { + storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; + if ( checkAndSetStore() ) { + TmPacket temp_packet( apid, service, subservice, packet_counter ); + uint32_t sourceDataSize = 0; + if (content != NULL) { + sourceDataSize += content->getSerializedSize(); + } + if (header != NULL) { + sourceDataSize += header->getSerializedSize(); + } + uint8_t* p_data = NULL; + ReturnValue_t returnValue = this->store->getFreeElement( &this->storeAddress, (TmPacketBase::TM_PACKET_MIN_SIZE + sourceDataSize), &p_data ); + if ( returnValue == store->RETURN_OK ) { + memcpy(p_data, temp_packet.getWholeData(), temp_packet.getFullSize() ); + this->setData( p_data ); + uint8_t* putDataHere = getSourceData(); + uint32_t size = 0; + if (header != NULL) { + header->serialize(&putDataHere, &size, sourceDataSize, true); + } + if (content != NULL) { + content->serialize(&putDataHere, &size, sourceDataSize, true); + } + this->setPacketDataLength( sourceDataSize + sizeof(PUSTmDataFieldHeader) + CRC_SIZE - 1 ); + this->setErrorControl(); + } + } +} + +store_address_t TmPacketStored::getStoreAddress() { + return this->storeAddress; +} + +void TmPacketStored::deletePacket() { + this->store->deleteData( this->storeAddress ); + this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; + this->setData( NULL ); +} + +void TmPacketStored::setStoreAddress(store_address_t setAddress) { + this->storeAddress = setAddress; + const uint8_t* temp_data = NULL; + uint32_t temp_size; + if (!checkAndSetStore()) { + return; + } + ReturnValue_t status = this->store->getData(this->storeAddress, &temp_data, &temp_size ); + if (status == StorageManagerIF::RETURN_OK) { + this->setData(temp_data); + } else { + this->setData(NULL); + this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; + } +} + +bool TmPacketStored::checkAndSetStore() { + if (this->store == NULL) { + this->store = objectManager->get( objects::TM_STORE ); + if ( this->store == NULL ) { + error << "TmPacketStored::TmPacketStored: TM Store not found!" << std::endl; + return false; + } + } + return true; +} + +StorageManagerIF* TmPacketStored::store = NULL; diff --git a/tmtcpacket/pus/TmPacketStored.h b/tmtcpacket/pus/TmPacketStored.h new file mode 100644 index 00000000..05037a22 --- /dev/null +++ b/tmtcpacket/pus/TmPacketStored.h @@ -0,0 +1,87 @@ +#ifndef TMPACKETSTORED_H_ +#define TMPACKETSTORED_H_ + +#include +#include +#include + +/** + * This class generates a ECSS PUS Telemetry packet within a given + * intermediate storage. + * As most packets are passed between tasks with the help of a storage + * anyway, it seems logical to create a Packet-In-Storage access class + * which saves the user almost all storage handling operation. + * Packets can both be newly created with the class and be "linked" to + * packets in a store with the help of a storeAddress. + * @ingroup tmtcpackets + */ +class TmPacketStored : public TmPacketBase { +private: + /** + * This is a pointer to the store all instances of the class use. + * If the store is not yet set (i.e. \c store is NULL), every constructor + * call tries to set it and throws an error message in case of failures. + * The default store is objects::TM_STORE. + */ + static StorageManagerIF* store; + /** + * The address where the packet data of the object instance is stored. + */ + store_address_t storeAddress; + /** + * A helper method to check if a store is assigned to the class. + * If not, the method tries to retrieve the store from the global + * ObjectManager. + * @return @li \c true if the store is linked or could be created. + * @li \c false otherwise. + */ + bool checkAndSetStore(); +public: + /** + * This is a default constructor which does not set the data pointer. + * However, it does try to set the packet store. + */ + TmPacketStored( store_address_t setAddress ); + /** + * With this constructor, new space is allocated in the packet store and + * a new PUS Telemetry Packet is created there. + * Packet Application Data passed in data is copied into the packet. The Application data is + * passed in two parts, first a header, then a data field. This allows building a Telemetry + * Packet from two separate data sources. + * @param apid Sets the packet's APID field. + * @param service Sets the packet's Service ID field. + * This specifies the source service. + * @param subservice Sets the packet's Service Subtype field. + * This specifies the source sub-service. + * @param packet_counter Sets the Packet counter field of this packet + * @param data The payload data to be copied to the Application Data Field + * @param size The amount of data to be copied. + * @param headerData The header Data of the Application field; will be copied in front of data + * @param headerSize The size of the headerDataF + */ + TmPacketStored( uint16_t apid, uint8_t service, uint8_t subservice, uint8_t packet_counter = 0, const uint8_t* data = NULL, uint32_t size = 0, const uint8_t* headerData = NULL, uint32_t headerSize = 0); + /** + * Another ctor to directly pass structured content and header data to the packet to avoid additional buffers. + */ + TmPacketStored( uint16_t apid, uint8_t service, uint8_t subservice, uint8_t packet_counter, SerializeIF* content, SerializeIF* header = NULL); + /** + * This is a getter for the current store address of the packet. + * @return The current store address. The (raw) value is \c StorageManagerIF::INVALID_ADDRESS if + * the packet is not linked. + */ + store_address_t getStoreAddress(); + /** + * With this call, the packet is deleted. + * It removes itself from the store and sets its data pointer to NULL. + */ + void deletePacket(); + /** + * With this call, a packet can be linked to another store. This is useful + * if the packet is a class member and used for more than one packet. + * @param setAddress The new packet id to link to. + */ + void setStoreAddress( store_address_t setAddress ); +}; + + +#endif /* TMPACKETSTORED_H_ */ diff --git a/tmtcservices/AcceptsTelecommandsIF.h b/tmtcservices/AcceptsTelecommandsIF.h new file mode 100644 index 00000000..236f6695 --- /dev/null +++ b/tmtcservices/AcceptsTelecommandsIF.h @@ -0,0 +1,43 @@ +#ifndef ACCEPTSTELECOMMANDSIF_H_ +#define ACCEPTSTELECOMMANDSIF_H_ + +#include + +/** + * @brief This interface is implemented by classes that are sinks for + * Telecommands. + * @details Any service receiving telecommands shall implement this interface + * and thus make the service id and the receiving message queue public. + */ +class AcceptsTelecommandsIF { +public: + static const uint8_t INTERFACE_ID = ACCEPTS_TELECOMMANDS_IF; + static const ReturnValue_t ACTIVITY_STARTED = MAKE_RETURN_CODE(1); + static const ReturnValue_t INVALID_SUBSERVICE = MAKE_RETURN_CODE(2); + static const ReturnValue_t ILLEGAL_APPLICATION_DATA = MAKE_RETURN_CODE(3); + static const ReturnValue_t SEND_TM_FAILED = MAKE_RETURN_CODE(4); + static const ReturnValue_t TIMEOUT = MAKE_RETURN_CODE(5); + /** + * @brief The virtual destructor as it is mandatory for C++ interfaces. + */ + virtual ~AcceptsTelecommandsIF() { + + } + /** + * @brief Getter for the service id. + * @details Any receiving service (at least any PUS service) shall have a + * service id. If the receiver can handle Telecommands, but for + * some reason has no service id, it shall return 0. + * @return The service id or 0. + */ + virtual uint16_t getIdentifier() = 0; + /** + * @brief This method returns the message queue id of the telecommand + * receiving message queue. + * @return The telecommand reception message queue id. + */ + virtual MessageQueueId_t getRequestQueue() = 0; +}; + + +#endif /* ACCEPTSTELECOMMANDSIF_H_ */ diff --git a/tmtcservices/AcceptsTelemetryIF.h b/tmtcservices/AcceptsTelemetryIF.h new file mode 100644 index 00000000..ee974aae --- /dev/null +++ b/tmtcservices/AcceptsTelemetryIF.h @@ -0,0 +1,26 @@ +#ifndef ACCEPTSTELEMETRYIF_H_ +#define ACCEPTSTELEMETRYIF_H_ + +#include +/** + * @brief This interface is implemented by classes that are sinks for + * Telemetry. + * @details Any object receiving telemetry shall implement this interface + * and thus make the service id and the receiving message queue public. + */ +class AcceptsTelemetryIF { +public: + /** + * @brief The virtual destructor as it is mandatory for C++ interfaces. + */ + virtual ~AcceptsTelemetryIF() { + } + /** + * @brief This method returns the message queue id of the telemetry + * receiving message queue. + * @return The telemetry reception message queue id. + */ + virtual MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel = 0) = 0; +}; + +#endif /* ACCEPTSTELEMETRYIF_H_ */ diff --git a/tmtcservices/AcceptsVerifyMessageIF.h b/tmtcservices/AcceptsVerifyMessageIF.h new file mode 100644 index 00000000..3a499c3d --- /dev/null +++ b/tmtcservices/AcceptsVerifyMessageIF.h @@ -0,0 +1,22 @@ +/* + * AcceptsVerifyMessageIF.h + * + * Created on: 23.11.2012 + * Author: baetz + */ + +#ifndef ACCEPTSVERIFICATIONMESSAGEIF_H_ +#define ACCEPTSVERIFICATIONMESSAGEIF_H_ + +#include + +class AcceptsVerifyMessageIF { +public: + virtual ~AcceptsVerifyMessageIF() { + + } + virtual MessageQueueId_t getVerificationQueue() = 0; +}; + + +#endif /* ACCEPTSVERIFICATIONMESSAGEIF_H_ */ diff --git a/tmtcservices/CommandingServiceBase.h b/tmtcservices/CommandingServiceBase.h new file mode 100644 index 00000000..825b23fa --- /dev/null +++ b/tmtcservices/CommandingServiceBase.h @@ -0,0 +1,486 @@ +#ifndef COMMANDINGSERVICEBASE_H_ +#define COMMANDINGSERVICEBASE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +template +class CommandingServiceBase: public SystemObject, + public AcceptsTelecommandsIF, + public ExecutableObjectIF, + public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = COMMAND_SERVICE_BASE; + static const ReturnValue_t EXECUTION_COMPLETE = MAKE_RETURN_CODE(1); + static const ReturnValue_t NO_STEP_MESSAGE = MAKE_RETURN_CODE(2); + static const ReturnValue_t OBJECT_BUSY = MAKE_RETURN_CODE(3); + static const ReturnValue_t BUSY = MAKE_RETURN_CODE(4); + static const ReturnValue_t INVALID_TC = MAKE_RETURN_CODE(5); + static const ReturnValue_t INVALID_OBJECT = MAKE_RETURN_CODE(6); + static const ReturnValue_t INVALID_REPLY = MAKE_RETURN_CODE(7); + + CommandingServiceBase(object_id_t setObjectId, uint16_t apid, + uint8_t service, uint8_t numberOfParallelCommands, + uint16_t commandTimeout_seconds, size_t queueDepth = 20); + virtual ~CommandingServiceBase(); + + virtual ReturnValue_t performOperation(); + + virtual uint16_t getIdentifier(); + + virtual MessageQueueId_t getRequestQueue(); + + virtual ReturnValue_t initialize(); + +protected: + struct CommandInfo { + struct tcInfo { + uint8_t ackFlags; + uint16_t tcPacketId; + uint16_t tcSequenceControl; + } tcInfo; + uint32_t uptimeOfStart; + uint8_t step; + uint8_t subservice; + STATE_T state; + Command_t command; + object_id_t objectId; + FIFO fifo; + }; + + const uint16_t apid; + + const uint8_t service; + + const uint16_t timeout_seconds; + + uint8_t tmPacketCounter; + + StorageManagerIF *IPCStore; + + StorageManagerIF *TCStore; + + MessageQueue commandQueue; + + MessageQueue requestQueue; + + VerificationReporter verificationReporter; + + FixedMap commandMap; + + uint32_t failureParameter1; //!< May be set be children to return a more precise failure condition. + uint32_t failureParameter2; //!< May be set be children to return a more precise failure condition. + + void sendTmPacket(uint8_t subservice, const uint8_t *data, + uint32_t dataLen); + void sendTmPacket(uint8_t subservice, object_id_t objectId, + const uint8_t *data, uint32_t dataLen); + + void sendTmPacket(uint8_t subservice, SerializeIF* content); + virtual ReturnValue_t isValidSubservice(uint8_t subservice) = 0; + + virtual ReturnValue_t prepareCommand(CommandMessage *message, + uint8_t subservice, const uint8_t *tcData, uint32_t tcDataLen, + STATE_T *state, object_id_t objectId) = 0; + + virtual ReturnValue_t handleReply(const CommandMessage *reply, + Command_t previousCommand, STATE_T *state, + CommandMessage *optionalNextCommand, object_id_t objectId, + bool *isStep) = 0; + + virtual ReturnValue_t getMessageQueueAndObject(uint8_t subservice, + const uint8_t *tcData, uint32_t tcDataLen, MessageQueueId_t *id, + object_id_t *objectId) = 0; + + virtual void handleUnrequestedReply(CommandMessage *reply); + + virtual void doPeriodicOperation(); + +private: + void handleCommandQueue(); + + void handleRequestQueue(); + + void rejectPacket(uint8_t reportId, TcPacketStored* packet, + ReturnValue_t errorCode); + + void acceptPacket(uint8_t reportId, TcPacketStored* packet); + + void startExecution(TcPacketStored *storedPacket, + typename FixedMap::Iterator *iter); + + void checkAndExecuteFifo( + typename FixedMap::Iterator *iter); + + void checkTimeout(); +}; + +template +CommandingServiceBase::CommandingServiceBase(object_id_t setObjectId, + uint16_t apid, uint8_t service, uint8_t numberOfParallelCommands, + uint16_t commandTimeout_seconds, size_t queueDepth) : + SystemObject(setObjectId), apid(apid), service(service), timeout_seconds( + commandTimeout_seconds), tmPacketCounter(0), IPCStore(NULL), TCStore( + NULL), commandQueue(queueDepth), requestQueue(20), commandMap( + numberOfParallelCommands), failureParameter1(0), failureParameter2( + 0) { +} + +template +CommandingServiceBase::~CommandingServiceBase() { +} + +template +ReturnValue_t CommandingServiceBase::performOperation() { + handleCommandQueue(); + handleRequestQueue(); + checkTimeout(); + doPeriodicOperation(); + return RETURN_OK; +} + +template +uint16_t CommandingServiceBase::getIdentifier() { + return service; +} + +template +MessageQueueId_t CommandingServiceBase::getRequestQueue() { + return requestQueue.getId(); +} + +template +ReturnValue_t CommandingServiceBase::initialize() { + ReturnValue_t result = SystemObject::initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + AcceptsTelemetryIF* packetForwarding = + objectManager->get( + objects::PUS_PACKET_FORWARDING); + PUSDistributorIF* distributor = objectManager->get( + objects::PUS_PACKET_DISTRIBUTOR); + if ((packetForwarding == NULL) && (distributor == NULL)) { + return RETURN_FAILED; + } + + distributor->registerService(this); + requestQueue.setDefaultDestination( + packetForwarding->getReportReceptionQueue()); + + IPCStore = objectManager->get(objects::IPC_STORE); + TCStore = objectManager->get(objects::TC_STORE); + + if ((IPCStore == NULL) || (TCStore == NULL)) { + return RETURN_FAILED; + } + + return RETURN_OK; + +} + +//Whole method works like this, but I don't like it. Leave it anyway. +template +void CommandingServiceBase::handleCommandQueue() { + CommandMessage reply, nextCommand; + ReturnValue_t result, sendResult = RETURN_OK; + bool isStep = false; + for (result = commandQueue.receiveMessage(&reply); result == RETURN_OK; + result = commandQueue.receiveMessage(&reply)) { + isStep = false; + typename FixedMap::CommandInfo>::Iterator iter; + if ((iter = commandMap.find(reply.getSender())) == commandMap.end()) { + handleUnrequestedReply(&reply); + continue; + } + nextCommand.setCommand(CommandMessage::CMD_NONE); + result = handleReply(&reply, iter->command, &iter->state, &nextCommand, + iter->objectId, &isStep); + switch (result) { + case EXECUTION_COMPLETE: + case RETURN_OK: + case NO_STEP_MESSAGE: + iter->command = nextCommand.getCommand(); + if (nextCommand.getCommand() != CommandMessage::CMD_NONE) { + sendResult = commandQueue.sendMessage(reply.getSender(), + &nextCommand); + } + if (sendResult == RETURN_OK) { + if (isStep) { + if (result != NO_STEP_MESSAGE) { + verificationReporter.sendSuccessReport( + TC_VERIFY::PROGRESS_SUCCESS, + iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId, + iter->tcInfo.tcSequenceControl, ++iter->step); + } + } else { + verificationReporter.sendSuccessReport( + TC_VERIFY::COMPLETION_SUCCESS, + iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId, + iter->tcInfo.tcSequenceControl, 0); + checkAndExecuteFifo(&iter); + } + } else { + if (isStep) { + nextCommand.clearCommandMessage(); + verificationReporter.sendFailureReport( + TC_VERIFY::PROGRESS_FAILURE, iter->tcInfo.ackFlags, + iter->tcInfo.tcPacketId, + iter->tcInfo.tcSequenceControl, sendResult, + ++iter->step, failureParameter1, failureParameter2); + } else { + nextCommand.clearCommandMessage(); + verificationReporter.sendFailureReport( + TC_VERIFY::COMPLETION_FAILURE, + iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId, + iter->tcInfo.tcSequenceControl, sendResult, + 0, failureParameter1, failureParameter2); + } + failureParameter1 = 0; + failureParameter2 = 0; + checkAndExecuteFifo(&iter); + } + break; + case INVALID_REPLY: + //might be just an unrequested reply at a bad moment + handleUnrequestedReply(&reply); + break; + default: + if (isStep) { + verificationReporter.sendFailureReport( + TC_VERIFY::PROGRESS_FAILURE, iter->tcInfo.ackFlags, + iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl, + result, ++iter->step, failureParameter1, failureParameter2); + } else { + verificationReporter.sendFailureReport( + TC_VERIFY::COMPLETION_FAILURE, iter->tcInfo.ackFlags, + iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl, + result, 0, failureParameter1, failureParameter2); + } + failureParameter1 = 0; + failureParameter2 = 0; + checkAndExecuteFifo(&iter); + break; + } + + } +} + +template +void CommandingServiceBase::handleRequestQueue() { + TmTcMessage message; + ReturnValue_t result; + store_address_t address; + TcPacketStored packet; + MessageQueueId_t queue; + object_id_t objectId; + for (result = requestQueue.receiveMessage(&message); result == RETURN_OK; + result = requestQueue.receiveMessage(&message)) { + address = message.getStorageId(); + packet.setStoreAddress(address); + + if ((packet.getSubService() == 0) + || (isValidSubservice(packet.getSubService()) != RETURN_OK)) { + rejectPacket(TC_VERIFY::START_FAILURE, &packet, INVALID_SUBSERVICE); + continue; + } + result = getMessageQueueAndObject(packet.getSubService(), + packet.getApplicationData(), packet.getApplicationDataSize(), + &queue, &objectId); + if (result != HasReturnvaluesIF::RETURN_OK) { + rejectPacket(TC_VERIFY::START_FAILURE, &packet, result); + continue; + } + + //is a command already active for the target object? + typename FixedMap::CommandInfo>::Iterator iter; + iter = commandMap.find(queue); + + if (iter != commandMap.end()) { + result = iter->fifo.insert(address); + if (result != RETURN_OK) { + rejectPacket(TC_VERIFY::START_FAILURE, &packet, OBJECT_BUSY); + } + } else { + CommandInfo newInfo; //Info will be set by startExecution if neccessary + newInfo.objectId = objectId; + result = commandMap.insert(queue, newInfo, &iter); + if (result != RETURN_OK) { + rejectPacket(TC_VERIFY::START_FAILURE, &packet, BUSY); + } else { + startExecution(&packet, &iter); + } + } + + } +} + +template +void CommandingServiceBase::sendTmPacket(uint8_t subservice, + const uint8_t* data, uint32_t dataLen) { + TmPacketStored tmPacketStored(this->apid, this->service, subservice, + this->tmPacketCounter++, data, dataLen); + TmTcMessage tmMessage(tmPacketStored.getStoreAddress()); + if (requestQueue.sendToDefault(&tmMessage) != RETURN_OK) { + tmPacketStored.deletePacket(); + } +} + +template +void CommandingServiceBase::sendTmPacket(uint8_t subservice, + object_id_t objectId, const uint8_t *data, uint32_t dataLen) { + uint8_t buffer[sizeof(object_id_t)]; + uint8_t* pBuffer = buffer; + uint32_t size = 0; + SerializeAdapter::serialize(&objectId, &pBuffer, &size, + sizeof(object_id_t), true); + TmPacketStored tmPacketStored(this->apid, this->service, subservice, + this->tmPacketCounter++, data, dataLen, buffer, size); + TmTcMessage tmMessage(tmPacketStored.getStoreAddress()); + if (requestQueue.sendToDefault(&tmMessage) != RETURN_OK) { + tmPacketStored.deletePacket(); + } +} + +template +void CommandingServiceBase::sendTmPacket(uint8_t subservice, + SerializeIF* content) { + TmPacketStored tmPacketStored(this->apid, this->service, subservice, + this->tmPacketCounter++, content); + TmTcMessage tmMessage(tmPacketStored.getStoreAddress()); + if (requestQueue.sendToDefault(&tmMessage) != RETURN_OK) { + tmPacketStored.deletePacket(); + } +} + +template +void CommandingServiceBase::startExecution( + TcPacketStored *storedPacket, + typename FixedMap::CommandInfo>::Iterator *iter) { + ReturnValue_t result, sendResult = RETURN_OK; + CommandMessage message; + (*iter)->subservice = storedPacket->getSubService(); + result = prepareCommand(&message, (*iter)->subservice, + storedPacket->getApplicationData(), + storedPacket->getApplicationDataSize(), &(*iter)->state, + (*iter)->objectId); + + switch (result) { + case RETURN_OK: + if (message.getCommand() != CommandMessage::CMD_NONE) { + sendResult = commandQueue.sendMessage((*iter).value->first, + &message); + } + if (sendResult == RETURN_OK) { + OSAL::getUptime(&(*iter)->uptimeOfStart); + (*iter)->step = 0; +// (*iter)->state = 0; + (*iter)->subservice = storedPacket->getSubService(); + (*iter)->command = message.getCommand(); + (*iter)->tcInfo.ackFlags = storedPacket->getAcknowledgeFlags(); + (*iter)->tcInfo.tcPacketId = storedPacket->getPacketId(); + (*iter)->tcInfo.tcSequenceControl = + storedPacket->getPacketSequenceControl(); + acceptPacket(TC_VERIFY::START_SUCCESS, storedPacket); + } else { + message.clearCommandMessage(); + rejectPacket(TC_VERIFY::START_FAILURE, storedPacket, sendResult); + checkAndExecuteFifo(iter); + } + break; + case EXECUTION_COMPLETE: + if (message.getCommand() != CommandMessage::CMD_NONE) { + //Fire-and-forget command. + sendResult = commandQueue.sendMessage((*iter).value->first, + &message); + } + if (sendResult == RETURN_OK) { + verificationReporter.sendSuccessReport(TC_VERIFY::START_SUCCESS, + storedPacket); + acceptPacket(TC_VERIFY::COMPLETION_SUCCESS, storedPacket); + checkAndExecuteFifo(iter); + } else { + message.clearCommandMessage(); + rejectPacket(TC_VERIFY::START_FAILURE, storedPacket, sendResult); + checkAndExecuteFifo(iter); + } + break; + default: + rejectPacket(TC_VERIFY::START_FAILURE, storedPacket, result); + checkAndExecuteFifo(iter); + break; + } +} + +template +void CommandingServiceBase::rejectPacket(uint8_t report_id, + TcPacketStored* packet, ReturnValue_t error_code) { + verificationReporter.sendFailureReport(report_id, packet, error_code); + packet->deletePacket(); +} + +template +void CommandingServiceBase::acceptPacket(uint8_t reportId, + TcPacketStored* packet) { + verificationReporter.sendSuccessReport(reportId, packet); + packet->deletePacket(); +} + +template +void CommandingServiceBase::checkAndExecuteFifo( + typename FixedMap::CommandInfo>::Iterator *iter) { + store_address_t address; + if ((*iter)->fifo.retrieve(&address) != RETURN_OK) { + commandMap.erase(iter); + } else { + TcPacketStored newPacket(address); + startExecution(&newPacket, iter); + } +} + +template +void CommandingServiceBase::handleUnrequestedReply( + CommandMessage* reply) { + reply->clearCommandMessage(); +} + +template +inline void CommandingServiceBase::doPeriodicOperation() { +} + +template +void CommandingServiceBase::checkTimeout() { + uint32_t uptime; + OSAL::getUptime(&uptime); + typename FixedMap::CommandInfo>::Iterator iter; + for (iter = commandMap.begin(); iter != commandMap.end(); ++iter) { + if ((iter->uptimeOfStart + (timeout_seconds * 1000)) < uptime) { + verificationReporter.sendFailureReport( + TC_VERIFY::COMPLETION_FAILURE, iter->tcInfo.ackFlags, + iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl, + TIMEOUT); + checkAndExecuteFifo(&iter); + } + } +} +#endif /* COMMANDINGSERVICEBASE_H_ */ diff --git a/tmtcservices/PusServiceBase.cpp b/tmtcservices/PusServiceBase.cpp new file mode 100644 index 00000000..882a7237 --- /dev/null +++ b/tmtcservices/PusServiceBase.cpp @@ -0,0 +1,99 @@ +/* + * PusServiceBase.cpp + * + * Created on: May 9, 2012 + * Author: baetz + */ +#include +#include +#include +#include +#include +#include + +PusServiceBase::PusServiceBase(object_id_t setObjectId, uint16_t set_apid, + uint8_t set_service_id) : + SystemObject(setObjectId), apid(set_apid), serviceId(set_service_id), errorParameter1( + 0), errorParameter2(0), requestQueue(PUS_SERVICE_MAX_RECEPTION) { +} + +PusServiceBase::~PusServiceBase() { + +} + +ReturnValue_t PusServiceBase::performOperation() { + TmTcMessage message; + for (uint8_t count = 0; count < PUS_SERVICE_MAX_RECEPTION; count++) { + ReturnValue_t status = this->requestQueue.receiveMessage(&message); + // debug << "PusServiceBase::performOperation: Receiving from MQ ID: " << std::hex << this->requestQueue.getId() << std::dec << " returned: " << status << std::endl; + if (status == RETURN_OK) { + this->currentPacket.setStoreAddress(message.getStorageId()); +// info << "Service " << (uint16_t) this->serviceId << ": new packet!" +// << std::endl; + + ReturnValue_t return_code = this->handleRequest(); + // debug << "Service " << (uint16_t)this->serviceId << ": handleRequest returned: " << (int)return_code << std::endl; + if (return_code == RETURN_OK) { + this->verifyReporter.sendSuccessReport( + TC_VERIFY::COMPLETION_SUCCESS, &this->currentPacket); + } else { + this->verifyReporter.sendFailureReport( + TC_VERIFY::COMPLETION_FAILURE, &this->currentPacket, + return_code, 0, errorParameter1, errorParameter2); + } + this->currentPacket.deletePacket(); + errorParameter1 = 0; + errorParameter2 = 0; + } else if (status == OSAL::QUEUE_EMPTY) { + status = RETURN_OK; + // debug << "PusService " << (uint16_t)this->serviceId << ": no new packet." << std::endl; + break; + } else { + + error << "PusServiceBase::performOperation: Service " + << (uint16_t) this->serviceId + << ": Error receiving packet. Code: " << std::hex << status + << std::dec << std::endl; + } + } + ReturnValue_t return_code = this->performService(); + if (return_code == RETURN_OK) { + return RETURN_OK; + } else { + + error << "PusService " << (uint16_t) this->serviceId + << ": performService returned with " << (int16_t) return_code + << std::endl; + return RETURN_FAILED; + } +} + +uint16_t PusServiceBase::getIdentifier() { + return this->serviceId; +} + +MessageQueueId_t PusServiceBase::getRequestQueue() { + return this->requestQueue.getId(); +} + +ReturnValue_t PusServiceBase::initialize() { + ReturnValue_t result = SystemObject::initialize(); + if (result != RETURN_OK) { + return result; + } + AcceptsTelemetryIF* dest_service = objectManager->get( + objects::PUS_PACKET_FORWARDING); + PUSDistributorIF* distributor = objectManager->get( + objects::PUS_PACKET_DISTRIBUTOR); + if ((dest_service != NULL) && (distributor != NULL)) { + this->requestQueue.setDefaultDestination( + dest_service->getReportReceptionQueue()); + distributor->registerService(this); + return RETURN_OK; + } else { + error << "PusServiceBase::PusServiceBase: Service " + << (uint32_t) this->serviceId << ": Configuration error." + << std::endl; + return RETURN_FAILED; + } +} diff --git a/tmtcservices/PusServiceBase.h b/tmtcservices/PusServiceBase.h new file mode 100644 index 00000000..77ac9f4a --- /dev/null +++ b/tmtcservices/PusServiceBase.h @@ -0,0 +1,112 @@ +#ifndef PUSSERVICEBASE_H_ +#define PUSSERVICEBASE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * \defgroup pus_services PUS Service Framework + * These group contains all implementations of PUS Services in the OBSW. + * Most of the Services are directly taken from the ECSS PUS Standard. + */ + +/** + * This class is the basis for all PUS Services, which can immediately process Telecommand Packets. + * It manages Telecommand reception and the generation of Verification Reports. Every class that inherits + * from this abstract class has to implement handleRequest and performService. Services that are created with this + * Base class have to handle any kind of request immediately on reception. + * All PUS Services are System Objects, so an Object ID needs to be specified on construction. + * \ingroup pus_services + */ +class PusServiceBase : public ExecutableObjectIF, public AcceptsTelecommandsIF, public SystemObject, public HasReturnvaluesIF { +private: + /** + * This constant sets the maximum number of packets accepted per call. + * Remember that one packet must be completely handled in one #handleRequest call. + */ + static const uint8_t PUS_SERVICE_MAX_RECEPTION = 10; +protected: + /** + * The APID of this instance of the Service. + */ + uint16_t apid; + /** + * The Service Identifier. + */ + uint8_t serviceId; + /** + * One of two error parameters for additional error information. + */ + uint32_t errorParameter1; + /** + * One of two error parameters for additional error information. + */ + uint8_t errorParameter2; + /** + * This is a complete instance of the Telecommand reception queue of the class. + * It is initialized on construction of the class. + */ + MessageQueue requestQueue; + /** + * An instance of the VerificationReporter class, that simplifies sending any kind of + * Verification Message to the TC Verification Service. + */ + VerificationReporter verifyReporter; + /** + * The current Telecommand to be processed. + * It is deleted after handleRequest was executed. + */ + TcPacketStored currentPacket; +public: + /** + * The constructor for the class. + * The passed values are set, but inter-object initialization is done in the initialize method. + * @param setObjectId The system object identifier of this Service instance. + * @param set_apid The APID the Service is instantiated for. + * @param set_service_id The Service Identifier as specified in ECSS PUS. + */ + PusServiceBase( object_id_t setObjectId, uint16_t set_apid, uint8_t set_service_id ); + /** + * The destructor is empty. + */ + virtual ~PusServiceBase(); + /** + * The handleRequest method shall handle any kind of Telecommand Request immediately. + * Implemetations can take the Telecommand in currentPacket and perform any kind of operation. + * They may send additional "Start Success (1,3)" messages with the verifyReporter, but Completion + * Success or Failure Reports are generated automatically after execution of this method. + * If a Telecommand can not be executed within one call cycle, this Base class is not the right parent. + * The child class may add additional error information in #errorParameters which are attached to the generated + * verification message. + * @return The returned status_code is directly taken as main error code in the Verification Report. + * On success, RETURN_OK shall be returned. + */ + virtual ReturnValue_t handleRequest() = 0; + /** + * In performService, implementations can handle periodic, non-TC-triggered activities. + * The performService method is always called. + * @return A success or failure code that does not trigger any kind of verification message. + */ + virtual ReturnValue_t performService() = 0; + /** + * This method implements the typical activity of a simple PUS Service. + * It checks for new requests, and, if found, calls handleRequest, sends completion verification messages and deletes + * the TC requests afterwards. + * performService is always executed afterwards. + * @return - \c RETURN_OK if the periodic performService was successfull. + * - \c RETURN_FAILED else. + */ + ReturnValue_t performOperation(); + virtual uint16_t getIdentifier(); + MessageQueueId_t getRequestQueue(); + virtual ReturnValue_t initialize(); +}; + +#endif /* PUSSERVICEBASE_H_ */ diff --git a/tmtcservices/PusVerificationReport.cpp b/tmtcservices/PusVerificationReport.cpp new file mode 100644 index 00000000..7e479b65 --- /dev/null +++ b/tmtcservices/PusVerificationReport.cpp @@ -0,0 +1,170 @@ +/* + * PusVerificationReport.cpp + * + * Created on: 22.05.2012 + * Author: baetz + */ + +#include +#include + +PusVerificationMessage::PusVerificationMessage() { +} + +//PusVerificationMessage::PusVerificationMessage(uint8_t set_report_id, +// TcPacketBase* current_packet, ReturnValue_t set_error_code, +// uint8_t set_step, uint32_t parameter1, uint32_t parameter2) { +// uint8_t ackFlags = current_packet->getAcknowledgeFlags(); +// uint16_t tcPacketId = current_packet->getPacketId(); +// uint16_t tcSequenceControl = current_packet->getPacketSequenceControl(); +// uint8_t* data = this->getBuffer(); +// data[messageSize] = set_report_id; +// messageSize += sizeof(set_report_id); +// data[messageSize] = ackFlags; +// messageSize += sizeof(ackFlags); +// memcpy(&data[messageSize], &tcPacketId, sizeof(tcPacketId)); +// messageSize += sizeof(tcPacketId); +// memcpy(&data[messageSize], &tcSequenceControl, sizeof(tcSequenceControl)); +// messageSize += sizeof(tcSequenceControl); +// data[messageSize] = set_step; +// messageSize += sizeof(set_step); +// memcpy(&data[messageSize], &set_error_code, sizeof(set_error_code)); +// messageSize += sizeof(set_error_code); +// memcpy(&data[messageSize], ¶meter1, sizeof(parameter1)); +// messageSize += sizeof(parameter1); +// memcpy(&data[messageSize], ¶meter2, sizeof(parameter2)); +// messageSize += sizeof(parameter2); +//} + +PusVerificationMessage::PusVerificationMessage(uint8_t set_report_id, + uint8_t ackFlags, uint16_t tcPacketId, uint16_t tcSequenceControl, + ReturnValue_t set_error_code, uint8_t set_step, uint32_t parameter1, + uint32_t parameter2) { + uint8_t* data = this->getBuffer(); + data[messageSize] = set_report_id; + messageSize += sizeof(set_report_id); + data[messageSize] = ackFlags; + messageSize += sizeof(ackFlags); + memcpy(&data[messageSize], &tcPacketId, sizeof(tcPacketId)); + messageSize += sizeof(tcPacketId); + memcpy(&data[messageSize], &tcSequenceControl, sizeof(tcSequenceControl)); + messageSize += sizeof(tcSequenceControl); + data[messageSize] = set_step; + messageSize += sizeof(set_step); + memcpy(&data[messageSize], &set_error_code, sizeof(set_error_code)); + messageSize += sizeof(set_error_code); + memcpy(&data[messageSize], ¶meter1, sizeof(parameter1)); + messageSize += sizeof(parameter1); + memcpy(&data[messageSize], ¶meter2, sizeof(parameter2)); + messageSize += sizeof(parameter2); +} + +uint8_t PusVerificationMessage::getReportId() { + + return getContent()->reportId; +} + +uint8_t PusVerificationMessage::getAckFlags() { + + return getContent()->ackFlags; +} + +uint16_t PusVerificationMessage::getTcPacketId() { + + uint16_t tcPacketId; + memcpy(&tcPacketId, &getContent()->packetId_0, sizeof(tcPacketId)); + return tcPacketId; +} + +uint16_t PusVerificationMessage::getTcSequenceControl() { + + uint16_t tcSequenceControl; + memcpy(&tcSequenceControl, &getContent()->tcSequenceControl_0, + sizeof(tcSequenceControl)); + return tcSequenceControl; +} + +uint8_t PusVerificationMessage::getStep() { + + return getContent()->step; +} + +ReturnValue_t PusVerificationMessage::getErrorCode() { + ReturnValue_t errorCode; + memcpy(&errorCode, &getContent()->error_code_0, sizeof(errorCode)); + return errorCode; +} + +PusVerificationMessage::verifciationMessageContent* PusVerificationMessage::getContent() { + return (verifciationMessageContent*) this->getData(); +} + +uint32_t PusVerificationMessage::getParameter1() { + uint32_t parameter; + memcpy(¶meter, &getContent()->parameter1_0, sizeof(parameter)); + return parameter; +} + +uint32_t PusVerificationMessage::getParameter2() { + uint32_t parameter; + memcpy(¶meter, &getContent()->parameter2_0, sizeof(parameter)); + return parameter; +} + +PusSuccessReport::PusSuccessReport(uint16_t setPacketId, + uint16_t setSequenceControl, uint8_t setStep) : + reportSize(0), pBuffer(reportBuffer) { + //Serialization won't fail, because we know the necessary max-size of the buffer. + SerializeAdapter::serialize(&setPacketId, &pBuffer, &reportSize, + sizeof(reportBuffer), true); + SerializeAdapter::serialize(&setSequenceControl, &pBuffer, + &reportSize, sizeof(reportBuffer), true); + if (setStep != 0) { + SerializeAdapter::serialize(&setStep, &pBuffer, &reportSize, + sizeof(reportBuffer), true); + } +} + +PusSuccessReport::~PusSuccessReport() { + +} + +uint32_t PusSuccessReport::getSize() { + return reportSize; +} + +uint8_t* PusSuccessReport::getReport() { + return reportBuffer; +} + +PusFailureReport::PusFailureReport(uint16_t setPacketId, + uint16_t setSequenceControl, ReturnValue_t setErrorCode, + uint8_t setStep, uint32_t parameter1, uint32_t parameter2) : + reportSize(0), pBuffer(reportBuffer) { + //Serialization won't fail, because we know the necessary max-size of the buffer. + SerializeAdapter::serialize(&setPacketId, &pBuffer, &reportSize, + sizeof(reportBuffer), true); + SerializeAdapter::serialize(&setSequenceControl, &pBuffer, + &reportSize, sizeof(reportBuffer), true); + if (setStep != 0) { + SerializeAdapter::serialize(&setStep, &pBuffer, &reportSize, + sizeof(reportBuffer), true); + } + SerializeAdapter::serialize(&setErrorCode, &pBuffer, + &reportSize, sizeof(reportBuffer), true); + SerializeAdapter::serialize(¶meter1, &pBuffer, &reportSize, + sizeof(reportBuffer), true); + SerializeAdapter::serialize(¶meter2, &pBuffer, &reportSize, + sizeof(reportBuffer), true); +} + +PusFailureReport::~PusFailureReport() { +} + +uint32_t PusFailureReport::getSize() { + return reportSize; +} + +uint8_t* PusFailureReport::getReport() { + return reportBuffer; +} diff --git a/tmtcservices/PusVerificationReport.h b/tmtcservices/PusVerificationReport.h new file mode 100644 index 00000000..c48e7d5e --- /dev/null +++ b/tmtcservices/PusVerificationReport.h @@ -0,0 +1,84 @@ +/* + * PusVerificationReport.h + * + * Created on: 22.05.2012 + * Author: baetz + */ + +#ifndef PUSVERIFICATIONREPORT_H_ +#define PUSVERIFICATIONREPORT_H_ + +#include +#include +#include + +class PusVerificationMessage: public MessageQueueMessage { +private: + struct verifciationMessageContent { + uint8_t reportId; + uint8_t ackFlags; + uint8_t packetId_0; + uint8_t packetId_1; + uint8_t tcSequenceControl_0; + uint8_t tcSequenceControl_1; + uint8_t step; + uint8_t error_code_0; + uint8_t error_code_1; + uint8_t parameter1_0; + uint8_t parameter1_1; + uint8_t parameter1_2; + uint8_t parameter1_3; + uint8_t parameter2_0; + uint8_t parameter2_1; + uint8_t parameter2_2; + uint8_t parameter2_3; + }; + verifciationMessageContent* getContent(); +public: + static const uint8_t VERIFICATION_MIN_SIZE = 6; + PusVerificationMessage(); +// PusVerificationMessage( uint8_t set_report_id, TcPacketBase* current_packet, ReturnValue_t set_error_code = 0, uint8_t set_step = 0, uint32_t parameter1 = 0, uint32_t parameter2 = 0 ); + PusVerificationMessage(uint8_t set_report_id, uint8_t ackFlags, + uint16_t tcPacketId, uint16_t tcSequenceControl, + ReturnValue_t set_error_code = 0, uint8_t set_step = 0, + uint32_t parameter1 = 0, uint32_t parameter2 = 0); + uint8_t getReportId(); + uint8_t getAckFlags(); + uint16_t getTcPacketId(); + uint16_t getTcSequenceControl(); + ReturnValue_t getErrorCode(); + uint8_t getStep(); + uint32_t getParameter1(); + uint32_t getParameter2(); +}; + +class PusSuccessReport { +private: + static const uint16_t MAX_SIZE = 7; + uint8_t reportBuffer[MAX_SIZE]; + uint32_t reportSize; + uint8_t * pBuffer; +public: + PusSuccessReport(uint16_t setPacketId, uint16_t setSequenceControl, + uint8_t set_step = 0); + ~PusSuccessReport(); + uint32_t getSize(); + uint8_t* getReport(); +}; + +class PusFailureReport { +private: + static const uint16_t MAX_SIZE = 16; + uint8_t reportBuffer[MAX_SIZE]; + uint32_t reportSize; + uint8_t * pBuffer; +public: + PusFailureReport(uint16_t setPacketId, uint16_t setSequenceControl, + ReturnValue_t setErrorCode, uint8_t setStep = 0, + uint32_t parameter1 = 0, uint32_t parameter2 = 0); + ~PusFailureReport(); + uint32_t getSize(); + uint8_t* getReport(); +}; + +#endif /* PUSVERIFICATIONREPORT_H_ */ diff --git a/tmtcservices/ServiceTypes.h b/tmtcservices/ServiceTypes.h new file mode 100644 index 00000000..1f5bff42 --- /dev/null +++ b/tmtcservices/ServiceTypes.h @@ -0,0 +1,32 @@ +/** + * @file ServiceTypes.h + * @brief This file defines the ServiceTypes class. + * @date 11.04.2013 + * @author baetz + */ + +#ifndef SERVICETYPES_H_ +#define SERVICETYPES_H_ + +namespace SERVICE { +enum ServiceTypes { + TELECOMMAND_VERIFICATION = 1, + DEVICE_COMMAND_DISTRIBUTION = 2, + HOUSEKEEPING_AND_DIAGNOSTIC_DATA_REPORTING = 3, + PARAMETER_STATISTICS_REPORTING = 4, + EVENT_REPORTING = 5, + MEMORY_MANAGEMENT = 6, + FUNCTION_MANAGEMENT = 8, + TIME_MANAGEMENT = 9, + ON_BOARD_OPERATIONS_SCHEDULING = 11, + ON_BOARD_MONITORING = 12, + LARGE_DATA_TRANSFER = 13, + PACKET_FORWARDING_CONTROL = 14, + ON_BOARD_STORAGE_AND_RETRIEVAL = 15, + TEST = 17, + ON_BOARD_OPERATIONS_PROCEDURE = 18, + EVENT_ACTION = 19 +}; +} + +#endif /* SERVICETYPES_H_ */ diff --git a/tmtcservices/SourceSequenceCounter.h b/tmtcservices/SourceSequenceCounter.h new file mode 100644 index 00000000..b5885dfd --- /dev/null +++ b/tmtcservices/SourceSequenceCounter.h @@ -0,0 +1,30 @@ +/** + * @file SourceSequenceCounter.h + * @brief This file defines the SourceSequenceCounter class. + * @date 04.02.2013 + * @author baetz + */ + +#ifndef SOURCESEQUENCECOUNTER_H_ +#define SOURCESEQUENCECOUNTER_H_ +#include + +class SourceSequenceCounter { +private: + uint16_t sequenceCount; +public: + SourceSequenceCounter() : sequenceCount(0) {} + void increment() { + this->sequenceCount = (++this->sequenceCount) % (SpacePacketBase::LIMIT_SEQUENCE_COUNT); + } + void decrement() { + this->sequenceCount = (--this->sequenceCount) % (SpacePacketBase::LIMIT_SEQUENCE_COUNT); + } + uint16_t get() { return this->sequenceCount; } + void reset(uint16_t toValue = 0) { + sequenceCount = toValue % (SpacePacketBase::LIMIT_SEQUENCE_COUNT); + } +}; + + +#endif /* SOURCESEQUENCECOUNTER_H_ */ diff --git a/tmtcservices/TmTcMessage.cpp b/tmtcservices/TmTcMessage.cpp new file mode 100644 index 00000000..da2c8bd3 --- /dev/null +++ b/tmtcservices/TmTcMessage.cpp @@ -0,0 +1,28 @@ +#include + + +TmTcMessage::TmTcMessage() { + this->messageSize += sizeof(store_address_t); +} + +TmTcMessage::~TmTcMessage() { +} + +store_address_t TmTcMessage::getStorageId() { + store_address_t temp_id; + memcpy(&temp_id, this->getData(), sizeof(store_address_t) ); + return temp_id; +} + +TmTcMessage::TmTcMessage(store_address_t store_id) { + this->messageSize += sizeof(store_address_t); + this->setStorageId(store_id); +} + +size_t TmTcMessage::getMinimumMessageSize() { + return this->HEADER_SIZE + sizeof(store_address_t); +} + +void TmTcMessage::setStorageId(store_address_t store_id) { + memcpy(this->getData(), &store_id, sizeof(store_address_t) ); +} diff --git a/tmtcservices/TmTcMessage.h b/tmtcservices/TmTcMessage.h new file mode 100644 index 00000000..73f741fb --- /dev/null +++ b/tmtcservices/TmTcMessage.h @@ -0,0 +1,50 @@ +#ifndef TMTCMESSAGE_H_ +#define TMTCMESSAGE_H_ + +#include +#include +/** + * @brief This message class is used to pass Telecommand and Telemetry + * packets between tasks. + * @details Within such a packet, nothing is transported but the identifier of + * a packet stored in one of the IPC stores (typically a special TM and + * a special TC store). This makes passing commands very simple and + * efficient. + * \ingroup message_queue + */ +class TmTcMessage : public MessageQueueMessage { +protected: + /** + * @brief This call always returns the same fixed size of the message. + * @return Returns HEADER_SIZE + \c sizeof(store_address_t). + */ + size_t getMinimumMessageSize(); +public: + /** + * @brief In the default constructor, only the message_size is set. + */ + TmTcMessage(); + /** + * @brief With this constructor, the passed packet id is directly put + * into the message. + * @param packet_id The packet id to put into the message. + */ + TmTcMessage( store_address_t packet_id ); + /** + * @brief The class's destructor is empty. + */ + ~TmTcMessage(); + /** + * @brief This getter returns the packet id in the correct format. + * @return Returns the packet id. + */ + store_address_t getStorageId(); + /** + * @brief In some cases it might be useful to have a setter for packet id + * as well. + * @param packet_id The packet id to put into the message. + */ + void setStorageId( store_address_t packet_id ); +}; + +#endif /* TMTCMESSAGE_H_ */ diff --git a/tmtcservices/VerificationCodes.h b/tmtcservices/VerificationCodes.h new file mode 100644 index 00000000..d7f81d0d --- /dev/null +++ b/tmtcservices/VerificationCodes.h @@ -0,0 +1,35 @@ +/* + * VerificationCodes.h + * + * Created on: Aug 8, 2012 + * Author: baetz + */ + +#ifndef VERIFICATIONCODES_H_ +#define VERIFICATIONCODES_H_ + +namespace TC_VERIFY { + +enum verification_flags { + NONE = 0b0000, + ACCEPTANCE = 0b0001, + START = 0b0010, + PROGRESS = 0b0100, + COMPLETION = 0b1000 +}; + +enum subservice_ids { + NOTHING_TO_REPORT = 0, + ACCEPTANCE_SUCCESS = 1, + ACCEPTANCE_FAILURE = 2, + START_SUCCESS = 3, + START_FAILURE = 4, + PROGRESS_SUCCESS = 5, + PROGRESS_FAILURE = 6, + COMPLETION_SUCCESS = 7, + COMPLETION_FAILURE = 8, +}; + +} + +#endif /* VERIFICATIONCODES_H_ */ diff --git a/tmtcservices/VerificationReporter.cpp b/tmtcservices/VerificationReporter.cpp new file mode 100644 index 00000000..4f7f37a0 --- /dev/null +++ b/tmtcservices/VerificationReporter.cpp @@ -0,0 +1,92 @@ +/* + * VerificationReporter.cpp + * + * Created on: 20.07.2012 + * Author: baetz + */ + +#include +#include +#include +#include + +VerificationReporter::VerificationReporter() : + acknowledge_queue() { +} + +VerificationReporter::~VerificationReporter() { + //Default, empty +} + +void VerificationReporter::sendSuccessReport(uint8_t set_report_id, + TcPacketBase* current_packet, uint8_t set_step) { + if (this->acknowledge_queue.getDefaultDestination() == 0) { + this->initialize(); + } + PusVerificationMessage message(set_report_id, current_packet->getAcknowledgeFlags(), current_packet->getPacketId(), current_packet->getPacketSequenceControl(), set_step); + ReturnValue_t status = this->acknowledge_queue.sendToDefault(&message); + if (status != OSAL::RETURN_OK) { + error + << "VerificationReporter::sendSuccessReport: Error writing to queue. Code: " + << (uint16_t) status << std::endl; + } +} + +void VerificationReporter::sendSuccessReport(uint8_t set_report_id, + uint8_t ackFlags, uint16_t tcPacketId, uint16_t tcSequenceControl, uint8_t set_step) { + if (this->acknowledge_queue.getDefaultDestination() == 0) { + this->initialize(); + } + PusVerificationMessage message(set_report_id, ackFlags, tcPacketId, tcSequenceControl, set_step); + ReturnValue_t status = this->acknowledge_queue.sendToDefault(&message); + if (status != OSAL::RETURN_OK) { + error + << "VerificationReporter::sendSuccessReport: Error writing to queue. Code: " + << (uint16_t) status << std::endl; + } +} + +void VerificationReporter::sendFailureReport(uint8_t report_id, + TcPacketBase* current_packet, ReturnValue_t error_code, uint8_t step, + uint32_t parameter1, uint32_t parameter2) { + if (this->acknowledge_queue.getDefaultDestination() == 0) { + this->initialize(); + } + PusVerificationMessage message(report_id, current_packet->getAcknowledgeFlags(), current_packet->getPacketId(), current_packet->getPacketSequenceControl(), error_code, step, + parameter1, parameter2); + ReturnValue_t status = this->acknowledge_queue.sendToDefault(&message); + if (status != OSAL::RETURN_OK) { + error + << "VerificationReporter::sendFailureReport Error writing to queue. Code: " + << (uint16_t) status << std::endl; + } +} + +void VerificationReporter::sendFailureReport(uint8_t report_id, + uint8_t ackFlags, uint16_t tcPacketId, uint16_t tcSequenceControl, ReturnValue_t error_code, uint8_t step, + uint32_t parameter1, uint32_t parameter2) { + if (this->acknowledge_queue.getDefaultDestination() == 0) { + this->initialize(); + } + PusVerificationMessage message(report_id, ackFlags, tcPacketId, tcSequenceControl, error_code, step, + parameter1, parameter2); + ReturnValue_t status = this->acknowledge_queue.sendToDefault(&message); + if (status != OSAL::RETURN_OK) { + error + << "VerificationReporter::sendFailureReport Error writing to queue. Code: " + << (uint16_t) status << std::endl; + } +} + +void VerificationReporter::initialize() { + AcceptsVerifyMessageIF* temp = objectManager->get( + objects::PUS_VERIFICATION_SERVICE); + if (temp != NULL) { + this->acknowledge_queue.setDefaultDestination( + temp->getVerificationQueue()); + } else { + error + << "VerificationReporter::VerificationReporter: Configuration error." + << std::endl; + } +} diff --git a/tmtcservices/VerificationReporter.h b/tmtcservices/VerificationReporter.h new file mode 100644 index 00000000..ee8278d1 --- /dev/null +++ b/tmtcservices/VerificationReporter.h @@ -0,0 +1,31 @@ +/* + * VerificationReporter.h + * + * Created on: 20.07.2012 + * Author: baetz + */ + +#ifndef VERIFICATIONREPORTER_H_ +#define VERIFICATIONREPORTER_H_ + +#include +#include +#include + +class VerificationReporter { +protected: + MessageQueueSender acknowledge_queue; +public: + VerificationReporter(); + virtual ~VerificationReporter(); + void sendSuccessReport( uint8_t set_report_id, TcPacketBase* current_packet, uint8_t set_step = 0 ); + void sendSuccessReport(uint8_t set_report_id, uint8_t ackFlags, uint16_t tcPacketId, uint16_t tcSequenceControl, uint8_t set_step = 0); + void sendFailureReport( uint8_t report_id, TcPacketBase* current_packet, ReturnValue_t error_code = 0, + uint8_t step = 0, uint32_t parameter1 = 0, uint32_t parameter2 = 0 ); + void sendFailureReport(uint8_t report_id, + uint8_t ackFlags, uint16_t tcPacketId, uint16_t tcSequenceControl, ReturnValue_t error_code = 0, uint8_t step = 0, + uint32_t parameter1 = 0, uint32_t parameter2 = 0); + void initialize(); +}; + +#endif /* VERIFICATIONREPORTER_H_ */ diff --git a/watchdog/Watchdog.cpp b/watchdog/Watchdog.cpp new file mode 100644 index 00000000..237fb5f7 --- /dev/null +++ b/watchdog/Watchdog.cpp @@ -0,0 +1,54 @@ +#include + +extern "C" { +#include +} + +#include +#include + +Watchdog::Watchdog(object_id_t objectId, uint32_t interval_us, uint32_t initial_interval_us) : + SystemObject(objectId), commandQueue(3, + WatchdogMessage::WATCHDOG_MESSAGE_SIZE) { + hw_timer_set_reload_value(hw_timer_watchdog, interval_us); + hw_timer_start_(hw_timer_watchdog, initial_interval_us); + hw_timer_watchdog ->control_register |= 8; + + hw_gpio_port ->direction |= (HW_GPIO_DDR_OUT << HW_GPIO_LEON_WD_EN); + hw_gpio_port ->output |= (HW_GPIO_DDR_OUT << HW_GPIO_LEON_WD_EN); +} + +Watchdog::~Watchdog() { +} + +ReturnValue_t Watchdog::performOperation() { + WatchdogMessage message; + ReturnValue_t result = commandQueue.receiveMessage(&message); + if (result != HasReturnvaluesIF::RETURN_OK) { + hw_timer_reload(hw_timer_watchdog ); + } else { + debug << "Watchdog::performOperation: Object 0x" << std::hex + << message.getSender() << std::dec << " requested "; + switch (message.getCommand()) { + case WatchdogMessage::ENABLE: + debug << "watchdog enable" << std::endl; + hw_timer_reload(hw_timer_watchdog ); + hw_timer_start(hw_timer_watchdog ); + break; + case WatchdogMessage::DISABLE: + debug << "watchdog disable" << std::endl; + hw_timer_stop(hw_timer_watchdog ); + break; + case WatchdogMessage::RESET_CPU: + debug << "CPU reset" << std::endl; + hw_timer_start_(hw_timer_watchdog, 10); + break; + } + } + return HasReturnvaluesIF::RETURN_OK; +} + +MessageQueueId_t Watchdog::getCommandQueue() { + return commandQueue.getId(); +} + diff --git a/watchdog/Watchdog.h b/watchdog/Watchdog.h new file mode 100644 index 00000000..373e1dc5 --- /dev/null +++ b/watchdog/Watchdog.h @@ -0,0 +1,20 @@ +#ifndef WATCHDOG_H_ +#define WATCHDOG_H_ + +#include +#include +#include +#include + +class Watchdog: public ExecutableObjectIF, public SystemObject { +public: + Watchdog(object_id_t objectId, uint32_t interval_us, uint32_t initial_interval_us); + virtual ~Watchdog(); + virtual ReturnValue_t performOperation(); + + MessageQueueId_t getCommandQueue(); +protected: + MessageQueue commandQueue; +}; + +#endif /* WATCHDOG_H_ */ diff --git a/watchdog/WatchdogMessage.cpp b/watchdog/WatchdogMessage.cpp new file mode 100644 index 00000000..7e92e0cb --- /dev/null +++ b/watchdog/WatchdogMessage.cpp @@ -0,0 +1,43 @@ +#include +#include + +WatchdogMessage::WatchdogMessage() { + messageSize = WATCHDOG_MESSAGE_SIZE; +} + +WatchdogMessage::WatchdogMessage(object_id_t sender, + WatchdogCommand_t command) { + messageSize = WATCHDOG_MESSAGE_SIZE; + setSender(sender); + setCommand(command); +} + +WatchdogMessage::WatchdogCommand_t WatchdogMessage::getCommand() { + WatchdogCommand_t command; + memcpy(&command, getData() + sizeof(object_id_t), + sizeof(WatchdogCommand_t)); + return command; +} + +void WatchdogMessage::setCommand(WatchdogCommand_t command) { + memcpy(getData() + sizeof(object_id_t), &command, + sizeof(WatchdogCommand_t)); +} + +object_id_t WatchdogMessage::getSender() { + object_id_t sender; + memcpy(&sender, getData(), sizeof(object_id_t)); + return sender; +} + +void WatchdogMessage::setSender(object_id_t sender) { + memcpy(getData(), &sender, sizeof(object_id_t)); +} + +WatchdogMessage::~WatchdogMessage() { + +} + +size_t WatchdogMessage::getMinimumMessageSize() { + return WATCHDOG_MESSAGE_SIZE; +} diff --git a/watchdog/WatchdogMessage.h b/watchdog/WatchdogMessage.h new file mode 100644 index 00000000..a08c83fd --- /dev/null +++ b/watchdog/WatchdogMessage.h @@ -0,0 +1,32 @@ +#ifndef WATCHDOGMESSAGE_H_ +#define WATCHDOGMESSAGE_H_ + +#include +#include + +class WatchdogMessage: public MessageQueueMessage { +public: + /** + * Commands that can be sent to the watchdog + */ + enum WatchdogCommand_t{ + ENABLE, //!< Enables the Watchdog (it is enabled by default) + DISABLE,//!< Disables the watchdog + RESET_CPU //!< Causes a reset of the Processor + }; + + static const uint8_t WATCHDOG_MESSAGE_SIZE = HEADER_SIZE + sizeof(object_id_t) + sizeof(WatchdogCommand_t); + + WatchdogMessage(); + WatchdogMessage(object_id_t sender, WatchdogCommand_t command); + virtual ~WatchdogMessage(); + WatchdogCommand_t getCommand(); + void setCommand(WatchdogCommand_t command); + object_id_t getSender(); + void setSender(object_id_t sender); + +protected: + virtual size_t getMinimumMessageSize(); +}; + +#endif /* WATCHDOGMESSAGE_H_ */