arm: interrupt_init: set sp in IRQ/FIQ modes

Before this commit, the stack addresses for IRQ and FIQ modes,
IRQ_STACK_START and FIQ_STACK_START, were computed in interrupt_init but
they were not used.

This commit sets the stack pointers for IRQ and FIQ modes.

Signed-off-by: Georges Savoundararadj <savoundg@gmail.com>
Acked-by: Albert ARIBAUD <albert.u.boot@aribaud.net>

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();
 }