36372 mbed / Mbed 2 deprecated SP14P1_skeleton

skeleton for lab1

Dependencies:   AvailableMemory mbed-rtos mbed

Fork of helloaabbc by 32314 mbed

mbed-rtos/rtx/rt_CMSIS.c@0:1c8f2727e9f5, 2014-04-03 (annotated)

Committer:
y7jin
Date:
Thu Apr 03 22:56:32 2014 +0000
Revision:
0:1c8f2727e9f5
hello

Who changed what in which revision?

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