diff --git a/sys/arm/arm/generic_timer.c b/sys/arm/arm/generic_timer.c --- a/sys/arm/arm/generic_timer.c +++ b/sys/arm/arm/generic_timer.c @@ -63,6 +63,10 @@ #include /* For arm_set_delay */ #endif +#if defined(__aarch64__) +#include +#endif + #ifdef FDT #include #include @@ -235,12 +239,52 @@ isb(); } +#ifdef __aarch64__ +static int +cntpct_handler(vm_offset_t va, uint32_t insn, struct trapframe *frame, + uint32_t esr) +{ + uint64_t val; + int reg; + + if ((insn & MRS_MASK) != MRS_VALUE) + return (0); + + if (MRS_SPECIAL(insn) != MRS_SPECIAL(CNTPCT_EL0)) + return (0); + + reg = MRS_REGISTER(insn); + val = READ_SPECIALREG(cntvct_el0); + if (reg < nitems(frame->tf_x)) { + frame->tf_x[reg] = val; + } else if (reg == 30) { + frame->tf_lr = val; + } + + /* + * We will handle this instruction, move to the next so we + * don't trap here again. + */ + frame->tf_elr += INSN_SIZE; + + return (1); +} +#endif + static void tmr_setup_user_access(void *arg __unused) { + int emulate; - if (arm_tmr_sc != NULL) + if (arm_tmr_sc != NULL) { smp_rendezvous(NULL, setup_user_access, NULL, NULL); +#ifdef __aarch64__ + if (TUNABLE_INT_FETCH("hw.emulate_phys_counter", &emulate) && + emulate != 0) { + install_undef_handler(true, cntpct_handler); + } +#endif + } } SYSINIT(tmr_ua, SI_SUB_SMP, SI_ORDER_ANY, tmr_setup_user_access, NULL);