diff --git a/cpu/msp430-common/cpu.c b/cpu/msp430-common/cpu.c index 6442f873e2..d6c8c9e0e0 100644 --- a/cpu/msp430-common/cpu.c +++ b/cpu/msp430-common/cpu.c @@ -7,6 +7,7 @@ */ #include "cpu.h" +#include "irq.h" #include "kernel.h" #include "kernel_internal.h" #include "sched.h" @@ -20,12 +21,14 @@ void thread_yield(void) { __save_context(); - dINT(); + /* disable IRQ, remembering if they are + to be reactivated after context switch */ + unsigned int irqen = disableIRQ(); + /* have sched_active_thread point to the next thread */ sched_run(); - eINT(); - __restore_context(); + __restore_context(irqen); } NORETURN void cpu_switch_context_exit(void) @@ -33,7 +36,7 @@ NORETURN void cpu_switch_context_exit(void) sched_active_thread = sched_threads[0]; sched_run(); - __restore_context(); + __restore_context(GIE); UNREACHABLE(); } diff --git a/cpu/msp430-common/include/cpu.h b/cpu/msp430-common/include/cpu.h index d8e304fd75..adadfdc719 100644 --- a/cpu/msp430-common/include/cpu.h +++ b/cpu/msp430-common/include/cpu.h @@ -99,9 +99,20 @@ inline void __save_context(void) __save_context_isr(); } -inline void __restore_context(void) +inline void __restore_context(unsigned int irqen) { __restore_context_isr(); + + /* we want to enable if appropriate IRQs *just after* + quitting the interrupt handler; to that end, + we change the GIE bit in the value to be restored + in R2 (a.k.a. SR) by the next RETI instruction */ + if (irqen) { + __asm__("bis.w #8, 0(r1)"); + } else { + __asm__("bic.w #8, 0(r1)"); + } + __asm__("reti"); }