diff --git a/CMakeLists.txt b/CMakeLists.txt index 8ba6a187..3d4053e2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -131,9 +131,9 @@ else() ) endif() -foreach(INCLUDE_PATH ${FSFW_ADDITIONAL_INC_PATH}) +foreach(INCLUDE_PATH ${FSFW_ADDITIONAL_INC_PATHS}) if(IS_ABSOLUTE ${INCLUDE_PATH}) - set(CURR_ABS_INC_PATH "${FREERTOS_PATH}") + set(CURR_ABS_INC_PATH "${INCLUDE_PATH}") else() get_filename_component(CURR_ABS_INC_PATH ${INCLUDE_PATH} REALPATH BASE_DIR ${CMAKE_SOURCE_DIR}) diff --git a/events/EventManager.cpp b/events/EventManager.cpp index 2337ba83..8e2a2a82 100644 --- a/events/EventManager.cpp +++ b/events/EventManager.cpp @@ -156,11 +156,11 @@ void EventManager::printUtility(sif::OutputTypes printType, EventMessage *messag sif::info << "0x" << std::hex << std::setw(8) << std::setfill('0') << message->getReporter() << std::setfill(' ') << std::dec; } - sif::info << " report event with ID " << message->getEventId() << std::endl; - sif::info << std::hex << "P1 Hex: 0x" << message->getParameter1() << - " | P1 Dec: " << std::dec << message->getParameter1() << std::hex << - " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " << std::dec << - message->getParameter2() << std::endl; + sif::info << " reported event with ID " << message->getEventId() << std::endl; + sif::debug << translateEvents(message->getEvent()) << " | " <getParameter1() << " | P1 Dec: " << std::dec << message->getParameter1() << + std::hex << " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " << + std::dec << message->getParameter2() << std::endl; #else if (string != 0) { sif::printInfo("Event Manager: %s reported event with ID %d\n", string, @@ -186,11 +186,11 @@ void EventManager::printUtility(sif::OutputTypes printType, EventMessage *messag sif::debug << "0x" << std::hex << std::setw(8) << std::setfill('0') << message->getReporter() << std::setfill(' ') << std::dec; } - sif::debug << " report event with ID " << message->getEventId() << std::endl; - sif::debug << std::hex << "P1 Hex: 0x" << message->getParameter1() << - " | P1 Dec: " << std::dec << message->getParameter1() << std::hex << - " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " << std::dec << - message->getParameter2() << std::endl; + sif::debug << " reported event with ID " << message->getEventId() << std::endl; + sif::debug << translateEvents(message->getEvent()) << " | " <getParameter1() << " | P1 Dec: " << std::dec << message->getParameter1() << + std::hex << " | P2 Hex: 0x" << message->getParameter2() << " | P2 Dec: " << + std::dec << message->getParameter2() << std::endl; #else if (string != 0) { sif::printDebug("Event Manager: %s reported event with ID %d\n", string, diff --git a/events/EventManager.h b/events/EventManager.h index 9189d9e7..012247db 100644 --- a/events/EventManager.h +++ b/events/EventManager.h @@ -3,8 +3,7 @@ #include "EventManagerIF.h" #include "eventmatching/EventMatchTree.h" - -#include +#include "FSFWConfig.h" #include "../serviceinterface/ServiceInterface.h" #include "../objectmanager/SystemObject.h" diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp index 0c16d808..a2137e27 100644 --- a/osal/host/MessageQueue.cpp +++ b/osal/host/MessageQueue.cpp @@ -14,8 +14,9 @@ MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize): auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId); if(result != HasReturnvaluesIF::RETURN_OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::MessageQueue:" - << " Could not be created" << std::endl; + sif::error << "MessageQueue::MessageQueue: Could not be created" << std::endl; +#else + sif::printError("MessageQueue::MessageQueue: Could not be created\n"); #endif } } diff --git a/osal/linux/BinarySemaphore.cpp b/osal/linux/BinarySemaphore.cpp index 5716e560..110b0a90 100644 --- a/osal/linux/BinarySemaphore.cpp +++ b/osal/linux/BinarySemaphore.cpp @@ -1,4 +1,5 @@ #include "BinarySemaphore.h" +#include "unixUtility.h" #include "../../serviceinterface/ServiceInterfacePrinter.h" #include "../../serviceinterface/ServiceInterfaceStream.h" @@ -8,154 +9,154 @@ BinarySemaphore::BinarySemaphore() { - // Using unnamed semaphores for now - initSemaphore(); + // Using unnamed semaphores for now + initSemaphore(); } BinarySemaphore::~BinarySemaphore() { - sem_destroy(&handle); + sem_destroy(&handle); } BinarySemaphore::BinarySemaphore(BinarySemaphore&& s) { - initSemaphore(); + initSemaphore(); } BinarySemaphore& BinarySemaphore::operator =( BinarySemaphore&& s) { - initSemaphore(); - return * this; + initSemaphore(); + return * this; } ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType, - uint32_t timeoutMs) { - int result = 0; - if(timeoutType == TimeoutType::POLLING) { - result = sem_trywait(&handle); - } - else if(timeoutType == TimeoutType::BLOCKING) { - result = sem_wait(&handle); - } - else if(timeoutType == TimeoutType::WAITING){ - timespec timeOut; - clock_gettime(CLOCK_REALTIME, &timeOut); - uint64_t nseconds = timeOut.tv_sec * 1000000000 + timeOut.tv_nsec; - nseconds += timeoutMs * 1000000; - timeOut.tv_sec = nseconds / 1000000000; - timeOut.tv_nsec = nseconds - timeOut.tv_sec * 1000000000; - result = sem_timedwait(&handle, &timeOut); - if(result != 0 and errno == EINVAL) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::debug << "BinarySemaphore::acquire: Invalid time value possible" - << std::endl; -#endif - } - } - if(result == 0) { - return HasReturnvaluesIF::RETURN_OK; - } + uint32_t timeoutMs) { + int result = 0; + if(timeoutType == TimeoutType::POLLING) { + result = sem_trywait(&handle); + } + else if(timeoutType == TimeoutType::BLOCKING) { + result = sem_wait(&handle); + } + else if(timeoutType == TimeoutType::WAITING){ + timespec timeOut; + clock_gettime(CLOCK_REALTIME, &timeOut); + uint64_t nseconds = timeOut.tv_sec * 1000000000 + timeOut.tv_nsec; + nseconds += timeoutMs * 1000000; + timeOut.tv_sec = nseconds / 1000000000; + timeOut.tv_nsec = nseconds - timeOut.tv_sec * 1000000000; + result = sem_timedwait(&handle, &timeOut); + if(result != 0 and errno == EINVAL) { + utility::printUnixErrorGeneric(CLASS_NAME, "acquire", "sem_timedwait"); + } + } + if(result == 0) { + return HasReturnvaluesIF::RETURN_OK; + } - switch(errno) { - case(EAGAIN): - // Operation could not be performed without blocking (for sem_trywait) - case(ETIMEDOUT): - // Semaphore is 0 - return SemaphoreIF::SEMAPHORE_TIMEOUT; - case(EINVAL): - // Semaphore invalid - return SemaphoreIF::SEMAPHORE_INVALID; - case(EINTR): - // Call was interrupted by signal handler -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::debug << "BinarySemaphore::acquire: Signal handler interrupted." - "Code " << strerror(errno) << std::endl; -#endif - /* No break */ - default: - return HasReturnvaluesIF::RETURN_FAILED; - } + switch(errno) { + case(EAGAIN): + // Operation could not be performed without blocking (for sem_trywait) + case(ETIMEDOUT): { + // Semaphore is 0 + utility::printUnixErrorGeneric(CLASS_NAME, "acquire", "ETIMEDOUT"); + return SemaphoreIF::SEMAPHORE_TIMEOUT; + } + case(EINVAL): { + // Semaphore invalid + utility::printUnixErrorGeneric(CLASS_NAME, "acquire", "EINVAL"); + return SemaphoreIF::SEMAPHORE_INVALID; + } + case(EINTR): { + // Call was interrupted by signal handler + utility::printUnixErrorGeneric(CLASS_NAME, "acquire", "EINTR"); + return HasReturnvaluesIF::RETURN_FAILED; + } + default: + return HasReturnvaluesIF::RETURN_FAILED; + } } ReturnValue_t BinarySemaphore::release() { - return BinarySemaphore::release(&this->handle); + return BinarySemaphore::release(&this->handle); } ReturnValue_t BinarySemaphore::release(sem_t *handle) { - ReturnValue_t countResult = checkCount(handle, 1); - if(countResult != HasReturnvaluesIF::RETURN_OK) { - return countResult; - } + ReturnValue_t countResult = checkCount(handle, 1); + if(countResult != HasReturnvaluesIF::RETURN_OK) { + return countResult; + } - int result = sem_post(handle); - if(result == 0) { - return HasReturnvaluesIF::RETURN_OK; - } + int result = sem_post(handle); + if(result == 0) { + return HasReturnvaluesIF::RETURN_OK; + } - switch(errno) { - case(EINVAL): - // Semaphore invalid - return SemaphoreIF::SEMAPHORE_INVALID; - case(EOVERFLOW): - // SEM_MAX_VALUE overflow. This should never happen - default: - return HasReturnvaluesIF::RETURN_FAILED; - } + switch(errno) { + case(EINVAL): { + // Semaphore invalid + utility::printUnixErrorGeneric(CLASS_NAME, "release", "EINVAL"); + return SemaphoreIF::SEMAPHORE_INVALID; + } + case(EOVERFLOW): { + // SEM_MAX_VALUE overflow. This should never happen + utility::printUnixErrorGeneric(CLASS_NAME, "release", "EOVERFLOW"); + return HasReturnvaluesIF::RETURN_FAILED; + } + default: + return HasReturnvaluesIF::RETURN_FAILED; + } } uint8_t BinarySemaphore::getSemaphoreCounter() const { - // And another ugly cast :-D - return getSemaphoreCounter(const_cast(&this->handle)); + // And another ugly cast :-D + return getSemaphoreCounter(const_cast(&this->handle)); } uint8_t BinarySemaphore::getSemaphoreCounter(sem_t *handle) { - int value = 0; - int result = sem_getvalue(handle, &value); - if (result == 0) { - return value; - } - else if(result != 0 and errno == EINVAL) { - // Could be called from interrupt, use lightweight printf - sif::printError("BinarySemaphore::getSemaphoreCounter: " - "Invalid semaphore\n"); - return 0; - } - else { - // This should never happen. - return 0; - } + int value = 0; + int result = sem_getvalue(handle, &value); + if (result == 0) { + return value; + } + else if(result != 0 and errno == EINVAL) { + // Could be called from interrupt, use lightweight printf + sif::printError("BinarySemaphore::getSemaphoreCounter: " + "Invalid semaphore\n"); + return 0; + } + else { + // This should never happen. + return 0; + } } void BinarySemaphore::initSemaphore(uint8_t initCount) { - auto result = sem_init(&handle, true, initCount); - if(result == -1) { - switch(errno) { - case(EINVAL): - // Value exceeds SEM_VALUE_MAX - case(ENOSYS): { - // System does not support process-shared semaphores -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "BinarySemaphore: Init failed with " - << strerror(errno) << std::endl; -#else - sif::printError("BinarySemaphore: Init failed with %s\n", - strerror(errno)); -#endif - } - } - } + auto result = sem_init(&handle, true, initCount); + if(result == -1) { + switch(errno) { + case(EINVAL): { + utility::printUnixErrorGeneric(CLASS_NAME, "initSemaphore", "EINVAL"); + break; + } + case(ENOSYS): { + // System does not support process-shared semaphores + utility::printUnixErrorGeneric(CLASS_NAME, "initSemaphore", "ENOSYS"); + break; + } + } + } } ReturnValue_t BinarySemaphore::checkCount(sem_t* handle, uint8_t maxCount) { - int value = getSemaphoreCounter(handle); - if(value >= maxCount) { - if(maxCount == 1 and value > 1) { - // Binary Semaphore special case. - // This is a config error use lightweight printf is this is called - // from an interrupt - printf("BinarySemaphore::release: Value of binary semaphore greater" - " than 1!\n"); - return HasReturnvaluesIF::RETURN_FAILED; - } - return SemaphoreIF::SEMAPHORE_NOT_OWNED; - } - return HasReturnvaluesIF::RETURN_OK; + int value = getSemaphoreCounter(handle); + if(value >= maxCount) { + if(maxCount == 1 and value > 1) { + // Binary Semaphore special case. + // This is a config error use lightweight printf is this is called + // from an interrupt + printf("BinarySemaphore::release: Value of binary semaphore greater than 1!\n"); + return HasReturnvaluesIF::RETURN_FAILED; + } + return SemaphoreIF::SEMAPHORE_NOT_OWNED; + } + return HasReturnvaluesIF::RETURN_OK; } diff --git a/osal/linux/BinarySemaphore.h b/osal/linux/BinarySemaphore.h index e9bb8bb6..2e6ded15 100644 --- a/osal/linux/BinarySemaphore.h +++ b/osal/linux/BinarySemaphore.h @@ -76,6 +76,7 @@ public: static ReturnValue_t checkCount(sem_t* handle, uint8_t maxCount); protected: sem_t handle; + static constexpr const char* CLASS_NAME = "BinarySemaphore"; }; #endif /* FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ */ diff --git a/osal/linux/CMakeLists.txt b/osal/linux/CMakeLists.txt index 332fe2f4..0fb66b3e 100644 --- a/osal/linux/CMakeLists.txt +++ b/osal/linux/CMakeLists.txt @@ -15,6 +15,7 @@ target_sources(${LIB_FSFW_NAME} TaskFactory.cpp Timer.cpp tcpipHelpers.cpp + unixUtility.cpp ) find_package(Threads REQUIRED) diff --git a/osal/linux/CountingSemaphore.cpp b/osal/linux/CountingSemaphore.cpp index 07c52212..752e150b 100644 --- a/osal/linux/CountingSemaphore.cpp +++ b/osal/linux/CountingSemaphore.cpp @@ -1,58 +1,70 @@ -#include "../../osal/linux/CountingSemaphore.h" +#include "CountingSemaphore.h" +#include "unixUtility.h" + #include "../../serviceinterface/ServiceInterface.h" #include CountingSemaphore::CountingSemaphore(const uint8_t maxCount, uint8_t initCount): - maxCount(maxCount), initCount(initCount) { - if(initCount > maxCount) { + maxCount(maxCount), initCount(initCount) { + if(initCount > maxCount) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "CountingSemaphoreUsingTask: Max count bigger than " - "intial cout. Setting initial count to max count." << std::endl; + sif::warning << "CountingSemaphoreUsingTask: Max count bigger than " + "intial cout. Setting initial count to max count" << std::endl; +#else + sif::printWarning("CountingSemaphoreUsingTask: Max count bigger than " + "intial cout. Setting initial count to max count\n"); #endif - initCount = maxCount; - } + initCount = maxCount; + } - initSemaphore(initCount); + initSemaphore(initCount); } CountingSemaphore::CountingSemaphore(CountingSemaphore&& other): - maxCount(other.maxCount), initCount(other.initCount) { - initSemaphore(initCount); + maxCount(other.maxCount), initCount(other.initCount) { + initSemaphore(initCount); } CountingSemaphore& CountingSemaphore::operator =( - CountingSemaphore&& other) { - initSemaphore(other.initCount); - return * this; + CountingSemaphore&& other) { + initSemaphore(other.initCount); + return * this; } ReturnValue_t CountingSemaphore::release() { - ReturnValue_t result = checkCount(&handle, maxCount); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - return CountingSemaphore::release(&this->handle); + ReturnValue_t result = checkCount(&handle, maxCount); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return CountingSemaphore::release(&this->handle); } ReturnValue_t CountingSemaphore::release(sem_t* handle) { - int result = sem_post(handle); - if(result == 0) { - return HasReturnvaluesIF::RETURN_OK; - } + int result = sem_post(handle); + if(result == 0) { + return HasReturnvaluesIF::RETURN_OK; + } - switch(errno) { - case(EINVAL): - // Semaphore invalid - return SemaphoreIF::SEMAPHORE_INVALID; - case(EOVERFLOW): - // SEM_MAX_VALUE overflow. This should never happen - default: - return HasReturnvaluesIF::RETURN_FAILED; - } + switch(errno) { + case(EINVAL): { + // Semaphore invalid + utility::printUnixErrorGeneric("CountingSemaphore", "release", "EINVAL"); + return SemaphoreIF::SEMAPHORE_INVALID; + } + + case(EOVERFLOW): { + // SEM_MAX_VALUE overflow. This should never happen + utility::printUnixErrorGeneric("CountingSemaphore", "release", "EOVERFLOW"); + return SemaphoreIF::SEMAPHORE_INVALID; + } + + default: + return HasReturnvaluesIF::RETURN_FAILED; + } } uint8_t CountingSemaphore::getMaxCount() const { - return maxCount; + return maxCount; } diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp index 0abb7a35..75376798 100644 --- a/osal/linux/MessageQueue.cpp +++ b/osal/linux/MessageQueue.cpp @@ -1,4 +1,5 @@ #include "MessageQueue.h" +#include "unixUtility.h" #include "../../serviceinterface/ServiceInterface.h" #include "../../objectmanager/ObjectManager.h" @@ -12,331 +13,246 @@ MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize): - id(MessageQueueIF::NO_QUEUE),lastPartner(MessageQueueIF::NO_QUEUE), - defaultDestination(MessageQueueIF::NO_QUEUE), - maxMessageSize(maxMessageSize) { - //debug << "MessageQueue::MessageQueue: Creating a queue" << std::endl; - mq_attr attributes; - this->id = 0; - //Set attributes - attributes.mq_curmsgs = 0; - attributes.mq_maxmsg = messageDepth; - attributes.mq_msgsize = maxMessageSize; - attributes.mq_flags = 0; //Flags are ignored on Linux during mq_open - //Set the name of the queue. The slash is mandatory! - sprintf(name, "/FSFW_MQ%u\n", queueCounter++); + id(MessageQueueIF::NO_QUEUE),lastPartner(MessageQueueIF::NO_QUEUE), + defaultDestination(MessageQueueIF::NO_QUEUE), maxMessageSize(maxMessageSize) { + mq_attr attributes; + this->id = 0; + //Set attributes + attributes.mq_curmsgs = 0; + attributes.mq_maxmsg = messageDepth; + attributes.mq_msgsize = maxMessageSize; + attributes.mq_flags = 0; //Flags are ignored on Linux during mq_open + //Set the name of the queue. The slash is mandatory! + sprintf(name, "/FSFW_MQ%u\n", queueCounter++); - // Create a nonblocking queue if the name is available (the queue is read - // and writable for the owner as well as the group) - int oflag = O_NONBLOCK | O_RDWR | O_CREAT | O_EXCL; - mode_t mode = S_IWUSR | S_IREAD | S_IWGRP | S_IRGRP | S_IROTH | S_IWOTH; - mqd_t tempId = mq_open(name, oflag, mode, &attributes); - if (tempId == -1) { - handleError(&attributes, messageDepth); - } - else { - //Successful mq_open call - this->id = tempId; - } + // Create a nonblocking queue if the name is available (the queue is read + // and writable for the owner as well as the group) + int oflag = O_NONBLOCK | O_RDWR | O_CREAT | O_EXCL; + mode_t mode = S_IWUSR | S_IREAD | S_IWGRP | S_IRGRP | S_IROTH | S_IWOTH; + mqd_t tempId = mq_open(name, oflag, mode, &attributes); + if (tempId == -1) { + handleOpenError(&attributes, messageDepth); + } + else { + //Successful mq_open call + this->id = tempId; + } } MessageQueue::~MessageQueue() { - int status = mq_close(this->id); - if(status != 0){ -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::Destructor: mq_close Failed with status: " - << strerror(errno) <> - defaultMqMaxMsg and defaultMqMaxMsg < messageDepth) { - /* - See: https://www.man7.org/linux/man-pages/man3/mq_open.3.html - This happens if the msg_max value is not large enough - It is ignored if the executable is run in privileged mode. - Run the unlockRealtime script or grant the mode manually by using: - sudo setcap 'CAP_SYS_RESOURCE=+ep' - - Persistent solution for session: - echo | sudo tee /proc/sys/fs/mqueue/msg_max - - Permanent solution: - sudo nano /etc/sysctl.conf - Append at end: fs/mqueue/msg_max = - Apply changes with: sudo sysctl -p - */ -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::MessageQueue: Default MQ size " - << defaultMqMaxMsg << " is too small for requested size " - << messageDepth << std::endl; - sif::error << "This error can be fixed by setting the maximum " - "allowed message size higher!" << std::endl; -#endif - - } - break; - } - case(EEXIST): { - // An error occured during open - // We need to distinguish if it is caused by an already created queue - //There's another queue with the same name - //We unlink the other queue - int status = mq_unlink(name); - if (status != 0) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "mq_unlink Failed with status: " << strerror(errno) - << std::endl; -#endif - } - else { - // Successful unlinking, try to open again - mqd_t tempId = mq_open(name, - O_NONBLOCK | O_RDWR | O_CREAT | O_EXCL, - S_IWUSR | S_IREAD | S_IWGRP | S_IRGRP, attributes); - if (tempId != -1) { - //Successful mq_open - this->id = tempId; - return HasReturnvaluesIF::RETURN_OK; - } - } - break; - } - - default: { - // Failed either the first time or the second time -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::MessageQueue: Creating Queue " << name - << " failed with status: " << strerror(errno) << std::endl; -#else - sif::printError("MessageQueue::MessageQueue: Creating Queue %s" - " failed with status: %s\n", name, strerror(errno)); -#endif - } - } - return HasReturnvaluesIF::RETURN_FAILED; - - - + int status = mq_close(this->id); + if(status != 0){ + utility::printUnixErrorGeneric(CLASS_NAME, "~MessageQueue", "close"); + } + status = mq_unlink(name); + if(status != 0){ + utility::printUnixErrorGeneric(CLASS_NAME, "~MessageQueue", "unlink"); + } } ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, - MessageQueueMessageIF* message, bool ignoreFault) { - return sendMessageFrom(sendTo, message, this->getId(), false); + MessageQueueMessageIF* message, bool ignoreFault) { + return sendMessageFrom(sendTo, message, this->getId(), false); } ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) { - return sendToDefaultFrom(message, this->getId()); + return sendToDefaultFrom(message, this->getId()); } ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) { - if (this->lastPartner != 0) { - return sendMessageFrom(this->lastPartner, message, this->getId()); - } else { - return NO_REPLY_PARTNER; - } + if (this->lastPartner != 0) { + return sendMessageFrom(this->lastPartner, message, this->getId()); + } else { + return NO_REPLY_PARTNER; + } } ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message, - MessageQueueId_t* receivedFrom) { - ReturnValue_t status = this->receiveMessage(message); - *receivedFrom = this->lastPartner; - return status; + MessageQueueId_t* receivedFrom) { + ReturnValue_t status = this->receiveMessage(message); + *receivedFrom = this->lastPartner; + return status; } ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { - if(message == nullptr) { + if(message == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::receiveMessage: Message is " - "nullptr!" << std::endl; + sif::error << "MessageQueue::receiveMessage: Message is " + "nullptr!" << std::endl; #endif - return HasReturnvaluesIF::RETURN_FAILED; - } + return HasReturnvaluesIF::RETURN_FAILED; + } - if(message->getMaximumMessageSize() < maxMessageSize) { + if(message->getMaximumMessageSize() < maxMessageSize) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::receiveMessage: Message size " - << message->getMaximumMessageSize() - << " too small to receive data!" << std::endl; + sif::error << "MessageQueue::receiveMessage: Message size " + << message->getMaximumMessageSize() + << " too small to receive data!" << std::endl; #endif - return HasReturnvaluesIF::RETURN_FAILED; - } + return HasReturnvaluesIF::RETURN_FAILED; + } - unsigned int messagePriority = 0; - int status = mq_receive(id,reinterpret_cast(message->getBuffer()), - message->getMaximumMessageSize(),&messagePriority); - if (status > 0) { - this->lastPartner = message->getSender(); - //Check size of incoming message. - if (message->getMessageSize() < message->getMinimumMessageSize()) { - return HasReturnvaluesIF::RETURN_FAILED; - } - return HasReturnvaluesIF::RETURN_OK; - } - else if (status==0) { - //Success but no message received - return MessageQueueIF::EMPTY; - } - else { - //No message was received. Keep lastPartner anyway, I might send - //something later. But still, delete packet content. - memset(message->getData(), 0, message->getMaximumDataSize()); - switch(errno){ - case EAGAIN: - //O_NONBLOCK or MQ_NONBLOCK was set and there are no messages - //currently on the specified queue. - return MessageQueueIF::EMPTY; - case EBADF: - //mqdes doesn't represent a valid queue open for reading. -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::receive: configuration error " - << strerror(errno) << std::endl; -#endif - /*NO BREAK*/ - case EINVAL: - /* - * This value indicates one of the following: - * - The pointer to the buffer for storing the received message, - * msg_ptr, is NULL. - * - The number of bytes requested, msg_len is less than zero. - * - msg_len is anything other than the mq_msgsize of the specified - * queue, and the QNX extended option MQ_READBUF_DYNAMIC hasn't - * been set in the queue's mq_flags. - */ -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::receive: configuration error " - << strerror(errno) << std::endl; -#endif - /*NO BREAK*/ - case EMSGSIZE: - /* - * This value indicates one of the following: - * - the QNX extended option MQ_READBUF_DYNAMIC hasn't been set, - * and the given msg_len is shorter than the mq_msgsize for - * the given queue. - * - the extended option MQ_READBUF_DYNAMIC has been set, but the - * given msg_len is too short for the message that would have - * been received. - */ -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::receive: configuration error " - << strerror(errno) << std::endl; -#endif - /*NO BREAK*/ - case EINTR: - //The operation was interrupted by a signal. - default: + unsigned int messagePriority = 0; + int status = mq_receive(id,reinterpret_cast(message->getBuffer()), + message->getMaximumMessageSize(),&messagePriority); + if (status > 0) { + this->lastPartner = message->getSender(); + //Check size of incoming message. + if (message->getMessageSize() < message->getMinimumMessageSize()) { + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; + } + else if (status==0) { + //Success but no message received + return MessageQueueIF::EMPTY; + } + else { + //No message was received. Keep lastPartner anyway, I might send + //something later. But still, delete packet content. + memset(message->getData(), 0, message->getMaximumDataSize()); + switch(errno){ + case EAGAIN: + //O_NONBLOCK or MQ_NONBLOCK was set and there are no messages + //currently on the specified queue. + return MessageQueueIF::EMPTY; + case EBADF: { + //mqdes doesn't represent a valid queue open for reading. + utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "EBADF"); + break; + } + case EINVAL: { + /* + * This value indicates one of the following: + * - The pointer to the buffer for storing the received message, + * msg_ptr, is NULL. + * - The number of bytes requested, msg_len is less than zero. + * - msg_len is anything other than the mq_msgsize of the specified + * queue, and the QNX extended option MQ_READBUF_DYNAMIC hasn't + * been set in the queue's mq_flags. + */ + utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "EINVAL"); + break; + } + case EMSGSIZE: { + /* + * This value indicates one of the following: + * - the QNX extended option MQ_READBUF_DYNAMIC hasn't been set, + * and the given msg_len is shorter than the mq_msgsize for + * the given queue. + * - the extended option MQ_READBUF_DYNAMIC has been set, but the + * given msg_len is too short for the message that would have + * been received. + */ + utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "EMSGSIZE"); + break; + } - return HasReturnvaluesIF::RETURN_FAILED; - } + case EINTR: { + //The operation was interrupted by a signal. + utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "EINTR"); + break; + } + case ETIMEDOUT: { + //The operation was interrupted by a signal. + utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "ETIMEDOUT"); + break; + } - } + default: + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_FAILED; + } } MessageQueueId_t MessageQueue::getLastPartner() const { - return this->lastPartner; + return this->lastPartner; } ReturnValue_t MessageQueue::flush(uint32_t* count) { - mq_attr attrib; - int status = mq_getattr(id,&attrib); - if(status != 0){ - switch(errno){ - case EBADF: - //mqdes doesn't represent a valid message queue. -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::flush configuration error, " - "called flush with an invalid queue ID" << std::endl; -#endif - /*NO BREAK*/ - case EINVAL: - //mq_attr is NULL - default: - return HasReturnvaluesIF::RETURN_FAILED; - } - } - *count = attrib.mq_curmsgs; - attrib.mq_curmsgs = 0; - status = mq_setattr(id,&attrib,NULL); - if(status != 0){ - switch(errno){ - case EBADF: - //mqdes doesn't represent a valid message queue. -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::flush configuration error, " - "called flush with an invalid queue ID" << std::endl; -#endif - /*NO BREAK*/ - case EINVAL: - /* - * This value indicates one of the following: - * - mq_attr is NULL. - * - MQ_MULT_NOTIFY had been set for this queue, and the given - * mq_flags includes a 0 in the MQ_MULT_NOTIFY bit. Once - * MQ_MULT_NOTIFY has been turned on, it may never be turned off. - */ - default: - return HasReturnvaluesIF::RETURN_FAILED; - } - } - return HasReturnvaluesIF::RETURN_OK; + mq_attr attrib; + int status = mq_getattr(id,&attrib); + if(status != 0){ + switch(errno){ + case EBADF: + //mqdes doesn't represent a valid message queue. + utility::printUnixErrorGeneric(CLASS_NAME, "flush", "EBADF"); + break; + /*NO BREAK*/ + case EINVAL: + //mq_attr is NULL + utility::printUnixErrorGeneric(CLASS_NAME, "flush", "EINVAL"); + break; + default: + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_FAILED; + } + *count = attrib.mq_curmsgs; + attrib.mq_curmsgs = 0; + status = mq_setattr(id,&attrib,NULL); + if(status != 0){ + switch(errno) { + case EBADF: + //mqdes doesn't represent a valid message queue. + utility::printUnixErrorGeneric(CLASS_NAME, "flush", "EBADF"); + break; + case EINVAL: + /* + * This value indicates one of the following: + * - mq_attr is NULL. + * - MQ_MULT_NOTIFY had been set for this queue, and the given + * mq_flags includes a 0 in the MQ_MULT_NOTIFY bit. Once + * MQ_MULT_NOTIFY has been turned on, it may never be turned off. + */ + utility::printUnixErrorGeneric(CLASS_NAME, "flush", "EINVAL"); + break; + default: + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; } MessageQueueId_t MessageQueue::getId() const { - return this->id; + return this->id; } void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) { - this->defaultDestination = defaultDestination; + this->defaultDestination = defaultDestination; } ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message, - MessageQueueId_t sentFrom, bool ignoreFault) { - return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault); + MessageQueueId_t sentFrom, bool ignoreFault) { + return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault); } ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, - MessageQueueMessageIF* message, MessageQueueId_t sentFrom, - bool ignoreFault) { - return sendMessageFromMessageQueue(sendTo,message, sentFrom,ignoreFault); + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault) { + return sendMessageFromMessageQueue(sendTo,message, sentFrom,ignoreFault); } MessageQueueId_t MessageQueue::getDefaultDestination() const { - return this->defaultDestination; + return this->defaultDestination; } bool MessageQueue::isDefaultDestinationSet() const { - return (defaultDestination != NO_QUEUE); + return (defaultDestination != NO_QUEUE); } uint16_t MessageQueue::queueCounter = 0; ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, - MessageQueueMessageIF *message, MessageQueueId_t sentFrom, - bool ignoreFault) { - if(message == nullptr) { + MessageQueueMessageIF *message, MessageQueueId_t sentFrom, + bool ignoreFault) { + if(message == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 +<<<<<<< HEAD sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is " "nullptr!" << std::endl; #endif @@ -373,37 +289,143 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, sif::error << strerror(errno) << " in " <<"mq_send to: " << sendTo << " sent from " << sentFrom << std::endl; +======= + sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is nullptr!" << std::endl; +#else + sif::printError("MessageQueue::sendMessageFromMessageQueue: Message is nullptr!\n"); +>>>>>>> 38910143400e455f5184ad85be98e05638c2eea6 #endif - return DESTINATION_INVALID; - } - case EINTR: - //The call was interrupted by a signal. - case EINVAL: - /* - * This value indicates one of the following: - * - msg_ptr is NULL. - * - msg_len is negative. - * - msg_prio is greater than MQ_PRIO_MAX. - * - msg_prio is less than 0. - * - MQ_PRIO_RESTRICT is set in the mq_attr of mq_des, and - * msg_prio is greater than the priority of the calling process. - */ + return HasReturnvaluesIF::RETURN_FAILED; + } + + message->setSender(sentFrom); + int result = mq_send(sendTo, + reinterpret_cast(message->getBuffer()), + message->getMessageSize(),0); + + //TODO: Check if we're in ISR. + if (result != 0) { + if(!ignoreFault){ + InternalErrorReporterIF* internalErrorReporter = + objectManager->get(objects::INTERNAL_ERROR_REPORTER); + if (internalErrorReporter != NULL) { + internalErrorReporter->queueMessageNotSent(); + } + } + switch(errno){ + case EAGAIN: + //The O_NONBLOCK flag was set when opening the queue, or the + //MQ_NONBLOCK flag was set in its attributes, and the + //specified queue is full. + return MessageQueueIF::FULL; + case EBADF: { + //mq_des doesn't represent a valid message queue descriptor, + //or mq_des wasn't opened for writing. + + utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EBADF"); #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::sendMessage: Configuration error " - << strerror(errno) << " in mq_send" << std::endl; + sif::warning << "mq_send to: " << sendTo << " sent from " + << sentFrom << "failed" << std::endl; +#else + sif::printWarning("mq_send to: %d sent from %d failed\n", sendTo, sentFrom); #endif - /*NO BREAK*/ - case EMSGSIZE: - // The msg_len is greater than the msgsize associated with - //the specified queue. -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MessageQueue::sendMessage: Size error [" << - strerror(errno) << "] in mq_send" << std::endl; -#endif - /*NO BREAK*/ - default: - return HasReturnvaluesIF::RETURN_FAILED; - } - } - return HasReturnvaluesIF::RETURN_OK; + return DESTINATION_INVALID; + } + case EINTR: + //The call was interrupted by a signal. + utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EINTR"); + break; + case EINVAL: + /* + * This value indicates one of the following: + * - msg_ptr is NULL. + * - msg_len is negative. + * - msg_prio is greater than MQ_PRIO_MAX. + * - msg_prio is less than 0. + * - MQ_PRIO_RESTRICT is set in the mq_attr of mq_des, and + * msg_prio is greater than the priority of the calling process. + */ + utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EINVAL"); + break; + case EMSGSIZE: + // The msg_len is greater than the msgsize associated with + //the specified queue. + utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EMSGSIZE"); + break; + default: + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MessageQueue::handleOpenError(mq_attr* attributes, + uint32_t messageDepth) { + switch(errno) { + case(EINVAL): { + utility::printUnixErrorGeneric(CLASS_NAME, "MessageQueue", "EINVAL"); + size_t defaultMqMaxMsg = 0; + // Not POSIX conformant, but should work for all UNIX systems. + // Just an additional helpful printout :-) + if(std::ifstream("/proc/sys/fs/mqueue/msg_max",std::ios::in) >> + defaultMqMaxMsg and defaultMqMaxMsg < messageDepth) { + /* + See: https://www.man7.org/linux/man-pages/man3/mq_open.3.html + This happens if the msg_max value is not large enough + It is ignored if the executable is run in privileged mode. + Run the unlockRealtime script or grant the mode manually by using: + sudo setcap 'CAP_SYS_RESOURCE=+ep' + + Persistent solution for session: + echo | sudo tee /proc/sys/fs/mqueue/msg_max + + Permanent solution: + sudo nano /etc/sysctl.conf + Append at end: fs/mqueue/msg_max = + Apply changes with: sudo sysctl -p + */ +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "MessageQueue::MessageQueue: Default MQ size " << defaultMqMaxMsg << + " is too small for requested size " << messageDepth << std::endl; + sif::error << "This error can be fixed by setting the maximum " + "allowed message size higher!" << std::endl; +#else + sif::printError("MessageQueue::MessageQueue: Default MQ size %d is too small for" + "requested size %d\n"); + sif::printError("This error can be fixes by setting the maximum allowed" + "message size higher!\n"); +#endif + } + break; + } + case(EEXIST): { + // An error occured during open. + // We need to distinguish if it is caused by an already created queue + // There's another queue with the same name + // We unlink the other queue + int status = mq_unlink(name); + if (status != 0) { + utility::printUnixErrorGeneric(CLASS_NAME, "MessageQueue", "EEXIST"); + } + else { + // Successful unlinking, try to open again + mqd_t tempId = mq_open(name, + O_NONBLOCK | O_RDWR | O_CREAT | O_EXCL, + S_IWUSR | S_IREAD | S_IWGRP | S_IRGRP, attributes); + if (tempId != -1) { + //Successful mq_open + this->id = tempId; + return HasReturnvaluesIF::RETURN_OK; + } + } + break; + } + + default: { + // Failed either the first time or the second time + utility::printUnixErrorGeneric(CLASS_NAME, "MessageQueue", "Unknown"); + } + } + return HasReturnvaluesIF::RETURN_FAILED; } diff --git a/osal/linux/MessageQueue.h b/osal/linux/MessageQueue.h index 239bbbdb..935ee948 100644 --- a/osal/linux/MessageQueue.h +++ b/osal/linux/MessageQueue.h @@ -181,7 +181,8 @@ private: static uint16_t queueCounter; const size_t maxMessageSize; - ReturnValue_t handleError(mq_attr* attributes, uint32_t messageDepth); + static constexpr const char* CLASS_NAME = "MessageQueue"; + ReturnValue_t handleOpenError(mq_attr* attributes, uint32_t messageDepth); }; #endif /* FSFW_OSAL_LINUX_MESSAGEQUEUE_H_ */ diff --git a/osal/linux/Mutex.cpp b/osal/linux/Mutex.cpp index c642b132..33e84aac 100644 --- a/osal/linux/Mutex.cpp +++ b/osal/linux/Mutex.cpp @@ -1,43 +1,34 @@ #include "Mutex.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" +#include "unixUtility.h" + +#include "../../serviceinterface/ServiceInterface.h" #include "../../timemanager/Clock.h" -uint8_t Mutex::count = 0; - - #include #include +uint8_t Mutex::count = 0; + Mutex::Mutex() { pthread_mutexattr_t mutexAttr; int status = pthread_mutexattr_init(&mutexAttr); if (status != 0) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Mutex: Attribute init failed with: " << strerror(status) << std::endl; -#endif + utility::printUnixErrorGeneric("Mutex", "Mutex", "pthread_mutexattr_init"); } status = pthread_mutexattr_setprotocol(&mutexAttr, PTHREAD_PRIO_INHERIT); if (status != 0) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Mutex: Attribute set PRIO_INHERIT failed with: " << strerror(status) - << std::endl; -#endif + utility::printUnixErrorGeneric("Mutex", "Mutex", "pthread_mutexattr_setprotocol"); } status = pthread_mutex_init(&mutex, &mutexAttr); if (status != 0) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Mutex: creation with name, id " << mutex.__data.__count - << ", " << " failed with " << strerror(status) << std::endl; -#endif + utility::printUnixErrorGeneric("Mutex", "Mutex", "pthread_mutex_init"); } // After a mutex attributes object has been used to initialize one or more // mutexes, any function affecting the attributes object // (including destruction) shall not affect any previously initialized mutexes. status = pthread_mutexattr_destroy(&mutexAttr); if (status != 0) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Mutex: Attribute destroy failed with " << strerror(status) << std::endl; -#endif + utility::printUnixErrorGeneric("Mutex", "Mutex", "pthread_mutexattr_destroy"); } } diff --git a/osal/linux/PeriodicPosixTask.cpp b/osal/linux/PeriodicPosixTask.cpp index 956d4fdf..1cba22ee 100644 --- a/osal/linux/PeriodicPosixTask.cpp +++ b/osal/linux/PeriodicPosixTask.cpp @@ -6,7 +6,6 @@ #include - PeriodicPosixTask::PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, uint32_t period_, void(deadlineMissedFunc_)()): PosixThread(name_, priority_, stackSize_), objectList(), started(false), @@ -32,6 +31,9 @@ ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" << " it implements ExecutableObjectIF!" << std::endl; +#else + sif::printError("PeriodicTask::addComponent: Invalid object. Make sure it" + "implements ExecutableObjectIF!\n"); #endif return HasReturnvaluesIF::RETURN_FAILED; } @@ -48,9 +50,6 @@ ReturnValue_t PeriodicPosixTask::sleepFor(uint32_t ms) { ReturnValue_t PeriodicPosixTask::startTask(void) { started = true; -#if FSFW_CPP_OSTREAM_ENABLED == 1 - //sif::info << stackSize << std::endl; -#endif PosixThread::createTask(&taskEntryPoint,this); return HasReturnvaluesIF::RETURN_OK; } diff --git a/osal/linux/PosixThread.cpp b/osal/linux/PosixThread.cpp index 72adfb14..36501282 100644 --- a/osal/linux/PosixThread.cpp +++ b/osal/linux/PosixThread.cpp @@ -1,4 +1,5 @@ #include "PosixThread.h" +#include "unixUtility.h" #include "../../serviceinterface/ServiceInterface.h" @@ -6,263 +7,240 @@ #include PosixThread::PosixThread(const char* name_, int priority_, size_t stackSize_): - thread(0), priority(priority_), stackSize(stackSize_) { + thread(0), priority(priority_), stackSize(stackSize_) { name[0] = '\0'; std::strncat(name, name_, PTHREAD_MAX_NAMELEN - 1); } PosixThread::~PosixThread() { - //No deletion and no free of Stack Pointer + //No deletion and no free of Stack Pointer } ReturnValue_t PosixThread::sleep(uint64_t ns) { - //TODO sleep might be better with timer instead of sleep() - timespec time; - time.tv_sec = ns/1000000000; - time.tv_nsec = ns - time.tv_sec*1e9; + //TODO sleep might be better with timer instead of sleep() + timespec time; + time.tv_sec = ns/1000000000; + time.tv_nsec = ns - time.tv_sec*1e9; - //Remaining Time is not set here - int status = nanosleep(&time,NULL); - if(status != 0){ - switch(errno){ - case EINTR: - //The nanosleep() function was interrupted by a signal. - return HasReturnvaluesIF::RETURN_FAILED; - case EINVAL: - //The rqtp argument specified a nanosecond value less than zero or - // greater than or equal to 1000 million. - return HasReturnvaluesIF::RETURN_FAILED; - default: - return HasReturnvaluesIF::RETURN_FAILED; - } + //Remaining Time is not set here + int status = nanosleep(&time,NULL); + if(status != 0){ + switch(errno){ + case EINTR: + //The nanosleep() function was interrupted by a signal. + return HasReturnvaluesIF::RETURN_FAILED; + case EINVAL: + //The rqtp argument specified a nanosecond value less than zero or + // greater than or equal to 1000 million. + return HasReturnvaluesIF::RETURN_FAILED; + default: + return HasReturnvaluesIF::RETURN_FAILED; + } - } - return HasReturnvaluesIF::RETURN_OK; + } + return HasReturnvaluesIF::RETURN_OK; } void PosixThread::suspend() { - //Wait for SIGUSR1 - int caughtSig = 0; - sigset_t waitSignal; - sigemptyset(&waitSignal); - sigaddset(&waitSignal, SIGUSR1); - sigwait(&waitSignal, &caughtSig); - if (caughtSig != SIGUSR1) { + //Wait for SIGUSR1 + int caughtSig = 0; + sigset_t waitSignal; + sigemptyset(&waitSignal); + sigaddset(&waitSignal, SIGUSR1); + sigwait(&waitSignal, &caughtSig); + if (caughtSig != SIGUSR1) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "FixedTimeslotTask: Unknown Signal received: " << - caughtSig << std::endl; + sif::error << "FixedTimeslotTask::suspend: Unknown Signal received: " << caughtSig << + std::endl; +#else + sif::printError("FixedTimeslotTask::suspend: Unknown Signal received: %d\n", caughtSig); #endif - } + } } void PosixThread::resume(){ - /* Signal the thread to start. Makes sense to call kill to start or? ;) - * - * According to Posix raise(signal) will call pthread_kill(pthread_self(), sig), - * but as the call must be done from the thread itsself this is not possible here - */ - pthread_kill(thread,SIGUSR1); + /* Signal the thread to start. Makes sense to call kill to start or? ;) + * + * According to Posix raise(signal) will call pthread_kill(pthread_self(), sig), + * but as the call must be done from the thread itsself this is not possible here + */ + pthread_kill(thread,SIGUSR1); } bool PosixThread::delayUntil(uint64_t* const prevoiusWakeTime_ms, - const uint64_t delayTime_ms) { - uint64_t nextTimeToWake_ms; - bool shouldDelay = false; - //Get current Time - const uint64_t currentTime_ms = getCurrentMonotonicTimeMs(); - /* Generate the tick time at which the task wants to wake. */ - nextTimeToWake_ms = (*prevoiusWakeTime_ms) + delayTime_ms; + const uint64_t delayTime_ms) { + uint64_t nextTimeToWake_ms; + bool shouldDelay = false; + //Get current Time + const uint64_t currentTime_ms = getCurrentMonotonicTimeMs(); + /* Generate the tick time at which the task wants to wake. */ + nextTimeToWake_ms = (*prevoiusWakeTime_ms) + delayTime_ms; - if (currentTime_ms < *prevoiusWakeTime_ms) { - /* The tick count has overflowed since this function was + if (currentTime_ms < *prevoiusWakeTime_ms) { + /* The tick count has overflowed since this function was lasted called. In this case the only time we should ever actually delay is if the wake time has also overflowed, and the wake time is greater than the tick time. When this is the case it is as if neither time had overflowed. */ - if ((nextTimeToWake_ms < *prevoiusWakeTime_ms) - && (nextTimeToWake_ms > currentTime_ms)) { - shouldDelay = true; - } - } else { - /* The tick time has not overflowed. In this case we will + if ((nextTimeToWake_ms < *prevoiusWakeTime_ms) + && (nextTimeToWake_ms > currentTime_ms)) { + shouldDelay = true; + } + } else { + /* The tick time has not overflowed. In this case we will delay if either the wake time has overflowed, and/or the tick time is less than the wake time. */ - if ((nextTimeToWake_ms < *prevoiusWakeTime_ms) - || (nextTimeToWake_ms > currentTime_ms)) { - shouldDelay = true; - } - } + if ((nextTimeToWake_ms < *prevoiusWakeTime_ms) + || (nextTimeToWake_ms > currentTime_ms)) { + shouldDelay = true; + } + } - /* Update the wake time ready for the next call. */ + /* Update the wake time ready for the next call. */ - (*prevoiusWakeTime_ms) = nextTimeToWake_ms; + (*prevoiusWakeTime_ms) = nextTimeToWake_ms; - if (shouldDelay) { - uint64_t sleepTime = nextTimeToWake_ms - currentTime_ms; - PosixThread::sleep(sleepTime * 1000000ull); - return true; - } - //We are shifting the time in case the deadline was missed like rtems - (*prevoiusWakeTime_ms) = currentTime_ms; - return false; + if (shouldDelay) { + uint64_t sleepTime = nextTimeToWake_ms - currentTime_ms; + PosixThread::sleep(sleepTime * 1000000ull); + return true; + } + //We are shifting the time in case the deadline was missed like rtems + (*prevoiusWakeTime_ms) = currentTime_ms; + return false; } uint64_t PosixThread::getCurrentMonotonicTimeMs(){ - timespec timeNow; - clock_gettime(CLOCK_MONOTONIC_RAW, &timeNow); - uint64_t currentTime_ms = (uint64_t) timeNow.tv_sec * 1000 - + timeNow.tv_nsec / 1000000; + timespec timeNow; + clock_gettime(CLOCK_MONOTONIC_RAW, &timeNow); + uint64_t currentTime_ms = (uint64_t) timeNow.tv_sec * 1000 + + timeNow.tv_nsec / 1000000; - return currentTime_ms; + return currentTime_ms; } void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - //sif::debug << "PosixThread::createTask" << std::endl; + //sif::debug << "PosixThread::createTask" << std::endl; #endif - /* - * The attr argument points to a pthread_attr_t structure whose contents + /* + * The attr argument points to a pthread_attr_t structure whose contents are used at thread creation time to determine attributes for the new thread; this structure is initialized using pthread_attr_init(3) and related functions. If attr is NULL, then the thread is created with default attributes. - */ - pthread_attr_t attributes; - int status = pthread_attr_init(&attributes); - if(status != 0){ + */ + pthread_attr_t attributes; + int status = pthread_attr_init(&attributes); + if(status != 0){ + utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_init"); + } + void* stackPointer; + status = posix_memalign(&stackPointer, sysconf(_SC_PAGESIZE), stackSize); + if(status != 0) { + if(errno == ENOMEM) { + size_t stackMb = stackSize/10e6; #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Posix Thread attribute init failed with: " << - strerror(status) << std::endl; -#endif - } - void* stackPointer; - status = posix_memalign(&stackPointer, sysconf(_SC_PAGESIZE), stackSize); - if(status != 0){ -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PosixThread::createTask: Stack init failed with: " << - strerror(status) << std::endl; -#endif - if(errno == ENOMEM) { - size_t stackMb = stackSize/10e6; -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PosixThread::createTask: Insufficient memory for" - " the requested " << stackMb << " MB" << std::endl; + sif::error << "PosixThread::createTask: Insufficient memory for" + " the requested " << stackMb << " MB" << std::endl; #else - sif::printError("PosixThread::createTask: Insufficient memory for " - "the requested %lu MB\n", static_cast(stackMb)); + sif::printError("PosixThread::createTask: Insufficient memory for " + "the requested %lu MB\n", static_cast(stackMb)); #endif - } - else if(errno == EINVAL) { + utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "ENOMEM"); + } + else if(errno == EINVAL) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PosixThread::createTask: Wrong alignment argument!" - << std::endl; + sif::error << "PosixThread::createTask: Wrong alignment argument!" + << std::endl; #else - sif::printError("PosixThread::createTask: " - "Wrong alignment argument!\n"); + sif::printError("PosixThread::createTask: Wrong alignment argument!\n"); #endif - } - return; - } + utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "EINVAL"); + } + return; + } - status = pthread_attr_setstack(&attributes, stackPointer, stackSize); - if(status != 0){ + status = pthread_attr_setstack(&attributes, stackPointer, stackSize); + if(status != 0) { + utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_setstack"); #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PosixThread::createTask: pthread_attr_setstack " - " failed with: " << strerror(status) << std::endl; - sif::error << "Make sure the specified stack size is valid and is " - "larger than the minimum allowed stack size." << std::endl; + sif::warning << "Make sure the specified stack size is valid and is " + "larger than the minimum allowed stack size." << std::endl; +#else + sif::printWarning("Make sure the specified stack size is valid and is " + "larger than the minimum allowed stack size.\n"); #endif - } + } - status = pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED); - if(status != 0){ -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Posix Thread attribute setinheritsched failed with: " << - strerror(status) << std::endl; -#endif - } + status = pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED); + if(status != 0){ + utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_setinheritsched"); + } #ifndef FSFW_USE_REALTIME_FOR_LINUX #error "Please define FSFW_USE_REALTIME_FOR_LINUX with either 0 or 1" #endif #if FSFW_USE_REALTIME_FOR_LINUX == 1 - // FIFO -> This needs root privileges for the process - status = pthread_attr_setschedpolicy(&attributes,SCHED_FIFO); - if(status != 0){ -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Posix Thread attribute schedule policy failed with: " << - strerror(status) << std::endl; -#endif - } + // FIFO -> This needs root privileges for the process + status = pthread_attr_setschedpolicy(&attributes,SCHED_FIFO); + if(status != 0){ + utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_setschedpolicy"); + } - sched_param scheduleParams; - scheduleParams.__sched_priority = priority; - status = pthread_attr_setschedparam(&attributes, &scheduleParams); - if(status != 0){ -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Posix Thread attribute schedule params failed with: " << - strerror(status) << std::endl; + sched_param scheduleParams; + scheduleParams.__sched_priority = priority; + status = pthread_attr_setschedparam(&attributes, &scheduleParams); + if(status != 0){ + utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_setschedparam"); + } #endif - } -#endif - //Set Signal Mask for suspend until startTask is called - sigset_t waitSignal; - sigemptyset(&waitSignal); - sigaddset(&waitSignal, SIGUSR1); - status = pthread_sigmask(SIG_BLOCK, &waitSignal, NULL); - if(status != 0){ -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Posix Thread sigmask failed failed with: " << - strerror(status) << " errno: " << strerror(errno) << std::endl; -#endif - } + //Set Signal Mask for suspend until startTask is called + sigset_t waitSignal; + sigemptyset(&waitSignal); + sigaddset(&waitSignal, SIGUSR1); + status = pthread_sigmask(SIG_BLOCK, &waitSignal, NULL); + if(status != 0){ + utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_sigmask"); + } - - status = pthread_create(&thread,&attributes,fnc_,arg_); - if(status != 0){ + status = pthread_create(&thread,&attributes,fnc_,arg_); + if(status != 0){ + utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_create"); #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PosixThread::createTask: Failed with: " << - strerror(status) << std::endl; - sif::error << "For FSFW_USE_REALTIME_FOR_LINUX == 1 make sure to call " << - "\"all sudo setcap 'cap_sys_nice=eip'\" on the application or set " - "/etc/security/limit.conf" << std::endl; + sif::error << "For FSFW_USE_REALTIME_FOR_LINUX == 1 make sure to call " << + "\"all sudo setcap 'cap_sys_nice=eip'\" on the application or set " + "/etc/security/limit.conf" << std::endl; #else - sif::printError("PosixThread::createTask: Create failed with: %s\n", strerror(status)); - sif::printError("For FSFW_USE_REALTIME_FOR_LINUX == 1 make sure to call " - "\"all sudo setcap 'cap_sys_nice=eip'\" on the application or set " + sif::printError("For FSFW_USE_REALTIME_FOR_LINUX == 1 make sure to call " + "\"all sudo setcap 'cap_sys_nice=eip'\" on the application or set " "/etc/security/limit.conf\n"); #endif - } + } - status = pthread_setname_np(thread,name); - if(status != 0){ + status = pthread_setname_np(thread,name); + if(status != 0){ + utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_setname_np"); + if(status == ERANGE) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PosixThread::createTask: setname failed with: " << - strerror(status) << std::endl; + sif::warning << "PosixThread::createTask: Task name length longer" + " than 16 chars. Truncating.." << std::endl; +#else + sif::printWarning("PosixThread::createTask: Task name length longer" + " than 16 chars. Truncating..\n"); #endif - if(status == ERANGE) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PosixThread::createTask: Task name length longer" - " than 16 chars. Truncating.." << std::endl; -#endif - name[15] = '\0'; - status = pthread_setname_np(thread,name); - if(status != 0){ -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PosixThread::createTask: Setting name" - " did not work.." << std::endl; -#endif - } - } - } + name[15] = '\0'; + status = pthread_setname_np(thread,name); + if(status != 0){ + utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_setname_np"); + } + } + } - status = pthread_attr_destroy(&attributes); - if(status!=0){ -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Posix Thread attribute destroy failed with: " << - strerror(status) << std::endl; -#endif - } + status = pthread_attr_destroy(&attributes); + if (status != 0) { + utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_destroy"); + } } diff --git a/osal/linux/PosixThread.h b/osal/linux/PosixThread.h index 7d8d349a..9c0ad39b 100644 --- a/osal/linux/PosixThread.h +++ b/osal/linux/PosixThread.h @@ -9,69 +9,71 @@ class PosixThread { public: - static constexpr uint8_t PTHREAD_MAX_NAMELEN = 16; - PosixThread(const char* name_, int priority_, size_t stackSize_); - virtual ~PosixThread(); - /** - * Set the Thread to sleep state - * @param ns Nanosecond sleep time - * @return Returns Failed if sleep fails - */ - static ReturnValue_t sleep(uint64_t ns); - /** - * @brief Function to suspend the task until SIGUSR1 was received - * - * @details Will be called in the beginning to suspend execution until startTask() is called explicitly. - */ - void suspend(); + static constexpr uint8_t PTHREAD_MAX_NAMELEN = 16; + PosixThread(const char* name_, int priority_, size_t stackSize_); + virtual ~PosixThread(); + /** + * Set the Thread to sleep state + * @param ns Nanosecond sleep time + * @return Returns Failed if sleep fails + */ + static ReturnValue_t sleep(uint64_t ns); + /** + * @brief Function to suspend the task until SIGUSR1 was received + * + * @details Will be called in the beginning to suspend execution until startTask() is called explicitly. + */ + void suspend(); - /** - * @brief Function to allow a other thread to start the thread again from suspend state - * - * @details Restarts the Thread after suspend call - */ - void resume(); + /** + * @brief Function to allow a other thread to start the thread again from suspend state + * + * @details Restarts the Thread after suspend call + */ + void resume(); - /** - * Delay function similar to FreeRtos delayUntil function - * - * @param prevoiusWakeTime_ms Needs the previous wake time and returns the next wakeup time - * @param delayTime_ms Time period to delay - * - * @return False If deadline was missed; True if task was delayed - */ - static bool delayUntil(uint64_t* const prevoiusWakeTime_ms, const uint64_t delayTime_ms); + /** + * Delay function similar to FreeRtos delayUntil function + * + * @param prevoiusWakeTime_ms Needs the previous wake time and returns the next wakeup time + * @param delayTime_ms Time period to delay + * + * @return False If deadline was missed; True if task was delayed + */ + static bool delayUntil(uint64_t* const prevoiusWakeTime_ms, const uint64_t delayTime_ms); - /** - * Returns the current time in milliseconds from CLOCK_MONOTONIC - * - * @return current time in milliseconds from CLOCK_MONOTONIC - */ - static uint64_t getCurrentMonotonicTimeMs(); + /** + * Returns the current time in milliseconds from CLOCK_MONOTONIC + * + * @return current time in milliseconds from CLOCK_MONOTONIC + */ + static uint64_t getCurrentMonotonicTimeMs(); protected: - pthread_t thread; + pthread_t thread; - /** - * @brief Function that has to be called by derived class because the - * derived class pointer has to be valid as argument. - * @details - * This function creates a pthread with the given parameters. As the - * function requires a pointer to the derived object it has to be called - * after the this pointer of the derived object is valid. - * Sets the taskEntryPoint as function to be called by new a thread. - * @param fnc_ Function which will be executed by the thread. - * @param arg_ - * argument of the taskEntryPoint function, needs to be this pointer - * of derived class - */ - void createTask(void* (*fnc_)(void*),void* arg_); + /** + * @brief Function that has to be called by derived class because the + * derived class pointer has to be valid as argument. + * @details + * This function creates a pthread with the given parameters. As the + * function requires a pointer to the derived object it has to be called + * after the this pointer of the derived object is valid. + * Sets the taskEntryPoint as function to be called by new a thread. + * @param fnc_ Function which will be executed by the thread. + * @param arg_ + * argument of the taskEntryPoint function, needs to be this pointer + * of derived class + */ + void createTask(void* (*fnc_)(void*),void* arg_); private: - char name[PTHREAD_MAX_NAMELEN]; - int priority; - size_t stackSize = 0; + char name[PTHREAD_MAX_NAMELEN]; + int priority; + size_t stackSize = 0; + + static constexpr const char* CLASS_NAME = "PosixThread"; }; #endif /* FRAMEWORK_OSAL_LINUX_POSIXTHREAD_H_ */ diff --git a/osal/linux/unixUtility.cpp b/osal/linux/unixUtility.cpp new file mode 100644 index 00000000..d7aab4ba --- /dev/null +++ b/osal/linux/unixUtility.cpp @@ -0,0 +1,32 @@ +#include "FSFWConfig.h" +#include "unixUtility.h" +#include "../../serviceinterface/ServiceInterface.h" + +#include +#include + +void utility::printUnixErrorGeneric(const char* const className, + const char* const function, const char* const failString, + sif::OutputTypes outputType) { + if(className == nullptr or failString == nullptr or function == nullptr) { + return; + } +#if FSFW_VERBOSE_LEVEL >= 1 + if(outputType == sif::OutputTypes::OUT_ERROR) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << className << "::" << function << ":" << failString << " error: " + << strerror(errno) << std::endl; +#else + sif::printError("%s::%s: %s error: %s\n", className, function, failString, strerror(errno)); +#endif + } + else { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << className << "::" << function << ":" << failString << " error: " + << strerror(errno) << std::endl; +#else + sif::printWarning("%s::%s: %s error: %s\n", className, function, failString, strerror(errno)); +#endif + } +#endif +} diff --git a/osal/linux/unixUtility.h b/osal/linux/unixUtility.h new file mode 100644 index 00000000..8a964f3a --- /dev/null +++ b/osal/linux/unixUtility.h @@ -0,0 +1,13 @@ +#ifndef FSFW_OSAL_LINUX_UNIXUTILITY_H_ +#define FSFW_OSAL_LINUX_UNIXUTILITY_H_ + +#include "../../serviceinterface/serviceInterfaceDefintions.h" + +namespace utility { + +void printUnixErrorGeneric(const char* const className, const char* const function, + const char* const failString, sif::OutputTypes outputType = sif::OutputTypes::OUT_ERROR); + +} + +#endif /* FSFW_OSAL_LINUX_UNIXUTILITY_H_ */ diff --git a/unittest/tests/datapoollocal/LocalPoolOwnerBase.h b/unittest/tests/datapoollocal/LocalPoolOwnerBase.h index 8d2073b0..4c244b16 100644 --- a/unittest/tests/datapoollocal/LocalPoolOwnerBase.h +++ b/unittest/tests/datapoollocal/LocalPoolOwnerBase.h @@ -1,7 +1,7 @@ #ifndef FSFW_UNITTEST_TESTS_DATAPOOLLOCAL_LOCALPOOLOWNERBASE_H_ #define FSFW_UNITTEST_TESTS_DATAPOOLLOCAL_LOCALPOOLOWNERBASE_H_ -#include +#include "objects/systemObjectList.h" #include #include