From 534a331bf225e1094b397447f17ffb4001bb1306 Mon Sep 17 00:00:00 2001 From: Jun Nie Date: Tue, 7 Aug 2018 12:20:49 +0800 Subject: 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 --- framework/aarch32/arch.c | 6 ++-- framework/aarch32/entrypoint.S | 40 ++++++++++++++++++++++ framework/main.c | 5 ++- include/lib/aarch32/arch.h | 2 ++ .../suspend/aarch32/asm_tftf_suspend.S | 39 ++++++++++++++++++++- 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 -- cgit v1.2.3