ARM: 7549/1: HYP: fix boot on some ARM1136 cores

It appears that performing a "movs pc, lr" to force the kernel into
SVC mode on the OMAP2420 (ARM1136) prevents the platform from booting
correctly (change introduced in 80c59daf [ARM: virt: allow the kernel
to be entered in HYP mode]).

While the reason it fails is not understood yet (the same code runs
fine on the OMAP2430, ARM1136 as well), partially revert that change
for platforms that do not enter in HYP mode, preserving the new
feature and restoring a working kernel on the OMAP2420.
Reported-by: NTony Lindgren <tony@atomide.com>
Acked-by: NNicolas Pitre <nico@linaro.org>
Tested-by: NTony Lindgren <tony@atomide.com>
Signed-off-by: NMarc Zyngier <marc.zyngier@arm.com>
Signed-off-by: NRussell King <rmk+kernel@arm.linux.org.uk>

arch/arm/include/asm/assembler.h

@@ -254,16 +254,17 @@
 	mov	lr , \reg
 	and	lr , lr , #MODE_MASK
 	cmp	lr , #HYP_MODE
-	orr	\reg , \reg , #PSR_A_BIT | PSR_I_BIT | PSR_F_BIT
+	orr	\reg , \reg , #PSR_I_BIT | PSR_F_BIT
 	bic	\reg , \reg , #MODE_MASK
 	orr	\reg , \reg , #SVC_MODE
 THUMB(	orr	\reg , \reg , #PSR_T_BIT	)
-	msr	spsr_cxsf, \reg
-	adr	lr, BSYM(2f)
 	bne	1f
+	orr	\reg, \reg, #PSR_A_BIT
+	adr	lr, BSYM(2f)
+	msr	spsr_cxsf, \reg
 	__MSR_ELR_HYP(14)
 	__ERET
-1:	movs	pc, lr
+1:	msr	cpsr_c, \reg
 2:
 .endm