[PATCH v2 3/6] Set HVBAR register correctly

[Jim Posen]

This is the base address register for the vector table when in
hypervisor mode.

Signed-off-by: Jim Posen <jim.posen at gmail.com>
---

Changes in v2:
- Fix CP15 register written to in start.S
- Check processor mode as HVBAR register is only accessible in HYP
        mode

 arch/arm/cpu/armv7/start.S |  8 ++++++++
 arch/arm/lib/relocate.S    | 13 +++++++++++++
 2 files changed, 21 insertions(+)

diff --git a/arch/arm/cpu/armv7/start.S b/arch/arm/cpu/armv7/start.S
index 698e15b8e1..dc66f592d9 100644
--- a/arch/arm/cpu/armv7/start.S
+++ b/arch/arm/cpu/armv7/start.S
@@ -113,6 +113,14 @@ switch_to_hypervisor_ret:
 	ldr	r0, =_start
 	mcr	p15, 0, r0, c12, c0, 0	@Set VBAR
 #endif
+#ifdef CONFIG_CPU_V7_HAS_VIRT
+	/* Z flag is still set from HYP mode test */
+	bne skip_set_hvbar
+	/* Set hyp vector address in CP15 HVBAR register */
+	ldr	r0, =hyp_vector_table
+	mcr	p15, 4, r0, c12, c0, 0	@Set HVBAR
+skip_set_hvbar:
+#endif
 #endif
 
 	/* the mask ROM code should have PLL and others stable */
diff --git a/arch/arm/lib/relocate.S b/arch/arm/lib/relocate.S
index 14b7f61c1a..b8db36795d 100644
--- a/arch/arm/lib/relocate.S
+++ b/arch/arm/lib/relocate.S
@@ -60,6 +60,19 @@ ENTRY(relocate_vectors)
 	ldmia	r0!, {r2-r8,r10}
 	stmia	r1!, {r2-r8,r10}
 #endif
+#ifdef CONFIG_CPU_V7_HAS_VIRT
+	/*
+	* If the ARM processor has the virtualization extensions and running in HYP
+	* mode, use HVBAR to relocate the EL2 exception vectors.
+	*/
+	mrs	r1, cpsr
+	and	r1, r1, #0x1f		@ mask mode bits
+	teq	r1, #0x1a		@ test for HYP mode
+	bne	skip_relocate_hvbar
+	ldr	r0, =hyp_vector_table   /* load dynamically relocated address */
+	mcr	p15, 4, r0, c12, c0, 0  /* Set HVBAR */
+skip_relocate_hvbar:
+#endif
 #endif
 	bx	lr
 
-- 
2.25.1