Aarch32: Add support for non-hyp case

some ARMv7 SoC does not support hypervisor mode. Add branch to
handle non-hypervisor version SoC instead of trigger panic.

Signed-off-by: Jun Nie <jun.nie@linaro.org>

5 files changed, 86 insertions, 6 deletions

diff --git a/framework/aarch32/arch.c b/framework/aarch32/arch.c
index 88b8101..d22e939 100644
--- a/framework/aarch32/arch.c
+++ b/framework/aarch32/arch.c
@@ -35,8 +35,6 @@
 
 void tftf_arch_setup(void)
 {
-	if (!IS_IN_HYP())
-		panic();
-
-	write_hcr(HCR_TGE_BIT);
+	if (IS_IN_HYP())
+		write_hcr(HCR_TGE_BIT);
 }
diff --git a/framework/aarch32/entrypoint.S b/framework/aarch32/entrypoint.S
index dbb88ba..746a12a 100644
--- a/framework/aarch32/entrypoint.S
+++ b/framework/aarch32/entrypoint.S
@@ -35,6 +35,7 @@
 	.globl	tftf_entrypoint
 	.globl	tftf_hotplug_entry
 
+
 /* ----------------------------------------------------------------------------
  * Cold boot entry point for the primary CPU.
  * ----------------------------------------------------------------------------
@@ -45,6 +46,13 @@ func tftf_entrypoint
 	 * --------------------------------------------------------------------
 	 */
 	ldr	r0, =tftf_vector
+
+	/* check hypervisor mode support status */
+	mrs	r2, cpsr
+	and	r1, r2, #0x1f		@ mask mode bits
+	teq	r1, #0x1a		@ test for HYP mode
+	bne	non_hyp_setup
+
 	stcopr	r0, HVBAR
 
 	/* --------------------------------------------------------------------
@@ -55,6 +63,21 @@ func tftf_entrypoint
 	ldr	r1, =(HSCTLR_I_BIT | HSCTLR_A_BIT)
 	orr	r0, r0, r1
 	stcopr	r0, HSCTLR
+	b	entrypoint_exit
+
+non_hyp_setup:
+	/* Set V=0 in CP15 SCTLR register - for VBAR to point to vector */
+	mrc	p15, 0, r1, c1, c0, 0	@ Read CP15 SCTLR Register
+	bic	r1, #0x2000		@ V = 0
+	mcr	p15, 0, r1, c1, c0, 0	@ Write CP15 SCTLR Register
+	stcopr	r0, VBAR
+
+	ldcopr	r0, SCTLR
+	ldr	r1, =(HSCTLR_I_BIT | HSCTLR_A_BIT)
+	orr	r0, r0, r1
+	stcopr	r0, SCTLR
+
+entrypoint_exit:
 	isb
 
 	/* --------------------------------------------------------------------
@@ -124,6 +147,13 @@ func tftf_hotplug_entry
 	 * --------------------------------------------------------------------
 	 */
 	ldr	r0, =tftf_vector
+
+	/* check hypervisor mode support status */
+	mrs	r2, cpsr
+	and	r1, r2, #0x1f		@ mask mode bits
+	teq	r1, #0x1a		@ test for HYP mode
+	bne	non_hyp_hotplug
+
 	stcopr	r0, HVBAR
 
 	/* --------------------------------------------------------------------
@@ -134,6 +164,16 @@ func tftf_hotplug_entry
 	ldr	r1, =(HSCTLR_I_BIT | HSCTLR_A_BIT)
 	orr	r0, r0, r1
 	stcopr	r0, HSCTLR
+	b	non_hyp_hotplug_done
+
+non_hyp_hotplug:
+	stcopr	r0, VBAR
+	ldcopr	r0, SCTLR
+	ldr	r1, =(HSCTLR_I_BIT | HSCTLR_A_BIT)
+	orr	r0, r0, r1
+	stcopr	r0, SCTLR
+
+non_hyp_hotplug_done:
 	isb
 
 	/* --------------------------------------------------------------------
diff --git a/framework/main.c b/framework/main.c
index ea69192..0084400 100644
--- a/framework/main.c
+++ b/framework/main.c
@@ -525,7 +525,10 @@ void __dead2 tftf_cold_boot_main(void)
 #ifndef AARCH32
 	NOTICE("Running at NS-EL%u\n", IS_IN_EL(1) ? 1 : 2);
 #else
-	NOTICE("Running in AArch32 HYP mode\n");
+	if (IS_IN_HYP())
+		NOTICE("Running in AArch32 HYP mode\n");
+	else
+		NOTICE("Running in AArch32 non-HYP mode\n");
 #endif
 
 	tftf_arch_setup();
diff --git a/include/lib/aarch32/arch.h b/include/lib/aarch32/arch.h
index 3b36f1b..dc28195 100644
--- a/include/lib/aarch32/arch.h
+++ b/include/lib/aarch32/arch.h
@@ -31,6 +31,7 @@
 #ifndef __ARCH_H__
 #define __ARCH_H__
 
+
 /*******************************************************************************
  * MIDR bit definitions
  ******************************************************************************/
@@ -427,6 +428,7 @@
 #define TLBIMVA		p15, 0, c8, c7, 1
 #define TLBIMVAA	p15, 0, c8, c7, 3
 #define HSCTLR		p15, 4, c1, c0, 0
+#define SCTLR		p15, 0, c1, c0, 0
 #define HCR		p15, 4, c1, c1, 0
 #define HCPTR		p15, 4, c1, c1, 2
 #define CNTHCTL		p15, 4, c14, c1, 0
diff --git a/lib/power_management/suspend/aarch32/asm_tftf_suspend.S b/lib/power_management/suspend/aarch32/asm_tftf_suspend.S
index 5405bfa..3daa0cd 100644
--- a/lib/power_management/suspend/aarch32/asm_tftf_suspend.S
+++ b/lib/power_management/suspend/aarch32/asm_tftf_suspend.S
@@ -68,6 +68,12 @@ func __tftf_suspend
 endfunc __tftf_suspend
 
 func __tftf_save_arch_context
+	/* check hypervisor mode support status */
+	mrs	r2, cpsr
+	and	r1, r2, #0x1f		@ mask mode bits
+	teq	r1, #0x1a		@ test for HYP mode
+	bne	non_hyp_save_ctx
+
 	ldcopr	r1, HMAIR0
 	ldcopr	r2, HCR
 	stm	r0!, {r1, r2}
@@ -76,6 +82,15 @@ func __tftf_save_arch_context
 	ldcopr	r1, HTCR
 	ldcopr	r2, HVBAR
 	ldcopr	r3, HSCTLR
+	b	save_ctx_exit
+
+non_hyp_save_ctx:
+	ldcopr	r1, MAIR0
+	stm	r0!, {r1, r2}
+	ldcopr	r2, VBAR
+	ldcopr	r3, SCTLR
+
+save_ctx_exit:
 	stm	r0, {r1, r2, r3}
 	bx	lr
 endfunc __tftf_save_arch_context
@@ -85,6 +100,12 @@ endfunc __tftf_save_arch_context
  * r0  -- Should contain the context pointer
  */
 func __tftf_cpu_resume_ep
+	/* check hypervisor mode support status */
+	mrs	r2, cpsr
+	and	r1, r2, #0x1f		@ mask mode bits
+	teq	r1, #0x1a		@ test for HYP mode
+	bne	non_hyp_resume_ep
+
 	/* Invalidate local tlb entries before turning on MMU */
 	stcopr	r0, TLBIALLH
 	mov 	r4, r0
@@ -96,12 +117,28 @@ func __tftf_cpu_resume_ep
 	ldm	r0, {r1, r2, r3}
 	stcopr	r1, HTCR
 	stcopr	r2, HVBAR
-
 	/*
 	 * TLB invalidations need to be completed before enabling MMU
 	 */
 	dsb	nsh
 	stcopr	r3, HSCTLR
+	b	resume_ep_exit
+
+non_hyp_resume_ep:
+	/* Invalidate local tlb entries before turning on MMU */
+	stcopr	r0, TLBIALL
+	mov 	r4, r0
+	ldm	r0!, {r1, r2}
+	stcopr	r1, MAIR0
+	ldm	r0, {r1, r2, r3}
+	stcopr	r2, VBAR
+	/*
+	 * TLB invalidations need to be completed before enabling MMU
+	 */
+	dsb	nsh
+	stcopr	r3, SCTLR
+
+resume_ep_exit:
 	/* Ensure the MMU enable takes effect immediately */
 	isb