Merge branch 'master' of git://git.denx.de/u-boot-arm

[karo-tx-uboot.git] / arch / arm / lib / interrupts.c

diff --git a/arch/arm/lib/interrupts.c b/arch/arm/lib/interrupts.c
index 9019736d2c74bec4da3fa4f97752954666a91ec1..4dacfd941f6dd220cfa2f5d2a7ae006b10d628a2 100644 (file)
--- a/arch/arm/lib/interrupts.c
+++ b/arch/arm/lib/interrupts.c
@@ -28,6 +28,8 @@ DECLARE_GLOBAL_DATA_PTR;
 #ifdef CONFIG_USE_IRQ
 int interrupt_init (void)
 {
+       unsigned long cpsr;
+
        /*
         * setup up stacks if necessary
         */
@@ -35,6 +37,31 @@ int interrupt_init (void)
        IRQ_STACK_START_IN = gd->irq_sp + 8;
        FIQ_STACK_START = IRQ_STACK_START - CONFIG_STACKSIZE_IRQ;
 
+
+       __asm__ __volatile__("mrs %0, cpsr\n"
+                            : "=r" (cpsr)
+                            :
+                            : "memory");
+
+       __asm__ __volatile__("msr cpsr_c, %0\n"
+                            "mov sp, %1\n"
+                            :
+                            : "r" (IRQ_MODE | I_BIT | F_BIT | (cpsr & ~FIQ_MODE)),
+                              "r" (IRQ_STACK_START)
+                            : "memory");
+
+       __asm__ __volatile__("msr cpsr_c, %0\n"
+                            "mov sp, %1\n"
+                            :
+                            : "r" (FIQ_MODE | I_BIT | F_BIT | (cpsr & ~IRQ_MODE)),
+                              "r" (FIQ_STACK_START)
+                            : "memory");
+
+       __asm__ __volatile__("msr cpsr_c, %0"
+                            :
+                            : "r" (cpsr)
+                            : "memory");
+
        return arch_interrupt_init();
 }