mbed-rtos - updated

Users » lvwei1990 » Code » mbed-rtos

updated

Dependents: NSDL_HelloWorld_WiFi_K64_test

Fork of mbed-rtos by ARM Shanghai IoT Team (Internal)

rtx/TARGET_CORTEX_M/rt_CMSIS.c@56:703a0ea0ca4d, 2015-01-16 (annotated)

Committer:: lvwei1990
Date:: Fri Jan 16 06:35:14 2015 +0000
Revision:: 56:703a0ea0ca4d
Parent:: 49:77c8e4604045
Child:: 53:c35dab33c427

Demo for the problem with nsdl_lib and rtos on FRDM- K64F

Who changed what in which revision?

User	Revision	Line number	New contents of line
mbed_official	49:77c8e4604045	1	/*----------------------------------------------------------------------------
mbed_official	49:77c8e4604045	2	* RL-ARM - RTX
mbed_official	49:77c8e4604045	3	*----------------------------------------------------------------------------
mbed_official	49:77c8e4604045	4	* Name: rt_CMSIS.c
mbed_official	49:77c8e4604045	5	* Purpose: CMSIS RTOS API
mbed_official	49:77c8e4604045	6	* Rev.: V4.60
mbed_official	49:77c8e4604045	7	*----------------------------------------------------------------------------
mbed_official	49:77c8e4604045	8	*
mbed_official	49:77c8e4604045	9	* Copyright (c) 1999-2009 KEIL, 2009-2012 ARM Germany GmbH
mbed_official	49:77c8e4604045	10	* All rights reserved.
mbed_official	49:77c8e4604045	11	* Redistribution and use in source and binary forms, with or without
mbed_official	49:77c8e4604045	12	* modification, are permitted provided that the following conditions are met:
mbed_official	49:77c8e4604045	13	* - Redistributions of source code must retain the above copyright
mbed_official	49:77c8e4604045	14	* notice, this list of conditions and the following disclaimer.
mbed_official	49:77c8e4604045	15	* - Redistributions in binary form must reproduce the above copyright
mbed_official	49:77c8e4604045	16	* notice, this list of conditions and the following disclaimer in the
mbed_official	49:77c8e4604045	17	* documentation and/or other materials provided with the distribution.
mbed_official	49:77c8e4604045	18	* - Neither the name of ARM nor the names of its contributors may be used
mbed_official	49:77c8e4604045	19	* to endorse or promote products derived from this software without
mbed_official	49:77c8e4604045	20	* specific prior written permission.
mbed_official	49:77c8e4604045	21	*
mbed_official	49:77c8e4604045	22	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
mbed_official	49:77c8e4604045	23	* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
mbed_official	49:77c8e4604045	24	* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
mbed_official	49:77c8e4604045	25	* ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
mbed_official	49:77c8e4604045	26	* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
mbed_official	49:77c8e4604045	27	* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
mbed_official	49:77c8e4604045	28	* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
mbed_official	49:77c8e4604045	29	* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
mbed_official	49:77c8e4604045	30	* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
mbed_official	49:77c8e4604045	31	* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
mbed_official	49:77c8e4604045	32	* POSSIBILITY OF SUCH DAMAGE.
mbed_official	49:77c8e4604045	33	---------------------------------------------------------------------------/
mbed_official	49:77c8e4604045	34
mbed_official	49:77c8e4604045	35	#define __CMSIS_GENERIC
mbed_official	49:77c8e4604045	36
mbed_official	49:77c8e4604045	37	#if defined (__CORTEX_M4) \|\| defined (__CORTEX_M4F)
mbed_official	49:77c8e4604045	38	#include "core_cm4.h"
mbed_official	49:77c8e4604045	39	#elif defined (__CORTEX_M3)
mbed_official	49:77c8e4604045	40	#include "core_cm3.h"
mbed_official	49:77c8e4604045	41	#elif defined (__CORTEX_M0)
mbed_official	49:77c8e4604045	42	#include "core_cm0.h"
mbed_official	49:77c8e4604045	43	#elif defined (__CORTEX_M0PLUS)
mbed_official	49:77c8e4604045	44	#include "core_cm0plus.h"
mbed_official	49:77c8e4604045	45	#else
mbed_official	49:77c8e4604045	46	#error "Missing __CORTEX_Mx definition"
mbed_official	49:77c8e4604045	47	#endif
mbed_official	49:77c8e4604045	48
mbed_official	49:77c8e4604045	49	#include "rt_TypeDef.h"
mbed_official	49:77c8e4604045	50	#include "RTX_Conf.h"
mbed_official	49:77c8e4604045	51	#include "rt_System.h"
mbed_official	49:77c8e4604045	52	#include "rt_Task.h"
mbed_official	49:77c8e4604045	53	#include "rt_Event.h"
mbed_official	49:77c8e4604045	54	#include "rt_List.h"
mbed_official	49:77c8e4604045	55	#include "rt_Time.h"
mbed_official	49:77c8e4604045	56	#include "rt_Mutex.h"
mbed_official	49:77c8e4604045	57	#include "rt_Semaphore.h"
mbed_official	49:77c8e4604045	58	#include "rt_Mailbox.h"
mbed_official	49:77c8e4604045	59	#include "rt_MemBox.h"
mbed_official	49:77c8e4604045	60	#include "rt_HAL_CM.h"
mbed_official	49:77c8e4604045	61
mbed_official	49:77c8e4604045	62	#define os_thread_cb OS_TCB
mbed_official	49:77c8e4604045	63
mbed_official	49:77c8e4604045	64	#include "cmsis_os.h"
mbed_official	49:77c8e4604045	65
mbed_official	49:77c8e4604045	66	#if (osFeature_Signals != 16)
mbed_official	49:77c8e4604045	67	#error Invalid "osFeature_Signals" value!
mbed_official	49:77c8e4604045	68	#endif
mbed_official	49:77c8e4604045	69	#if (osFeature_Semaphore > 65535)
mbed_official	49:77c8e4604045	70	#error Invalid "osFeature_Semaphore" value!
mbed_official	49:77c8e4604045	71	#endif
mbed_official	49:77c8e4604045	72	#if (osFeature_Wait != 0)
mbed_official	49:77c8e4604045	73	#error osWait not supported!
mbed_official	49:77c8e4604045	74	#endif
mbed_official	49:77c8e4604045	75
mbed_official	49:77c8e4604045	76
mbed_official	49:77c8e4604045	77	// ==== Enumeration, structures, defines ====
mbed_official	49:77c8e4604045	78
mbed_official	49:77c8e4604045	79	// Service Calls defines
mbed_official	49:77c8e4604045	80
mbed_official	49:77c8e4604045	81	#if defined (__CC_ARM) /* ARM Compiler */
mbed_official	49:77c8e4604045	82
mbed_official	49:77c8e4604045	83	#define __NO_RETURN __declspec(noreturn)
mbed_official	49:77c8e4604045	84
mbed_official	49:77c8e4604045	85	#define osEvent_type osEvent
mbed_official	49:77c8e4604045	86	#define osEvent_ret_status ret
mbed_official	49:77c8e4604045	87	#define osEvent_ret_value ret
mbed_official	49:77c8e4604045	88	#define osEvent_ret_msg ret
mbed_official	49:77c8e4604045	89	#define osEvent_ret_mail ret
mbed_official	49:77c8e4604045	90
mbed_official	49:77c8e4604045	91	#define osCallback_type osCallback
mbed_official	49:77c8e4604045	92	#define osCallback_ret ret
mbed_official	49:77c8e4604045	93
mbed_official	49:77c8e4604045	94	#define SVC_0_1(f,t,...) \
mbed_official	49:77c8e4604045	95	__svc_indirect(0) t _##f (t(*)()); \
mbed_official	49:77c8e4604045	96	t f (void); \
mbed_official	49:77c8e4604045	97	__attribute__((always_inline)) \
mbed_official	49:77c8e4604045	98	static __inline t __##f (void) { \
mbed_official	49:77c8e4604045	99	return _##f(f); \
mbed_official	49:77c8e4604045	100	}
mbed_official	49:77c8e4604045	101
mbed_official	49:77c8e4604045	102	#define SVC_1_1(f,t,t1,...) \
mbed_official	49:77c8e4604045	103	__svc_indirect(0) t _##f (t(*)(t1),t1); \
mbed_official	49:77c8e4604045	104	t f (t1 a1); \
mbed_official	49:77c8e4604045	105	__attribute__((always_inline)) \
mbed_official	49:77c8e4604045	106	static __inline t __##f (t1 a1) { \
mbed_official	49:77c8e4604045	107	return _##f(f,a1); \
mbed_official	49:77c8e4604045	108	}
mbed_official	49:77c8e4604045	109
mbed_official	49:77c8e4604045	110	#define SVC_2_1(f,t,t1,t2,...) \
mbed_official	49:77c8e4604045	111	__svc_indirect(0) t _##f (t(*)(t1,t2),t1,t2); \
mbed_official	49:77c8e4604045	112	t f (t1 a1, t2 a2); \
mbed_official	49:77c8e4604045	113	__attribute__((always_inline)) \
mbed_official	49:77c8e4604045	114	static __inline t __##f (t1 a1, t2 a2) { \
mbed_official	49:77c8e4604045	115	return _##f(f,a1,a2); \
mbed_official	49:77c8e4604045	116	}
mbed_official	49:77c8e4604045	117
mbed_official	49:77c8e4604045	118	#define SVC_3_1(f,t,t1,t2,t3,...) \
mbed_official	49:77c8e4604045	119	__svc_indirect(0) t _##f (t(*)(t1,t2,t3),t1,t2,t3); \
mbed_official	49:77c8e4604045	120	t f (t1 a1, t2 a2, t3 a3); \
mbed_official	49:77c8e4604045	121	__attribute__((always_inline)) \
mbed_official	49:77c8e4604045	122	static __inline t __##f (t1 a1, t2 a2, t3 a3) { \
mbed_official	49:77c8e4604045	123	return _##f(f,a1,a2,a3); \
mbed_official	49:77c8e4604045	124	}
mbed_official	49:77c8e4604045	125
mbed_official	49:77c8e4604045	126	#define SVC_4_1(f,t,t1,t2,t3,t4,...) \
mbed_official	49:77c8e4604045	127	__svc_indirect(0) t _##f (t(*)(t1,t2,t3,t4),t1,t2,t3,t4); \
mbed_official	49:77c8e4604045	128	t f (t1 a1, t2 a2, t3 a3, t4 a4); \
mbed_official	49:77c8e4604045	129	__attribute__((always_inline)) \
mbed_official	49:77c8e4604045	130	static __inline t __##f (t1 a1, t2 a2, t3 a3, t4 a4) { \
mbed_official	49:77c8e4604045	131	return _##f(f,a1,a2,a3,a4); \
mbed_official	49:77c8e4604045	132	}
mbed_official	49:77c8e4604045	133
mbed_official	49:77c8e4604045	134	#define SVC_1_2 SVC_1_1
mbed_official	49:77c8e4604045	135	#define SVC_1_3 SVC_1_1
mbed_official	49:77c8e4604045	136	#define SVC_2_3 SVC_2_1
mbed_official	49:77c8e4604045	137
mbed_official	49:77c8e4604045	138	#elif defined (__GNUC__) /* GNU Compiler */
mbed_official	49:77c8e4604045	139
mbed_official	49:77c8e4604045	140	#define __NO_RETURN __attribute__((noreturn))
mbed_official	49:77c8e4604045	141
mbed_official	49:77c8e4604045	142	typedef uint32_t __attribute__((vector_size(8))) ret64;
mbed_official	49:77c8e4604045	143	typedef uint32_t __attribute__((vector_size(16))) ret128;
mbed_official	49:77c8e4604045	144
mbed_official	49:77c8e4604045	145	#define RET_pointer __r0
mbed_official	49:77c8e4604045	146	#define RET_int32_t __r0
mbed_official	49:77c8e4604045	147	#define RET_osStatus __r0
mbed_official	49:77c8e4604045	148	#define RET_osPriority __r0
mbed_official	49:77c8e4604045	149	#define RET_osEvent {(osStatus)__r0, {(uint32_t)__r1}, {(void *)__r2}}
mbed_official	49:77c8e4604045	150	#define RET_osCallback {(void )__r0, (void )__r1}
mbed_official	49:77c8e4604045	151
mbed_official	49:77c8e4604045	152	#define osEvent_type ret128
mbed_official	49:77c8e4604045	153	#define osEvent_ret_status (ret128){ret.status}
mbed_official	49:77c8e4604045	154	#define osEvent_ret_value (ret128){ret.status, ret.value.v}
mbed_official	49:77c8e4604045	155	#define osEvent_ret_msg (ret128){ret.status, ret.value.v, (uint32_t)ret.def.message_id}
mbed_official	49:77c8e4604045	156	#define osEvent_ret_mail (ret128){ret.status, ret.value.v, (uint32_t)ret.def.mail_id}
mbed_official	49:77c8e4604045	157
mbed_official	49:77c8e4604045	158	#define osCallback_type ret64
mbed_official	49:77c8e4604045	159	#define osCallback_ret (ret64) {(uint32_t)ret.fp, (uint32_t)ret.arg}
mbed_official	49:77c8e4604045	160
mbed_official	49:77c8e4604045	161	#define SVC_ArgN(n) \
mbed_official	49:77c8e4604045	162	register int __r##n __asm("r"#n);
mbed_official	49:77c8e4604045	163
mbed_official	49:77c8e4604045	164	#define SVC_ArgR(n,t,a) \
mbed_official	49:77c8e4604045	165	register t __r##n __asm("r"#n) = a;
mbed_official	49:77c8e4604045	166
mbed_official	49:77c8e4604045	167	#define SVC_Arg0() \
mbed_official	49:77c8e4604045	168	SVC_ArgN(0) \
mbed_official	49:77c8e4604045	169	SVC_ArgN(1) \
mbed_official	49:77c8e4604045	170	SVC_ArgN(2) \
mbed_official	49:77c8e4604045	171	SVC_ArgN(3)
mbed_official	49:77c8e4604045	172
mbed_official	49:77c8e4604045	173	#define SVC_Arg1(t1) \
mbed_official	49:77c8e4604045	174	SVC_ArgR(0,t1,a1) \
mbed_official	49:77c8e4604045	175	SVC_ArgN(1) \
mbed_official	49:77c8e4604045	176	SVC_ArgN(2) \
mbed_official	49:77c8e4604045	177	SVC_ArgN(3)
mbed_official	49:77c8e4604045	178
mbed_official	49:77c8e4604045	179	#define SVC_Arg2(t1,t2) \
mbed_official	49:77c8e4604045	180	SVC_ArgR(0,t1,a1) \
mbed_official	49:77c8e4604045	181	SVC_ArgR(1,t2,a2) \
mbed_official	49:77c8e4604045	182	SVC_ArgN(2) \
mbed_official	49:77c8e4604045	183	SVC_ArgN(3)
mbed_official	49:77c8e4604045	184
mbed_official	49:77c8e4604045	185	#define SVC_Arg3(t1,t2,t3) \
mbed_official	49:77c8e4604045	186	SVC_ArgR(0,t1,a1) \
mbed_official	49:77c8e4604045	187	SVC_ArgR(1,t2,a2) \
mbed_official	49:77c8e4604045	188	SVC_ArgR(2,t3,a3) \
mbed_official	49:77c8e4604045	189	SVC_ArgN(3)
mbed_official	49:77c8e4604045	190
mbed_official	49:77c8e4604045	191	#define SVC_Arg4(t1,t2,t3,t4) \
mbed_official	49:77c8e4604045	192	SVC_ArgR(0,t1,a1) \
mbed_official	49:77c8e4604045	193	SVC_ArgR(1,t2,a2) \
mbed_official	49:77c8e4604045	194	SVC_ArgR(2,t3,a3) \
mbed_official	49:77c8e4604045	195	SVC_ArgR(3,t4,a4)
mbed_official	49:77c8e4604045	196
mbed_official	49:77c8e4604045	197	#if (defined (__CORTEX_M0)) \|\| defined (__CORTEX_M0PLUS)
mbed_official	49:77c8e4604045	198	#define SVC_Call(f) \
mbed_official	49:77c8e4604045	199	__asm volatile \
mbed_official	49:77c8e4604045	200	( \
mbed_official	49:77c8e4604045	201	"ldr r7,="#f"\n\t" \
mbed_official	49:77c8e4604045	202	"mov r12,r7\n\t" \
mbed_official	49:77c8e4604045	203	"svc 0" \
mbed_official	49:77c8e4604045	204	: "=r" (__r0), "=r" (__r1), "=r" (__r2), "=r" (__r3) \
mbed_official	49:77c8e4604045	205	: "r" (__r0), "r" (__r1), "r" (__r2), "r" (__r3) \
mbed_official	49:77c8e4604045	206	: "r7", "r12", "lr", "cc" \
mbed_official	49:77c8e4604045	207	);
mbed_official	49:77c8e4604045	208	#else
mbed_official	49:77c8e4604045	209	#define SVC_Call(f) \
mbed_official	49:77c8e4604045	210	__asm volatile \
mbed_official	49:77c8e4604045	211	( \
mbed_official	49:77c8e4604045	212	"ldr r12,="#f"\n\t" \
mbed_official	49:77c8e4604045	213	"svc 0" \
mbed_official	49:77c8e4604045	214	: "=r" (__r0), "=r" (__r1), "=r" (__r2), "=r" (__r3) \
mbed_official	49:77c8e4604045	215	: "r" (__r0), "r" (__r1), "r" (__r2), "r" (__r3) \
mbed_official	49:77c8e4604045	216	: "r12", "lr", "cc" \
mbed_official	49:77c8e4604045	217	);
mbed_official	49:77c8e4604045	218	#endif
mbed_official	49:77c8e4604045	219
mbed_official	49:77c8e4604045	220	#define SVC_0_1(f,t,rv) \
mbed_official	49:77c8e4604045	221	__attribute__((always_inline)) \
mbed_official	49:77c8e4604045	222	static inline t __##f (void) { \
mbed_official	49:77c8e4604045	223	SVC_Arg0(); \
mbed_official	49:77c8e4604045	224	SVC_Call(f); \
mbed_official	49:77c8e4604045	225	return (t) rv; \
mbed_official	49:77c8e4604045	226	}
mbed_official	49:77c8e4604045	227
mbed_official	49:77c8e4604045	228	#define SVC_1_1(f,t,t1,rv) \
mbed_official	49:77c8e4604045	229	__attribute__((always_inline)) \
mbed_official	49:77c8e4604045	230	static inline t __##f (t1 a1) { \
mbed_official	49:77c8e4604045	231	SVC_Arg1(t1); \
mbed_official	49:77c8e4604045	232	SVC_Call(f); \
mbed_official	49:77c8e4604045	233	return (t) rv; \
mbed_official	49:77c8e4604045	234	}
mbed_official	49:77c8e4604045	235
mbed_official	49:77c8e4604045	236	#define SVC_2_1(f,t,t1,t2,rv) \
mbed_official	49:77c8e4604045	237	__attribute__((always_inline)) \
mbed_official	49:77c8e4604045	238	static inline t __##f (t1 a1, t2 a2) { \
mbed_official	49:77c8e4604045	239	SVC_Arg2(t1,t2); \
mbed_official	49:77c8e4604045	240	SVC_Call(f); \
mbed_official	49:77c8e4604045	241	return (t) rv; \
mbed_official	49:77c8e4604045	242	}
mbed_official	49:77c8e4604045	243
mbed_official	49:77c8e4604045	244	#define SVC_3_1(f,t,t1,t2,t3,rv) \
mbed_official	49:77c8e4604045	245	__attribute__((always_inline)) \
mbed_official	49:77c8e4604045	246	static inline t __##f (t1 a1, t2 a2, t3 a3) { \
mbed_official	49:77c8e4604045	247	SVC_Arg3(t1,t2,t3); \
mbed_official	49:77c8e4604045	248	SVC_Call(f); \
mbed_official	49:77c8e4604045	249	return (t) rv; \
mbed_official	49:77c8e4604045	250	}
mbed_official	49:77c8e4604045	251
mbed_official	49:77c8e4604045	252	#define SVC_4_1(f,t,t1,t2,t3,t4,rv) \
mbed_official	49:77c8e4604045	253	__attribute__((always_inline)) \
mbed_official	49:77c8e4604045	254	static inline t __##f (t1 a1, t2 a2, t3 a3, t4 a4) { \
mbed_official	49:77c8e4604045	255	SVC_Arg4(t1,t2,t3,t4); \
mbed_official	49:77c8e4604045	256	SVC_Call(f); \
mbed_official	49:77c8e4604045	257	return (t) rv; \
mbed_official	49:77c8e4604045	258	}
mbed_official	49:77c8e4604045	259
mbed_official	49:77c8e4604045	260	#define SVC_1_2 SVC_1_1
mbed_official	49:77c8e4604045	261	#define SVC_1_3 SVC_1_1
mbed_official	49:77c8e4604045	262	#define SVC_2_3 SVC_2_1
mbed_official	49:77c8e4604045	263
mbed_official	49:77c8e4604045	264	#elif defined (__ICCARM__) /* IAR Compiler */
mbed_official	49:77c8e4604045	265
mbed_official	49:77c8e4604045	266	#define __NO_RETURN __noreturn
mbed_official	49:77c8e4604045	267
mbed_official	49:77c8e4604045	268	#define RET_osEvent "=r"(ret.status), "=r"(ret.value), "=r"(ret.def)
mbed_official	49:77c8e4604045	269	#define RET_osCallback "=r"(ret.fp), "=r"(ret.arg)
mbed_official	49:77c8e4604045	270
mbed_official	49:77c8e4604045	271	#define osEvent_type osEvent
mbed_official	49:77c8e4604045	272	#define osEvent_ret_status ret
mbed_official	49:77c8e4604045	273	#define osEvent_ret_value ret
mbed_official	49:77c8e4604045	274	#define osEvent_ret_msg ret
mbed_official	49:77c8e4604045	275	#define osEvent_ret_mail ret
mbed_official	49:77c8e4604045	276
mbed_official	49:77c8e4604045	277	#define osCallback_type uint64_t
mbed_official	49:77c8e4604045	278	#define osCallback_ret ((uint64_t)ret.fp \| ((uint64_t)ret.arg)<<32)
mbed_official	49:77c8e4604045	279
mbed_official	49:77c8e4604045	280	#define SVC_Setup(f) \
mbed_official	49:77c8e4604045	281	__asm( \
mbed_official	49:77c8e4604045	282	"mov r12,%0\n" \
mbed_official	49:77c8e4604045	283	:: "r"(&f): "r12" \
mbed_official	49:77c8e4604045	284	);
mbed_official	49:77c8e4604045	285
mbed_official	49:77c8e4604045	286	#define SVC_Ret3() \
mbed_official	49:77c8e4604045	287	__asm( \
mbed_official	49:77c8e4604045	288	"ldr r0,[sp,#0]\n" \
mbed_official	49:77c8e4604045	289	"ldr r1,[sp,#4]\n" \
mbed_official	49:77c8e4604045	290	"ldr r2,[sp,#8]\n" \
mbed_official	49:77c8e4604045	291	);
mbed_official	49:77c8e4604045	292
mbed_official	49:77c8e4604045	293	#define SVC_0_1(f,t,...) \
mbed_official	49:77c8e4604045	294	t f (void); \
mbed_official	49:77c8e4604045	295	_Pragma("swi_number=0") __swi t _##f (void); \
mbed_official	49:77c8e4604045	296	static inline t __##f (void) { \
mbed_official	49:77c8e4604045	297	SVC_Setup(f); \
mbed_official	49:77c8e4604045	298	return _##f(); \
mbed_official	49:77c8e4604045	299	}
mbed_official	49:77c8e4604045	300
mbed_official	49:77c8e4604045	301	#define SVC_1_1(f,t,t1,...) \
mbed_official	49:77c8e4604045	302	t f (t1 a1); \
mbed_official	49:77c8e4604045	303	_Pragma("swi_number=0") __swi t _##f (t1 a1); \
mbed_official	49:77c8e4604045	304	static inline t __##f (t1 a1) { \
mbed_official	49:77c8e4604045	305	SVC_Setup(f); \
mbed_official	49:77c8e4604045	306	return _##f(a1); \
mbed_official	49:77c8e4604045	307	}
mbed_official	49:77c8e4604045	308
mbed_official	49:77c8e4604045	309	#define SVC_2_1(f,t,t1,t2,...) \
mbed_official	49:77c8e4604045	310	t f (t1 a1, t2 a2); \
mbed_official	49:77c8e4604045	311	_Pragma("swi_number=0") __swi t _##f (t1 a1, t2 a2); \
mbed_official	49:77c8e4604045	312	static inline t __##f (t1 a1, t2 a2) { \
mbed_official	49:77c8e4604045	313	SVC_Setup(f); \
mbed_official	49:77c8e4604045	314	return _##f(a1,a2); \
mbed_official	49:77c8e4604045	315	}
mbed_official	49:77c8e4604045	316
mbed_official	49:77c8e4604045	317	#define SVC_3_1(f,t,t1,t2,t3,...) \
mbed_official	49:77c8e4604045	318	t f (t1 a1, t2 a2, t3 a3); \
mbed_official	49:77c8e4604045	319	_Pragma("swi_number=0") __swi t _##f (t1 a1, t2 a2, t3 a3); \
mbed_official	49:77c8e4604045	320	static inline t __##f (t1 a1, t2 a2, t3 a3) { \
mbed_official	49:77c8e4604045	321	SVC_Setup(f); \
mbed_official	49:77c8e4604045	322	return _##f(a1,a2,a3); \
mbed_official	49:77c8e4604045	323	}
mbed_official	49:77c8e4604045	324
mbed_official	49:77c8e4604045	325	#define SVC_4_1(f,t,t1,t2,t3,t4,...) \
mbed_official	49:77c8e4604045	326	t f (t1 a1, t2 a2, t3 a3, t4 a4); \
mbed_official	49:77c8e4604045	327	_Pragma("swi_number=0") __swi t _##f (t1 a1, t2 a2, t3 a3, t4 a4); \
mbed_official	49:77c8e4604045	328	static inline t __##f (t1 a1, t2 a2, t3 a3, t4 a4) { \
mbed_official	49:77c8e4604045	329	SVC_Setup(f); \
mbed_official	49:77c8e4604045	330	return _##f(a1,a2,a3,a4); \
mbed_official	49:77c8e4604045	331	}
mbed_official	49:77c8e4604045	332
mbed_official	49:77c8e4604045	333	#define SVC_1_2(f,t,t1,rr) \
mbed_official	49:77c8e4604045	334	uint64_t f (t1 a1); \
mbed_official	49:77c8e4604045	335	_Pragma("swi_number=0") __swi uint64_t _##f (t1 a1); \
mbed_official	49:77c8e4604045	336	static inline t __##f (t1 a1) { \
mbed_official	49:77c8e4604045	337	t ret; \
mbed_official	49:77c8e4604045	338	SVC_Setup(f); \
mbed_official	49:77c8e4604045	339	_##f(a1); \
mbed_official	49:77c8e4604045	340	__asm("" : rr : :); \
mbed_official	49:77c8e4604045	341	return ret; \
mbed_official	49:77c8e4604045	342	}
mbed_official	49:77c8e4604045	343
mbed_official	49:77c8e4604045	344	#define SVC_1_3(f,t,t1,rr) \
mbed_official	49:77c8e4604045	345	t f (t1 a1); \
mbed_official	49:77c8e4604045	346	void f##_ (t1 a1) { \
mbed_official	49:77c8e4604045	347	f(a1); \
mbed_official	49:77c8e4604045	348	SVC_Ret3(); \
mbed_official	49:77c8e4604045	349	} \
mbed_official	49:77c8e4604045	350	_Pragma("swi_number=0") __swi void _##f (t1 a1); \
mbed_official	49:77c8e4604045	351	static inline t __##f (t1 a1) { \
mbed_official	49:77c8e4604045	352	t ret; \
mbed_official	49:77c8e4604045	353	SVC_Setup(f##_); \
mbed_official	49:77c8e4604045	354	_##f(a1); \
mbed_official	49:77c8e4604045	355	__asm("" : rr : :); \
mbed_official	49:77c8e4604045	356	return ret; \
mbed_official	49:77c8e4604045	357	}
mbed_official	49:77c8e4604045	358
mbed_official	49:77c8e4604045	359	#define SVC_2_3(f,t,t1,t2,rr) \
mbed_official	49:77c8e4604045	360	t f (t1 a1, t2 a2); \
mbed_official	49:77c8e4604045	361	void f##_ (t1 a1, t2 a2) { \
mbed_official	49:77c8e4604045	362	f(a1,a2); \
mbed_official	49:77c8e4604045	363	SVC_Ret3(); \
mbed_official	49:77c8e4604045	364	} \
mbed_official	49:77c8e4604045	365	_Pragma("swi_number=0") __swi void _##f (t1 a1, t2 a2); \
mbed_official	49:77c8e4604045	366	static inline t __##f (t1 a1, t2 a2) { \
mbed_official	49:77c8e4604045	367	t ret; \
mbed_official	49:77c8e4604045	368	SVC_Setup(f##_); \
mbed_official	49:77c8e4604045	369	_##f(a1,a2); \
mbed_official	49:77c8e4604045	370	__asm("" : rr : :); \
mbed_official	49:77c8e4604045	371	return ret; \
mbed_official	49:77c8e4604045	372	}
mbed_official	49:77c8e4604045	373
mbed_official	49:77c8e4604045	374	#endif
mbed_official	49:77c8e4604045	375
mbed_official	49:77c8e4604045	376
mbed_official	49:77c8e4604045	377	// Callback structure
mbed_official	49:77c8e4604045	378	typedef struct {
mbed_official	49:77c8e4604045	379	void *fp; // Function pointer
mbed_official	49:77c8e4604045	380	void *arg; // Function argument
mbed_official	49:77c8e4604045	381	} osCallback;
mbed_official	49:77c8e4604045	382
mbed_official	49:77c8e4604045	383
mbed_official	49:77c8e4604045	384	// OS Section definitions
mbed_official	49:77c8e4604045	385	#ifdef OS_SECTIONS_LINK_INFO
mbed_official	49:77c8e4604045	386	extern const uint32_t os_section_id$$Base;
mbed_official	49:77c8e4604045	387	extern const uint32_t os_section_id$$Limit;
mbed_official	49:77c8e4604045	388	#endif
mbed_official	49:77c8e4604045	389
mbed_official	49:77c8e4604045	390	// OS Timers external resources
mbed_official	49:77c8e4604045	391	extern osThreadDef_t os_thread_def_osTimerThread;
mbed_official	49:77c8e4604045	392	extern osThreadId osThreadId_osTimerThread;
mbed_official	49:77c8e4604045	393	extern osMessageQDef_t os_messageQ_def_osTimerMessageQ;
mbed_official	49:77c8e4604045	394	extern osMessageQId osMessageQId_osTimerMessageQ;
mbed_official	49:77c8e4604045	395
mbed_official	49:77c8e4604045	396
mbed_official	49:77c8e4604045	397	// ==== Helper Functions ====
mbed_official	49:77c8e4604045	398
mbed_official	49:77c8e4604045	399	/// Convert timeout in millisec to system ticks
mbed_official	49:77c8e4604045	400	static uint32_t rt_ms2tick (uint32_t millisec) {
mbed_official	49:77c8e4604045	401	uint32_t tick;
mbed_official	49:77c8e4604045	402
mbed_official	49:77c8e4604045	403	if (millisec == osWaitForever) return 0xFFFF; // Indefinite timeout
mbed_official	49:77c8e4604045	404	if (millisec > 4000000) return 0xFFFE; // Max ticks supported
mbed_official	49:77c8e4604045	405
mbed_official	49:77c8e4604045	406	tick = ((1000 * millisec) + os_clockrate - 1) / os_clockrate;
mbed_official	49:77c8e4604045	407	if (tick > 0xFFFE) return 0xFFFE;
mbed_official	49:77c8e4604045	408
mbed_official	49:77c8e4604045	409	return tick;
mbed_official	49:77c8e4604045	410	}
mbed_official	49:77c8e4604045	411
mbed_official	49:77c8e4604045	412	/// Convert Thread ID to TCB pointer
mbed_official	49:77c8e4604045	413	static P_TCB rt_tid2ptcb (osThreadId thread_id) {
mbed_official	49:77c8e4604045	414	P_TCB ptcb;
mbed_official	49:77c8e4604045	415
mbed_official	49:77c8e4604045	416	if (thread_id == NULL) return NULL;
mbed_official	49:77c8e4604045	417
mbed_official	49:77c8e4604045	418	if ((uint32_t)thread_id & 3) return NULL;
mbed_official	49:77c8e4604045	419
mbed_official	49:77c8e4604045	420	#ifdef OS_SECTIONS_LINK_INFO
mbed_official	49:77c8e4604045	421	if ((os_section_id$$Base != 0) && (os_section_id$$Limit != 0)) {
mbed_official	49:77c8e4604045	422	if (thread_id < (osThreadId)os_section_id$$Base) return NULL;
mbed_official	49:77c8e4604045	423	if (thread_id >= (osThreadId)os_section_id$$Limit) return NULL;
mbed_official	49:77c8e4604045	424	}
mbed_official	49:77c8e4604045	425	#endif
mbed_official	49:77c8e4604045	426
mbed_official	49:77c8e4604045	427	ptcb = thread_id;
mbed_official	49:77c8e4604045	428
mbed_official	49:77c8e4604045	429	if (ptcb->cb_type != TCB) return NULL;
mbed_official	49:77c8e4604045	430
mbed_official	49:77c8e4604045	431	return ptcb;
mbed_official	49:77c8e4604045	432	}
mbed_official	49:77c8e4604045	433
mbed_official	49:77c8e4604045	434	/// Convert ID pointer to Object pointer
mbed_official	49:77c8e4604045	435	static void rt_id2obj (void id) {
mbed_official	49:77c8e4604045	436
mbed_official	49:77c8e4604045	437	if ((uint32_t)id & 3) return NULL;
mbed_official	49:77c8e4604045	438
mbed_official	49:77c8e4604045	439	#ifdef OS_SECTIONS_LINK_INFO
mbed_official	49:77c8e4604045	440	if ((os_section_id$$Base != 0) && (os_section_id$$Limit != 0)) {
mbed_official	49:77c8e4604045	441	if (id < (void *)os_section_id$$Base) return NULL;
mbed_official	49:77c8e4604045	442	if (id >= (void *)os_section_id$$Limit) return NULL;
mbed_official	49:77c8e4604045	443	}
mbed_official	49:77c8e4604045	444	#endif
mbed_official	49:77c8e4604045	445
mbed_official	49:77c8e4604045	446	return id;
mbed_official	49:77c8e4604045	447	}
mbed_official	49:77c8e4604045	448
mbed_official	49:77c8e4604045	449
mbed_official	49:77c8e4604045	450	// ==== Kernel Control ====
mbed_official	49:77c8e4604045	451
mbed_official	49:77c8e4604045	452	uint8_t os_initialized; // Kernel Initialized flag
mbed_official	49:77c8e4604045	453	uint8_t os_running; // Kernel Running flag
mbed_official	49:77c8e4604045	454
mbed_official	49:77c8e4604045	455	// Kernel Control Service Calls declarations
mbed_official	49:77c8e4604045	456	SVC_0_1(svcKernelInitialize, osStatus, RET_osStatus)
mbed_official	49:77c8e4604045	457	SVC_0_1(svcKernelStart, osStatus, RET_osStatus)
mbed_official	49:77c8e4604045	458	SVC_0_1(svcKernelRunning, int32_t, RET_int32_t)
mbed_official	49:77c8e4604045	459
mbed_official	49:77c8e4604045	460	extern void sysThreadError (osStatus status);
mbed_official	49:77c8e4604045	461	osThreadId svcThreadCreate (osThreadDef_t thread_def, void argument);
mbed_official	49:77c8e4604045	462	osMessageQId svcMessageCreate (osMessageQDef_t *queue_def, osThreadId thread_id);
mbed_official	49:77c8e4604045	463
mbed_official	49:77c8e4604045	464	// Kernel Control Service Calls
mbed_official	49:77c8e4604045	465
mbed_official	49:77c8e4604045	466	/// Initialize the RTOS Kernel for creating objects
mbed_official	49:77c8e4604045	467	osStatus svcKernelInitialize (void) {
mbed_official	49:77c8e4604045	468	if (os_initialized) return osOK;
mbed_official	49:77c8e4604045	469
mbed_official	49:77c8e4604045	470	rt_sys_init(); // RTX System Initialization
mbed_official	49:77c8e4604045	471	os_tsk.run->prio = 255; // Highest priority
mbed_official	49:77c8e4604045	472
mbed_official	49:77c8e4604045	473	sysThreadError(osOK);
mbed_official	49:77c8e4604045	474
mbed_official	49:77c8e4604045	475	os_initialized = 1;
mbed_official	49:77c8e4604045	476
mbed_official	49:77c8e4604045	477	return osOK;
mbed_official	49:77c8e4604045	478	}
mbed_official	49:77c8e4604045	479
mbed_official	49:77c8e4604045	480	/// Start the RTOS Kernel
mbed_official	49:77c8e4604045	481	osStatus svcKernelStart (void) {
mbed_official	49:77c8e4604045	482
mbed_official	49:77c8e4604045	483	if (os_running) return osOK;
mbed_official	49:77c8e4604045	484
mbed_official	49:77c8e4604045	485	// Create OS Timers resources (Message Queue & Thread)
mbed_official	49:77c8e4604045	486	osMessageQId_osTimerMessageQ = svcMessageCreate (&os_messageQ_def_osTimerMessageQ, NULL);
mbed_official	49:77c8e4604045	487	osThreadId_osTimerThread = svcThreadCreate(&os_thread_def_osTimerThread, NULL);
mbed_official	49:77c8e4604045	488
mbed_official	49:77c8e4604045	489	rt_tsk_prio(0, 0); // Lowest priority
mbed_official	49:77c8e4604045	490	__set_PSP(os_tsk.run->tsk_stack + 8*4); // New context
mbed_official	49:77c8e4604045	491	os_tsk.run = NULL; // Force context switch
mbed_official	49:77c8e4604045	492
mbed_official	49:77c8e4604045	493	rt_sys_start();
mbed_official	49:77c8e4604045	494
mbed_official	49:77c8e4604045	495	os_running = 1;
mbed_official	49:77c8e4604045	496
mbed_official	49:77c8e4604045	497	return osOK;
mbed_official	49:77c8e4604045	498	}
mbed_official	49:77c8e4604045	499
mbed_official	49:77c8e4604045	500	/// Check if the RTOS kernel is already started
mbed_official	49:77c8e4604045	501	int32_t svcKernelRunning(void) {
mbed_official	49:77c8e4604045	502	return os_running;
mbed_official	49:77c8e4604045	503	}
mbed_official	49:77c8e4604045	504
mbed_official	49:77c8e4604045	505	// Kernel Control Public API
mbed_official	49:77c8e4604045	506
mbed_official	49:77c8e4604045	507	/// Initialize the RTOS Kernel for creating objects
mbed_official	49:77c8e4604045	508	osStatus osKernelInitialize (void) {
mbed_official	49:77c8e4604045	509	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	510	if ((__get_CONTROL() & 1) == 0) { // Privileged mode
mbed_official	49:77c8e4604045	511	return svcKernelInitialize();
mbed_official	49:77c8e4604045	512	} else {
mbed_official	49:77c8e4604045	513	return __svcKernelInitialize();
mbed_official	49:77c8e4604045	514	}
mbed_official	49:77c8e4604045	515	}
mbed_official	49:77c8e4604045	516
mbed_official	49:77c8e4604045	517	/// Start the RTOS Kernel
mbed_official	49:77c8e4604045	518	osStatus osKernelStart (void) {
mbed_official	49:77c8e4604045	519	uint32_t stack[8];
mbed_official	49:77c8e4604045	520
mbed_official	49:77c8e4604045	521	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	522	switch (__get_CONTROL() & 0x03) {
mbed_official	49:77c8e4604045	523	case 0x00: // Privileged Thread mode & MSP
mbed_official	49:77c8e4604045	524	__set_PSP((uint32_t)(stack + 8)); // Initial PSP
mbed_official	49:77c8e4604045	525	if (os_flags & 1) {
mbed_official	49:77c8e4604045	526	__set_CONTROL(0x02); // Set Privileged Thread mode & PSP
mbed_official	49:77c8e4604045	527	} else {
mbed_official	49:77c8e4604045	528	__set_CONTROL(0x03); // Set Unprivileged Thread mode & PSP
mbed_official	49:77c8e4604045	529	}
mbed_official	49:77c8e4604045	530	__DSB();
mbed_official	49:77c8e4604045	531	__ISB();
mbed_official	49:77c8e4604045	532	break;
mbed_official	49:77c8e4604045	533	case 0x01: // Unprivileged Thread mode & MSP
mbed_official	49:77c8e4604045	534	return osErrorOS;
mbed_official	49:77c8e4604045	535	case 0x02: // Privileged Thread mode & PSP
mbed_official	49:77c8e4604045	536	if ((os_flags & 1) == 0) { // Unprivileged Thread mode requested
mbed_official	49:77c8e4604045	537	__set_CONTROL(0x03); // Set Unprivileged Thread mode & PSP
mbed_official	49:77c8e4604045	538	__DSB();
mbed_official	49:77c8e4604045	539	__ISB();
mbed_official	49:77c8e4604045	540	}
mbed_official	49:77c8e4604045	541	break;
mbed_official	49:77c8e4604045	542	case 0x03: // Unprivileged Thread mode & PSP
mbed_official	49:77c8e4604045	543	if (os_flags & 1) return osErrorOS; // Privileged Thread mode requested
mbed_official	49:77c8e4604045	544	break;
mbed_official	49:77c8e4604045	545	}
mbed_official	49:77c8e4604045	546	return __svcKernelStart();
mbed_official	49:77c8e4604045	547	}
mbed_official	49:77c8e4604045	548
mbed_official	49:77c8e4604045	549	/// Check if the RTOS kernel is already started
mbed_official	49:77c8e4604045	550	int32_t osKernelRunning(void) {
mbed_official	49:77c8e4604045	551	if ((__get_IPSR() != 0) \|\| ((__get_CONTROL() & 1) == 0)) {
mbed_official	49:77c8e4604045	552	// in ISR or Privileged
mbed_official	49:77c8e4604045	553	return os_running;
mbed_official	49:77c8e4604045	554	} else {
mbed_official	49:77c8e4604045	555	return __svcKernelRunning();
mbed_official	49:77c8e4604045	556	}
mbed_official	49:77c8e4604045	557	}
mbed_official	49:77c8e4604045	558
mbed_official	49:77c8e4604045	559
mbed_official	49:77c8e4604045	560	// ==== Thread Management ====
mbed_official	49:77c8e4604045	561
mbed_official	49:77c8e4604045	562	__NO_RETURN void osThreadExit (void);
mbed_official	49:77c8e4604045	563
mbed_official	49:77c8e4604045	564	// Thread Service Calls declarations
mbed_official	49:77c8e4604045	565	SVC_2_1(svcThreadCreate, osThreadId, osThreadDef_t , void , RET_pointer)
mbed_official	49:77c8e4604045	566	SVC_0_1(svcThreadGetId, osThreadId, RET_pointer)
mbed_official	49:77c8e4604045	567	SVC_1_1(svcThreadTerminate, osStatus, osThreadId, RET_osStatus)
mbed_official	49:77c8e4604045	568	SVC_0_1(svcThreadYield, osStatus, RET_osStatus)
mbed_official	49:77c8e4604045	569	SVC_2_1(svcThreadSetPriority, osStatus, osThreadId, osPriority, RET_osStatus)
mbed_official	49:77c8e4604045	570	SVC_1_1(svcThreadGetPriority, osPriority, osThreadId, RET_osPriority)
mbed_official	49:77c8e4604045	571
mbed_official	49:77c8e4604045	572	// Thread Service Calls
mbed_official	49:77c8e4604045	573	extern OS_TID rt_get_TID (void);
mbed_official	49:77c8e4604045	574	extern void rt_init_context (P_TCB p_TCB, U8 priority, FUNCP task_body);
mbed_official	49:77c8e4604045	575
mbed_official	49:77c8e4604045	576	/// Create a thread and add it to Active Threads and set it to state READY
mbed_official	49:77c8e4604045	577	osThreadId svcThreadCreate (osThreadDef_t thread_def, void argument) {
mbed_official	49:77c8e4604045	578	P_TCB ptcb;
mbed_official	49:77c8e4604045	579
mbed_official	49:77c8e4604045	580	if ((thread_def == NULL) \|\|
mbed_official	49:77c8e4604045	581	(thread_def->pthread == NULL) \|\|
mbed_official	49:77c8e4604045	582	(thread_def->tpriority < osPriorityIdle) \|\|
mbed_official	49:77c8e4604045	583	(thread_def->tpriority > osPriorityRealtime) \|\|
mbed_official	49:77c8e4604045	584	(thread_def->stacksize == 0) \|\|
mbed_official	49:77c8e4604045	585	(thread_def->stack_pointer == NULL) ) {
mbed_official	49:77c8e4604045	586	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	587	return NULL;
mbed_official	49:77c8e4604045	588	}
mbed_official	49:77c8e4604045	589
mbed_official	49:77c8e4604045	590	U8 priority = thread_def->tpriority - osPriorityIdle + 1;
mbed_official	49:77c8e4604045	591	P_TCB task_context = &thread_def->tcb;
mbed_official	49:77c8e4604045	592
mbed_official	49:77c8e4604045	593	/* If "size != 0" use a private user provided stack. */
mbed_official	49:77c8e4604045	594	task_context->stack = (U32*)thread_def->stack_pointer;
mbed_official	49:77c8e4604045	595	task_context->priv_stack = thread_def->stacksize;
mbed_official	49:77c8e4604045	596	/* Pass parameter 'argv' to 'rt_init_context' */
mbed_official	49:77c8e4604045	597	task_context->msg = argument;
mbed_official	49:77c8e4604045	598	/* For 'size == 0' system allocates the user stack from the memory pool. */
mbed_official	49:77c8e4604045	599	rt_init_context (task_context, priority, (FUNCP)thread_def->pthread);
mbed_official	49:77c8e4604045	600
mbed_official	49:77c8e4604045	601	/* Find a free entry in 'os_active_TCB' table. */
mbed_official	49:77c8e4604045	602	OS_TID tsk = rt_get_TID ();
mbed_official	49:77c8e4604045	603	os_active_TCB[tsk-1] = task_context;
mbed_official	49:77c8e4604045	604	task_context->task_id = tsk;
mbed_official	49:77c8e4604045	605	DBG_TASK_NOTIFY(task_context, __TRUE);
mbed_official	49:77c8e4604045	606	rt_dispatch (task_context);
mbed_official	49:77c8e4604045	607
mbed_official	49:77c8e4604045	608	ptcb = (P_TCB)os_active_TCB[tsk - 1]; // TCB pointer
mbed_official	49:77c8e4604045	609
mbed_official	49:77c8e4604045	610	((uint32_t )ptcb->tsk_stack + 13) = (uint32_t)osThreadExit;
mbed_official	49:77c8e4604045	611
mbed_official	49:77c8e4604045	612	return ptcb;
mbed_official	49:77c8e4604045	613	}
mbed_official	49:77c8e4604045	614
mbed_official	49:77c8e4604045	615	/// Return the thread ID of the current running thread
mbed_official	49:77c8e4604045	616	osThreadId svcThreadGetId (void) {
mbed_official	49:77c8e4604045	617	OS_TID tsk;
mbed_official	49:77c8e4604045	618
mbed_official	49:77c8e4604045	619	tsk = rt_tsk_self();
mbed_official	49:77c8e4604045	620	if (tsk == 0) return NULL;
mbed_official	49:77c8e4604045	621	return (P_TCB)os_active_TCB[tsk - 1];
mbed_official	49:77c8e4604045	622	}
mbed_official	49:77c8e4604045	623
mbed_official	49:77c8e4604045	624	/// Terminate execution of a thread and remove it from ActiveThreads
mbed_official	49:77c8e4604045	625	osStatus svcThreadTerminate (osThreadId thread_id) {
mbed_official	49:77c8e4604045	626	OS_RESULT res;
mbed_official	49:77c8e4604045	627	P_TCB ptcb;
mbed_official	49:77c8e4604045	628
mbed_official	49:77c8e4604045	629	ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
mbed_official	49:77c8e4604045	630	if (ptcb == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	631
mbed_official	49:77c8e4604045	632	res = rt_tsk_delete(ptcb->task_id); // Delete task
mbed_official	49:77c8e4604045	633
mbed_official	49:77c8e4604045	634	if (res == OS_R_NOK) return osErrorResource; // Delete task failed
mbed_official	49:77c8e4604045	635
mbed_official	49:77c8e4604045	636	return osOK;
mbed_official	49:77c8e4604045	637	}
mbed_official	49:77c8e4604045	638
mbed_official	49:77c8e4604045	639	/// Pass control to next thread that is in state READY
mbed_official	49:77c8e4604045	640	osStatus svcThreadYield (void) {
mbed_official	49:77c8e4604045	641	rt_tsk_pass(); // Pass control to next task
mbed_official	49:77c8e4604045	642	return osOK;
mbed_official	49:77c8e4604045	643	}
mbed_official	49:77c8e4604045	644
mbed_official	49:77c8e4604045	645	/// Change priority of an active thread
mbed_official	49:77c8e4604045	646	osStatus svcThreadSetPriority (osThreadId thread_id, osPriority priority) {
mbed_official	49:77c8e4604045	647	OS_RESULT res;
mbed_official	49:77c8e4604045	648	P_TCB ptcb;
mbed_official	49:77c8e4604045	649
mbed_official	49:77c8e4604045	650	ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
mbed_official	49:77c8e4604045	651	if (ptcb == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	652
mbed_official	49:77c8e4604045	653	if ((priority < osPriorityIdle) \|\| (priority > osPriorityRealtime)) {
mbed_official	49:77c8e4604045	654	return osErrorValue;
mbed_official	49:77c8e4604045	655	}
mbed_official	49:77c8e4604045	656
mbed_official	49:77c8e4604045	657	res = rt_tsk_prio( // Change task priority
mbed_official	49:77c8e4604045	658	ptcb->task_id, // Task ID
mbed_official	49:77c8e4604045	659	priority - osPriorityIdle + 1 // New task priority
mbed_official	49:77c8e4604045	660	);
mbed_official	49:77c8e4604045	661
mbed_official	49:77c8e4604045	662	if (res == OS_R_NOK) return osErrorResource; // Change task priority failed
mbed_official	49:77c8e4604045	663
mbed_official	49:77c8e4604045	664	return osOK;
mbed_official	49:77c8e4604045	665	}
mbed_official	49:77c8e4604045	666
mbed_official	49:77c8e4604045	667	/// Get current priority of an active thread
mbed_official	49:77c8e4604045	668	osPriority svcThreadGetPriority (osThreadId thread_id) {
mbed_official	49:77c8e4604045	669	P_TCB ptcb;
mbed_official	49:77c8e4604045	670
mbed_official	49:77c8e4604045	671	ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
mbed_official	49:77c8e4604045	672	if (ptcb == NULL) return osPriorityError;
mbed_official	49:77c8e4604045	673
mbed_official	49:77c8e4604045	674	return (osPriority)(ptcb->prio - 1 + osPriorityIdle);
mbed_official	49:77c8e4604045	675	}
mbed_official	49:77c8e4604045	676
mbed_official	49:77c8e4604045	677
mbed_official	49:77c8e4604045	678	// Thread Public API
mbed_official	49:77c8e4604045	679
mbed_official	49:77c8e4604045	680	/// Create a thread and add it to Active Threads and set it to state READY
mbed_official	49:77c8e4604045	681	osThreadId osThreadCreate (osThreadDef_t thread_def, void argument) {
mbed_official	49:77c8e4604045	682	if (__get_IPSR() != 0) return NULL; // Not allowed in ISR
mbed_official	49:77c8e4604045	683	if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) {
mbed_official	49:77c8e4604045	684	// Privileged and not running
mbed_official	49:77c8e4604045	685	return svcThreadCreate(thread_def, argument);
mbed_official	49:77c8e4604045	686	} else {
mbed_official	49:77c8e4604045	687	return __svcThreadCreate(thread_def, argument);
mbed_official	49:77c8e4604045	688	}
mbed_official	49:77c8e4604045	689	}
mbed_official	49:77c8e4604045	690
mbed_official	49:77c8e4604045	691	/// Return the thread ID of the current running thread
mbed_official	49:77c8e4604045	692	osThreadId osThreadGetId (void) {
mbed_official	49:77c8e4604045	693	if (__get_IPSR() != 0) return NULL; // Not allowed in ISR
mbed_official	49:77c8e4604045	694	return __svcThreadGetId();
mbed_official	49:77c8e4604045	695	}
mbed_official	49:77c8e4604045	696
mbed_official	49:77c8e4604045	697	/// Terminate execution of a thread and remove it from ActiveThreads
mbed_official	49:77c8e4604045	698	osStatus osThreadTerminate (osThreadId thread_id) {
mbed_official	49:77c8e4604045	699	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	700	return __svcThreadTerminate(thread_id);
mbed_official	49:77c8e4604045	701	}
mbed_official	49:77c8e4604045	702
mbed_official	49:77c8e4604045	703	/// Pass control to next thread that is in state READY
mbed_official	49:77c8e4604045	704	osStatus osThreadYield (void) {
mbed_official	49:77c8e4604045	705	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	706	return __svcThreadYield();
mbed_official	49:77c8e4604045	707	}
mbed_official	49:77c8e4604045	708
mbed_official	49:77c8e4604045	709	/// Change priority of an active thread
mbed_official	49:77c8e4604045	710	osStatus osThreadSetPriority (osThreadId thread_id, osPriority priority) {
mbed_official	49:77c8e4604045	711	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	712	return __svcThreadSetPriority(thread_id, priority);
mbed_official	49:77c8e4604045	713	}
mbed_official	49:77c8e4604045	714
mbed_official	49:77c8e4604045	715	/// Get current priority of an active thread
mbed_official	49:77c8e4604045	716	osPriority osThreadGetPriority (osThreadId thread_id) {
mbed_official	49:77c8e4604045	717	if (__get_IPSR() != 0) return osPriorityError;// Not allowed in ISR
mbed_official	49:77c8e4604045	718	return __svcThreadGetPriority(thread_id);
mbed_official	49:77c8e4604045	719	}
mbed_official	49:77c8e4604045	720
mbed_official	49:77c8e4604045	721	/// INTERNAL - Not Public
mbed_official	49:77c8e4604045	722	/// Auto Terminate Thread on exit (used implicitly when thread exists)
mbed_official	49:77c8e4604045	723	__NO_RETURN void osThreadExit (void) {
mbed_official	49:77c8e4604045	724	__svcThreadTerminate(__svcThreadGetId());
mbed_official	49:77c8e4604045	725	for (;;); // Should never come here
mbed_official	49:77c8e4604045	726	}
mbed_official	49:77c8e4604045	727
mbed_official	49:77c8e4604045	728
mbed_official	49:77c8e4604045	729	// ==== Generic Wait Functions ====
mbed_official	49:77c8e4604045	730
mbed_official	49:77c8e4604045	731	// Generic Wait Service Calls declarations
mbed_official	49:77c8e4604045	732	SVC_1_1(svcDelay, osStatus, uint32_t, RET_osStatus)
mbed_official	49:77c8e4604045	733	#if osFeature_Wait != 0
mbed_official	49:77c8e4604045	734	SVC_1_3(svcWait, os_InRegs osEvent, uint32_t, RET_osEvent)
mbed_official	49:77c8e4604045	735	#endif
mbed_official	49:77c8e4604045	736
mbed_official	49:77c8e4604045	737	// Generic Wait Service Calls
mbed_official	49:77c8e4604045	738
mbed_official	49:77c8e4604045	739	/// Wait for Timeout (Time Delay)
mbed_official	49:77c8e4604045	740	osStatus svcDelay (uint32_t millisec) {
mbed_official	49:77c8e4604045	741	if (millisec == 0) return osOK;
mbed_official	49:77c8e4604045	742	rt_dly_wait(rt_ms2tick(millisec));
mbed_official	49:77c8e4604045	743	return osEventTimeout;
mbed_official	49:77c8e4604045	744	}
mbed_official	49:77c8e4604045	745
mbed_official	49:77c8e4604045	746	/// Wait for Signal, Message, Mail, or Timeout
mbed_official	49:77c8e4604045	747	#if osFeature_Wait != 0
mbed_official	49:77c8e4604045	748	os_InRegs osEvent_type svcWait (uint32_t millisec) {
mbed_official	49:77c8e4604045	749	osEvent ret;
mbed_official	49:77c8e4604045	750
mbed_official	49:77c8e4604045	751	if (millisec == 0) {
mbed_official	49:77c8e4604045	752	ret.status = osOK;
mbed_official	49:77c8e4604045	753	return osEvent_ret_status;
mbed_official	49:77c8e4604045	754	}
mbed_official	49:77c8e4604045	755
mbed_official	49:77c8e4604045	756	/* To Do: osEventSignal, osEventMessage, osEventMail */
mbed_official	49:77c8e4604045	757	rt_dly_wait(rt_ms2tick(millisec));
mbed_official	49:77c8e4604045	758	ret.status = osEventTimeout;
mbed_official	49:77c8e4604045	759
mbed_official	49:77c8e4604045	760	return osEvent_ret_status;
mbed_official	49:77c8e4604045	761	}
mbed_official	49:77c8e4604045	762	#endif
mbed_official	49:77c8e4604045	763
mbed_official	49:77c8e4604045	764
mbed_official	49:77c8e4604045	765	// Generic Wait API
mbed_official	49:77c8e4604045	766
mbed_official	49:77c8e4604045	767	/// Wait for Timeout (Time Delay)
mbed_official	49:77c8e4604045	768	osStatus osDelay (uint32_t millisec) {
mbed_official	49:77c8e4604045	769	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	770	return __svcDelay(millisec);
mbed_official	49:77c8e4604045	771	}
mbed_official	49:77c8e4604045	772
mbed_official	49:77c8e4604045	773	/// Wait for Signal, Message, Mail, or Timeout
mbed_official	49:77c8e4604045	774	os_InRegs osEvent osWait (uint32_t millisec) {
mbed_official	49:77c8e4604045	775	osEvent ret;
mbed_official	49:77c8e4604045	776
mbed_official	49:77c8e4604045	777	#if osFeature_Wait == 0
mbed_official	49:77c8e4604045	778	ret.status = osErrorOS;
mbed_official	49:77c8e4604045	779	return ret;
mbed_official	49:77c8e4604045	780	#else
mbed_official	49:77c8e4604045	781	if (__get_IPSR() != 0) { // Not allowed in ISR
mbed_official	49:77c8e4604045	782	ret.status = osErrorISR;
mbed_official	49:77c8e4604045	783	return ret;
mbed_official	49:77c8e4604045	784	}
mbed_official	49:77c8e4604045	785	return __svcWait(millisec);
mbed_official	49:77c8e4604045	786	#endif
mbed_official	49:77c8e4604045	787	}
mbed_official	49:77c8e4604045	788
mbed_official	49:77c8e4604045	789
mbed_official	49:77c8e4604045	790	// ==== Timer Management ====
mbed_official	49:77c8e4604045	791
mbed_official	49:77c8e4604045	792	// Timer definitions
mbed_official	49:77c8e4604045	793	#define osTimerInvalid 0
mbed_official	49:77c8e4604045	794	#define osTimerStopped 1
mbed_official	49:77c8e4604045	795	#define osTimerRunning 2
mbed_official	49:77c8e4604045	796
mbed_official	49:77c8e4604045	797	// Timer structures
mbed_official	49:77c8e4604045	798
mbed_official	49:77c8e4604045	799	typedef struct os_timer_cb_ { // Timer Control Block
mbed_official	49:77c8e4604045	800	struct os_timer_cb_ *next; // Pointer to next active Timer
mbed_official	49:77c8e4604045	801	uint8_t state; // Timer State
mbed_official	49:77c8e4604045	802	uint8_t type; // Timer Type (Periodic/One-shot)
mbed_official	49:77c8e4604045	803	uint16_t reserved; // Reserved
mbed_official	49:77c8e4604045	804	uint16_t tcnt; // Timer Delay Count
mbed_official	49:77c8e4604045	805	uint16_t icnt; // Timer Initial Count
mbed_official	49:77c8e4604045	806	void *arg; // Timer Function Argument
mbed_official	49:77c8e4604045	807	osTimerDef_t *timer; // Pointer to Timer definition
mbed_official	49:77c8e4604045	808	} os_timer_cb;
mbed_official	49:77c8e4604045	809
mbed_official	49:77c8e4604045	810	// Timer variables
mbed_official	49:77c8e4604045	811	os_timer_cb *os_timer_head; // Pointer to first active Timer
mbed_official	49:77c8e4604045	812
mbed_official	49:77c8e4604045	813
mbed_official	49:77c8e4604045	814	// Timer Helper Functions
mbed_official	49:77c8e4604045	815
mbed_official	49:77c8e4604045	816	// Insert Timer into the list sorted by time
mbed_official	49:77c8e4604045	817	static void rt_timer_insert (os_timer_cb *pt, uint32_t tcnt) {
mbed_official	49:77c8e4604045	818	os_timer_cb p, prev;
mbed_official	49:77c8e4604045	819
mbed_official	49:77c8e4604045	820	prev = NULL;
mbed_official	49:77c8e4604045	821	p = os_timer_head;
mbed_official	49:77c8e4604045	822	while (p != NULL) {
mbed_official	49:77c8e4604045	823	if (tcnt < p->tcnt) break;
mbed_official	49:77c8e4604045	824	tcnt -= p->tcnt;
mbed_official	49:77c8e4604045	825	prev = p;
mbed_official	49:77c8e4604045	826	p = p->next;
mbed_official	49:77c8e4604045	827	}
mbed_official	49:77c8e4604045	828	pt->next = p;
mbed_official	49:77c8e4604045	829	pt->tcnt = (uint16_t)tcnt;
mbed_official	49:77c8e4604045	830	if (p != NULL) {
mbed_official	49:77c8e4604045	831	p->tcnt -= pt->tcnt;
mbed_official	49:77c8e4604045	832	}
mbed_official	49:77c8e4604045	833	if (prev != NULL) {
mbed_official	49:77c8e4604045	834	prev->next = pt;
mbed_official	49:77c8e4604045	835	} else {
mbed_official	49:77c8e4604045	836	os_timer_head = pt;
mbed_official	49:77c8e4604045	837	}
mbed_official	49:77c8e4604045	838	}
mbed_official	49:77c8e4604045	839
mbed_official	49:77c8e4604045	840	// Remove Timer from the list
mbed_official	49:77c8e4604045	841	static int rt_timer_remove (os_timer_cb *pt) {
mbed_official	49:77c8e4604045	842	os_timer_cb p, prev;
mbed_official	49:77c8e4604045	843
mbed_official	49:77c8e4604045	844	prev = NULL;
mbed_official	49:77c8e4604045	845	p = os_timer_head;
mbed_official	49:77c8e4604045	846	while (p != NULL) {
mbed_official	49:77c8e4604045	847	if (p == pt) break;
mbed_official	49:77c8e4604045	848	prev = p;
mbed_official	49:77c8e4604045	849	p = p->next;
mbed_official	49:77c8e4604045	850	}
mbed_official	49:77c8e4604045	851	if (p == NULL) return -1;
mbed_official	49:77c8e4604045	852	if (prev != NULL) {
mbed_official	49:77c8e4604045	853	prev->next = pt->next;
mbed_official	49:77c8e4604045	854	} else {
mbed_official	49:77c8e4604045	855	os_timer_head = pt->next;
mbed_official	49:77c8e4604045	856	}
mbed_official	49:77c8e4604045	857	if (pt->next != NULL) {
mbed_official	49:77c8e4604045	858	pt->next->tcnt += pt->tcnt;
mbed_official	49:77c8e4604045	859	}
mbed_official	49:77c8e4604045	860
mbed_official	49:77c8e4604045	861	return 0;
mbed_official	49:77c8e4604045	862	}
mbed_official	49:77c8e4604045	863
mbed_official	49:77c8e4604045	864
mbed_official	49:77c8e4604045	865	// Timer Service Calls declarations
mbed_official	49:77c8e4604045	866	SVC_3_1(svcTimerCreate, osTimerId, osTimerDef_t , os_timer_type, void , RET_pointer)
mbed_official	49:77c8e4604045	867	SVC_2_1(svcTimerStart, osStatus, osTimerId, uint32_t, RET_osStatus)
mbed_official	49:77c8e4604045	868	SVC_1_1(svcTimerStop, osStatus, osTimerId, RET_osStatus)
mbed_official	49:77c8e4604045	869	SVC_1_1(svcTimerDelete, osStatus, osTimerId, RET_osStatus)
mbed_official	49:77c8e4604045	870	SVC_1_2(svcTimerCall, os_InRegs osCallback, osTimerId, RET_osCallback)
mbed_official	49:77c8e4604045	871
mbed_official	49:77c8e4604045	872	// Timer Management Service Calls
mbed_official	49:77c8e4604045	873
mbed_official	49:77c8e4604045	874	/// Create timer
mbed_official	49:77c8e4604045	875	osTimerId svcTimerCreate (osTimerDef_t timer_def, os_timer_type type, void argument) {
mbed_official	49:77c8e4604045	876	os_timer_cb *pt;
mbed_official	49:77c8e4604045	877
mbed_official	49:77c8e4604045	878	if ((timer_def == NULL) \|\| (timer_def->ptimer == NULL)) {
mbed_official	49:77c8e4604045	879	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	880	return NULL;
mbed_official	49:77c8e4604045	881	}
mbed_official	49:77c8e4604045	882
mbed_official	49:77c8e4604045	883	pt = timer_def->timer;
mbed_official	49:77c8e4604045	884	if (pt == NULL) {
mbed_official	49:77c8e4604045	885	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	886	return NULL;
mbed_official	49:77c8e4604045	887	}
mbed_official	49:77c8e4604045	888
mbed_official	49:77c8e4604045	889	if ((type != osTimerOnce) && (type != osTimerPeriodic)) {
mbed_official	49:77c8e4604045	890	sysThreadError(osErrorValue);
mbed_official	49:77c8e4604045	891	return NULL;
mbed_official	49:77c8e4604045	892	}
mbed_official	49:77c8e4604045	893
mbed_official	49:77c8e4604045	894	if (osThreadId_osTimerThread == NULL) {
mbed_official	49:77c8e4604045	895	sysThreadError(osErrorResource);
mbed_official	49:77c8e4604045	896	return NULL;
mbed_official	49:77c8e4604045	897	}
mbed_official	49:77c8e4604045	898
mbed_official	49:77c8e4604045	899	if (pt->state != osTimerInvalid){
mbed_official	49:77c8e4604045	900	sysThreadError(osErrorResource);
mbed_official	49:77c8e4604045	901	return NULL;
mbed_official	49:77c8e4604045	902	}
mbed_official	49:77c8e4604045	903
mbed_official	49:77c8e4604045	904	pt->state = osTimerStopped;
mbed_official	49:77c8e4604045	905	pt->type = (uint8_t)type;
mbed_official	49:77c8e4604045	906	pt->arg = argument;
mbed_official	49:77c8e4604045	907	pt->timer = timer_def;
mbed_official	49:77c8e4604045	908
mbed_official	49:77c8e4604045	909	return (osTimerId)pt;
mbed_official	49:77c8e4604045	910	}
mbed_official	49:77c8e4604045	911
mbed_official	49:77c8e4604045	912	/// Start or restart timer
mbed_official	49:77c8e4604045	913	osStatus svcTimerStart (osTimerId timer_id, uint32_t millisec) {
mbed_official	49:77c8e4604045	914	os_timer_cb *pt;
mbed_official	49:77c8e4604045	915	uint32_t tcnt;
mbed_official	49:77c8e4604045	916
mbed_official	49:77c8e4604045	917	pt = rt_id2obj(timer_id);
mbed_official	49:77c8e4604045	918	if (pt == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	919
mbed_official	49:77c8e4604045	920	tcnt = rt_ms2tick(millisec);
mbed_official	49:77c8e4604045	921	if (tcnt == 0) return osErrorValue;
mbed_official	49:77c8e4604045	922
mbed_official	49:77c8e4604045	923	switch (pt->state) {
mbed_official	49:77c8e4604045	924	case osTimerRunning:
mbed_official	49:77c8e4604045	925	if (rt_timer_remove(pt) != 0) {
mbed_official	49:77c8e4604045	926	return osErrorResource;
mbed_official	49:77c8e4604045	927	}
mbed_official	49:77c8e4604045	928	break;
mbed_official	49:77c8e4604045	929	case osTimerStopped:
mbed_official	49:77c8e4604045	930	pt->state = osTimerRunning;
mbed_official	49:77c8e4604045	931	pt->icnt = (uint16_t)tcnt;
mbed_official	49:77c8e4604045	932	break;
mbed_official	49:77c8e4604045	933	default:
mbed_official	49:77c8e4604045	934	return osErrorResource;
mbed_official	49:77c8e4604045	935	}
mbed_official	49:77c8e4604045	936
mbed_official	49:77c8e4604045	937	rt_timer_insert(pt, tcnt);
mbed_official	49:77c8e4604045	938
mbed_official	49:77c8e4604045	939	return osOK;
mbed_official	49:77c8e4604045	940	}
mbed_official	49:77c8e4604045	941
mbed_official	49:77c8e4604045	942	/// Stop timer
mbed_official	49:77c8e4604045	943	osStatus svcTimerStop (osTimerId timer_id) {
mbed_official	49:77c8e4604045	944	os_timer_cb *pt;
mbed_official	49:77c8e4604045	945
mbed_official	49:77c8e4604045	946	pt = rt_id2obj(timer_id);
mbed_official	49:77c8e4604045	947	if (pt == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	948
mbed_official	49:77c8e4604045	949	if (pt->state != osTimerRunning) return osErrorResource;
mbed_official	49:77c8e4604045	950
mbed_official	49:77c8e4604045	951	pt->state = osTimerStopped;
mbed_official	49:77c8e4604045	952
mbed_official	49:77c8e4604045	953	if (rt_timer_remove(pt) != 0) {
mbed_official	49:77c8e4604045	954	return osErrorResource;
mbed_official	49:77c8e4604045	955	}
mbed_official	49:77c8e4604045	956
mbed_official	49:77c8e4604045	957	return osOK;
mbed_official	49:77c8e4604045	958	}
mbed_official	49:77c8e4604045	959
mbed_official	49:77c8e4604045	960	/// Delete timer
mbed_official	49:77c8e4604045	961	osStatus svcTimerDelete (osTimerId timer_id) {
mbed_official	49:77c8e4604045	962	os_timer_cb *pt;
mbed_official	49:77c8e4604045	963
mbed_official	49:77c8e4604045	964	pt = rt_id2obj(timer_id);
mbed_official	49:77c8e4604045	965	if (pt == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	966
mbed_official	49:77c8e4604045	967	switch (pt->state) {
mbed_official	49:77c8e4604045	968	case osTimerRunning:
mbed_official	49:77c8e4604045	969	rt_timer_remove(pt);
mbed_official	49:77c8e4604045	970	break;
mbed_official	49:77c8e4604045	971	case osTimerStopped:
mbed_official	49:77c8e4604045	972	break;
mbed_official	49:77c8e4604045	973	default:
mbed_official	49:77c8e4604045	974	return osErrorResource;
mbed_official	49:77c8e4604045	975	}
mbed_official	49:77c8e4604045	976
mbed_official	49:77c8e4604045	977	pt->state = osTimerInvalid;
mbed_official	49:77c8e4604045	978
mbed_official	49:77c8e4604045	979	return osOK;
mbed_official	49:77c8e4604045	980	}
mbed_official	49:77c8e4604045	981
mbed_official	49:77c8e4604045	982	/// Get timer callback parameters
mbed_official	49:77c8e4604045	983	os_InRegs osCallback_type svcTimerCall (osTimerId timer_id) {
mbed_official	49:77c8e4604045	984	os_timer_cb *pt;
mbed_official	49:77c8e4604045	985	osCallback ret;
mbed_official	49:77c8e4604045	986
mbed_official	49:77c8e4604045	987	pt = rt_id2obj(timer_id);
mbed_official	49:77c8e4604045	988	if (pt == NULL) {
mbed_official	49:77c8e4604045	989	ret.fp = NULL;
mbed_official	49:77c8e4604045	990	ret.arg = NULL;
mbed_official	49:77c8e4604045	991	return osCallback_ret;
mbed_official	49:77c8e4604045	992	}
mbed_official	49:77c8e4604045	993
mbed_official	49:77c8e4604045	994	ret.fp = (void *)pt->timer->ptimer;
mbed_official	49:77c8e4604045	995	ret.arg = pt->arg;
mbed_official	49:77c8e4604045	996
mbed_official	49:77c8e4604045	997	return osCallback_ret;
mbed_official	49:77c8e4604045	998	}
mbed_official	49:77c8e4604045	999
mbed_official	49:77c8e4604045	1000	static __INLINE osStatus isrMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec);
mbed_official	49:77c8e4604045	1001
mbed_official	49:77c8e4604045	1002	/// Timer Tick (called each SysTick)
mbed_official	49:77c8e4604045	1003	void sysTimerTick (void) {
mbed_official	49:77c8e4604045	1004	os_timer_cb pt, p;
mbed_official	49:77c8e4604045	1005
mbed_official	49:77c8e4604045	1006	p = os_timer_head;
mbed_official	49:77c8e4604045	1007	if (p == NULL) return;
mbed_official	49:77c8e4604045	1008
mbed_official	49:77c8e4604045	1009	p->tcnt--;
mbed_official	49:77c8e4604045	1010	while ((p != NULL) && (p->tcnt == 0)) {
mbed_official	49:77c8e4604045	1011	pt = p;
mbed_official	49:77c8e4604045	1012	p = p->next;
mbed_official	49:77c8e4604045	1013	os_timer_head = p;
mbed_official	49:77c8e4604045	1014	isrMessagePut(osMessageQId_osTimerMessageQ, (uint32_t)pt, 0);
mbed_official	49:77c8e4604045	1015	if (pt->type == osTimerPeriodic) {
mbed_official	49:77c8e4604045	1016	rt_timer_insert(pt, pt->icnt);
mbed_official	49:77c8e4604045	1017	} else {
mbed_official	49:77c8e4604045	1018	pt->state = osTimerStopped;
mbed_official	49:77c8e4604045	1019	}
mbed_official	49:77c8e4604045	1020	}
mbed_official	49:77c8e4604045	1021	}
mbed_official	49:77c8e4604045	1022
mbed_official	49:77c8e4604045	1023
mbed_official	49:77c8e4604045	1024	// Timer Management Public API
mbed_official	49:77c8e4604045	1025
mbed_official	49:77c8e4604045	1026	/// Create timer
mbed_official	49:77c8e4604045	1027	osTimerId osTimerCreate (osTimerDef_t timer_def, os_timer_type type, void argument) {
mbed_official	49:77c8e4604045	1028	if (__get_IPSR() != 0) return NULL; // Not allowed in ISR
mbed_official	49:77c8e4604045	1029	if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) {
mbed_official	49:77c8e4604045	1030	// Privileged and not running
mbed_official	49:77c8e4604045	1031	return svcTimerCreate(timer_def, type, argument);
mbed_official	49:77c8e4604045	1032	} else {
mbed_official	49:77c8e4604045	1033	return __svcTimerCreate(timer_def, type, argument);
mbed_official	49:77c8e4604045	1034	}
mbed_official	49:77c8e4604045	1035	}
mbed_official	49:77c8e4604045	1036
mbed_official	49:77c8e4604045	1037	/// Start or restart timer
mbed_official	49:77c8e4604045	1038	osStatus osTimerStart (osTimerId timer_id, uint32_t millisec) {
mbed_official	49:77c8e4604045	1039	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	1040	return __svcTimerStart(timer_id, millisec);
mbed_official	49:77c8e4604045	1041	}
mbed_official	49:77c8e4604045	1042
mbed_official	49:77c8e4604045	1043	/// Stop timer
mbed_official	49:77c8e4604045	1044	osStatus osTimerStop (osTimerId timer_id) {
mbed_official	49:77c8e4604045	1045	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	1046	return __svcTimerStop(timer_id);
mbed_official	49:77c8e4604045	1047	}
mbed_official	49:77c8e4604045	1048
mbed_official	49:77c8e4604045	1049	/// Delete timer
mbed_official	49:77c8e4604045	1050	osStatus osTimerDelete (osTimerId timer_id) {
mbed_official	49:77c8e4604045	1051	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	1052	return __svcTimerDelete(timer_id);
mbed_official	49:77c8e4604045	1053	}
mbed_official	49:77c8e4604045	1054
mbed_official	49:77c8e4604045	1055	/// INTERNAL - Not Public
mbed_official	49:77c8e4604045	1056	/// Get timer callback parameters (used by OS Timer Thread)
mbed_official	49:77c8e4604045	1057	os_InRegs osCallback osTimerCall (osTimerId timer_id) {
mbed_official	49:77c8e4604045	1058	return __svcTimerCall(timer_id);
mbed_official	49:77c8e4604045	1059	}
mbed_official	49:77c8e4604045	1060
mbed_official	49:77c8e4604045	1061
mbed_official	49:77c8e4604045	1062	// Timer Thread
mbed_official	49:77c8e4604045	1063	__NO_RETURN void osTimerThread (void const *argument) {
mbed_official	49:77c8e4604045	1064	osCallback cb;
mbed_official	49:77c8e4604045	1065	osEvent evt;
mbed_official	49:77c8e4604045	1066
mbed_official	49:77c8e4604045	1067	for (;;) {
mbed_official	49:77c8e4604045	1068	evt = osMessageGet(osMessageQId_osTimerMessageQ, osWaitForever);
mbed_official	49:77c8e4604045	1069	if (evt.status == osEventMessage) {
mbed_official	49:77c8e4604045	1070	cb = osTimerCall(evt.value.p);
mbed_official	49:77c8e4604045	1071	if (cb.fp != NULL) {
mbed_official	49:77c8e4604045	1072	(*(os_ptimer)cb.fp)(cb.arg);
mbed_official	49:77c8e4604045	1073	}
mbed_official	49:77c8e4604045	1074	}
mbed_official	49:77c8e4604045	1075	}
mbed_official	49:77c8e4604045	1076	}
mbed_official	49:77c8e4604045	1077
mbed_official	49:77c8e4604045	1078
mbed_official	49:77c8e4604045	1079	// ==== Signal Management ====
mbed_official	49:77c8e4604045	1080
mbed_official	49:77c8e4604045	1081	// Signal Service Calls declarations
mbed_official	49:77c8e4604045	1082	SVC_2_1(svcSignalSet, int32_t, osThreadId, int32_t, RET_int32_t)
mbed_official	49:77c8e4604045	1083	SVC_2_1(svcSignalClear, int32_t, osThreadId, int32_t, RET_int32_t)
mbed_official	49:77c8e4604045	1084	SVC_1_1(svcSignalGet, int32_t, osThreadId, RET_int32_t)
mbed_official	49:77c8e4604045	1085	SVC_2_3(svcSignalWait, os_InRegs osEvent, int32_t, uint32_t, RET_osEvent)
mbed_official	49:77c8e4604045	1086
mbed_official	49:77c8e4604045	1087	// Signal Service Calls
mbed_official	49:77c8e4604045	1088
mbed_official	49:77c8e4604045	1089	/// Set the specified Signal Flags of an active thread
mbed_official	49:77c8e4604045	1090	int32_t svcSignalSet (osThreadId thread_id, int32_t signals) {
mbed_official	49:77c8e4604045	1091	P_TCB ptcb;
mbed_official	49:77c8e4604045	1092	int32_t sig;
mbed_official	49:77c8e4604045	1093
mbed_official	49:77c8e4604045	1094	ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
mbed_official	49:77c8e4604045	1095	if (ptcb == NULL) return 0x80000000;
mbed_official	49:77c8e4604045	1096
mbed_official	49:77c8e4604045	1097	if (signals & (0xFFFFFFFF << osFeature_Signals)) return 0x80000000;
mbed_official	49:77c8e4604045	1098
mbed_official	49:77c8e4604045	1099	sig = ptcb->events; // Previous signal flags
mbed_official	49:77c8e4604045	1100
mbed_official	49:77c8e4604045	1101	rt_evt_set(signals, ptcb->task_id); // Set event flags
mbed_official	49:77c8e4604045	1102
mbed_official	49:77c8e4604045	1103	return sig;
mbed_official	49:77c8e4604045	1104	}
mbed_official	49:77c8e4604045	1105
mbed_official	49:77c8e4604045	1106	/// Clear the specified Signal Flags of an active thread
mbed_official	49:77c8e4604045	1107	int32_t svcSignalClear (osThreadId thread_id, int32_t signals) {
mbed_official	49:77c8e4604045	1108	P_TCB ptcb;
mbed_official	49:77c8e4604045	1109	int32_t sig;
mbed_official	49:77c8e4604045	1110
mbed_official	49:77c8e4604045	1111	ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
mbed_official	49:77c8e4604045	1112	if (ptcb == NULL) return 0x80000000;
mbed_official	49:77c8e4604045	1113
mbed_official	49:77c8e4604045	1114	if (signals & (0xFFFFFFFF << osFeature_Signals)) return 0x80000000;
mbed_official	49:77c8e4604045	1115
mbed_official	49:77c8e4604045	1116	sig = ptcb->events; // Previous signal flags
mbed_official	49:77c8e4604045	1117
mbed_official	49:77c8e4604045	1118	rt_evt_clr(signals, ptcb->task_id); // Clear event flags
mbed_official	49:77c8e4604045	1119
mbed_official	49:77c8e4604045	1120	return sig;
mbed_official	49:77c8e4604045	1121	}
mbed_official	49:77c8e4604045	1122
mbed_official	49:77c8e4604045	1123	/// Get Signal Flags status of an active thread
mbed_official	49:77c8e4604045	1124	int32_t svcSignalGet (osThreadId thread_id) {
mbed_official	49:77c8e4604045	1125	P_TCB ptcb;
mbed_official	49:77c8e4604045	1126
mbed_official	49:77c8e4604045	1127	ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
mbed_official	49:77c8e4604045	1128	if (ptcb == NULL) return 0x80000000;
mbed_official	49:77c8e4604045	1129
mbed_official	49:77c8e4604045	1130	return ptcb->events; // Return event flags
mbed_official	49:77c8e4604045	1131	}
mbed_official	49:77c8e4604045	1132
mbed_official	49:77c8e4604045	1133	/// Wait for one or more Signal Flags to become signaled for the current RUNNING thread
mbed_official	49:77c8e4604045	1134	os_InRegs osEvent_type svcSignalWait (int32_t signals, uint32_t millisec) {
mbed_official	49:77c8e4604045	1135	OS_RESULT res;
mbed_official	49:77c8e4604045	1136	osEvent ret;
mbed_official	49:77c8e4604045	1137
mbed_official	49:77c8e4604045	1138	if (signals & (0xFFFFFFFF << osFeature_Signals)) {
mbed_official	49:77c8e4604045	1139	ret.status = osErrorValue;
mbed_official	49:77c8e4604045	1140	return osEvent_ret_status;
mbed_official	49:77c8e4604045	1141	}
mbed_official	49:77c8e4604045	1142
mbed_official	49:77c8e4604045	1143	if (signals != 0) { // Wait for all specified signals
mbed_official	49:77c8e4604045	1144	res = rt_evt_wait(signals, rt_ms2tick(millisec), __TRUE);
mbed_official	49:77c8e4604045	1145	} else { // Wait for any signal
mbed_official	49:77c8e4604045	1146	res = rt_evt_wait(0xFFFF, rt_ms2tick(millisec), __FALSE);
mbed_official	49:77c8e4604045	1147	}
mbed_official	49:77c8e4604045	1148
mbed_official	49:77c8e4604045	1149	if (res == OS_R_EVT) {
mbed_official	49:77c8e4604045	1150	ret.status = osEventSignal;
mbed_official	49:77c8e4604045	1151	ret.value.signals = signals ? signals : os_tsk.run->waits;
mbed_official	49:77c8e4604045	1152	} else {
mbed_official	49:77c8e4604045	1153	ret.status = millisec ? osEventTimeout : osOK;
mbed_official	49:77c8e4604045	1154	ret.value.signals = 0;
mbed_official	49:77c8e4604045	1155	}
mbed_official	49:77c8e4604045	1156
mbed_official	49:77c8e4604045	1157	return osEvent_ret_value;
mbed_official	49:77c8e4604045	1158	}
mbed_official	49:77c8e4604045	1159
mbed_official	49:77c8e4604045	1160
mbed_official	49:77c8e4604045	1161	// Signal ISR Calls
mbed_official	49:77c8e4604045	1162
mbed_official	49:77c8e4604045	1163	/// Set the specified Signal Flags of an active thread
mbed_official	49:77c8e4604045	1164	static __INLINE int32_t isrSignalSet (osThreadId thread_id, int32_t signals) {
mbed_official	49:77c8e4604045	1165	P_TCB ptcb;
mbed_official	49:77c8e4604045	1166	int32_t sig;
mbed_official	49:77c8e4604045	1167
mbed_official	49:77c8e4604045	1168	ptcb = rt_tid2ptcb(thread_id); // Get TCB pointer
mbed_official	49:77c8e4604045	1169	if (ptcb == NULL) return 0x80000000;
mbed_official	49:77c8e4604045	1170
mbed_official	49:77c8e4604045	1171	if (signals & (0xFFFFFFFF << osFeature_Signals)) return 0x80000000;
mbed_official	49:77c8e4604045	1172
mbed_official	49:77c8e4604045	1173	sig = ptcb->events; // Previous signal flags
mbed_official	49:77c8e4604045	1174
mbed_official	49:77c8e4604045	1175	isr_evt_set(signals, ptcb->task_id); // Set event flags
mbed_official	49:77c8e4604045	1176
mbed_official	49:77c8e4604045	1177	return sig;
mbed_official	49:77c8e4604045	1178	}
mbed_official	49:77c8e4604045	1179
mbed_official	49:77c8e4604045	1180
mbed_official	49:77c8e4604045	1181	// Signal Public API
mbed_official	49:77c8e4604045	1182
mbed_official	49:77c8e4604045	1183	/// Set the specified Signal Flags of an active thread
mbed_official	49:77c8e4604045	1184	int32_t osSignalSet (osThreadId thread_id, int32_t signals) {
mbed_official	49:77c8e4604045	1185	if (__get_IPSR() != 0) { // in ISR
mbed_official	49:77c8e4604045	1186	return isrSignalSet(thread_id, signals);
mbed_official	49:77c8e4604045	1187	} else { // in Thread
mbed_official	49:77c8e4604045	1188	return __svcSignalSet(thread_id, signals);
mbed_official	49:77c8e4604045	1189	}
mbed_official	49:77c8e4604045	1190	}
mbed_official	49:77c8e4604045	1191
mbed_official	49:77c8e4604045	1192	/// Clear the specified Signal Flags of an active thread
mbed_official	49:77c8e4604045	1193	int32_t osSignalClear (osThreadId thread_id, int32_t signals) {
mbed_official	49:77c8e4604045	1194	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	1195	return __svcSignalClear(thread_id, signals);
mbed_official	49:77c8e4604045	1196	}
mbed_official	49:77c8e4604045	1197
mbed_official	49:77c8e4604045	1198	/// Get Signal Flags status of an active thread
mbed_official	49:77c8e4604045	1199	int32_t osSignalGet (osThreadId thread_id) {
mbed_official	49:77c8e4604045	1200	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	1201	return __svcSignalGet(thread_id);
mbed_official	49:77c8e4604045	1202	}
mbed_official	49:77c8e4604045	1203
mbed_official	49:77c8e4604045	1204	/// Wait for one or more Signal Flags to become signaled for the current RUNNING thread
mbed_official	49:77c8e4604045	1205	os_InRegs osEvent osSignalWait (int32_t signals, uint32_t millisec) {
mbed_official	49:77c8e4604045	1206	osEvent ret;
mbed_official	49:77c8e4604045	1207
mbed_official	49:77c8e4604045	1208	if (__get_IPSR() != 0) { // Not allowed in ISR
mbed_official	49:77c8e4604045	1209	ret.status = osErrorISR;
mbed_official	49:77c8e4604045	1210	return ret;
mbed_official	49:77c8e4604045	1211	}
mbed_official	49:77c8e4604045	1212	return __svcSignalWait(signals, millisec);
mbed_official	49:77c8e4604045	1213	}
mbed_official	49:77c8e4604045	1214
mbed_official	49:77c8e4604045	1215
mbed_official	49:77c8e4604045	1216	// ==== Mutex Management ====
mbed_official	49:77c8e4604045	1217
mbed_official	49:77c8e4604045	1218	// Mutex Service Calls declarations
mbed_official	49:77c8e4604045	1219	SVC_1_1(svcMutexCreate, osMutexId, osMutexDef_t *, RET_pointer)
mbed_official	49:77c8e4604045	1220	SVC_2_1(svcMutexWait, osStatus, osMutexId, uint32_t, RET_osStatus)
mbed_official	49:77c8e4604045	1221	SVC_1_1(svcMutexRelease, osStatus, osMutexId, RET_osStatus)
mbed_official	49:77c8e4604045	1222	SVC_1_1(svcMutexDelete, osStatus, osMutexId, RET_osStatus)
mbed_official	49:77c8e4604045	1223
mbed_official	49:77c8e4604045	1224	// Mutex Service Calls
mbed_official	49:77c8e4604045	1225
mbed_official	49:77c8e4604045	1226	/// Create and Initialize a Mutex object
mbed_official	49:77c8e4604045	1227	osMutexId svcMutexCreate (osMutexDef_t *mutex_def) {
mbed_official	49:77c8e4604045	1228	OS_ID mut;
mbed_official	49:77c8e4604045	1229
mbed_official	49:77c8e4604045	1230	if (mutex_def == NULL) {
mbed_official	49:77c8e4604045	1231	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	1232	return NULL;
mbed_official	49:77c8e4604045	1233	}
mbed_official	49:77c8e4604045	1234
mbed_official	49:77c8e4604045	1235	mut = mutex_def->mutex;
mbed_official	49:77c8e4604045	1236	if (mut == NULL) {
mbed_official	49:77c8e4604045	1237	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	1238	return NULL;
mbed_official	49:77c8e4604045	1239	}
mbed_official	49:77c8e4604045	1240
mbed_official	49:77c8e4604045	1241	if (((P_MUCB)mut)->cb_type != 0) {
mbed_official	49:77c8e4604045	1242	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	1243	return NULL;
mbed_official	49:77c8e4604045	1244	}
mbed_official	49:77c8e4604045	1245
mbed_official	49:77c8e4604045	1246	rt_mut_init(mut); // Initialize Mutex
mbed_official	49:77c8e4604045	1247
mbed_official	49:77c8e4604045	1248	return mut;
mbed_official	49:77c8e4604045	1249	}
mbed_official	49:77c8e4604045	1250
mbed_official	49:77c8e4604045	1251	/// Wait until a Mutex becomes available
mbed_official	49:77c8e4604045	1252	osStatus svcMutexWait (osMutexId mutex_id, uint32_t millisec) {
mbed_official	49:77c8e4604045	1253	OS_ID mut;
mbed_official	49:77c8e4604045	1254	OS_RESULT res;
mbed_official	49:77c8e4604045	1255
mbed_official	49:77c8e4604045	1256	mut = rt_id2obj(mutex_id);
mbed_official	49:77c8e4604045	1257	if (mut == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	1258
mbed_official	49:77c8e4604045	1259	if (((P_MUCB)mut)->cb_type != MUCB) return osErrorParameter;
mbed_official	49:77c8e4604045	1260
mbed_official	49:77c8e4604045	1261	res = rt_mut_wait(mut, rt_ms2tick(millisec)); // Wait for Mutex
mbed_official	49:77c8e4604045	1262
mbed_official	49:77c8e4604045	1263	if (res == OS_R_TMO) {
mbed_official	49:77c8e4604045	1264	return (millisec ? osErrorTimeoutResource : osErrorResource);
mbed_official	49:77c8e4604045	1265	}
mbed_official	49:77c8e4604045	1266
mbed_official	49:77c8e4604045	1267	return osOK;
mbed_official	49:77c8e4604045	1268	}
mbed_official	49:77c8e4604045	1269
mbed_official	49:77c8e4604045	1270	/// Release a Mutex that was obtained with osMutexWait
mbed_official	49:77c8e4604045	1271	osStatus svcMutexRelease (osMutexId mutex_id) {
mbed_official	49:77c8e4604045	1272	OS_ID mut;
mbed_official	49:77c8e4604045	1273	OS_RESULT res;
mbed_official	49:77c8e4604045	1274
mbed_official	49:77c8e4604045	1275	mut = rt_id2obj(mutex_id);
mbed_official	49:77c8e4604045	1276	if (mut == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	1277
mbed_official	49:77c8e4604045	1278	if (((P_MUCB)mut)->cb_type != MUCB) return osErrorParameter;
mbed_official	49:77c8e4604045	1279
mbed_official	49:77c8e4604045	1280	res = rt_mut_release(mut); // Release Mutex
mbed_official	49:77c8e4604045	1281
mbed_official	49:77c8e4604045	1282	if (res == OS_R_NOK) return osErrorResource; // Thread not owner or Zero Counter
mbed_official	49:77c8e4604045	1283
mbed_official	49:77c8e4604045	1284	return osOK;
mbed_official	49:77c8e4604045	1285	}
mbed_official	49:77c8e4604045	1286
mbed_official	49:77c8e4604045	1287	/// Delete a Mutex that was created by osMutexCreate
mbed_official	49:77c8e4604045	1288	osStatus svcMutexDelete (osMutexId mutex_id) {
mbed_official	49:77c8e4604045	1289	OS_ID mut;
mbed_official	49:77c8e4604045	1290
mbed_official	49:77c8e4604045	1291	mut = rt_id2obj(mutex_id);
mbed_official	49:77c8e4604045	1292	if (mut == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	1293
mbed_official	49:77c8e4604045	1294	if (((P_MUCB)mut)->cb_type != MUCB) return osErrorParameter;
mbed_official	49:77c8e4604045	1295
mbed_official	49:77c8e4604045	1296	rt_mut_delete(mut); // Release Mutex
mbed_official	49:77c8e4604045	1297
mbed_official	49:77c8e4604045	1298	return osOK;
mbed_official	49:77c8e4604045	1299	}
mbed_official	49:77c8e4604045	1300
mbed_official	49:77c8e4604045	1301
mbed_official	49:77c8e4604045	1302	// Mutex Public API
mbed_official	49:77c8e4604045	1303
mbed_official	49:77c8e4604045	1304	/// Create and Initialize a Mutex object
mbed_official	49:77c8e4604045	1305	osMutexId osMutexCreate (osMutexDef_t *mutex_def) {
mbed_official	49:77c8e4604045	1306	if (__get_IPSR() != 0) return NULL; // Not allowed in ISR
mbed_official	49:77c8e4604045	1307	if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) {
mbed_official	49:77c8e4604045	1308	// Privileged and not running
mbed_official	49:77c8e4604045	1309	return svcMutexCreate(mutex_def);
mbed_official	49:77c8e4604045	1310	} else {
mbed_official	49:77c8e4604045	1311	return __svcMutexCreate(mutex_def);
mbed_official	49:77c8e4604045	1312	}
mbed_official	49:77c8e4604045	1313	}
mbed_official	49:77c8e4604045	1314
mbed_official	49:77c8e4604045	1315	/// Wait until a Mutex becomes available
mbed_official	49:77c8e4604045	1316	osStatus osMutexWait (osMutexId mutex_id, uint32_t millisec) {
mbed_official	49:77c8e4604045	1317	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	1318	return __svcMutexWait(mutex_id, millisec);
mbed_official	49:77c8e4604045	1319	}
mbed_official	49:77c8e4604045	1320
mbed_official	49:77c8e4604045	1321	/// Release a Mutex that was obtained with osMutexWait
mbed_official	49:77c8e4604045	1322	osStatus osMutexRelease (osMutexId mutex_id) {
mbed_official	49:77c8e4604045	1323	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	1324	return __svcMutexRelease(mutex_id);
mbed_official	49:77c8e4604045	1325	}
mbed_official	49:77c8e4604045	1326
mbed_official	49:77c8e4604045	1327	/// Delete a Mutex that was created by osMutexCreate
mbed_official	49:77c8e4604045	1328	osStatus osMutexDelete (osMutexId mutex_id) {
mbed_official	49:77c8e4604045	1329	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	1330	return __svcMutexDelete(mutex_id);
mbed_official	49:77c8e4604045	1331	}
mbed_official	49:77c8e4604045	1332
mbed_official	49:77c8e4604045	1333
mbed_official	49:77c8e4604045	1334	// ==== Semaphore Management ====
mbed_official	49:77c8e4604045	1335
mbed_official	49:77c8e4604045	1336	// Semaphore Service Calls declarations
mbed_official	49:77c8e4604045	1337	SVC_2_1(svcSemaphoreCreate, osSemaphoreId, const osSemaphoreDef_t *, int32_t, RET_pointer)
mbed_official	49:77c8e4604045	1338	SVC_2_1(svcSemaphoreWait, int32_t, osSemaphoreId, uint32_t, RET_int32_t)
mbed_official	49:77c8e4604045	1339	SVC_1_1(svcSemaphoreRelease, osStatus, osSemaphoreId, RET_osStatus)
mbed_official	49:77c8e4604045	1340	SVC_1_1(svcSemaphoreDelete, osStatus, osSemaphoreId, RET_osStatus)
mbed_official	49:77c8e4604045	1341
mbed_official	49:77c8e4604045	1342	// Semaphore Service Calls
mbed_official	49:77c8e4604045	1343
mbed_official	49:77c8e4604045	1344	/// Create and Initialize a Semaphore object
mbed_official	49:77c8e4604045	1345	osSemaphoreId svcSemaphoreCreate (const osSemaphoreDef_t *semaphore_def, int32_t count) {
mbed_official	49:77c8e4604045	1346	OS_ID sem;
mbed_official	49:77c8e4604045	1347
mbed_official	49:77c8e4604045	1348	if (semaphore_def == NULL) {
mbed_official	49:77c8e4604045	1349	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	1350	return NULL;
mbed_official	49:77c8e4604045	1351	}
mbed_official	49:77c8e4604045	1352
mbed_official	49:77c8e4604045	1353	sem = semaphore_def->semaphore;
mbed_official	49:77c8e4604045	1354	if (sem == NULL) {
mbed_official	49:77c8e4604045	1355	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	1356	return NULL;
mbed_official	49:77c8e4604045	1357	}
mbed_official	49:77c8e4604045	1358
mbed_official	49:77c8e4604045	1359	if (((P_SCB)sem)->cb_type != 0) {
mbed_official	49:77c8e4604045	1360	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	1361	return NULL;
mbed_official	49:77c8e4604045	1362	}
mbed_official	49:77c8e4604045	1363
mbed_official	49:77c8e4604045	1364	if (count > osFeature_Semaphore) {
mbed_official	49:77c8e4604045	1365	sysThreadError(osErrorValue);
mbed_official	49:77c8e4604045	1366	return NULL;
mbed_official	49:77c8e4604045	1367	}
mbed_official	49:77c8e4604045	1368
mbed_official	49:77c8e4604045	1369	rt_sem_init(sem, count); // Initialize Semaphore
mbed_official	49:77c8e4604045	1370
mbed_official	49:77c8e4604045	1371	return sem;
mbed_official	49:77c8e4604045	1372	}
mbed_official	49:77c8e4604045	1373
mbed_official	49:77c8e4604045	1374	/// Wait until a Semaphore becomes available
mbed_official	49:77c8e4604045	1375	int32_t svcSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec) {
mbed_official	49:77c8e4604045	1376	OS_ID sem;
mbed_official	49:77c8e4604045	1377	OS_RESULT res;
mbed_official	49:77c8e4604045	1378
mbed_official	49:77c8e4604045	1379	sem = rt_id2obj(semaphore_id);
mbed_official	49:77c8e4604045	1380	if (sem == NULL) return -1;
mbed_official	49:77c8e4604045	1381
mbed_official	49:77c8e4604045	1382	if (((P_SCB)sem)->cb_type != SCB) return -1;
mbed_official	49:77c8e4604045	1383
mbed_official	49:77c8e4604045	1384	res = rt_sem_wait(sem, rt_ms2tick(millisec)); // Wait for Semaphore
mbed_official	49:77c8e4604045	1385
mbed_official	49:77c8e4604045	1386	if (res == OS_R_TMO) return 0; // Timeout
mbed_official	49:77c8e4604045	1387
mbed_official	49:77c8e4604045	1388	return (((P_SCB)sem)->tokens + 1);
mbed_official	49:77c8e4604045	1389	}
mbed_official	49:77c8e4604045	1390
mbed_official	49:77c8e4604045	1391	/// Release a Semaphore
mbed_official	49:77c8e4604045	1392	osStatus svcSemaphoreRelease (osSemaphoreId semaphore_id) {
mbed_official	49:77c8e4604045	1393	OS_ID sem;
mbed_official	49:77c8e4604045	1394
mbed_official	49:77c8e4604045	1395	sem = rt_id2obj(semaphore_id);
mbed_official	49:77c8e4604045	1396	if (sem == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	1397
mbed_official	49:77c8e4604045	1398	if (((P_SCB)sem)->cb_type != SCB) return osErrorParameter;
mbed_official	49:77c8e4604045	1399
mbed_official	49:77c8e4604045	1400	if (((P_SCB)sem)->tokens == osFeature_Semaphore) return osErrorResource;
mbed_official	49:77c8e4604045	1401
mbed_official	49:77c8e4604045	1402	rt_sem_send(sem); // Release Semaphore
mbed_official	49:77c8e4604045	1403
mbed_official	49:77c8e4604045	1404	return osOK;
mbed_official	49:77c8e4604045	1405	}
mbed_official	49:77c8e4604045	1406
mbed_official	49:77c8e4604045	1407	/// Delete a Semaphore that was created by osSemaphoreCreate
mbed_official	49:77c8e4604045	1408	osStatus svcSemaphoreDelete (osSemaphoreId semaphore_id) {
mbed_official	49:77c8e4604045	1409	OS_ID sem;
mbed_official	49:77c8e4604045	1410
mbed_official	49:77c8e4604045	1411	sem = rt_id2obj(semaphore_id);
mbed_official	49:77c8e4604045	1412	if (sem == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	1413
mbed_official	49:77c8e4604045	1414	if (((P_SCB)sem)->cb_type != SCB) return osErrorParameter;
mbed_official	49:77c8e4604045	1415
mbed_official	49:77c8e4604045	1416	rt_sem_delete(sem); // Delete Semaphore
mbed_official	49:77c8e4604045	1417
mbed_official	49:77c8e4604045	1418	return osOK;
mbed_official	49:77c8e4604045	1419	}
mbed_official	49:77c8e4604045	1420
mbed_official	49:77c8e4604045	1421
mbed_official	49:77c8e4604045	1422	// Semaphore ISR Calls
mbed_official	49:77c8e4604045	1423
mbed_official	49:77c8e4604045	1424	/// Release a Semaphore
mbed_official	49:77c8e4604045	1425	static __INLINE osStatus isrSemaphoreRelease (osSemaphoreId semaphore_id) {
mbed_official	49:77c8e4604045	1426	OS_ID sem;
mbed_official	49:77c8e4604045	1427
mbed_official	49:77c8e4604045	1428	sem = rt_id2obj(semaphore_id);
mbed_official	49:77c8e4604045	1429	if (sem == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	1430
mbed_official	49:77c8e4604045	1431	if (((P_SCB)sem)->cb_type != SCB) return osErrorParameter;
mbed_official	49:77c8e4604045	1432
mbed_official	49:77c8e4604045	1433	if (((P_SCB)sem)->tokens == osFeature_Semaphore) return osErrorResource;
mbed_official	49:77c8e4604045	1434
mbed_official	49:77c8e4604045	1435	isr_sem_send(sem); // Release Semaphore
mbed_official	49:77c8e4604045	1436
mbed_official	49:77c8e4604045	1437	return osOK;
mbed_official	49:77c8e4604045	1438	}
mbed_official	49:77c8e4604045	1439
mbed_official	49:77c8e4604045	1440
mbed_official	49:77c8e4604045	1441	// Semaphore Public API
mbed_official	49:77c8e4604045	1442
mbed_official	49:77c8e4604045	1443	/// Create and Initialize a Semaphore object
mbed_official	49:77c8e4604045	1444	osSemaphoreId osSemaphoreCreate (osSemaphoreDef_t *semaphore_def, int32_t count) {
mbed_official	49:77c8e4604045	1445	if (__get_IPSR() != 0) return NULL; // Not allowed in ISR
mbed_official	49:77c8e4604045	1446	if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) {
mbed_official	49:77c8e4604045	1447	// Privileged and not running
mbed_official	49:77c8e4604045	1448	return svcSemaphoreCreate(semaphore_def, count);
mbed_official	49:77c8e4604045	1449	} else {
mbed_official	49:77c8e4604045	1450	return __svcSemaphoreCreate(semaphore_def, count);
mbed_official	49:77c8e4604045	1451	}
mbed_official	49:77c8e4604045	1452	}
mbed_official	49:77c8e4604045	1453
mbed_official	49:77c8e4604045	1454	/// Wait until a Semaphore becomes available
mbed_official	49:77c8e4604045	1455	int32_t osSemaphoreWait (osSemaphoreId semaphore_id, uint32_t millisec) {
mbed_official	49:77c8e4604045	1456	if (__get_IPSR() != 0) return -1; // Not allowed in ISR
mbed_official	49:77c8e4604045	1457	return __svcSemaphoreWait(semaphore_id, millisec);
mbed_official	49:77c8e4604045	1458	}
mbed_official	49:77c8e4604045	1459
mbed_official	49:77c8e4604045	1460	/// Release a Semaphore
mbed_official	49:77c8e4604045	1461	osStatus osSemaphoreRelease (osSemaphoreId semaphore_id) {
mbed_official	49:77c8e4604045	1462	if (__get_IPSR() != 0) { // in ISR
mbed_official	49:77c8e4604045	1463	return isrSemaphoreRelease(semaphore_id);
mbed_official	49:77c8e4604045	1464	} else { // in Thread
mbed_official	49:77c8e4604045	1465	return __svcSemaphoreRelease(semaphore_id);
mbed_official	49:77c8e4604045	1466	}
mbed_official	49:77c8e4604045	1467	}
mbed_official	49:77c8e4604045	1468
mbed_official	49:77c8e4604045	1469	/// Delete a Semaphore that was created by osSemaphoreCreate
mbed_official	49:77c8e4604045	1470	osStatus osSemaphoreDelete (osSemaphoreId semaphore_id) {
mbed_official	49:77c8e4604045	1471	if (__get_IPSR() != 0) return osErrorISR; // Not allowed in ISR
mbed_official	49:77c8e4604045	1472	return __svcSemaphoreDelete(semaphore_id);
mbed_official	49:77c8e4604045	1473	}
mbed_official	49:77c8e4604045	1474
mbed_official	49:77c8e4604045	1475
mbed_official	49:77c8e4604045	1476	// ==== Memory Management Functions ====
mbed_official	49:77c8e4604045	1477
mbed_official	49:77c8e4604045	1478	// Memory Management Helper Functions
mbed_official	49:77c8e4604045	1479
mbed_official	49:77c8e4604045	1480	// Clear Memory Box (Zero init)
mbed_official	49:77c8e4604045	1481	static void rt_clr_box (void box_mem, void box) {
mbed_official	49:77c8e4604045	1482	uint32_t *p, n;
mbed_official	49:77c8e4604045	1483
mbed_official	49:77c8e4604045	1484	if (box) {
mbed_official	49:77c8e4604045	1485	p = box;
mbed_official	49:77c8e4604045	1486	for (n = ((P_BM)box_mem)->blk_size; n; n -= 4) {
mbed_official	49:77c8e4604045	1487	*p++ = 0;
mbed_official	49:77c8e4604045	1488	}
mbed_official	49:77c8e4604045	1489	}
mbed_official	49:77c8e4604045	1490	}
mbed_official	49:77c8e4604045	1491
mbed_official	49:77c8e4604045	1492	// Memory Management Service Calls declarations
mbed_official	49:77c8e4604045	1493	SVC_1_1(svcPoolCreate, osPoolId, const osPoolDef_t *, RET_pointer)
mbed_official	49:77c8e4604045	1494	SVC_2_1(sysPoolAlloc, void *, osPoolId, uint32_t, RET_pointer)
mbed_official	49:77c8e4604045	1495	SVC_2_1(sysPoolFree, osStatus, osPoolId, void *, RET_osStatus)
mbed_official	49:77c8e4604045	1496
mbed_official	49:77c8e4604045	1497	// Memory Management Service & ISR Calls
mbed_official	49:77c8e4604045	1498
mbed_official	49:77c8e4604045	1499	/// Create and Initialize memory pool
mbed_official	49:77c8e4604045	1500	osPoolId svcPoolCreate (const osPoolDef_t *pool_def) {
mbed_official	49:77c8e4604045	1501	uint32_t blk_sz;
mbed_official	49:77c8e4604045	1502
mbed_official	49:77c8e4604045	1503	if ((pool_def == NULL) \|\|
mbed_official	49:77c8e4604045	1504	(pool_def->pool_sz == 0) \|\|
mbed_official	49:77c8e4604045	1505	(pool_def->item_sz == 0) \|\|
mbed_official	49:77c8e4604045	1506	(pool_def->pool == NULL)) {
mbed_official	49:77c8e4604045	1507	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	1508	return NULL;
mbed_official	49:77c8e4604045	1509	}
mbed_official	49:77c8e4604045	1510
mbed_official	49:77c8e4604045	1511	blk_sz = (pool_def->item_sz + 3) & ~3;
mbed_official	49:77c8e4604045	1512
mbed_official	49:77c8e4604045	1513	_init_box(pool_def->pool, sizeof(struct OS_BM) + pool_def->pool_sz * blk_sz, blk_sz);
mbed_official	49:77c8e4604045	1514
mbed_official	49:77c8e4604045	1515	return pool_def->pool;
mbed_official	49:77c8e4604045	1516	}
mbed_official	49:77c8e4604045	1517
mbed_official	49:77c8e4604045	1518	/// Allocate a memory block from a memory pool
mbed_official	49:77c8e4604045	1519	void *sysPoolAlloc (osPoolId pool_id, uint32_t clr) {
mbed_official	49:77c8e4604045	1520	void *ptr;
mbed_official	49:77c8e4604045	1521
mbed_official	49:77c8e4604045	1522	if (pool_id == NULL) return NULL;
mbed_official	49:77c8e4604045	1523
mbed_official	49:77c8e4604045	1524	ptr = rt_alloc_box(pool_id);
mbed_official	49:77c8e4604045	1525	if (clr) {
mbed_official	49:77c8e4604045	1526	rt_clr_box(pool_id, ptr);
mbed_official	49:77c8e4604045	1527	}
mbed_official	49:77c8e4604045	1528
mbed_official	49:77c8e4604045	1529	return ptr;
mbed_official	49:77c8e4604045	1530	}
mbed_official	49:77c8e4604045	1531
mbed_official	49:77c8e4604045	1532	/// Return an allocated memory block back to a specific memory pool
mbed_official	49:77c8e4604045	1533	osStatus sysPoolFree (osPoolId pool_id, void *block) {
mbed_official	49:77c8e4604045	1534	int32_t res;
mbed_official	49:77c8e4604045	1535
mbed_official	49:77c8e4604045	1536	if (pool_id == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	1537
mbed_official	49:77c8e4604045	1538	res = rt_free_box(pool_id, block);
mbed_official	49:77c8e4604045	1539	if (res != 0) return osErrorValue;
mbed_official	49:77c8e4604045	1540
mbed_official	49:77c8e4604045	1541	return osOK;
mbed_official	49:77c8e4604045	1542	}
mbed_official	49:77c8e4604045	1543
mbed_official	49:77c8e4604045	1544
mbed_official	49:77c8e4604045	1545	// Memory Management Public API
mbed_official	49:77c8e4604045	1546
mbed_official	49:77c8e4604045	1547	/// Create and Initialize memory pool
mbed_official	49:77c8e4604045	1548	osPoolId osPoolCreate (osPoolDef_t *pool_def) {
mbed_official	49:77c8e4604045	1549	if (__get_IPSR() != 0) return NULL; // Not allowed in ISR
mbed_official	49:77c8e4604045	1550	if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) {
mbed_official	49:77c8e4604045	1551	// Privileged and not running
mbed_official	49:77c8e4604045	1552	return svcPoolCreate(pool_def);
mbed_official	49:77c8e4604045	1553	} else {
mbed_official	49:77c8e4604045	1554	return __svcPoolCreate(pool_def);
mbed_official	49:77c8e4604045	1555	}
mbed_official	49:77c8e4604045	1556	}
mbed_official	49:77c8e4604045	1557
mbed_official	49:77c8e4604045	1558	/// Allocate a memory block from a memory pool
mbed_official	49:77c8e4604045	1559	void *osPoolAlloc (osPoolId pool_id) {
mbed_official	49:77c8e4604045	1560	if ((__get_IPSR() != 0) \|\| ((__get_CONTROL() & 1) == 0)) { // in ISR or Privileged
mbed_official	49:77c8e4604045	1561	return sysPoolAlloc(pool_id, 0);
mbed_official	49:77c8e4604045	1562	} else { // in Thread
mbed_official	49:77c8e4604045	1563	return __sysPoolAlloc(pool_id, 0);
mbed_official	49:77c8e4604045	1564	}
mbed_official	49:77c8e4604045	1565	}
mbed_official	49:77c8e4604045	1566
mbed_official	49:77c8e4604045	1567	/// Allocate a memory block from a memory pool and set memory block to zero
mbed_official	49:77c8e4604045	1568	void *osPoolCAlloc (osPoolId pool_id) {
mbed_official	49:77c8e4604045	1569	if ((__get_IPSR() != 0) \|\| ((__get_CONTROL() & 1) == 0)) { // in ISR or Privileged
mbed_official	49:77c8e4604045	1570	return sysPoolAlloc(pool_id, 1);
mbed_official	49:77c8e4604045	1571	} else { // in Thread
mbed_official	49:77c8e4604045	1572	return __sysPoolAlloc(pool_id, 1);
mbed_official	49:77c8e4604045	1573	}
mbed_official	49:77c8e4604045	1574	}
mbed_official	49:77c8e4604045	1575
mbed_official	49:77c8e4604045	1576	/// Return an allocated memory block back to a specific memory pool
mbed_official	49:77c8e4604045	1577	osStatus osPoolFree (osPoolId pool_id, void *block) {
mbed_official	49:77c8e4604045	1578	if ((__get_IPSR() != 0) \|\| ((__get_CONTROL() & 1) == 0)) { // in ISR or Privileged
mbed_official	49:77c8e4604045	1579	return sysPoolFree(pool_id, block);
mbed_official	49:77c8e4604045	1580	} else { // in Thread
mbed_official	49:77c8e4604045	1581	return __sysPoolFree(pool_id, block);
mbed_official	49:77c8e4604045	1582	}
mbed_official	49:77c8e4604045	1583	}
mbed_official	49:77c8e4604045	1584
mbed_official	49:77c8e4604045	1585
mbed_official	49:77c8e4604045	1586	// ==== Message Queue Management Functions ====
mbed_official	49:77c8e4604045	1587
mbed_official	49:77c8e4604045	1588	// Message Queue Management Service Calls declarations
mbed_official	49:77c8e4604045	1589	SVC_2_1(svcMessageCreate, osMessageQId, osMessageQDef_t *, osThreadId, RET_pointer)
mbed_official	49:77c8e4604045	1590	SVC_3_1(svcMessagePut, osStatus, osMessageQId, uint32_t, uint32_t, RET_osStatus)
mbed_official	49:77c8e4604045	1591	SVC_2_3(svcMessageGet, os_InRegs osEvent, osMessageQId, uint32_t, RET_osEvent)
mbed_official	49:77c8e4604045	1592
mbed_official	49:77c8e4604045	1593	// Message Queue Service Calls
mbed_official	49:77c8e4604045	1594
mbed_official	49:77c8e4604045	1595	/// Create and Initialize Message Queue
mbed_official	49:77c8e4604045	1596	osMessageQId svcMessageCreate (osMessageQDef_t *queue_def, osThreadId thread_id) {
mbed_official	49:77c8e4604045	1597
mbed_official	49:77c8e4604045	1598	if ((queue_def == NULL) \|\|
mbed_official	49:77c8e4604045	1599	(queue_def->queue_sz == 0) \|\|
mbed_official	49:77c8e4604045	1600	(queue_def->pool == NULL)) {
mbed_official	49:77c8e4604045	1601	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	1602	return NULL;
mbed_official	49:77c8e4604045	1603	}
mbed_official	49:77c8e4604045	1604
mbed_official	49:77c8e4604045	1605	if (((P_MCB)queue_def->pool)->cb_type != 0) {
mbed_official	49:77c8e4604045	1606	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	1607	return NULL;
mbed_official	49:77c8e4604045	1608	}
mbed_official	49:77c8e4604045	1609
mbed_official	49:77c8e4604045	1610	rt_mbx_init(queue_def->pool, 4*(queue_def->queue_sz + 4));
mbed_official	49:77c8e4604045	1611
mbed_official	49:77c8e4604045	1612	return queue_def->pool;
mbed_official	49:77c8e4604045	1613	}
mbed_official	49:77c8e4604045	1614
mbed_official	49:77c8e4604045	1615	/// Put a Message to a Queue
mbed_official	49:77c8e4604045	1616	osStatus svcMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec) {
mbed_official	49:77c8e4604045	1617	OS_RESULT res;
mbed_official	49:77c8e4604045	1618
mbed_official	49:77c8e4604045	1619	if (queue_id == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	1620
mbed_official	49:77c8e4604045	1621	if (((P_MCB)queue_id)->cb_type != MCB) return osErrorParameter;
mbed_official	49:77c8e4604045	1622
mbed_official	49:77c8e4604045	1623	res = rt_mbx_send(queue_id, (void *)info, rt_ms2tick(millisec));
mbed_official	49:77c8e4604045	1624
mbed_official	49:77c8e4604045	1625	if (res == OS_R_TMO) {
mbed_official	49:77c8e4604045	1626	return (millisec ? osErrorTimeoutResource : osErrorResource);
mbed_official	49:77c8e4604045	1627	}
mbed_official	49:77c8e4604045	1628
mbed_official	49:77c8e4604045	1629	return osOK;
mbed_official	49:77c8e4604045	1630	}
mbed_official	49:77c8e4604045	1631
mbed_official	49:77c8e4604045	1632	/// Get a Message or Wait for a Message from a Queue
mbed_official	49:77c8e4604045	1633	os_InRegs osEvent_type svcMessageGet (osMessageQId queue_id, uint32_t millisec) {
mbed_official	49:77c8e4604045	1634	OS_RESULT res;
mbed_official	49:77c8e4604045	1635	osEvent ret;
mbed_official	49:77c8e4604045	1636
mbed_official	49:77c8e4604045	1637	if (queue_id == NULL) {
mbed_official	49:77c8e4604045	1638	ret.status = osErrorParameter;
mbed_official	49:77c8e4604045	1639	return osEvent_ret_status;
mbed_official	49:77c8e4604045	1640	}
mbed_official	49:77c8e4604045	1641
mbed_official	49:77c8e4604045	1642	if (((P_MCB)queue_id)->cb_type != MCB) {
mbed_official	49:77c8e4604045	1643	ret.status = osErrorParameter;
mbed_official	49:77c8e4604045	1644	return osEvent_ret_status;
mbed_official	49:77c8e4604045	1645	}
mbed_official	49:77c8e4604045	1646
mbed_official	49:77c8e4604045	1647	res = rt_mbx_wait(queue_id, &ret.value.p, rt_ms2tick(millisec));
mbed_official	49:77c8e4604045	1648
mbed_official	49:77c8e4604045	1649	if (res == OS_R_TMO) {
mbed_official	49:77c8e4604045	1650	ret.status = millisec ? osEventTimeout : osOK;
mbed_official	49:77c8e4604045	1651	return osEvent_ret_value;
mbed_official	49:77c8e4604045	1652	}
mbed_official	49:77c8e4604045	1653
mbed_official	49:77c8e4604045	1654	ret.status = osEventMessage;
mbed_official	49:77c8e4604045	1655
mbed_official	49:77c8e4604045	1656	return osEvent_ret_value;
mbed_official	49:77c8e4604045	1657	}
mbed_official	49:77c8e4604045	1658
mbed_official	49:77c8e4604045	1659
mbed_official	49:77c8e4604045	1660	// Message Queue ISR Calls
mbed_official	49:77c8e4604045	1661
mbed_official	49:77c8e4604045	1662	/// Put a Message to a Queue
mbed_official	49:77c8e4604045	1663	static __INLINE osStatus isrMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec) {
mbed_official	49:77c8e4604045	1664
mbed_official	49:77c8e4604045	1665	if ((queue_id == NULL) \|\| (millisec != 0)) {
mbed_official	49:77c8e4604045	1666	return osErrorParameter;
mbed_official	49:77c8e4604045	1667	}
mbed_official	49:77c8e4604045	1668
mbed_official	49:77c8e4604045	1669	if (((P_MCB)queue_id)->cb_type != MCB) return osErrorParameter;
mbed_official	49:77c8e4604045	1670
mbed_official	49:77c8e4604045	1671	if (rt_mbx_check(queue_id) == 0) { // Check if Queue is full
mbed_official	49:77c8e4604045	1672	return osErrorResource;
mbed_official	49:77c8e4604045	1673	}
mbed_official	49:77c8e4604045	1674
mbed_official	49:77c8e4604045	1675	isr_mbx_send(queue_id, (void *)info);
mbed_official	49:77c8e4604045	1676
mbed_official	49:77c8e4604045	1677	return osOK;
mbed_official	49:77c8e4604045	1678	}
mbed_official	49:77c8e4604045	1679
mbed_official	49:77c8e4604045	1680	/// Get a Message or Wait for a Message from a Queue
mbed_official	49:77c8e4604045	1681	static __INLINE os_InRegs osEvent isrMessageGet (osMessageQId queue_id, uint32_t millisec) {
mbed_official	49:77c8e4604045	1682	OS_RESULT res;
mbed_official	49:77c8e4604045	1683	osEvent ret;
mbed_official	49:77c8e4604045	1684
mbed_official	49:77c8e4604045	1685	if ((queue_id == NULL) \|\| (millisec != 0)) {
mbed_official	49:77c8e4604045	1686	ret.status = osErrorParameter;
mbed_official	49:77c8e4604045	1687	return ret;
mbed_official	49:77c8e4604045	1688	}
mbed_official	49:77c8e4604045	1689
mbed_official	49:77c8e4604045	1690	if (((P_MCB)queue_id)->cb_type != MCB) {
mbed_official	49:77c8e4604045	1691	ret.status = osErrorParameter;
mbed_official	49:77c8e4604045	1692	return ret;
mbed_official	49:77c8e4604045	1693	}
mbed_official	49:77c8e4604045	1694
mbed_official	49:77c8e4604045	1695	res = isr_mbx_receive(queue_id, &ret.value.p);
mbed_official	49:77c8e4604045	1696
mbed_official	49:77c8e4604045	1697	if (res != OS_R_MBX) {
mbed_official	49:77c8e4604045	1698	ret.status = osOK;
mbed_official	49:77c8e4604045	1699	return ret;
mbed_official	49:77c8e4604045	1700	}
mbed_official	49:77c8e4604045	1701
mbed_official	49:77c8e4604045	1702	ret.status = osEventMessage;
mbed_official	49:77c8e4604045	1703
mbed_official	49:77c8e4604045	1704	return ret;
mbed_official	49:77c8e4604045	1705	}
mbed_official	49:77c8e4604045	1706
mbed_official	49:77c8e4604045	1707
mbed_official	49:77c8e4604045	1708	// Message Queue Management Public API
mbed_official	49:77c8e4604045	1709
mbed_official	49:77c8e4604045	1710	/// Create and Initialize Message Queue
mbed_official	49:77c8e4604045	1711	osMessageQId osMessageCreate (osMessageQDef_t *queue_def, osThreadId thread_id) {
mbed_official	49:77c8e4604045	1712	if (__get_IPSR() != 0) return NULL; // Not allowed in ISR
mbed_official	49:77c8e4604045	1713	if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) {
mbed_official	49:77c8e4604045	1714	// Privileged and not running
mbed_official	49:77c8e4604045	1715	return svcMessageCreate(queue_def, thread_id);
mbed_official	49:77c8e4604045	1716	} else {
mbed_official	49:77c8e4604045	1717	return __svcMessageCreate(queue_def, thread_id);
mbed_official	49:77c8e4604045	1718	}
mbed_official	49:77c8e4604045	1719	}
mbed_official	49:77c8e4604045	1720
mbed_official	49:77c8e4604045	1721	/// Put a Message to a Queue
mbed_official	49:77c8e4604045	1722	osStatus osMessagePut (osMessageQId queue_id, uint32_t info, uint32_t millisec) {
mbed_official	49:77c8e4604045	1723	if (__get_IPSR() != 0) { // in ISR
mbed_official	49:77c8e4604045	1724	return isrMessagePut(queue_id, info, millisec);
mbed_official	49:77c8e4604045	1725	} else { // in Thread
mbed_official	49:77c8e4604045	1726	return __svcMessagePut(queue_id, info, millisec);
mbed_official	49:77c8e4604045	1727	}
mbed_official	49:77c8e4604045	1728	}
mbed_official	49:77c8e4604045	1729
mbed_official	49:77c8e4604045	1730	/// Get a Message or Wait for a Message from a Queue
mbed_official	49:77c8e4604045	1731	os_InRegs osEvent osMessageGet (osMessageQId queue_id, uint32_t millisec) {
mbed_official	49:77c8e4604045	1732	if (__get_IPSR() != 0) { // in ISR
mbed_official	49:77c8e4604045	1733	return isrMessageGet(queue_id, millisec);
mbed_official	49:77c8e4604045	1734	} else { // in Thread
mbed_official	49:77c8e4604045	1735	return __svcMessageGet(queue_id, millisec);
mbed_official	49:77c8e4604045	1736	}
mbed_official	49:77c8e4604045	1737	}
mbed_official	49:77c8e4604045	1738
mbed_official	49:77c8e4604045	1739
mbed_official	49:77c8e4604045	1740	// ==== Mail Queue Management Functions ====
mbed_official	49:77c8e4604045	1741
mbed_official	49:77c8e4604045	1742	// Mail Queue Management Service Calls declarations
mbed_official	49:77c8e4604045	1743	SVC_2_1(svcMailCreate, osMailQId, osMailQDef_t *, osThreadId, RET_pointer)
mbed_official	49:77c8e4604045	1744	SVC_4_1(sysMailAlloc, void *, osMailQId, uint32_t, uint32_t, uint32_t, RET_pointer)
mbed_official	49:77c8e4604045	1745	SVC_3_1(sysMailFree, osStatus, osMailQId, void *, uint32_t, RET_osStatus)
mbed_official	49:77c8e4604045	1746
mbed_official	49:77c8e4604045	1747	// Mail Queue Management Service & ISR Calls
mbed_official	49:77c8e4604045	1748
mbed_official	49:77c8e4604045	1749	/// Create and Initialize mail queue
mbed_official	49:77c8e4604045	1750	osMailQId svcMailCreate (osMailQDef_t *queue_def, osThreadId thread_id) {
mbed_official	49:77c8e4604045	1751	uint32_t blk_sz;
mbed_official	49:77c8e4604045	1752	P_MCB pmcb;
mbed_official	49:77c8e4604045	1753	void *pool;
mbed_official	49:77c8e4604045	1754
mbed_official	49:77c8e4604045	1755	if ((queue_def == NULL) \|\|
mbed_official	49:77c8e4604045	1756	(queue_def->queue_sz == 0) \|\|
mbed_official	49:77c8e4604045	1757	(queue_def->item_sz == 0) \|\|
mbed_official	49:77c8e4604045	1758	(queue_def->pool == NULL)) {
mbed_official	49:77c8e4604045	1759	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	1760	return NULL;
mbed_official	49:77c8e4604045	1761	}
mbed_official	49:77c8e4604045	1762
mbed_official	49:77c8e4604045	1763	pmcb = (((void *)queue_def->pool) + 0);
mbed_official	49:77c8e4604045	1764	pool = (((void *)queue_def->pool) + 1);
mbed_official	49:77c8e4604045	1765
mbed_official	49:77c8e4604045	1766	if ((pool == NULL) \|\| (pmcb == NULL) \|\| (pmcb->cb_type != 0)) {
mbed_official	49:77c8e4604045	1767	sysThreadError(osErrorParameter);
mbed_official	49:77c8e4604045	1768	return NULL;
mbed_official	49:77c8e4604045	1769	}
mbed_official	49:77c8e4604045	1770
mbed_official	49:77c8e4604045	1771	blk_sz = (queue_def->item_sz + 3) & ~3;
mbed_official	49:77c8e4604045	1772
mbed_official	49:77c8e4604045	1773	_init_box(pool, sizeof(struct OS_BM) + queue_def->queue_sz * blk_sz, blk_sz);
mbed_official	49:77c8e4604045	1774
mbed_official	49:77c8e4604045	1775	rt_mbx_init(pmcb, 4*(queue_def->queue_sz + 4));
mbed_official	49:77c8e4604045	1776
mbed_official	49:77c8e4604045	1777
mbed_official	49:77c8e4604045	1778	return queue_def->pool;
mbed_official	49:77c8e4604045	1779	}
mbed_official	49:77c8e4604045	1780
mbed_official	49:77c8e4604045	1781	/// Allocate a memory block from a mail
mbed_official	49:77c8e4604045	1782	void *sysMailAlloc (osMailQId queue_id, uint32_t millisec, uint32_t isr, uint32_t clr) {
mbed_official	49:77c8e4604045	1783	P_MCB pmcb;
mbed_official	49:77c8e4604045	1784	void *pool;
mbed_official	49:77c8e4604045	1785	void *mem;
mbed_official	49:77c8e4604045	1786
mbed_official	49:77c8e4604045	1787	if (queue_id == NULL) return NULL;
mbed_official	49:77c8e4604045	1788
mbed_official	49:77c8e4604045	1789	pmcb = (((void *)queue_id) + 0);
mbed_official	49:77c8e4604045	1790	pool = (((void *)queue_id) + 1);
mbed_official	49:77c8e4604045	1791
mbed_official	49:77c8e4604045	1792	if ((pool == NULL) \|\| (pmcb == NULL)) return NULL;
mbed_official	49:77c8e4604045	1793
mbed_official	49:77c8e4604045	1794	if (isr && (millisec != 0)) return NULL;
mbed_official	49:77c8e4604045	1795
mbed_official	49:77c8e4604045	1796	mem = rt_alloc_box(pool);
mbed_official	49:77c8e4604045	1797	if (clr) {
mbed_official	49:77c8e4604045	1798	rt_clr_box(pool, mem);
mbed_official	49:77c8e4604045	1799	}
mbed_official	49:77c8e4604045	1800
mbed_official	49:77c8e4604045	1801	if ((mem == NULL) && (millisec != 0)) {
mbed_official	49:77c8e4604045	1802	// Put Task to sleep when Memory not available
mbed_official	49:77c8e4604045	1803	if (pmcb->p_lnk != NULL) {
mbed_official	49:77c8e4604045	1804	rt_put_prio((P_XCB)pmcb, os_tsk.run);
mbed_official	49:77c8e4604045	1805	} else {
mbed_official	49:77c8e4604045	1806	pmcb->p_lnk = os_tsk.run;
mbed_official	49:77c8e4604045	1807	os_tsk.run->p_lnk = NULL;
mbed_official	49:77c8e4604045	1808	os_tsk.run->p_rlnk = (P_TCB)pmcb;
mbed_official	49:77c8e4604045	1809	// Task is waiting to allocate a message
mbed_official	49:77c8e4604045	1810	pmcb->state = 3;
mbed_official	49:77c8e4604045	1811	}
mbed_official	49:77c8e4604045	1812	rt_block(rt_ms2tick(millisec), WAIT_MBX);
mbed_official	49:77c8e4604045	1813	}
mbed_official	49:77c8e4604045	1814
mbed_official	49:77c8e4604045	1815	return mem;
mbed_official	49:77c8e4604045	1816	}
mbed_official	49:77c8e4604045	1817
mbed_official	49:77c8e4604045	1818	/// Free a memory block from a mail
mbed_official	49:77c8e4604045	1819	osStatus sysMailFree (osMailQId queue_id, void *mail, uint32_t isr) {
mbed_official	49:77c8e4604045	1820	P_MCB pmcb;
mbed_official	49:77c8e4604045	1821	P_TCB ptcb;
mbed_official	49:77c8e4604045	1822	void *pool;
mbed_official	49:77c8e4604045	1823	void *mem;
mbed_official	49:77c8e4604045	1824	int32_t res;
mbed_official	49:77c8e4604045	1825
mbed_official	49:77c8e4604045	1826	if (queue_id == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	1827
mbed_official	49:77c8e4604045	1828	pmcb = (((void *)queue_id) + 0);
mbed_official	49:77c8e4604045	1829	pool = (((void *)queue_id) + 1);
mbed_official	49:77c8e4604045	1830
mbed_official	49:77c8e4604045	1831	if ((pmcb == NULL) \|\| (pool == NULL)) return osErrorParameter;
mbed_official	49:77c8e4604045	1832
mbed_official	49:77c8e4604045	1833	res = rt_free_box(pool, mail);
mbed_official	49:77c8e4604045	1834
mbed_official	49:77c8e4604045	1835	if (res != 0) return osErrorValue;
mbed_official	49:77c8e4604045	1836
mbed_official	49:77c8e4604045	1837	if (pmcb->state == 3) {
mbed_official	49:77c8e4604045	1838	// Task is waiting to allocate a message
mbed_official	49:77c8e4604045	1839	if (isr) {
mbed_official	49:77c8e4604045	1840	rt_psq_enq (pmcb, (U32)pool);
mbed_official	49:77c8e4604045	1841	rt_psh_req ();
mbed_official	49:77c8e4604045	1842	} else {
mbed_official	49:77c8e4604045	1843	mem = rt_alloc_box(pool);
mbed_official	49:77c8e4604045	1844	if (mem != NULL) {
mbed_official	49:77c8e4604045	1845	ptcb = rt_get_first((P_XCB)pmcb);
mbed_official	49:77c8e4604045	1846	if (pmcb->p_lnk == NULL) {
mbed_official	49:77c8e4604045	1847	pmcb->state = 0;
mbed_official	49:77c8e4604045	1848	}
mbed_official	49:77c8e4604045	1849	rt_ret_val(ptcb, (U32)mem);
mbed_official	49:77c8e4604045	1850	rt_rmv_dly(ptcb);
mbed_official	49:77c8e4604045	1851	rt_dispatch(ptcb);
mbed_official	49:77c8e4604045	1852	}
mbed_official	49:77c8e4604045	1853	}
mbed_official	49:77c8e4604045	1854	}
mbed_official	49:77c8e4604045	1855
mbed_official	49:77c8e4604045	1856	return osOK;
mbed_official	49:77c8e4604045	1857	}
mbed_official	49:77c8e4604045	1858
mbed_official	49:77c8e4604045	1859
mbed_official	49:77c8e4604045	1860	// Mail Queue Management Public API
mbed_official	49:77c8e4604045	1861
mbed_official	49:77c8e4604045	1862	/// Create and Initialize mail queue
mbed_official	49:77c8e4604045	1863	osMailQId osMailCreate (osMailQDef_t *queue_def, osThreadId thread_id) {
mbed_official	49:77c8e4604045	1864	if (__get_IPSR() != 0) return NULL; // Not allowed in ISR
mbed_official	49:77c8e4604045	1865	if (((__get_CONTROL() & 1) == 0) && (os_running == 0)) {
mbed_official	49:77c8e4604045	1866	// Privileged and not running
mbed_official	49:77c8e4604045	1867	return svcMailCreate(queue_def, thread_id);
mbed_official	49:77c8e4604045	1868	} else {
mbed_official	49:77c8e4604045	1869	return __svcMailCreate(queue_def, thread_id);
mbed_official	49:77c8e4604045	1870	}
mbed_official	49:77c8e4604045	1871	}
mbed_official	49:77c8e4604045	1872
mbed_official	49:77c8e4604045	1873	/// Allocate a memory block from a mail
mbed_official	49:77c8e4604045	1874	void *osMailAlloc (osMailQId queue_id, uint32_t millisec) {
mbed_official	49:77c8e4604045	1875	if (__get_IPSR() != 0) { // in ISR
mbed_official	49:77c8e4604045	1876	return sysMailAlloc(queue_id, millisec, 1, 0);
mbed_official	49:77c8e4604045	1877	} else { // in Thread
mbed_official	49:77c8e4604045	1878	return __sysMailAlloc(queue_id, millisec, 0, 0);
mbed_official	49:77c8e4604045	1879	}
mbed_official	49:77c8e4604045	1880	}
mbed_official	49:77c8e4604045	1881
mbed_official	49:77c8e4604045	1882	/// Allocate a memory block from a mail and set memory block to zero
mbed_official	49:77c8e4604045	1883	void *osMailCAlloc (osMailQId queue_id, uint32_t millisec) {
mbed_official	49:77c8e4604045	1884	if (__get_IPSR() != 0) { // in ISR
mbed_official	49:77c8e4604045	1885	return sysMailAlloc(queue_id, millisec, 1, 1);
mbed_official	49:77c8e4604045	1886	} else { // in Thread
mbed_official	49:77c8e4604045	1887	return __sysMailAlloc(queue_id, millisec, 0, 1);
mbed_official	49:77c8e4604045	1888	}
mbed_official	49:77c8e4604045	1889	}
mbed_official	49:77c8e4604045	1890
mbed_official	49:77c8e4604045	1891	/// Free a memory block from a mail
mbed_official	49:77c8e4604045	1892	osStatus osMailFree (osMailQId queue_id, void *mail) {
mbed_official	49:77c8e4604045	1893	if (__get_IPSR() != 0) { // in ISR
mbed_official	49:77c8e4604045	1894	return sysMailFree(queue_id, mail, 1);
mbed_official	49:77c8e4604045	1895	} else { // in Thread
mbed_official	49:77c8e4604045	1896	return __sysMailFree(queue_id, mail, 0);
mbed_official	49:77c8e4604045	1897	}
mbed_official	49:77c8e4604045	1898	}
mbed_official	49:77c8e4604045	1899
mbed_official	49:77c8e4604045	1900	/// Put a mail to a queue
mbed_official	49:77c8e4604045	1901	osStatus osMailPut (osMailQId queue_id, void *mail) {
mbed_official	49:77c8e4604045	1902	if (queue_id == NULL) return osErrorParameter;
mbed_official	49:77c8e4604045	1903	if (mail == NULL) return osErrorValue;
mbed_official	49:77c8e4604045	1904	return osMessagePut(((void *)queue_id), (uint32_t)mail, 0);
mbed_official	49:77c8e4604045	1905	}
mbed_official	49:77c8e4604045	1906
mbed_official	49:77c8e4604045	1907	/// Get a mail from a queue
mbed_official	49:77c8e4604045	1908	os_InRegs osEvent osMailGet (osMailQId queue_id, uint32_t millisec) {
mbed_official	49:77c8e4604045	1909	osEvent ret;
mbed_official	49:77c8e4604045	1910
mbed_official	49:77c8e4604045	1911	if (queue_id == NULL) {
mbed_official	49:77c8e4604045	1912	ret.status = osErrorParameter;
mbed_official	49:77c8e4604045	1913	return ret;
mbed_official	49:77c8e4604045	1914	}
mbed_official	49:77c8e4604045	1915
mbed_official	49:77c8e4604045	1916	ret = osMessageGet(((void *)queue_id), millisec);
mbed_official	49:77c8e4604045	1917	if (ret.status == osEventMessage) ret.status = osEventMail;
mbed_official	49:77c8e4604045	1918
mbed_official	49:77c8e4604045	1919	return ret;
mbed_official	49:77c8e4604045	1920	}

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	16 Jan 2015
Imports:	1
Forks:	0
Commits:	57
Dependents:	1
Dependencies:	0
Followers:	1

rtx/TARGET_CORTEX_M/rt_CMSIS.c@56:703a0ea0ca4d, 2015-01-16 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning