Freeman Oldman / NUCLEO_STM32F401RE_CC3000_ILI9341

The code from https://github.com/vpcola/Nucleo

mbed-rtos/rtx/rt_CMSIS.c@0:5464d5e415e5, 2014-10-08 (annotated)

Committer:
sinrab
Date:
Wed Oct 08 11:00:24 2014 +0000
Revision:
0:5464d5e415e5
The code from https://github.com/vpcola/Nucleo

Who changed what in which revision?

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