diff options
Diffstat (limited to 'libraries/mbed/rtos/rtx/TARGET_CORTEX_M/rt_CMSIS.c')
| -rw-r--r-- | libraries/mbed/rtos/rtx/TARGET_CORTEX_M/rt_CMSIS.c | 1887 |
1 files changed, 0 insertions, 1887 deletions
diff --git a/libraries/mbed/rtos/rtx/TARGET_CORTEX_M/rt_CMSIS.c b/libraries/mbed/rtos/rtx/TARGET_CORTEX_M/rt_CMSIS.c deleted file mode 100644 index a747caf..0000000 --- a/libraries/mbed/rtos/rtx/TARGET_CORTEX_M/rt_CMSIS.c +++ /dev/null @@ -1,1887 +0,0 @@ -/*---------------------------------------------------------------------------- - * RL-ARM - RTX - *---------------------------------------------------------------------------- - * Name: rt_CMSIS.c - * Purpose: CMSIS RTOS API - * Rev.: V4.60 - *---------------------------------------------------------------------------- - * - * Copyright (c) 1999-2009 KEIL, 2009-2012 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 - -#include "rt_TypeDef.h" -#include "RTX_Conf.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_HAL_CM.h" - -#define os_thread_cb OS_TCB - -#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_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_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 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 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_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): "r12" \ - ); - - -#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_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 - -// OS Timers external resources -extern osThreadDef_t os_thread_def_osTimerThread; -extern osThreadId osThreadId_osTimerThread; -extern osMessageQDef_t os_messageQ_def_osTimerMessageQ; -extern osMessageQId osMessageQId_osTimerMessageQ; - - -// ==== Helper Functions ==== - -/// Convert timeout in millisec to system ticks -static uint32_t rt_ms2tick (uint32_t millisec) { - uint32_t tick; - - if (millisec == osWaitForever) return 0xFFFF; // Indefinite timeout - if (millisec > 4000000) return 0xFFFE; // Max ticks supported - - tick = ((1000 * millisec) + os_clockrate - 1) / os_clockrate; - if (tick > 0xFFFE) return 0xFFFE; - - return tick; -} - -/// Convert Thread ID to TCB pointer -static P_TCB rt_tid2ptcb (osThreadId thread_id) { - P_TCB ptcb; - - if (thread_id == NULL) return NULL; - - if ((uint32_t)thread_id & 3) return NULL; - -#ifdef OS_SECTIONS_LINK_INFO - if ((os_section_id$$Base != 0) && (os_section_id$$Limit != 0)) { - 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 & 3) return NULL; - -#ifdef OS_SECTIONS_LINK_INFO - if ((os_section_id$$Base != 0) && (os_section_id$$Limit != 0)) { - 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) - -extern void sysThreadError (osStatus status); -osThreadId svcThreadCreate (osThreadDef_t *thread_def, void *argument); -osMessageQId svcMessageCreate (osMessageQDef_t *queue_def, osThreadId thread_id); - -// Kernel Control Service Calls - -/// Initialize the RTOS Kernel for creating objects -osStatus svcKernelInitialize (void) { - if (os_initialized) return osOK; - - rt_sys_init(); // RTX System Initialization - os_tsk.run->prio = 255; // Highest priority - - sysThreadError(osOK); - - os_initialized = 1; - - return osOK; -} - -/// Start the RTOS Kernel -osStatus svcKernelStart (void) { - - if (os_running) return osOK; - - // Create OS Timers resources (Message Queue & Thread) - osMessageQId_osTimerMessageQ = svcMessageCreate (&os_messageQ_def_osTimerMessageQ, NULL); - osThreadId_osTimerThread = svcThreadCreate(&os_thread_def_osTimerThread, NULL); - - rt_tsk_prio(0, 0); // Lowest priority - __set_PSP(os_tsk.run->tsk_stack + 8*4); // New context - os_tsk.run = NULL; // Force context switch - - rt_sys_start(); - - os_running = 1; - - return osOK; -} - -/// Check if the RTOS kernel is already started -int32_t svcKernelRunning(void) { - return os_running; -} - -// Kernel Control Public API - -/// Initialize the RTOS Kernel for creating objects -osStatus osKernelInitialize (void) { - if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR - if ((__get_CONTROL() & 1) == 0) { // Privileged mode - return svcKernelInitialize(); - } else { - return __svcKernelInitialize(); - } -} - -/// Start the RTOS Kernel -osStatus osKernelStart (void) { - uint32_t stack[8]; - - if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR - switch (__get_CONTROL() & 0x03) { - case 0x00: // Privileged Thread mode & MSP - __set_PSP((uint32_t)(stack + 8)); // Initial PSP - if (os_flags & 1) { - __set_CONTROL(0x02); // Set Privileged Thread mode & PSP - } else { - __set_CONTROL(0x03); // Set Unprivileged Thread mode & PSP - } - __DSB(); - __ISB(); - break; - case 0x01: // Unprivileged Thread mode & MSP - return osErrorOS; - case 0x02: // Privileged Thread mode & PSP - if ((os_flags & 1) == 0) { // Unprivileged Thread mode requested - __set_CONTROL(0x03); // Set Unprivileged Thread mode & PSP - __DSB(); - __ISB(); - } - break; - case 0x03: // Unprivileged Thread mode & PSP - if (os_flags & 1) return osErrorOS; // Privileged Thread mode requested - break; - } - return __svcKernelStart(); -} - -/// Check if the RTOS kernel is already started -int32_t osKernelRunning(void) { - if ((__get_IPSR() != 0) || ((__get_CONTROL() & 1) == 0)) { - // in ISR or Privileged - return os_running; - } else { - return __svcKernelRunning(); - } -} - - -// ==== Thread Management ==== - -__NO_RETURN void osThreadExit (void); - -// Thread Service Calls declarations -SVC_2_1(svcThreadCreate, osThreadId, osThreadDef_t *, 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) - -// Thread Service Calls -extern OS_TID rt_get_TID (void); -extern void rt_init_context (P_TCB p_TCB, U8 priority, FUNCP task_body); - -/// Create a thread and add it to Active Threads and set it to state READY -osThreadId svcThreadCreate (osThreadDef_t *thread_def, void *argument) { - P_TCB ptcb; - - if ((thread_def == NULL) || - (thread_def->pthread == NULL) || - (thread_def->tpriority < osPriorityIdle) || - (thread_def->tpriority > osPriorityRealtime) || - (thread_def->stacksize == 0) || - (thread_def->stack_pointer == NULL) ) { - sysThreadError(osErrorParameter); - return NULL; - } - - U8 priority = thread_def->tpriority - osPriorityIdle + 1; - P_TCB task_context = &thread_def->tcb; - - /* Utilize the user provided stack. */ - task_context->stack = (U32*)thread_def->stack_pointer; - task_context->priv_stack = thread_def->stacksize; - /* Find a free entry in 'os_active_TCB' table. */ - OS_TID tsk = rt_get_TID (); - os_active_TCB[tsk-1] = task_context; - task_context->task_id = tsk; - /* Pass parameter 'argv' to 'rt_init_context' */ - task_context->msg = argument; - /* Initialize thread context structure, including the thread's stack. */ - rt_init_context (task_context, priority, (FUNCP)thread_def->pthread); - - /* Dispatch this task to the scheduler for execution. */ - DBG_TASK_NOTIFY(task_context, __TRUE); - rt_dispatch (task_context); - - ptcb = (P_TCB)os_active_TCB[tsk - 1]; // TCB pointer - - *((uint32_t *)ptcb->tsk_stack + 13) = (uint32_t)osThreadExit; - - return ptcb; -} - -/// Return the thread ID of the current running thread -osThreadId svcThreadGetId (void) { - OS_TID tsk; - - tsk = rt_tsk_self(); - if (tsk == 0) return NULL; - return (P_TCB)os_active_TCB[tsk - 1]; -} - -/// Terminate execution of a thread and remove it from ActiveThreads -osStatus svcThreadTerminate (osThreadId thread_id) { - OS_RESULT res; - P_TCB ptcb; - - ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer - if (ptcb == NULL) return osErrorParameter; - - res = rt_tsk_delete(ptcb->task_id); // Delete task - - if (res == OS_R_NOK) return osErrorResource; // Delete task failed - - 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 - 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); -} - - -// Thread Public API - -/// Create a thread and add it to Active Threads and set it to state READY -osThreadId osThreadCreate (osThreadDef_t *thread_def, void *argument) { - if (__get_IPSR() != 0) return NULL; // Not allowed in ISR - if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) { - // Privileged and not running - return svcThreadCreate(thread_def, argument); - } else { - return __svcThreadCreate(thread_def, argument); - } -} - -/// Return the thread ID of the current running thread -osThreadId osThreadGetId (void) { - if (__get_IPSR() != 0) return NULL; // Not allowed in ISR - return __svcThreadGetId(); -} - -/// Terminate execution of a thread and remove it from ActiveThreads -osStatus osThreadTerminate (osThreadId thread_id) { - if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR - return __svcThreadTerminate(thread_id); -} - -/// Pass control to next thread that is in state READY -osStatus osThreadYield (void) { - if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR - return __svcThreadYield(); -} - -/// Change priority of an active thread -osStatus osThreadSetPriority (osThreadId thread_id, osPriority priority) { - if (__get_IPSR() != 0) 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_IPSR() != 0) 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) { - __svcThreadTerminate(__svcThreadGetId()); - for (;;); // Should never come here -} - - -// ==== 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 == 0) 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 == 0) { - 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_IPSR() != 0) 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_IPSR() != 0) { // Not allowed in ISR - ret.status = osErrorISR; - return ret; - } - return __svcWait(millisec); -#endif -} - - -// ==== Timer Management ==== - -// Timer definitions -#define osTimerInvalid 0 -#define osTimerStopped 1 -#define osTimerRunning 2 - -// 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 - uint16_t tcnt; // Timer Delay Count - uint16_t icnt; // Timer Initial Count - void *arg; // Timer Function Argument - 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 = (uint16_t)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 int 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, 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 (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->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; - - tcnt = rt_ms2tick(millisec); - if (tcnt == 0) return osErrorValue; - - switch (pt->state) { - case osTimerRunning: - if (rt_timer_remove(pt) != 0) { - return osErrorResource; - } - break; - case osTimerStopped: - pt->state = osTimerRunning; - pt->icnt = (uint16_t)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; -} - -static __INLINE osStatus isrMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec); - -/// Timer Tick (called each SysTick) -void sysTimerTick (void) { - os_timer_cb *pt, *p; - - p = os_timer_head; - if (p == NULL) return; - - p->tcnt--; - while ((p != NULL) && (p->tcnt == 0)) { - pt = p; - p = p->next; - os_timer_head = p; - isrMessagePut(osMessageQId_osTimerMessageQ, (uint32_t)pt, 0); - if (pt->type == osTimerPeriodic) { - rt_timer_insert(pt, pt->icnt); - } else { - pt->state = osTimerStopped; - } - } -} - - -// Timer Management Public API - -/// Create timer -osTimerId osTimerCreate (osTimerDef_t *timer_def, os_timer_type type, void *argument) { - if (__get_IPSR() != 0) return NULL; // Not allowed in ISR - if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) { - // 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_IPSR() != 0) return osErrorISR; // Not allowed in ISR - return __svcTimerStart(timer_id, millisec); -} - -/// Stop timer -osStatus osTimerStop (osTimerId timer_id) { - if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR - return __svcTimerStop(timer_id); -} - -/// Delete timer -osStatus osTimerDelete (osTimerId timer_id) { - if (__get_IPSR() != 0) 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_1_1(svcSignalGet, int32_t, osThreadId, 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 0x80000000; - - if (signals & (0xFFFFFFFF << osFeature_Signals)) return 0x80000000; - - sig = ptcb->events; // Previous signal flags - - rt_evt_set(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 0x80000000; - - if (signals & (0xFFFFFFFF << osFeature_Signals)) return 0x80000000; - - sig = ptcb->events; // Previous signal flags - - rt_evt_clr(signals, ptcb->task_id); // Clear event flags - - return sig; -} - -/// Get Signal Flags status of an active thread -int32_t svcSignalGet (osThreadId thread_id) { - P_TCB ptcb; - - ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer - if (ptcb == NULL) return 0x80000000; - - return ptcb->events; // Return event flags -} - -/// 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 (signals & (0xFFFFFFFF << osFeature_Signals)) { - ret.status = osErrorValue; - return osEvent_ret_status; - } - - if (signals != 0) { // Wait for all specified signals - res = rt_evt_wait(signals, rt_ms2tick(millisec), __TRUE); - } else { // Wait for any signal - res = rt_evt_wait(0xFFFF, rt_ms2tick(millisec), __FALSE); - } - - if (res == OS_R_EVT) { - ret.status = osEventSignal; - ret.value.signals = signals ? signals : os_tsk.run->waits; - } else { - ret.status = millisec ? osEventTimeout : osOK; - ret.value.signals = 0; - } - - return osEvent_ret_value; -} - - -// Signal ISR Calls - -/// Set the specified Signal Flags of an active thread -static __INLINE 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 0x80000000; - - if (signals & (0xFFFFFFFF << osFeature_Signals)) return 0x80000000; - - sig = ptcb->events; // Previous signal flags - - isr_evt_set(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_IPSR() != 0) { // 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_IPSR() != 0) return osErrorISR; // Not allowed in ISR - return __svcSignalClear(thread_id, signals); -} - -/// Get Signal Flags status of an active thread -int32_t osSignalGet (osThreadId thread_id) { - if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR - return __svcSignalGet(thread_id); -} - -/// 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_IPSR() != 0) { // Not allowed in ISR - ret.status = osErrorISR; - return ret; - } - return __svcSignalWait(signals, millisec); -} - - -// ==== Mutex Management ==== - -// Mutex Service Calls declarations -SVC_1_1(svcMutexCreate, osMutexId, 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 (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 != 0) { - 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 ? 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 (osMutexDef_t *mutex_def) { - if (__get_IPSR() != 0) return NULL; // Not allowed in ISR - if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) { - // 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_IPSR() != 0) 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_IPSR() != 0) 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_IPSR() != 0) 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 != 0) { - sysThreadError(osErrorParameter); - return NULL; - } - - if (count > osFeature_Semaphore) { - sysThreadError(osErrorValue); - return NULL; - } - - rt_sem_init(sem, 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 (((P_SCB)sem)->tokens + 1); -} - -/// 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 (((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 -static __INLINE 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 (((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 (osSemaphoreDef_t *semaphore_def, int32_t count) { - if (__get_IPSR() != 0) return NULL; // Not allowed in ISR - if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) { - // 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_IPSR() != 0) return -1; // Not allowed in ISR - return __svcSemaphoreWait(semaphore_id, millisec); -} - -/// Release a Semaphore -osStatus osSemaphoreRelease (osSemaphoreId semaphore_id) { - if (__get_IPSR() != 0) { // 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_IPSR() != 0) 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) { - p = box; - for (n = ((P_BM)box_mem)->blk_size; n; n -= 4) { - *p++ = 0; - } - } -} - -// Memory Management Service Calls declarations -SVC_1_1(svcPoolCreate, osPoolId, const osPoolDef_t *, RET_pointer) -SVC_2_1(sysPoolAlloc, void *, osPoolId, uint32_t, 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 == 0) || - (pool_def->item_sz == 0) || - (pool_def->pool == NULL)) { - sysThreadError(osErrorParameter); - return NULL; - } - - blk_sz = (pool_def->item_sz + 3) & ~3; - - _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, uint32_t clr) { - void *ptr; - - if (pool_id == NULL) return NULL; - - ptr = rt_alloc_box(pool_id); - if (clr) { - rt_clr_box(pool_id, ptr); - } - - return ptr; -} - -/// Return an allocated memory block back to a specific memory pool -osStatus sysPoolFree (osPoolId pool_id, void *block) { - int32_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 (osPoolDef_t *pool_def) { - if (__get_IPSR() != 0) return NULL; // Not allowed in ISR - if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) { - // 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_IPSR() != 0) || ((__get_CONTROL() & 1) == 0)) { // in ISR or Privileged - return sysPoolAlloc(pool_id, 0); - } else { // in Thread - return __sysPoolAlloc(pool_id, 0); - } -} - -/// Allocate a memory block from a memory pool and set memory block to zero -void *osPoolCAlloc (osPoolId pool_id) { - if ((__get_IPSR() != 0) || ((__get_CONTROL() & 1) == 0)) { // in ISR or Privileged - return sysPoolAlloc(pool_id, 1); - } else { // in Thread - return __sysPoolAlloc(pool_id, 1); - } -} - -/// Return an allocated memory block back to a specific memory pool -osStatus osPoolFree (osPoolId pool_id, void *block) { - if ((__get_IPSR() != 0) || ((__get_CONTROL() & 1) == 0)) { // 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, 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 (osMessageQDef_t *queue_def, osThreadId thread_id) { - - if ((queue_def == NULL) || - (queue_def->queue_sz == 0) || - (queue_def->pool == NULL)) { - sysThreadError(osErrorParameter); - return NULL; - } - - if (((P_MCB)queue_def->pool)->cb_type != 0) { - sysThreadError(osErrorParameter); - return NULL; - } - - rt_mbx_init(queue_def->pool, 4*(queue_def->queue_sz + 4)); - - 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 ? 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 ? osEventTimeout : osOK; - return osEvent_ret_value; - } - - ret.status = osEventMessage; - - return osEvent_ret_value; -} - - -// Message Queue ISR Calls - -/// Put a Message to a Queue -static __INLINE osStatus isrMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec) { - - if ((queue_id == NULL) || (millisec != 0)) { - return osErrorParameter; - } - - if (((P_MCB)queue_id)->cb_type != MCB) return osErrorParameter; - - if (rt_mbx_check(queue_id) == 0) { // 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 -static __INLINE os_InRegs osEvent isrMessageGet (osMessageQId queue_id, uint32_t millisec) { - OS_RESULT res; - osEvent ret; - - if ((queue_id == NULL) || (millisec != 0)) { - 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 (osMessageQDef_t *queue_def, osThreadId thread_id) { - if (__get_IPSR() != 0) return NULL; // Not allowed in ISR - if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) { - // 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_IPSR() != 0) { // 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_IPSR() != 0) { // 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, osMailQDef_t *, osThreadId, RET_pointer) -SVC_4_1(sysMailAlloc, void *, osMailQId, uint32_t, 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 (osMailQDef_t *queue_def, osThreadId thread_id) { - uint32_t blk_sz; - P_MCB pmcb; - void *pool; - - if ((queue_def == NULL) || - (queue_def->queue_sz == 0) || - (queue_def->item_sz == 0) || - (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 != 0)) { - sysThreadError(osErrorParameter); - return NULL; - } - - blk_sz = (queue_def->item_sz + 3) & ~3; - - _init_box(pool, sizeof(struct OS_BM) + queue_def->queue_sz * blk_sz, blk_sz); - - rt_mbx_init(pmcb, 4*(queue_def->queue_sz + 4)); - - - return queue_def->pool; -} - -/// Allocate a memory block from a mail -void *sysMailAlloc (osMailQId queue_id, uint32_t millisec, uint32_t isr, uint32_t clr) { - 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 && (millisec != 0)) return NULL; - - mem = rt_alloc_box(pool); - if (clr) { - rt_clr_box(pool, mem); - } - - if ((mem == NULL) && (millisec != 0)) { - // 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 = 3; - } - 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; - int32_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 != 0) return osErrorValue; - - if (pmcb->state == 3) { - // Task is waiting to allocate a message - if (isr) { - 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); - if (pmcb->p_lnk == NULL) { - pmcb->state = 0; - } - 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 (osMailQDef_t *queue_def, osThreadId thread_id) { - if (__get_IPSR() != 0) return NULL; // Not allowed in ISR - if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) { - // 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_IPSR() != 0) { // in ISR - return sysMailAlloc(queue_id, millisec, 1, 0); - } else { // in Thread - return __sysMailAlloc(queue_id, millisec, 0, 0); - } -} - -/// Allocate a memory block from a mail and set memory block to zero -void *osMailCAlloc (osMailQId queue_id, uint32_t millisec) { - if (__get_IPSR() != 0) { // in ISR - return sysMailAlloc(queue_id, millisec, 1, 1); - } else { // in Thread - return __sysMailAlloc(queue_id, millisec, 0, 1); - } -} - -/// Free a memory block from a mail -osStatus osMailFree (osMailQId queue_id, void *mail) { - if (__get_IPSR() != 0) { // in ISR - return sysMailFree(queue_id, mail, 1); - } else { // in Thread - return __sysMailFree(queue_id, mail, 0); - } -} - -/// 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, 0); -} - -#ifdef __CC_ARM -#pragma push -#pragma Ospace -#endif // __arm__ -/// 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; -} -#ifdef __CC_ARM -#pragma pop -#endif // __arm__ |