[BOOT-WRAPPER 03/11] aarch64: Implement cpu_init_arch()

[Mark Rutland]

When the boot-wrapper is entered at EL2 it does not initialise
CNTFRQ_EL0, and in future it may need to initialize other CPU state
regardless of the exeption level it was entered at.

Use a common cpu_init_arch() function to initialize CPU state regardless
of the exception level the boot-wrapper was entered at.

Signed-off-by: Mark Rutland <mark.rutland at arm.com>
Cc: Akos Denke <akos.denke at arm.com>
Cc: Andre Przywara <andre.przywara at arm.com>
Cc: Luca Fancellu <luca.fancellu at arm.com>
Cc: Marc Zyngier <maz at kernel.org>
---
 arch/aarch64/boot.S |  4 +++-
 arch/aarch64/init.c | 12 +++++++++---
 2 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
index 51ef41b..d8d38dd 100644
--- a/arch/aarch64/boot.S
+++ b/arch/aarch64/boot.S
@@ -51,7 +51,7 @@ reset_at_el3:
 
 	bl	cpu_init_bootwrapper
 
-	bl	cpu_init_el3
+	bl	cpu_init_arch
 
 	bl	gic_secure_init
 
@@ -82,6 +82,8 @@ reset_at_el2:
 
 	bl	cpu_init_bootwrapper
 
+	bl	cpu_init_arch
+
 	b	start_bootmethod
 
 err_invalid_id:
diff --git a/arch/aarch64/init.c b/arch/aarch64/init.c
index c9fc7f1..49abdf7 100644
--- a/arch/aarch64/init.c
+++ b/arch/aarch64/init.c
@@ -52,7 +52,7 @@ static inline bool cpu_has_permission_indirection(void)
 	return mrs(ID_AA64MMFR3_EL1) & mask;
 }
 
-void cpu_init_el3(void)
+static void cpu_init_el3(void)
 {
 	unsigned long scr = SCR_EL3_RES1 | SCR_EL3_NS | SCR_EL3_HCE;
 	unsigned long mdcr = 0;
@@ -153,8 +153,6 @@ void cpu_init_el3(void)
 
 		msr(SMCR_EL3, smcr);
 	}
-
-	msr(CNTFRQ_EL0, COUNTER_FREQ);
 }
 
 #ifdef PSCI
@@ -171,3 +169,11 @@ bool cpu_init_psci_arch(void)
 	return true;
 }
 #endif
+
+void cpu_init_arch(void)
+{
+	if (mrs(CurrentEL) == CURRENTEL_EL3)
+		cpu_init_el3();
+
+	msr(CNTFRQ_EL0, COUNTER_FREQ);
+}
-- 
2.30.2