portability: cmsis: Support static CMSIS-RTOSv2 control blocks

Do not use memory slabs for the control blocks when
the application provides the memory for it. This
implements manual user-defined allocation memory
management support in CMSIS-RTOSv2 API.

Signed-off-by: Utsav Munendra <utsavm@meta.com>

include/zephyr/portability/cmsis_types.h

@@ -7,6 +7,7 @@
 #ifndef ZEPHYR_INCLUDE_CMSIS_TYPES_H_
 #define ZEPHYR_INCLUDE_CMSIS_TYPES_H_
 
+#include <stdbool.h>
 #include <zephyr/kernel.h>
 #include <zephyr/portability/cmsis_os2.h>
 
@@ -41,6 +42,7 @@ struct cmsis_rtos_timer_cb {
 	struct k_timer z_timer;
 	osTimerType_t type;
 	uint32_t status;
+	bool is_cb_dynamic_allocation;
 	char name[CMSIS_OBJ_NAME_MAX_LEN];
 	void (*callback_function)(void *argument);
 	void *arg;
@@ -54,6 +56,7 @@ struct cmsis_rtos_timer_cb {
  */
 struct cmsis_rtos_mutex_cb {
 	struct k_mutex z_mutex;
+	bool is_cb_dynamic_allocation;
 	char name[CMSIS_OBJ_NAME_MAX_LEN];
 	uint32_t state;
 };
@@ -66,6 +69,7 @@ struct cmsis_rtos_mutex_cb {
  */
 struct cmsis_rtos_semaphore_cb {
 	struct k_sem z_semaphore;
+	bool is_cb_dynamic_allocation;
 	char name[CMSIS_OBJ_NAME_MAX_LEN];
 };
 
@@ -79,6 +83,7 @@ struct cmsis_rtos_mempool_cb {
 	struct k_mem_slab z_mslab;
 	void *pool;
 	char is_dynamic_allocation;
+	bool is_cb_dynamic_allocation;
 	char name[CMSIS_OBJ_NAME_MAX_LEN];
 };
 
@@ -92,6 +97,7 @@ struct cmsis_rtos_msgq_cb {
 	struct k_msgq z_msgq;
 	void *pool;
 	char is_dynamic_allocation;
+	bool is_cb_dynamic_allocation;
 	char name[CMSIS_OBJ_NAME_MAX_LEN];
 };
 
@@ -105,6 +111,7 @@ struct cmsis_rtos_event_cb {
 	struct k_poll_signal poll_signal;
 	struct k_poll_event poll_event;
 	uint32_t signal_results;
+	bool is_cb_dynamic_allocation;
 	char name[CMSIS_OBJ_NAME_MAX_LEN];
 };
 

subsys/portability/cmsis_rtos_v2/event_flags.c

@@ -35,11 +35,15 @@ osEventFlagsId_t osEventFlagsNew(const osEventFlagsAttr_t *attr)
 		attr = &init_event_flags_attrs;
 	}
 
-	if (k_mem_slab_alloc(&cmsis_rtos_event_cb_slab, (void **)&events, K_MSEC(100)) == 0) {
-		memset(events, 0, sizeof(struct cmsis_rtos_event_cb));
-	} else {
+	if (attr->cb_mem != NULL) {
+		__ASSERT(attr->cb_size == sizeof(struct cmsis_rtos_event_cb), "Invalid cb_size\n");
+		events = (struct cmsis_rtos_event_cb *)attr->cb_mem;
+	} else if (k_mem_slab_alloc(&cmsis_rtos_event_cb_slab, (void **)&events, K_MSEC(100)) !=
+		   0) {
 		return NULL;
 	}
+	memset(events, 0, sizeof(struct cmsis_rtos_event_cb));
+	events->is_cb_dynamic_allocation = attr->cb_mem == NULL;
 
 	k_poll_signal_init(&events->poll_signal);
 	k_poll_event_init(&events->poll_event, K_POLL_TYPE_SIGNAL, K_POLL_MODE_NOTIFY_ONLY,
@@ -247,8 +251,8 @@ osStatus_t osEventFlagsDelete(osEventFlagsId_t ef_id)
 	/* The status code "osErrorParameter" (the value of the parameter
 	 * ef_id is incorrect) is not supported in Zephyr.
 	 */
-
+	if (events->is_cb_dynamic_allocation) {
 		k_mem_slab_free(&cmsis_rtos_event_cb_slab, (void *)events);
-
+	}
 	return osOK;
 }

subsys/portability/cmsis_rtos_v2/mempool.c

@@ -46,11 +46,15 @@ osMemoryPoolId_t osMemoryPoolNew(uint32_t block_count, uint32_t block_size,
 		attr = &init_mslab_attrs;
 	}
 
-	if (k_mem_slab_alloc(&cv2_mem_slab, (void **)&mslab, K_MSEC(100)) == 0) {
-		(void)memset(mslab, 0, sizeof(struct cmsis_rtos_mempool_cb));
-	} else {
+	if (attr->cb_mem != NULL) {
+		__ASSERT(attr->cb_size == sizeof(struct cmsis_rtos_mempool_cb),
+			 "Invalid cb_size\n");
+		mslab = (struct cmsis_rtos_mempool_cb *)attr->cb_mem;
+	} else if (k_mem_slab_alloc(&cv2_mem_slab, (void **)&mslab, K_MSEC(100)) != 0) {
 		return NULL;
 	}
+	(void)memset(mslab, 0, sizeof(struct cmsis_rtos_mempool_cb));
+	mslab->is_cb_dynamic_allocation = attr->cb_mem == NULL;
 
 	if (attr->mp_mem == NULL) {
 		__ASSERT((block_count * block_size) <= CONFIG_CMSIS_V2_MEM_SLAB_MAX_DYNAMIC_SIZE,
@@ -58,7 +62,9 @@ osMemoryPoolId_t osMemoryPoolNew(uint32_t block_count, uint32_t block_size,
 
 		mslab->pool = k_calloc(block_count, block_size);
 		if (mslab->pool == NULL) {
+			if (mslab->is_cb_dynamic_allocation) {
 				k_mem_slab_free(&cv2_mem_slab, (void *)mslab);
+			}
 			return NULL;
 		}
 		mslab->is_dynamic_allocation = TRUE;
@@ -68,9 +74,10 @@ osMemoryPoolId_t osMemoryPoolNew(uint32_t block_count, uint32_t block_size,
 	}
 
 	int rc = k_mem_slab_init(&mslab->z_mslab, mslab->pool, block_size, block_count);
-
 	if (rc != 0) {
+		if (mslab->is_cb_dynamic_allocation) {
 			k_mem_slab_free(&cv2_mem_slab, (void *)mslab);
+		}
 		if (attr->mp_mem == NULL) {
 			k_free(mslab->pool);
 		}
@@ -137,9 +144,9 @@ osStatus_t osMemoryPoolFree(osMemoryPoolId_t mp_id, void *block)
 	 *       osErrorResource: the memory pool specified by parameter mp_id
 	 *       is in an invalid memory pool state.
 	 */
-
+	if (mslab->is_cb_dynamic_allocation) {
 		k_mem_slab_free((struct k_mem_slab *)(&mslab->z_mslab), (void *)block);
-
+	}
 	return osOK;
 }
 
@@ -236,7 +243,8 @@ osStatus_t osMemoryPoolDelete(osMemoryPoolId_t mp_id)
 	if (mslab->is_dynamic_allocation) {
 		k_free(mslab->pool);
 	}
+	if (mslab->is_cb_dynamic_allocation) {
 		k_mem_slab_free(&cv2_mem_slab, (void *)mslab);
-
+	}
 	return osOK;
 }

subsys/portability/cmsis_rtos_v2/msgq.c

@@ -44,11 +44,14 @@ osMessageQueueId_t osMessageQueueNew(uint32_t msg_count, uint32_t msg_size,
 		attr = &init_msgq_attrs;
 	}
 
-	if (k_mem_slab_alloc(&cmsis_rtos_msgq_cb_slab, (void **)&msgq, K_MSEC(100)) == 0) {
-		(void)memset(msgq, 0, sizeof(struct cmsis_rtos_msgq_cb));
-	} else {
+	if (attr->cb_mem != NULL) {
+		__ASSERT(attr->cb_size == sizeof(struct cmsis_rtos_msgq_cb), "Invalid cb_size\n");
+		msgq = (struct cmsis_rtos_msgq_cb *)attr->cb_mem;
+	} else if (k_mem_slab_alloc(&cmsis_rtos_msgq_cb_slab, (void **)&msgq, K_MSEC(100)) != 0) {
 		return NULL;
 	}
+	(void)memset(msgq, 0, sizeof(struct cmsis_rtos_msgq_cb));
+	msgq->is_cb_dynamic_allocation = attr->cb_mem == NULL;
 
 	if (attr->mq_mem == NULL) {
 		__ASSERT((msg_count * msg_size) <= CONFIG_CMSIS_V2_MSGQ_MAX_DYNAMIC_SIZE,
@@ -57,12 +60,16 @@ osMessageQueueId_t osMessageQueueNew(uint32_t msg_count, uint32_t msg_size,
 #if (K_HEAP_MEM_POOL_SIZE > 0)
 		msgq->pool = k_calloc(msg_count, msg_size);
 		if (msgq->pool == NULL) {
+			if (msgq->is_cb_dynamic_allocation) {
 				k_mem_slab_free(&cmsis_rtos_msgq_cb_slab, (void *)msgq);
+			}
 			return NULL;
 		}
 		msgq->is_dynamic_allocation = TRUE;
 #else
+		if (msgq->is_cb_dynamic_allocation) {
 			k_mem_slab_free(&cmsis_rtos_msgq_cb_slab, (void *)msgq);
+		}
 		return NULL;
 #endif
 	} else {
@@ -275,7 +282,8 @@ osStatus_t osMessageQueueDelete(osMessageQueueId_t msgq_id)
 	if (msgq->is_dynamic_allocation) {
 		k_free(msgq->pool);
 	}
+	if (msgq->is_cb_dynamic_allocation) {
 		k_mem_slab_free(&cmsis_rtos_msgq_cb_slab, (void *)msgq);
-
+	}
 	return osOK;
 }

subsys/portability/cmsis_rtos_v2/mutex.c

@@ -39,11 +39,14 @@ osMutexId_t osMutexNew(const osMutexAttr_t *attr)
 
 	__ASSERT(!(attr->attr_bits & osMutexRobust), "Zephyr does not support osMutexRobust.\n");
 
-	if (k_mem_slab_alloc(&cmsis_rtos_mutex_cb_slab, (void **)&mutex, K_MSEC(100)) == 0) {
-		memset(mutex, 0, sizeof(struct cmsis_rtos_mutex_cb));
-	} else {
+	if (attr->cb_mem != NULL) {
+		__ASSERT(attr->cb_size == sizeof(struct cmsis_rtos_mutex_cb), "Invalid cb_size\n");
+		mutex = (struct cmsis_rtos_mutex_cb *)attr->cb_mem;
+	} else if (k_mem_slab_alloc(&cmsis_rtos_mutex_cb_slab, (void **)&mutex, K_MSEC(100)) != 0) {
 		return NULL;
 	}
+	memset(mutex, 0, sizeof(struct cmsis_rtos_mutex_cb));
+	mutex->is_cb_dynamic_allocation = attr->cb_mem == NULL;
 
 	k_mutex_init(&mutex->z_mutex);
 	mutex->state = attr->attr_bits;
@@ -130,8 +133,9 @@ osStatus_t osMutexDelete(osMutexId_t mutex_id)
 	/* The status code "osErrorResource" (mutex specified by parameter
 	 * mutex_id is in an invalid mutex state) is not supported in Zephyr.
 	 */
-
+	if (mutex->is_cb_dynamic_allocation) {
 		k_mem_slab_free(&cmsis_rtos_mutex_cb_slab, (void *)mutex);
+	}
 
 	return osOK;
 }

subsys/portability/cmsis_rtos_v2/semaphore.c

@@ -34,12 +34,16 @@ osSemaphoreId_t osSemaphoreNew(uint32_t max_count, uint32_t initial_count,
 		attr = &init_sema_attrs;
 	}
 
-	if (k_mem_slab_alloc(&cmsis_rtos_semaphore_cb_slab, (void **)&semaphore, K_MSEC(100)) ==
-	    0) {
-		(void)memset(semaphore, 0, sizeof(struct cmsis_rtos_semaphore_cb));
-	} else {
+	if (attr->cb_mem != NULL) {
+		__ASSERT(attr->cb_size == sizeof(struct cmsis_rtos_semaphore_cb),
+			 "Invalid cb_size\n");
+		semaphore = (struct cmsis_rtos_semaphore_cb *)attr->cb_mem;
+	} else if (k_mem_slab_alloc(&cmsis_rtos_semaphore_cb_slab, (void **)&semaphore,
+				    K_MSEC(100)) != 0) {
 		return NULL;
 	}
+	(void)memset(semaphore, 0, sizeof(struct cmsis_rtos_semaphore_cb));
+	semaphore->is_cb_dynamic_allocation = attr->cb_mem == NULL;
 
 	k_sem_init(&semaphore->z_semaphore, initial_count, max_count);
 
@@ -137,8 +141,9 @@ osStatus_t osSemaphoreDelete(osSemaphoreId_t semaphore_id)
 	 * parameter semaphore_id is in an invalid semaphore state) is not
 	 * supported in Zephyr.
 	 */
-
+	if (semaphore->is_cb_dynamic_allocation) {
 		k_mem_slab_free(&cmsis_rtos_semaphore_cb_slab, (void *)semaphore);
+	}
 
 	return osOK;
 }

subsys/portability/cmsis_rtos_v2/thread.c

@@ -112,6 +112,7 @@ osThreadId_t osThreadNew(osThreadFunc_t threadfunc, void *arg, const osThreadAtt
 	void *stack;
 	size_t stack_size;
 	uint32_t this_thread_num;
+	uint32_t this_dynamic_cb;
 
 	if (k_is_in_isr()) {
 		return NULL;
@@ -162,8 +163,13 @@ osThreadId_t osThreadNew(osThreadFunc_t threadfunc, void *arg, const osThreadAtt
 
 	this_thread_num = atomic_inc(&thread_num);
 
-	uint32_t this_dynamic_cb = atomic_inc(&num_dynamic_cb);
+	if (attr->cb_mem == NULL) {
+		this_dynamic_cb = atomic_inc(&num_dynamic_cb);
 		tid = &cmsis_rtos_thread_cb_pool[this_dynamic_cb];
+	} else {
+		tid = (struct cmsis_rtos_thread_cb *)attr->cb_mem;
+	}
+
 	tid->attr_bits = attr->attr_bits;
 
 #if CONFIG_CMSIS_V2_THREAD_DYNAMIC_MAX_COUNT != 0

subsys/portability/cmsis_rtos_v2/timer.c

@@ -51,11 +51,14 @@ osTimerId_t osTimerNew(osTimerFunc_t func, osTimerType_t type, void *argument,
 		attr = &init_timer_attrs;
 	}
 
-	if (k_mem_slab_alloc(&cmsis_rtos_timer_cb_slab, (void **)&timer, K_MSEC(100)) == 0) {
-		(void)memset(timer, 0, sizeof(struct cmsis_rtos_timer_cb));
-	} else {
+	if (attr->cb_mem != NULL) {
+		__ASSERT(attr->cb_size == sizeof(struct cmsis_rtos_timer_cb), "Invalid cb_size\n");
+		timer = (struct cmsis_rtos_timer_cb *)attr->cb_mem;
+	} else if (k_mem_slab_alloc(&cmsis_rtos_timer_cb_slab, (void **)&timer, K_MSEC(100)) != 0) {
 		return NULL;
 	}
+	(void)memset(timer, 0, sizeof(struct cmsis_rtos_timer_cb));
+	timer->is_cb_dynamic_allocation = attr->cb_mem == NULL;
 
 	timer->callback_function = func;
 	timer->arg = argument;
@@ -142,7 +145,9 @@ osStatus_t osTimerDelete(osTimerId_t timer_id)
 		timer->status = NOT_ACTIVE;
 	}
 
+	if (timer->is_cb_dynamic_allocation) {
 		k_mem_slab_free(&cmsis_rtos_timer_cb_slab, (void *)timer);
+	}
 	return osOK;
 }