From c4a4e2e20ca226948b62ed116df98f7a3932f2ac Mon Sep 17 00:00:00 2001
From: Andre Przywara <andre.przywara@linaro.org>
Date: Tue, 2 Apr 2013 05:43:36 +0000
Subject: [PATCH] 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 | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/arch/arm/cpu/armv7/start.S b/arch/arm/cpu/armv7/start.S
index 61da7acde9..5feae7ace3 100644
--- 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
 
 /*
-- 
2.39.5