Revision 119:6635230e06ba, committed 2016-07-25

Comitter:

Kojto

Date:

Mon Jul 25 14:12:24 2016 +0100

Parent:

117:4c105b8d7cae

Child:

120:19af2d39a542

Commit message:

RTOS rev118

Compatible with the mbed library v122

Changes:
- warnings about duplicated CM symbols fix
- init sequence update - allows init array to be run prior kernel start
- RTOS with OS_TIMERS=0 fix for thread id
- Thread ctor is deprecated, use start() method
- main stack fix for IAR (set via linker script)
- add TCB context pointer
- provide thread safety for toolchains (std lib locks)
- add MBED_RTOS_SINGLE_THREAD macro (sets TSKCNT to 1 and TIMERS to 0)
- nrf51, nucleo l423kc, nucleo f767zi, nucleo f446ze, efm32 support addition
- add OSObserver function

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed_lib.json	Mon Jul 25 14:12:24 2016 +0100
@@ -0,0 +1,6 @@
+{
+    "name": "rtos",
+    "config": {
+        "present": 1
+    }
+}

--- a/rtos/Thread.cpp	Mon May 23 11:00:15 2016 +0100
+++ b/rtos/Thread.cpp	Mon Jul 25 14:12:24 2016 +0100
@@ -21,7 +21,7 @@
  */
 #include "Thread.h"
 
-#include "mbed_error.h"
+#include "mbed.h"
 #include "rtos_idle.h"
 
 // rt_tid2ptcb is an internal function which we exposed to get TCB for thread id
@@ -32,34 +32,81 @@
 
 namespace rtos {
 
-Thread::Thread(void (*task)(void const *argument), void *argument,
-        osPriority priority, uint32_t stack_size, unsigned char *stack_pointer) {
+void Thread::constructor(osPriority priority,
+        uint32_t stack_size, unsigned char *stack_pointer) {
+    _tid = 0;
+    _dynamic_stack = (stack_pointer == NULL);
+
 #if defined(__MBED_CMSIS_RTOS_CA9) || defined(__MBED_CMSIS_RTOS_CM)
-    _thread_def.pthread = task;
     _thread_def.tpriority = priority;
     _thread_def.stacksize = stack_size;
-    if (stack_pointer != NULL) {
-        _thread_def.stack_pointer = (uint32_t*)stack_pointer;
-        _dynamic_stack = false;
-    } else {
-        _thread_def.stack_pointer = new uint32_t[stack_size/sizeof(uint32_t)];
+    _thread_def.stack_pointer = (uint32_t*)stack_pointer;
+#endif
+}
+
+void Thread::constructor(Callback<void()> task,
+        osPriority priority, uint32_t stack_size, unsigned char *stack_pointer) {
+    constructor(priority, stack_size, stack_pointer);
+
+    switch (start(task)) {
+        case osErrorResource:
+            error("OS ran out of threads!\n");
+            break;
+        case osErrorParameter:
+            error("Thread already running!\n");
+            break;
+        case osErrorNoMemory:
+            error("Error allocating the stack memory\n");
+        default:
+            break;
+    }
+}
+
+osStatus Thread::start(Callback<void()> task) {
+    if (_tid != 0) {
+        return osErrorParameter;
+    }
+
+#if defined(__MBED_CMSIS_RTOS_CA9) || defined(__MBED_CMSIS_RTOS_CM)
+    _thread_def.pthread = (void (*)(const void *))Callback<void()>::thunk;
+    if (_thread_def.stack_pointer == NULL) {
+        _thread_def.stack_pointer = new uint32_t[_thread_def.stacksize/sizeof(uint32_t)];
         if (_thread_def.stack_pointer == NULL)
-            error("Error allocating the stack memory\n");
-        _dynamic_stack = true;
+            return osErrorNoMemory;
     }
-    
+
     //Fill the stack with a magic word for maximum usage checking
-    for (uint32_t i = 0; i < (stack_size / sizeof(uint32_t)); i++) {
+    for (uint32_t i = 0; i < (_thread_def.stacksize / sizeof(uint32_t)); i++) {
         _thread_def.stack_pointer[i] = 0xE25A2EA5;
     }
 #endif
-    _tid = osThreadCreate(&_thread_def, argument);
+    _task = task;
+    _tid = osThreadCreate(&_thread_def, &_task);
+    if (_tid == NULL) {
+        if (_dynamic_stack) delete[] (_thread_def.stack_pointer);
+        return osErrorResource;
+    }
+    return osOK;
 }
 
 osStatus Thread::terminate() {
     return osThreadTerminate(_tid);
 }
 
+osStatus Thread::join() {
+    while (true) {
+        uint8_t state = get_state();
+        if (state == Thread::Inactive || state == osErrorParameter) {
+            return osOK;
+        }
+
+        osStatus status = yield();
+        if (status != osOK) {
+            return status;
+        }
+    }
+}
+
 osStatus Thread::set_priority(osPriority priority) {
     return osThreadSetPriority(_tid, priority);
 }

--- a/rtos/Thread.h	Mon May 23 11:00:15 2016 +0100
+++ b/rtos/Thread.h	Mon Jul 25 14:12:24 2016 +0100
@@ -24,23 +24,146 @@
 
 #include <stdint.h>
 #include "cmsis_os.h"
+#include "Callback.h"
+#include "toolchain.h"
 
 namespace rtos {
 
 /** The Thread class allow defining, creating, and controlling thread functions in the system. */
 class Thread {
 public:
+    /** Allocate a new thread without starting execution
+      @param   priority       initial priority of the thread function. (default: osPriorityNormal).
+      @param   stack_size      stack size (in bytes) requirements for the thread function. (default: DEFAULT_STACK_SIZE).
+      @param   stack_pointer  pointer to the stack area to be used by this thread (default: NULL).
+    */
+    Thread(osPriority priority=osPriorityNormal,
+           uint32_t stack_size=DEFAULT_STACK_SIZE,
+           unsigned char *stack_pointer=NULL) {
+        constructor(priority, stack_size, stack_pointer);
+    }
+
     /** Create a new thread, and start it executing the specified function.
       @param   task           function to be executed by this thread.
       @param   argument       pointer that is passed to the thread function as start argument. (default: NULL).
       @param   priority       initial priority of the thread function. (default: osPriorityNormal).
       @param   stack_size      stack size (in bytes) requirements for the thread function. (default: DEFAULT_STACK_SIZE).
       @param   stack_pointer  pointer to the stack area to be used by this thread (default: NULL).
+      @deprecated
+        Thread-spawning constructors hide errors and may lead to complex
+        program state when a thread is declared.
+
+        The explicit Thread::start member function should be used to spawn
+        a thread.
     */
+    MBED_DEPRECATED(
+        "Thread-spawning constructors hide errors and may lead to complex "
+        "program state when a thread is declared")
+    Thread(mbed::Callback<void()> task,
+           osPriority priority=osPriorityNormal,
+           uint32_t stack_size=DEFAULT_STACK_SIZE,
+           unsigned char *stack_pointer=NULL) {
+        constructor(task, priority, stack_size, stack_pointer);
+    }
+
+    /** Create a new thread, and start it executing the specified function.
+      @param   obj            argument to task.
+      @param   method         function to be executed by this thread.
+      @param   argument       pointer that is passed to the thread function as start argument. (default: NULL).
+      @param   priority       initial priority of the thread function. (default: osPriorityNormal).
+      @param   stack_size      stack size (in bytes) requirements for the thread function. (default: DEFAULT_STACK_SIZE).
+      @param   stack_pointer  pointer to the stack area to be used by this thread (default: NULL).
+      @deprecated
+        Thread-spawning constructors hide errors and may lead to complex
+        program state when a thread is declared.
+
+        The explicit Thread::start member function should be used to spawn
+        a thread.
+    */
+    template <typename T>
+    MBED_DEPRECATED(
+        "Thread-spawning constructors hide errors and may lead to complex "
+        "program state when a thread is declared")
+    Thread(T *obj, void (T::*method)(),
+           osPriority priority=osPriorityNormal,
+           uint32_t stack_size=DEFAULT_STACK_SIZE,
+           unsigned char *stack_pointer=NULL) {
+        constructor(mbed::Callback<void()>(obj, method),
+                    priority, stack_size, stack_pointer);
+    }
+
+    /** Create a new thread, and start it executing the specified function.
+      @param   obj            argument to task.
+      @param   method         function to be executed by this thread.
+      @param   argument       pointer that is passed to the thread function as start argument. (default: NULL).
+      @param   priority       initial priority of the thread function. (default: osPriorityNormal).
+      @param   stack_size      stack size (in bytes) requirements for the thread function. (default: DEFAULT_STACK_SIZE).
+      @param   stack_pointer  pointer to the stack area to be used by this thread (default: NULL).
+      @deprecated
+        Thread-spawning constructors hide errors and may lead to complex
+        program state when a thread is declared.
+
+        The explicit Thread::start member function should be used to spawn
+        a thread.
+    */
+    template <typename T>
+    MBED_DEPRECATED(
+        "Thread-spawning constructors hide errors and may lead to complex "
+        "program state when a thread is declared")
+    Thread(T *obj, void (*method)(T *),
+           osPriority priority=osPriorityNormal,
+           uint32_t stack_size=DEFAULT_STACK_SIZE,
+           unsigned char *stack_pointer=NULL) {
+        constructor(mbed::Callback<void()>(obj, method),
+                    priority, stack_size, stack_pointer);
+    }
+
+    /** Create a new thread, and start it executing the specified function.
+        Provided for backwards compatibility
+      @param   task           function to be executed by this thread.
+      @param   argument       pointer that is passed to the thread function as start argument. (default: NULL).
+      @param   priority       initial priority of the thread function. (default: osPriorityNormal).
+      @param   stack_size      stack size (in bytes) requirements for the thread function. (default: DEFAULT_STACK_SIZE).
+      @param   stack_pointer  pointer to the stack area to be used by this thread (default: NULL).
+      @deprecated
+        Thread-spawning constructors hide errors and may lead to complex
+        program state when a thread is declared.
+
+        The explicit Thread::start member function should be used to spawn
+        a thread.
+    */
+    MBED_DEPRECATED(
+        "Thread-spawning constructors hide errors and may lead to complex "
+        "program state when a thread is declared")
     Thread(void (*task)(void const *argument), void *argument=NULL,
            osPriority priority=osPriorityNormal,
            uint32_t stack_size=DEFAULT_STACK_SIZE,
-           unsigned char *stack_pointer=NULL);
+           unsigned char *stack_pointer=NULL) {
+        constructor(mbed::Callback<void()>(argument, (void (*)(void *))task),
+                    priority, stack_size, stack_pointer);
+    }
+
+    /** Starts a thread executing the specified function.
+      @param   task           function to be executed by this thread.
+      @return  status code that indicates the execution status of the function.
+    */
+    osStatus start(mbed::Callback<void()> task);
+
+    /** Starts a thread executing the specified function.
+      @param   obj            argument to task
+      @param   method         function to be executed by this thread.
+      @return  status code that indicates the execution status of the function.
+    */
+    template <typename T, typename M>
+    osStatus start(T *obj, M method) {
+        return start(mbed::Callback<void()>(obj, method));
+    }
+
+    /** Wait for thread to terminate
+      @return  status code that indicates the execution status of the function.
+      @note not callable from interrupt
+    */
+    osStatus join();
 
     /** Terminate execution of a thread and remove it from Active Threads
       @return  status code that indicates the execution status of the function.
@@ -113,17 +236,20 @@
       @param   signals   wait until all specified signal flags set or 0 for any single signal flag.
       @param   millisec  timeout value or 0 in case of no time-out. (default: osWaitForever).
       @return  event flag information or error code.
+      @note not callable from interrupt
     */
     static osEvent signal_wait(int32_t signals, uint32_t millisec=osWaitForever);
 
     /** Wait for a specified time period in millisec:
       @param   millisec  time delay value
       @return  status code that indicates the execution status of the function.
+      @note not callable from interrupt
     */
     static osStatus wait(uint32_t millisec);
 
     /** Pass control to next thread that is in state READY.
       @return  status code that indicates the execution status of the function.
+      @note not callable from interrupt
     */
     static osStatus yield();
 
@@ -140,6 +266,17 @@
     virtual ~Thread();
 
 private:
+    // Required to share definitions without
+    // delegated constructors
+    void constructor(osPriority priority=osPriorityNormal,
+                     uint32_t stack_size=DEFAULT_STACK_SIZE,
+                     unsigned char *stack_pointer=NULL);
+    void constructor(mbed::Callback<void()> task,
+                     osPriority priority=osPriorityNormal,
+                     uint32_t stack_size=DEFAULT_STACK_SIZE,
+                     unsigned char *stack_pointer=NULL);
+
+    mbed::Callback<void()> _task;
     osThreadId _tid;
     osThreadDef_t _thread_def;
     bool _dynamic_stack;

--- a/rtos/rtos.h	Mon May 23 11:00:15 2016 +0100
+++ b/rtos/rtos.h	Mon Jul 25 14:12:24 2016 +0100
@@ -32,4 +32,13 @@
 
 using namespace rtos;
 
+/* Get mbed lib version number, as RTOS depends on mbed lib features
+   like mbed_error, Callback and others.
+*/
+#include "mbed.h"
+
+#if (MBED_LIBRARY_VERSION < 122)
+#error "This version of RTOS requires mbed library version > 121"
 #endif
+
+#endif

--- a/rtx/TARGET_ARM7/RTX_CM_lib.h	Mon May 23 11:00:15 2016 +0100
+++ b/rtx/TARGET_ARM7/RTX_CM_lib.h	Mon Jul 25 14:12:24 2016 +0100
@@ -339,7 +339,7 @@
     main(0, NULL);
 }
 
-__attribute__((naked)) void software_init_hook (void) {
+__attribute__((naked)) void software_init_hook_rtos (void) {
   __asm (
     ".syntax unified\n"
     ".thumb\n"

--- a/rtx/TARGET_CORTEX_A/RTX_CM_lib.h	Mon May 23 11:00:15 2016 +0100
+++ b/rtx/TARGET_CORTEX_A/RTX_CM_lib.h	Mon Jul 25 14:12:24 2016 +0100
@@ -50,7 +50,7 @@
 #define _declare_box(pool,size,cnt)  uint32_t pool[(((size)+3)/4)*(cnt) + 3]
 #define _declare_box8(pool,size,cnt) uint64_t pool[(((size)+7)/8)*(cnt) + 2]
 
-#define OS_TCB_SIZE     52
+#define OS_TCB_SIZE     60
 #define OS_TMR_SIZE     8
 
 #if defined (__CC_ARM) && !defined (__MICROLIB)
@@ -395,18 +395,22 @@
  *---------------------------------------------------------------------------*/
 
 /* Main Thread definition */
-extern int main (void);
+extern void pre_main (void);
 #ifdef __MBED_CMSIS_RTOS_CA9
 uint32_t os_thread_def_stack_main [(4 * OS_MAINSTKSIZE) / sizeof(uint32_t)];
-osThreadDef_t os_thread_def_main = {(os_pthread)main, osPriorityNormal, 1, 4*OS_MAINSTKSIZE, os_thread_def_stack_main };
+osThreadDef_t os_thread_def_main = {(os_pthread)pre_main, osPriorityNormal, 1, 4*OS_MAINSTKSIZE, os_thread_def_stack_main };
 #else
-osThreadDef_t os_thread_def_main = {(os_pthread)main, osPriorityNormal, 1, 4*OS_MAINSTKSIZE };
+osThreadDef_t os_thread_def_main = {(os_pthread)pre_main, osPriorityNormal, 1, 4*OS_MAINSTKSIZE };
 #endif
 
 #if defined (__CC_ARM)
 
 #ifdef __MICROLIB
+
+int main(void);
 void _main_init (void) __attribute__((section(".ARM.Collect$$$$000000FF")));
+void $Super$$__cpp_initialize__aeabi_(void);
+
 #if __TARGET_ARCH_ARM
 #pragma push
 #pragma arm
@@ -420,66 +424,134 @@
 #if __TARGET_ARCH_ARM
 #pragma pop
 #endif
+
+void $Sub$$__cpp_initialize__aeabi_(void)  
+{  
+  // this should invoke C++ initializers prior _main_init, we keep this empty and  
+  // invoke them after _main_init (=starts RTX kernel)  
+}  
+
+void pre_main()  
+{  
+  $Super$$__cpp_initialize__aeabi_();  
+  main();  
+}
+
 #else
+
+void * armcc_heap_base;
+void * armcc_heap_top;
+
+__asm void pre_main (void)
+{
+  IMPORT  __rt_lib_init
+  IMPORT  main
+  IMPORT  armcc_heap_base
+  IMPORT  armcc_heap_top
+
+  LDR     R0,=armcc_heap_base
+  LDR     R1,=armcc_heap_top
+  LDR     R0,[R0]
+  LDR     R1,[R1]
+  /* Save link register (keep 8 byte alignment with dummy R4) */
+  PUSH    {R4, LR}
+  BL      __rt_lib_init
+  BL       main
+  /* Return to the thread destroy function.
+   */
+  POP     {R4, PC}
+  ALIGN
+}
+
 __asm void __rt_entry (void) {
 
   IMPORT  __user_setup_stackheap
-  IMPORT  __rt_lib_init
   IMPORT  os_thread_def_main
+  IMPORT  armcc_heap_base
+  IMPORT  armcc_heap_top
   IMPORT  osKernelInitialize
   IMPORT  osKernelStart
   IMPORT  osThreadCreate
-  IMPORT  exit
 
   BL      __user_setup_stackheap
-  MOV     R1,R2
-  BL      __rt_lib_init
+  LDR     R3,=armcc_heap_base
+  LDR     R4,=armcc_heap_top
+  STR     R0,[R3]
+  STR     R2,[R4]
   BL      osKernelInitialize
   LDR     R0,=os_thread_def_main
   MOVS    R1,#0
   BL      osThreadCreate
   BL      osKernelStart
-  BL      exit
+  /* osKernelStart should not return */
+  B       .
 
   ALIGN
 }
 #endif
 
 #elif defined (__GNUC__)
+extern void __libc_fini_array(void);
+extern void __libc_init_array (void);
+extern int main(int argc, char **argv);
 
-__attribute__((naked)) void software_init_hook (void) {
+void pre_main(void) {
+    atexit(__libc_fini_array);
+    __libc_init_array();
+    main(0, NULL);
+}
+
+__attribute__((naked)) void software_init_hook_rtos (void) {
   __asm (
     ".syntax unified\n"
     ".arm\n"
-    "movs r0,#0\n"
-    "movs r1,#0\n"
-    "mov  r4,r0\n"
-    "mov  r5,r1\n"
-    "ldr  r0,= __libc_fini_array\n"
-    "bl   atexit\n"
-    "bl   __libc_init_array\n"
-    "mov  r0,r4\n"
-    "mov  r1,r5\n"
     "bl   osKernelInitialize\n"
     "ldr  r0,=os_thread_def_main\n"
     "movs r1,#0\n"
     "bl   osThreadCreate\n"
     "bl   osKernelStart\n"
-    "bl   exit\n"
+    /* osKernelStart should not return */ 
+    "B       .\n"
   );
 }
 
 #elif defined (__ICCARM__)
+extern void* __vector_core_a9;
+extern int  __low_level_init(void);
+extern void __iar_data_init3(void);
+extern __weak void __iar_init_core( void );
+extern __weak void __iar_init_vfp( void );
+extern void __iar_dynamic_initialization(void);
+extern void mbed_sdk_init(void);
+static uint8_t low_level_init_needed;
 
-extern void exit(int arg);
+void pre_main(void) {
+    if (low_level_init_needed) {
+        __iar_dynamic_initialization();
+    }
+    main();
+}
 
-void mbed_main(void) {
-  int a;
-
+#pragma required=__vector_core_a9
+void __iar_program_start( void )
+{
+  __iar_init_core();
+  __iar_init_vfp();
+  
+  uint8_t low_level_init_needed_local;
+  
+  low_level_init_needed_local = __low_level_init();
+  if (low_level_init_needed_local) {
+     __iar_data_init3();
+     mbed_sdk_init();
+   }
+  /* Store in a global variable after RAM has been initialized */
+  low_level_init_needed = low_level_init_needed_local;
   osKernelInitialize();
   osThreadCreate(&os_thread_def_main, NULL);
-  a = osKernelStart();
-  exit(a);
+  osKernelStart();
+  /* osKernelStart should not return */
+  while (1);
 }
 
 #endif

--- a/rtx/TARGET_CORTEX_A/RTX_Conf_CA.c	Mon May 23 11:00:15 2016 +0100
+++ b/rtx/TARGET_CORTEX_A/RTX_Conf_CA.c	Mon Jul 25 14:12:24 2016 +0100
@@ -38,6 +38,11 @@
  *      RTX User configuration part BEGIN
  *---------------------------------------------------------------------------*/
 
+#if defined(MBED_RTOS_SINGLE_THREAD)
+#define OS_TASKCNT  1
+#define OS_TIMERS   0
+#endif
+
 //-------- <<< Use Configuration Wizard in Context Menu >>> -----------------
 //
 // <h>Thread Configuration
@@ -67,9 +72,9 @@
 
 //   <o>Main Thread stack size [bytes] <64-4096:8><#/4>
 //   <i> Defines stack size for main thread.
-//   <i> Default: 200
+//   <i> Default: 4096
 #ifndef OS_MAINSTKSIZE
- #define OS_MAINSTKSIZE 2048
+ #define OS_MAINSTKSIZE 4096
 #endif
 
 #ifndef __MBED_CMSIS_RTOS_CA9

--- a/rtx/TARGET_CORTEX_M/HAL_CM.c	Mon May 23 11:00:15 2016 +0100
+++ b/rtx/TARGET_CORTEX_M/HAL_CM.c	Mon Jul 25 14:12:24 2016 +0100
@@ -35,7 +35,7 @@
 #include "rt_TypeDef.h"
 #include "RTX_Config.h"
 #include "rt_HAL_CM.h"
-
+#include "cmsis_os.h"
 
 /*----------------------------------------------------------------------------
  *      Global Variables
@@ -93,12 +93,12 @@
 
 #ifdef __MBED_CMSIS_RTOS_CM
   /* Set a magic word for checking of stack overflow.
-   For the main thread (ID: 0x02) the stack is in a memory area shared with the
+   For the main thread (ID: MAIN_THREAD_ID) the stack is in a memory area shared with the
    heap, therefore the last word of the stack is a moving target.
    We want to do stack/heap collision detection instead.
    Similar applies to stack filling for the magic pattern.
   */
-  if (p_TCB->task_id != 0x02) {
+  if (p_TCB->task_id != MAIN_THREAD_ID) {
     p_TCB->stack[0] = MAGIC_WORD;
 
     /* Initialize stack with magic pattern. */

--- a/rtx/TARGET_CORTEX_M/RTX_CM_lib.h	Mon May 23 11:00:15 2016 +0100
+++ b/rtx/TARGET_CORTEX_M/RTX_CM_lib.h	Mon Jul 25 14:12:24 2016 +0100
@@ -51,16 +51,16 @@
 #define _declare_box(pool,size,cnt)  uint32_t pool[(((size)+3)/4)*(cnt) + 3]
 #define _declare_box8(pool,size,cnt) uint64_t pool[(((size)+7)/8)*(cnt) + 2]
 
-#define OS_TCB_SIZE     52
+#define OS_TCB_SIZE     60
 #define OS_TMR_SIZE     8
 
-#if defined (__CC_ARM) && !defined (__MICROLIB)
-
 typedef void    *OS_ID;
 typedef uint32_t OS_TID;
 typedef uint32_t OS_MUT[4];
 typedef uint32_t OS_RESULT;
 
+#if defined (__CC_ARM) && !defined (__MICROLIB)
+
 #define runtask_id()    rt_tsk_self()
 #define mutex_init(m)   rt_mut_init(m)
 #define mutex_wait(m)   os_mut_wait(m,0xFFFFU)
@@ -122,7 +122,11 @@
 uint16_t const os_tickus_i   = OS_CLOCK/1000000;
 uint16_t const os_tickus_f   = (((uint64_t)(OS_CLOCK-1000000*(OS_CLOCK/1000000)))<<16)/1000000;
 uint32_t const os_trv        = OS_TRV;
+#if       defined(FEATURE_UVISOR) && defined(TARGET_UVISOR_SUPPORTED)
+uint8_t  const os_flags      = 0;
+#else  /* defined(FEATURE_UVISOR) && defined(TARGET_UVISOR_SUPPORTED) */
 uint8_t  const os_flags      = OS_RUNPRIV;
+#endif /* defined(FEATURE_UVISOR) && defined(TARGET_UVISOR_SUPPORTED) */
 
 /* Export following defines to uVision debugger. */
 __USED uint32_t const CMSIS_RTOS_API_Version = osCMSIS;
@@ -178,7 +182,7 @@
 #endif
 
 /* Legacy RTX User Timers not used */
-uint32_t       os_tmr = 0U; 
+uint32_t       os_tmr = 0U;
 uint32_t const *m_tmr = NULL;
 uint16_t const mp_tmr_size = 0U;
 
@@ -190,6 +194,77 @@
  extern void  *__libspace_start;
 #endif
 
+#if defined (__ICCARM__)
+static osMutexId  std_mutex_id_sys[_MAX_LOCK] = {0};
+static OS_MUT     std_mutex_sys[_MAX_LOCK] = {0};
+#define _FOPEN_MAX 10
+static osMutexId  std_mutex_id_file[_FOPEN_MAX] = {0};
+static OS_MUT     std_mutex_file[_FOPEN_MAX] = {0};
+void __iar_system_Mtxinit(__iar_Rmtx *mutex) /* Initialize a system lock */
+{
+  osMutexDef_t def;
+  uint32_t index;
+  for (index = 0; index < _MAX_LOCK; index++) {
+    if (0 == std_mutex_id_sys[index]) {
+      def.mutex = &std_mutex_sys[index];
+      std_mutex_id_sys[index] = osMutexCreate(&def);
+      *mutex = (__iar_Rmtx*)&std_mutex_id_sys[index];
+      return;
+    }
+  }
+  // This should never happen
+  error("Not enough mutexes\n");
+}
+
+void __iar_system_Mtxdst(__iar_Rmtx *mutex)/*Destroy a system lock */
+{
+  osMutexDelete(*(osMutexId*)*mutex);
+  *mutex = 0;
+}
+
+void __iar_system_Mtxlock(__iar_Rmtx *mutex) /* Lock a system lock */
+{
+  osMutexWait(*(osMutexId*)*mutex, osWaitForever);
+}
+
+void __iar_system_Mtxunlock(__iar_Rmtx *mutex) /* Unlock a system lock */
+{
+  osMutexRelease(*(osMutexId*)*mutex);
+}
+
+void __iar_file_Mtxinit(__iar_Rmtx *mutex)/*Initialize a file lock */
+{
+    osMutexDef_t def;
+    uint32_t index;
+    for (index = 0; index < _FOPEN_MAX; index++) {
+      if (0 == std_mutex_id_file[index]) {
+        def.mutex = &std_mutex_file[index];
+        std_mutex_id_file[index] = osMutexCreate(&def);
+        *mutex = (__iar_Rmtx*)&std_mutex_id_file[index];
+        return;
+      }
+    }
+    // The variable _FOPEN_MAX needs to be increased
+    error("Not enough mutexes\n");
+}
+
+void __iar_file_Mtxdst(__iar_Rmtx *mutex) /* Destroy a file lock */
+{
+  osMutexDelete(*(osMutexId*)*mutex);
+  *mutex = 0;
+}
+
+void __iar_file_Mtxlock(__iar_Rmtx *mutex) /* Lock a file lock */
+{
+  osMutexWait(*(osMutexId*)*mutex, osWaitForever);
+}
+
+void __iar_file_Mtxunlock(__iar_Rmtx *mutex) /* Unlock a file lock */
+{
+  osMutexRelease(*(osMutexId*)*mutex);
+}
+
+#endif
 
 /*----------------------------------------------------------------------------
  *      RTX Optimizations (empty functions)
@@ -301,7 +376,12 @@
 #define INITIAL_SP            (0x20003000UL)
 
 #elif defined(TARGET_K64F)
+#if defined(FEATURE_UVISOR) && defined(TARGET_UVISOR_SUPPORTED)
+extern uint32_t __StackTop[];
+#define INITIAL_SP            (__StackTop)
+#else
 #define INITIAL_SP            (0x20030000UL)
+#endif
 
 #elif defined(TARGET_K22F)
 #define INITIAL_SP            (0x20010000UL)
@@ -402,12 +482,15 @@
 #elif defined(TARGET_NZ32_SC151)
 #define INITIAL_SP            (0x20008000UL)
 
-#elif (defined(TARGET_STM32F446RE) || defined(TARGET_STM32F446VE))
+#elif defined(TARGET_STM32F446RE) || defined(TARGET_STM32F446VE) || defined(TARGET_STM32F446ZE)
 #define INITIAL_SP            (0x20020000UL)
 
 #elif defined(TARGET_STM32F070RB) || defined(TARGET_STM32F030R8)
 #define INITIAL_SP            (0x20002000UL)
 
+#elif defined(TARGET_STM32L432KC)
+#define INITIAL_SP            (0x2000C000UL)
+
 #elif defined(TARGET_STM32L476VG)
 #define INITIAL_SP            (0x20018000UL)
 
@@ -420,6 +503,23 @@
 #elif defined(TARGET_STM32L152RC)
 #define INITIAL_SP            (0x20008000UL)
 
+#elif defined(TARGET_EFM32GG_STK3700) || defined(TARGET_BEETLE)
+#define INITIAL_SP            (0x20020000UL)
+
+#elif defined(TARGET_EFM32HG_STK3400)
+#define INITIAL_SP            (0x20002000UL)
+
+#elif defined(TARGET_EFM32LG_STK3600) || defined(TARGET_EFM32WG_STK3800) || defined(TARGET_EFM32PG_STK3401)
+#define INITIAL_SP            (0x20008000UL)
+
+#elif defined(TARGET_MCU_NORDIC_32K)
+#define INITIAL_SP            (0x20008000UL)
+
+#elif defined(TARGET_MCU_NORDIC_16K)
+#define INITIAL_SP            (0x20004000UL)
+
+#elif (defined(TARGET_STM32F767ZI))
+#define INITIAL_SP            (0x20080000UL)
 
 #else
 #error "no target defined"
@@ -434,15 +534,28 @@
 #define HEAP_START      (__end__)
 #elif defined(__ICCARM__)
 #pragma section="HEAP"
-#define HEAP_START     (void *)__section_begin("HEAP")
+#define HEAP_END  (void *)__section_end("HEAP")
 #endif
 
 void set_main_stack(void) {
+    uint32_t interrupt_stack_size = ((uint32_t)OS_MAINSTKSIZE * 4);
+#if defined(__ICCARM__)
+	/* For IAR heap is defined  .icf file */
+	uint32_t main_stack_size = ((uint32_t)INITIAL_SP - (uint32_t)HEAP_END) - interrupt_stack_size;
+#else
+	/* For ARM , uARM, or GCC_ARM , heap can grow and reach main stack */
+    uint32_t heap_plus_stack_size = ((uint32_t)INITIAL_SP - (uint32_t)HEAP_START) - interrupt_stack_size;
+    // Main thread's stack is 1/4 of the heap
+    uint32_t main_stack_size = heap_plus_stack_size/4;
+#endif
+    // The main thread must be 4 byte aligned
+    uint32_t main_stack_start = ((uint32_t)INITIAL_SP - interrupt_stack_size - main_stack_size) & ~0x7;
+
     // That is the bottom of the main stack block: no collision detection
-    os_thread_def_main.stack_pointer = HEAP_START;
+    os_thread_def_main.stack_pointer = (uint32_t*)main_stack_start;
 
     // Leave OS_MAINSTKSIZE words for the scheduler and interrupts
-    os_thread_def_main.stacksize = (INITIAL_SP - (unsigned int)HEAP_START) - (OS_MAINSTKSIZE * 4);
+    os_thread_def_main.stacksize = main_stack_size;
 }
 
 #if defined (__CC_ARM)
@@ -553,20 +666,25 @@
 
 #elif defined (__GNUC__)
 
+osMutexDef(malloc_mutex);
+static osMutexId malloc_mutex_id;
+osMutexDef(env_mutex);
+static osMutexId env_mutex_id;
+
 extern void __libc_fini_array(void);
 extern void __libc_init_array (void);
 extern int main(int argc, char **argv);
 
 void pre_main(void) {
+    malloc_mutex_id = osMutexCreate(osMutex(malloc_mutex));
+    env_mutex_id = osMutexCreate(osMutex(env_mutex));
     atexit(__libc_fini_array);
     __libc_init_array();
     main(0, NULL);
 }
 
-__attribute__((naked)) void software_init_hook (void) {
+__attribute__((naked)) void software_init_hook_rtos (void) {
   __asm (
-    ".syntax unified\n"
-    ".thumb\n"
     "bl   osKernelInitialize\n"
 #ifdef __MBED_CMSIS_RTOS_CM
     "bl   set_main_stack\n"
@@ -580,6 +698,29 @@
   );
 }
 
+// Opaque declaration of _reent structure
+struct _reent;
+
+void __rtos_malloc_lock( struct _reent *_r )
+{
+    osMutexWait(malloc_mutex_id, osWaitForever);
+}
+
+void __rtos_malloc_unlock( struct _reent *_r )
+{
+    osMutexRelease(malloc_mutex_id);
+}
+
+void __rtos_env_lock( struct _reent *_r )
+{
+    osMutexWait(env_mutex_id, osWaitForever);
+}
+
+void __rtos_env_unlock( struct _reent *_r )
+{
+    osMutexRelease(env_mutex_id);
+}
+
 #elif defined (__ICCARM__)
 
 extern void* __vector_table;
@@ -589,6 +730,8 @@
 extern __weak void __iar_init_vfp( void );
 extern void __iar_dynamic_initialization(void);
 extern void mbed_sdk_init(void);
+extern void mbed_main(void);
+extern int main(void);
 extern void exit(int arg);
 
 static uint8_t low_level_init_needed;
@@ -597,6 +740,7 @@
     if (low_level_init_needed) {
         __iar_dynamic_initialization();
     }
+    mbed_main();
     main();
 }
 

--- a/rtx/TARGET_CORTEX_M/RTX_Conf_CM.c	Mon May 23 11:00:15 2016 +0100
+++ b/rtx/TARGET_CORTEX_M/RTX_Conf_CM.c	Mon Jul 25 14:12:24 2016 +0100
@@ -51,13 +51,15 @@
 #  if   defined(TARGET_LPC1768) || defined(TARGET_LPC2368)   || defined(TARGET_LPC4088) || defined(TARGET_LPC4088_DM) || defined(TARGET_LPC4330) || defined(TARGET_LPC4337) || defined(TARGET_LPC1347) || defined(TARGET_K64F) || defined(TARGET_STM32F401RE)\
    || defined(TARGET_STM32F410RB) || defined(TARGET_KL46Z) || defined(TARGET_KL43Z)  || defined(TARGET_STM32F407) || defined(TARGET_F407VG)  || defined(TARGET_STM32F303VC) || defined(TARGET_LPC1549) || defined(TARGET_LPC11U68) \
    || defined(TARGET_STM32F411RE) || defined(TARGET_STM32F405RG) || defined(TARGET_K22F) || defined(TARGET_STM32F429ZI) || defined(TARGET_STM32F401VC) || defined(TARGET_MAX32610) || defined(TARGET_MAX32600) || defined(TARGET_TEENSY3_1) \
-   || defined(TARGET_STM32L152RE) || defined(TARGET_STM32F446RE) || defined(TARGET_STM32F446VE) || defined(TARGET_STM32L476VG) || defined(TARGET_STM32L476RG) || defined(TARGET_STM32F469NI) || defined(TARGET_STM32F746NG) || defined(TARGET_STM32F746ZG) || defined(TARGET_STM32L152RC)
+   || defined(TARGET_STM32L152RE) || defined(TARGET_STM32F446RE) || defined(TARGET_STM32F446VE) || defined(TARGET_STM32F446ZE) || defined(TARGET_STM32L432KC) || defined(TARGET_STM32L476VG) || defined(TARGET_STM32L476RG) || defined(TARGET_STM32F469NI) || defined(TARGET_STM32F746NG) || defined(TARGET_STM32F746ZG) || defined(TARGET_STM32L152RC) \
+   || defined(TARGET_EFM32GG_STK3700) || defined(TARGET_EFM32WG_STK3800) || defined(TARGET_EFM32LG_STK3600) || defined(TARGET_EFM32PG_STK3401) || defined(TARGET_STM32F767ZI)
 #    define OS_TASKCNT         14
 #  elif defined(TARGET_LPC11U24) || defined(TARGET_STM32F303RE) || defined(TARGET_STM32F303K8) || defined(TARGET_LPC11U35_401)  || defined(TARGET_LPC11U35_501) || defined(TARGET_LPCCAPPUCCINO) || defined(TARGET_LPC1114) \
    || defined(TARGET_LPC812)   || defined(TARGET_KL25Z)         || defined(TARGET_KL26Z)       || defined(TARGET_KL27Z)         || defined(TARGET_KL05Z)        || defined(TARGET_STM32F100RB)  || defined(TARGET_STM32F051R8) \
    || defined(TARGET_STM32F103RB) || defined(TARGET_LPC824) || defined(TARGET_STM32F302R8) || defined(TARGET_STM32F334R8) || defined(TARGET_STM32F334C8) \
    || defined(TARGET_STM32L031K6) || defined(TARGET_STM32L053R8) || defined(TARGET_STM32L053C8) || defined(TARGET_STM32L073RZ) || defined(TARGET_STM32F072RB) || defined(TARGET_STM32F091RC) || defined(TARGET_NZ32_SC151) \
-   || defined(TARGET_SSCI824)  || defined(TARGET_STM32F030R8) || defined(TARGET_STM32F070RB)
+   || defined(TARGET_SSCI824)  || defined(TARGET_STM32F030R8) || defined(TARGET_STM32F070RB) \
+   || defined(TARGET_EFM32HG_STK3400) || defined(TARGET_MCU_NRF51822) || defined(TARGET_BEETLE)
 #    define OS_TASKCNT         6
 #  else
 #    error "no target defined"
@@ -84,15 +86,21 @@
 #  if   defined(TARGET_LPC1768) || defined(TARGET_LPC2368)   || defined(TARGET_LPC4088) || defined(TARGET_LPC4088_DM) || defined(TARGET_LPC4330) || defined(TARGET_LPC4337) || defined(TARGET_LPC1347)  || defined(TARGET_K64F) || defined(TARGET_STM32F401RE)\
    || defined(TARGET_STM32F410RB) || defined(TARGET_KL46Z) || defined(TARGET_KL43Z) || defined(TARGET_STM32F407) || defined(TARGET_F407VG)  || defined(TARGET_STM32F303VC) || defined(TARGET_LPC1549) || defined(TARGET_LPC11U68) \
    || defined(TARGET_STM32F411RE) || defined(TARGET_STM32F405RG) || defined(TARGET_K22F) || defined(TARGET_STM32F429ZI) || defined(TARGET_STM32F401VC) || defined(TARGET_MAX32610) || defined(TARGET_MAX32600) || defined(TARGET_TEENSY3_1) \
-   || defined(TARGET_STM32L152RE) || defined(TARGET_STM32F446RE) || defined(TARGET_STM32F446VE) || defined(TARGET_STM32L476VG) || defined(TARGET_STM32L476RG) || defined(TARGET_STM32F469NI) || defined(TARGET_STM32F746NG) || defined(TARGET_STM32F746ZG) || defined(TARGET_STM32L152RC)
+   || defined(TARGET_STM32L152RE) || defined(TARGET_STM32F446RE) || defined(TARGET_STM32F446VE) || defined(TARGET_STM32F446ZE) || defined(TARGET_STM32L432KC) || defined(TARGET_STM32L476VG) || defined(TARGET_STM32L476RG) || defined(TARGET_STM32F469NI) || defined(TARGET_STM32F746NG) || defined(TARGET_STM32F746ZG) || defined(TARGET_STM32L152RC) \
+   ||defined(TARGET_EFM32GG_STK3700) || defined(TARGET_STM32F767ZI)
 #      define OS_MAINSTKSIZE    256
 #  elif defined(TARGET_LPC11U24) || defined(TARGET_LPC11U35_401)  || defined(TARGET_LPC11U35_501) || defined(TARGET_LPCCAPPUCCINO)  || defined(TARGET_LPC1114) \
    || defined(TARGET_LPC812)   || defined(TARGET_KL25Z)         || defined(TARGET_KL26Z)        || defined(TARGET_KL27Z)        || defined(TARGET_KL05Z)        || defined(TARGET_STM32F100RB)  || defined(TARGET_STM32F051R8) \
    || defined(TARGET_STM32F103RB) || defined(TARGET_LPC824) || defined(TARGET_STM32F302R8) || defined(TARGET_STM32F072RB) || defined(TARGET_STM32F091RC) || defined(TARGET_NZ32_SC151) \
-   || defined(TARGET_SSCI824) || defined(TARGET_STM32F030R8) || defined(TARGET_STM32F070RB)
+   || defined(TARGET_SSCI824) || defined(TARGET_STM32F030R8) || defined(TARGET_STM32F070RB) \
+   || defined(TARGET_EFM32WG_STK3800) || defined(TARGET_EFM32LG_STK3600) || defined(TARGET_EFM32PG_STK3401)
 #      define OS_MAINSTKSIZE    128
-#  elif defined(TARGET_STM32F334R8) || defined(TARGET_STM32F303RE) ||  defined(TARGET_STM32F303K8) ||  defined(TARGET_STM32F334C8) || defined(TARGET_STM32L031K6) || defined(TARGET_STM32L053R8) || defined(TARGET_STM32L053C8) || defined(TARGET_STM32L073RZ)
+#  elif defined(TARGET_STM32F334R8) || defined(TARGET_STM32F303RE) ||  defined(TARGET_STM32F303K8) ||  defined(TARGET_STM32F334C8) \
+   || defined(TARGET_STM32L031K6) || defined(TARGET_STM32L053R8) || defined(TARGET_STM32L053C8) || defined(TARGET_STM32L073RZ) \
+   || defined(TARGET_EFM32HG_STK3400) || defined(TARGET_BEETLE)
 #      define OS_MAINSTKSIZE    112
+#  elif defined(TARGET_MCU_NRF51822)
+#      define OS_MAINSTKSIZE    512
 #  else
 #    error "no target defined"
 #  endif
@@ -105,7 +113,7 @@
 #ifndef OS_PRIVCNT
  #define OS_PRIVCNT     0
 #endif
- 
+
 //   <o>Total stack size [bytes] for threads with user-provided stack size <0-1048576:8><#/4>
 //   <i> Defines the combined stack size for threads with user-provided stack size.
 //   <i> Default: 0
@@ -120,16 +128,16 @@
 #ifndef OS_STKCHECK
  #define OS_STKCHECK    1
 #endif
- 
+
 //   <q>Stack usage watermark
 //   <i> Initialize thread stack with watermark pattern for analyzing stack usage (current/maximum) in System and Thread Viewer.
 //   <i> Enabling this option increases significantly the execution time of osThreadCreate.
 #ifndef OS_STKINIT
 #define OS_STKINIT      0
 #endif
- 
-//   <o>Processor mode for thread execution 
-//     <0=> Unprivileged mode 
+
+//   <o>Processor mode for thread execution
+//     <0=> Unprivileged mode
 //     <1=> Privileged mode
 //   <i> Default: Privileged mode
 #ifndef OS_RUNPRIV
@@ -137,19 +145,23 @@
 #endif
 
 // </h>
- 
+
 // <h>RTX Kernel Timer Tick Configuration
 // ======================================
 //   <q> Use Cortex-M SysTick timer as RTX Kernel Timer
-//   <i> Cortex-M processors provide in most cases a SysTick timer that can be used as 
+//   <i> Cortex-M processors provide in most cases a SysTick timer that can be used as
 //   <i> as time-base for RTX.
 #ifndef OS_SYSTICK
- #define OS_SYSTICK     1
+#   if defined(TARGET_MCU_NRF51822)
+#       define OS_SYSTICK                0
+#   else
+#       define OS_SYSTICK                1
+#   endif
 #endif
 //
 //   <o>RTOS Kernel Timer input clock frequency [Hz] <1-1000000000>
-//   <i> Defines the input frequency of the RTOS Kernel Timer.  
-//   <i> When the Cortex-M SysTick timer is used, the input clock 
+//   <i> Defines the input frequency of the RTOS Kernel Timer.
+//   <i> When the Cortex-M SysTick timer is used, the input clock
 //   <i> is on most systems identical with the core clock.
 #ifndef OS_CLOCK
 #  if defined(TARGET_LPC1768) || defined(TARGET_LPC2368) || defined(TARGET_TEENSY3_1)
@@ -171,7 +183,7 @@
 #  elif defined(TARGET_LPC824) || defined(TARGET_SSCI824)
 #    define OS_CLOCK       30000000
 
-#  elif  defined(TARGET_STM32F100RB)
+#  elif  defined(TARGET_STM32F100RB) || defined(TARGET_BEETLE)
 #    define OS_CLOCK       24000000
 
 #  elif defined(TARGET_LPC4088) || defined(TARGET_LPC4088_DM) || defined(TARGET_K64F) || defined(TARGET_K22F)
@@ -210,7 +222,7 @@
 #elif defined(TARGET_STM32F401VC)
 #    define OS_CLOCK       84000000
 
-#  elif defined(TARGET_STM32F746NG) || defined(TARGET_STM32F746ZG)
+#  elif defined(TARGET_STM32F746NG) || defined(TARGET_STM32F746ZG) || defined(TARGET_STM32F767ZI)
 #     define OS_CLOCK      216000000
 
 #elif defined(TARGET_MAX32610) || defined(TARGET_MAX32600)
@@ -222,7 +234,7 @@
 #elif defined(TARGET_STM32L152RE)
 #    define OS_CLOCK       24000000
 
-#elif (defined(TARGET_STM32F446RE) || defined(TARGET_STM32F446VE))
+#elif defined(TARGET_STM32F446RE) || defined(TARGET_STM32F446ZE) || defined(TARGET_STM32F446VE)
 #    define OS_CLOCK       180000000
 
 #elif defined(TARGET_STM32F030R8)
@@ -231,7 +243,7 @@
 #elif defined(TARGET_STM32F070RB)
 #    define OS_CLOCK       48000000
 
-#elif defined(TARGET_STM32L476VG) || defined(TARGET_STM32L476RG)
+#elif defined(TARGET_STM32L432KC) || defined(TARGET_STM32L476VG) || defined(TARGET_STM32L476RG)
 #    define OS_CLOCK       80000000
 
 #elif defined(TARGET_STM32F469NI)
@@ -240,11 +252,18 @@
 #elif defined(TARGET_STM32L152RC)
 #    define OS_CLOCK       24000000
 
+#elif defined(TARGET_EFM32)
+#    include "clocking.h"
+#    define OS_CLOCK       REFERENCE_FREQUENCY
+
+#elif defined(TARGET_MCU_NRF51822)
+#    define OS_CLOCK        32768
+
 #  else
 #    error "no target defined"
 #  endif
 #endif
- 
+
 //   <o>RTX Timer tick interval value [us] <1-1000000>
 //   <i> The RTX Timer tick interval value is used to calculate timeout values.
 //   <i> When the Cortex-M SysTick timer is enabled, the value also configures the SysTick timer.
@@ -292,14 +311,14 @@
 #ifndef OS_TIMERPRIO
  #define OS_TIMERPRIO   5
 #endif
- 
+
 //   <o>Timer Thread stack size [bytes] <64-4096:8><#/4>
 //   <i> Defines stack size for Timer thread.
 //   <i> Default: 200
 #ifndef OS_TIMERSTKSZ
  #define OS_TIMERSTKSZ  200
 #endif
- 
+
 //   <o>Timer Callback Queue size <1-32>
 //   <i> Number of concurrent active timer callback functions.
 //   <i> Default: 4

--- a/rtx/TARGET_CORTEX_M/cmsis_os.h	Mon May 23 11:00:15 2016 +0100
+++ b/rtx/TARGET_CORTEX_M/cmsis_os.h	Mon Jul 25 14:12:24 2016 +0100
@@ -57,9 +57,13 @@
 #define CMSIS_OS_RTX
 
 // __MBED_CMSIS_RTOS_CM captures our changes to the RTX kernel
+#ifndef __MBED_CMSIS_RTOS_CM
 #define __MBED_CMSIS_RTOS_CM
+#endif
 // we use __CMSIS_RTOS version, which changes some API in the kernel
+#ifndef __CMSIS_RTOS
 #define __CMSIS_RTOS
+#endif
 
 // The stack space occupied is mainly dependent on the underling C standard library
 #if defined(TOOLCHAIN_GCC) || defined(TOOLCHAIN_ARM_STD) || defined(TOOLCHAIN_IAR)
@@ -68,6 +72,24 @@
 #    define WORDS_STACK_SIZE   128
 #endif
 
+#ifdef __MBED_CMSIS_RTOS_CM
+
+/* Single thread - disable timers and set task count to one */
+#if defined(MBED_RTOS_SINGLE_THREAD)
+#define OS_TASKCNT  1
+#define OS_TIMERS   0
+#endif
+
+/* If os timers macro is set to 0, there's no timer thread created, therefore
+ * main thread has tid 0x01  
+ */
+#if (OS_TIMERS != 0)
+#define MAIN_THREAD_ID 0x02
+#else
+#define MAIN_THREAD_ID 0x01
+#endif
+#endif
+
 #define DEFAULT_STACK_SIZE         (WORDS_STACK_SIZE*4)
 
 #define osCMSIS           0x10002U     ///< CMSIS-RTOS API version (main [31:16] .sub [15:0])
@@ -313,6 +335,8 @@
 /// \return thread ID for reference by other functions or NULL in case of error.
 osThreadId osThreadCreate (const osThreadDef_t *thread_def, void *argument);
 
+osThreadId osThreadContextCreate (const osThreadDef_t *thread_def, void *argument, void *context);
+
 /// Return the thread ID of the current running thread.
 /// \return thread ID for reference by other functions or NULL in case of error.
 osThreadId osThreadGetId (void);

--- a/rtx/TARGET_CORTEX_M/rt_CMSIS.c	Mon May 23 11:00:15 2016 +0100
+++ b/rtx/TARGET_CORTEX_M/rt_CMSIS.c	Mon Jul 25 14:12:24 2016 +0100
@@ -65,6 +65,7 @@
 #include "rt_MemBox.h"
 #include "rt_Memory.h"
 #include "rt_HAL_CM.h"
+#include "rt_OsEventObserver.h"
 
 #include "cmsis_os.h"
 
@@ -301,7 +302,7 @@
 #define SVC_Setup(f)                                                           \
   __asm(                                                                       \
     "mov r12,%0\n"                                                             \
-    :: "r"(&f): "r12"                                                          \
+    :: "r"(&f): "r0", "r1", "r2", "r3", "r12"                                  \
   );
 
 #define SVC_Ret3()                                                             \
@@ -458,7 +459,7 @@
 SVC_0_1(svcKernelSysTick,    uint32_t, RET_uint32_t)
 
 static void  sysThreadError   (osStatus status);
-osThreadId   svcThreadCreate  (const osThreadDef_t *thread_def, void *argument);
+osThreadId   svcThreadCreate  (const osThreadDef_t *thread_def, void *argument, void *context);
 osMessageQId svcMessageCreate (const osMessageQDef_t *queue_def, osThreadId thread_id);
 
 // Kernel Control Service Calls
@@ -488,7 +489,7 @@
   if (os_initialized == 0U) {
     // Create OS Timers resources (Message Queue & Thread)
     osMessageQId_osTimerMessageQ = svcMessageCreate (&os_messageQ_def_osTimerMessageQ, NULL);
-    osThreadId_osTimerThread = svcThreadCreate(&os_thread_def_osTimerThread, NULL);
+    osThreadId_osTimerThread = svcThreadCreate(&os_thread_def_osTimerThread, NULL, NULL);
   }
 
   sysThreadError(osOK);
@@ -562,6 +563,15 @@
   if (__get_IPSR() != 0U) {
     return osErrorISR;                          // Not allowed in ISR
   }
+
+  /* Call the pre-start event (from unprivileged mode) if the handler exists
+   * and the kernel is not running. */
+  /* FIXME osEventObs needs to be readable but not writable from unprivileged
+   * code. */
+  if (!osKernelRunning() && osEventObs && osEventObs->pre_start) {
+    osEventObs->pre_start();
+  }
+
   switch (__get_CONTROL() & 0x03U) {
     case 0x00U:                                 // Privileged Thread mode & MSP
       __set_PSP((uint32_t)(stack + 8));         // Initial PSP
@@ -616,7 +626,7 @@
 __NO_RETURN void osThreadExit (void);
 
 // Thread Service Calls declarations
-SVC_2_1(svcThreadCreate,      osThreadId, const osThreadDef_t *, void *,     RET_pointer)
+SVC_3_1(svcThreadCreate,      osThreadId, const osThreadDef_t *, void *, void *, RET_pointer)
 SVC_0_1(svcThreadGetId,       osThreadId,                                    RET_pointer)
 SVC_1_1(svcThreadTerminate,   osStatus,         osThreadId,                  RET_osStatus)
 SVC_0_1(svcThreadYield,       osStatus,                                      RET_osStatus)
@@ -626,7 +636,7 @@
 // Thread Service Calls
 
 /// Create a thread and add it to Active Threads and set it to state READY
-osThreadId svcThreadCreate (const osThreadDef_t *thread_def, void *argument) {
+osThreadId svcThreadCreate (const osThreadDef_t *thread_def, void *argument, void *context) {
   P_TCB  ptcb;
   OS_TID tsk;
   void  *stk;
@@ -683,6 +693,12 @@
 
   *((uint32_t *)ptcb->tsk_stack + 13) = (uint32_t)osThreadExit;
 
+  if (osEventObs && osEventObs->thread_create) {
+    ptcb->context = osEventObs->thread_create(ptcb->task_id, context);
+  } else {
+    ptcb->context = context;
+  }
+
   return ptcb;
 }
 
@@ -712,6 +728,10 @@
   stk = ptcb->priv_stack ? ptcb->stack : NULL;  // Private stack
 #endif
 
+  if (osEventObs && osEventObs->thread_destroy) {
+    osEventObs->thread_destroy(ptcb->context);
+  }
+
   res = rt_tsk_delete(ptcb->task_id);           // Delete task
 
   if (res == OS_R_NOK) {
@@ -776,14 +796,17 @@
 
 /// Create a thread and add it to Active Threads and set it to state READY
 osThreadId osThreadCreate (const osThreadDef_t *thread_def, void *argument) {
-  if (__get_IPSR() != 0U) { 
+  return osThreadContextCreate(thread_def, argument, NULL);
+}
+osThreadId osThreadContextCreate (const osThreadDef_t *thread_def, void *argument, void *context) {
+  if (__get_IPSR() != 0U) {
     return NULL;                                // Not allowed in ISR
   }
   if (((__get_CONTROL() & 1U) == 0U) && (os_running == 0U)) {
     // Privileged and not running
-    return   svcThreadCreate(thread_def, argument);
+    return   svcThreadCreate(thread_def, argument, context);
   } else {
-    return __svcThreadCreate(thread_def, argument);
+    return __svcThreadCreate(thread_def, argument, context);
   }
 }
 

--- a/rtx/TARGET_CORTEX_M/rt_HAL_CM.h	Mon May 23 11:00:15 2016 +0100
+++ b/rtx/TARGET_CORTEX_M/rt_HAL_CM.h	Mon Jul 25 14:12:24 2016 +0100
@@ -255,7 +255,11 @@
   if (prigroup >= sh) {
     sh = prigroup + 1U;
   }
+
+/* Only change the SVCall priority if uVisor is not present. */
+#if !(defined(FEATURE_UVISOR) && defined(TARGET_UVISOR_SUPPORTED))
   NVIC_SYS_PRI2 = ((0xFEFFFFFFU << sh) & 0xFF000000U) | (NVIC_SYS_PRI2 & 0x00FFFFFFU);
+#endif /* !(defined(FEATURE_UVISOR) && defined(TARGET_UVISOR_SUPPORTED)) */
 #endif
 }
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rtx/TARGET_CORTEX_M/rt_OsEventObserver.c	Mon Jul 25 14:12:24 2016 +0100
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------
+ *      CMSIS-RTOS  -  RTX
+ *----------------------------------------------------------------------------
+ *      Name:    rt_OsEventObserver.c
+ *      Purpose: OS Event Callbacks for CMSIS RTOS
+ *      Rev.:    VX.XX
+ *----------------------------------------------------------------------------
+ *
+ * Copyright (c) 1999-2009 KEIL, 2009-2015 ARM Germany GmbH
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *  - Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  - Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *  - Neither the name of ARM  nor the names of its contributors may be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *---------------------------------------------------------------------------*/
+
+#include "rt_OsEventObserver.h"
+
+/*
+ *  _____ _____  ____  __ _____
+ * |  ___|_ _\ \/ /  \/  | ____|
+ * | |_   | | \  /| |\/| |  _|
+ * |  _|  | | /  \| |  | | |___
+ * |_|   |___/_/\_\_|  |_|_____|
+ *
+ * FIXME:
+ * The osEventObs variable must be in protected memory. If not every box
+ * and box 0 can modify osEventObs to point to any handler to run code
+ * privileged. This issue is tracked at
+ * <https://github.com/ARMmbed/uvisor/issues/235>.
+ */
+const OsEventObserver *osEventObs;
+
+void osRegisterForOsEvents(const OsEventObserver *observer)
+{
+    static uint8_t has_been_called = 0;
+    if (has_been_called) {
+        return;
+    }
+    has_been_called = 1;
+
+    osEventObs = observer;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rtx/TARGET_CORTEX_M/rt_OsEventObserver.h	Mon Jul 25 14:12:24 2016 +0100
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------
+ *      CMSIS-RTOS  -  RTX
+ *----------------------------------------------------------------------------
+ *      Name:    os_events.h
+ *      Purpose: OS Event Callbacks for CMSIS RTOS
+ *      Rev.:    VX.XX
+ *----------------------------------------------------------------------------
+ *
+ * Copyright (c) 1999-2009 KEIL, 2009-2016 ARM Germany GmbH
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *  - Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  - Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *  - Neither the name of ARM  nor the names of its contributors may be used
+ *    to endorse or promote products derived from this software without
+ *    specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *---------------------------------------------------------------------------*/
+#ifndef _RT_OS_EVENT_OBSERVER_H
+#define _RT_OS_EVENT_OBSERVER_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+    uint32_t version;
+    void (*pre_start)(void);
+    void *(*thread_create)(int thread_id, void *context);
+    void (*thread_destroy)(void *context);
+    void (*thread_switch)(void *context);
+} OsEventObserver;
+extern const OsEventObserver *osEventObs;
+
+void osRegisterForOsEvents(const OsEventObserver *observer);
+
+#ifdef __cplusplus
+};
+#endif
+
+#endif

--- a/rtx/TARGET_CORTEX_M/rt_System.c	Mon May 23 11:00:15 2016 +0100
+++ b/rtx/TARGET_CORTEX_M/rt_System.c	Mon Jul 25 14:12:24 2016 +0100
@@ -315,7 +315,7 @@
 __weak void rt_stk_check (void) {
 #ifdef __MBED_CMSIS_RTOS_CM
     /* Check for stack overflow. */
-    if (os_tsk.run->task_id == 0x02) {
+    if (os_tsk.run->task_id == MAIN_THREAD_ID) {
         // TODO: For the main thread the check should be done against the main heap pointer
     } else {
         if ((os_tsk.run->tsk_stack < (U32)os_tsk.run->stack) ||

--- a/rtx/TARGET_CORTEX_M/rt_Task.c	Mon May 23 11:00:15 2016 +0100
+++ b/rtx/TARGET_CORTEX_M/rt_Task.c	Mon Jul 25 14:12:24 2016 +0100
@@ -40,6 +40,7 @@
 #include "rt_MemBox.h"
 #include "rt_Robin.h"
 #include "rt_HAL_CM.h"
+#include "rt_OsEventObserver.h"
 
 /*----------------------------------------------------------------------------
  *      Global Variables
@@ -101,6 +102,9 @@
   /* Switch to next task (identified by "p_new"). */
   os_tsk.new_tsk   = p_new;
   p_new->state = RUNNING;
+  if (osEventObs && osEventObs->thread_switch) {
+    osEventObs->thread_switch(p_new->context);
+  }
   DBG_TASK_SWITCH(p_new->task_id);
 }
 
@@ -402,6 +406,10 @@
   os_tsk.run = &os_idle_TCB;
   os_tsk.run->state = RUNNING;
 
+  /* Set the current thread to idle, so that on exit from this SVCall we do not
+   * de-reference a NULL TCB. */
+  rt_switch_req(&os_idle_TCB);
+
   /* Initialize ps queue */
   os_psq->first = 0U;
   os_psq->last  = 0U;

--- a/rtx/TARGET_CORTEX_M/rt_TypeDef.h	Mon May 23 11:00:15 2016 +0100
+++ b/rtx/TARGET_CORTEX_M/rt_TypeDef.h	Mon Jul 25 14:12:24 2016 +0100
@@ -79,6 +79,7 @@
 
   /* Task entry point used for uVision debugger                              */
   FUNCP  ptask;                   /* Task entry address                      */
+  void   *context;                /* Pointer to thread context               */
 } *P_TCB;
 #define TCB_STACKF      37        /* 'stack_frame' offset                    */
 #define TCB_TSTACK      44        /* 'tsk_stack' offset                      */