Lee Kai Xuan / mbed-os

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of mbed-os by erkin yucel

rtos/rtx/TARGET_CORTEX_M/rt_CMSIS.c@1:3deb71413561, 2017-07-20 (annotated)

Committer:
xuaner
Date:
Thu Jul 20 14:26:57 2017 +0000
Revision:
1:3deb71413561
Parent:
0:f269e3021894 
mbed_os

Who changed what in which revision?

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