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