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>

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
 
 /*