From 3bb243c6bb73f26c35580c491e548a2e71668527 Mon Sep 17 00:00:00 2001
From: Rajavardhan Gundi <rajavardhan.gundi@intel.com>
Date: Fri, 20 Jul 2018 15:38:24 +0530
Subject: [PATCH] lib/cmsis_rtos_v1: Implement support for Message Queue APIs

These APIs provide message queue functionality like create,
put and get.

Signed-off-by: Rajavardhan Gundi <rajavardhan.gundi@intel.com>
---
 include/cmsis_rtos_v1/cmsis_os.h |  5 +-
 lib/cmsis_rtos_v1/CMakeLists.txt |  1 +
 lib/cmsis_rtos_v1/cmsis_msgq.c   | 93 ++++++++++++++++++++++++++++++++
 3 files changed, 98 insertions(+), 1 deletion(-)
 create mode 100644 lib/cmsis_rtos_v1/cmsis_msgq.c

diff --git a/include/cmsis_rtos_v1/cmsis_os.h b/include/cmsis_rtos_v1/cmsis_os.h
index f0cbdc35a3e..842ee96f67c 100644
--- a/include/cmsis_rtos_v1/cmsis_os.h
+++ b/include/cmsis_rtos_v1/cmsis_os.h
@@ -203,6 +203,7 @@ typedef struct os_messageQ_def  {
   uint32_t                queue_sz;    ///< number of elements in the queue
   uint32_t                 item_sz;    ///< size of an item
   void                       *pool;    ///< memory array for messages
+  struct k_msgq              *msgq;
 } osMessageQDef_t;
  
 /// Definition structure for mail queue.
@@ -591,8 +592,10 @@ osStatus osPoolFree (osPoolId pool_id, void *block);
 extern const osMessageQDef_t os_messageQ_def_##name
 #else                            // define the object
 #define osMessageQDef(name, queue_sz, type)   \
+struct k_msgq msgq_##name; \
+static char __aligned(4) buf_##name[queue_sz * sizeof(type)]; \
 const osMessageQDef_t os_messageQ_def_##name = \
-{ (queue_sz), sizeof (type)  }
+{ (queue_sz), sizeof (type), (buf_##name), (&msgq_##name) }
 #endif
  
 /// \brief Access a Message Queue Definition.
diff --git a/lib/cmsis_rtos_v1/CMakeLists.txt b/lib/cmsis_rtos_v1/CMakeLists.txt
index 8c13b4a1b9a..c4fd2fc6f96 100644
--- a/lib/cmsis_rtos_v1/CMakeLists.txt
+++ b/lib/cmsis_rtos_v1/CMakeLists.txt
@@ -14,6 +14,7 @@ zephyr_library_sources_ifdef(
   cmsis_semaphore.c
   cmsis_mempool.c
   cmsis_mailq.c
+  cmsis_msgq.c
 )
 
 zephyr_library_link_libraries(CMSIS)
diff --git a/lib/cmsis_rtos_v1/cmsis_msgq.c b/lib/cmsis_rtos_v1/cmsis_msgq.c
new file mode 100644
index 00000000000..6c2f9a83489
--- /dev/null
+++ b/lib/cmsis_rtos_v1/cmsis_msgq.c
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2018 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <kernel_structs.h>
+#include <cmsis_os.h>
+
+/**
+ * @brief Create and Initialize Message queue.
+ */
+osMessageQId osMessageCreate(const osMessageQDef_t *queue_def,
+				osThreadId thread_id)
+{
+	if (queue_def == NULL) {
+		return NULL;
+	}
+
+	if (_is_in_isr()) {
+		return NULL;
+	}
+
+	k_msgq_init(queue_def->msgq, queue_def->pool,
+			queue_def->item_sz, queue_def->queue_sz);
+	return (osMessageQId)(queue_def);
+}
+
+/**
+ * @brief Put a message to a Queue.
+ */
+osStatus osMessagePut(osMessageQId queue_id, uint32_t info, uint32_t millisec)
+{
+	osMessageQDef_t *queue_def = (osMessageQDef_t *)queue_id;
+	int retval;
+
+	if (queue_def == NULL) {
+		return osErrorParameter;
+	}
+
+	if (millisec == 0) {
+		retval = k_msgq_put(queue_def->msgq, (void *)&info, K_NO_WAIT);
+	} else if (millisec == osWaitForever) {
+		retval = k_msgq_put(queue_def->msgq, (void *)&info, K_FOREVER);
+	} else {
+		retval = k_msgq_put(queue_def->msgq, (void *)&info, millisec);
+	}
+
+	if (retval == 0) {
+		return osOK;
+	} else {
+		return osErrorResource;
+	}
+}
+
+/**
+ * @brief Get a message or Wait for a Message from a Queue.
+ */
+osEvent osMessageGet(osMessageQId queue_id, uint32_t millisec)
+{
+	osMessageQDef_t *queue_def = (osMessageQDef_t *)queue_id;
+	u32_t info;
+	osEvent evt;
+	int retval;
+
+	if (queue_def == NULL) {
+		evt.status = osErrorParameter;
+		return evt;
+	}
+
+	if (millisec == 0) {
+		retval = k_msgq_get(queue_def->msgq, &info, K_NO_WAIT);
+	} else if (millisec == osWaitForever) {
+		retval = k_msgq_get(queue_def->msgq, &info, K_FOREVER);
+	} else {
+		retval = k_msgq_get(queue_def->msgq, &info, millisec);
+	}
+
+	if (retval == 0) {
+		evt.status = osEventMessage;
+	} else if (retval == -EAGAIN) {
+		evt.status = osEventTimeout;
+	} else if (retval == -ENOMSG) {
+		evt.status = osOK;
+	} else {
+		evt.status = osErrorValue;
+	}
+
+	evt.value.v = info;
+	evt.def.message_id = queue_id;
+
+	return evt;
+}