aarch32: move the bulk of Secure PL1 initialization to C

The majority of state that we initialize at Secure PL1 is necessary for
code at lower PLs to function, but isnt' necessary for the boot-wrapper
itself.  Given that, it would be better to write this in C where it can
be written mode clearly, and where it will be possible to add
logging/debug logic.

This patch migrates the AArch32 Secure PL1 initialization to C.

There should be no functional change as a result of this patch.

Signed-off-by: Mark Rutland <mark.rutland@arm.com>
Reviewed-by: Andre Przywara <andre.przywara@arm.com>
diff --git a/arch/aarch32/boot.S b/arch/aarch32/boot.S
index ee073ea..820957b 100644
--- a/arch/aarch32/boot.S
+++ b/arch/aarch32/boot.S
@@ -63,16 +63,7 @@
 	/* Move the stack to Monitor mode*/
 	mrs	sp, sp_svc
 
-	/* Setup secure registers and devices */
-	mov	r0, #1				@ Non-secure lower level
-	orr	r0, #(1 << 8)			@ HVC enable
-	mcr	p15, 0, r0, c1, c1, 0		@ SCR
-
-	mov	r0, #(1 << 10 | 1 << 11)	@ Enable NS access to CPACR
-	mcr	p15, 0, r0, c1, c1, 2		@ NSACR
-
-	ldr	r0, =COUNTER_FREQ
-	mcr	p15, 0, r0, c14, c0, 0		@ CNTFRQ
+	bl	cpu_init_secure_pl1
 
 	bl	cpu_init_bootwrapper
 
diff --git a/arch/aarch32/include/asm/cpu.h b/arch/aarch32/include/asm/cpu.h
index aa72204..c1bce9a 100644
--- a/arch/aarch32/include/asm/cpu.h
+++ b/arch/aarch32/include/asm/cpu.h
@@ -30,6 +30,11 @@
 #define PSR_I			(1 << 7)
 #define PSR_A			(1 << 8)
 
+#define SCR_NS			BIT(0)
+#define SCR_HCE			BIT(8)
+
+#define NSACR_CP10		BIT(10)
+#define NSACR_CP11		BIT(11)
 
 #define SPSR_KERNEL		(PSR_A | PSR_I | PSR_F | PSR_HYP)
 
@@ -55,11 +60,15 @@
 
 #define MPIDR		"p15, 0, %0, c0, c0, 5"
 #define ID_PFR1		"p15, 0, %0, c0, c1, 1"
+#define SCR		"p15, 0, %0, c1, c1, 0"
+#define NSACR		"p15, 0, %0, c1, c1, 2"
 #define ICIALLU		"p15, 0, %0, c7, c5, 0"
 
 #define ICC_SRE		"p15, 6, %0, c12, c12, 5"
 #define ICC_CTLR	"p15, 6, %0, c12, c12, 4"
 
+#define CNTFRQ		"p15, 0, %0, c14, c0, 0"
+
 #define mrc(reg)						\
 ({								\
 	unsigned long __mrc_val;				\
diff --git a/arch/aarch32/init.c b/arch/aarch32/init.c
index 785a428..7143c66 100644
--- a/arch/aarch32/init.c
+++ b/arch/aarch32/init.c
@@ -27,3 +27,15 @@
 	print_string(mode_string());
 	print_string("\r\n");
 }
+
+void cpu_init_secure_pl1(void)
+{
+	unsigned long scr = SCR_NS | SCR_HCE;
+	unsigned long nsacr = NSACR_CP10 | NSACR_CP11;
+
+	mcr(SCR, scr);
+
+	mcr(NSACR, nsacr);
+
+	mcr(CNTFRQ, COUNTER_FREQ);
+}