armv8: Initialize CNTFRQ if at highest exception level
CNTFRQ_EL0 is only writable from the highest supported exception
level on the platform. For Armv8-A, this is typically EL3, but
technically EL2 and EL3 are optional so it may need to be
initialized at EL2 or EL1. For Armv8-R, the highest exception
level is always EL2.
This patch moves the initialization outside of the switch_el
block and uses a new macro branch_if_not_highest_el which
dynamically detects whether it is at the highest supported
exception level.
Linux's docs state that CNTFRQ_EL0 should be initialized by the
bootloader. If not set, the the U-Boot prompt countdown hangs.
Signed-off-by: Peter Hoyes <Peter.Hoyes@arm.com>
diff --git a/arch/arm/include/asm/macro.h b/arch/arm/include/asm/macro.h
index bb33b4b..485310d 100644
--- a/arch/arm/include/asm/macro.h
+++ b/arch/arm/include/asm/macro.h
@@ -78,6 +78,24 @@
.endm
/*
+ * Branch if we are not in the highest exception level
+ */
+.macro branch_if_not_highest_el, xreg, label
+ switch_el \xreg, 3f, 2f, 1f
+
+2: mrs \xreg, ID_AA64PFR0_EL1
+ and \xreg, \xreg, #(ID_AA64PFR0_EL1_EL3)
+ cbnz \xreg, \label
+ b 3f
+
+1: mrs \xreg, ID_AA64PFR0_EL1
+ and \xreg, \xreg, #(ID_AA64PFR0_EL1_EL3 | ID_AA64PFR0_EL1_EL2)
+ cbnz \xreg, \label
+
+3:
+.endm
+
+/*
* Branch if current processor is a Cortex-A57 core.
*/
.macro branch_if_a57_core, xreg, a57_label