Enable AMU for Cortex-Hercules

Change-Id: Ie0a94783d0c8e111ae19fd592304e6485f04ca29
Signed-off-by: Balint Dobszay <balint.dobszay@arm.com>

diff --git a/include/lib/cpus/aarch64/cortex_hercules.h b/include/lib/cpus/aarch64/cortex_hercules.h
index 86e8af034bc860ae9ee73c5be0a24612b9577f43..b943e7a4d318db66ad61dabec40c0feee386de70 100644 (file)
--- a/include/lib/cpus/aarch64/cortex_hercules.h
+++ b/include/lib/cpus/aarch64/cortex_hercules.h
@@ -22,4 +22,20 @@
 #define CORTEX_HERCULES_CPUPWRCTLR_EL1                         S3_0_C15_C2_7
 #define CORTEX_HERCULES_CPUPWRCTLR_EL1_CORE_PWRDN_EN_BIT       U(1)
 
+/*******************************************************************************
+ * CPU Auxiliary Control register specific definitions.
+ ******************************************************************************/
+#define CORTEX_HERCULES_ACTLR_TAM_BIT                          (ULL(1) << 30)
+
+/*******************************************************************************
+ * CPU Activity Monitor Unit register specific definitions.
+ ******************************************************************************/
+#define CPUAMCNTENCLR0_EL0                                     S3_3_C15_C2_4
+#define CPUAMCNTENSET0_EL0                                     S3_3_C15_C2_5
+#define CPUAMCNTENCLR1_EL0                                     S3_3_C15_C3_0
+#define CPUAMCNTENSET1_EL0                                     S3_3_C15_C3_1
+
+#define CORTEX_HERCULES_AMU_GROUP0_MASK                                U(0xF)
+#define CORTEX_HERCULES_AMU_GROUP1_MASK                                U(0x7)
+
 #endif /* CORTEX_HERCULES_H */

diff --git a/lib/cpus/aarch64/cortex_hercules.S b/lib/cpus/aarch64/cortex_hercules.S
index 25287de871837ae9f6190364e46231ecfab41850..4e048145f4db5910fa615e53169eeaab7e002503 100644 (file)
--- a/lib/cpus/aarch64/cortex_hercules.S
+++ b/lib/cpus/aarch64/cortex_hercules.S
@@ -14,6 +14,35 @@
 /* Hardware handled coherency */
 #if HW_ASSISTED_COHERENCY == 0
 #error "cortex_hercules must be compiled with HW_ASSISTED_COHERENCY enabled"
+#endif
+
+       /* -------------------------------------------------
+        * The CPU Ops reset function for Cortex-Hercules
+        * -------------------------------------------------
+        */
+#if ENABLE_AMU
+func cortex_hercules_reset_func
+       /* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */
+       mrs     x0, actlr_el3
+       bic     x0, x0, #CORTEX_HERCULES_ACTLR_TAM_BIT
+       msr     actlr_el3, x0
+
+       /* Make sure accesses from non-secure EL0/EL1 are not trapped to EL2 */
+       mrs     x0, actlr_el2
+       bic     x0, x0, #CORTEX_HERCULES_ACTLR_TAM_BIT
+       msr     actlr_el2, x0
+
+       /* Enable group0 counters */
+       mov     x0, #CORTEX_HERCULES_AMU_GROUP0_MASK
+       msr     CPUAMCNTENSET0_EL0, x0
+
+       /* Enable group1 counters */
+       mov     x0, #CORTEX_HERCULES_AMU_GROUP1_MASK
+       msr     CPUAMCNTENSET1_EL0, x0
+       isb
+
+       ret
+endfunc cortex_hercules_reset_func
 #endif
 
        /* ---------------------------------------------
@@ -60,6 +89,12 @@ func cortex_hercules_cpu_reg_dump
        ret
 endfunc cortex_hercules_cpu_reg_dump
 
+#if ENABLE_AMU
+#define HERCULES_RESET_FUNC cortex_hercules_reset_func
+#else
+#define HERCULES_RESET_FUNC CPU_NO_RESET_FUNC
+#endif
+
 declare_cpu_ops cortex_hercules, CORTEX_HERCULES_MIDR, \
-       CPU_NO_RESET_FUNC, \
+       HERCULES_RESET_FUNC, \
        cortex_hercules_core_pwr_dwn