HT-TEAM / Mbed 2 deprecated Test_Mainboard

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:   F7_Ethernet mbed BSP_DISCO_F746NG SDFileSystem RF24

TARGET_CORTEX_M/rt_CMSIS.c@3:1c6da30ca347, 2021-06-08 (annotated)

Committer:
lowlowry
Date:
Tue Jun 08 16:21:16 2021 +0000
Revision:
3:1c6da30ca347
Parent:
0:d984976f1f1c 
test final

Who changed what in which revision?

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