From aff8c511281ec781cf212fd5ae234f147b7e73ee Mon Sep 17 00:00:00 2001
From: Rajavardhan Gundi <rajavardhan.gundi@intel.com>
Date: Thu, 12 Jul 2018 15:15:57 +0530
Subject: [PATCH] lib/cmsis_rtos_v1: Implement support for mempool APIs

These APIs allow creating, allocating and freeing
of mempools.

Note: "Mempool" in CMSIS actually means memslabs in Zephyr.

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

diff --git a/include/cmsis_rtos_v1/cmsis_os.h b/include/cmsis_rtos_v1/cmsis_os.h
index d275490c06f..1e0cd44ce08 100644
--- a/include/cmsis_rtos_v1/cmsis_os.h
+++ b/include/cmsis_rtos_v1/cmsis_os.h
@@ -535,8 +535,9 @@ osStatus osSemaphoreDelete (osSemaphoreId semaphore_id);
 extern const osPoolDef_t os_pool_def_##name
 #else                            // define the object
 #define osPoolDef(name, no, type)   \
+K_MEM_SLAB_DEFINE(os_mem_##name, sizeof(type), no, 4); \
 const osPoolDef_t os_pool_def_##name = \
-{ (no), sizeof(type), NULL }
+{ (no), sizeof(type), &os_mem_##name }
 #endif
  
 /// \brief Access a Memory Pool definition.
diff --git a/lib/cmsis_rtos_v1/CMakeLists.txt b/lib/cmsis_rtos_v1/CMakeLists.txt
index fc54fedac9d..9be9da1df23 100644
--- a/lib/cmsis_rtos_v1/CMakeLists.txt
+++ b/lib/cmsis_rtos_v1/CMakeLists.txt
@@ -12,6 +12,7 @@ zephyr_library_sources_ifdef(
   cmsis_timer.c
   cmsis_mutex.c
   cmsis_semaphore.c
+  cmsis_mempool.c
 )
 
 zephyr_library_link_libraries(CMSIS)
diff --git a/lib/cmsis_rtos_v1/cmsis_mempool.c b/lib/cmsis_rtos_v1/cmsis_mempool.c
new file mode 100644
index 00000000000..b155705645a
--- /dev/null
+++ b/lib/cmsis_rtos_v1/cmsis_mempool.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2018 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <kernel_structs.h>
+#include <cmsis_os.h>
+
+#define TIME_OUT	100
+
+/**
+ * @brief Create and Initialize a memory pool.
+ */
+osPoolId osPoolCreate(const osPoolDef_t *pool_def)
+{
+	if (_is_in_isr()) {
+		return NULL;
+	}
+
+	return (osPoolId)pool_def;
+}
+
+/**
+ * @brief Allocate a memory block from a memory pool.
+ */
+void *osPoolAlloc(osPoolId pool_id)
+{
+	osPoolDef_t *osPool = (osPoolDef_t *)pool_id;
+	void *ptr;
+
+	if (k_mem_slab_alloc((struct k_mem_slab *)(osPool->pool),
+				&ptr, TIME_OUT) == 0) {
+		return ptr;
+	} else {
+		return NULL;
+	}
+}
+
+/**
+ * @brief Allocate a memory block from a memory pool and set it to zero.
+ */
+void *osPoolCAlloc(osPoolId pool_id)
+{
+	osPoolDef_t *osPool = (osPoolDef_t *)pool_id;
+	void *ptr;
+
+	if (k_mem_slab_alloc((struct k_mem_slab *)(osPool->pool),
+				&ptr, TIME_OUT) == 0) {
+		memset(ptr, 0, osPool->item_sz);
+		return ptr;
+	} else {
+		return NULL;
+	}
+}
+
+/**
+ * @brief Return an allocated memory block back to a specific memory pool.
+ */
+osStatus osPoolFree(osPoolId pool_id, void *block)
+{
+	osPoolDef_t *osPool = (osPoolDef_t *)pool_id;
+
+	/* Note: Below 2 error codes are not supported.
+	 *       osErrorValue: block does not belong to the memory pool.
+	 *       osErrorParameter: a parameter is invalid or outside of a
+	 *                         permitted range.
+	 */
+
+	k_mem_slab_free((struct k_mem_slab *)(osPool->pool), (void *)&block);
+
+	return osOK;
+}