75381150fd
Formatting Pass 1 Lots of fixups to adding stdint and stdbool all over the place Formatting Pass 2 Formatting Pass 3 Formatting Pass 4 Update app_bt_stream.cpp
2154 lines
62 KiB
C
2154 lines
62 KiB
C
/*----------------------------------------------------------------------------
|
|
* 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
|
|
|
|
#include "cmsis.h"
|
|
|
|
#include "RTX_Config.h"
|
|
#include "rt_Event.h"
|
|
#include "rt_HAL_CM.h"
|
|
#include "rt_List.h"
|
|
#include "rt_Mailbox.h"
|
|
#include "rt_MemBox.h"
|
|
#include "rt_Memory.h"
|
|
#include "rt_Mutex.h"
|
|
#include "rt_Semaphore.h"
|
|
#include "rt_System.h"
|
|
#include "rt_Task.h"
|
|
#include "rt_Time.h"
|
|
#include "rt_TypeDef.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_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 }
|
|
|
|
#if defined(__ARM_PCS_VFP)
|
|
|
|
#define osEvent_type void
|
|
#define osEvent_ret_status \
|
|
{ \
|
|
__asm("MOV r0, %0;" \
|
|
: /* no outputs */ \
|
|
: "r"(ret.status) \
|
|
: "r0"); \
|
|
}
|
|
#define osEvent_ret_value \
|
|
{ \
|
|
__asm("MOV r1, %0;" \
|
|
"MOV r0, %1;" \
|
|
: /* no outputs */ \
|
|
: "r"(ret.value.v), "r"(ret.status) \
|
|
: "r0", "r1"); \
|
|
}
|
|
#define osEvent_ret_msg \
|
|
{ \
|
|
__asm("MOV r2, %0;" \
|
|
"MOV r1, %1;" \
|
|
"MOV r0, %2;" \
|
|
: /* no outputs */ \
|
|
: "r"(ret.def.message_id), "r"(ret.value.v), "r"(ret.status) \
|
|
: "r0", "r1", "r2"); \
|
|
}
|
|
|
|
#define osEvent_ret_mail \
|
|
{ \
|
|
__asm("MOV r2, %0;" \
|
|
"MOV r1, %1;" \
|
|
"MOV r0, %2;" \
|
|
: /* no outputs */ \
|
|
: "r"(ret.def.mail_id), "r"(ret.value.v), "r"(ret.status) \
|
|
: "r0", "r1", "r2"); \
|
|
}
|
|
|
|
#define osCallback_type void
|
|
#define osCallback_ret \
|
|
{ \
|
|
__asm("MOV r1, %0;" \
|
|
"MOV r0, %1;" \
|
|
: /* no outputs */ \
|
|
: "r"(ret.arg), "r"(ret.fp) \
|
|
: "r0", "r1"); \
|
|
}
|
|
|
|
#else /* defined (__ARM_PCS_VFP) */
|
|
|
|
#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 }
|
|
|
|
#endif /* defined (__ARM_PCS_VFP) */
|
|
|
|
#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))
|
|
#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 RET_osEvent "=r"(ret.status), "=r"(ret.value), "=r"(ret.def)
|
|
#define RET_osCallback "=r"(ret.fp), "=r"(ret.arg)
|
|
|
|
#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 uint64_t
|
|
#define osCallback_ret ((uint64_t)ret.fp | ((uint64_t)ret.arg) << 32)
|
|
|
|
#define SVC_Setup(f) __asm("mov r12,%0\n" ::"r"(&f) : "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_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(f, t, t1, rr) \
|
|
uint64_t f(t1 a1); \
|
|
_Pragma("swi_number=0") __swi uint64_t _##f(t1 a1); \
|
|
static inline t __##f(t1 a1) { \
|
|
t ret; \
|
|
SVC_Setup(f); \
|
|
_##f(a1); \
|
|
__asm("" : rr : :); \
|
|
return ret; \
|
|
}
|
|
|
|
#define SVC_1_3(f, t, t1, rr) \
|
|
t f(t1 a1); \
|
|
void f##_(t1 a1) { \
|
|
f(a1); \
|
|
SVC_Ret3(); \
|
|
} \
|
|
_Pragma("swi_number=0") __swi void _##f(t1 a1); \
|
|
static inline t __##f(t1 a1) { \
|
|
t ret; \
|
|
SVC_Setup(f##_); \
|
|
_##f(a1); \
|
|
__asm("" : rr : :); \
|
|
return ret; \
|
|
}
|
|
|
|
#define SVC_2_3(f, t, t1, t2, rr) \
|
|
t f(t1 a1, t2 a2); \
|
|
void f##_(t1 a1, t2 a2) { \
|
|
f(a1, a2); \
|
|
SVC_Ret3(); \
|
|
} \
|
|
_Pragma("swi_number=0") __swi void _##f(t1 a1, t2 a2); \
|
|
static inline t __##f(t1 a1, t2 a2) { \
|
|
t ret; \
|
|
SVC_Setup(f##_); \
|
|
_##f(a1, a2); \
|
|
__asm("" : rr : :); \
|
|
return ret; \
|
|
}
|
|
|
|
#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_CA9
|
|
// 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;
|
|
|
|
extern U32 IRQNestLevel; /* Indicates whether inside an ISR, and the depth of
|
|
nesting. 0 = not in ISR. */
|
|
|
|
// ==== 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;
|
|
}
|
|
|
|
// === Helper functions for system call interface ===
|
|
|
|
static __inline char __get_mode(void) { return (char)(__get_CPSR() & 0x1f); }
|
|
|
|
static __inline char __exceptional_mode(void) {
|
|
switch (__get_mode()) {
|
|
case MODE_USR:
|
|
case MODE_SYS:
|
|
return 0;
|
|
case MODE_SVC:
|
|
if (IRQNestLevel == 0)
|
|
return 0; /* handling a regular service call */
|
|
else
|
|
return 1; /* handling an ISR in SVC mode */
|
|
default:
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
// ==== 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)
|
|
|
|
static void sysThreadError(osStatus status);
|
|
osThreadId svcThreadCreate(const osThreadDef_t *thread_def, void *argument);
|
|
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_CA9
|
|
if (!os_initialized) {
|
|
rt_sys_init(); // RTX System Initialization
|
|
}
|
|
#else
|
|
int ret;
|
|
|
|
if (!os_initialized) {
|
|
|
|
// Init Thread Stack Memory (must be 8-byte aligned)
|
|
if ((uint32_t)os_stack_mem & 7)
|
|
return osErrorNoMemory;
|
|
ret = rt_init_mem(os_stack_mem, os_stack_sz);
|
|
if (ret != 0)
|
|
return osErrorNoMemory;
|
|
|
|
rt_sys_init(); // RTX System Initialization
|
|
}
|
|
#endif
|
|
|
|
os_tsk.run->prio = 255; // Highest priority
|
|
|
|
if (!os_initialized) {
|
|
// Create OS Timers resources (Message Queue & Thread)
|
|
osMessageQId_osTimerMessageQ =
|
|
svcMessageCreate(&os_messageQ_def_osTimerMessageQ, NULL);
|
|
osThreadId_osTimerThread =
|
|
svcThreadCreate(&os_thread_def_osTimerThread, NULL);
|
|
}
|
|
|
|
sysThreadError(osOK);
|
|
|
|
os_initialized = 1;
|
|
|
|
return osOK;
|
|
}
|
|
|
|
/// Start the RTOS Kernel
|
|
osStatus svcKernelStart(void) {
|
|
|
|
if (os_running)
|
|
return osOK;
|
|
|
|
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 (__exceptional_mode())
|
|
return osErrorISR; // Not allowed in ISR
|
|
if (__get_mode() != MODE_USR) {
|
|
return svcKernelInitialize();
|
|
} else {
|
|
return __svcKernelInitialize();
|
|
}
|
|
}
|
|
|
|
/// Start the RTOS Kernel
|
|
osStatus osKernelStart(void) {
|
|
char mode = __get_mode();
|
|
|
|
switch (mode) {
|
|
case MODE_USR:
|
|
if (os_flags & 1)
|
|
return osErrorOS; // Privileged Thread mode requested from Unprivileged
|
|
break;
|
|
case MODE_SYS:
|
|
if (!(os_flags & 1)) {
|
|
__set_CPS_USR();
|
|
}
|
|
break;
|
|
default:
|
|
return osErrorISR; // Not allowed in ISR
|
|
}
|
|
return __svcKernelStart();
|
|
}
|
|
|
|
/// Check if the RTOS kernel is already started
|
|
int32_t osKernelRunning(void) {
|
|
if (__get_mode() != MODE_USR) {
|
|
return os_running;
|
|
} else {
|
|
return __svcKernelRunning();
|
|
}
|
|
}
|
|
|
|
// ==== 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_2_1(svcThreadCreate, osThreadId, const 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
|
|
|
|
/// Create a thread and add it to Active Threads and set it to state READY
|
|
osThreadId svcThreadCreate(const osThreadDef_t *thread_def, void *argument) {
|
|
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_CA9
|
|
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
|
|
(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 == 0) { // Invalid task ID
|
|
#ifndef __MBED_CMSIS_RTOS_CA9
|
|
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 - 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;
|
|
#ifndef __MBED_CMSIS_RTOS_CA9
|
|
void *stk;
|
|
#endif
|
|
|
|
ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
|
|
if (ptcb == NULL)
|
|
return osErrorParameter;
|
|
|
|
#ifndef __MBED_CMSIS_RTOS_CA9
|
|
stk = ptcb->priv_stack ? ptcb->stack : NULL; // Private stack
|
|
#endif
|
|
|
|
res = rt_tsk_delete(ptcb->task_id); // Delete task
|
|
|
|
if (res == OS_R_NOK)
|
|
return osErrorResource; // Delete task failed
|
|
|
|
#ifndef __MBED_CMSIS_RTOS_CA9
|
|
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
|
|
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(const osThreadDef_t *thread_def, void *argument) {
|
|
if (__exceptional_mode())
|
|
return NULL; // Not allowed in ISR
|
|
if ((__get_mode() != MODE_USR) && (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 (__exceptional_mode())
|
|
return NULL; // Not allowed in ISR
|
|
return __svcThreadGetId();
|
|
}
|
|
|
|
/// Terminate execution of a thread and remove it from ActiveThreads
|
|
osStatus osThreadTerminate(osThreadId thread_id) {
|
|
if (__exceptional_mode())
|
|
return osErrorISR; // Not allowed in ISR
|
|
return __svcThreadTerminate(thread_id);
|
|
}
|
|
|
|
/// Pass control to next thread that is in state READY
|
|
osStatus osThreadYield(void) {
|
|
if (__exceptional_mode())
|
|
return osErrorISR; // Not allowed in ISR
|
|
return __svcThreadYield();
|
|
}
|
|
|
|
/// Change priority of an active thread
|
|
osStatus osThreadSetPriority(osThreadId thread_id, osPriority priority) {
|
|
if (__exceptional_mode())
|
|
return osErrorISR; // Not allowed in ISR
|
|
return __svcThreadSetPriority(thread_id, priority);
|
|
}
|
|
|
|
/// Get current priority of an active thread
|
|
osPriority osThreadGetPriority(osThreadId thread_id) {
|
|
if (__exceptional_mode())
|
|
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
|
|
}
|
|
|
|
#ifdef __MBED_CMSIS_RTOS_CA9
|
|
/// Get current thread state
|
|
uint8_t osThreadGetState(osThreadId thread_id) {
|
|
P_TCB ptcb;
|
|
|
|
if (__exceptional_mode())
|
|
return osErrorISR; // Not allowed in ISR
|
|
|
|
ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
|
|
if (ptcb == NULL)
|
|
return osErrorParameter;
|
|
|
|
return ptcb->state;
|
|
}
|
|
#endif
|
|
|
|
// ==== 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;
|
|
#if defined(__GNUC__) && defined(__ARM_PCS_VFP)
|
|
osEvent_ret_status;
|
|
return;
|
|
#else
|
|
return osEvent_ret_status;
|
|
#endif
|
|
}
|
|
|
|
/* To Do: osEventSignal, osEventMessage, osEventMail */
|
|
rt_dly_wait(rt_ms2tick(millisec));
|
|
ret.status = osEventTimeout;
|
|
|
|
#if defined(__GNUC__) && defined(__ARM_PCS_VFP)
|
|
osEvent_ret_status;
|
|
return;
|
|
#else
|
|
return osEvent_ret_status;
|
|
#endif
|
|
}
|
|
#endif
|
|
|
|
// Generic Wait API
|
|
|
|
/// Wait for Timeout (Time Delay)
|
|
osStatus osDelay(uint32_t millisec) {
|
|
if (__exceptional_mode())
|
|
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 (__exceptional_mode()) { // 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
|
|
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 = (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, 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->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;
|
|
#if defined(__GNUC__) && defined(__ARM_PCS_VFP)
|
|
osCallback_ret;
|
|
return;
|
|
#else
|
|
return osCallback_ret;
|
|
#endif
|
|
}
|
|
|
|
ret.fp = (void *)pt->timer->ptimer;
|
|
ret.arg = pt->arg;
|
|
|
|
#if defined(__GNUC__) && defined(__ARM_PCS_VFP)
|
|
osCallback_ret;
|
|
return;
|
|
#else
|
|
return osCallback_ret;
|
|
#endif
|
|
}
|
|
|
|
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(const osTimerDef_t *timer_def, os_timer_type type,
|
|
void *argument) {
|
|
if (__exceptional_mode())
|
|
return NULL; // Not allowed in ISR
|
|
if ((__get_mode() != MODE_USR) && (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 (__exceptional_mode())
|
|
return osErrorISR; // Not allowed in ISR
|
|
return __svcTimerStart(timer_id, millisec);
|
|
}
|
|
|
|
/// Stop timer
|
|
osStatus osTimerStop(osTimerId timer_id) {
|
|
if (__exceptional_mode())
|
|
return osErrorISR; // Not allowed in ISR
|
|
return __svcTimerStop(timer_id);
|
|
}
|
|
|
|
/// Delete timer
|
|
osStatus osTimerDelete(osTimerId timer_id) {
|
|
if (__exceptional_mode())
|
|
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;
|
|
#if defined(__GNUC__) && defined(__ARM_PCS_VFP)
|
|
osEvent_ret_status;
|
|
return;
|
|
#else
|
|
return osEvent_ret_status;
|
|
#endif
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
#if defined(__GNUC__) && defined(__ARM_PCS_VFP)
|
|
osEvent_ret_value;
|
|
return;
|
|
#else
|
|
return osEvent_ret_value;
|
|
#endif
|
|
}
|
|
|
|
// 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 (__exceptional_mode()) { // 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 (__exceptional_mode())
|
|
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 (__exceptional_mode())
|
|
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 (__exceptional_mode()) { // 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 != 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(const osMutexDef_t *mutex_def) {
|
|
if (__exceptional_mode())
|
|
return NULL; // Not allowed in ISR
|
|
if ((__get_mode() != MODE_USR) && (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 (__exceptional_mode())
|
|
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 (__exceptional_mode())
|
|
return osErrorISR; // Not allowed in ISR
|
|
return __svcMutexRelease(mutex_id);
|
|
}
|
|
|
|
/// Delete a Mutex that was created by osMutexCreate
|
|
osStatus osMutexDelete(osMutexId mutex_id) {
|
|
if (__exceptional_mode())
|
|
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(const osSemaphoreDef_t *semaphore_def,
|
|
int32_t count) {
|
|
if (__exceptional_mode())
|
|
return NULL; // Not allowed in ISR
|
|
if ((__get_mode() != MODE_USR) && (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 (__exceptional_mode())
|
|
return -1; // Not allowed in ISR
|
|
return __svcSemaphoreWait(semaphore_id, millisec);
|
|
}
|
|
|
|
/// Release a Semaphore
|
|
osStatus osSemaphoreRelease(osSemaphoreId semaphore_id) {
|
|
if (__exceptional_mode()) { // 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 (__exceptional_mode())
|
|
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(const osPoolDef_t *pool_def) {
|
|
if (__exceptional_mode())
|
|
return NULL; // Not allowed in ISR
|
|
if ((__get_mode() != MODE_USR) && (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_mode() != MODE_USR) { // 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_mode() != MODE_USR) { // 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_mode() != MODE_USR) { // 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 == 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;
|
|
#if defined(__GNUC__) && defined(__ARM_PCS_VFP)
|
|
osEvent_ret_status;
|
|
return;
|
|
#else
|
|
return osEvent_ret_status;
|
|
#endif
|
|
}
|
|
|
|
if (((P_MCB)queue_id)->cb_type != MCB) {
|
|
ret.status = osErrorParameter;
|
|
#if defined(__GNUC__) && defined(__ARM_PCS_VFP)
|
|
osEvent_ret_status;
|
|
return;
|
|
#else
|
|
return osEvent_ret_status;
|
|
#endif
|
|
}
|
|
|
|
res = rt_mbx_wait(queue_id, &ret.value.p, rt_ms2tick(millisec));
|
|
|
|
if (res == OS_R_TMO) {
|
|
ret.status = millisec ? osEventTimeout : osOK;
|
|
#if defined(__GNUC__) && defined(__ARM_PCS_VFP)
|
|
osEvent_ret_value;
|
|
return;
|
|
#else
|
|
return osEvent_ret_value;
|
|
#endif
|
|
}
|
|
|
|
ret.status = osEventMessage;
|
|
|
|
#if defined(__GNUC__) && defined(__ARM_PCS_VFP)
|
|
osEvent_ret_value;
|
|
return;
|
|
#else
|
|
return osEvent_ret_value;
|
|
#endif
|
|
}
|
|
|
|
// 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(const osMessageQDef_t *queue_def,
|
|
osThreadId thread_id) {
|
|
if (__exceptional_mode())
|
|
return NULL; // Not allowed in ISR
|
|
if ((__get_mode() != MODE_USR) && (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 (__exceptional_mode()) { // 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 (__exceptional_mode()) { // 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_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(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 == 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(const osMailQDef_t *queue_def, osThreadId thread_id) {
|
|
if (__exceptional_mode())
|
|
return NULL; // Not allowed in ISR
|
|
if ((__get_mode() != MODE_USR) && (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 (__exceptional_mode()) { // 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 (__exceptional_mode()) { // 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 (__exceptional_mode()) { // 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__
|