Merge pull request #1274 from dp-arm/dp/a75

AMU fixes for Cortex-A75

7 files changed, 240 insertions, 183 deletions

diff --git a/include/lib/cpus/aarch64/cortex_a75.h b/include/lib/cpus/aarch64/cortex_a75.h
index 940125da..20f02518 100644
--- a/include/lib/cpus/aarch64/cortex_a75.h
+++ b/include/lib/cpus/aarch64/cortex_a75.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,28 +19,6 @@
 /* Definitions of register field mask in CORTEX_A75_CPUPWRCTLR_EL1 */
 #define CORTEX_A75_CORE_PWRDN_EN_MASK	0x1
 
-/*******************************************************************************
- * CPU Activity Monitor Unit register specific definitions.
- ******************************************************************************/
-#define CPUAMCNTENCLR_EL0	S3_3_C15_C9_7
-#define CPUAMCNTENSET_EL0	S3_3_C15_C9_6
-#define CPUAMCFGR_EL0		S3_3_C15_C10_6
-#define CPUAMUSERENR_EL0	S3_3_C15_C10_7
-
-/* Activity Monitor Event Counter Registers */
-#define CPUAMEVCNTR0_EL0	S3_3_C15_C9_0
-#define CPUAMEVCNTR1_EL0	S3_3_C15_C9_1
-#define CPUAMEVCNTR2_EL0	S3_3_C15_C9_2
-#define CPUAMEVCNTR3_EL0	S3_3_C15_C9_3
-#define CPUAMEVCNTR4_EL0	S3_3_C15_C9_4
-
-/* Activity Monitor Event Type Registers */
-#define CPUAMEVTYPER0_EL0	S3_3_C15_C10_0
-#define CPUAMEVTYPER1_EL0	S3_3_C15_C10_1
-#define CPUAMEVTYPER2_EL0	S3_3_C15_C10_2
-#define CPUAMEVTYPER3_EL0	S3_3_C15_C10_3
-#define CPUAMEVTYPER4_EL0	S3_3_C15_C10_4
-
 #define CORTEX_A75_ACTLR_AMEN_BIT	(U(1) << 4)
 
 /*
@@ -50,9 +28,9 @@
  * CPUAMEVTYPER<n> register and are disabled by default. Platforms may
  * enable this with suitable programming.
  */
-#define CORTEX_A75_AMU_NR_COUNTERS	5
-#define CORTEX_A75_AMU_GROUP0_MASK	0x7
-#define CORTEX_A75_AMU_GROUP1_MASK	(0 << 3)
+#define CORTEX_A75_AMU_NR_COUNTERS	U(5)
+#define CORTEX_A75_AMU_GROUP0_MASK	U(0x7)
+#define CORTEX_A75_AMU_GROUP1_MASK	(U(0) << 3)
 
 #ifndef __ASSEMBLY__
 #include <stdint.h>
diff --git a/include/lib/cpus/aarch64/cpuamu.h b/include/lib/cpus/aarch64/cpuamu.h
new file mode 100644
index 00000000..960a5248
--- /dev/null
+++ b/include/lib/cpus/aarch64/cpuamu.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef __CPUAMU_H__
+#define __CPUAMU_H__
+
+/*******************************************************************************
+ * CPU Activity Monitor Unit register specific definitions.
+ ******************************************************************************/
+#define CPUAMCNTENCLR_EL0	S3_3_C15_C9_7
+#define CPUAMCNTENSET_EL0	S3_3_C15_C9_6
+#define CPUAMCFGR_EL0		S3_3_C15_C10_6
+#define CPUAMUSERENR_EL0	S3_3_C15_C10_7
+
+/* Activity Monitor Event Counter Registers */
+#define CPUAMEVCNTR0_EL0	S3_3_C15_C9_0
+#define CPUAMEVCNTR1_EL0	S3_3_C15_C9_1
+#define CPUAMEVCNTR2_EL0	S3_3_C15_C9_2
+#define CPUAMEVCNTR3_EL0	S3_3_C15_C9_3
+#define CPUAMEVCNTR4_EL0	S3_3_C15_C9_4
+
+/* Activity Monitor Event Type Registers */
+#define CPUAMEVTYPER0_EL0	S3_3_C15_C10_0
+#define CPUAMEVTYPER1_EL0	S3_3_C15_C10_1
+#define CPUAMEVTYPER2_EL0	S3_3_C15_C10_2
+#define CPUAMEVTYPER3_EL0	S3_3_C15_C10_3
+#define CPUAMEVTYPER4_EL0	S3_3_C15_C10_4
+
+#ifndef __ASSEMBLY__
+#include <stdint.h>
+
+uint64_t cpuamu_cnt_read(int idx);
+void cpuamu_cnt_write(int idx, uint64_t val);
+unsigned int cpuamu_read_cpuamcntenset_el0(void);
+unsigned int cpuamu_read_cpuamcntenclr_el0(void);
+void cpuamu_write_cpuamcntenset_el0(unsigned int mask);
+void cpuamu_write_cpuamcntenclr_el0(unsigned int mask);
+
+int midr_match(unsigned int cpu_midr);
+void cpuamu_context_save(unsigned int nr_counters);
+void cpuamu_context_restore(unsigned int nr_counters);
+
+#endif /* __ASSEMBLY__ */
+
+#endif /* __CPUAMU_H__ */
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..16f62f47 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) != 0)
+		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) != 0)
+		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..b9bad860
--- /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	5U
+
+struct amu_ctx {
+	uint64_t cnts[CPUAMU_NR_COUNTERS];
+	unsigned int mask;
+};
+
+static struct amu_ctx amu_ctxs[PLATFORM_CORE_COUNT];
+
+int midr_match(unsigned int cpu_midr)
+{
+	unsigned int midr, midr_mask;
+
+	midr = (unsigned int)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()];
+	unsigned 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()];
+	unsigned 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);
+}
diff --git a/lib/cpus/aarch64/cpuamu_helpers.S b/lib/cpus/aarch64/cpuamu_helpers.S
new file mode 100644
index 00000000..8965d6d0
--- /dev/null
+++ b/lib/cpus/aarch64/cpuamu_helpers.S
@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <cpuamu.h>
+
+	.globl	cpuamu_cnt_read
+	.globl	cpuamu_cnt_write
+	.globl	cpuamu_read_cpuamcntenset_el0
+	.globl	cpuamu_read_cpuamcntenclr_el0
+	.globl	cpuamu_write_cpuamcntenset_el0
+	.globl	cpuamu_write_cpuamcntenclr_el0
+
+/*
+ * uint64_t cpuamu_cnt_read(int idx);
+ *
+ * Given `idx`, read the corresponding AMU counter
+ * and return it in `x0`.
+ */
+func cpuamu_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 cpuamu_cnt_read
+
+/*
+ * void cpuamu_cnt_write(int idx, uint64_t val);
+ *
+ * Given `idx`, write `val` to the corresponding AMU counter.
+ */
+func cpuamu_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 cpuamu_cnt_write
+
+/*
+ * unsigned int cpuamu_read_cpuamcntenset_el0(void);
+ *
+ * Read the `CPUAMCNTENSET_EL0` CPU register and return
+ * it in `x0`.
+ */
+func cpuamu_read_cpuamcntenset_el0
+	mrs	x0, CPUAMCNTENSET_EL0
+	ret
+endfunc cpuamu_read_cpuamcntenset_el0
+
+/*
+ * unsigned int cpuamu_read_cpuamcntenclr_el0(void);
+ *
+ * Read the `CPUAMCNTENCLR_EL0` CPU register and return
+ * it in `x0`.
+ */
+func cpuamu_read_cpuamcntenclr_el0
+	mrs	x0, CPUAMCNTENCLR_EL0
+	ret
+endfunc cpuamu_read_cpuamcntenclr_el0
+
+/*
+ * void cpuamu_write_cpuamcntenset_el0(unsigned int mask);
+ *
+ * Write `mask` to the `CPUAMCNTENSET_EL0` CPU register.
+ */
+func cpuamu_write_cpuamcntenset_el0
+	msr	CPUAMCNTENSET_EL0, x0
+	ret
+endfunc cpuamu_write_cpuamcntenset_el0
+
+/*
+ * void cpuamu_write_cpuamcntenclr_el0(unsigned int mask);
+ *
+ * Write `mask` to the `CPUAMCNTENCLR_EL0` CPU register.
+ */
+func cpuamu_write_cpuamcntenclr_el0
+	msr	CPUAMCNTENCLR_EL0, x0
+	ret
+endfunc cpuamu_write_cpuamcntenclr_el0
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 6857c1f8..8b913fb2 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -182,7 +182,9 @@ ENABLE_PLAT_COMPAT	:= 	0
 ENABLE_AMU			:=	1
 
 ifeq (${ENABLE_AMU},1)
-BL31_SOURCES		+= lib/cpus/aarch64/cortex_a75_pubsub.c
+BL31_SOURCES		+=	lib/cpus/aarch64/cortex_a75_pubsub.c	\
+				lib/cpus/aarch64/cpuamu.c		\
+				lib/cpus/aarch64/cpuamu_helpers.S
 endif
 
 ifneq (${ENABLE_STACK_PROTECTOR},0)