Merge pull request 'FreeRTOS Queue Map Manager' (#423) from KSat/fsfw:mueller/freertos-queue-map-manager into development

Reviewed-on: fsfw/fsfw#423

osal/FreeRTOS/CMakeLists.txt

@@ -15,6 +15,7 @@ target_sources(${LIB_FSFW_NAME}
 		TaskFactory.cpp
 		Timekeeper.cpp
 		TaskManagement.cpp
+		QueueMapManager.cpp
 )
 
 # FreeRTOS is required to link the FSFW now. It is recommended to compile 

osal/FreeRTOS/MessageQueue.cpp

@@ -1,27 +1,24 @@
 #include "MessageQueue.h"
+#include "QueueMapManager.h"
 #include "../../objectmanager/ObjectManagerIF.h"
 #include "../../serviceinterface/ServiceInterfaceStream.h"
 
-// TODO I guess we should have a way of checking if we are in an ISR and then
-// use the "fromISR" versions of all calls
-// As a first step towards this, introduces system context variable which needs
-// to be switched manually
-// Haven't found function to find system context.
 MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize):
         maxMessageSize(maxMessageSize) {
     handle = xQueueCreate(messageDepth, maxMessageSize);
-#if FSFW_CPP_OSTREAM_ENABLED == 1
     if (handle == nullptr) {
-		sif::error << "MessageQueue::MessageQueue:"
-		        << " Creation failed." << std::endl;
-		sif::error << "Specified Message Depth: " << messageDepth
-		        << std::endl;
-		sif::error << "Specified Maximum Message Size: "
-		        << maxMessageSize << std::endl;
-
-	}
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "MessageQueue::MessageQueue: Creation failed" << std::endl;
+        sif::error << "Specified Message Depth: " << messageDepth << std::endl;
+        sif::error << "Specified Maximum Message Size: " << maxMessageSize << std::endl;
+#else
+        sif::printError("MessageQueue::MessageQueue: Creation failed\n");
+        sif::printError("Specified Message Depth: %d\n", messageDepth);
+        sif::printError("Specified MAximum Message Size: %d\n", maxMessageSize);
 #endif
     }
+    QueueMapManager::instance()->addMessageQueue(handle, &queueId);
+}
 
 MessageQueue::~MessageQueue() {
     if (handle != nullptr) {
@@ -62,13 +59,15 @@ ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
             callContext);
 }
 
+QueueHandle_t MessageQueue::getNativeQueueHandle() {
+    return handle;
+}
 
 ReturnValue_t MessageQueue::handleSendResult(BaseType_t result, bool ignoreFault) {
     if (result != pdPASS) {
         if (not ignoreFault) {
             InternalErrorReporterIF* internalErrorReporter = objectManager->
-					get<InternalErrorReporterIF>(
-					objects::INTERNAL_ERROR_REPORTER);
+                    get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
             if (internalErrorReporter != nullptr) {
                 internalErrorReporter->queueMessageNotSent();
             }
@@ -110,7 +109,7 @@ ReturnValue_t MessageQueue::flush(uint32_t* count) {
 }
 
 MessageQueueId_t MessageQueue::getId() const {
-	return reinterpret_cast<MessageQueueId_t>(handle);
+    return queueId;
 }
 
 void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
@@ -132,29 +131,24 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
         MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
         bool ignoreFault, CallContext callContext) {
     BaseType_t result = pdFALSE;
-	QueueHandle_t destination = nullptr;
-
-	if(sendTo == MessageQueueIF::NO_QUEUE or sendTo == 0x00) {
+    if(sendTo == MessageQueueIF::NO_QUEUE) {
         return MessageQueueIF::DESTINATION_INVALID;
     }
-	else  {
-		destination = reinterpret_cast<QueueHandle_t>(sendTo);
+
+    QueueHandle_t destination = QueueMapManager::instance()->getMessageQueue(sendTo);
+    if(destination == nullptr) {
+        return MessageQueueIF::DESTINATION_INVALID;
     }
 
     message->setSender(sentFrom);
-
-
     if(callContext == CallContext::TASK) {
-        result = xQueueSendToBack(destination,
-                static_cast<const void*>(message->getBuffer()), 0);
+        result = xQueueSendToBack(destination, static_cast<const void*>(message->getBuffer()), 0);
     }
     else {
-        /* If the call context is from an interrupt,
-         * request a context switch if a higher priority task
-         * was blocked by the interrupt. */
+        /* If the call context is from an interrupt, request a context switch if a higher priority
+        task was blocked by the interrupt. */
         BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-        result = xQueueSendFromISR(reinterpret_cast<QueueHandle_t>(sendTo),
-                static_cast<const void*>(message->getBuffer()),
+        result = xQueueSendFromISR(destination, static_cast<const void*>(message->getBuffer()),
                 &xHigherPriorityTaskWoken);
         if(xHigherPriorityTaskWoken == pdTRUE) {
             TaskManagement::requestContextSwitch(callContext);

osal/FreeRTOS/MessageQueue.h

@@ -11,11 +11,6 @@
 #include <freertos/queue.h>
 #include <fsfw/ipc/MessageQueueMessage.h>
 
-// TODO: this class assumes that MessageQueueId_t is the same size as void*
-// (the FreeRTOS handle type), compiler will catch this but it might be nice
-// to have something checking or even an always working solution
-// https://scaryreasoner.wordpress.com/2009/02/28/checking-sizeof-at-compile-time/
-
 /**
  * @brief		This class manages sending and receiving of
  * 				message queue messages.
@@ -112,6 +107,8 @@ public:
 
     bool isDefaultDestinationSet() const override;
 
+    QueueHandle_t getNativeQueueHandle();
+
 protected:
     /**
      * @brief 	Implementation to be called from any send Call within
@@ -141,6 +138,8 @@ protected:
 private:
     bool defaultDestinationSet = false;
     QueueHandle_t handle;
+    MessageQueueId_t queueId = MessageQueueIF::NO_QUEUE;
+
     MessageQueueId_t defaultDestination = MessageQueueIF::NO_QUEUE;
     MessageQueueId_t lastPartner = MessageQueueIF::NO_QUEUE;
     const size_t maxMessageSize;

osal/FreeRTOS/QueueMapManager.cpp

@@ -0,0 +1,58 @@
+#include "QueueMapManager.h"
+#include "../../ipc/MutexFactory.h"
+#include "../../ipc/MutexGuard.h"
+
+QueueMapManager* QueueMapManager::mqManagerInstance = nullptr;
+
+QueueMapManager::QueueMapManager() {
+    mapLock = MutexFactory::instance()->createMutex();
+}
+
+QueueMapManager* QueueMapManager::instance() {
+    if (mqManagerInstance == nullptr){
+        mqManagerInstance = new QueueMapManager();
+    }
+    return QueueMapManager::mqManagerInstance;
+}
+
+ReturnValue_t QueueMapManager::addMessageQueue(QueueHandle_t queue, MessageQueueId_t* id) {
+    MutexGuard lock(mapLock);
+    uint32_t currentId = queueCounter++;
+    auto returnPair = queueMap.emplace(currentId, queue);
+    if(not returnPair.second) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "QueueMapManager::addMessageQueue This ID is already "
+                "inside the map!" << std::endl;
+#else
+        sif::printError("QueueMapManager::addMessageQueue This ID is already "
+                "inside the map!\n");
+#endif
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    if (id != nullptr) {
+        *id = currentId;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+
+}
+
+QueueHandle_t QueueMapManager::getMessageQueue(MessageQueueId_t messageQueueId) const {
+    auto queueIter = queueMap.find(messageQueueId);
+    if(queueIter != queueMap.end()) {
+        return queueIter->second;
+    }
+    else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "QueueMapManager::getQueueHandle: The ID " << messageQueueId <<
+                " does not exists in the map!" << std::endl;
+#else
+        sif::printWarning("QueueMapManager::getQueueHandle: The ID %d does not exist in the map!\n",
+                messageQueueId);
+#endif
+    }
+    return nullptr;
+}
+
+QueueMapManager::~QueueMapManager() {
+    MutexFactory::instance()->deleteMutex(mapLock);
+}

osal/FreeRTOS/QueueMapManager.h

@@ -0,0 +1,50 @@
+#ifndef FSFW_OSAL_FREERTOS_QUEUEMAPMANAGER_H_
+#define FSFW_OSAL_FREERTOS_QUEUEMAPMANAGER_H_
+
+#include "../../ipc/MutexIF.h"
+#include "../../ipc/messageQueueDefinitions.h"
+#include "../../ipc/MessageQueueIF.h"
+
+#include "freertos/FreeRTOS.h"
+#include "freertos/queue.h"
+
+#include <map>
+
+using QueueMap = std::map<MessageQueueId_t, QueueHandle_t>;
+
+class QueueMapManager {
+public:
+
+    //! Returns the single instance of QueueMapManager
+    static QueueMapManager* instance();
+
+    /**
+     * Insert a message queue and the corresponding QueueHandle into the map
+     * @param queue The message queue to insert.
+     * @param id The passed value will be set unless a nullptr is passed
+     * @return
+     */
+    ReturnValue_t addMessageQueue(QueueHandle_t queue, MessageQueueId_t* id);
+
+    /**
+     * Get the message queue handle by providing a message queue ID. Returns nullptr
+     * if the queue ID does not exist in the internal map.
+     * @param messageQueueId
+     * @return
+     */
+    QueueHandle_t getMessageQueue(MessageQueueId_t messageQueueId) const;
+
+private:
+    //! External instantiation forbidden. Constructor still required for singleton instantiation.
+    QueueMapManager();
+    ~QueueMapManager();
+
+    uint32_t queueCounter = 0;
+    MutexIF* mapLock;
+    QueueMap queueMap;
+    static QueueMapManager* mqManagerInstance;
+};
+
+
+
+#endif /* FSFW_OSAL_FREERTOS_QUEUEMAPMANAGER_H_ */

osal/host/QueueMapManager.cpp

@@ -23,9 +23,7 @@ QueueMapManager* QueueMapManager::instance() {
 
 ReturnValue_t QueueMapManager::addMessageQueue(
         MessageQueueIF* queueToInsert, MessageQueueId_t* id) {
-	/* Not thread-safe, but it is assumed all message queues are created at software initialization
-	now. If this is to be made thread-safe in the future, it propably would be sufficient to lock
-	the increment operation here. */
+    MutexGuard lock(mapLock);
     uint32_t currentId = queueCounter++;
     auto returnPair = queueMap.emplace(currentId, queueToInsert);
     if(not returnPair.second) {
@@ -47,7 +45,6 @@ ReturnValue_t QueueMapManager::addMessageQueue(
 
 MessageQueueIF* QueueMapManager::getMessageQueue(
         MessageQueueId_t messageQueueId) const {
-	MutexGuard(mapLock, MutexIF::TimeoutType::WAITING, 50);
     auto queueIter = queueMap.find(messageQueueId);
     if(queueIter != queueMap.end()) {
         return queueIter->second;
@@ -60,7 +57,7 @@ MessageQueueIF* QueueMapManager::getMessageQueue(
         sif::printWarning("QueueMapManager::getQueueHandle: The ID %d does not exist in the map!\n",
                 messageQueueId);
 #endif
+    }
     return nullptr;
 }
-}
 

osal/host/QueueMapManager.h

@@ -15,7 +15,9 @@ using QueueMap = std::unordered_map<MessageQueueId_t, MessageQueueIF*>;
  */
 class QueueMapManager {
 public:
-	//! Returns the single instance of SemaphoreFactory.
+
+
+    //! Returns the single instance of QueueMapManager.
     static QueueMapManager* instance();
 
     /**
@@ -27,14 +29,15 @@ public:
     ReturnValue_t addMessageQueue(MessageQueueIF* queue, MessageQueueId_t*
             id = nullptr);
     /**
-	 * Get the message queue handle by providing a message queue ID.
+     * Get the message queue handle by providing a message queue ID. Returns nullptr
+     * if the queue ID is not contained inside the internal map.
      * @param messageQueueId
      * @return
      */
     MessageQueueIF* getMessageQueue(MessageQueueId_t messageQueueId) const;
 
 private:
-	//! External instantiation is forbidden.
+    //! External instantiation is forbidden. Constructor still required for singleton instantiation.
     QueueMapManager();
     ~QueueMapManager();