Refactor AMU support for Cortex A75

This patch also fixes the assumption that the counters are disabled on
the resume path.  This is incorrect as the AMU counters are enabled
early in the CPU reset function before `cpuamu_context_restore()`
runs.

Change-Id: I38a94eb166a523f00de18e86860434ffccff2131
Signed-off-by: Dimitris Papastamos <dimitris.papastamos@arm.com>

3 files changed, 78 insertions, 156 deletions

diff --git a/lib/cpus/aarch64/cortex_a75.S b/lib/cpus/aarch64/cortex_a75.S
index 946f9884..12ea304d 100644
--- a/lib/cpus/aarch64/cortex_a75.S
+++ b/lib/cpus/aarch64/cortex_a75.S
@@ -6,108 +6,9 @@
 
 #include <arch.h>
 #include <asm_macros.S>
-#include <bl_common.h>
-#include <cpu_macros.S>
-#include <plat_macros.S>
 #include <cortex_a75.h>
-
-	.globl	cortex_a75_amu_cnt_read
-	.globl	cortex_a75_amu_cnt_write
-	.globl	cortex_a75_amu_read_cpuamcntenset_el0
-	.globl	cortex_a75_amu_read_cpuamcntenclr_el0
-	.globl	cortex_a75_amu_write_cpuamcntenset_el0
-	.globl	cortex_a75_amu_write_cpuamcntenclr_el0
-
-/*
- * uint64_t cortex_a75_amu_cnt_read(int idx);
- *
- * Given `idx`, read the corresponding AMU counter
- * and return it in `x0`.
- */
-func cortex_a75_amu_cnt_read
-	adr	x1, 1f
-	lsl	x0, x0, #3
-	add	x1, x1, x0
-	br	x1
-
-1:
-	mrs	x0, CPUAMEVCNTR0_EL0
-	ret
-	mrs	x0, CPUAMEVCNTR1_EL0
-	ret
-	mrs	x0, CPUAMEVCNTR2_EL0
-	ret
-	mrs	x0, CPUAMEVCNTR3_EL0
-	ret
-	mrs	x0, CPUAMEVCNTR4_EL0
-	ret
-endfunc cortex_a75_amu_cnt_read
-
-/*
- * void cortex_a75_amu_cnt_write(int idx, uint64_t val);
- *
- * Given `idx`, write `val` to the corresponding AMU counter.
- */
-func cortex_a75_amu_cnt_write
-	adr	x2, 1f
-	lsl	x0, x0, #3
-	add	x2, x2, x0
-	br	x2
-
-1:
-	msr	CPUAMEVCNTR0_EL0, x0
-	ret
-	msr	CPUAMEVCNTR1_EL0, x0
-	ret
-	msr	CPUAMEVCNTR2_EL0, x0
-	ret
-	msr	CPUAMEVCNTR3_EL0, x0
-	ret
-	msr	CPUAMEVCNTR4_EL0, x0
-	ret
-endfunc cortex_a75_amu_cnt_write
-
-/*
- * unsigned int cortex_a75_amu_read_cpuamcntenset_el0(void);
- *
- * Read the `CPUAMCNTENSET_EL0` CPU register and return
- * it in `x0`.
- */
-func cortex_a75_amu_read_cpuamcntenset_el0
-	mrs	x0, CPUAMCNTENSET_EL0
-	ret
-endfunc cortex_a75_amu_read_cpuamcntenset_el0
-
-/*
- * unsigned int cortex_a75_amu_read_cpuamcntenclr_el0(void);
- *
- * Read the `CPUAMCNTENCLR_EL0` CPU register and return
- * it in `x0`.
- */
-func cortex_a75_amu_read_cpuamcntenclr_el0
-	mrs	x0, CPUAMCNTENCLR_EL0
-	ret
-endfunc cortex_a75_amu_read_cpuamcntenclr_el0
-
-/*
- * void cortex_a75_amu_write_cpuamcntenset_el0(unsigned int mask);
- *
- * Write `mask` to the `CPUAMCNTENSET_EL0` CPU register.
- */
-func cortex_a75_amu_write_cpuamcntenset_el0
-	msr	CPUAMCNTENSET_EL0, x0
-	ret
-endfunc cortex_a75_amu_write_cpuamcntenset_el0
-
-/*
- * void cortex_a75_amu_write_cpuamcntenclr_el0(unsigned int mask);
- *
- * Write `mask` to the `CPUAMCNTENCLR_EL0` CPU register.
- */
-func cortex_a75_amu_write_cpuamcntenclr_el0
-	mrs	x0, CPUAMCNTENCLR_EL0
-	ret
-endfunc cortex_a75_amu_write_cpuamcntenclr_el0
+#include <cpuamu.h>
+#include <cpu_macros.S>
 
 func cortex_a75_reset_func
 #if IMAGE_BL31 && WORKAROUND_CVE_2017_5715
diff --git a/lib/cpus/aarch64/cortex_a75_pubsub.c b/lib/cpus/aarch64/cortex_a75_pubsub.c
index a1ffcb04..87beca27 100644
--- a/lib/cpus/aarch64/cortex_a75_pubsub.c
+++ b/lib/cpus/aarch64/cortex_a75_pubsub.c
@@ -1,73 +1,24 @@
 /*
- * 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 <cortex_a75.h>
-#include <platform.h>
+#include <cpuamu.h>
 #include <pubsub_events.h>
 
-struct amu_ctx {
-	uint64_t cnts[CORTEX_A75_AMU_NR_COUNTERS];
-	uint16_t mask;
-};
-
-static struct amu_ctx amu_ctxs[PLATFORM_CORE_COUNT];
-
 static void *cortex_a75_context_save(const void *arg)
 {
-	struct amu_ctx *ctx = &amu_ctxs[plat_my_core_pos()];
-	unsigned int midr;
-	unsigned int midr_mask;
-	int i;
-
-	midr = read_midr();
-	midr_mask = (MIDR_IMPL_MASK << MIDR_IMPL_SHIFT) |
-		(MIDR_PN_MASK << MIDR_PN_SHIFT);
-	if ((midr & midr_mask) != (CORTEX_A75_MIDR & midr_mask))
-		return 0;
-
-	/* Save counter configuration */
-	ctx->mask = cortex_a75_amu_read_cpuamcntenset_el0();
-
-	/* Ensure counters are disabled */
-	cortex_a75_amu_write_cpuamcntenclr_el0(ctx->mask);
-	isb();
-
-	/* Save counters */
-	for (i = 0; i < CORTEX_A75_AMU_NR_COUNTERS; i++)
-		ctx->cnts[i] = cortex_a75_amu_cnt_read(i);
-
+	if (midr_match(CORTEX_A75_MIDR))
+		cpuamu_context_save(CORTEX_A75_AMU_NR_COUNTERS);
 	return 0;
 }
 
 static void *cortex_a75_context_restore(const void *arg)
 {
-	struct amu_ctx *ctx = &amu_ctxs[plat_my_core_pos()];
-	unsigned int midr;
-	unsigned int midr_mask;
-	int i;
-
-	midr = read_midr();
-	midr_mask = (MIDR_IMPL_MASK << MIDR_IMPL_SHIFT) |
-		(MIDR_PN_MASK << MIDR_PN_SHIFT);
-	if ((midr & midr_mask) != (CORTEX_A75_MIDR & midr_mask))
-		return 0;
-
-	ctx = &amu_ctxs[plat_my_core_pos()];
-
-	/* Counters were disabled in `cortex_a75_context_save()` */
-	assert(cortex_a75_amu_read_cpuamcntenset_el0() == 0);
-
-	/* Restore counters */
-	for (i = 0; i < CORTEX_A75_AMU_NR_COUNTERS; i++)
-		cortex_a75_amu_cnt_write(i, ctx->cnts[i]);
-	isb();
-
-	/* Restore counter configuration */
-	cortex_a75_amu_write_cpuamcntenset_el0(ctx->mask);
-
+	if (midr_match(CORTEX_A75_MIDR))
+		cpuamu_context_restore(CORTEX_A75_AMU_NR_COUNTERS);
 	return 0;
 }
 
diff --git a/lib/cpus/aarch64/cpuamu.c b/lib/cpus/aarch64/cpuamu.c
new file mode 100644
index 00000000..38c093ac
--- /dev/null
+++ b/lib/cpus/aarch64/cpuamu.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <cpuamu.h>
+#include <platform.h>
+#include <pubsub_events.h>
+
+#define CPUAMU_NR_COUNTERS	5
+
+struct amu_ctx {
+	uint64_t cnts[CPUAMU_NR_COUNTERS];
+	uint16_t mask;
+};
+
+static struct amu_ctx amu_ctxs[PLATFORM_CORE_COUNT];
+
+int midr_match(unsigned int cpu_midr)
+{
+	unsigned int midr, midr_mask;
+
+	midr = read_midr();
+	midr_mask = (MIDR_IMPL_MASK << MIDR_IMPL_SHIFT) |
+		(MIDR_PN_MASK << MIDR_PN_SHIFT);
+	return ((midr & midr_mask) == (cpu_midr & midr_mask));
+}
+
+void cpuamu_context_save(unsigned int nr_counters)
+{
+	struct amu_ctx *ctx = &amu_ctxs[plat_my_core_pos()];
+	int i;
+
+	assert(nr_counters <= CPUAMU_NR_COUNTERS);
+
+	/* Save counter configuration */
+	ctx->mask = cpuamu_read_cpuamcntenset_el0();
+
+	/* Disable counters */
+	cpuamu_write_cpuamcntenclr_el0(ctx->mask);
+	isb();
+
+	/* Save counters */
+	for (i = 0; i < nr_counters; i++)
+		ctx->cnts[i] = cpuamu_cnt_read(i);
+}
+
+void cpuamu_context_restore(unsigned int nr_counters)
+{
+	struct amu_ctx *ctx = &amu_ctxs[plat_my_core_pos()];
+	int i;
+
+	assert(nr_counters <= CPUAMU_NR_COUNTERS);
+
+	/*
+	 * Disable counters.  They were enabled early in the
+	 * CPU reset function.
+	 */
+	cpuamu_write_cpuamcntenclr_el0(ctx->mask);
+	isb();
+
+	/* Restore counters */
+	for (i = 0; i < nr_counters; i++)
+		cpuamu_cnt_write(i, ctx->cnts[i]);
+	isb();
+
+	/* Restore counter configuration */
+	cpuamu_write_cpuamcntenset_el0(ctx->mask);
+}