diff --git a/boards/arduino-mega2560/include/board.h b/boards/arduino-mega2560/include/board.h
index b09d20df4ff0e893247d820bb1b1cdd93f79d6bf..715a8e8a8b70e0fb14a948f10f2384205a787c62 100644
--- a/boards/arduino-mega2560/include/board.h
+++ b/boards/arduino-mega2560/include/board.h
@@ -52,6 +52,22 @@ extern "C" {
 #define LED0_TOGGLE         (PORTB ^=  LED0_MASK)
 /** @} */
 
+
+/**
+ * Context swap defines
+ * Setup to use PJ6 which is pin change interrupt 15 (PCINT15)
+ * This emulates a software triggered interrupt
+ **/
+#define AVR_CONTEXT_SWAP_INIT do { \
+    DDRJ |= (1 << PJ6); \
+    PCICR |= (1 << PCIE1); \
+    PCMSK1 |= (1 << PCINT15); \
+} while (0)
+#define AVR_CONTEXT_SWAP_INTERRUPT_VECT  PCINT1_vect
+#define AVR_CONTEXT_SWAP_TRIGGER   PORTJ ^= (1 << PJ6)
+
+
+
 /**
  * @brief xtimer configuration values
  * @{
diff --git a/boards/waspmote-pro/include/board.h b/boards/waspmote-pro/include/board.h
index a63f84aaa7fd01e59c249d19b84c1b5a24e71b30..5bebfe900a65d47507af9694a510dfcdf41edc89 100644
--- a/boards/waspmote-pro/include/board.h
+++ b/boards/waspmote-pro/include/board.h
@@ -150,6 +150,19 @@ extern "C" {
 
 /** @} */
 
+/**
+ * Context swap defines
+ * Setup to use PB5 which is pin change interrupt 5
+ * This emulates a software triggered interrupt
+ **/
+#define AVR_CONTEXT_SWAP_INIT do { \
+    DDRB |= (1 << PB5); \
+    PCICR |= (1 << PCIE0); \
+    PCMSK0 |= (1 << PCINT5); \
+} while (0)
+#define AVR_CONTEXT_SWAP_INTERRUPT_VECT  PCINT0_vect
+#define AVR_CONTEXT_SWAP_TRIGGER   PORTB ^= (1 << PB5)
+
 /**
  * @brief xtimer configuration values
  * @{
diff --git a/cpu/atmega_common/thread_arch.c b/cpu/atmega_common/thread_arch.c
index 5baf8f70e0f69e0951855a8cdebd4addc1ec41f2..0193dd5457762b6df82bb375f8da9feb47d1ded1 100644
--- a/cpu/atmega_common/thread_arch.c
+++ b/cpu/atmega_common/thread_arch.c
@@ -25,11 +25,37 @@
 #include "sched.h"
 #include "irq.h"
 #include "cpu.h"
+#include "board.h"
+
+
+/**
+ * @brief AVR_CONTEXT_SWAP_INIT intialize the context swap trigger
+ * Called when threading is first started.
+ */
+#ifndef AVR_CONTEXT_SWAP_INIT
+#error AVR_CONTEXT_SWAP_INIT must be defined in board.h
+#endif
+
+/**
+ * @brief AVR_CONTEXT_SWAP_INTERRUPT_VECT Name of the ISR to use for context swapping
+ */
+#ifndef AVR_CONTEXT_SWAP_INTERRUPT_VECT
+#error AVR_CONTEXT_SWAP_INTERRUPT_VECT must be defined in board.h
+#endif
+
+/**
+ * @brief AVR_CONTEXT_SWAP_TRIGGER executed to start the context swap
+ * When executed, this should result in the interrupt named in
+ * AVR_CONTEXT_SWAP_INTERRUPT_VECT being called
+ */
+#ifndef AVR_CONTEXT_SWAP_TRIGGER
+#error ARV_CONTEXT_SWAP_TRIGGER must be defined in board.h
+#endif
+
 
 /*
  * local function declarations  (prefixed with __)
  */
-
 static void __context_save(void);
 static void __context_restore(void);
 static void __enter_thread_mode(void);
@@ -122,7 +148,6 @@ char *thread_arch_stack_init(thread_task_func_t task_func, void *arg,
     stk--;
     *stk = (uint8_t) 0x00;
 #endif
-
 #if defined(RAMPZ)
     stk--;
     *stk = (uint8_t) 0x00;
@@ -221,6 +246,7 @@ void thread_arch_start_threading(void) __attribute__((naked));
 void thread_arch_start_threading(void)
 {
     sched_run();
+    AVR_CONTEXT_SWAP_INIT;
     __enter_thread_mode();
 }
 
@@ -238,17 +264,17 @@ void NORETURN __enter_thread_mode(void)
     UNREACHABLE();
 }
 
-void thread_arch_yield(void) __attribute__((naked));
-void thread_arch_yield(void)
-{
-    __context_save();
+void thread_arch_yield(void) {
+    AVR_CONTEXT_SWAP_TRIGGER;
+}
 
-    /* irq_disable(); */ /* gets already disabled during __context_save() */
-    sched_run();
-    irq_enable();
 
+// Use this interrupt to perform all context switches
+ISR(AVR_CONTEXT_SWAP_INTERRUPT_VECT, ISR_NAKED) {
+    __context_save();
+    sched_run();
     __context_restore();
-    __asm__ volatile("ret");
+    __asm__ volatile("reti");
 }