18 changed files with 1083 additions and 107 deletions
--- a/osal/FreeRTOS/BinSemaphUsingTask.cpp
+++ b/osal/FreeRTOS/BinSemaphUsingTask.cpp
@ -0,0 +1,91 @@
+#include <framework/osal/FreeRTOS/BinSemaphUsingTask.h>
+#include <framework/osal/FreeRTOS/TaskManagement.h>
+#include <framework/serviceinterface/ServiceInterfaceStream.h>
+
+BinarySemaphoreUsingTask::BinarySemaphoreUsingTask() {
+	handle = TaskManagement::getCurrentTaskHandle();
+	if(handle == nullptr) {
+		sif::error << "Could not retrieve task handle. Please ensure the"
+				"constructor was called inside a task." << std::endl;
+	}
+	xTaskNotifyGive(handle);
+}
+
+BinarySemaphoreUsingTask::~BinarySemaphoreUsingTask() {
+	// Clear notification value on destruction.
+	xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, nullptr);
+}
+
+ReturnValue_t BinarySemaphoreUsingTask::acquire(uint32_t timeoutMs) {
+	TickType_t timeout = SemaphoreIF::NO_TIMEOUT;
+	if(timeoutMs == SemaphoreIF::MAX_TIMEOUT) {
+		timeout = SemaphoreIF::MAX_TIMEOUT;
+	}
+	else if(timeoutMs > SemaphoreIF::NO_TIMEOUT){
+		timeout = pdMS_TO_TICKS(timeoutMs);
+	}
+	return acquireWithTickTimeout(timeout);
+}
+
+ReturnValue_t BinarySemaphoreUsingTask::acquireWithTickTimeout(
+        TickType_t timeoutTicks) {
+	BaseType_t returncode = ulTaskNotifyTake(pdTRUE, timeoutTicks);
+	if (returncode == pdPASS) {
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	else {
+		return SemaphoreIF::SEMAPHORE_TIMEOUT;
+	}
+}
+
+ReturnValue_t BinarySemaphoreUsingTask::release() {
+	return release(this->handle);
+}
+
+ReturnValue_t BinarySemaphoreUsingTask::release(
+		TaskHandle_t taskHandle) {
+	if(getSemaphoreCounter(taskHandle) == 1) {
+		return SemaphoreIF::SEMAPHORE_NOT_OWNED;
+	}
+	BaseType_t returncode = xTaskNotifyGive(taskHandle);
+	if (returncode == pdPASS) {
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	else {
+		// This should never happen.
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+}
+
+TaskHandle_t BinarySemaphoreUsingTask::getTaskHandle() {
+	return handle;
+}
+
+uint8_t BinarySemaphoreUsingTask::getSemaphoreCounter() const {
+	return getSemaphoreCounter(this->handle);
+}
+
+uint8_t BinarySemaphoreUsingTask::getSemaphoreCounter(
+		TaskHandle_t taskHandle) {
+	uint32_t notificationValue;
+	xTaskNotifyAndQuery(taskHandle, 0, eNoAction, &notificationValue);
+	return notificationValue;
+}
+
+// Be careful with the stack size here. This is called from an ISR!
+ReturnValue_t BinarySemaphoreUsingTask::releaseFromISR(
+		TaskHandle_t taskHandle, BaseType_t * higherPriorityTaskWoken) {
+	if(getSemaphoreCounterFromISR(taskHandle, higherPriorityTaskWoken) == 1) {
+		return SemaphoreIF::SEMAPHORE_NOT_OWNED;
+	}
+	vTaskNotifyGiveFromISR(taskHandle, higherPriorityTaskWoken);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+uint8_t BinarySemaphoreUsingTask::getSemaphoreCounterFromISR(
+		TaskHandle_t taskHandle, BaseType_t* higherPriorityTaskWoken) {
+	uint32_t notificationValue = 0;
+	xTaskNotifyAndQueryFromISR(taskHandle, 0, eNoAction, &notificationValue,
+			higherPriorityTaskWoken);
+	return notificationValue;
+}
--- a/osal/FreeRTOS/BinSemaphUsingTask.h
+++ b/osal/FreeRTOS/BinSemaphUsingTask.h
@ -0,0 +1,75 @@
+#ifndef FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_
+#define FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_
+
+#include <framework/returnvalues/HasReturnvaluesIF.h>
+#include <framework/tasks/SemaphoreIF.h>
+
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+
+/**
+ * @brief 	Binary Semaphore implementation using the task notification value.
+ * 			The notification value should therefore not be used
+ * 			for other purposes.
+ * @details
+ * Additional information: https://www.freertos.org/RTOS-task-notifications.html
+ * and general semaphore documentation.
+ */
+class BinarySemaphoreUsingTask: public SemaphoreIF,
+		public HasReturnvaluesIF {
+public:
+	static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF;
+
+	//! @brief Default ctor
+	BinarySemaphoreUsingTask();
+	//! @brief Default dtor
+	virtual~ BinarySemaphoreUsingTask();
+
+	ReturnValue_t acquire(uint32_t timeoutMs =
+	        SemaphoreIF::NO_TIMEOUT) override;
+	ReturnValue_t release() override;
+	uint8_t getSemaphoreCounter() const override;
+	static uint8_t getSemaphoreCounter(TaskHandle_t taskHandle);
+	static uint8_t getSemaphoreCounterFromISR(TaskHandle_t taskHandle,
+			BaseType_t* higherPriorityTaskWoken);
+
+	/**
+	 * Same as acquire() with timeout in FreeRTOS ticks.
+	 * @param timeoutTicks
+	 * @return - @c RETURN_OK on success
+	 *         - @c RETURN_FAILED on failure
+	 */
+	ReturnValue_t acquireWithTickTimeout(TickType_t timeoutTicks =
+	        SemaphoreIF::NO_TIMEOUT);
+
+	/**
+	 * Get handle to the task related to the semaphore.
+	 * @return
+	 */
+	TaskHandle_t getTaskHandle();
+
+	 /**
+	 * Wrapper function to give back semaphore from handle
+	 * @param semaphore
+	 * @return - @c RETURN_OK on success
+	 *         - @c RETURN_FAILED on failure
+	 */
+	static ReturnValue_t release(TaskHandle_t taskToNotify);
+
+	/**
+	 * Wrapper function to give back semaphore from handle when called from an ISR
+	 * @param semaphore
+	 * @param higherPriorityTaskWoken This will be set to pdPASS if a task with
+	 * a higher priority was unblocked. A context switch should be requested
+	 * from an ISR if this is the case (see TaskManagement functions)
+	 * @return - @c RETURN_OK on success
+	 *         - @c RETURN_FAILED on failure
+	 */
+	static ReturnValue_t releaseFromISR(TaskHandle_t taskToNotify,
+				BaseType_t * higherPriorityTaskWoken);
+
+protected:
+	TaskHandle_t handle;
+};
+
+#endif /* FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ */
--- a/osal/FreeRTOS/BinarySemaphore.cpp
+++ b/osal/FreeRTOS/BinarySemaphore.cpp
@ -0,0 +1,103 @@
+#include <framework/osal/FreeRTOS/BinarySemaphore.h>
+#include <framework/osal/FreeRTOS/TaskManagement.h>
+#include <framework/serviceinterface/ServiceInterfaceStream.h>
+
+BinarySemaphore::BinarySemaphore() {
+	handle = xSemaphoreCreateBinary();
+	if(handle == nullptr) {
+		sif::error << "Semaphore: Binary semaph creation failure" << std::endl;
+	}
+	// Initiated semaphore must be given before it can be taken.
+	xSemaphoreGive(handle);
+}
+
+BinarySemaphore::~BinarySemaphore() {
+	vSemaphoreDelete(handle);
+}
+
+BinarySemaphore::BinarySemaphore(BinarySemaphore&& s) {
+    handle = xSemaphoreCreateBinary();
+    if(handle == nullptr) {
+        sif::error << "Binary semaphore creation failure" << std::endl;
+    }
+    xSemaphoreGive(handle);
+}
+
+BinarySemaphore& BinarySemaphore::operator =(
+        BinarySemaphore&& s) {
+    if(&s != this) {
+        handle = xSemaphoreCreateBinary();
+        if(handle == nullptr) {
+            sif::error << "Binary semaphore creation failure" << std::endl;
+        }
+        xSemaphoreGive(handle);
+    }
+    return *this;
+}
+
+ReturnValue_t BinarySemaphore::acquire(uint32_t timeoutMs) {
+	TickType_t timeout = SemaphoreIF::NO_TIMEOUT;
+	if(timeoutMs == SemaphoreIF::MAX_TIMEOUT) {
+	    timeout = SemaphoreIF::MAX_TIMEOUT;
+	}
+	else if(timeoutMs > SemaphoreIF::NO_TIMEOUT){
+	    timeout = pdMS_TO_TICKS(timeoutMs);
+	}
+	return acquireWithTickTimeout(timeout);
+}
+
+ReturnValue_t BinarySemaphore::acquireWithTickTimeout(TickType_t timeoutTicks) {
+	if(handle == nullptr) {
+		return SemaphoreIF::SEMAPHORE_INVALID;
+	}
+
+	BaseType_t returncode = xSemaphoreTake(handle, timeoutTicks);
+	if (returncode == pdPASS) {
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	else {
+		return SemaphoreIF::SEMAPHORE_TIMEOUT;
+	}
+}
+
+ReturnValue_t BinarySemaphore::release() {
+	return release(handle);
+}
+
+ReturnValue_t BinarySemaphore::release(SemaphoreHandle_t semaphore) {
+	if (semaphore == nullptr) {
+		return SemaphoreIF::SEMAPHORE_INVALID;
+	}
+	BaseType_t returncode = xSemaphoreGive(semaphore);
+	if (returncode == pdPASS) {
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	else {
+		return SemaphoreIF::SEMAPHORE_NOT_OWNED;
+	}
+}
+
+uint8_t BinarySemaphore::getSemaphoreCounter() const {
+	return uxSemaphoreGetCount(handle);
+}
+
+SemaphoreHandle_t BinarySemaphore::getSemaphore() {
+	return handle;
+}
+
+
+// Be careful with the stack size here. This is called from an ISR!
+ReturnValue_t BinarySemaphore::releaseFromISR(
+		SemaphoreHandle_t semaphore, BaseType_t * higherPriorityTaskWoken) {
+	if (semaphore == nullptr) {
+		return SemaphoreIF::SEMAPHORE_INVALID;
+	}
+	BaseType_t returncode = xSemaphoreGiveFromISR(semaphore,
+			higherPriorityTaskWoken);
+	if (returncode == pdPASS) {
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	else {
+		return SemaphoreIF::SEMAPHORE_NOT_OWNED;
+	}
+}
--- a/osal/FreeRTOS/BinarySemaphore.h
+++ b/osal/FreeRTOS/BinarySemaphore.h
@ -0,0 +1,107 @@
+#ifndef FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_
+#define FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_
+
+#include <framework/returnvalues/HasReturnvaluesIF.h>
+#include <framework/tasks/SemaphoreIF.h>
+
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+
+/**
+ * @brief 	OS Tool to achieve synchronization of between tasks or between
+ * 			task and ISR. The default semaphore implementation creates a
+ * 			binary semaphore, which can only be taken once.
+ * @details
+ * Documentation: https://www.freertos.org/Embedded-RTOS-Binary-Semaphores.html
+ *
+ * Please note that if the semaphore implementation is only related to
+ * the synchronization of one task, the new task notifications can be used,
+ * also see the BinSemaphUsingTask and CountingSemaphUsingTask classes.
+ * These use the task notification value instead of a queue and are
+ * faster and more efficient.
+ *
+ * @author 	R. Mueller
+ * @ingroup osal
+ */
+class BinarySemaphore: public SemaphoreIF,
+		public HasReturnvaluesIF {
+public:
+	static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF;
+
+	//! @brief Default ctor
+	BinarySemaphore();
+	//! @brief Copy ctor, deleted explicitely.
+	BinarySemaphore(const BinarySemaphore&) = delete;
+	//! @brief Copy assignment, deleted explicitely.
+	BinarySemaphore& operator=(const BinarySemaphore&) = delete;
+	//! @brief Move ctor
+	BinarySemaphore (BinarySemaphore &&);
+	//! @brief Move assignment
+	BinarySemaphore & operator=(BinarySemaphore &&);
+	//! @brief Destructor
+	virtual ~BinarySemaphore();
+
+	uint8_t getSemaphoreCounter() const override;
+
+	/**
+	 * Take the binary semaphore.
+	 * If the semaphore has already been taken, the task will be blocked
+	 * for a maximum of #timeoutMs or until the semaphore is given back,
+	 * for example by an ISR or another task.
+	 * @param timeoutMs
+	 * @return -@c RETURN_OK on success
+	 *         -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout
+	 */
+	ReturnValue_t acquire(uint32_t timeoutMs =
+	       	   SemaphoreIF::NO_TIMEOUT) override;
+
+	/**
+	 * Same as lockBinarySemaphore() with timeout in FreeRTOS ticks.
+	 * @param timeoutTicks
+	 * @return -@c RETURN_OK on success
+	 *         -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout
+	 */
+	ReturnValue_t  acquireWithTickTimeout(TickType_t timeoutTicks =
+	        BinarySemaphore::NO_TIMEOUT);
+
+	/**
+	 * Release the binary semaphore.
+	 * @return -@c RETURN_OK on success
+	 *         -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is
+	 *         	already available.
+	 */
+	ReturnValue_t release() override;
+
+	/**
+	 * Get Handle to the semaphore.
+	 * @return
+	 */
+	SemaphoreHandle_t getSemaphore();
+
+	 /**
+	 * Wrapper function to give back semaphore from handle
+	 * @param semaphore
+	 * @return -@c RETURN_OK on success
+	 *         -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is
+	 *         	already available.
+	 */
+	static ReturnValue_t release(SemaphoreHandle_t semaphore);
+
+	/**
+	 * Wrapper function to give back semaphore from handle when called from an ISR
+	 * @param semaphore
+	 * @param higherPriorityTaskWoken This will be set to pdPASS if a task with
+	 * a higher priority was unblocked. A context switch from an ISR should
+	 * then be requested (see TaskManagement functions)
+	 * @return -@c RETURN_OK on success
+	 *         -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is
+	 *         	already available.
+	 */
+	static ReturnValue_t releaseFromISR(SemaphoreHandle_t semaphore,
+				BaseType_t * higherPriorityTaskWoken);
+
+protected:
+	SemaphoreHandle_t handle;
+};
+
+#endif /* FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ */
--- a/osal/FreeRTOS/CountingSemaphUsingTask.cpp
+++ b/osal/FreeRTOS/CountingSemaphUsingTask.cpp
@ -0,0 +1,110 @@
+#include <framework/osal/FreeRTOS/CountingSemaphUsingTask.h>
+#include <framework/osal/FreeRTOS/TaskManagement.h>
+#include <framework/serviceinterface/ServiceInterfaceStream.h>
+
+CountingSemaphoreUsingTask::CountingSemaphoreUsingTask(const uint8_t maxCount,
+		uint8_t initCount): maxCount(maxCount) {
+	if(initCount > maxCount) {
+		sif::error << "CountingSemaphoreUsingTask: Max count bigger than "
+				"intial cout. Setting initial count to max count." << std::endl;
+		initCount = maxCount;
+	}
+
+	handle = TaskManagement::getCurrentTaskHandle();
+	if(handle == nullptr) {
+		sif::error << "CountingSemaphoreUsingTask: Could not retrieve task "
+				"handle. Please ensure the constructor was called inside a "
+				"task." << std::endl;
+	}
+
+	uint32_t oldNotificationValue;
+	xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite,
+			&oldNotificationValue);
+	if(oldNotificationValue != 0) {
+		sif::warning << "CountinSemaphoreUsingTask: Semaphore initiated but "
+				"current notification value is not 0. Please ensure the "
+				"notification value is not used for other purposes!" << std::endl;
+	}
+	for(int i = 0; i < initCount; i++) {
+		xTaskNotifyGive(handle);
+	}
+}
+
+CountingSemaphoreUsingTask::~CountingSemaphoreUsingTask() {
+	// Clear notification value on destruction.
+	// If this is not desired, don't call the destructor
+	// (or implement a boolean which disables the reset)
+	xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, nullptr);
+}
+
+ReturnValue_t CountingSemaphoreUsingTask::acquire(uint32_t timeoutMs) {
+	TickType_t timeout = SemaphoreIF::NO_TIMEOUT;
+	if(timeoutMs == SemaphoreIF::MAX_TIMEOUT) {
+		timeout = SemaphoreIF::MAX_TIMEOUT;
+	}
+	else if(timeoutMs > SemaphoreIF::NO_TIMEOUT){
+		timeout = pdMS_TO_TICKS(timeoutMs);
+	}
+	return acquireWithTickTimeout(timeout);
+
+}
+
+ReturnValue_t CountingSemaphoreUsingTask::acquireWithTickTimeout(
+		TickType_t timeoutTicks) {
+	// Decrement notfication value without resetting it.
+	BaseType_t oldCount = ulTaskNotifyTake(pdFALSE, timeoutTicks);
+	if (getSemaphoreCounter() == oldCount - 1) {
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	else {
+		return SemaphoreIF::SEMAPHORE_TIMEOUT;
+	}
+}
+
+ReturnValue_t CountingSemaphoreUsingTask::release() {
+	if(getSemaphoreCounter() == maxCount) {
+		return SemaphoreIF::SEMAPHORE_NOT_OWNED;
+	}
+	return release(handle);
+}
+
+ReturnValue_t CountingSemaphoreUsingTask::release(
+		TaskHandle_t taskToNotify) {
+	BaseType_t returncode = xTaskNotifyGive(taskToNotify);
+	if (returncode == pdPASS) {
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	else {
+		// This should never happen.
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+}
+
+
+uint8_t CountingSemaphoreUsingTask::getSemaphoreCounter() const {
+	uint32_t notificationValue = 0;
+	xTaskNotifyAndQuery(handle, 0, eNoAction, &notificationValue);
+	return notificationValue;
+}
+
+TaskHandle_t CountingSemaphoreUsingTask::getTaskHandle() {
+	return handle;
+}
+
+ReturnValue_t CountingSemaphoreUsingTask::releaseFromISR(
+		TaskHandle_t taskToNotify, BaseType_t* higherPriorityTaskWoken) {
+	vTaskNotifyGiveFromISR(taskToNotify, higherPriorityTaskWoken);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+uint8_t CountingSemaphoreUsingTask::getSemaphoreCounterFromISR(
+		TaskHandle_t task, BaseType_t* higherPriorityTaskWoken) {
+	uint32_t notificationValue;
+	xTaskNotifyAndQueryFromISR(task, 0, eNoAction, &notificationValue,
+			higherPriorityTaskWoken);
+	return notificationValue;
+}
+
+uint8_t CountingSemaphoreUsingTask::getMaxCount() const {
+	return maxCount;
+}
--- a/osal/FreeRTOS/CountingSemaphUsingTask.h
+++ b/osal/FreeRTOS/CountingSemaphUsingTask.h
@ -0,0 +1,100 @@
+#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_
+#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_
+
+#include <framework/osal/FreeRTOS/CountingSemaphUsingTask.h>
+#include <framework/tasks/SemaphoreIF.h>
+
+extern "C" {
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+}
+
+/**
+ * @brief 	Couting Semaphore implementation which uses the notification value
+ * 			of the task. The notification value should therefore not be used
+ * 			for other purposes.
+ * @details
+ *  Additional information: https://www.freertos.org/RTOS-task-notifications.html
+ *  and general semaphore documentation.
+ */
+class CountingSemaphoreUsingTask: public SemaphoreIF {
+public:
+	CountingSemaphoreUsingTask(const uint8_t maxCount, uint8_t initCount);
+	virtual ~CountingSemaphoreUsingTask();
+
+	/**
+	 * Acquire the counting semaphore.
+	 * If no semaphores are available, the task will be blocked
+	 * for a maximum of #timeoutMs or until one is given back,
+	 * for example by an ISR or another task.
+	 * @param timeoutMs
+	 * @return -@c RETURN_OK on success
+	 *         -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout
+	 */
+	ReturnValue_t acquire(uint32_t timeoutMs = SemaphoreIF::NO_TIMEOUT) override;
+
+	/**
+	 * Release a semaphore, increasing the number of available counting
+	 * semaphores up to the #maxCount value.
+	 * @return -@c RETURN_OK on success
+	 *         -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are
+	 *         	already available.
+	 */
+	ReturnValue_t release() override;
+
+	uint8_t getSemaphoreCounter() const override;
+	/**
+	 * Get the semaphore counter from an ISR.
+	 * @param task
+	 * @param higherPriorityTaskWoken This will be set to pdPASS if a task with
+	 * a higher priority was unblocked. A context switch should be requested
+	 * from an ISR if this is the case (see TaskManagement functions)
+	 * @return
+	 */
+	static uint8_t getSemaphoreCounterFromISR(TaskHandle_t task,
+			BaseType_t* higherPriorityTaskWoken);
+
+	/**
+	 * Acquire with a timeout value in ticks
+	 * @param timeoutTicks
+	 * @return -@c RETURN_OK on success
+	 *         -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout
+	 */
+	ReturnValue_t acquireWithTickTimeout(
+			TickType_t timeoutTicks = SemaphoreIF::NO_TIMEOUT);
+
+	/**
+	 * Get handle to the task related to the semaphore.
+	 * @return
+	 */
+	TaskHandle_t getTaskHandle();
+
+	/**
+	 * Release semaphore of task by supplying task handle
+	 * @param taskToNotify
+	 * @return -@c RETURN_OK on success
+	 *         -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are
+	 *         	already available.
+	 */
+	static ReturnValue_t release(TaskHandle_t taskToNotify);
+	/**
+	 * Release seamphore of a task from an ISR.
+	 * @param taskToNotify
+	 * @param higherPriorityTaskWoken This will be set to pdPASS if a task with
+	 * a higher priority was unblocked. A context switch should be requested
+	 * from an ISR if this is the case (see TaskManagement functions)
+	 * @return -@c RETURN_OK on success
+	 *         -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are
+	 *         	already available.
+	 */
+	static ReturnValue_t releaseFromISR(TaskHandle_t taskToNotify,
+			BaseType_t* higherPriorityTaskWoken);
+
+	uint8_t getMaxCount() const;
+
+private:
+	TaskHandle_t handle;
+	const uint8_t maxCount;
+};
+
+#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ */
--- a/osal/FreeRTOS/CountingSemaphore.cpp
+++ b/osal/FreeRTOS/CountingSemaphore.cpp
@ -0,0 +1,43 @@
+#include <framework/osal/FreeRTOS/CountingSemaphore.h>
+#include <framework/serviceinterface/ServiceInterfaceStream.h>
+#include <framework/osal/FreeRTOS/TaskManagement.h>
+
+#include <freertos/semphr.h>
+
+// Make sure #define configUSE_COUNTING_SEMAPHORES 1 is set in
+// free FreeRTOSConfig.h file.
+CountingSemaphore::CountingSemaphore(const uint8_t maxCount, uint8_t initCount):
+		maxCount(maxCount), initCount(initCount) {
+	if(initCount > maxCount) {
+		sif::error << "CountingSemaphoreUsingTask: Max count bigger than "
+				"intial cout. Setting initial count to max count." << std::endl;
+		initCount = maxCount;
+	}
+
+	handle = xSemaphoreCreateCounting(maxCount, initCount);
+	if(handle == nullptr) {
+		sif::error << "CountingSemaphore: Creation failure" << std::endl;
+	}
+}
+
+CountingSemaphore::CountingSemaphore(CountingSemaphore&& other):
+		maxCount(other.maxCount), initCount(other.initCount) {
+	handle = xSemaphoreCreateCounting(other.maxCount, other.initCount);
+	if(handle == nullptr) {
+		sif::error << "CountingSemaphore: Creation failure" << std::endl;
+	}
+}
+
+CountingSemaphore& CountingSemaphore::operator =(
+		CountingSemaphore&& other) {
+	handle = xSemaphoreCreateCounting(other.maxCount, other.initCount);
+	if(handle == nullptr) {
+		sif::error << "CountingSemaphore: Creation failure" << std::endl;
+	}
+	return * this;
+}
+
+
+uint8_t CountingSemaphore::getMaxCount() const {
+	return maxCount;
+}
--- a/osal/FreeRTOS/CountingSemaphore.h
+++ b/osal/FreeRTOS/CountingSemaphore.h
@ -0,0 +1,34 @@
+#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_
+#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_
+#include <framework/osal/FreeRTOS/BinarySemaphore.h>
+
+/**
+ * @brief 	Counting semaphores, which can be acquire more than once.
+ * @details
+ * See: https://www.freertos.org/CreateCounting.html
+ * API of counting semaphores is almost identical to binary semaphores,
+ * so we just inherit from binary semaphore and provide the respective
+ * constructors.
+ */
+class CountingSemaphore: public BinarySemaphore {
+public:
+	CountingSemaphore(const uint8_t maxCount, uint8_t initCount);
+	//! @brief Copy ctor, disabled
+	CountingSemaphore(const CountingSemaphore&) = delete;
+	//! @brief Copy assignment, disabled
+	CountingSemaphore& operator=(const CountingSemaphore&) = delete;
+	//! @brief Move ctor
+	CountingSemaphore (CountingSemaphore &&);
+	//! @brief Move assignment
+	CountingSemaphore & operator=(CountingSemaphore &&);
+
+	/* Same API as binary semaphore otherwise. acquire() can be called
+	 * until there are not semaphores left and release() can be called
+	 * until maxCount is reached. */
+	uint8_t getMaxCount() const;
+private:
+	const uint8_t maxCount;
+	uint8_t initCount = 0;
+};
+
+#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ */
--- a/osal/FreeRTOS/MessageQueue.cpp
+++ b/osal/FreeRTOS/MessageQueue.cpp
@ -1,14 +1,16 @@
-#include "MessageQueue.h"
+#include <framework/osal/FreeRTOS/MessageQueue.h>

 #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
-
-MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) :
-defaultDestination(0),lastPartner(0)  {
-	handle = xQueueCreate(message_depth, max_message_size);
+// 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) {
+	handle = xQueueCreate(messageDepth, maxMessageSize);
 	if (handle == NULL) {
-		sif::error << "MessageQueue creation failed" << std::endl;
+		sif::error << "MessageQueue: Creation failed" << std::endl;
 	}
 }

@ -18,6 +20,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 +33,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 +46,29 @@ 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 (not ignoreFault) {
+			InternalErrorReporterIF* internalErrorReporter =
+					objectManager->get<InternalErrorReporterIF>(
+					objects::INTERNAL_ERROR_REPORTER);
+			if (internalErrorReporter != nullptr) {
+				internalErrorReporter->queueMessageNotSent();
+			}
+		}
+		return MessageQueueIF::FULL;
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message,
 		MessageQueueId_t* receivedFrom) {
 	ReturnValue_t status = this->receiveMessage(message);
@ -45,7 +79,8 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message,
 }

 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message) {
-	BaseType_t result = xQueueReceive(handle,reinterpret_cast<void*>(message->getBuffer()), 0);
+	BaseType_t result = xQueueReceive(handle,reinterpret_cast<void*>(
+			message->getBuffer()), 0);
 	if (result == pdPASS){
 		this->lastPartner = message->getSender();
 		return HasReturnvaluesIF::RETURN_OK;
@ -70,45 +105,40 @@ MessageQueueId_t MessageQueue::getId() const {
 }

 void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
+	defaultDestinationSet = true;
 	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;
 }

 bool MessageQueue::isDefaultDestinationSet() const {
-	return 0;
+	return defaultDestinationSet;
 }

+
+// static core function to send messages.
 ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
-		MessageQueueMessage *message, MessageQueueId_t sentFrom,
-		bool ignoreFault) {
-	message->setSender(sentFrom);
-
-	BaseType_t result = xQueueSendToBack(reinterpret_cast<void*>(sendTo),reinterpret_cast<const void*>(message->getBuffer()), 0);
-	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;
-
+        MessageQueueMessage *message, MessageQueueId_t sentFrom,
+        bool ignoreFault, CallContext callContext) {
+    message->setSender(sentFrom);
+    BaseType_t result;
+    if(callContext == CallContext::TASK) {
+        result = xQueueSendToBack(reinterpret_cast<QueueHandle_t>(sendTo),
+                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. */
+        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 handleSendResult(result, ignoreFault);
 }
-
--- a/osal/FreeRTOS/MessageQueue.h
+++ b/osal/FreeRTOS/MessageQueue.h
@ -4,50 +4,78 @@
 #include <framework/internalError/InternalErrorReporterIF.h>
 #include <framework/ipc/MessageQueueIF.h>
 #include <framework/ipc/MessageQueueMessage.h>
+#include <framework/osal/FreeRTOS/TaskManagement.h>

-#include <FreeRTOS.h>
-#include <queue.h>
+#include <freertos/FreeRTOS.h>
+#include <freertos/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
+// 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.
+ * @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.
 *
- *	@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
+ * 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.
+ *
+ * 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;
 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.
+	 * @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 );
+	MessageQueue( size_t messageDepth = 3,
+			size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE );
+
+	/** Copying message queues forbidden */
+	MessageQueue(const MessageQueue&) = delete;
+	MessageQueue& operator=(const MessageQueue&) = delete;
+
 	/**
 	 * @brief	The destructor deletes the formerly created message queue.
-	 * @details	This is accomplished by using the delete call provided by the operating system.
+	 * @details	This is accomplished by using the delete call provided
+	 * 			by the operating system.
 	 */
 	virtual ~MessageQueue();
+
+	/**
+	 * This function is used to switch the call context. This has to be called
+	 * if a message is sent or received from an ISR!
+	 * @param callContext
+	 */
+	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 +102,30 @@ 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,53 +159,50 @@ 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.
+	 * @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.
+	 * @brief	This method is a simple getter for the default destination.
 	 */
 	MessageQueueId_t getDefaultDestination() const;

 	bool isDefaultDestinationSet() const;
 protected:
 	/**
-	 * Implementation to be called from any send Call within MessageQueue and MessageQueueSenderIF
-	 * 	 \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.
+	 * @brief 	Implementation to be called from any send Call within
+	 * 			MessageQueue and MessageQueueSenderIF.
+	 * @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.
+	 * @param context Specify whether call is made from task or from an ISR.
 	 */
-	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:
+	bool defaultDestinationSet = false;
 	QueueHandle_t handle;
-	MessageQueueId_t defaultDestination;
-	MessageQueueId_t lastPartner;
+	MessageQueueId_t defaultDestination = 0;
+	MessageQueueId_t lastPartner = 0;
+	//!< Stores the current system context
+	CallContext callContext =  CallContext::TASK;
 };

 #endif /* MESSAGEQUEUE_H_ */
--- a/osal/FreeRTOS/Mutex.h
+++ b/osal/FreeRTOS/Mutex.h
@ -3,10 +3,10 @@

 #include <framework/ipc/MutexIF.h>

-
-#include <FreeRTOS.h>
-#include "semphr.h"
-
+extern "C" {
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+}


 class Mutex : public MutexIF {
--- a/osal/FreeRTOS/MutexFactory.cpp
+++ b/osal/FreeRTOS/MutexFactory.cpp
@ -1,10 +1,11 @@
 #include <framework/ipc/MutexFactory.h>

-#include "../FreeRTOS/Mutex.h"
+#include <framework/osal/FreeRTOS/Mutex.h>

-//TODO: Different variant than the lazy loading in QueueFactory. What's better and why? -> one is on heap the other on bss/data
+//TODO: Different variant than the lazy loading in QueueFactory.
+//What's better and why? -> one is on heap the other on bss/data
 //MutexFactory* MutexFactory::factoryInstance = new MutexFactory();
-MutexFactory* MutexFactory::factoryInstance = NULL;
+MutexFactory* MutexFactory::factoryInstance = nullptr;

 MutexFactory::MutexFactory() {
 }
@ -13,7 +14,7 @@ MutexFactory::~MutexFactory() {
 }

 MutexFactory* MutexFactory::instance() {
-	if (factoryInstance == NULL){
+	if (factoryInstance == nullptr){
 		factoryInstance = new MutexFactory();
 	}
 	return MutexFactory::factoryInstance;
--- a/osal/FreeRTOS/SemaphoreFactory.cpp
+++ b/osal/FreeRTOS/SemaphoreFactory.cpp
@ -0,0 +1,35 @@
+#include <framework/osal/FreeRTOS/BinarySemaphore.h>
+#include <framework/osal/FreeRTOS/CountingSemaphore.h>
+#include <framework/tasks/SemaphoreFactory.h>
+#include <framework/serviceinterface/ServiceInterfaceStream.h>
+
+SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr;
+const uint32_t SemaphoreIF::NO_TIMEOUT = 0;
+const uint32_t SemaphoreIF::MAX_TIMEOUT = portMAX_DELAY;
+
+SemaphoreFactory::SemaphoreFactory() {
+}
+
+SemaphoreFactory::~SemaphoreFactory() {
+	delete factoryInstance;
+}
+
+SemaphoreFactory* SemaphoreFactory::instance() {
+	if (factoryInstance == nullptr){
+		factoryInstance = new SemaphoreFactory();
+	}
+	return SemaphoreFactory::factoryInstance;
+}
+
+SemaphoreIF* SemaphoreFactory::createBinarySemaphore() {
+	return new BinarySemaphore();
+}
+
+SemaphoreIF* SemaphoreFactory::createCountingSemaphore(uint8_t count,
+		uint8_t initCount) {
+	return new CountingSemaphore(count, initCount);
+}
+
+void SemaphoreFactory::deleteMutex(SemaphoreIF* semaphore) {
+	delete semaphore;
+}
--- a/osal/FreeRTOS/TaskManagement.cpp
+++ b/osal/FreeRTOS/TaskManagement.cpp
@ -0,0 +1,23 @@
+#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());
+}
--- a/osal/FreeRTOS/TaskManagement.h
+++ b/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 class CallContext {
+	TASK = 0x00,//!< task_context
+	ISR = 0xFF  //!< isr_context
+};
+
+
+class TaskManagement {
+public:
+	/**
+	 * @brief	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_ */
--- a/returnvalues/FwClassIds.h
+++ b/returnvalues/FwClassIds.h
@ -24,6 +24,7 @@ enum {
 	MEMORY_HELPER, //MH
 	SERIALIZE_IF, //SE
 	FIXED_MAP, //FM
+	FIXED_MULTIMAP, //FMM
 	HAS_HEALTH_IF, //HHI
 	FIFO_CLASS, //FF
 	MESSAGE_PROXY, //MQP
@ -59,6 +60,7 @@ enum {
 	SGP4PROPAGATOR_CLASS, //SGP4 53
 	MUTEX_IF, //MUX 54
 	MESSAGE_QUEUE_IF,//MQI 55
+	SEMAPHORE_IF, //SPH 56
 	FW_CLASS_ID_COUNT //is actually count + 1 !

 };
--- a/tasks/SemaphoreFactory.h
+++ b/tasks/SemaphoreFactory.h
@ -0,0 +1,49 @@
+#ifndef FRAMEWORK_TASKS_SEMAPHOREFACTORY_H_
+#define FRAMEWORK_TASKS_SEMAPHOREFACTORY_H_
+#include <framework/tasks/SemaphoreIF.h>
+
+/**
+ * Creates Semaphore.
+ * This class is a "singleton" interface, i.e. it provides an
+ * interface, but also is the base class for a singleton.
+ */
+class SemaphoreFactory {
+public:
+	virtual ~SemaphoreFactory();
+	/**
+	 * Returns the single instance of SemaphoreFactory.
+	 * The implementation of #instance is found in its subclasses.
+	 * Thus, we choose link-time variability of the  instance.
+	 */
+	static SemaphoreFactory* instance();
+
+	/**
+	 * Create a binary semaphore.
+	 * Creator function for a binary semaphore which may only be acquired once
+	 * @param argument Can be used to pass implementation specific information.
+	 * @return Pointer to newly created semaphore class instance.
+	 */
+	SemaphoreIF* createBinarySemaphore(uint32_t arguments = 0);
+	/**
+	 * Create a counting semaphore.
+	 * Creator functons for a counting semaphore which may be acquired multiple
+	 * times.
+	 * @param count Semaphore can be taken count times.
+	 * @param initCount Initial count value.
+	 * @param argument Can be used to pass implementation specific information.
+	 * @return
+	 */
+	SemaphoreIF* createCountingSemaphore(const uint8_t maxCount,
+			uint8_t initCount, uint32_t arguments = 0);
+
+	void deleteSemaphore(SemaphoreIF* semaphore);
+
+private:
+	/**
+	 * External instantiation is not allowed.
+	 */
+	SemaphoreFactory();
+	static SemaphoreFactory* factoryInstance;
+};
+
+#endif /* FRAMEWORK_TASKS_SEMAPHOREFACTORY_H_ */
--- a/tasks/SemaphoreIF.h
+++ b/tasks/SemaphoreIF.h
@ -0,0 +1,61 @@
+#ifndef FRAMEWORK_TASKS_SEMAPHOREIF_H_
+#define FRAMEWORK_TASKS_SEMAPHOREIF_H_
+#include <framework/returnvalues/FwClassIds.h>
+#include <framework/returnvalues/HasReturnvaluesIF.h>
+#include <cstdint>
+
+/**
+ * @brief 	Generic interface for semaphores, which can be used to achieve
+ * 			task synchronization. This is a generic interface which can be
+ * 			used for both binary semaphores and counting semaphores.
+ * @details
+ * A semaphore is a synchronization primitive.
+ * See: https://en.wikipedia.org/wiki/Semaphore_(programming)
+ * A semaphore can be used to achieve task synchonization and track the
+ * availability of resources by using either the binary or the counting
+ * semaphore types.
+ *
+ * If mutual exlcusion of a resource is desired, a mutex should be used,
+ * which is a special form of a semaphore and has an own interface.
+ */
+class SemaphoreIF {
+public:
+	virtual~ SemaphoreIF() {};
+	//! Needs to be defined in implementation. No blocking time
+	static const uint32_t NO_TIMEOUT;
+	//! Needs to be defined in implementation. Blocks indefinitely.
+	static const uint32_t MAX_TIMEOUT;
+	static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF;
+	//! Semaphore timeout
+	static constexpr ReturnValue_t SEMAPHORE_TIMEOUT = MAKE_RETURN_CODE(1);
+	//! The current semaphore can not be given, because it is not owned
+	static constexpr ReturnValue_t SEMAPHORE_NOT_OWNED = MAKE_RETURN_CODE(2);
+	static constexpr ReturnValue_t SEMAPHORE_INVALID = MAKE_RETURN_CODE(3);
+
+	/**
+	 * Generic call to acquire a semaphore.
+	 * If there are no more semaphores to be taken (for a counting semaphore,
+	 * a semaphore may be taken more than once), the taks will block
+	 * for a maximum of timeoutMs while trying to acquire the semaphore.
+	 * This can be used to achieve task synchrnization.
+	 * @param timeoutMs
+	 * @return - c RETURN_OK for successfull acquisition
+	 */
+	virtual ReturnValue_t acquire(uint32_t timeoutMs) = 0;
+
+	/**
+	 * Corrensponding call to release a semaphore.
+	 * @return -@c RETURN_OK for successfull release
+	 */
+	virtual ReturnValue_t release() = 0;
+
+	/**
+	 * If the semaphore is a counting semaphore then the semaphores current
+	 * count value is returned. If the semaphore is a binary semaphore then 1
+	 * is returned if the semaphore is available, and 0 is returned if the
+	 * semaphore is not available.
+	 */
+	virtual uint8_t getSemaphoreCounter() const = 0;
+};
+
+#endif /* FRAMEWORK_TASKS_SEMAPHOREIF_H_ */