diff --git a/libraries/rtos/rtos/Mutex.h b/libraries/rtos/rtos/Mutex.h
index 5275339113..841d10a840 100644
--- a/libraries/rtos/rtos/Mutex.h
+++ b/libraries/rtos/rtos/Mutex.h
@@ -57,7 +57,7 @@ private:
     osMutexId _osMutexId;
     osMutexDef_t _osMutexDef;
 #ifdef CMSIS_OS_RTX
-#ifdef __MBED_CMSIS_RTOS_CA9
+#if defined(__MBED_CMSIS_RTOS_CA9) || defined(__MBED_CMSIS_RTOS_CM)
     int32_t _mutex_data[4];
 #else
     int32_t _mutex_data[3];
diff --git a/libraries/rtos/rtos/RtosTimer.h b/libraries/rtos/rtos/RtosTimer.h
index 9e7004c84d..4fc2eae53b 100644
--- a/libraries/rtos/rtos/RtosTimer.h
+++ b/libraries/rtos/rtos/RtosTimer.h
@@ -61,8 +61,10 @@ public:
 private:
     osTimerId _timer_id;
     osTimerDef_t _timer;
-#ifdef CMSIS_OS_RTX
+#if defined(CMSIS_OS_RTX) && !defined(__MBED_CMSIS_RTOS_CM)
     uint32_t _timer_data[5];
+#else
+    uint32_t _timer_data[6];
 #endif
 };
 
diff --git a/libraries/rtos/rtos/Thread.cpp b/libraries/rtos/rtos/Thread.cpp
index 2f6d002ed1..ed8270a36a 100644
--- a/libraries/rtos/rtos/Thread.cpp
+++ b/libraries/rtos/rtos/Thread.cpp
@@ -28,7 +28,7 @@ namespace rtos {
 
 Thread::Thread(void (*task)(void const *argument), void *argument,
         osPriority priority, uint32_t stack_size, unsigned char *stack_pointer) {
-#ifdef CMSIS_OS_RTX
+#ifdef __MBED_CMSIS_RTOS_CM
     _thread_def.pthread = task;
     _thread_def.tpriority = priority;
     _thread_def.stacksize = stack_size;
@@ -71,8 +71,10 @@ int32_t Thread::signal_clr(int32_t signals) {
 }
 
 Thread::State Thread::get_state() {
-#ifndef __MBED_CMSIS_RTOS_CA9
+#if !defined(__MBED_CMSIS_RTOS_CA9) && !defined(__MBED_CMSIS_RTOS_CM)
+#ifdef CMSIS_OS_RTX
     return ((State)_thread_def.tcb.state);
+#endif
 #else
     uint8_t status;
     status = osThreadGetState(_tid);
@@ -82,32 +84,45 @@ Thread::State Thread::get_state() {
 
 uint32_t Thread::stack_size() {
 #ifndef __MBED_CMSIS_RTOS_CA9
+#if defined(CMSIS_OS_RTX) && !defined(__MBED_CMSIS_RTOS_CM)
     return _thread_def.tcb.priv_stack;
 #else
     return 0;
 #endif
+#else
+    return 0;
+#endif
 }
 
 uint32_t Thread::free_stack() {
 #ifndef __MBED_CMSIS_RTOS_CA9
+#if defined(CMSIS_OS_RTX) && !defined(__MBED_CMSIS_RTOS_CM)
     uint32_t bottom = (uint32_t)_thread_def.tcb.stack;
     return _thread_def.tcb.tsk_stack - bottom;
 #else
     return 0;
 #endif
+#else
+    return 0;
+#endif
 }
 
 uint32_t Thread::used_stack() {
 #ifndef __MBED_CMSIS_RTOS_CA9
+#if defined(CMSIS_OS_RTX) && !defined(__MBED_CMSIS_RTOS_CM)
     uint32_t top = (uint32_t)_thread_def.tcb.stack + _thread_def.tcb.priv_stack;
     return top - _thread_def.tcb.tsk_stack;
 #else
     return 0;
 #endif
+#else
+    return 0;
+#endif
 }
 
 uint32_t Thread::max_stack() {
 #ifndef __MBED_CMSIS_RTOS_CA9
+#if defined(CMSIS_OS_RTX) && !defined(__MBED_CMSIS_RTOS_CM)
     uint32_t high_mark = 0;
     while (_thread_def.tcb.stack[high_mark] == 0xE25A2EA5)
         high_mark++;
@@ -115,6 +130,9 @@ uint32_t Thread::max_stack() {
 #else
     return 0;
 #endif
+#else
+    return 0;
+#endif
 }
 
 osEvent Thread::signal_wait(int32_t signals, uint32_t millisec) {
@@ -139,9 +157,11 @@ void Thread::attach_idle_hook(void (*fptr)(void)) {
 
 Thread::~Thread() {
     terminate();
+#ifdef __MBED_CMSIS_RTOS_CM
     if (_dynamic_stack) {
         delete[] (_thread_def.stack_pointer);
     }
+#endif
 }
 
 }
diff --git a/libraries/rtos/rtx/TARGET_CORTEX_M/HAL_CM.c b/libraries/rtos/rtx/TARGET_CORTEX_M/HAL_CM.c
index 7f0a333cfd..97e12b1ddd 100644
--- a/libraries/rtos/rtx/TARGET_CORTEX_M/HAL_CM.c
+++ b/libraries/rtos/rtx/TARGET_CORTEX_M/HAL_CM.c
@@ -90,6 +90,32 @@ void rt_init_stack (P_TCB p_TCB, FUNCP task_body) {
   /* Task entry point. */
   p_TCB->ptask = task_body;
 
+
+#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
+   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) {
+    p_TCB->stack[0] = MAGIC_WORD;
+
+    /* Initialize stack with magic pattern. */
+    if (os_stackinfo & 0x10000000U) {
+      if (size > (16U+1U)) {
+        for (i = ((size - 16U)/2U) - 1U; i; i--) {
+          stk -= 2U;
+          stk[1] = MAGIC_PATTERN;
+          stk[0] = MAGIC_PATTERN;
+        }
+        if (--stk > p_TCB->stack) {
+          *stk = MAGIC_PATTERN;
+        }
+      }
+    }
+  }
+#else
   /* Initialize stack with magic pattern. */
   if (os_stackinfo & 0x10000000U) {
     if (size > (16U+1U)) {
@@ -104,15 +130,6 @@ void rt_init_stack (P_TCB p_TCB, FUNCP task_body) {
     }
   }
 
-#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
-   heap, therefore the last word of the stack is a moving target.
-   We want to do stack/heap collision detection instead.
-  */
-  if (p_TCB->task_id != 0x02)
-      p_TCB->stack[0] = MAGIC_WORD;
-#else
   /* Set a magic word for checking of stack overflow. */
   p_TCB->stack[0] = MAGIC_WORD;
 #endif
diff --git a/libraries/rtos/rtx/TARGET_CORTEX_M/RTX_CM_lib.h b/libraries/rtos/rtx/TARGET_CORTEX_M/RTX_CM_lib.h
index 3d13425ed6..5f5970f13b 100755
--- a/libraries/rtos/rtx/TARGET_CORTEX_M/RTX_CM_lib.h
+++ b/libraries/rtos/rtx/TARGET_CORTEX_M/RTX_CM_lib.h
@@ -571,7 +571,7 @@ __attribute__((naked)) void software_init_hook (void) {
     "mov  r0,r4\n"
     "mov  r1,r5\n"
     "bl   osKernelInitialize\n"
-#ifdef
+#ifdef __MBED_CMSIS_RTOS_CM
     "bl   set_main_stack\n"
 #endif
     "ldr  r0,=os_thread_def_main\n"