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>
diff --git a/lib/cpus/aarch64/cpuamu.c b/lib/cpus/aarch64/cpuamu.c
new file mode 100644
index 0000000..38c093a
--- /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);
+}