From 109fdad8b36ca1fe99f0ebad89fb28b04fa7d197 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Mon, 22 Jun 2020 15:34:35 +0200
Subject: [PATCH] size check for message queue

---
 osal/FreeRTOS/MessageQueue.cpp | 11 +++++++++--
 osal/FreeRTOS/MessageQueue.h   |  1 +
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/osal/FreeRTOS/MessageQueue.cpp b/osal/FreeRTOS/MessageQueue.cpp
index d44b0bc0f..cf6130f83 100644
--- a/osal/FreeRTOS/MessageQueue.cpp
+++ b/osal/FreeRTOS/MessageQueue.cpp
@@ -7,7 +7,8 @@
 // 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) {
+MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize):
+		maxMessageSize(maxMessageSize) {
 	handle = xQueueCreate(messageDepth, maxMessageSize);
 	if (handle == NULL) {
 		sif::error << "MessageQueue: Creation failed" << std::endl;
@@ -120,10 +121,16 @@ bool MessageQueue::isDefaultDestinationSet() const {
 
 // static core function to send messages.
 ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
-        MessageQueueMessageIF *message, MessageQueueId_t sentFrom,
+        MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
         bool ignoreFault, CallContext callContext) {
     message->setSender(sentFrom);
     BaseType_t result;
+    if(message->getMaximumMessageSize() > maxMessageSize) {
+    	sif::error << "MessageQueue::sendMessageFromMessageQueue: Message size"
+    			"too large for queue!" << std::endl;
+    	return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
     if(callContext == CallContext::TASK) {
         result = xQueueSendToBack(reinterpret_cast<QueueHandle_t>(sendTo),
                 static_cast<const void*>(message->getBuffer()), 0);
diff --git a/osal/FreeRTOS/MessageQueue.h b/osal/FreeRTOS/MessageQueue.h
index c13a8a200..b7e52bb39 100644
--- a/osal/FreeRTOS/MessageQueue.h
+++ b/osal/FreeRTOS/MessageQueue.h
@@ -204,6 +204,7 @@ private:
 	QueueHandle_t handle;
 	MessageQueueId_t defaultDestination = 0;
 	MessageQueueId_t lastPartner = 0;
+	const size_t maxMessageSize;
 	//!< Stores the current system context
 	CallContext callContext =  CallContext::TASK;
 };