diff --git a/core/msg.c b/core/msg.c
index 2afc1def316e..b773bd1aa1b3 100644
--- a/core/msg.c
+++ b/core/msg.c
@@ -165,7 +165,7 @@ static int _msg_send(msg_t *m, kernel_pid_t target_pid, bool block, unsigned sta
         sched_set_status(target, STATUS_PENDING);
 
         irq_restore(state);
-        thread_yield_higher();
+
     }
 
     return 1;
@@ -207,8 +207,11 @@ int msg_send_int(msg_t *m, kernel_pid_t target_pid)
         msg_t *target_message = (msg_t*) target->wait_data;
         *target_message = *m;
         sched_set_status(target, STATUS_PENDING);
-
+#if !defined(ISR_CONTEXT_SWITCH_ALLOWED)
         sched_context_switch_request = 1;
+#else
+        thread_yield_higher();
+#endif
         return 1;
     }
     else {
@@ -272,7 +275,11 @@ int msg_reply_int(msg_t *m, msg_t *reply)
     msg_t *target_message = (msg_t*) target->wait_data;
     *target_message = *reply;
     sched_set_status(target, STATUS_PENDING);
-    sched_context_switch_request = 1;
+#if !defined(ISR_CONTEXT_SWITCH_ALLOWED)
+        sched_context_switch_request = 1;
+#else
+        thread_yield_higher();
+#endif
     return 1;
 }
 
diff --git a/core/sched.c b/core/sched.c
index 84151433d041..87521c57c244 100644
--- a/core/sched.c
+++ b/core/sched.c
@@ -81,7 +81,9 @@ schedstat sched_pidlist[KERNEL_PID_LAST + 1];
 
 int __attribute__((used)) sched_run(void)
 {
+#if !defined(ISR_CONTEXT_SWITCH_ALLOWED)
     sched_context_switch_request = 0;
+#endif
 
     thread_t *active_thread = (thread_t *)sched_active_thread;
 
@@ -194,6 +196,7 @@ void sched_switch(uint16_t other_prio)
           active_thread->pid, current_prio, on_runqueue, other_prio);
 
     if (!on_runqueue || (current_prio > other_prio)) {
+#if !defined(ISR_CONTEXT_SWITCH_ALLOWED)
         if (irq_is_in()) {
             DEBUG("sched_switch: setting sched_context_switch_request.\n");
             sched_context_switch_request = 1;
@@ -202,6 +205,10 @@ void sched_switch(uint16_t other_prio)
             DEBUG("sched_switch: yielding immediately.\n");
             thread_yield_higher();
         }
+#else
+        DEBUG("sched_switch: yielding immediately.\n");
+        thread_yield_higher();
+#endif
     }
     else {
         DEBUG("sched_switch: continuing without yield.\n");
diff --git a/core/thread_flags.c b/core/thread_flags.c
index 314b77a29e4c..38a225084b9b 100644
--- a/core/thread_flags.c
+++ b/core/thread_flags.c
@@ -114,7 +114,11 @@ inline int __attribute__((always_inline)) thread_flags_wake(thread_t *thread)
     if (wakeup) {
         DEBUG("_thread_flags_wake(): waking up pid %"PRIkernel_pid"\n", thread->pid);
         sched_set_status(thread, STATUS_PENDING);
+#if !defined(ISR_CONTEXT_SWITCH_ALLOWED)
         sched_context_switch_request = 1;
+#else
+        thread_yield_higher();
+#endif
     }
 
     return wakeup;
diff --git a/cpu/atmega_common/periph/timer.c b/cpu/atmega_common/periph/timer.c
index 3b860ee1be40..e9eb3c298053 100644
--- a/cpu/atmega_common/periph/timer.c
+++ b/cpu/atmega_common/periph/timer.c
@@ -174,11 +174,12 @@ static inline void _isr(tim_t tim, int chan)
     *ctx[tim].mask &= ~(1 << (chan + OCIE1A));
     ctx[tim].cb(ctx[tim].arg, chan);
 
+#if !defined(ISR_CONTEXT_SWITCH_ALLOWED)
     if (sched_context_switch_request) {
         thread_yield();
         thread_yield_isr();
     }
-
+#endif
     __exit_isr();
 }
 #endif
diff --git a/cpu/atmega_common/periph/uart.c b/cpu/atmega_common/periph/uart.c
index 3d13a3bf30da..39a04788f848 100644
--- a/cpu/atmega_common/periph/uart.c
+++ b/cpu/atmega_common/periph/uart.c
@@ -168,10 +168,12 @@ static inline void isr_handler(int num)
 {
     isr_ctx[num].rx_cb(isr_ctx[num].arg, dev[num]->DR);
 
+#if !defined(ISR_CONTEXT_SWITCH_ALLOWED)
     if (sched_context_switch_request) {
         thread_yield();
         thread_yield_isr();
     }
+#endif
 }
 
 #ifdef UART_0_ISR
diff --git a/cpu/atmega_common/thread_arch.c b/cpu/atmega_common/thread_arch.c
index d76995e4e1c6..8b7d9a8b39b7 100644
--- a/cpu/atmega_common/thread_arch.c
+++ b/cpu/atmega_common/thread_arch.c
@@ -221,13 +221,24 @@ void NORETURN __enter_thread_mode(void)
 }
 
 void thread_yield_higher(void) {
+
     if (irq_is_in() == 0) {
         __context_save();
         sched_run();
         __context_restore();
+
         __asm__ volatile("ret");
     } else {
+#if !defined(ISR_CONTEXT_SWITCH_ALLOWED)
         sched_context_switch_request = 1;
+#else
+        __context_save();
+        sched_run();
+        __context_restore();
+
+        __exit_isr();
+        __asm__ volatile("reti");
+#endif
     }
 }
 
@@ -237,7 +248,8 @@ void thread_yield_isr(void) {
     __context_restore();
 
     __exit_isr();
-
+    PORTF |= (1 << 5);
+    PORTF &= ~(1 << 5);
     __asm__ volatile("reti");
 }
 
diff --git a/sys/evtimer/evtimer.c b/sys/evtimer/evtimer.c
index b4eae653753f..0f321eec37a4 100644
--- a/sys/evtimer/evtimer.c
+++ b/sys/evtimer/evtimer.c
@@ -150,9 +150,12 @@ void evtimer_add(evtimer_t *evtimer, evtimer_event_t *event)
         _set_timer(&evtimer->timer, event->offset);
     }
     irq_restore(state);
+#if !defined(ISR_CONTEXT_SWITCH_ALLOWED)
     if (sched_context_switch_request) {
         thread_yield_higher();
     }
+#endif
+
 }
 
 void evtimer_del(evtimer_t *evtimer, evtimer_event_t *event)