From b4f5b9e23770f2c9cddf95fef7b9af32da309f7f Mon Sep 17 00:00:00 2001 From: Peng Fan Date: Mon, 8 Mar 2021 18:56:19 +0800 Subject: [PATCH] aarch64: reset: initialize CNTFRQ_EL0 in the highest EL Can only be written at the highest Exception level implemented. For example, if EL3 is the highest implemented Exception level, CNTFRQ_EL0 can only be written at EL3. Also move z_arm64_el_highest_plat_init to be called when is_el_highest Signed-off-by: Peng Fan --- arch/arm/core/aarch64/reset.c | 4 +++- include/arch/arm/aarch64/lib_helpers.h | 16 ++++++++++++++++ 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/arch/arm/core/aarch64/reset.c b/arch/arm/core/aarch64/reset.c index c8f354b308f..a6013d425b7 100644 --- a/arch/arm/core/aarch64/reset.c +++ b/arch/arm/core/aarch64/reset.c @@ -31,7 +31,9 @@ void __weak z_arm64_el1_plat_init(void) void z_arm64_el_highest_init(void) { - write_cntfrq_el0(CONFIG_SYS_CLOCK_HW_CYCLES_PER_SEC); + if (is_el_highest_implemented()) { + write_cntfrq_el0(CONFIG_SYS_CLOCK_HW_CYCLES_PER_SEC); + } z_arm64_el_highest_plat_init(); diff --git a/include/arch/arm/aarch64/lib_helpers.h b/include/arch/arm/aarch64/lib_helpers.h index 29064dd1258..b314b5dda6a 100644 --- a/include/arch/arm/aarch64/lib_helpers.h +++ b/include/arch/arm/aarch64/lib_helpers.h @@ -153,6 +153,22 @@ static inline bool is_el_implemented(unsigned int el) return (((read_id_aa64pfr0_el1() >> shift) & ID_AA64PFR0_ELX_MASK) != 0U); } +static inline bool is_el_highest_implemented(void) +{ + uint32_t el_highest; + uint32_t curr_el; + + el_highest = read_id_aa64pfr0_el1() & 0xFFFF; + el_highest = (31U - __builtin_clz(el_highest)) / 4; + + curr_el = GET_EL(read_currentel()); + + if (curr_el < el_highest) + return false; + + return true; +} + static inline bool is_el2_sec_supported(void) { return (((read_id_aa64pfr0_el1() >> ID_AA64PFR0_SEL2_SHIFT) &