Rework common init C code

In init_platform() we initialize a UART and announce the presence of the
bootwrapper to the world. We do this relatively late in the boot-flow,
and prior to this will silently ignore errors (e.g. in gic_secure_init).

To make it possible to provide improved diagnostics, and to allow us to
move more initialization into C, this patch reworks the init code to
call a C function earlier, where we can announce the presence of the
boot-wrapper and perform other initialization.

In subsequent patches this will be expanded with more CPU
initialization.

Signed-off-by: Mark Rutland <mark.rutland@arm.com>
Reviewed-by: Andre Przywara <andre.przywara@arm.com>
diff --git a/Makefile.am b/Makefile.am
index 5731a19..91a6a02 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -34,7 +34,7 @@
 PSCI_CPU_OFF	:= 0x84000002
 
 COMMON_SRC	:= common/
-COMMON_OBJ	:= boot.o bakery_lock.o platform.o lib.o
+COMMON_OBJ	:= boot.o bakery_lock.o platform.o lib.o init.o
 
 ARCH_OBJ	:= boot.o stack.o utils.o
 
diff --git a/arch/aarch32/boot.S b/arch/aarch32/boot.S
index 00c432d..ee073ea 100644
--- a/arch/aarch32/boot.S
+++ b/arch/aarch32/boot.S
@@ -48,6 +48,9 @@
 	mov	r0, #1
 	ldr	r1, =flag_no_el3
 	str	r0, [r1]
+
+	bl	cpu_init_bootwrapper
+
 	b	start_no_el3
 
 _switch_monitor:
@@ -71,6 +74,8 @@
 	ldr	r0, =COUNTER_FREQ
 	mcr	p15, 0, r0, c14, c0, 0		@ CNTFRQ
 
+	bl	cpu_init_bootwrapper
+
 	bl	gic_secure_init
 
 	/* Initialise boot method */
diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
index 204c199..7872daf 100644
--- a/arch/aarch64/boot.S
+++ b/arch/aarch64/boot.S
@@ -149,6 +149,8 @@
 	b.eq	err_invalid_id
 	bl	setup_stack
 
+	bl	cpu_init_bootwrapper
+
 	bl	gic_secure_init
 
 	b	start_el3
@@ -189,6 +191,8 @@
 	ldr	x1, =flag_no_el3
 	str	w0, [x1]
 
+	bl	cpu_init_bootwrapper
+
 	b	start_no_el3
 
 err_invalid_id:
diff --git a/common/boot.c b/common/boot.c
index c74d34c..29d53a4 100644
--- a/common/boot.c
+++ b/common/boot.c
@@ -12,8 +12,6 @@
 extern unsigned long entrypoint;
 extern unsigned long dtb;
 
-void init_platform(void);
-
 void __noreturn jump_kernel(unsigned long address,
 			    unsigned long a0,
 			    unsigned long a1,
@@ -62,8 +60,6 @@
 			   unsigned long invalid)
 {
 	if (cpu == 0) {
-		init_platform();
-
 		*mbox = (unsigned long)&entrypoint;
 		sevl();
 		spin(mbox, invalid, 1);
diff --git a/common/init.c b/common/init.c
new file mode 100644
index 0000000..9c471c9
--- /dev/null
+++ b/common/init.c
@@ -0,0 +1,24 @@
+/*
+ * init.c - common boot-wrapper initialization
+ *
+ * Copyright (C) 2021 ARM Limited. All rights reserved.
+ *
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE.txt file.
+ */
+#include <cpu.h>
+#include <platform.h>
+
+static void announce_bootwrapper(void)
+{
+	print_string("Boot-wrapper v0.2\r\n\r\n");
+}
+
+void cpu_init_bootwrapper(void)
+{
+	if (this_cpu_logical_id() == 0) {
+		init_uart();
+		announce_bootwrapper();
+		init_platform();
+	}
+}
diff --git a/common/platform.c b/common/platform.c
index d11f568..47bf547 100644
--- a/common/platform.c
+++ b/common/platform.c
@@ -1,5 +1,5 @@
 /*
- * platform.c - code to initialise everything required when first booting.
+ * platform.c - Platform initialization and I/O.
  *
  * Copyright (C) 2015 ARM Limited. All rights reserved.
  *
@@ -7,6 +7,7 @@
  * found in the LICENSE.txt file.
  */
 
+#include <cpu.h>
 #include <stdint.h>
 
 #include <asm/io.h>
@@ -30,7 +31,7 @@
 #define V2M_SYS(reg)	((void *)SYSREGS_BASE + V2M_SYS_##reg)
 #endif
 
-static void print_string(const char *str)
+void print_string(const char *str)
 {
 	uint32_t flags;
 
@@ -47,7 +48,7 @@
 	}
 }
 
-void init_platform(void)
+void init_uart(void)
 {
 	/*
 	 * UART initialisation (38400 8N1)
@@ -58,9 +59,10 @@
 	raw_writel(0x70,	PL011(UART_LCR_H));
 	/* Enable the UART, TXen and RXen */
 	raw_writel(0x301,	PL011(UARTCR));
+}
 
-	print_string("Boot-wrapper v0.2\r\n\r\n");
-
+void init_platform(void)
+{
 #ifdef SYSREGS_BASE
 	/*
 	 * CLCD output site MB
diff --git a/include/platform.h b/include/platform.h
new file mode 100644
index 0000000..e5248e1
--- /dev/null
+++ b/include/platform.h
@@ -0,0 +1,17 @@
+/*
+ * include/platform.h - Platform initialization and I/O.
+ *
+ * Copyright (C) 2021 ARM Limited. All rights reserved.
+ *
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE.txt file.
+ */
+#ifndef __PLATFORM_H
+#define __PLATFORM_H
+
+void print_string(const char *str);
+void init_uart(void);
+
+void init_platform(void);
+
+#endif /* __PLATFORM_H */