diff --git a/cpu/msp430_common/cpu.c b/cpu/msp430_common/cpu.c
index 6bc2c710f2..8fc4989a45 100644
--- a/cpu/msp430_common/cpu.c
+++ b/cpu/msp430_common/cpu.c
@@ -14,23 +14,49 @@
 #include "thread.h"
 
 /*
- * we must prevent the compiler to generate a prologue or an epilogue
- * for thread_yield_higher(), since we rely on the RETI instruction at the end
- * of its execution, in the inlined __restore_context() sub-function
+ * This function can both be called from ISR and from thread context.
+ *
+ * In both cases, the caller will use "CALL", which pushes the return
+ * address on the stack before executing the function's first instruction.
+ *
+ * If called within ISR, just set sched_context_switch_request and
+ * directly return to the call site. A regular function return does this.
+ *
+ * If called from stack context, it needs to prepare the stack so it looks exactly
+ * as if the thread had been interrupted by an ISR, which requires the SR to be on
+ * stack right after the return address. So we do this manually.
+ * __save_context() will then save the remaining registers, then store the stack
+ * pointer in the thread's thread_t.
+ *
+ * At this point, the thread context is properly saved. sched_run (possibly) changes
+ * the currently active thread, which __restore_context() then restores, resuming
+ * execution at the call site using reti.
+ *
  */
-__attribute__((naked)) void thread_yield_higher(void)
+void thread_yield_higher(void)
 {
-    __asm__("push r2"); /* save SR */
-    irq_disable();
+    if (irq_is_in()) {
+        sched_context_switch_request = 1;
+    }
+    else {
+        __asm__ volatile (
+            "push r2"   "\n\t"  /* save SR */
+            "dint"      "\n\t"  /* reti will restore SR, and thus, IRQ state */
+            "nop"       "\n\t"  /* dint takes an additional CPU cycle to take into effect */
+            :                   /* no outputs */
+            :                   /* no inputs */
+            :                   /* no clobbers */
+            );
 
-    __save_context();
+        __save_context();
 
-    /* have sched_active_thread point to the next thread */
-    sched_run();
+        /* have sched_active_thread point to the next thread */
+        sched_run();
 
-    __restore_context();
+        __restore_context();
 
-    UNREACHABLE();
+        UNREACHABLE();
+    }
 }
 
 /* This function calculates the ISR_usage */