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