IBMIoTClientEthernetExample - Contains example code to connect the mbed LPC1768…

IBM Watson IoT » Code » IBMIoTClientEthernetExample

IBM Watson IoT / Mbed OS IBMIoTClientEthernetExample

Contains example code to connect the mbed LPC1768 or FRDM-K64F devices to the IBM Internet of Things Cloud service via ethernet.

Dependencies: C12832 MQTT LM75B MMA7660

Dependents: MFT_IoT_demo_USB400 IBM_RFID

mbed-rtos/rtx/rt_CMSIS.c@6:37b6d0d56190, 2014-08-20 (annotated)

Committer:: samdanbury
Date:: Wed Aug 20 12:45:14 2014 +0000
Revision:: 6:37b6d0d56190

Code completely changed to improve the structure, flow and memory usage of the application

Who changed what in which revision?

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