TeamGyro
OS2
Dependents: GYRO_MPU6050 Bluetooth_Powered_Multimeter_Using_STM32F429_and_RTOS fyp
Diff: rtx/TARGET_CORTEX_M/rt_CMSIS.c
- Revision:
- 0:a7c449cd2d5a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rtx/TARGET_CORTEX_M/rt_CMSIS.c Mon Feb 03 13:41:14 2020 +0000
@@ -0,0 +1,2344 @@
+/*----------------------------------------------------------------------------
+ * CMSIS-RTOS - RTX
+ *----------------------------------------------------------------------------
+ * Name: rt_CMSIS.c
+ * Purpose: CMSIS RTOS API
+ * Rev.: V4.80
+ *----------------------------------------------------------------------------
+ *
+ * Copyright (c) 1999-2009 KEIL, 2009-2015 ARM Germany GmbH
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * - Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * - Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without
+ * specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *---------------------------------------------------------------------------*/
+
+#define __CMSIS_GENERIC
+
+#if defined (__CORTEX_M4) || defined (__CORTEX_M4F)
+ #include "core_cm4.h"
+#elif defined (__CORTEX_M7) || defined (__CORTEX_M7F)
+ #include "core_cm7.h"
+#elif defined (__CORTEX_M3)
+ #include "core_cm3.h"
+#elif defined (__CORTEX_M0)
+ #include "core_cm0.h"
+#elif defined (__CORTEX_M0PLUS)
+ #include "core_cm0plus.h"
+#else
+ #error "Missing __CORTEX_Mx definition"
+#endif
+
+// This affects cmsis_os only, as it's not used anywhere else. This was left by kernel team
+// to suppress the warning in rt_tid2ptcb about incompatible pointer assignment.
+#define os_thread_cb OS_TCB
+
+#include "rt_TypeDef.h"
+#include "RTX_Config.h"
+#include "rt_System.h"
+#include "rt_Task.h"
+#include "rt_Event.h"
+#include "rt_List.h"
+#include "rt_Time.h"
+#include "rt_Mutex.h"
+#include "rt_Semaphore.h"
+#include "rt_Mailbox.h"
+#include "rt_MemBox.h"
+#include "rt_Memory.h"
+#include "rt_HAL_CM.h"
+#include "rt_OsEventObserver.h"
+
+#include "cmsis_os.h"
+
+#if (osFeature_Signals != 16)
+#error Invalid "osFeature_Signals" value!
+#endif
+#if (osFeature_Semaphore > 65535)
+#error Invalid "osFeature_Semaphore" value!
+#endif
+#if (osFeature_Wait != 0)
+#error osWait not supported!
+#endif
+
+
+// ==== Enumeration, structures, defines ====
+
+// Service Calls defines
+
+#if defined (__CC_ARM) /* ARM Compiler */
+
+#define __NO_RETURN __declspec(noreturn)
+
+#define osEvent_type osEvent
+#define osEvent_ret_status ret
+#define osEvent_ret_value ret
+#define osEvent_ret_msg ret
+#define osEvent_ret_mail ret
+
+#define osCallback_type osCallback
+#define osCallback_ret ret
+
+#define SVC_0_1(f,t,...) \
+__svc_indirect(0) t _##f (t(*)()); \
+ t f (void); \
+__attribute__((always_inline)) \
+static __inline t __##f (void) { \
+ return _##f(f); \
+}
+
+#define SVC_1_0(f,t,t1,...) \
+__svc_indirect(0) t _##f (t(*)(t1),t1); \
+ t f (t1 a1); \
+__attribute__((always_inline)) \
+static __inline t __##f (t1 a1) { \
+ _##f(f,a1); \
+}
+
+#define SVC_1_1(f,t,t1,...) \
+__svc_indirect(0) t _##f (t(*)(t1),t1); \
+ t f (t1 a1); \
+__attribute__((always_inline)) \
+static __inline t __##f (t1 a1) { \
+ return _##f(f,a1); \
+}
+
+#define SVC_2_1(f,t,t1,t2,...) \
+__svc_indirect(0) t _##f (t(*)(t1,t2),t1,t2); \
+ t f (t1 a1, t2 a2); \
+__attribute__((always_inline)) \
+static __inline t __##f (t1 a1, t2 a2) { \
+ return _##f(f,a1,a2); \
+}
+
+#define SVC_3_1(f,t,t1,t2,t3,...) \
+__svc_indirect(0) t _##f (t(*)(t1,t2,t3),t1,t2,t3); \
+ t f (t1 a1, t2 a2, t3 a3); \
+__attribute__((always_inline)) \
+static __inline t __##f (t1 a1, t2 a2, t3 a3) { \
+ return _##f(f,a1,a2,a3); \
+}
+
+#define SVC_4_1(f,t,t1,t2,t3,t4,...) \
+__svc_indirect(0) t _##f (t(*)(t1,t2,t3,t4),t1,t2,t3,t4); \
+ t f (t1 a1, t2 a2, t3 a3, t4 a4); \
+__attribute__((always_inline)) \
+static __inline t __##f (t1 a1, t2 a2, t3 a3, t4 a4) { \
+ return _##f(f,a1,a2,a3,a4); \
+}
+
+#define SVC_1_2 SVC_1_1
+#define SVC_1_3 SVC_1_1
+#define SVC_2_3 SVC_2_1
+
+#elif defined (__GNUC__) /* GNU Compiler */
+
+#define __NO_RETURN __attribute__((noreturn))
+
+typedef uint32_t __attribute__((vector_size(8))) ret64;
+typedef uint32_t __attribute__((vector_size(16))) ret128;
+
+#define RET_pointer __r0
+#define RET_int32_t __r0
+#define RET_uint32_t __r0
+#define RET_osStatus __r0
+#define RET_osPriority __r0
+#define RET_osEvent {(osStatus)__r0, {(uint32_t)__r1}, {(void *)__r2}}
+#define RET_osCallback {(void *)__r0, (void *)__r1}
+
+#define osEvent_type __attribute__((pcs("aapcs"))) ret128
+#define osEvent_ret_status (ret128){ret.status}
+#define osEvent_ret_value (ret128){ret.status, ret.value.v}
+#define osEvent_ret_msg (ret128){ret.status, ret.value.v, (uint32_t)ret.def.message_id}
+#define osEvent_ret_mail (ret128){ret.status, ret.value.v, (uint32_t)ret.def.mail_id}
+
+#define osCallback_type __attribute__((pcs("aapcs"))) ret64
+#define osCallback_ret (ret64) {(uint32_t)ret.fp, (uint32_t)ret.arg}
+
+#define SVC_ArgN(n) \
+ register int __r##n __asm("r"#n);
+
+#define SVC_ArgR(n,t,a) \
+ register t __r##n __asm("r"#n) = a;
+
+#define SVC_Arg0() \
+ SVC_ArgN(0) \
+ SVC_ArgN(1) \
+ SVC_ArgN(2) \
+ SVC_ArgN(3)
+
+#define SVC_Arg1(t1) \
+ SVC_ArgR(0,t1,a1) \
+ SVC_ArgN(1) \
+ SVC_ArgN(2) \
+ SVC_ArgN(3)
+
+#define SVC_Arg2(t1,t2) \
+ SVC_ArgR(0,t1,a1) \
+ SVC_ArgR(1,t2,a2) \
+ SVC_ArgN(2) \
+ SVC_ArgN(3)
+
+#define SVC_Arg3(t1,t2,t3) \
+ SVC_ArgR(0,t1,a1) \
+ SVC_ArgR(1,t2,a2) \
+ SVC_ArgR(2,t3,a3) \
+ SVC_ArgN(3)
+
+#define SVC_Arg4(t1,t2,t3,t4) \
+ SVC_ArgR(0,t1,a1) \
+ SVC_ArgR(1,t2,a2) \
+ SVC_ArgR(2,t3,a3) \
+ SVC_ArgR(3,t4,a4)
+
+#if (defined (__CORTEX_M0)) || defined (__CORTEX_M0PLUS)
+#define SVC_Call(f) \
+ __asm volatile \
+ ( \
+ "ldr r7,="#f"\n\t" \
+ "mov r12,r7\n\t" \
+ "svc 0" \
+ : "=r" (__r0), "=r" (__r1), "=r" (__r2), "=r" (__r3) \
+ : "r" (__r0), "r" (__r1), "r" (__r2), "r" (__r3) \
+ : "r7", "r12", "lr", "cc" \
+ );
+#else
+#define SVC_Call(f) \
+ __asm volatile \
+ ( \
+ "ldr r12,="#f"\n\t" \
+ "svc 0" \
+ : "=r" (__r0), "=r" (__r1), "=r" (__r2), "=r" (__r3) \
+ : "r" (__r0), "r" (__r1), "r" (__r2), "r" (__r3) \
+ : "r12", "lr", "cc" \
+ );
+#endif
+
+#define SVC_0_1(f,t,rv) \
+__attribute__((always_inline)) \
+static inline t __##f (void) { \
+ SVC_Arg0(); \
+ SVC_Call(f); \
+ return (t) rv; \
+}
+
+#define SVC_1_0(f,t,t1) \
+__attribute__((always_inline)) \
+static inline t __##f (t1 a1) { \
+ SVC_Arg1(t1); \
+ SVC_Call(f); \
+}
+
+#define SVC_1_1(f,t,t1,rv) \
+__attribute__((always_inline)) \
+static inline t __##f (t1 a1) { \
+ SVC_Arg1(t1); \
+ SVC_Call(f); \
+ return (t) rv; \
+}
+
+#define SVC_2_1(f,t,t1,t2,rv) \
+__attribute__((always_inline)) \
+static inline t __##f (t1 a1, t2 a2) { \
+ SVC_Arg2(t1,t2); \
+ SVC_Call(f); \
+ return (t) rv; \
+}
+
+#define SVC_3_1(f,t,t1,t2,t3,rv) \
+__attribute__((always_inline)) \
+static inline t __##f (t1 a1, t2 a2, t3 a3) { \
+ SVC_Arg3(t1,t2,t3); \
+ SVC_Call(f); \
+ return (t) rv; \
+}
+
+#define SVC_4_1(f,t,t1,t2,t3,t4,rv) \
+__attribute__((always_inline)) \
+static inline t __##f (t1 a1, t2 a2, t3 a3, t4 a4) { \
+ SVC_Arg4(t1,t2,t3,t4); \
+ SVC_Call(f); \
+ return (t) rv; \
+}
+
+#define SVC_1_2 SVC_1_1
+#define SVC_1_3 SVC_1_1
+#define SVC_2_3 SVC_2_1
+
+#elif defined (__ICCARM__) /* IAR Compiler */
+
+#define __NO_RETURN __noreturn
+
+#define osEvent_type osEvent
+#define osEvent_ret_status ret
+#define osEvent_ret_value ret
+#define osEvent_ret_msg ret
+#define osEvent_ret_mail ret
+
+#define osCallback_type osCallback
+#define osCallback_ret ret
+
+#define RET_osEvent osEvent
+#define RET_osCallback osCallback
+
+#define SVC_Setup(f) \
+ __asm( \
+ "mov r12,%0\n" \
+ :: "r"(&f): "r0", "r1", "r2", "r3", "r12" \
+ );
+
+#define SVC_Ret3() \
+ __asm( \
+ "ldr r0,[sp,#0]\n" \
+ "ldr r1,[sp,#4]\n" \
+ "ldr r2,[sp,#8]\n" \
+ );
+
+#define SVC_0_1(f,t,...) \
+t f (void); \
+_Pragma("swi_number=0") __swi t _##f (void); \
+static inline t __##f (void) { \
+ SVC_Setup(f); \
+ return _##f(); \
+}
+
+#define SVC_1_0(f,t,t1,...) \
+t f (t1 a1); \
+_Pragma("swi_number=0") __swi t _##f (t1 a1); \
+static inline t __##f (t1 a1) { \
+ SVC_Setup(f); \
+ _##f(a1); \
+}
+
+#define SVC_1_1(f,t,t1,...) \
+t f (t1 a1); \
+_Pragma("swi_number=0") __swi t _##f (t1 a1); \
+static inline t __##f (t1 a1) { \
+ SVC_Setup(f); \
+ return _##f(a1); \
+}
+
+#define SVC_2_1(f,t,t1,t2,...) \
+t f (t1 a1, t2 a2); \
+_Pragma("swi_number=0") __swi t _##f (t1 a1, t2 a2); \
+static inline t __##f (t1 a1, t2 a2) { \
+ SVC_Setup(f); \
+ return _##f(a1,a2); \
+}
+
+#define SVC_3_1(f,t,t1,t2,t3,...) \
+t f (t1 a1, t2 a2, t3 a3); \
+_Pragma("swi_number=0") __swi t _##f (t1 a1, t2 a2, t3 a3); \
+static inline t __##f (t1 a1, t2 a2, t3 a3) { \
+ SVC_Setup(f); \
+ return _##f(a1,a2,a3); \
+}
+
+#define SVC_4_1(f,t,t1,t2,t3,t4,...) \
+t f (t1 a1, t2 a2, t3 a3, t4 a4); \
+_Pragma("swi_number=0") __swi t _##f (t1 a1, t2 a2, t3 a3, t4 a4); \
+static inline t __##f (t1 a1, t2 a2, t3 a3, t4 a4) { \
+ SVC_Setup(f); \
+ return _##f(a1,a2,a3,a4); \
+}
+
+#define SVC_1_2 SVC_1_1
+#define SVC_1_3 SVC_1_1
+#define SVC_2_3 SVC_2_1
+
+#endif
+
+
+// Callback structure
+typedef struct {
+ void *fp; // Function pointer
+ void *arg; // Function argument
+} osCallback;
+
+
+// OS Section definitions
+#ifdef OS_SECTIONS_LINK_INFO
+extern const uint32_t os_section_id$$Base;
+extern const uint32_t os_section_id$$Limit;
+#endif
+
+#ifndef __MBED_CMSIS_RTOS_CM
+// OS Stack Memory for Threads definitions
+extern uint64_t os_stack_mem[];
+extern const uint32_t os_stack_sz;
+#endif
+
+// OS Timers external resources
+extern const osThreadDef_t os_thread_def_osTimerThread;
+extern osThreadId osThreadId_osTimerThread;
+extern const osMessageQDef_t os_messageQ_def_osTimerMessageQ;
+extern osMessageQId osMessageQId_osTimerMessageQ;
+
+// Thread creation and destruction
+osMutexDef(osThreadMutex);
+osMutexId osMutexId_osThreadMutex;
+void sysThreadTerminate(osThreadId id);
+
+// ==== Helper Functions ====
+
+/// Convert timeout in millisec to system ticks
+static uint16_t rt_ms2tick (uint32_t millisec) {
+ uint32_t tick;
+
+ if (millisec == 0U) { return 0x0U; } // No timeout
+ if (millisec == osWaitForever) { return 0xFFFFU; } // Indefinite timeout
+ if (millisec > 4000000U) { return 0xFFFEU; } // Max ticks supported
+
+ tick = ((1000U * millisec) + os_clockrate - 1U) / os_clockrate;
+ if (tick > 0xFFFEU) { return 0xFFFEU; }
+
+ return (uint16_t)tick;
+}
+
+/// Convert Thread ID to TCB pointer
+P_TCB rt_tid2ptcb (osThreadId thread_id) {
+ P_TCB ptcb;
+
+ if (thread_id == NULL) { return NULL; }
+
+ if ((uint32_t)thread_id & 3U) { return NULL; }
+
+#ifdef OS_SECTIONS_LINK_INFO
+ if ((os_section_id$$Base != 0U) && (os_section_id$$Limit != 0U)) {
+ if (thread_id < (osThreadId)os_section_id$$Base) { return NULL; }
+ if (thread_id >= (osThreadId)os_section_id$$Limit) { return NULL; }
+ }
+#endif
+
+ ptcb = thread_id;
+
+ if (ptcb->cb_type != TCB) { return NULL; }
+
+ return ptcb;
+}
+
+/// Convert ID pointer to Object pointer
+static void *rt_id2obj (void *id) {
+
+ if ((uint32_t)id & 3U) { return NULL; }
+
+#ifdef OS_SECTIONS_LINK_INFO
+ if ((os_section_id$$Base != 0U) && (os_section_id$$Limit != 0U)) {
+ if (id < (void *)os_section_id$$Base) { return NULL; }
+ if (id >= (void *)os_section_id$$Limit) { return NULL; }
+ }
+#endif
+
+ return id;
+}
+
+
+// ==== Kernel Control ====
+
+uint8_t os_initialized; // Kernel Initialized flag
+uint8_t os_running; // Kernel Running flag
+
+// Kernel Control Service Calls declarations
+SVC_0_1(svcKernelInitialize, osStatus, RET_osStatus)
+SVC_0_1(svcKernelStart, osStatus, RET_osStatus)
+SVC_0_1(svcKernelRunning, int32_t, RET_int32_t)
+SVC_0_1(svcKernelSysTick, uint32_t, RET_uint32_t)
+
+static void sysThreadError (osStatus status);
+osThreadId svcThreadCreate (const osThreadDef_t *thread_def, void *argument, void *context);
+osMessageQId svcMessageCreate (const osMessageQDef_t *queue_def, osThreadId thread_id);
+
+// Kernel Control Service Calls
+
+/// Initialize the RTOS Kernel for creating objects
+osStatus svcKernelInitialize (void) {
+#ifdef __MBED_CMSIS_RTOS_CM
+ if (!os_initialized) {
+ rt_sys_init(); // RTX System Initialization
+ }
+#else
+ uint32_t ret;
+
+ if (os_initialized == 0U) {
+
+ // Init Thread Stack Memory (must be 8-byte aligned)
+ if (((uint32_t)os_stack_mem & 7U) != 0U) { return osErrorNoMemory; }
+ ret = rt_init_mem(os_stack_mem, os_stack_sz);
+ if (ret != 0U) { return osErrorNoMemory; }
+
+ rt_sys_init(); // RTX System Initialization
+ }
+#endif
+
+ os_tsk.run->prio = 255U; // Highest priority
+
+ if (os_initialized == 0U) {
+ // Create OS Timers resources (Message Queue & Thread)
+ osMessageQId_osTimerMessageQ = svcMessageCreate (&os_messageQ_def_osTimerMessageQ, NULL);
+ osThreadId_osTimerThread = svcThreadCreate(&os_thread_def_osTimerThread, NULL, NULL);
+ // Initialize thread mutex
+ osMutexId_osThreadMutex = osMutexCreate(osMutex(osThreadMutex));
+ }
+
+ sysThreadError(osOK);
+
+ os_initialized = 1U;
+ os_running = 0U;
+
+ return osOK;
+}
+
+/// Start the RTOS Kernel
+osStatus svcKernelStart (void) {
+
+ if (os_running) { return osOK; }
+
+ rt_tsk_prio(0U, os_tsk.run->prio_base); // Restore priority
+ if (os_tsk.run->task_id == 0xFFU) { // Idle Thread
+ __set_PSP(os_tsk.run->tsk_stack + (8U*4U)); // Setup PSP
+ }
+ if (os_tsk.new_tsk == NULL) { // Force context switch
+ os_tsk.new_tsk = os_tsk.run;
+ os_tsk.run = NULL;
+ }
+
+ rt_sys_start();
+
+ os_running = 1U;
+
+ return osOK;
+}
+
+/// Check if the RTOS kernel is already started
+int32_t svcKernelRunning (void) {
+ return (int32_t)os_running;
+}
+
+/// Get the RTOS kernel system timer counter
+uint32_t svcKernelSysTick (void) {
+ uint32_t tick, tick0;
+
+ tick = os_tick_val();
+ if (os_tick_ovf()) {
+ tick0 = os_tick_val();
+ if (tick0 < tick) { tick = tick0; }
+ tick += (os_trv + 1U) * (os_time + 1U);
+ } else {
+ tick += (os_trv + 1U) * os_time;
+ }
+
+ return tick;
+}
+
+// Kernel Control Public API
+
+/// Initialize the RTOS Kernel for creating objects
+osStatus osKernelInitialize (void) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osErrorISR; // Not allowed in ISR
+ }
+ if ((__get_CONTROL() & 1U) == 0U) { // Privileged mode
+ return svcKernelInitialize();
+ } else {
+ return __svcKernelInitialize();
+ }
+}
+
+/// Start the RTOS Kernel
+osStatus osKernelStart (void) {
+ uint32_t stack[8];
+
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osErrorISR; // Not allowed in ISR
+ }
+
+ /* Call the pre-start event (from unprivileged mode) if the handler exists
+ * and the kernel is not running. */
+ /* FIXME osEventObs needs to be readable but not writable from unprivileged
+ * code. */
+ if (!osKernelRunning() && osEventObs && osEventObs->pre_start) {
+ osEventObs->pre_start();
+ }
+
+ switch (__get_CONTROL() & 0x03U) {
+ case 0x00U: // Privileged Thread mode & MSP
+ __set_PSP((uint32_t)(stack + 8)); // Initial PSP
+ if (os_flags & 1U) {
+ __set_CONTROL(0x02U); // Set Privileged Thread mode & PSP
+ } else {
+ __set_CONTROL(0x03U); // Set Unprivileged Thread mode & PSP
+ }
+ __DSB();
+ __ISB();
+ break;
+ case 0x01U: // Unprivileged Thread mode & MSP
+ return osErrorOS;
+ case 0x02U: // Privileged Thread mode & PSP
+ if ((os_flags & 1U) == 0U) { // Unprivileged Thread mode requested
+ __set_CONTROL(0x03U); // Set Unprivileged Thread mode & PSP
+ __DSB();
+ __ISB();
+ }
+ break;
+ case 0x03U: // Unprivileged Thread mode & PSP
+ if (os_flags & 1U) { return osErrorOS; } // Privileged Thread mode requested
+ break;
+ }
+ return __svcKernelStart();
+}
+
+/// Check if the RTOS kernel is already started
+int32_t osKernelRunning (void) {
+ if ((__get_PRIMASK() != 0U || __get_IPSR() != 0U) || ((__get_CONTROL() & 1U) == 0U)) {
+ // in ISR or Privileged
+ return (int32_t)os_running;
+ } else {
+ return __svcKernelRunning();
+ }
+}
+
+/// Get the RTOS kernel system timer counter
+uint32_t osKernelSysTick (void) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { return 0U; } // Not allowed in ISR
+ return __svcKernelSysTick();
+}
+
+
+// ==== Thread Management ====
+
+/// Set Thread Error (for Create functions which return IDs)
+static void sysThreadError (osStatus status) {
+ // To Do
+}
+
+__NO_RETURN void osThreadExit (void);
+
+// Thread Service Calls declarations
+SVC_3_1(svcThreadCreate, osThreadId, const osThreadDef_t *, void *, void *, RET_pointer)
+SVC_0_1(svcThreadGetId, osThreadId, RET_pointer)
+SVC_1_1(svcThreadTerminate, osStatus, osThreadId, RET_osStatus)
+SVC_0_1(svcThreadYield, osStatus, RET_osStatus)
+SVC_2_1(svcThreadSetPriority, osStatus, osThreadId, osPriority, RET_osStatus)
+SVC_1_1(svcThreadGetPriority, osPriority, osThreadId, RET_osPriority)
+SVC_2_3(svcThreadGetInfo, os_InRegs osEvent, osThreadId, osThreadInfo, RET_osEvent)
+
+// Thread Service Calls
+
+/// Create a thread and add it to Active Threads and set it to state READY
+osThreadId svcThreadCreate (const osThreadDef_t *thread_def, void *argument, void *context) {
+ P_TCB ptcb;
+ OS_TID tsk;
+ void *stk;
+
+ if ((thread_def == NULL) ||
+ (thread_def->pthread == NULL) ||
+ (thread_def->tpriority < osPriorityIdle) ||
+ (thread_def->tpriority > osPriorityRealtime)) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+#ifdef __MBED_CMSIS_RTOS_CM
+ if (thread_def->stacksize != 0) { // Custom stack size
+ stk = (void *)thread_def->stack_pointer;
+ } else { // Default stack size
+ stk = NULL;
+ }
+#else
+ if (thread_def->stacksize != 0) { // Custom stack size
+ stk = rt_alloc_mem( // Allocate stack
+ os_stack_mem,
+ thread_def->stacksize
+ );
+ if (stk == NULL) {
+ sysThreadError(osErrorNoMemory); // Out of memory
+ return NULL;
+ }
+ } else { // Default stack size
+ stk = NULL;
+ }
+#endif
+
+ tsk = rt_tsk_create( // Create task
+ (FUNCP)thread_def->pthread, // Task function pointer
+ (uint32_t)
+ (thread_def->tpriority-osPriorityIdle+1) | // Task priority
+ (thread_def->stacksize << 8), // Task stack size in bytes
+ stk, // Pointer to task's stack
+ argument // Argument to the task
+ );
+
+ if (tsk == 0U) { // Invalid task ID
+#ifndef __MBED_CMSIS_RTOS_CM
+ if (stk != NULL) {
+ rt_free_mem(os_stack_mem, stk); // Free allocated stack
+ }
+#endif
+ sysThreadError(osErrorNoMemory); // Create task failed (Out of memory)
+ return NULL;
+ }
+
+ ptcb = (P_TCB)os_active_TCB[tsk - 1U]; // TCB pointer
+
+ *((uint32_t *)ptcb->tsk_stack + 13) = (uint32_t)osThreadExit;
+
+ if (osEventObs && osEventObs->thread_create) {
+ ptcb->context = osEventObs->thread_create(ptcb->task_id, context);
+ } else {
+ ptcb->context = context;
+ }
+
+ return ptcb;
+}
+
+/// Return the thread ID of the current running thread
+osThreadId svcThreadGetId (void) {
+ OS_TID tsk;
+
+ tsk = rt_tsk_self();
+ if (tsk == 0U) { return NULL; }
+ return (P_TCB)os_active_TCB[tsk - 1U];
+}
+
+/// Terminate execution of a thread and remove it from ActiveThreads
+osStatus svcThreadTerminate (osThreadId thread_id) {
+ OS_RESULT res;
+ P_TCB ptcb;
+#ifndef __MBED_CMSIS_RTOS_CM
+ void *stk;
+#endif
+
+ ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
+ if (ptcb == NULL) {
+ return osErrorParameter;
+ }
+
+#ifndef __MBED_CMSIS_RTOS_CM
+ stk = ptcb->priv_stack ? ptcb->stack : NULL; // Private stack
+#endif
+
+ if (osEventObs && osEventObs->thread_destroy) {
+ osEventObs->thread_destroy(ptcb->context);
+ }
+
+ res = rt_tsk_delete(ptcb->task_id); // Delete task
+
+ if (res == OS_R_NOK) {
+ return osErrorResource; // Delete task failed
+ }
+
+#ifndef __MBED_CMSIS_RTOS_CM
+ if (stk != NULL) {
+ rt_free_mem(os_stack_mem, stk); // Free private stack
+ }
+#endif
+
+ return osOK;
+}
+
+/// Pass control to next thread that is in state READY
+osStatus svcThreadYield (void) {
+ rt_tsk_pass(); // Pass control to next task
+ return osOK;
+}
+
+/// Change priority of an active thread
+osStatus svcThreadSetPriority (osThreadId thread_id, osPriority priority) {
+ OS_RESULT res;
+ P_TCB ptcb;
+
+ ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
+ if (ptcb == NULL) {
+ return osErrorParameter;
+ }
+
+ if ((priority < osPriorityIdle) || (priority > osPriorityRealtime)) {
+ return osErrorValue;
+ }
+
+ res = rt_tsk_prio( // Change task priority
+ ptcb->task_id, // Task ID
+ (uint8_t)(priority - osPriorityIdle + 1) // New task priority
+ );
+
+ if (res == OS_R_NOK) {
+ return osErrorResource; // Change task priority failed
+ }
+
+ return osOK;
+}
+
+/// Get current priority of an active thread
+osPriority svcThreadGetPriority (osThreadId thread_id) {
+ P_TCB ptcb;
+
+ ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
+ if (ptcb == NULL) {
+ return osPriorityError;
+ }
+
+ return (osPriority)(ptcb->prio - 1 + osPriorityIdle);
+}
+
+/// Get info from an active thread
+os_InRegs osEvent_type svcThreadGetInfo (osThreadId thread_id, osThreadInfo info) {
+ P_TCB ptcb;
+ osEvent ret;
+ ret.status = osOK;
+
+ ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
+ if (ptcb == NULL) {
+ ret.status = osErrorValue;
+ return osEvent_ret_status;
+ }
+
+ if (osThreadInfoStackSize == info) {
+ uint32_t size;
+ size = ptcb->priv_stack;
+ if (0 == size) {
+ // This is an OS task - always a fixed size
+ size = os_stackinfo & 0x3FFFF;
+ }
+ ret.value.v = size;
+ return osEvent_ret_value;
+ }
+
+ if (osThreadInfoStackMax == info) {
+ uint32_t i;
+ uint32_t *stack_ptr;
+ uint32_t stack_size;
+ if (!(os_stackinfo & (1 << 28))) {
+ // Stack init must be turned on for max stack usage
+ ret.status = osErrorResource;
+ return osEvent_ret_status;
+ }
+ stack_ptr = (uint32_t*)ptcb->stack;
+ stack_size = ptcb->priv_stack;
+ if (0 == stack_size) {
+ // This is an OS task - always a fixed size
+ stack_size = os_stackinfo & 0x3FFFF;
+ }
+ for (i = 1; i <stack_size / 4; i++) {
+ if (stack_ptr[i] != MAGIC_PATTERN) {
+ break;
+ }
+ }
+ ret.value.v = stack_size - i * 4;
+ return osEvent_ret_value;
+ }
+
+ if (osThreadInfoEntry == info) {
+ ret.value.p = (void*)ptcb->ptask;
+ return osEvent_ret_value;
+ }
+
+ if (osThreadInfoArg == info) {
+ ret.value.p = (void*)ptcb->argv;
+ return osEvent_ret_value;
+ }
+
+ // Unsupported option so return error
+ ret.status = osErrorParameter;
+ return osEvent_ret_status;
+}
+
+// Thread Public API
+
+/// Create a thread and add it to Active Threads and set it to state READY
+osThreadId osThreadCreate (const osThreadDef_t *thread_def, void *argument) {
+ return osThreadContextCreate(thread_def, argument, NULL);
+}
+osThreadId osThreadContextCreate (const osThreadDef_t *thread_def, void *argument, void *context) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return NULL; // Not allowed in ISR
+ }
+ if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) {
+ // Privileged and not running
+ return svcThreadCreate(thread_def, argument, context);
+ } else {
+ osThreadId id;
+ osMutexWait(osMutexId_osThreadMutex, osWaitForever);
+ // Thread mutex must be held when a thread is created or terminated
+ id = __svcThreadCreate(thread_def, argument, context);
+ osMutexRelease(osMutexId_osThreadMutex);
+ return id;
+ }
+}
+
+/// Return the thread ID of the current running thread
+osThreadId osThreadGetId (void) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return NULL; // Not allowed in ISR
+ }
+ return __svcThreadGetId();
+}
+
+/// Terminate execution of a thread and remove it from ActiveThreads
+osStatus osThreadTerminate (osThreadId thread_id) {
+ osStatus status;
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osErrorISR; // Not allowed in ISR
+ }
+ osMutexWait(osMutexId_osThreadMutex, osWaitForever);
+ sysThreadTerminate(thread_id);
+ // Thread mutex must be held when a thread is created or terminated
+ status = __svcThreadTerminate(thread_id);
+ osMutexRelease(osMutexId_osThreadMutex);
+ return status;
+}
+
+/// Pass control to next thread that is in state READY
+osStatus osThreadYield (void) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osErrorISR; // Not allowed in ISR
+ }
+ return __svcThreadYield();
+}
+
+/// Change priority of an active thread
+osStatus osThreadSetPriority (osThreadId thread_id, osPriority priority) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osErrorISR; // Not allowed in ISR
+ }
+ return __svcThreadSetPriority(thread_id, priority);
+}
+
+/// Get current priority of an active thread
+osPriority osThreadGetPriority (osThreadId thread_id) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osPriorityError; // Not allowed in ISR
+ }
+ return __svcThreadGetPriority(thread_id);
+}
+
+/// INTERNAL - Not Public
+/// Auto Terminate Thread on exit (used implicitly when thread exists)
+__NO_RETURN void osThreadExit (void) {
+ osThreadId id;
+ // Thread mutex must be held when a thread is created or terminated
+ // Note - the mutex will be released automatically by the os when
+ // the thread is terminated
+ osMutexWait(osMutexId_osThreadMutex, osWaitForever);
+ id = __svcThreadGetId();
+ sysThreadTerminate(id);
+ __svcThreadTerminate(id);
+ for (;;); // Should never come here
+}
+
+#ifdef __MBED_CMSIS_RTOS_CM
+/// Get current thread state
+uint8_t osThreadGetState (osThreadId thread_id) {
+ P_TCB ptcb;
+
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) return osErrorISR; // Not allowed in ISR
+
+ ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
+ if (ptcb == NULL) return INACTIVE;
+
+ return ptcb->state;
+}
+#endif
+
+/// Get the requested info from the specified active thread
+os_InRegs osEvent _osThreadGetInfo(osThreadId thread_id, osThreadInfo info) {
+ osEvent ret;
+
+ if (__get_IPSR() != 0U) { // Not allowed in ISR
+ ret.status = osErrorISR;
+ return ret;
+ }
+ return __svcThreadGetInfo(thread_id, info);
+}
+
+osThreadEnumId _osThreadsEnumStart() {
+ static uint32_t thread_enum_index;
+ osMutexWait(osMutexId_osThreadMutex, osWaitForever);
+ thread_enum_index = 0;
+ return &thread_enum_index;
+}
+
+osThreadId _osThreadEnumNext(osThreadEnumId enum_id) {
+ uint32_t i;
+ osThreadId id = NULL;
+ uint32_t *index = (uint32_t*)enum_id;
+ for (i = *index; i < os_maxtaskrun; i++) {
+ if (os_active_TCB[i] != NULL) {
+ id = (osThreadId)os_active_TCB[i];
+ break;
+ }
+ }
+ if (i == os_maxtaskrun) {
+ // Include the idle task at the end of the enumeration
+ id = &os_idle_TCB;
+ }
+ *index = i + 1;
+ return id;
+}
+
+osStatus _osThreadEnumFree(osThreadEnumId enum_id) {
+ uint32_t *index = (uint32_t*)enum_id;
+ *index = 0;
+ osMutexRelease(osMutexId_osThreadMutex);
+ return osOK;
+}
+
+// ==== Generic Wait Functions ====
+
+// Generic Wait Service Calls declarations
+SVC_1_1(svcDelay, osStatus, uint32_t, RET_osStatus)
+#if osFeature_Wait != 0
+SVC_1_3(svcWait, os_InRegs osEvent, uint32_t, RET_osEvent)
+#endif
+
+// Generic Wait Service Calls
+
+/// Wait for Timeout (Time Delay)
+osStatus svcDelay (uint32_t millisec) {
+ if (millisec == 0U) { return osOK; }
+ rt_dly_wait(rt_ms2tick(millisec));
+ return osEventTimeout;
+}
+
+/// Wait for Signal, Message, Mail, or Timeout
+#if osFeature_Wait != 0
+os_InRegs osEvent_type svcWait (uint32_t millisec) {
+ osEvent ret;
+
+ if (millisec == 0U) {
+ ret.status = osOK;
+ return osEvent_ret_status;
+ }
+
+ /* To Do: osEventSignal, osEventMessage, osEventMail */
+ rt_dly_wait(rt_ms2tick(millisec));
+ ret.status = osEventTimeout;
+
+ return osEvent_ret_status;
+}
+#endif
+
+
+// Generic Wait API
+
+/// Wait for Timeout (Time Delay)
+osStatus osDelay (uint32_t millisec) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osErrorISR; // Not allowed in ISR
+ }
+ return __svcDelay(millisec);
+}
+
+/// Wait for Signal, Message, Mail, or Timeout
+os_InRegs osEvent osWait (uint32_t millisec) {
+ osEvent ret;
+
+#if osFeature_Wait == 0
+ ret.status = osErrorOS;
+ return ret;
+#else
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // Not allowed in ISR
+ ret.status = osErrorISR;
+ return ret;
+ }
+ return __svcWait(millisec);
+#endif
+}
+
+
+// ==== Timer Management ====
+
+// Timer definitions
+#define osTimerInvalid 0U
+#define osTimerStopped 1U
+#define osTimerRunning 2U
+
+// Timer structures
+
+typedef struct os_timer_cb_ { // Timer Control Block
+ struct os_timer_cb_ *next; // Pointer to next active Timer
+ uint8_t state; // Timer State
+ uint8_t type; // Timer Type (Periodic/One-shot)
+ uint16_t reserved; // Reserved
+ uint32_t tcnt; // Timer Delay Count
+ uint32_t icnt; // Timer Initial Count
+ void *arg; // Timer Function Argument
+ const osTimerDef_t *timer; // Pointer to Timer definition
+} os_timer_cb;
+
+// Timer variables
+os_timer_cb *os_timer_head; // Pointer to first active Timer
+
+
+// Timer Helper Functions
+
+// Insert Timer into the list sorted by time
+static void rt_timer_insert (os_timer_cb *pt, uint32_t tcnt) {
+ os_timer_cb *p, *prev;
+
+ prev = NULL;
+ p = os_timer_head;
+ while (p != NULL) {
+ if (tcnt < p->tcnt) { break; }
+ tcnt -= p->tcnt;
+ prev = p;
+ p = p->next;
+ }
+ pt->next = p;
+ pt->tcnt = tcnt;
+ if (p != NULL) {
+ p->tcnt -= pt->tcnt;
+ }
+ if (prev != NULL) {
+ prev->next = pt;
+ } else {
+ os_timer_head = pt;
+ }
+}
+
+// Remove Timer from the list
+static int32_t rt_timer_remove (os_timer_cb *pt) {
+ os_timer_cb *p, *prev;
+
+ prev = NULL;
+ p = os_timer_head;
+ while (p != NULL) {
+ if (p == pt) { break; }
+ prev = p;
+ p = p->next;
+ }
+ if (p == NULL) { return -1; }
+ if (prev != NULL) {
+ prev->next = pt->next;
+ } else {
+ os_timer_head = pt->next;
+ }
+ if (pt->next != NULL) {
+ pt->next->tcnt += pt->tcnt;
+ }
+
+ return 0;
+}
+
+
+// Timer Service Calls declarations
+SVC_3_1(svcTimerCreate, osTimerId, const osTimerDef_t *, os_timer_type, void *, RET_pointer)
+SVC_2_1(svcTimerStart, osStatus, osTimerId, uint32_t, RET_osStatus)
+SVC_1_1(svcTimerStop, osStatus, osTimerId, RET_osStatus)
+SVC_1_1(svcTimerDelete, osStatus, osTimerId, RET_osStatus)
+SVC_1_2(svcTimerCall, os_InRegs osCallback, osTimerId, RET_osCallback)
+
+// Timer Management Service Calls
+
+/// Create timer
+osTimerId svcTimerCreate (const osTimerDef_t *timer_def, os_timer_type type, void *argument) {
+ os_timer_cb *pt;
+
+ if ((timer_def == NULL) || (timer_def->ptimer == NULL)) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+ pt = timer_def->timer;
+ if (pt == NULL) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+ if ((type != osTimerOnce) && (type != osTimerPeriodic)) {
+ sysThreadError(osErrorValue);
+ return NULL;
+ }
+
+ if (osThreadId_osTimerThread == NULL) {
+ sysThreadError(osErrorResource);
+ return NULL;
+ }
+
+ if (pt->state != osTimerInvalid){
+ sysThreadError(osErrorResource);
+ return NULL;
+ }
+
+ pt->next = NULL;
+ pt->state = osTimerStopped;
+ pt->type = (uint8_t)type;
+ pt->arg = argument;
+ pt->timer = timer_def;
+
+ return (osTimerId)pt;
+}
+
+/// Start or restart timer
+osStatus svcTimerStart (osTimerId timer_id, uint32_t millisec) {
+ os_timer_cb *pt;
+ uint32_t tcnt;
+
+ pt = rt_id2obj(timer_id);
+ if (pt == NULL) {
+ return osErrorParameter;
+ }
+
+ if (millisec == 0U) { return osErrorValue; }
+
+ tcnt = (uint32_t)(((1000U * (uint64_t)millisec) + os_clockrate - 1U) / os_clockrate);
+
+ switch (pt->state) {
+ case osTimerRunning:
+ if (rt_timer_remove(pt) != 0) {
+ return osErrorResource;
+ }
+ break;
+ case osTimerStopped:
+ pt->state = osTimerRunning;
+ pt->icnt = tcnt;
+ break;
+ default:
+ return osErrorResource;
+ }
+
+ rt_timer_insert(pt, tcnt);
+
+ return osOK;
+}
+
+/// Stop timer
+osStatus svcTimerStop (osTimerId timer_id) {
+ os_timer_cb *pt;
+
+ pt = rt_id2obj(timer_id);
+ if (pt == NULL) {
+ return osErrorParameter;
+ }
+
+ if (pt->state != osTimerRunning) { return osErrorResource; }
+
+ pt->state = osTimerStopped;
+
+ if (rt_timer_remove(pt) != 0) {
+ return osErrorResource;
+ }
+
+ return osOK;
+}
+
+/// Delete timer
+osStatus svcTimerDelete (osTimerId timer_id) {
+ os_timer_cb *pt;
+
+ pt = rt_id2obj(timer_id);
+ if (pt == NULL) {
+ return osErrorParameter;
+ }
+
+ switch (pt->state) {
+ case osTimerRunning:
+ rt_timer_remove(pt);
+ break;
+ case osTimerStopped:
+ break;
+ default:
+ return osErrorResource;
+ }
+
+ pt->state = osTimerInvalid;
+
+ return osOK;
+}
+
+/// Get timer callback parameters
+os_InRegs osCallback_type svcTimerCall (osTimerId timer_id) {
+ os_timer_cb *pt;
+ osCallback ret;
+
+ pt = rt_id2obj(timer_id);
+ if (pt == NULL) {
+ ret.fp = NULL;
+ ret.arg = NULL;
+ return osCallback_ret;
+ }
+
+ ret.fp = (void *)pt->timer->ptimer;
+ ret.arg = pt->arg;
+
+ return osCallback_ret;
+}
+
+osStatus isrMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec);
+
+/// Timer Tick (called each SysTick)
+void sysTimerTick (void) {
+ os_timer_cb *pt, *p;
+ osStatus status;
+
+ p = os_timer_head;
+ if (p == NULL) { return; }
+
+ p->tcnt--;
+ while ((p != NULL) && (p->tcnt == 0U)) {
+ pt = p;
+ p = p->next;
+ os_timer_head = p;
+ status = isrMessagePut(osMessageQId_osTimerMessageQ, (uint32_t)pt, 0U);
+ if (status != osOK) {
+ os_error(OS_ERR_TIMER_OVF);
+ }
+ if (pt->type == (uint8_t)osTimerPeriodic) {
+ rt_timer_insert(pt, pt->icnt);
+ } else {
+ pt->state = osTimerStopped;
+ }
+ }
+}
+
+/// Get user timers wake-up time
+uint32_t sysUserTimerWakeupTime (void) {
+
+ if (os_timer_head) {
+ return os_timer_head->tcnt;
+ }
+ return 0xFFFFFFFFU;
+}
+
+/// Update user timers on resume
+void sysUserTimerUpdate (uint32_t sleep_time) {
+
+ while ((os_timer_head != NULL) && (sleep_time != 0U)) {
+ if (sleep_time >= os_timer_head->tcnt) {
+ sleep_time -= os_timer_head->tcnt;
+ os_timer_head->tcnt = 1U;
+ sysTimerTick();
+ } else {
+ os_timer_head->tcnt -= sleep_time;
+ break;
+ }
+ }
+}
+
+
+// Timer Management Public API
+
+/// Create timer
+osTimerId osTimerCreate (const osTimerDef_t *timer_def, os_timer_type type, void *argument) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return NULL; // Not allowed in ISR
+ }
+ if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) {
+ // Privileged and not running
+ return svcTimerCreate(timer_def, type, argument);
+ } else {
+ return __svcTimerCreate(timer_def, type, argument);
+ }
+}
+
+/// Start or restart timer
+osStatus osTimerStart (osTimerId timer_id, uint32_t millisec) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osErrorISR; // Not allowed in ISR
+ }
+ return __svcTimerStart(timer_id, millisec);
+}
+
+/// Stop timer
+osStatus osTimerStop (osTimerId timer_id) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osErrorISR; // Not allowed in ISR
+ }
+ return __svcTimerStop(timer_id);
+}
+
+/// Delete timer
+osStatus osTimerDelete (osTimerId timer_id) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osErrorISR; // Not allowed in ISR
+ }
+ return __svcTimerDelete(timer_id);
+}
+
+/// INTERNAL - Not Public
+/// Get timer callback parameters (used by OS Timer Thread)
+os_InRegs osCallback osTimerCall (osTimerId timer_id) {
+ return __svcTimerCall(timer_id);
+}
+
+
+// Timer Thread
+__NO_RETURN void osTimerThread (void const *argument) {
+ osCallback cb;
+ osEvent evt;
+
+ for (;;) {
+ evt = osMessageGet(osMessageQId_osTimerMessageQ, osWaitForever);
+ if (evt.status == osEventMessage) {
+ cb = osTimerCall(evt.value.p);
+ if (cb.fp != NULL) {
+ (*(os_ptimer)cb.fp)(cb.arg);
+ }
+ }
+ }
+}
+
+
+// ==== Signal Management ====
+
+// Signal Service Calls declarations
+SVC_2_1(svcSignalSet, int32_t, osThreadId, int32_t, RET_int32_t)
+SVC_2_1(svcSignalClear, int32_t, osThreadId, int32_t, RET_int32_t)
+SVC_2_3(svcSignalWait, os_InRegs osEvent, int32_t, uint32_t, RET_osEvent)
+
+// Signal Service Calls
+
+/// Set the specified Signal Flags of an active thread
+int32_t svcSignalSet (osThreadId thread_id, int32_t signals) {
+ P_TCB ptcb;
+ int32_t sig;
+
+ ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
+ if (ptcb == NULL) {
+ return (int32_t)0x80000000U;
+ }
+
+ if ((uint32_t)signals & (0xFFFFFFFFU << osFeature_Signals)) {
+ return (int32_t)0x80000000U;
+ }
+
+ sig = (int32_t)ptcb->events; // Previous signal flags
+
+ rt_evt_set((uint16_t)signals, ptcb->task_id); // Set event flags
+
+ return sig;
+}
+
+/// Clear the specified Signal Flags of an active thread
+int32_t svcSignalClear (osThreadId thread_id, int32_t signals) {
+ P_TCB ptcb;
+ int32_t sig;
+
+ ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
+ if (ptcb == NULL) {
+ return (int32_t)0x80000000U;
+ }
+
+ if ((uint32_t)signals & (0xFFFFFFFFU << osFeature_Signals)) {
+ return (int32_t)0x80000000U;
+ }
+
+ sig = (int32_t)ptcb->events; // Previous signal flags
+
+ rt_evt_clr((uint16_t)signals, ptcb->task_id); // Clear event flags
+
+ return sig;
+}
+
+/// Wait for one or more Signal Flags to become signaled for the current RUNNING thread
+os_InRegs osEvent_type svcSignalWait (int32_t signals, uint32_t millisec) {
+ OS_RESULT res;
+ osEvent ret;
+
+ if ((uint32_t)signals & (0xFFFFFFFFU << osFeature_Signals)) {
+ ret.status = osErrorValue;
+ return osEvent_ret_status;
+ }
+
+ if (signals != 0) { // Wait for all specified signals
+ res = rt_evt_wait((uint16_t)signals, rt_ms2tick(millisec), __TRUE);
+ } else { // Wait for any signal
+ res = rt_evt_wait(0xFFFFU, rt_ms2tick(millisec), __FALSE);
+ }
+
+ if (res == OS_R_EVT) {
+ ret.status = osEventSignal;
+ ret.value.signals = (signals != 0) ? signals : (int32_t)os_tsk.run->waits;
+ } else {
+ ret.status = (millisec != 0U) ? osEventTimeout : osOK;
+ ret.value.signals = 0;
+ }
+
+ return osEvent_ret_value;
+}
+
+
+// Signal ISR Calls
+
+/// Set the specified Signal Flags of an active thread
+int32_t isrSignalSet (osThreadId thread_id, int32_t signals) {
+ P_TCB ptcb;
+ int32_t sig;
+
+ ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
+ if (ptcb == NULL) {
+ return (int32_t)0x80000000U;
+ }
+
+ if ((uint32_t)signals & (0xFFFFFFFFU << osFeature_Signals)) {
+ return (int32_t)0x80000000U;
+ }
+
+ sig = (int32_t)ptcb->events; // Previous signal flags
+
+ isr_evt_set((uint16_t)signals, ptcb->task_id);// Set event flags
+
+ return sig;
+}
+
+
+// Signal Public API
+
+/// Set the specified Signal Flags of an active thread
+int32_t osSignalSet (osThreadId thread_id, int32_t signals) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR
+ return isrSignalSet(thread_id, signals);
+ } else { // in Thread
+ return __svcSignalSet(thread_id, signals);
+ }
+}
+
+/// Clear the specified Signal Flags of an active thread
+int32_t osSignalClear (osThreadId thread_id, int32_t signals) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return (int32_t)0x80000000U; // Not allowed in ISR
+ }
+ return __svcSignalClear(thread_id, signals);
+}
+
+/// Wait for one or more Signal Flags to become signaled for the current RUNNING thread
+os_InRegs osEvent osSignalWait (int32_t signals, uint32_t millisec) {
+ osEvent ret;
+
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // Not allowed in ISR
+ ret.status = osErrorISR;
+ return ret;
+ }
+ return __svcSignalWait(signals, millisec);
+}
+
+
+// ==== Mutex Management ====
+
+// Mutex Service Calls declarations
+SVC_1_1(svcMutexCreate, osMutexId, const osMutexDef_t *, RET_pointer)
+SVC_2_1(svcMutexWait, osStatus, osMutexId, uint32_t, RET_osStatus)
+SVC_1_1(svcMutexRelease, osStatus, osMutexId, RET_osStatus)
+SVC_1_1(svcMutexDelete, osStatus, osMutexId, RET_osStatus)
+
+// Mutex Service Calls
+
+/// Create and Initialize a Mutex object
+osMutexId svcMutexCreate (const osMutexDef_t *mutex_def) {
+ OS_ID mut;
+
+ if (mutex_def == NULL) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+ mut = mutex_def->mutex;
+ if (mut == NULL) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+ if (((P_MUCB)mut)->cb_type != 0U) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+ rt_mut_init(mut); // Initialize Mutex
+
+ return mut;
+}
+
+/// Wait until a Mutex becomes available
+osStatus svcMutexWait (osMutexId mutex_id, uint32_t millisec) {
+ OS_ID mut;
+ OS_RESULT res;
+
+ mut = rt_id2obj(mutex_id);
+ if (mut == NULL) {
+ return osErrorParameter;
+ }
+
+ if (((P_MUCB)mut)->cb_type != MUCB) {
+ return osErrorParameter;
+ }
+
+ res = rt_mut_wait(mut, rt_ms2tick(millisec)); // Wait for Mutex
+
+ if (res == OS_R_TMO) {
+ return ((millisec != 0U) ? osErrorTimeoutResource : osErrorResource);
+ }
+
+ return osOK;
+}
+
+/// Release a Mutex that was obtained with osMutexWait
+osStatus svcMutexRelease (osMutexId mutex_id) {
+ OS_ID mut;
+ OS_RESULT res;
+
+ mut = rt_id2obj(mutex_id);
+ if (mut == NULL) {
+ return osErrorParameter;
+ }
+
+ if (((P_MUCB)mut)->cb_type != MUCB) {
+ return osErrorParameter;
+ }
+
+ res = rt_mut_release(mut); // Release Mutex
+
+ if (res == OS_R_NOK) {
+ return osErrorResource; // Thread not owner or Zero Counter
+ }
+
+ return osOK;
+}
+
+/// Delete a Mutex that was created by osMutexCreate
+osStatus svcMutexDelete (osMutexId mutex_id) {
+ OS_ID mut;
+
+ mut = rt_id2obj(mutex_id);
+ if (mut == NULL) {
+ return osErrorParameter;
+ }
+
+ if (((P_MUCB)mut)->cb_type != MUCB) {
+ return osErrorParameter;
+ }
+
+ rt_mut_delete(mut); // Release Mutex
+
+ return osOK;
+}
+
+
+// Mutex Public API
+
+/// Create and Initialize a Mutex object
+osMutexId osMutexCreate (const osMutexDef_t *mutex_def) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return NULL; // Not allowed in ISR
+ }
+ if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) {
+ // Privileged and not running
+ return svcMutexCreate(mutex_def);
+ } else {
+ return __svcMutexCreate(mutex_def);
+ }
+}
+
+/// Wait until a Mutex becomes available
+osStatus osMutexWait (osMutexId mutex_id, uint32_t millisec) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osErrorISR; // Not allowed in ISR
+ }
+ return __svcMutexWait(mutex_id, millisec);
+}
+
+/// Release a Mutex that was obtained with osMutexWait
+osStatus osMutexRelease (osMutexId mutex_id) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osErrorISR; // Not allowed in ISR
+ }
+ return __svcMutexRelease(mutex_id);
+}
+
+/// Delete a Mutex that was created by osMutexCreate
+osStatus osMutexDelete (osMutexId mutex_id) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osErrorISR; // Not allowed in ISR
+ }
+ return __svcMutexDelete(mutex_id);
+}
+
+
+// ==== Semaphore Management ====
+
+// Semaphore Service Calls declarations
+SVC_2_1(svcSemaphoreCreate, osSemaphoreId, const osSemaphoreDef_t *, int32_t, RET_pointer)
+SVC_2_1(svcSemaphoreWait, int32_t, osSemaphoreId, uint32_t, RET_int32_t)
+SVC_1_1(svcSemaphoreRelease, osStatus, osSemaphoreId, RET_osStatus)
+SVC_1_1(svcSemaphoreDelete, osStatus, osSemaphoreId, RET_osStatus)
+
+// Semaphore Service Calls
+
+/// Create and Initialize a Semaphore object
+osSemaphoreId svcSemaphoreCreate (const osSemaphoreDef_t *semaphore_def, int32_t count) {
+ OS_ID sem;
+
+ if (semaphore_def == NULL) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+ sem = semaphore_def->semaphore;
+ if (sem == NULL) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+ if (((P_SCB)sem)->cb_type != 0U) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+ if (count > osFeature_Semaphore) {
+ sysThreadError(osErrorValue);
+ return NULL;
+ }
+
+ rt_sem_init(sem, (uint16_t)count); // Initialize Semaphore
+
+ return sem;
+}
+
+/// Wait until a Semaphore becomes available
+int32_t svcSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec) {
+ OS_ID sem;
+ OS_RESULT res;
+
+ sem = rt_id2obj(semaphore_id);
+ if (sem == NULL) {
+ return -1;
+ }
+
+ if (((P_SCB)sem)->cb_type != SCB) {
+ return -1;
+ }
+
+ res = rt_sem_wait(sem, rt_ms2tick(millisec)); // Wait for Semaphore
+
+ if (res == OS_R_TMO) { return 0; } // Timeout
+
+ return (int32_t)(((P_SCB)sem)->tokens + 1U);
+}
+
+/// Release a Semaphore
+osStatus svcSemaphoreRelease (osSemaphoreId semaphore_id) {
+ OS_ID sem;
+
+ sem = rt_id2obj(semaphore_id);
+ if (sem == NULL) {
+ return osErrorParameter;
+ }
+
+ if (((P_SCB)sem)->cb_type != SCB) {
+ return osErrorParameter;
+ }
+
+ if ((int32_t)((P_SCB)sem)->tokens == osFeature_Semaphore) {
+ return osErrorResource;
+ }
+
+ rt_sem_send(sem); // Release Semaphore
+
+ return osOK;
+}
+
+/// Delete a Semaphore that was created by osSemaphoreCreate
+osStatus svcSemaphoreDelete (osSemaphoreId semaphore_id) {
+ OS_ID sem;
+
+ sem = rt_id2obj(semaphore_id);
+ if (sem == NULL) {
+ return osErrorParameter;
+ }
+
+ if (((P_SCB)sem)->cb_type != SCB) {
+ return osErrorParameter;
+ }
+
+ rt_sem_delete(sem); // Delete Semaphore
+
+ return osOK;
+}
+
+
+// Semaphore ISR Calls
+
+/// Release a Semaphore
+osStatus isrSemaphoreRelease (osSemaphoreId semaphore_id) {
+ OS_ID sem;
+
+ sem = rt_id2obj(semaphore_id);
+ if (sem == NULL) {
+ return osErrorParameter;
+ }
+
+ if (((P_SCB)sem)->cb_type != SCB) {
+ return osErrorParameter;
+ }
+
+ if ((int32_t)((P_SCB)sem)->tokens == osFeature_Semaphore) {
+ return osErrorResource;
+ }
+
+ isr_sem_send(sem); // Release Semaphore
+
+ return osOK;
+}
+
+
+// Semaphore Public API
+
+/// Create and Initialize a Semaphore object
+osSemaphoreId osSemaphoreCreate (const osSemaphoreDef_t *semaphore_def, int32_t count) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return NULL; // Not allowed in ISR
+ }
+ if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) {
+ // Privileged and not running
+ return svcSemaphoreCreate(semaphore_def, count);
+ } else {
+ return __svcSemaphoreCreate(semaphore_def, count);
+ }
+}
+
+/// Wait until a Semaphore becomes available
+int32_t osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return -1; // Not allowed in ISR
+ }
+ return __svcSemaphoreWait(semaphore_id, millisec);
+}
+
+/// Release a Semaphore
+osStatus osSemaphoreRelease (osSemaphoreId semaphore_id) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR
+ return isrSemaphoreRelease(semaphore_id);
+ } else { // in Thread
+ return __svcSemaphoreRelease(semaphore_id);
+ }
+}
+
+/// Delete a Semaphore that was created by osSemaphoreCreate
+osStatus osSemaphoreDelete (osSemaphoreId semaphore_id) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return osErrorISR; // Not allowed in ISR
+ }
+ return __svcSemaphoreDelete(semaphore_id);
+}
+
+
+// ==== Memory Management Functions ====
+
+// Memory Management Helper Functions
+
+// Clear Memory Box (Zero init)
+static void rt_clr_box (void *box_mem, void *box) {
+ uint32_t *p, n;
+
+ if ((box_mem != NULL) && (box != NULL)) {
+ p = box;
+ for (n = ((P_BM)box_mem)->blk_size; n; n -= 4U) {
+ *p++ = 0U;
+ }
+ }
+}
+
+// Memory Management Service Calls declarations
+SVC_1_1(svcPoolCreate, osPoolId, const osPoolDef_t *, RET_pointer)
+SVC_1_1(sysPoolAlloc, void *, osPoolId, RET_pointer)
+SVC_2_1(sysPoolFree, osStatus, osPoolId, void *, RET_osStatus)
+
+// Memory Management Service & ISR Calls
+
+/// Create and Initialize memory pool
+osPoolId svcPoolCreate (const osPoolDef_t *pool_def) {
+ uint32_t blk_sz;
+
+ if ((pool_def == NULL) ||
+ (pool_def->pool_sz == 0U) ||
+ (pool_def->item_sz == 0U) ||
+ (pool_def->pool == NULL)) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+ blk_sz = (pool_def->item_sz + 3U) & (uint32_t)~3U;
+
+ _init_box(pool_def->pool, sizeof(struct OS_BM) + (pool_def->pool_sz * blk_sz), blk_sz);
+
+ return pool_def->pool;
+}
+
+/// Allocate a memory block from a memory pool
+void *sysPoolAlloc (osPoolId pool_id) {
+ void *mem;
+
+ if (pool_id == NULL) {
+ return NULL;
+ }
+
+ mem = rt_alloc_box(pool_id);
+
+ return mem;
+}
+
+/// Return an allocated memory block back to a specific memory pool
+osStatus sysPoolFree (osPoolId pool_id, void *block) {
+ uint32_t res;
+
+ if (pool_id == NULL) {
+ return osErrorParameter;
+ }
+
+ res = rt_free_box(pool_id, block);
+ if (res != 0) {
+ return osErrorValue;
+ }
+
+ return osOK;
+}
+
+
+// Memory Management Public API
+
+/// Create and Initialize memory pool
+osPoolId osPoolCreate (const osPoolDef_t *pool_def) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return NULL; // Not allowed in ISR
+ }
+ if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) {
+ // Privileged and not running
+ return svcPoolCreate(pool_def);
+ } else {
+ return __svcPoolCreate(pool_def);
+ }
+}
+
+/// Allocate a memory block from a memory pool
+void *osPoolAlloc (osPoolId pool_id) {
+ if ((__get_PRIMASK() != 0U || __get_IPSR() != 0U) || ((__get_CONTROL() & 1U) == 0U)) { // in ISR or Privileged
+ return sysPoolAlloc(pool_id);
+ } else { // in Thread
+ return __sysPoolAlloc(pool_id);
+ }
+}
+
+/// Allocate a memory block from a memory pool and set memory block to zero
+void *osPoolCAlloc (osPoolId pool_id) {
+ void *mem;
+
+ if ((__get_PRIMASK() != 0U || __get_IPSR() != 0U) || ((__get_CONTROL() & 1U) == 0U)) { // in ISR or Privileged
+ mem = sysPoolAlloc(pool_id);
+ } else { // in Thread
+ mem = __sysPoolAlloc(pool_id);
+ }
+
+ rt_clr_box(pool_id, mem);
+
+ return mem;
+}
+
+/// Return an allocated memory block back to a specific memory pool
+osStatus osPoolFree (osPoolId pool_id, void *block) {
+ if ((__get_PRIMASK() != 0U || __get_IPSR() != 0U) || ((__get_CONTROL() & 1U) == 0U)) { // in ISR or Privileged
+ return sysPoolFree(pool_id, block);
+ } else { // in Thread
+ return __sysPoolFree(pool_id, block);
+ }
+}
+
+
+// ==== Message Queue Management Functions ====
+
+// Message Queue Management Service Calls declarations
+SVC_2_1(svcMessageCreate, osMessageQId, const osMessageQDef_t *, osThreadId, RET_pointer)
+SVC_3_1(svcMessagePut, osStatus, osMessageQId, uint32_t, uint32_t, RET_osStatus)
+SVC_2_3(svcMessageGet, os_InRegs osEvent, osMessageQId, uint32_t, RET_osEvent)
+
+// Message Queue Service Calls
+
+/// Create and Initialize Message Queue
+osMessageQId svcMessageCreate (const osMessageQDef_t *queue_def, osThreadId thread_id) {
+
+ if ((queue_def == NULL) ||
+ (queue_def->queue_sz == 0U) ||
+ (queue_def->pool == NULL)) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+ if (((P_MCB)queue_def->pool)->cb_type != 0U) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+ rt_mbx_init(queue_def->pool, (uint16_t)(4U*(queue_def->queue_sz + 4U)));
+
+ return queue_def->pool;
+}
+
+/// Put a Message to a Queue
+osStatus svcMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec) {
+ OS_RESULT res;
+
+ if (queue_id == NULL) {
+ return osErrorParameter;
+ }
+
+ if (((P_MCB)queue_id)->cb_type != MCB) {
+ return osErrorParameter;
+ }
+
+ res = rt_mbx_send(queue_id, (void *)info, rt_ms2tick(millisec));
+
+ if (res == OS_R_TMO) {
+ return ((millisec != 0U) ? osErrorTimeoutResource : osErrorResource);
+ }
+
+ return osOK;
+}
+
+/// Get a Message or Wait for a Message from a Queue
+os_InRegs osEvent_type svcMessageGet (osMessageQId queue_id, uint32_t millisec) {
+ OS_RESULT res;
+ osEvent ret;
+
+ if (queue_id == NULL) {
+ ret.status = osErrorParameter;
+ return osEvent_ret_status;
+ }
+
+ if (((P_MCB)queue_id)->cb_type != MCB) {
+ ret.status = osErrorParameter;
+ return osEvent_ret_status;
+ }
+
+ res = rt_mbx_wait(queue_id, &ret.value.p, rt_ms2tick(millisec));
+
+ if (res == OS_R_TMO) {
+ ret.status = (millisec != 0U) ? osEventTimeout : osOK;
+ return osEvent_ret_value;
+ }
+
+ ret.status = osEventMessage;
+
+ return osEvent_ret_value;
+}
+
+
+// Message Queue ISR Calls
+
+/// Put a Message to a Queue
+osStatus isrMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec) {
+
+ if ((queue_id == NULL) || (millisec != 0U)) {
+ return osErrorParameter;
+ }
+
+ if (((P_MCB)queue_id)->cb_type != MCB) {
+ return osErrorParameter;
+ }
+
+ if (rt_mbx_check(queue_id) == 0U) { // Check if Queue is full
+ return osErrorResource;
+ }
+
+ isr_mbx_send(queue_id, (void *)info);
+
+ return osOK;
+}
+
+/// Get a Message or Wait for a Message from a Queue
+os_InRegs osEvent isrMessageGet (osMessageQId queue_id, uint32_t millisec) {
+ OS_RESULT res;
+ osEvent ret;
+
+ if ((queue_id == NULL) || (millisec != 0U)) {
+ ret.status = osErrorParameter;
+ return ret;
+ }
+
+ if (((P_MCB)queue_id)->cb_type != MCB) {
+ ret.status = osErrorParameter;
+ return ret;
+ }
+
+ res = isr_mbx_receive(queue_id, &ret.value.p);
+
+ if (res != OS_R_MBX) {
+ ret.status = osOK;
+ return ret;
+ }
+
+ ret.status = osEventMessage;
+
+ return ret;
+}
+
+
+// Message Queue Management Public API
+
+/// Create and Initialize Message Queue
+osMessageQId osMessageCreate (const osMessageQDef_t *queue_def, osThreadId thread_id) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return NULL; // Not allowed in ISR
+ }
+ if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) {
+ // Privileged and not running
+ return svcMessageCreate(queue_def, thread_id);
+ } else {
+ return __svcMessageCreate(queue_def, thread_id);
+ }
+}
+
+/// Put a Message to a Queue
+osStatus osMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR
+ return isrMessagePut(queue_id, info, millisec);
+ } else { // in Thread
+ return __svcMessagePut(queue_id, info, millisec);
+ }
+}
+
+/// Get a Message or Wait for a Message from a Queue
+os_InRegs osEvent osMessageGet (osMessageQId queue_id, uint32_t millisec) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR
+ return isrMessageGet(queue_id, millisec);
+ } else { // in Thread
+ return __svcMessageGet(queue_id, millisec);
+ }
+}
+
+
+// ==== Mail Queue Management Functions ====
+
+// Mail Queue Management Service Calls declarations
+SVC_2_1(svcMailCreate, osMailQId, const osMailQDef_t *, osThreadId, RET_pointer)
+SVC_3_1(sysMailAlloc, void *, osMailQId, uint32_t, uint32_t, RET_pointer)
+SVC_3_1(sysMailFree, osStatus, osMailQId, void *, uint32_t, RET_osStatus)
+
+// Mail Queue Management Service & ISR Calls
+
+/// Create and Initialize mail queue
+osMailQId svcMailCreate (const osMailQDef_t *queue_def, osThreadId thread_id) {
+ uint32_t blk_sz;
+ P_MCB pmcb;
+ void *pool;
+
+ if ((queue_def == NULL) ||
+ (queue_def->queue_sz == 0U) ||
+ (queue_def->item_sz == 0U) ||
+ (queue_def->pool == NULL)) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+ pmcb = *(((void **)queue_def->pool) + 0);
+ pool = *(((void **)queue_def->pool) + 1);
+
+ if ((pool == NULL) || (pmcb == NULL) || (pmcb->cb_type != 0U)) {
+ sysThreadError(osErrorParameter);
+ return NULL;
+ }
+
+ blk_sz = (queue_def->item_sz + 3U) & (uint32_t)~3U;
+
+ _init_box(pool, sizeof(struct OS_BM) + (queue_def->queue_sz * blk_sz), blk_sz);
+
+ rt_mbx_init(pmcb, (uint16_t)(4U*(queue_def->queue_sz + 4U)));
+
+ return queue_def->pool;
+}
+
+/// Allocate a memory block from a mail
+void *sysMailAlloc (osMailQId queue_id, uint32_t millisec, uint32_t isr) {
+ P_MCB pmcb;
+ void *pool;
+ void *mem;
+
+ if (queue_id == NULL) {
+ return NULL;
+ }
+
+ pmcb = *(((void **)queue_id) + 0);
+ pool = *(((void **)queue_id) + 1);
+
+ if ((pool == NULL) || (pmcb == NULL)) {
+ return NULL;
+ }
+
+ if ((isr != 0U) && (millisec != 0U)) {
+ return NULL;
+ }
+
+ mem = rt_alloc_box(pool);
+
+ if ((mem == NULL) && (millisec != 0U)) {
+ // Put Task to sleep when Memory not available
+ if (pmcb->p_lnk != NULL) {
+ rt_put_prio((P_XCB)pmcb, os_tsk.run);
+ } else {
+ pmcb->p_lnk = os_tsk.run;
+ os_tsk.run->p_lnk = NULL;
+ os_tsk.run->p_rlnk = (P_TCB)pmcb;
+ // Task is waiting to allocate a message
+ pmcb->state = 3U;
+ }
+ rt_block(rt_ms2tick(millisec), WAIT_MBX);
+ }
+
+ return mem;
+}
+
+/// Free a memory block from a mail
+osStatus sysMailFree (osMailQId queue_id, void *mail, uint32_t isr) {
+ P_MCB pmcb;
+ P_TCB ptcb;
+ void *pool;
+ void *mem;
+ uint32_t res;
+
+ if (queue_id == NULL) {
+ return osErrorParameter;
+ }
+
+ pmcb = *(((void **)queue_id) + 0);
+ pool = *(((void **)queue_id) + 1);
+
+ if ((pmcb == NULL) || (pool == NULL)) {
+ return osErrorParameter;
+ }
+
+ res = rt_free_box(pool, mail);
+
+ if (res != 0U) {
+ return osErrorValue;
+ }
+
+ if ((pmcb->p_lnk != NULL) && (pmcb->state == 3U)) {
+ // Task is waiting to allocate a message
+ if (isr != 0U) {
+ rt_psq_enq (pmcb, (U32)pool);
+ rt_psh_req ();
+ } else {
+ mem = rt_alloc_box(pool);
+ if (mem != NULL) {
+ ptcb = rt_get_first((P_XCB)pmcb);
+ rt_ret_val(ptcb, (U32)mem);
+ rt_rmv_dly(ptcb);
+ rt_dispatch(ptcb);
+ }
+ }
+ }
+
+ return osOK;
+}
+
+
+// Mail Queue Management Public API
+
+/// Create and Initialize mail queue
+osMailQId osMailCreate (const osMailQDef_t *queue_def, osThreadId thread_id) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) {
+ return NULL; // Not allowed in ISR
+ }
+ if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) {
+ // Privileged and not running
+ return svcMailCreate(queue_def, thread_id);
+ } else {
+ return __svcMailCreate(queue_def, thread_id);
+ }
+}
+
+/// Allocate a memory block from a mail
+void *osMailAlloc (osMailQId queue_id, uint32_t millisec) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR
+ return sysMailAlloc(queue_id, millisec, 1U);
+ } else { // in Thread
+ return __sysMailAlloc(queue_id, millisec, 0U);
+ }
+}
+
+/// Allocate a memory block from a mail and set memory block to zero
+void *osMailCAlloc (osMailQId queue_id, uint32_t millisec) {
+ void *pool;
+ void *mem;
+
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR
+ mem = sysMailAlloc(queue_id, millisec, 1U);
+ } else { // in Thread
+ mem = __sysMailAlloc(queue_id, millisec, 0U);
+ }
+
+ pool = *(((void **)queue_id) + 1);
+
+ rt_clr_box(pool, mem);
+
+ return mem;
+}
+
+/// Free a memory block from a mail
+osStatus osMailFree (osMailQId queue_id, void *mail) {
+ if (__get_PRIMASK() != 0U || __get_IPSR() != 0U) { // in ISR
+ return sysMailFree(queue_id, mail, 1U);
+ } else { // in Thread
+ return __sysMailFree(queue_id, mail, 0U);
+ }
+}
+
+/// Put a mail to a queue
+osStatus osMailPut (osMailQId queue_id, void *mail) {
+ if (queue_id == NULL) {
+ return osErrorParameter;
+ }
+ if (mail == NULL) {
+ return osErrorValue;
+ }
+ return osMessagePut(*((void **)queue_id), (uint32_t)mail, 0U);
+}
+
+/// Get a mail from a queue
+os_InRegs osEvent osMailGet (osMailQId queue_id, uint32_t millisec) {
+ osEvent ret;
+
+ if (queue_id == NULL) {
+ ret.status = osErrorParameter;
+ return ret;
+ }
+
+ ret = osMessageGet(*((void **)queue_id), millisec);
+ if (ret.status == osEventMessage) ret.status = osEventMail;
+
+ return ret;
+}
+
+
+// ==== RTX Extensions ====
+
+// Service Calls declarations
+SVC_0_1(rt_suspend, uint32_t, RET_uint32_t)
+SVC_1_0(rt_resume, void, uint32_t)
+
+
+// Public API
+
+/// Suspends the OS task scheduler
+uint32_t os_suspend (void) {
+ return __rt_suspend();
+}
+
+/// Resumes the OS task scheduler
+void os_resume (uint32_t sleep_time) {
+ __rt_resume(sleep_time);
+}