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 86e8af0..b943e7a 100644
--- 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 25287de..4e04814 100644
--- a/lib/cpus/aarch64/cortex_hercules.S
+++ b/lib/cpus/aarch64/cortex_hercules.S
@@ -16,6 +16,35 @@
#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
+
/* ---------------------------------------------
* HW will do the cache maintenance while powering down
* ---------------------------------------------
@@ -60,6 +89,12 @@
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