Doubea Pierre / Mbed 2 deprecated HW3

Lab Checkoff

Dependencies:   SDFileSystem TextLCD mbed-rtos mbed wave_player FATFileSystem

mbed-rtos/rtx/rt_CMSIS.c@0:67dbd54e60d4, 2013-11-13 (annotated)

Committer:
doubster
Date:
Wed Nov 13 20:00:28 2013 +0000
Revision:
0:67dbd54e60d4
Lab Checkoff

Who changed what in which revision?

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