Announce boot-wrapper mode / exception level

When something goes wrong within the boot-wrapper, it can be very
helpful to know where we started from. Add an arch_announce() function
to log this early in the boot process. More information can be added
here in future.

This is logged ot the serial console as:

| Boot-wrapper v0.2
| Entered at EL3

Signed-off-by: Mark Rutland <mark.rutland@arm.com>
Reviewed-by: Andre Przywara <andre.przywara@arm.com>

7 files changed, 78 insertions, 12 deletions

diff --git a/Makefile.am b/Makefile.am
index 91a6a02..5a0ebd2 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -36,7 +36,7 @@ PSCI_CPU_OFF	:= 0x84000002
 COMMON_SRC	:= common/
 COMMON_OBJ	:= boot.o bakery_lock.o platform.o lib.o init.o
 
-ARCH_OBJ	:= boot.o stack.o utils.o
+ARCH_OBJ	:= boot.o stack.o utils.o init.o
 
 if BOOTWRAPPER_32
 CPPFLAGS	+= -DBOOTWRAPPER_32
diff --git a/arch/aarch32/include/asm/cpu.h b/arch/aarch32/include/asm/cpu.h
index d691c7b..aa72204 100644
--- a/arch/aarch32/include/asm/cpu.h
+++ b/arch/aarch32/include/asm/cpu.h
@@ -44,6 +44,15 @@
 #define sevl()		asm volatile ("sev" : : : "memory")
 #endif
 
+static inline unsigned long read_cpsr(void)
+{
+	unsigned long cpsr;
+	asm volatile ("mrs      %0, cpsr\n" : "=r" (cpsr));
+	return cpsr;
+}
+
+#define read_cpsr_mode()       (read_cpsr() & PSR_MODE_MASK)
+
 #define MPIDR		"p15, 0, %0, c0, c0, 5"
 #define ID_PFR1		"p15, 0, %0, c0, c1, 1"
 #define ICIALLU		"p15, 0, %0, c7, c5, 0"
diff --git a/arch/aarch32/init.c b/arch/aarch32/init.c
new file mode 100644
index 0000000..785a428
--- /dev/null
+++ b/arch/aarch32/init.c
@@ -0,0 +1,29 @@
+/*
+ * 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 const char *mode_string(void)
+{
+	switch (read_cpsr_mode()) {
+	case PSR_MON:
+		return "PL1";
+	case PSR_HYP:
+		return "PL2 (Non-secure)";
+	default:
+		return "<UNKNOWN MODE>";
+	}
+}
+
+void announce_arch(void)
+{
+	print_string("Entered at ");
+	print_string(mode_string());
+	print_string("\r\n");
+}
diff --git a/arch/aarch64/init.c b/arch/aarch64/init.c
new file mode 100644
index 0000000..82816e7
--- /dev/null
+++ b/arch/aarch64/init.c
@@ -0,0 +1,19 @@
+/*
+ * 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>
+
+void announce_arch(void)
+{
+	unsigned char el = mrs(CurrentEl) >> 2;
+
+	print_string("Entered at EL");
+	print_char('0' + el);
+	print_string("\r\n");
+}
diff --git a/common/init.c b/common/init.c
index 9c471c9..2600f73 100644
--- a/common/init.c
+++ b/common/init.c
@@ -11,14 +11,18 @@
 
 static void announce_bootwrapper(void)
 {
-	print_string("Boot-wrapper v0.2\r\n\r\n");
+	print_string("Boot-wrapper v0.2\r\n");
 }
 
+void announce_arch(void);
+
 void cpu_init_bootwrapper(void)
 {
 	if (this_cpu_logical_id() == 0) {
 		init_uart();
 		announce_bootwrapper();
+		announce_arch();
+		print_string("\r\n");
 		init_platform();
 	}
 }
diff --git a/common/platform.c b/common/platform.c
index 47bf547..80d0562 100644
--- a/common/platform.c
+++ b/common/platform.c
@@ -31,21 +31,25 @@
 #define V2M_SYS(reg)	((void *)SYSREGS_BASE + V2M_SYS_##reg)
 #endif
 
-void print_string(const char *str)
+void print_char(char c)
 {
 	uint32_t flags;
 
-	while (*str) {
-		do
-			flags = raw_readl(PL011(UARTFR));
-		while (flags & PL011_UARTFR_FIFO_FULL);
+	do {
+		flags = raw_readl(PL011(UARTFR));
+	} while (flags & PL011_UARTFR_FIFO_FULL);
+
+	raw_writel(c, PL011(UARTDR));
 
-		raw_writel(*str++, PL011(UARTDR));
+	do {
+		flags = raw_readl(PL011(UARTFR));
+	} while (flags & PL011_UARTFR_BUSY);
+}
 
-		do
-			flags = raw_readl(PL011(UARTFR));
-		while (flags & PL011_UARTFR_BUSY);
-	}
+void print_string(const char *str)
+{
+	while (*str)
+		print_char(*str++);
 }
 
 void init_uart(void)
diff --git a/include/platform.h b/include/platform.h
index e5248e1..237b481 100644
--- a/include/platform.h
+++ b/include/platform.h
@@ -9,6 +9,7 @@
 #ifndef __PLATFORM_H
 #define __PLATFORM_H
 
+void print_char(char c);
 void print_string(const char *str);
 void init_uart(void);