feat(el3-runtime): introduce system register trap handler

At the moment we only handle SMC traps from lower ELs, but ignore any
other synchronous traps and just panic.
To cope with system register traps, which we might need to emulate,
introduce a C function to handle those traps, and wire that up in the
exception handler to be called.

We provide a dispatcher function (in C), that will call platform
specific implementation for certain (classes of) system registers.
For now this is empty.

Signed-off-by: Andre Przywara <andre.przywara@arm.com>
Change-Id: If147bcb49472eb02791498700300926afbcf75ff

4 files changed, 114 insertions, 4 deletions

diff --git a/bl31/aarch64/runtime_exceptions.S b/bl31/aarch64/runtime_exceptions.S
index 614ea717e..c9c3da99f 100644
--- a/bl31/aarch64/runtime_exceptions.S
+++ b/bl31/aarch64/runtime_exceptions.S
@@ -10,6 +10,7 @@
 #include <asm_macros.S>
 #include <bl31/ea_handle.h>
 #include <bl31/interrupt_mgmt.h>
+#include <bl31/sync_handle.h>
 #include <common/runtime_svc.h>
 #include <context.h>
 #include <el3_common_macros.S>
@@ -191,7 +192,10 @@ exp_from_EL3:
 	b.eq	smc_handler32
 
 	cmp	x30, #EC_AARCH64_SMC
-	b.eq	smc_handler64
+	b.eq	sync_handler64
+
+	cmp	x30, #EC_AARCH64_SYS
+	b.eq	sync_handler64
 
 	/* Synchronous exceptions other than the above are assumed to be EA */
 	ldr	x30, [sp, #CTX_GPREGS_OFFSET + CTX_GPREG_LR]
@@ -452,12 +456,12 @@ brk_message:
 	 * Note that x30 has been explicitly saved and can be used here
 	 * ---------------------------------------------------------------------
 	 */
-func smc_handler
+func sync_exception_handler
 smc_handler32:
 	/* Check whether aarch32 issued an SMC64 */
 	tbnz	x0, #FUNCID_CC_SHIFT, smc_prohibited
 
-smc_handler64:
+sync_handler64:
 	/* NOTE: The code below must preserve x0-x4 */
 
 	/*
@@ -504,6 +508,12 @@ smc_handler64:
 	/* Load SCR_EL3 */
 	mrs	x18, scr_el3
 
+	/* check for system register traps */
+	mrs	x16, esr_el3
+	ubfx	x17, x16, #ESR_EC_SHIFT, #ESR_EC_LENGTH
+	cmp	x17, #EC_AARCH64_SYS
+	b.eq	sysreg_handler64
+
 	/* Clear flag register */
 	mov	x7, xzr
 
@@ -569,6 +579,32 @@ smc_handler64:
 
 	b	el3_exit
 
+sysreg_handler64:
+	mov	x0, x16		/* ESR_EL3, containing syndrome information */
+	mov	x1, x6		/* lower EL's context */
+	mov	x19, x6		/* save context pointer for after the call */
+	mov	sp, x12		/* EL3 runtime stack, as loaded above */
+
+	/* int handle_sysreg_trap(uint64_t esr_el3, cpu_context_t *ctx); */
+	bl	handle_sysreg_trap
+	/*
+	 * returns:
+	 *   -1: unhandled trap, panic
+	 *    0: handled trap, return to the trapping instruction (repeating it)
+	 *    1: handled trap, return to the next instruction
+	 */
+
+	tst	w0, w0
+	b.mi	do_panic	/* negative return value: panic */
+	b.eq	1f		/* zero: do not change ELR_EL3 */
+
+	/* advance the PC to continue after the instruction */
+	ldr	x1, [x19, #CTX_EL3STATE_OFFSET + CTX_ELR_EL3]
+	add	x1, x1, #4
+	str	x1, [x19, #CTX_EL3STATE_OFFSET + CTX_ELR_EL3]
+1:
+	b	el3_exit
+
 smc_unknown:
 	/*
 	 * Unknown SMC call. Populate return value with SMC_UNK and call
@@ -593,7 +629,7 @@ rt_svc_fw_critical_error:
 	msr	spsel, #MODE_SP_ELX
 	no_ret	report_unhandled_exception
 #endif
-endfunc smc_handler
+endfunc sync_exception_handler
 
 	/* ---------------------------------------------------------------------
 	 * The following code handles exceptions caused by BRK instructions.
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index 4c93a55ad..ac15f9f58 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -42,6 +42,7 @@ BL31_SOURCES		+=	bl31/bl31_main.c				\
 				bl31/aarch64/ea_delegate.S			\
 				bl31/aarch64/runtime_exceptions.S		\
 				bl31/bl31_context_mgmt.c			\
+				bl31/bl31_traps.c				\
 				common/runtime_svc.c				\
 				lib/cpus/aarch64/dsu_helpers.S			\
 				plat/common/aarch64/platform_mp_stack.S		\
diff --git a/bl31/bl31_traps.c b/bl31/bl31_traps.c
new file mode 100644
index 000000000..7c01338dd
--- /dev/null
+++ b/bl31/bl31_traps.c
@@ -0,0 +1,18 @@
+/*
+ * Copyright (c) 2022, ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * Dispatch synchronous system register traps from lower ELs.
+ */
+
+#include <bl31/sync_handle.h>
+#include <context.h>
+
+int handle_sysreg_trap(uint64_t esr_el3, cpu_context_t *ctx)
+{
+	switch (esr_el3 & ISS_SYSREG_OPCODE_MASK) {
+	default:
+		return TRAP_RET_UNHANDLED;
+	}
+}
diff --git a/include/bl31/sync_handle.h b/include/bl31/sync_handle.h
new file mode 100644
index 000000000..6ff88f949
--- /dev/null
+++ b/include/bl31/sync_handle.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2022, ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef TRAP_HANDLE_H
+#define TRAP_HANDLE_H
+
+#include <stdbool.h>
+#include <context.h>
+
+#define ISS_SYSREG_OPCODE_MASK		0x3ffc1eUL
+#define ISS_SYSREG_REG_MASK		0x0003e0UL
+#define ISS_SYSREG_REG_SHIFT		5U
+#define ISS_SYSREG_DIRECTION_MASK	0x000001UL
+
+#define TRAP_RET_UNHANDLED		-1
+#define TRAP_RET_REPEAT			0
+#define TRAP_RET_CONTINUE		1
+
+#ifndef __ASSEMBLER__
+static inline unsigned int get_sysreg_iss_rt(uint64_t esr)
+{
+	return (esr & ISS_SYSREG_REG_MASK) >> ISS_SYSREG_REG_SHIFT;
+}
+
+static inline bool is_sysreg_iss_write(uint64_t esr)
+{
+	return !(esr & ISS_SYSREG_DIRECTION_MASK);
+}
+
+/**
+ * handle_sysreg_trap() - Handle AArch64 system register traps from lower ELs
+ * @esr_el3: The content of ESR_EL3, containing the trap syndrome information
+ * @ctx: Pointer to the lower EL context, containing saved registers
+ *
+ * Called by the exception handler when a synchronous trap identifies as a
+ * system register trap (EC=0x18). ESR contains the encoding of the op[x] and
+ * CRm/CRn fields, to identify the system register, and the target/source
+ * GPR plus the direction (MRS/MSR). The lower EL's context can be altered
+ * by the function, to inject back the result of the emulation.
+ *
+ * Return: indication how to proceed with the trap:
+ *   TRAP_RET_UNHANDLED(-1): trap is unhandled, trigger panic
+ *   TRAP_RET_REPEAT(0): trap was handled, return to the trapping instruction
+ *			 (repeating it)
+ *   TRAP_RET_CONTINUE(1): trap was handled, return to the next instruction
+ *		           (continuing after it)
+ */
+int handle_sysreg_trap(uint64_t esr_el3, cpu_context_t *ctx);
+
+#endif /* __ASSEMBLER__ */
+
+#endif