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