diff --git a/ports/arm7a/Makefile b/ports/arm7a/Makefile
index 816ebe8..ade2dce 100644
--- a/ports/arm7a/Makefile
+++ b/ports/arm7a/Makefile
@@ -75,21 +75,18 @@ TEST_OBJECTS = $(notdir $(patsubst %.c,%.o,$(wildcard $(tests_dir)/*.c)))
 TEST_ELFS = $(patsubst %.o,%.elf,$(TEST_OBJECTS))
 
 # Search build/output directory for dependencies
-vpath %.o ./$(build_dir)
-vpath %.elf ./$(build_dir)
+#vpath %.o ./$(build_dir)
+#vpath %.elf ./$(build_dir)
 
 # GCC flags
 CFLAGS= -g \
 	-Wall \
 	-Werror \
-	-O \
 	-mcpu=$(CPU) \
-	-fstrength-reduce \
-	-fomit-frame-pointer \
-	-finline-functions \
 	-nostdinc \
-	-fno-builtin \
-	-fno-stack-protector
+	-nostdlib \
+	-nodefaultlibs \
+	-fno-builtin
 
 # Enable stack-checking (disable if not required)
 ifeq ($(STACK_CHECK),true)
@@ -107,7 +104,7 @@ all: $(TEST_ELFS) Makefile
 $(TEST_ELFS): %.elf: %.o $(APP_ASM_OBJECTS) $(KERNEL_OBJECTS) $(APP_OBJECTS)
 	$(V)mkdir -p `dirname $(build_dir)/$@`
 	$(if $(V), @echo " (ELF)        $(subst $(build_dir)/,,$@)")
-	$(V)$(CC) $(CFLAGS) -nostdlib -nodefaultlibs $(build_dir)/$(notdir $<) $(BUILT_OBJECTS) -static-libgcc -lgcc --output $(build_dir)/$@ -Wl -T linker.ld
+	$(V)$(CC) $(CFLAGS) $(build_dir)/$(notdir $<) $(BUILT_OBJECTS) -static-libgcc -lgcc --output $(build_dir)/$@ -Wl -T linker.ld
 
 # Kernel objects builder
 $(KERNEL_OBJECTS): %.o: $(kernel_dir)/%.c
diff --git a/ports/arm7a/arm_main.c b/ports/arm7a/arm_main.c
index 4e60eba..d8ace0e 100644
--- a/ports/arm7a/arm_main.c
+++ b/ports/arm7a/arm_main.c
@@ -131,9 +131,6 @@ static uint8_t idle_thread_stack[IDLE_STACK_SIZE_BYTES];
 /* Forward declarations */
 static void main_thread_func (uint32_t data);
 
-/* Global Data */
-uint32_t at_preempt_count;
-
 /**
  * \b main
  *
@@ -147,8 +144,6 @@ int main ( void )
 {
     int8_t status;
 
-    at_preempt_count = 0;
-
     /**
      * Note: to protect OS structures and data during initialisation,
      * interrupts must remain disabled until the first thread
diff --git a/ports/arm7a/atomport-asm.s b/ports/arm7a/atomport-asm.s
index ee6c1bd..fa09386 100644
--- a/ports/arm7a/atomport-asm.s
+++ b/ports/arm7a/atomport-asm.s
@@ -29,29 +29,46 @@
 
 #include <arm_asm_macro.h>
 
-.section .text
+	.section .text
+
+/**
+ * uint32_t archGetCPSR(void)
+ */
+	.globl archGetCPSR
+archGetCPSR:
+	mrs	r0, cpsr_all
+	bx	lr
 
 /**
  * int archSetJumpLowLevel(pt_regs_t *regs)
  */
-.globl archSetJumpLowLevel
+	.globl archSetJumpLowLevel
 archSetJumpLowLevel:
+	add	r0, r0, #(4 * 16)
+	str	lr, [r0]
+	sub	r0, r0, #(4 * 14)
+	stm	r0, {r1-r14}
+	mov	r0, r0 /* NOP */
+	sub	sp, sp, #4
+	str	r1, [sp]
+	mov	r1, #0
+	sub	r0, r0, #4
+	str	r1, [r0]
+	mrs	r1, cpsr_all
+	add	r0, r0, #4
+	str	r1, [r0]
+	ldr	r1, [sp]
+	sub	sp, sp, #4
+	mov	r0, #1
 	bx	lr
 
 /**
  * void archLongJumpLowLevel(pt_regs_t *regs)
  */
-.globl archLongJumpLowLevel
+	.globl archLongJumpLowLevel
 archLongJumpLowLevel:
-	bx	lr
-
-/**
- * void archFirstThreadRestoreLowLevel(pt_regs_t *regs)
- */
-.globl archFirstThreadRestoreLowLevel
-archFirstThreadRestoreLowLevel:
 	add	r0, r0, #(4 * 17)
-	mrs	r1, cpsr
+	mrs	r1, cpsr_all
 	SET_CURRENT_MODE CPSR_MODE_UNDEFINED
 	mov	sp, r0
 	SET_CURRENT_MODE CPSR_MODE_ABORT
@@ -60,12 +77,10 @@ archFirstThreadRestoreLowLevel:
 	mov	sp, r0
 	SET_CURRENT_MODE CPSR_MODE_FIQ
 	mov	sp, r0
-	msr	cpsr, r1
-	mov	sp, r0
-	sub	sp, sp, #(4 * 17)
-	ldr     r0, [sp], #0x0004;      /* Get CPSR from stack */
-	msr     spsr_all, r0;
-	ldmia   sp, {r0-r14};          /* Restore registers */
-	mov     r0, r0;                 /* NOP for previous isnt */
-	movs	pc, lr
+	msr	cpsr_all, r1
+	sub	r0, r0, #(4 * 17)
+	ldr     r1, [r0], #4 /* Get CPSR from stack */
+	msr	cpsr_all, r1
+	ldm	r0, {r0-r15}
+	mov	r0, r0 /* NOP */
 
diff --git a/ports/arm7a/atomport.c b/ports/arm7a/atomport.c
index 67496ce..96f2cc4 100644
--- a/ports/arm7a/atomport.c
+++ b/ports/arm7a/atomport.c
@@ -68,7 +68,9 @@ void archThreadContextInit (ATOM_TCB *tcb_ptr, void *stack_top,
 	regs->pc = (uint32_t)entry_point;
 }
 
-extern void archFirstThreadRestoreLowLevel(pt_regs_t *regs);
+extern int archSetJumpLowLevel(pt_regs_t *regs);
+extern void archLongJumpLowLevel(pt_regs_t *regs);
+extern uint32_t archGetCPSR(void);
 
 /**
  * archFirstThreadRestore(ATOM_TCB *new_tcb)
@@ -87,12 +89,9 @@ void archFirstThreadRestore(ATOM_TCB *new_tcb)
 	pt_regs_t *regs = NULL;
 	regs = (pt_regs_t *)((uint32_t)new_tcb->sp_save_ptr 
 							- sizeof(pt_regs_t));
-	archFirstThreadRestoreLowLevel(regs);
+	archLongJumpLowLevel(regs);
 }
 
-extern int archSetJumpLowLevel(pt_regs_t *regs);
-extern void archLongJumpLowLevel(pt_regs_t *regs);
-
 /**
  * Function that performs the contextSwitch. Whether its a voluntary release
  * of CPU by thread or a pre-emption, under both conditions this function is
@@ -102,14 +101,28 @@ extern void archLongJumpLowLevel(pt_regs_t *regs);
  */
 void archContextSwitch(ATOM_TCB *old_tcb, ATOM_TCB *new_tcb)
 {
+	uint32_t mode;
+	pt_regs_t tmp;
 	pt_regs_t *old_regs = NULL;
 	pt_regs_t *new_regs = NULL;
 	old_regs = (pt_regs_t *)((uint32_t)old_tcb->sp_save_ptr 
 							- sizeof(pt_regs_t));
 	new_regs = (pt_regs_t *)((uint32_t)new_tcb->sp_save_ptr 
 							- sizeof(pt_regs_t));
-	if (archSetJumpLowLevel(old_regs)) {
-		archLongJumpLowLevel(new_regs);
+	mode = archGetCPSR() & CPSR_MODE_MASK;
+	if ((mode == CPSR_MODE_IRQ) || (mode == CPSR_MODE_FIQ)) {
+		/* Interrupt Context */
+		memcpy(&tmp, old_regs, sizeof(pt_regs_t));
+		if (archSetJumpLowLevel(old_regs)) {
+			archLongJumpLowLevel(new_regs);
+		} else {
+			memcpy(old_regs, &tmp, sizeof(pt_regs_t));
+		}
+	} else {
+		/* Thread Context */
+		if (archSetJumpLowLevel(old_regs)) {
+			archLongJumpLowLevel(new_regs);
+		}
 	}
 }