ublox_C200Test - Integrating the ublox LISA C200 modem

Users » mpulice » Code » ublox_C200Test

Integrating the ublox LISA C200 modem

Fork of SprintUSBModemHTTPClientTest by Donatien Garnier

mbed-rtos/rtx/rt_CMSIS.c@5:3f93dd1d4cb3, 2013-09-26 (annotated)

Committer:: sam_grove
Date:: Thu Sep 26 00:44:20 2013 -0500
Revision:: 5:3f93dd1d4cb3

Exported program and replaced contents of the repo with the source

to build and debug using keil mdk. Libs NOT upto date are lwip, lwip-sys

and socket. these have newer versions under mbed_official but were starting

from a know working point

Who changed what in which revision?

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