diff --git a/arch/arm64/arch.c b/arch/arm64/arch.c index aee197723..b460815a9 100644 --- a/arch/arm64/arch.c +++ b/arch/arm64/arch.c @@ -25,9 +25,41 @@ #define LOCAL_TRACE 0 +#define ID_AA64DFR0_PMUVER_SHIFT 8 +#define ID_AA64DFR0_PMUVER_MASK (0xfU << ID_AA64DFR0_PMUVER_SHIFT) +#define ID_AA64DFR0_PMUVER_NONE 0 +#define ID_AA64DFR0_PMUVER_IMPDEF 0xf + +#define PMCR_EL0_E (1U << 0) +#define PMCR_EL0_C (1U << 2) +#define PMCR_EL0_D (1U << 3) + +#define PMCNTENSET_EL0_C (1U << 31) + /* Defined in start.S. */ extern uint64_t arm64_boot_el; +bool arm64_cycle_counter_enabled; + +static void arm64_enable_cycle_counter(void) { + uint64_t dfr0 = ARM64_READ_SYSREG(id_aa64dfr0_el1); + uint pmuver = (dfr0 & ID_AA64DFR0_PMUVER_MASK) >> ID_AA64DFR0_PMUVER_SHIFT; + + if (pmuver == ID_AA64DFR0_PMUVER_NONE || pmuver == ID_AA64DFR0_PMUVER_IMPDEF) { + arm64_cycle_counter_enabled = false; + return; + } + + uint64_t pmcr = ARM64_READ_SYSREG(pmcr_el0); + pmcr |= PMCR_EL0_E | PMCR_EL0_C; + pmcr &= ~PMCR_EL0_D; + + ARM64_WRITE_SYSREG(pmcr_el0, pmcr); + ARM64_WRITE_SYSREG(pmcntenset_el0, PMCNTENSET_EL0_C); + + arm64_cycle_counter_enabled = true; +} + // initial setup per cpu immediately after entering C code void arm64_early_init_percpu(void) { // set the vector base @@ -63,7 +95,7 @@ void arm64_early_init_percpu(void) { ARM64_WRITE_SYSREG(TPIDRRO_EL0, 0UL); // TODO: read feature bits on cpu 0 - // TODO: enable cycle counter if present + arm64_enable_cycle_counter(); arch_enable_fiqs(); } diff --git a/arch/arm64/include/arch/arch_ops.h b/arch/arm64/include/arch/arch_ops.h index dea0dc242..01f8b9c52 100644 --- a/arch/arm64/include/arch/arch_ops.h +++ b/arch/arm64/include/arch/arch_ops.h @@ -21,6 +21,8 @@ __BEGIN_CDECLS #define ENABLE_CYCLE_COUNTER 1 +extern bool arm64_cycle_counter_enabled; + void arch_stacktrace(uint64_t fp, uint64_t pc); static inline void arch_enable_fiqs(void) { @@ -57,8 +59,11 @@ static inline bool arch_fiqs_disabled(void) { #endif static inline ulong arch_cycle_count(void) { -//#warning no arch_cycle_count implementation - return 0; + if (!arm64_cycle_counter_enabled) { + return 0; + } + + return ARM64_READ_SYSREG(pmccntr_el0); } /* use the cpu local thread context pointer to store current_thread */ @@ -73,4 +78,3 @@ static inline void arch_set_current_thread(struct thread *t) { __END_CDECLS #endif // ASSEMBLY -