1 files changed, 11 insertions, 0 deletions

diff --git a/firmware/thread.c b/firmware/thread.c
index 655af1a940..91fe81be4a 100644
--- a/firmware/thread.c
+++ b/firmware/thread.c
@@ -1149,8 +1149,19 @@ void switch_thread(void)
     }
 
 #ifdef RB_PROFILE
+#ifdef CPU_COLDFIRE
+    /* Call this from asm to make sure the sp is pointing to the
+       correct place before the context is saved */
+    uint16_t id = thread->id & THREAD_ID_SLOT_MASK;
+    asm volatile ("move.l %[id], -(%%sp)\n\t"
+                  "jsr profile_thread_stopped\n\t"
+                  "addq.l #4, %%sp\n\t"
+                  :: [id] "r" (id)
+                  : "cc", "memory");
+#else
     profile_thread_stopped(thread->id & THREAD_ID_SLOT_MASK);
 #endif
+#endif
 
     /* Begin task switching by saving our current context so that we can
      * restore the state of the current thread later to the point prior