diff options
| -rw-r--r-- | arch/arm64/include/asm/esr.h | 3 | ||||
| -rw-r--r-- | arch/arm64/kernel/traps.c | 12 |
2 files changed, 15 insertions, 0 deletions
diff --git a/arch/arm64/include/asm/esr.h b/arch/arm64/include/asm/esr.h index f40f2f4a4e84..2d4e9c26f8f6 100644 --- a/arch/arm64/include/asm/esr.h +++ b/arch/arm64/include/asm/esr.h @@ -146,6 +146,9 @@ #define ESR_ELx_SYS64_ISS_SYS_CNTVCT (ESR_ELx_SYS64_ISS_SYS_VAL(3, 3, 2, 14, 0) | \ ESR_ELx_SYS64_ISS_DIR_READ) +#define ESR_ELx_SYS64_ISS_SYS_CNTFRQ (ESR_ELx_SYS64_ISS_SYS_VAL(3, 3, 0, 14, 0) | \ + ESR_ELx_SYS64_ISS_DIR_READ) + #ifndef __ASSEMBLY__ #include <asm/types.h> diff --git a/arch/arm64/kernel/traps.c b/arch/arm64/kernel/traps.c index 42615da5b2e9..ef77a5f4f075 100644 --- a/arch/arm64/kernel/traps.c +++ b/arch/arm64/kernel/traps.c @@ -418,11 +418,23 @@ static void cntvct_read_handler(unsigned int esr, struct pt_regs *regs) regs->pc += 4; } +static void cntfrq_read_handler(unsigned int esr, struct pt_regs *regs) +{ + int rt = (esr & ESR_ELx_SYS64_ISS_RT_MASK) >> ESR_ELx_SYS64_ISS_RT_SHIFT; + + if (rt != 31) + regs->regs[rt] = read_sysreg(cntfrq_el0); + regs->pc += 4; +} + asmlinkage void __exception do_sysinstr(unsigned int esr, struct pt_regs *regs) { if ((esr & ESR_ELx_SYS64_ISS_SYS_OP_MASK) == ESR_ELx_SYS64_ISS_SYS_CNTVCT) { cntvct_read_handler(esr, regs); return; + } else if ((esr & ESR_ELx_SYS64_ISS_SYS_OP_MASK) == ESR_ELx_SYS64_ISS_SYS_CNTFRQ) { + cntfrq_read_handler(esr, regs); + return; } do_undefinstr(regs); |