Alan Ffrench / Mbed 2 deprecated Experiment_2_5

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/rt_CMSIS.c@0:52468b19aa21, 2020-07-29 (annotated)

Committer:
alanffrench
Date:
Wed Jul 29 18:51:38 2020 +0000
Revision:
0:52468b19aa21
Threads

Who changed what in which revision?

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