From 08268e27ab9dfe41b420f6aae33459adb6cd7f73 Mon Sep 17 00:00:00 2001
From: Dimitris Papastamos <dimitris.papastamos@arm.com>
Date: Tue, 13 Feb 2018 11:28:02 +0000
Subject: [PATCH] Add AMU support for Cortex-Ares

Change-Id: Ia170c12d3929a616ba80eb7645c301066641f5cc
Signed-off-by: Dimitris Papastamos <dimitris.papastamos@arm.com>
---
 include/lib/cpus/aarch64/cortex_ares.h |  7 ++++++-
 lib/cpus/aarch64/cortex_ares.S         | 29 ++++++++++++++++++++++----
 lib/cpus/aarch64/cortex_ares_pubsub.c  | 26 +++++++++++++++++++++++
 plat/arm/board/fvp/platform.mk         |  1 +
 4 files changed, 58 insertions(+), 5 deletions(-)
 create mode 100644 lib/cpus/aarch64/cortex_ares_pubsub.c

diff --git a/include/lib/cpus/aarch64/cortex_ares.h b/include/lib/cpus/aarch64/cortex_ares.h
index 680811cf2..b2bb6331a 100644
--- a/include/lib/cpus/aarch64/cortex_ares.h
+++ b/include/lib/cpus/aarch64/cortex_ares.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -19,4 +19,9 @@
 /* Definitions of register field mask in CORTEX_ARES_CPUPWRCTLR_EL1 */
 #define CORTEX_ARES_CORE_PWRDN_EN_MASK	0x1
 
+#define CORTEX_ARES_ACTLR_AMEN_BIT	(U(1) << 4)
+
+#define CORTEX_ARES_AMU_NR_COUNTERS	U(5)
+#define CORTEX_ARES_AMU_GROUP0_MASK	U(0x1f)
+
 #endif /* __CORTEX_ARES_H__ */
diff --git a/lib/cpus/aarch64/cortex_ares.S b/lib/cpus/aarch64/cortex_ares.S
index 98e904406..e1302019b 100644
--- a/lib/cpus/aarch64/cortex_ares.S
+++ b/lib/cpus/aarch64/cortex_ares.S
@@ -1,15 +1,36 @@
 /*
- * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <arch.h>
 #include <asm_macros.S>
-#include <bl_common.h>
 #include <cortex_ares.h>
+#include <cpuamu.h>
 #include <cpu_macros.S>
-#include <plat_macros.S>
+
+func cortex_ares_reset_func
+#if ENABLE_AMU
+	/* Make sure accesses from EL0/EL1 and EL2 are not trapped to EL3 */
+	mrs	x0, actlr_el3
+	orr	x0, x0, #CORTEX_ARES_ACTLR_AMEN_BIT
+	msr	actlr_el3, x0
+	isb
+
+	/* Make sure accesses from EL0/EL1 are not trapped to EL2 */
+	mrs	x0, actlr_el2
+	orr	x0, x0, #CORTEX_ARES_ACTLR_AMEN_BIT
+	msr	actlr_el2, x0
+	isb
+
+	/* Enable group0 counters */
+	mov	x0, #CORTEX_ARES_AMU_GROUP0_MASK
+	msr	CPUAMCNTENSET_EL0, x0
+	isb
+#endif
+	ret
+endfunc cortex_ares_reset_func
 
 	/* ---------------------------------------------
 	 * HW will do the cache maintenance while powering down
@@ -47,5 +68,5 @@ func cortex_ares_cpu_reg_dump
 endfunc cortex_ares_cpu_reg_dump
 
 declare_cpu_ops cortex_ares, CORTEX_ARES_MIDR, \
-	CPU_NO_RESET_FUNC, \
+	cortex_ares_reset_func, \
 	cortex_ares_core_pwr_dwn
diff --git a/lib/cpus/aarch64/cortex_ares_pubsub.c b/lib/cpus/aarch64/cortex_ares_pubsub.c
new file mode 100644
index 000000000..c7d850a00
--- /dev/null
+++ b/lib/cpus/aarch64/cortex_ares_pubsub.c
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <cortex_ares.h>
+#include <cpuamu.h>
+#include <pubsub_events.h>
+
+static void *cortex_ares_context_save(const void *arg)
+{
+	if (midr_match(CORTEX_ARES_MIDR) != 0)
+		cpuamu_context_save(CORTEX_ARES_AMU_NR_COUNTERS);
+	return 0;
+}
+
+static void *cortex_ares_context_restore(const void *arg)
+{
+	if (midr_match(CORTEX_ARES_MIDR) != 0)
+		cpuamu_context_restore(CORTEX_ARES_AMU_NR_COUNTERS);
+	return 0;
+}
+
+SUBSCRIBE_TO_EVENT(psci_suspend_pwrdown_start, cortex_ares_context_save);
+SUBSCRIBE_TO_EVENT(psci_suspend_pwrdown_finish, cortex_ares_context_restore);
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 2a09bba56..677beea04 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -208,6 +208,7 @@ ENABLE_AMU			:=	1
 
 ifeq (${ENABLE_AMU},1)
 BL31_SOURCES		+=	lib/cpus/aarch64/cortex_a75_pubsub.c	\
+				lib/cpus/aarch64/cortex_ares_pubsub.c	\
 				lib/cpus/aarch64/cpuamu.c		\
 				lib/cpus/aarch64/cpuamu_helpers.S
 endif