[BOOT-WRAPPER v3 02/10] 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.

This change means that the boot-wrapper can only be used when enetered
at the highest implemented exception level, as accesses to CNTFRQ_EL0
will be UNDEFINED at lower exception levels. However, the boot-wrapper
only supports being booted at the highest implemented exception level,
as the comment at the top of boot.S describes:

| The boot-wrapper must be entered from the reset vector at the
| highest implemented exception level.

... so this should not adversely affect any supported configuration.

Signed-off-by: Mark Rutland <mark.rutland at arm.com>
Acked-by: Marc Zyngier <maz at kernel.org>
Reviewed-by: Andre Przywara <andre.przywara at arm.com>
Cc: Akos Denke <akos.denke at arm.com>
Cc: Luca Fancellu <luca.fancellu at arm.com>
---
 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 73ddcd0..52c617d 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