ARMv7: start.S: stay in HYP mode if u-boot is entered in it

The KVM and Xen hypervisors for the Cortex-A15 virtualization
implementation need to be entered in HYP mode. Should the primary
board firmware already enter HYP mode (Calxeda firmware does that),
we should not deliberately drop back to SVC mode.
Since U-boot does not use the MMU, running in HYP mode is just fine.

Signed-off-by: Andre Przywara <andre.przywara@linaro.org>

diff --git a/arch/arm/cpu/armv7/start.S b/arch/arm/cpu/armv7/start.S
index 61da7acde910d37b67b45cdad80461cb3e85a2dc..5feae7ace3c1560806f34e0fc97369cefe4c03ea 100644 (file)
--- a/arch/arm/cpu/armv7/start.S
+++ b/arch/arm/cpu/armv7/start.S
@@ -130,11 +130,15 @@ IRQ_STACK_START_IN:
 reset:
        bl      save_boot_params
        /*
-        * set the cpu to SVC32 mode
+        * disable interrupts (FIQ and IRQ), also set the cpu to SVC32 mode,
+        * except if in HYP mode already
         */
        mrs     r0, cpsr
-       bic     r0, r0, #0x1f
-       orr     r0, r0, #0xd3
+       and     r1, r0, #0x1f           @ mask mode bits
+       teq     r1, #0x1a               @ test for HYP mode
+       bicne   r0, r0, #0x1f           @ clear all mode bits
+       orrne   r0, r0, #0x13           @ set SVC mode
+       orr     r0, r0, #0xc0           @ disable FIQ and IRQ
        msr     cpsr,r0
 
 /*