Anasse Abdoul / Mbed 2 deprecated Test_MPU6050

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.

Dependencies:   mbed

mbed-rtos/rtx/TARGET_ARM7/rt_CMSIS.c@0:a59a3d743804, 2022-03-31 (annotated)

Committer:
anasse
Date:
Thu Mar 31 07:43:50 2022 +0000
Revision:
0:a59a3d743804
vers0

Who changed what in which revision?

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