Add support for FEAT_MTPMU for Armv8.6
If FEAT_PMUv3 is implemented and PMEVTYPER<n>(_EL0).MT bit is implemented
as well, it is possible to control whether PMU counters take into account
events happening on other threads.
If FEAT_MTPMU is implemented, EL3 (or EL2) can override the MT bit
leaving it to effective state of 0 regardless of any write to it.
This patch introduces the DISABLE_MTPMU flag, which allows to diable
multithread event count from EL3 (or EL2). The flag is disabled
by default so the behavior is consistent with those architectures
that do not implement FEAT_MTPMU.
Signed-off-by: Javier Almansa Sobrino <javier.almansasobrino@arm.com>
Change-Id: Iee3a8470ae8ba13316af1bd40c8d4aa86e0cb85e
diff --git a/lib/extensions/mtpmu/aarch32/mtpmu.S b/lib/extensions/mtpmu/aarch32/mtpmu.S
new file mode 100644
index 0000000..834cee3
--- /dev/null
+++ b/lib/extensions/mtpmu/aarch32/mtpmu.S
@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2020, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+
+ .global mtpmu_disable
+
+/* -------------------------------------------------------------
+ * The functions in this file are called at entrypoint, before
+ * the CPU has decided whether this is a cold or a warm boot.
+ * Therefore there are no stack yet to rely on for a C function
+ * call.
+ * -------------------------------------------------------------
+ */
+
+/*
+ * bool mtpmu_supported(void)
+ *
+ * Return a boolean indicating whether FEAT_MTPMU is supported or not.
+ *
+ * Trash registers: r0.
+ */
+func mtpmu_supported
+ ldcopr r0, ID_DFR1
+ and r0, r0, #(ID_DFR1_MTPMU_MASK >> ID_DFR1_MTPMU_SHIFT)
+ cmp r0, #ID_DFR1_MTPMU_SUPPORTED
+ mov r0, #0
+ addeq r0, r0, #1
+ bx lr
+endfunc mtpmu_supported
+
+/*
+ * bool el_implemented(unsigned int el)
+ *
+ * Return a boolean indicating if the specified EL (2 or 3) is implemented.
+ *
+ * Trash registers: r0
+ */
+func el_implemented
+ cmp r0, #3
+ ldcopr r0, ID_PFR1
+ lsreq r0, r0, #ID_PFR1_SEC_SHIFT
+ lsrne r0, r0, #ID_PFR1_VIRTEXT_SHIFT
+ /*
+ * ID_PFR1_VIRTEXT_MASK is the same as ID_PFR1_SEC_MASK
+ * so use any one of them
+ */
+ and r0, r0, #ID_PFR1_VIRTEXT_MASK
+ cmp r0, #ID_PFR1_ELx_ENABLED
+ mov r0, #0
+ addeq r0, r0, #1
+ bx lr
+endfunc el_implemented
+
+/*
+ * void mtpmu_disable(void)
+ *
+ * Disable mtpmu feature if supported.
+ *
+ * Trash register: r0, r1, r2
+ */
+func mtpmu_disable
+ mov r2, lr
+ bl mtpmu_supported
+ cmp r0, #0
+ bxeq r2 /* FEAT_MTPMU not supported */
+
+ /* FEAT_MTMPU Supported */
+ mov r0, #3
+ bl el_implemented
+ cmp r0, #0
+ beq 1f
+
+ /* EL3 implemented */
+ ldcopr r0, SDCR
+ ldr r1, =SDCR_MTPME_BIT
+ bic r0, r0, r1
+ stcopr r0, SDCR
+
+ /*
+ * If EL3 is implemented, HDCR.MTPME is implemented as Res0 and
+ * FEAT_MTPMU is controlled only from EL3, so no need to perform
+ * any operations for EL2.
+ */
+ isb
+ bx r2
+1:
+ /* EL3 not implemented */
+ mov r0, #2
+ bl el_implemented
+ cmp r0, #0
+ bxeq r2 /* No EL2 or EL3 implemented */
+
+ /* EL2 implemented */
+ ldcopr r0, HDCR
+ ldr r1, =HDCR_MTPME_BIT
+ orr r0, r0, r1
+ stcopr r0, HDCR
+ isb
+ bx r2
+endfunc mtpmu_disable