[BOOT-WRAPPER 06/11] aarch32: Implement cpu_init_arch()

[Mark Rutland]

When the boot-wrapper is entered at EL2/Hyp it does not initialise
CNTFRQ, 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. For clarity
cpu_init_secure_pl1() is renamed to cpu_init_monitor(), which better
matches PSR_MON and will allow for the addition of cppu_init_hyp() and
cpu_init_svc() in future.

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/aarch32/boot.S |  4 +++-
 arch/aarch32/init.c | 12 +++++++++---
 2 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/arch/aarch32/boot.S b/arch/aarch32/boot.S
index 43dce75..a6f0751 100644
--- a/arch/aarch32/boot.S
+++ b/arch/aarch32/boot.S
@@ -62,7 +62,7 @@ reset_at_mon:
 
 	bl	cpu_init_bootwrapper
 
-	bl	cpu_init_secure_pl1
+	bl	cpu_init_arch
 
 	bl	gic_secure_init
 
@@ -82,6 +82,8 @@ reset_at_hyp:
 
 	bl	cpu_init_bootwrapper
 
+	bl	cpu_init_arch
+
 	b	start_bootmethod
 
 err_invalid_id:
diff --git a/arch/aarch32/init.c b/arch/aarch32/init.c
index e25f0c7..35da37c 100644
--- a/arch/aarch32/init.c
+++ b/arch/aarch32/init.c
@@ -29,7 +29,7 @@ void announce_arch(void)
 	print_string("\r\n");
 }
 
-void cpu_init_secure_pl1(void)
+static void cpu_init_monitor(void)
 {
 	unsigned long scr = SCR_NS | SCR_HCE;
 	unsigned long nsacr = NSACR_CP10 | NSACR_CP11;
@@ -37,8 +37,6 @@ void cpu_init_secure_pl1(void)
 	mcr(SCR, scr);
 
 	mcr(NSACR, nsacr);
-
-	mcr(CNTFRQ, COUNTER_FREQ);
 }
 
 #ifdef PSCI
@@ -55,3 +53,11 @@ bool cpu_init_psci_arch(void)
 	return true;
 }
 #endif
+
+void cpu_init_arch(void)
+{
+	if (read_cpsr_mode() == PSR_MON)
+		cpu_init_monitor();
+
+	mcr(CNTFRQ, COUNTER_FREQ);
+}
-- 
2.30.2