diff options
| author | Jun Nie <jun.nie@linaro.org> | 2018-08-07 12:23:18 +0800 |
|---|---|---|
| committer | Jun Nie <jun.nie@linaro.org> | 2018-08-07 12:33:57 +0800 |
| commit | d791077531b4f3d4ab802278266d5311da7a8d43 (patch) | |
| tree | b99a9e05acd534e0eebe01b7fe15b3a26643a31f | |
| parent | d5a4e05a2295f56c581f9d419c65d3579f281759 (diff) | |
Aarch32: handle non-hyp mode in entry point
Add vector base address handling in non-hyp mode in entry point
Signed-off-by: Jun Nie <jun.nie@linaro.org>
| -rw-r--r-- | framework/aarch32/entrypoint.S | 40 | ||||
| -rw-r--r-- | include/lib/aarch32/arch.h | 2 |
2 files changed, 42 insertions, 0 deletions
diff --git a/framework/aarch32/entrypoint.S b/framework/aarch32/entrypoint.S index dbb88ba..31e58f4 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 r0, #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/include/lib/aarch32/arch.h b/include/lib/aarch32/arch.h index d292537..e8bde12 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 ******************************************************************************/ @@ -420,6 +421,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 |