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/cpu/armv8/start.S b/arch/arm/cpu/armv8/start.S
index 9e9c6140cd4a520b5a6fac0469ea10c2fdecdc88..b3eef705a53e4503d5154c917286babc1cec8910 100644 (file)
--- a/arch/arm/cpu/armv8/start.S
+++ b/arch/arm/cpu/armv8/start.S
@@ -127,10 +127,6 @@ pie_fixup_done:
        orr     x0, x0, #0xf                    /* SCR_EL3.NS|IRQ|FIQ|EA */
        msr     scr_el3, x0
        msr     cptr_el3, xzr                   /* Enable FP/SIMD */
-#ifdef COUNTER_FREQUENCY
-       ldr     x0, =COUNTER_FREQUENCY
-       msr     cntfrq_el0, x0                  /* Initialize CNTFRQ */
-#endif
        b       0f
 2:     mrs     x1, hcr_el2
        tbnz    x1, #34, 1f                     /* HCR_EL2.E2H */
@@ -142,7 +138,14 @@ pie_fixup_done:
        mov     x0, #3 << 20
        msr     cpacr_el1, x0                   /* Enable FP/SIMD */
 0:
-       isb
+
+#ifdef COUNTER_FREQUENCY
+       branch_if_not_highest_el x0, 4f
+       ldr     x0, =COUNTER_FREQUENCY
+       msr     cntfrq_el0, x0                  /* Initialize CNTFRQ */
+#endif
+
+4:     isb
 
        /*
         * Enable SMPEN bit for coherency.

diff --git a/arch/arm/include/asm/macro.h b/arch/arm/include/asm/macro.h
index bb33b4bc892bf2c13ad891fa43ab8e9e92b59599..485310d66086de326810355be02de177ccfe55fa 100644 (file)
--- a/arch/arm/include/asm/macro.h
+++ b/arch/arm/include/asm/macro.h
@@ -77,6 +77,24 @@ lr   .req    x30
        b.eq    \el1_label
 .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.
  */

diff --git a/arch/arm/include/asm/system.h b/arch/arm/include/asm/system.h
index 11fceec4d2f179d0d9e14f39648640d8f21b9eea..8b3a54e64c82ae9ddf4141aaa8f22c034dfc16ac 100644 (file)
--- a/arch/arm/include/asm/system.h
+++ b/arch/arm/include/asm/system.h
@@ -79,6 +79,12 @@
 #define HCR_EL2_RW_AARCH32     (0 << 31) /* Lower levels are AArch32         */
 #define HCR_EL2_HCD_DIS                (1 << 29) /* Hypervisor Call disabled         */
 
+/*
+ * ID_AA64PFR0_EL1 bits definitions
+ */
+#define ID_AA64PFR0_EL1_EL3    (0xF << 12) /* EL3 implemented                */
+#define ID_AA64PFR0_EL1_EL2    (0xF << 8)  /* EL2 implemented                */
+
 /*
  * CPACR_EL1 bits definitions
  */