Eric Patterson / mbed-rtos

Fork of official mbed Real Time Operating System based on the RTX implementation of the CMSIS-RTOS API open standard.

Dependents:   Final_Coffee_Roaster

Fork of mbed-rtos by mbed official

rtx/rt_CMSIS.c@11:db1fc233faa9, 2013-04-30 (annotated)

Committer:
emilmont
Date:
Tue Apr 30 10:51:19 2013 +0000
Revision:
11:db1fc233faa9
Parent:
10:fcb1f103f7a1
Child:
31:015df9e602b6
In the Keil MDK toolchain "RTX_Config.h" is a special system library header file

Who changed what in which revision?

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