diff --git a/Demo/Common/drivers/Atmel/at91lib/boards/at91sam9xe-ek/board_cstartup_iar.s b/Demo/Common/drivers/Atmel/at91lib/boards/at91sam9xe-ek/board_cstartup_iar.s
index fabc1d194..5e4e14704 100644
--- a/Demo/Common/drivers/Atmel/at91lib/boards/at91sam9xe-ek/board_cstartup_iar.s
+++ b/Demo/Common/drivers/Atmel/at91lib/boards/at91sam9xe-ek/board_cstartup_iar.s
@@ -113,13 +113,9 @@ irqHandler:
         LDR     r0, [r14, #AIC_IVR]
         STR     lr, [r14, #AIC_IVR]
 
-        /* Branch to interrupt handler in Supervisor mode */
-        MSR     CPSR_c, #ARM_MODE_SVC
-        STMFD   sp!, {r1-r3, r12, lr}
+        /* Branch to C portion of the interrupt handler */
         MOV     lr, pc
         BX      r0
-        LDMIA   sp!, {r1-r3, r12, lr}
-        MSR     CPSR_c, #ARM_MODE_IRQ | I_BIT
 
         /* Acknowledge interrupt */
         LDR     lr, =AT91C_BASE_AIC