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) &