Mark Vandermeulen / FYDP_Final

Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

mbed/mbed-rtos/rtx/rt_CMSIS.c@0:964eb6a2ef00, 2015-03-18 (annotated)

Committer:
majik
Date:
Wed Mar 18 22:23:48 2015 +0000
Revision:
0:964eb6a2ef00
This is our FYDP code, but only one IMU works with the RTOS.

Who changed what in which revision?

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