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 #include #include +#include #include #include #include @@ -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 +#include + +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 +#include + +#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