V M / Mbed 2 deprecated NRF24L01_receiver

NRF com

Dependencies:   mbed nRF24L01P

mbed-rtos/rtx/rt_CMSIS.c@0:fdfe93cb9255, 2013-05-27 (annotated)

Committer:
vmihalcut
Date:
Mon May 27 06:06:31 2013 +0000
Revision:
0:fdfe93cb9255
NRF24L01 receiver

Who changed what in which revision?

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