diff --git a/sys/arm64/arm64/undefined.c b/sys/arm64/arm64/undefined.c --- a/sys/arm64/arm64/undefined.c +++ b/sys/arm64/arm64/undefined.c @@ -82,10 +82,17 @@ undef_handler_t uh_handler; }; +/* System instruction handlers, e.g. msr, mrs, sys */ +struct sys_handler { + LIST_ENTRY(sys_handler) sys_link; + undef_sys_handler_t sys_handler; +}; + /* * Create the undefined instruction handler lists. * This allows us to handle instructions that will trap. */ +LIST_HEAD(, sys_handler) sys_handlers = LIST_HEAD_INITIALIZER(sys_handler); LIST_HEAD(, undef_handler) undef_handlers = LIST_HEAD_INITIALIZER(undef_handlers); #ifdef COMPAT_FREEBSD32 @@ -293,6 +300,72 @@ free(handle, M_UNDEF); } +void +install_sys_handler(undef_sys_handler_t func) +{ + struct sys_handler *sysh; + + sysh = malloc(sizeof(*sysh), M_UNDEF, M_WAITOK); + sysh->sys_handler = func; + LIST_INSERT_HEAD(&sys_handlers, sysh, sys_link); +} + +bool +undef_sys(uint64_t esr, struct trapframe *frame) +{ + struct sys_handler *sysh; + + LIST_FOREACH(sysh, &sys_handlers, sys_link) { + if (sysh->sys_handler(esr, frame)) + return (true); + } + + return (false); +} + +static bool +undef_sys_insn(struct trapframe *frame, uint32_t insn) +{ + uint64_t esr; + int op0; + bool read; + + read = false; + switch (insn & MRS_MASK) { + case MRS_VALUE: + read = true; + /* FALLTHROUGH */ + case MSR_REG_VALUE: + op0 = mrs_Op0(insn); + break; + case MSR_IMM_VALUE: + /* + * MSR (immediate) needs special handling. The + * source register is always 31 (xzr), CRn is 4, + * and op0 is hard coded as 0. + */ + if (MRS_REGISTER(insn) != 31) + return (false); + if (mrs_CRn(insn) != 4) + return (false); + op0 = 0; + break; + default: + return (false); + } + + /* Create a fake EXCP_MSR esr value */ + esr = EXCP_MSR << ESR_ELx_EC_SHIFT; + esr |= ESR_ELx_IL; + esr |= __ISS_MSR_REG(op0, mrs_Op1(insn), mrs_CRn(insn), mrs_CRm(insn), + mrs_Op2(insn)); + esr |= MRS_REGISTER(insn) << ISS_MSR_Rt_SHIFT; + if (read) + esr |= ISS_MSR_DIR; + + return (undef_sys(esr, frame)); +} + int undef_insn(struct trapframe *frame) { @@ -317,6 +390,9 @@ } #endif + if (undef_sys_insn(frame, insn)) + return (1); + LIST_FOREACH(uh, &undef_handlers, uh_link) { ret = uh->uh_handler(frame->tf_elr, insn, frame, frame->tf_esr); if (ret) diff --git a/sys/arm64/include/armreg.h b/sys/arm64/include/armreg.h --- a/sys/arm64/include/armreg.h +++ b/sys/arm64/include/armreg.h @@ -38,6 +38,8 @@ #define MRS_MASK 0xfff00000 #define MRS_VALUE 0xd5300000 +#define MSR_REG_VALUE 0xd5100000 +#define MSR_IMM_VALUE 0xd5000000 #define MRS_SPECIAL(insn) ((insn) & 0x000fffe0) #define MRS_REGISTER(insn) ((insn) & 0x0000001f) #define MRS_Op0_SHIFT 19 diff --git a/sys/arm64/include/undefined.h b/sys/arm64/include/undefined.h --- a/sys/arm64/include/undefined.h +++ b/sys/arm64/include/undefined.h @@ -35,6 +35,7 @@ typedef int (*undef_handler_t)(vm_offset_t, uint32_t, struct trapframe *, uint32_t); +typedef bool (*undef_sys_handler_t)(uint64_t, struct trapframe *); static inline int mrs_Op0(uint32_t insn) @@ -57,11 +58,13 @@ MRS_GET(Op2) void undef_init(void); +void install_sys_handler(undef_sys_handler_t); void *install_undef_handler(undef_handler_t); #ifdef COMPAT_FREEBSD32 void *install_undef32_handler(undef_handler_t); #endif void remove_undef_handler(void *); +bool undef_sys(uint64_t, struct trapframe *); int undef_insn(struct trapframe *); #endif /* _KERNEL */