Added ISR calls for MQ and task mgmt

The task management defines an external function which
implements a context switch call from an ISR

osal/FreeRTOS/MessageQueue.cpp

@@ -2,11 +2,14 @@
 
 #include <framework/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
+// 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
-MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) :
+// As a first step towards this, introduces system context variable which needs
-defaultDestination(0),lastPartner(0)  {
+// to be switched manually
-	handle = xQueueCreate(message_depth, max_message_size);
+// Haven't found function to find system context.
+MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize) :
+defaultDestination(0),lastPartner(0), callContext(CallContext::task)  {
+	handle = xQueueCreate(messageDepth, maxMessageSize);
 	if (handle == NULL) {
 		sif::error << "MessageQueue creation failed" << std::endl;
 	}
@@ -18,6 +21,10 @@ MessageQueue::~MessageQueue() {
 	}
 }
 
+void MessageQueue::switchSystemContext(CallContext callContext) {
+	this->callContext = callContext;
+}
+
 ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo,
 		MessageQueueMessage* message, bool ignoreFault) {
 	return sendMessageFrom(sendTo, message, this->getId(), ignoreFault);
@@ -27,6 +34,11 @@ ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessage* message) {
 	return sendToDefaultFrom(message, this->getId());
 }
 
+ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessage* message,
+		MessageQueueId_t sentFrom, bool ignoreFault) {
+	return sendMessageFrom(defaultDestination,message,sentFrom,ignoreFault);
+}
+
 ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) {
 	if (this->lastPartner != 0) {
 		return sendMessageFrom(this->lastPartner, message, this->getId());
@@ -35,6 +47,27 @@ ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) {
 	}
 }
 
+ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
+		MessageQueueMessage* message, MessageQueueId_t sentFrom,
+		bool ignoreFault) {
+	return sendMessageFromMessageQueue(sendTo,message,sentFrom,ignoreFault, callContext);
+}
+
+
+ReturnValue_t MessageQueue::handleSendResult(BaseType_t result, bool ignoreFault) {
+	if (result != pdPASS) {
+		if (!ignoreFault) {
+			InternalErrorReporterIF* internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
+					objects::INTERNAL_ERROR_REPORTER);
+			if (internalErrorReporter != NULL) {
+				internalErrorReporter->queueMessageNotSent();
+			}
+		}
+		return MessageQueueIF::FULL;
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message,
 		MessageQueueId_t* receivedFrom) {
 	ReturnValue_t status = this->receiveMessage(message);
@@ -73,17 +106,6 @@ void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
 	this->defaultDestination = defaultDestination;
 }
 
-ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
-		MessageQueueMessage* message, MessageQueueId_t sentFrom,
-		bool ignoreFault) {
-	return sendMessageFromMessageQueue(sendTo,message,sentFrom,ignoreFault);
-}
-
-ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessage* message,
-		MessageQueueId_t sentFrom, bool ignoreFault) {
-	return sendMessageFrom(defaultDestination,message,sentFrom,ignoreFault);
-}
-
 MessageQueueId_t MessageQueue::getDefaultDestination() const {
 	return defaultDestination;
 }
@@ -92,23 +114,28 @@ bool MessageQueue::isDefaultDestinationSet() const {
 	return 0;
 }
 
+
+// static core function to send messages.
 ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
         MessageQueueMessage *message, MessageQueueId_t sentFrom,
-		bool ignoreFault) {
+        bool ignoreFault, CallContext callContext) {
     message->setSender(sentFrom);
-
+    BaseType_t result;
-	BaseType_t result = xQueueSendToBack(reinterpret_cast<void*>(sendTo),reinterpret_cast<const void*>(message->getBuffer()), 0);
+    if(callContext == CallContext::task) {
-	if (result != pdPASS) {
+        result = xQueueSendToBack(reinterpret_cast<QueueHandle_t>(sendTo),
-		if (!ignoreFault) {
+                static_cast<const void*>(message->getBuffer()), 0);
-			InternalErrorReporterIF* internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
+    }
-					objects::INTERNAL_ERROR_REPORTER);
+    else {
-			if (internalErrorReporter != NULL) {
+        // If the call context is from an interrupt,
-				internalErrorReporter->queueMessageNotSent();
+        // 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()),
+                &xHigherPriorityTaskWoken);
+        if(xHigherPriorityTaskWoken == pdTRUE) {
+            TaskManagement::requestContextSwitch(callContext);
         }
     }
-		return MessageQueueIF::FULL;
+    return handleSendResult(result, ignoreFault);
 }
-	return HasReturnvaluesIF::RETURN_OK;
-
-}
-

osal/FreeRTOS/MessageQueue.h

@@ -4,9 +4,14 @@
 #include <framework/internalError/InternalErrorReporterIF.h>
 #include <framework/ipc/MessageQueueIF.h>
 #include <framework/ipc/MessageQueueMessage.h>
+#include <framework/osal/FreeRTOS/TaskManagement.h>
+
+extern "C" {
+#include <freertos/FreeRTOS.h>
+#include <freertos/queue.h>
+}
+
 
-#include <FreeRTOS.h>
-#include <queue.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/
@@ -21,11 +26,17 @@
  *				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
+ *
+ *				Please keep in mind that FreeRTOS offers
+ *				different calls for message queue operations if called from an ISR.
+ *				For now, the system context needs to be switched manually.
+ *	@ingroup osal
+ *	@ingroup message_queue
  */
 class MessageQueue : public MessageQueueIF {
 	friend class MessageQueueSenderIF;
@@ -43,11 +54,15 @@ public:
 	 * 							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();
+
+	void switchSystemContext(CallContext callContext);
+
 	/**
 	 * @brief	This operation sends a message to the given destination.
 	 * @details	It directly uses the sendMessage call of the MessageQueueSender parent, but passes its
@@ -74,6 +89,29 @@ public:
 	 */
 	ReturnValue_t reply( MessageQueueMessage* message );
 
+	/**
+	 * \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.
+	 * \param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full.
+	 */
+	virtual ReturnValue_t sendMessageFrom( MessageQueueId_t sendTo, MessageQueueMessage* message,
+			MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false );
+
+	/**
+	 * \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 sendToDefaultFrom( MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false );
+
 	/**
 	 * @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
@@ -107,26 +145,7 @@ public:
 	 * @brief	This method returns the message queue id of this class's message queue.
 	 */
 	MessageQueueId_t getId() const;
-	/**
+
-	 * \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.
-	 * \param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full.
-	 */
-	virtual ReturnValue_t sendMessageFrom( MessageQueueId_t sendTo, MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false );
-	/**
-	 * \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 sendToDefaultFrom( MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false );
 	/**
 	 * \brief	This method is a simple setter for the default destination.
 	 */
@@ -148,12 +167,20 @@ protected:
 	 * \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.
 	 * \param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full.
+	 * \param context
 	 */
-	static ReturnValue_t sendMessageFromMessageQueue(MessageQueueId_t sendTo,MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE,bool ignoreFault=false);
+	static ReturnValue_t sendMessageFromMessageQueue(MessageQueueId_t sendTo,
+			MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE,
+			bool ignoreFault=false, CallContext callContext = CallContext::task);
+
+	static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault);
+
+
 private:
 	QueueHandle_t handle;
 	MessageQueueId_t defaultDestination;
 	MessageQueueId_t lastPartner;
+	CallContext callContext; //!< Stores the current system context
 };
 
 #endif /* MESSAGEQUEUE_H_ */

osal/FreeRTOS/TaskManagement.cpp

@@ -0,0 +1,24 @@
+#include <framework/osal/FreeRTOS/TaskManagement.h>
+
+void TaskManagement::requestContextSwitchFromTask() {
+	vTaskDelay(0);
+}
+
+void TaskManagement::requestContextSwitch(CallContext callContext = CallContext::task) {
+	if(callContext == CallContext::isr) {
+		// This function depends on the partmacro.h definition for the specific device
+		requestContextSwitchFromISR();
+	} else {
+		requestContextSwitchFromTask();
+	}
+}
+
+TaskHandle_t TaskManagement::getCurrentTaskHandle() {
+	return xTaskGetCurrentTaskHandle();
+}
+
+configSTACK_DEPTH_TYPE TaskManagement::getTaskStackHighWatermark() {
+	return uxTaskGetStackHighWaterMark(TaskManagement::getCurrentTaskHandle());
+}
+
+

osal/FreeRTOS/TaskManagement.h

@@ -0,0 +1,63 @@
+#ifndef FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_
+#define FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_
+
+#include <framework/returnvalues/HasReturnvaluesIF.h>
+
+extern "C" {
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+}
+#include <cstdint>
+
+/**
+ * Architecture dependant portmacro.h function call.
+ * Should be implemented in bsp.
+ */
+extern "C" void requestContextSwitchFromISR();
+
+/*!
+ * Used by functions to tell if they are being called from
+ * within an ISR or from a regular task. This is required because FreeRTOS
+ * has different functions for handling semaphores and messages from within an ISR and task.
+ */
+
+enum CallContext {
+	task = 0x00,//!< task_context
+	isr = 0xFF  //!< isr_context
+};
+
+
+class TaskManagement {
+public:
+	/**
+	 * In this function, a function dependant on the portmacro.h header function calls
+	 * to request a context switch can be specified.
+	 * This can be used if sending to the queue from an ISR caused a task to unblock
+	 * and a context switch is required.
+	 */
+	static void requestContextSwitch(CallContext callContext);
+
+	/**
+	 * If task preemption in FreeRTOS is disabled, a context switch
+	 * can be requested manually by calling this function.
+	 */
+	static void requestContextSwitchFromTask(void);
+
+	/**
+	 * @return The current task handle
+	 */
+	static TaskHandle_t getCurrentTaskHandle();
+
+	/**
+	 * Get returns the minimum amount of remaining stack space in words
+	 * that was a available to the task since the task started executing.
+	 * Please note that the actual value in bytes depends
+	 * on the stack depth type.
+	 * E.g. on a 32 bit machine, a value of 200 means 800 bytes.
+	 * @return Smallest value of stack remaining since the task was started in
+	 * 		   words.
+	 */
+	static configSTACK_DEPTH_TYPE getTaskStackHighWatermark();
+};
+
+#endif /* FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ */