summaryrefslogtreecommitdiffstats
path: root/lib/extensions
diff options
context:
space:
mode:
Diffstat (limited to 'lib/extensions')
-rw-r--r--lib/extensions/amu/aarch32/amu.c421
-rw-r--r--lib/extensions/amu/aarch32/amu_helpers.S271
-rw-r--r--lib/extensions/amu/aarch64/amu.c655
-rw-r--r--lib/extensions/amu/aarch64/amu_helpers.S389
-rw-r--r--lib/extensions/amu/amu.mk24
-rw-r--r--lib/extensions/amu/amu_private.h38
-rw-r--r--lib/extensions/brbe/brbe.c27
-rw-r--r--lib/extensions/mpam/mpam.c38
-rw-r--r--lib/extensions/mtpmu/aarch32/mtpmu.S105
-rw-r--r--lib/extensions/mtpmu/aarch64/mtpmu.S96
-rw-r--r--lib/extensions/pauth/pauth_helpers.S141
-rw-r--r--lib/extensions/ras/ras_common.c184
-rw-r--r--lib/extensions/ras/std_err_record.c79
-rw-r--r--lib/extensions/sme/sme.c109
-rw-r--r--lib/extensions/spe/spe.c98
-rw-r--r--lib/extensions/sve/sve.c70
-rw-r--r--lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c36
-rw-r--r--lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c37
-rw-r--r--lib/extensions/trbe/trbe.c55
-rw-r--r--lib/extensions/trf/aarch32/trf.c35
-rw-r--r--lib/extensions/trf/aarch64/trf.c36
21 files changed, 2944 insertions, 0 deletions
diff --git a/lib/extensions/amu/aarch32/amu.c b/lib/extensions/amu/aarch32/amu.c
new file mode 100644
index 0000000..57b1158
--- /dev/null
+++ b/lib/extensions/amu/aarch32/amu.c
@@ -0,0 +1,421 @@
+/*
+ * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <cdefs.h>
+#include <stdbool.h>
+
+#include "../amu_private.h"
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/el3_runtime/pubsub_events.h>
+#include <lib/extensions/amu.h>
+
+#include <plat/common/platform.h>
+
+struct amu_ctx {
+ uint64_t group0_cnts[AMU_GROUP0_MAX_COUNTERS];
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ uint64_t group1_cnts[AMU_GROUP1_MAX_COUNTERS];
+#endif
+
+ uint16_t group0_enable;
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ uint16_t group1_enable;
+#endif
+};
+
+static struct amu_ctx amu_ctxs_[PLATFORM_CORE_COUNT];
+
+CASSERT((sizeof(amu_ctxs_[0].group0_enable) * CHAR_BIT) <= AMU_GROUP0_MAX_COUNTERS,
+ amu_ctx_group0_enable_cannot_represent_all_group0_counters);
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+CASSERT((sizeof(amu_ctxs_[0].group1_enable) * CHAR_BIT) <= AMU_GROUP1_MAX_COUNTERS,
+ amu_ctx_group1_enable_cannot_represent_all_group1_counters);
+#endif
+
+static inline __unused uint32_t read_id_pfr0_amu(void)
+{
+ return (read_id_pfr0() >> ID_PFR0_AMU_SHIFT) &
+ ID_PFR0_AMU_MASK;
+}
+
+static inline __unused void write_hcptr_tam(uint32_t value)
+{
+ write_hcptr((read_hcptr() & ~TAM_BIT) |
+ ((value << TAM_SHIFT) & TAM_BIT));
+}
+
+static inline __unused void write_amcr_cg1rz(uint32_t value)
+{
+ write_amcr((read_amcr() & ~AMCR_CG1RZ_BIT) |
+ ((value << AMCR_CG1RZ_SHIFT) & AMCR_CG1RZ_BIT));
+}
+
+static inline __unused uint32_t read_amcfgr_ncg(void)
+{
+ return (read_amcfgr() >> AMCFGR_NCG_SHIFT) &
+ AMCFGR_NCG_MASK;
+}
+
+static inline __unused uint32_t read_amcgcr_cg0nc(void)
+{
+ return (read_amcgcr() >> AMCGCR_CG0NC_SHIFT) &
+ AMCGCR_CG0NC_MASK;
+}
+
+static inline __unused uint32_t read_amcgcr_cg1nc(void)
+{
+ return (read_amcgcr() >> AMCGCR_CG1NC_SHIFT) &
+ AMCGCR_CG1NC_MASK;
+}
+
+static inline __unused uint32_t read_amcntenset0_px(void)
+{
+ return (read_amcntenset0() >> AMCNTENSET0_Pn_SHIFT) &
+ AMCNTENSET0_Pn_MASK;
+}
+
+static inline __unused uint32_t read_amcntenset1_px(void)
+{
+ return (read_amcntenset1() >> AMCNTENSET1_Pn_SHIFT) &
+ AMCNTENSET1_Pn_MASK;
+}
+
+static inline __unused void write_amcntenset0_px(uint32_t px)
+{
+ uint32_t value = read_amcntenset0();
+
+ value &= ~AMCNTENSET0_Pn_MASK;
+ value |= (px << AMCNTENSET0_Pn_SHIFT) &
+ AMCNTENSET0_Pn_MASK;
+
+ write_amcntenset0(value);
+}
+
+static inline __unused void write_amcntenset1_px(uint32_t px)
+{
+ uint32_t value = read_amcntenset1();
+
+ value &= ~AMCNTENSET1_Pn_MASK;
+ value |= (px << AMCNTENSET1_Pn_SHIFT) &
+ AMCNTENSET1_Pn_MASK;
+
+ write_amcntenset1(value);
+}
+
+static inline __unused void write_amcntenclr0_px(uint32_t px)
+{
+ uint32_t value = read_amcntenclr0();
+
+ value &= ~AMCNTENCLR0_Pn_MASK;
+ value |= (px << AMCNTENCLR0_Pn_SHIFT) & AMCNTENCLR0_Pn_MASK;
+
+ write_amcntenclr0(value);
+}
+
+static inline __unused void write_amcntenclr1_px(uint32_t px)
+{
+ uint32_t value = read_amcntenclr1();
+
+ value &= ~AMCNTENCLR1_Pn_MASK;
+ value |= (px << AMCNTENCLR1_Pn_SHIFT) & AMCNTENCLR1_Pn_MASK;
+
+ write_amcntenclr1(value);
+}
+
+static __unused bool amu_supported(void)
+{
+ return read_id_pfr0_amu() >= ID_PFR0_AMU_V1;
+}
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+static __unused bool amu_group1_supported(void)
+{
+ return read_amcfgr_ncg() > 0U;
+}
+#endif
+
+/*
+ * Enable counters. This function is meant to be invoked by the context
+ * management library before exiting from EL3.
+ */
+void amu_enable(bool el2_unused)
+{
+ uint32_t id_pfr0_amu; /* AMU version */
+
+ uint32_t amcfgr_ncg; /* Number of counter groups */
+ uint32_t amcgcr_cg0nc; /* Number of group 0 counters */
+
+ uint32_t amcntenset0_px = 0x0; /* Group 0 enable mask */
+ uint32_t amcntenset1_px = 0x0; /* Group 1 enable mask */
+
+ id_pfr0_amu = read_id_pfr0_amu();
+ if (id_pfr0_amu == ID_PFR0_AMU_NOT_SUPPORTED) {
+ /*
+ * If the AMU is unsupported, nothing needs to be done.
+ */
+
+ return;
+ }
+
+ if (el2_unused) {
+ /*
+ * HCPTR.TAM: Set to zero so any accesses to the Activity
+ * Monitor registers do not trap to EL2.
+ */
+ write_hcptr_tam(0U);
+ }
+
+ /*
+ * Retrieve the number of architected counters. All of these counters
+ * are enabled by default.
+ */
+
+ amcgcr_cg0nc = read_amcgcr_cg0nc();
+ amcntenset0_px = (UINT32_C(1) << (amcgcr_cg0nc)) - 1U;
+
+ assert(amcgcr_cg0nc <= AMU_AMCGCR_CG0NC_MAX);
+
+ /*
+ * The platform may opt to enable specific auxiliary counters. This can
+ * be done via the common FCONF getter, or via the platform-implemented
+ * function.
+ */
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ const struct amu_topology *topology;
+
+#if ENABLE_AMU_FCONF
+ topology = FCONF_GET_PROPERTY(amu, config, topology);
+#else
+ topology = plat_amu_topology();
+#endif /* ENABLE_AMU_FCONF */
+
+ if (topology != NULL) {
+ unsigned int core_pos = plat_my_core_pos();
+
+ amcntenset1_el0_px = topology->cores[core_pos].enable;
+ } else {
+ ERROR("AMU: failed to generate AMU topology\n");
+ }
+#endif /* ENABLE_AMU_AUXILIARY_COUNTERS */
+
+ /*
+ * Enable the requested counters.
+ */
+
+ write_amcntenset0_px(amcntenset0_px);
+
+ amcfgr_ncg = read_amcfgr_ncg();
+ if (amcfgr_ncg > 0U) {
+ write_amcntenset1_px(amcntenset1_px);
+
+#if !ENABLE_AMU_AUXILIARY_COUNTERS
+ VERBOSE("AMU: auxiliary counters detected but support is disabled\n");
+#endif
+ }
+
+ /* Initialize FEAT_AMUv1p1 features if present. */
+ if (id_pfr0_amu < ID_PFR0_AMU_V1P1) {
+ return;
+ }
+
+#if AMU_RESTRICT_COUNTERS
+ /*
+ * FEAT_AMUv1p1 adds a register field to restrict access to group 1
+ * counters at all but the highest implemented EL. This is controlled
+ * with the AMU_RESTRICT_COUNTERS compile time flag, when set, system
+ * register reads at lower ELs return zero. Reads from the memory
+ * mapped view are unaffected.
+ */
+ VERBOSE("AMU group 1 counter access restricted.\n");
+ write_amcr_cg1rz(1U);
+#else
+ write_amcr_cg1rz(0U);
+#endif
+}
+
+/* Read the group 0 counter identified by the given `idx`. */
+static uint64_t amu_group0_cnt_read(unsigned int idx)
+{
+ assert(amu_supported());
+ assert(idx < read_amcgcr_cg0nc());
+
+ return amu_group0_cnt_read_internal(idx);
+}
+
+/* Write the group 0 counter identified by the given `idx` with `val` */
+static void amu_group0_cnt_write(unsigned int idx, uint64_t val)
+{
+ assert(amu_supported());
+ assert(idx < read_amcgcr_cg0nc());
+
+ amu_group0_cnt_write_internal(idx, val);
+ isb();
+}
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+/* Read the group 1 counter identified by the given `idx` */
+static uint64_t amu_group1_cnt_read(unsigned int idx)
+{
+ assert(amu_supported());
+ assert(amu_group1_supported());
+ assert(idx < read_amcgcr_cg1nc());
+
+ return amu_group1_cnt_read_internal(idx);
+}
+
+/* Write the group 1 counter identified by the given `idx` with `val` */
+static void amu_group1_cnt_write(unsigned int idx, uint64_t val)
+{
+ assert(amu_supported());
+ assert(amu_group1_supported());
+ assert(idx < read_amcgcr_cg1nc());
+
+ amu_group1_cnt_write_internal(idx, val);
+ isb();
+}
+#endif
+
+static void *amu_context_save(const void *arg)
+{
+ uint32_t i;
+
+ unsigned int core_pos;
+ struct amu_ctx *ctx;
+
+ uint32_t id_pfr0_amu; /* AMU version */
+ uint32_t amcgcr_cg0nc; /* Number of group 0 counters */
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ uint32_t amcfgr_ncg; /* Number of counter groups */
+ uint32_t amcgcr_cg1nc; /* Number of group 1 counters */
+#endif
+
+ id_pfr0_amu = read_id_pfr0_amu();
+ if (id_pfr0_amu == ID_PFR0_AMU_NOT_SUPPORTED) {
+ return (void *)0;
+ }
+
+ core_pos = plat_my_core_pos();
+ ctx = &amu_ctxs_[core_pos];
+
+ amcgcr_cg0nc = read_amcgcr_cg0nc();
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ amcfgr_ncg = read_amcfgr_ncg();
+ amcgcr_cg1nc = (amcfgr_ncg > 0U) ? read_amcgcr_cg1nc() : 0U;
+#endif
+
+ /*
+ * Disable all AMU counters.
+ */
+
+ ctx->group0_enable = read_amcntenset0_px();
+ write_amcntenclr0_px(ctx->group0_enable);
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ if (amcfgr_ncg > 0U) {
+ ctx->group1_enable = read_amcntenset1_px();
+ write_amcntenclr1_px(ctx->group1_enable);
+ }
+#endif
+
+ /*
+ * Save the counters to the local context.
+ */
+
+ isb(); /* Ensure counters have been stopped */
+
+ for (i = 0U; i < amcgcr_cg0nc; i++) {
+ ctx->group0_cnts[i] = amu_group0_cnt_read(i);
+ }
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ for (i = 0U; i < amcgcr_cg1nc; i++) {
+ ctx->group1_cnts[i] = amu_group1_cnt_read(i);
+ }
+#endif
+
+ return (void *)0;
+}
+
+static void *amu_context_restore(const void *arg)
+{
+ uint32_t i;
+
+ unsigned int core_pos;
+ struct amu_ctx *ctx;
+
+ uint32_t id_pfr0_amu; /* AMU version */
+
+ uint32_t amcfgr_ncg; /* Number of counter groups */
+ uint32_t amcgcr_cg0nc; /* Number of group 0 counters */
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ uint32_t amcgcr_cg1nc; /* Number of group 1 counters */
+#endif
+
+ id_pfr0_amu = read_id_pfr0_amu();
+ if (id_pfr0_amu == ID_PFR0_AMU_NOT_SUPPORTED) {
+ return (void *)0;
+ }
+
+ core_pos = plat_my_core_pos();
+ ctx = &amu_ctxs_[core_pos];
+
+ amcfgr_ncg = read_amcfgr_ncg();
+ amcgcr_cg0nc = read_amcgcr_cg0nc();
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ amcgcr_cg1nc = (amcfgr_ncg > 0U) ? read_amcgcr_cg1nc() : 0U;
+#endif
+
+ /*
+ * Sanity check that all counters were disabled when the context was
+ * previously saved.
+ */
+
+ assert(read_amcntenset0_px() == 0U);
+
+ if (amcfgr_ncg > 0U) {
+ assert(read_amcntenset1_px() == 0U);
+ }
+
+ /*
+ * Restore the counter values from the local context.
+ */
+
+ for (i = 0U; i < amcgcr_cg0nc; i++) {
+ amu_group0_cnt_write(i, ctx->group0_cnts[i]);
+ }
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ for (i = 0U; i < amcgcr_cg1nc; i++) {
+ amu_group1_cnt_write(i, ctx->group1_cnts[i]);
+ }
+#endif
+
+ /*
+ * Re-enable counters that were disabled during context save.
+ */
+
+ write_amcntenset0_px(ctx->group0_enable);
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ if (amcfgr_ncg > 0U) {
+ write_amcntenset1_px(ctx->group1_enable);
+ }
+#endif
+
+ return (void *)0;
+}
+
+SUBSCRIBE_TO_EVENT(psci_suspend_pwrdown_start, amu_context_save);
+SUBSCRIBE_TO_EVENT(psci_suspend_pwrdown_finish, amu_context_restore);
diff --git a/lib/extensions/amu/aarch32/amu_helpers.S b/lib/extensions/amu/aarch32/amu_helpers.S
new file mode 100644
index 0000000..8ac7678
--- /dev/null
+++ b/lib/extensions/amu/aarch32/amu_helpers.S
@@ -0,0 +1,271 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <assert_macros.S>
+#include <asm_macros.S>
+
+ .globl amu_group0_cnt_read_internal
+ .globl amu_group0_cnt_write_internal
+ .globl amu_group1_cnt_read_internal
+ .globl amu_group1_cnt_write_internal
+ .globl amu_group1_set_evtype_internal
+
+/*
+ * uint64_t amu_group0_cnt_read_internal(int idx);
+ *
+ * Given `idx`, read the corresponding AMU counter
+ * and return it in `r0` and `r1`.
+ */
+func amu_group0_cnt_read_internal
+#if ENABLE_ASSERTIONS
+ /* `idx` should be between [0, 3] */
+ mov r1, r0
+ lsr r1, r1, #2
+ cmp r1, #0
+ ASM_ASSERT(eq)
+#endif
+
+ /*
+ * Given `idx` calculate address of ldcopr16/bx lr instruction pair
+ * in the table below.
+ */
+ adr r1, 1f
+ lsl r0, r0, #3 /* each ldcopr16/bx lr sequence is 8 bytes */
+ add r1, r1, r0
+ bx r1
+1:
+ ldcopr16 r0, r1, AMEVCNTR00 /* index 0 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR01 /* index 1 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR02 /* index 2 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR03 /* index 3 */
+ bx lr
+endfunc amu_group0_cnt_read_internal
+
+/*
+ * void amu_group0_cnt_write_internal(int idx, uint64_t val);
+ *
+ * Given `idx`, write `val` to the corresponding AMU counter.
+ * `idx` is passed in `r0` and `val` is passed in `r2` and `r3`.
+ * `r1` is used as a scratch register.
+ */
+func amu_group0_cnt_write_internal
+#if ENABLE_ASSERTIONS
+ /* `idx` should be between [0, 3] */
+ mov r1, r0
+ lsr r1, r1, #2
+ cmp r1, #0
+ ASM_ASSERT(eq)
+#endif
+
+ /*
+ * Given `idx` calculate address of stcopr16/bx lr instruction pair
+ * in the table below.
+ */
+ adr r1, 1f
+ lsl r0, r0, #3 /* each stcopr16/bx lr sequence is 8 bytes */
+ add r1, r1, r0
+ bx r1
+
+1:
+ stcopr16 r2, r3, AMEVCNTR00 /* index 0 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR01 /* index 1 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR02 /* index 2 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR03 /* index 3 */
+ bx lr
+endfunc amu_group0_cnt_write_internal
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+/*
+ * uint64_t amu_group1_cnt_read_internal(int idx);
+ *
+ * Given `idx`, read the corresponding AMU counter
+ * and return it in `r0` and `r1`.
+ */
+func amu_group1_cnt_read_internal
+#if ENABLE_ASSERTIONS
+ /* `idx` should be between [0, 15] */
+ mov r1, r0
+ lsr r1, r1, #4
+ cmp r1, #0
+ ASM_ASSERT(eq)
+#endif
+
+ /*
+ * Given `idx` calculate address of ldcopr16/bx lr instruction pair
+ * in the table below.
+ */
+ adr r1, 1f
+ lsl r0, r0, #3 /* each ldcopr16/bx lr sequence is 8 bytes */
+ add r1, r1, r0
+ bx r1
+
+1:
+ ldcopr16 r0, r1, AMEVCNTR10 /* index 0 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR11 /* index 1 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR12 /* index 2 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR13 /* index 3 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR14 /* index 4 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR15 /* index 5 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR16 /* index 6 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR17 /* index 7 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR18 /* index 8 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR19 /* index 9 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR1A /* index 10 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR1B /* index 11 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR1C /* index 12 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR1D /* index 13 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR1E /* index 14 */
+ bx lr
+ ldcopr16 r0, r1, AMEVCNTR1F /* index 15 */
+ bx lr
+endfunc amu_group1_cnt_read_internal
+
+/*
+ * void amu_group1_cnt_write_internal(int idx, uint64_t val);
+ *
+ * Given `idx`, write `val` to the corresponding AMU counter.
+ * `idx` is passed in `r0` and `val` is passed in `r2` and `r3`.
+ * `r1` is used as a scratch register.
+ */
+func amu_group1_cnt_write_internal
+#if ENABLE_ASSERTIONS
+ /* `idx` should be between [0, 15] */
+ mov r1, r0
+ lsr r1, r1, #4
+ cmp r1, #0
+ ASM_ASSERT(eq)
+#endif
+
+ /*
+ * Given `idx` calculate address of ldcopr16/bx lr instruction pair
+ * in the table below.
+ */
+ adr r1, 1f
+ lsl r0, r0, #3 /* each stcopr16/bx lr sequence is 8 bytes */
+ add r1, r1, r0
+ bx r1
+
+1:
+ stcopr16 r2, r3, AMEVCNTR10 /* index 0 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR11 /* index 1 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR12 /* index 2 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR13 /* index 3 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR14 /* index 4 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR15 /* index 5 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR16 /* index 6 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR17 /* index 7 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR18 /* index 8 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR19 /* index 9 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR1A /* index 10 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR1B /* index 11 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR1C /* index 12 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR1D /* index 13 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR1E /* index 14 */
+ bx lr
+ stcopr16 r2, r3, AMEVCNTR1F /* index 15 */
+ bx lr
+endfunc amu_group1_cnt_write_internal
+
+/*
+ * void amu_group1_set_evtype_internal(int idx, unsigned int val);
+ *
+ * Program the AMU event type register indexed by `idx`
+ * with the value `val`.
+ */
+func amu_group1_set_evtype_internal
+#if ENABLE_ASSERTIONS
+ /* `idx` should be between [0, 15] */
+ mov r2, r0
+ lsr r2, r2, #4
+ cmp r2, #0
+ ASM_ASSERT(eq)
+
+ /* val should be between [0, 65535] */
+ mov r2, r1
+ lsr r2, r2, #16
+ cmp r2, #0
+ ASM_ASSERT(eq)
+#endif
+
+ /*
+ * Given `idx` calculate address of stcopr/bx lr instruction pair
+ * in the table below.
+ */
+ adr r2, 1f
+ lsl r0, r0, #3 /* each stcopr/bx lr sequence is 8 bytes */
+ add r2, r2, r0
+ bx r2
+
+1:
+ stcopr r1, AMEVTYPER10 /* index 0 */
+ bx lr
+ stcopr r1, AMEVTYPER11 /* index 1 */
+ bx lr
+ stcopr r1, AMEVTYPER12 /* index 2 */
+ bx lr
+ stcopr r1, AMEVTYPER13 /* index 3 */
+ bx lr
+ stcopr r1, AMEVTYPER14 /* index 4 */
+ bx lr
+ stcopr r1, AMEVTYPER15 /* index 5 */
+ bx lr
+ stcopr r1, AMEVTYPER16 /* index 6 */
+ bx lr
+ stcopr r1, AMEVTYPER17 /* index 7 */
+ bx lr
+ stcopr r1, AMEVTYPER18 /* index 8 */
+ bx lr
+ stcopr r1, AMEVTYPER19 /* index 9 */
+ bx lr
+ stcopr r1, AMEVTYPER1A /* index 10 */
+ bx lr
+ stcopr r1, AMEVTYPER1B /* index 11 */
+ bx lr
+ stcopr r1, AMEVTYPER1C /* index 12 */
+ bx lr
+ stcopr r1, AMEVTYPER1D /* index 13 */
+ bx lr
+ stcopr r1, AMEVTYPER1E /* index 14 */
+ bx lr
+ stcopr r1, AMEVTYPER1F /* index 15 */
+ bx lr
+endfunc amu_group1_set_evtype_internal
+#endif
diff --git a/lib/extensions/amu/aarch64/amu.c b/lib/extensions/amu/aarch64/amu.c
new file mode 100644
index 0000000..72566fd
--- /dev/null
+++ b/lib/extensions/amu/aarch64/amu.c
@@ -0,0 +1,655 @@
+/*
+ * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <cdefs.h>
+#include <inttypes.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "../amu_private.h"
+#include <arch.h>
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/el3_runtime/pubsub_events.h>
+#include <lib/extensions/amu.h>
+
+#include <plat/common/platform.h>
+
+#if ENABLE_AMU_FCONF
+# include <lib/fconf/fconf.h>
+# include <lib/fconf/fconf_amu_getter.h>
+#endif
+
+#if ENABLE_MPMM
+# include <lib/mpmm/mpmm.h>
+#endif
+
+struct amu_ctx {
+ uint64_t group0_cnts[AMU_GROUP0_MAX_COUNTERS];
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ uint64_t group1_cnts[AMU_GROUP1_MAX_COUNTERS];
+#endif
+
+ /* Architected event counter 1 does not have an offset register */
+ uint64_t group0_voffsets[AMU_GROUP0_MAX_COUNTERS - 1U];
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ uint64_t group1_voffsets[AMU_GROUP1_MAX_COUNTERS];
+#endif
+
+ uint16_t group0_enable;
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ uint16_t group1_enable;
+#endif
+};
+
+static struct amu_ctx amu_ctxs_[PLATFORM_CORE_COUNT];
+
+CASSERT((sizeof(amu_ctxs_[0].group0_enable) * CHAR_BIT) <= AMU_GROUP0_MAX_COUNTERS,
+ amu_ctx_group0_enable_cannot_represent_all_group0_counters);
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+CASSERT((sizeof(amu_ctxs_[0].group1_enable) * CHAR_BIT) <= AMU_GROUP1_MAX_COUNTERS,
+ amu_ctx_group1_enable_cannot_represent_all_group1_counters);
+#endif
+
+static inline __unused uint64_t read_id_aa64pfr0_el1_amu(void)
+{
+ return (read_id_aa64pfr0_el1() >> ID_AA64PFR0_AMU_SHIFT) &
+ ID_AA64PFR0_AMU_MASK;
+}
+
+static inline __unused uint64_t read_hcr_el2_amvoffen(void)
+{
+ return (read_hcr_el2() & HCR_AMVOFFEN_BIT) >>
+ HCR_AMVOFFEN_SHIFT;
+}
+
+static inline __unused void write_cptr_el2_tam(uint64_t value)
+{
+ write_cptr_el2((read_cptr_el2() & ~CPTR_EL2_TAM_BIT) |
+ ((value << CPTR_EL2_TAM_SHIFT) & CPTR_EL2_TAM_BIT));
+}
+
+static inline __unused void ctx_write_cptr_el3_tam(cpu_context_t *ctx, uint64_t tam)
+{
+ uint64_t value = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
+
+ value &= ~TAM_BIT;
+ value |= (tam << TAM_SHIFT) & TAM_BIT;
+
+ write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, value);
+}
+
+static inline __unused void ctx_write_scr_el3_amvoffen(cpu_context_t *ctx, uint64_t amvoffen)
+{
+ uint64_t value = read_ctx_reg(get_el3state_ctx(ctx), CTX_SCR_EL3);
+
+ value &= ~SCR_AMVOFFEN_BIT;
+ value |= (amvoffen << SCR_AMVOFFEN_SHIFT) & SCR_AMVOFFEN_BIT;
+
+ write_ctx_reg(get_el3state_ctx(ctx), CTX_SCR_EL3, value);
+}
+
+static inline __unused void write_hcr_el2_amvoffen(uint64_t value)
+{
+ write_hcr_el2((read_hcr_el2() & ~HCR_AMVOFFEN_BIT) |
+ ((value << HCR_AMVOFFEN_SHIFT) & HCR_AMVOFFEN_BIT));
+}
+
+static inline __unused void write_amcr_el0_cg1rz(uint64_t value)
+{
+ write_amcr_el0((read_amcr_el0() & ~AMCR_CG1RZ_BIT) |
+ ((value << AMCR_CG1RZ_SHIFT) & AMCR_CG1RZ_BIT));
+}
+
+static inline __unused uint64_t read_amcfgr_el0_ncg(void)
+{
+ return (read_amcfgr_el0() >> AMCFGR_EL0_NCG_SHIFT) &
+ AMCFGR_EL0_NCG_MASK;
+}
+
+static inline __unused uint64_t read_amcgcr_el0_cg0nc(void)
+{
+ return (read_amcgcr_el0() >> AMCGCR_EL0_CG0NC_SHIFT) &
+ AMCGCR_EL0_CG0NC_MASK;
+}
+
+static inline __unused uint64_t read_amcg1idr_el0_voff(void)
+{
+ return (read_amcg1idr_el0() >> AMCG1IDR_VOFF_SHIFT) &
+ AMCG1IDR_VOFF_MASK;
+}
+
+static inline __unused uint64_t read_amcgcr_el0_cg1nc(void)
+{
+ return (read_amcgcr_el0() >> AMCGCR_EL0_CG1NC_SHIFT) &
+ AMCGCR_EL0_CG1NC_MASK;
+}
+
+static inline __unused uint64_t read_amcntenset0_el0_px(void)
+{
+ return (read_amcntenset0_el0() >> AMCNTENSET0_EL0_Pn_SHIFT) &
+ AMCNTENSET0_EL0_Pn_MASK;
+}
+
+static inline __unused uint64_t read_amcntenset1_el0_px(void)
+{
+ return (read_amcntenset1_el0() >> AMCNTENSET1_EL0_Pn_SHIFT) &
+ AMCNTENSET1_EL0_Pn_MASK;
+}
+
+static inline __unused void write_amcntenset0_el0_px(uint64_t px)
+{
+ uint64_t value = read_amcntenset0_el0();
+
+ value &= ~AMCNTENSET0_EL0_Pn_MASK;
+ value |= (px << AMCNTENSET0_EL0_Pn_SHIFT) & AMCNTENSET0_EL0_Pn_MASK;
+
+ write_amcntenset0_el0(value);
+}
+
+static inline __unused void write_amcntenset1_el0_px(uint64_t px)
+{
+ uint64_t value = read_amcntenset1_el0();
+
+ value &= ~AMCNTENSET1_EL0_Pn_MASK;
+ value |= (px << AMCNTENSET1_EL0_Pn_SHIFT) & AMCNTENSET1_EL0_Pn_MASK;
+
+ write_amcntenset1_el0(value);
+}
+
+static inline __unused void write_amcntenclr0_el0_px(uint64_t px)
+{
+ uint64_t value = read_amcntenclr0_el0();
+
+ value &= ~AMCNTENCLR0_EL0_Pn_MASK;
+ value |= (px << AMCNTENCLR0_EL0_Pn_SHIFT) & AMCNTENCLR0_EL0_Pn_MASK;
+
+ write_amcntenclr0_el0(value);
+}
+
+static inline __unused void write_amcntenclr1_el0_px(uint64_t px)
+{
+ uint64_t value = read_amcntenclr1_el0();
+
+ value &= ~AMCNTENCLR1_EL0_Pn_MASK;
+ value |= (px << AMCNTENCLR1_EL0_Pn_SHIFT) & AMCNTENCLR1_EL0_Pn_MASK;
+
+ write_amcntenclr1_el0(value);
+}
+
+static __unused bool amu_supported(void)
+{
+ return read_id_aa64pfr0_el1_amu() >= ID_AA64PFR0_AMU_V1;
+}
+
+static __unused bool amu_v1p1_supported(void)
+{
+ return read_id_aa64pfr0_el1_amu() >= ID_AA64PFR0_AMU_V1P1;
+}
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+static __unused bool amu_group1_supported(void)
+{
+ return read_amcfgr_el0_ncg() > 0U;
+}
+#endif
+
+/*
+ * Enable counters. This function is meant to be invoked by the context
+ * management library before exiting from EL3.
+ */
+void amu_enable(bool el2_unused, cpu_context_t *ctx)
+{
+ uint64_t id_aa64pfr0_el1_amu; /* AMU version */
+
+ uint64_t amcfgr_el0_ncg; /* Number of counter groups */
+ uint64_t amcgcr_el0_cg0nc; /* Number of group 0 counters */
+
+ uint64_t amcntenset0_el0_px = 0x0; /* Group 0 enable mask */
+ uint64_t amcntenset1_el0_px = 0x0; /* Group 1 enable mask */
+
+ id_aa64pfr0_el1_amu = read_id_aa64pfr0_el1_amu();
+ if (id_aa64pfr0_el1_amu == ID_AA64PFR0_AMU_NOT_SUPPORTED) {
+ /*
+ * If the AMU is unsupported, nothing needs to be done.
+ */
+
+ return;
+ }
+
+ if (el2_unused) {
+ /*
+ * CPTR_EL2.TAM: Set to zero so any accesses to the Activity
+ * Monitor registers do not trap to EL2.
+ */
+ write_cptr_el2_tam(0U);
+ }
+
+ /*
+ * Retrieve and update the CPTR_EL3 value from the context mentioned
+ * in 'ctx'. Set CPTR_EL3.TAM to zero so that any accesses to
+ * the Activity Monitor registers do not trap to EL3.
+ */
+ ctx_write_cptr_el3_tam(ctx, 0U);
+
+ /*
+ * Retrieve the number of architected counters. All of these counters
+ * are enabled by default.
+ */
+
+ amcgcr_el0_cg0nc = read_amcgcr_el0_cg0nc();
+ amcntenset0_el0_px = (UINT64_C(1) << (amcgcr_el0_cg0nc)) - 1U;
+
+ assert(amcgcr_el0_cg0nc <= AMU_AMCGCR_CG0NC_MAX);
+
+ /*
+ * The platform may opt to enable specific auxiliary counters. This can
+ * be done via the common FCONF getter, or via the platform-implemented
+ * function.
+ */
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ const struct amu_topology *topology;
+
+#if ENABLE_AMU_FCONF
+ topology = FCONF_GET_PROPERTY(amu, config, topology);
+#else
+ topology = plat_amu_topology();
+#endif /* ENABLE_AMU_FCONF */
+
+ if (topology != NULL) {
+ unsigned int core_pos = plat_my_core_pos();
+
+ amcntenset1_el0_px = topology->cores[core_pos].enable;
+ } else {
+ ERROR("AMU: failed to generate AMU topology\n");
+ }
+#endif /* ENABLE_AMU_AUXILIARY_COUNTERS */
+
+ /*
+ * Enable the requested counters.
+ */
+
+ write_amcntenset0_el0_px(amcntenset0_el0_px);
+
+ amcfgr_el0_ncg = read_amcfgr_el0_ncg();
+ if (amcfgr_el0_ncg > 0U) {
+ write_amcntenset1_el0_px(amcntenset1_el0_px);
+
+#if !ENABLE_AMU_AUXILIARY_COUNTERS
+ VERBOSE("AMU: auxiliary counters detected but support is disabled\n");
+#endif
+ }
+
+ /* Initialize FEAT_AMUv1p1 features if present. */
+ if (id_aa64pfr0_el1_amu >= ID_AA64PFR0_AMU_V1P1) {
+ if (el2_unused) {
+ /*
+ * Make sure virtual offsets are disabled if EL2 not
+ * used.
+ */
+ write_hcr_el2_amvoffen(0U);
+ } else {
+ /*
+ * Virtual offset registers are only accessible from EL3
+ * and EL2, when clear, this bit traps accesses from EL2
+ * so we set it to 1 when EL2 is present.
+ */
+ ctx_write_scr_el3_amvoffen(ctx, 1U);
+ }
+
+#if AMU_RESTRICT_COUNTERS
+ /*
+ * FEAT_AMUv1p1 adds a register field to restrict access to
+ * group 1 counters at all but the highest implemented EL. This
+ * is controlled with the `AMU_RESTRICT_COUNTERS` compile time
+ * flag, when set, system register reads at lower ELs return
+ * zero. Reads from the memory mapped view are unaffected.
+ */
+ VERBOSE("AMU group 1 counter access restricted.\n");
+ write_amcr_el0_cg1rz(1U);
+#else
+ write_amcr_el0_cg1rz(0U);
+#endif
+ }
+
+#if ENABLE_MPMM
+ mpmm_enable();
+#endif
+}
+
+/* Read the group 0 counter identified by the given `idx`. */
+static uint64_t amu_group0_cnt_read(unsigned int idx)
+{
+ assert(amu_supported());
+ assert(idx < read_amcgcr_el0_cg0nc());
+
+ return amu_group0_cnt_read_internal(idx);
+}
+
+/* Write the group 0 counter identified by the given `idx` with `val` */
+static void amu_group0_cnt_write(unsigned int idx, uint64_t val)
+{
+ assert(amu_supported());
+ assert(idx < read_amcgcr_el0_cg0nc());
+
+ amu_group0_cnt_write_internal(idx, val);
+ isb();
+}
+
+/*
+ * Unlike with auxiliary counters, we cannot detect at runtime whether an
+ * architected counter supports a virtual offset. These are instead fixed
+ * according to FEAT_AMUv1p1, but this switch will need to be updated if later
+ * revisions of FEAT_AMU add additional architected counters.
+ */
+static bool amu_group0_voffset_supported(uint64_t idx)
+{
+ switch (idx) {
+ case 0U:
+ case 2U:
+ case 3U:
+ return true;
+
+ case 1U:
+ return false;
+
+ default:
+ ERROR("AMU: can't set up virtual offset for unknown "
+ "architected counter %" PRIu64 "!\n", idx);
+
+ panic();
+ }
+}
+
+/*
+ * Read the group 0 offset register for a given index. Index must be 0, 2,
+ * or 3, the register for 1 does not exist.
+ *
+ * Using this function requires FEAT_AMUv1p1 support.
+ */
+static uint64_t amu_group0_voffset_read(unsigned int idx)
+{
+ assert(amu_v1p1_supported());
+ assert(idx < read_amcgcr_el0_cg0nc());
+ assert(idx != 1U);
+
+ return amu_group0_voffset_read_internal(idx);
+}
+
+/*
+ * Write the group 0 offset register for a given index. Index must be 0, 2, or
+ * 3, the register for 1 does not exist.
+ *
+ * Using this function requires FEAT_AMUv1p1 support.
+ */
+static void amu_group0_voffset_write(unsigned int idx, uint64_t val)
+{
+ assert(amu_v1p1_supported());
+ assert(idx < read_amcgcr_el0_cg0nc());
+ assert(idx != 1U);
+
+ amu_group0_voffset_write_internal(idx, val);
+ isb();
+}
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+/* Read the group 1 counter identified by the given `idx` */
+static uint64_t amu_group1_cnt_read(unsigned int idx)
+{
+ assert(amu_supported());
+ assert(amu_group1_supported());
+ assert(idx < read_amcgcr_el0_cg1nc());
+
+ return amu_group1_cnt_read_internal(idx);
+}
+
+/* Write the group 1 counter identified by the given `idx` with `val` */
+static void amu_group1_cnt_write(unsigned int idx, uint64_t val)
+{
+ assert(amu_supported());
+ assert(amu_group1_supported());
+ assert(idx < read_amcgcr_el0_cg1nc());
+
+ amu_group1_cnt_write_internal(idx, val);
+ isb();
+}
+
+/*
+ * Read the group 1 offset register for a given index.
+ *
+ * Using this function requires FEAT_AMUv1p1 support.
+ */
+static uint64_t amu_group1_voffset_read(unsigned int idx)
+{
+ assert(amu_v1p1_supported());
+ assert(amu_group1_supported());
+ assert(idx < read_amcgcr_el0_cg1nc());
+ assert((read_amcg1idr_el0_voff() & (UINT64_C(1) << idx)) != 0U);
+
+ return amu_group1_voffset_read_internal(idx);
+}
+
+/*
+ * Write the group 1 offset register for a given index.
+ *
+ * Using this function requires FEAT_AMUv1p1 support.
+ */
+static void amu_group1_voffset_write(unsigned int idx, uint64_t val)
+{
+ assert(amu_v1p1_supported());
+ assert(amu_group1_supported());
+ assert(idx < read_amcgcr_el0_cg1nc());
+ assert((read_amcg1idr_el0_voff() & (UINT64_C(1) << idx)) != 0U);
+
+ amu_group1_voffset_write_internal(idx, val);
+ isb();
+}
+#endif
+
+static void *amu_context_save(const void *arg)
+{
+ uint64_t i, j;
+
+ unsigned int core_pos;
+ struct amu_ctx *ctx;
+
+ uint64_t id_aa64pfr0_el1_amu; /* AMU version */
+ uint64_t hcr_el2_amvoffen; /* AMU virtual offsets enabled */
+ uint64_t amcgcr_el0_cg0nc; /* Number of group 0 counters */
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ uint64_t amcg1idr_el0_voff; /* Auxiliary counters with virtual offsets */
+ uint64_t amcfgr_el0_ncg; /* Number of counter groups */
+ uint64_t amcgcr_el0_cg1nc; /* Number of group 1 counters */
+#endif
+
+ id_aa64pfr0_el1_amu = read_id_aa64pfr0_el1_amu();
+ if (id_aa64pfr0_el1_amu == ID_AA64PFR0_AMU_NOT_SUPPORTED) {
+ return (void *)0;
+ }
+
+ core_pos = plat_my_core_pos();
+ ctx = &amu_ctxs_[core_pos];
+
+ amcgcr_el0_cg0nc = read_amcgcr_el0_cg0nc();
+ hcr_el2_amvoffen = (id_aa64pfr0_el1_amu >= ID_AA64PFR0_AMU_V1P1) ?
+ read_hcr_el2_amvoffen() : 0U;
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ amcfgr_el0_ncg = read_amcfgr_el0_ncg();
+ amcgcr_el0_cg1nc = (amcfgr_el0_ncg > 0U) ? read_amcgcr_el0_cg1nc() : 0U;
+ amcg1idr_el0_voff = (hcr_el2_amvoffen != 0U) ? read_amcg1idr_el0_voff() : 0U;
+#endif
+
+ /*
+ * Disable all AMU counters.
+ */
+
+ ctx->group0_enable = read_amcntenset0_el0_px();
+ write_amcntenclr0_el0_px(ctx->group0_enable);
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ if (amcfgr_el0_ncg > 0U) {
+ ctx->group1_enable = read_amcntenset1_el0_px();
+ write_amcntenclr1_el0_px(ctx->group1_enable);
+ }
+#endif
+
+ /*
+ * Save the counters to the local context.
+ */
+
+ isb(); /* Ensure counters have been stopped */
+
+ for (i = 0U; i < amcgcr_el0_cg0nc; i++) {
+ ctx->group0_cnts[i] = amu_group0_cnt_read(i);
+ }
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ for (i = 0U; i < amcgcr_el0_cg1nc; i++) {
+ ctx->group1_cnts[i] = amu_group1_cnt_read(i);
+ }
+#endif
+
+ /*
+ * Save virtual offsets for counters that offer them.
+ */
+
+ if (hcr_el2_amvoffen != 0U) {
+ for (i = 0U, j = 0U; i < amcgcr_el0_cg0nc; i++) {
+ if (!amu_group0_voffset_supported(i)) {
+ continue; /* No virtual offset */
+ }
+
+ ctx->group0_voffsets[j++] = amu_group0_voffset_read(i);
+ }
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ for (i = 0U, j = 0U; i < amcgcr_el0_cg1nc; i++) {
+ if ((amcg1idr_el0_voff >> i) & 1U) {
+ continue; /* No virtual offset */
+ }
+
+ ctx->group1_voffsets[j++] = amu_group1_voffset_read(i);
+ }
+#endif
+ }
+
+ return (void *)0;
+}
+
+static void *amu_context_restore(const void *arg)
+{
+ uint64_t i, j;
+
+ unsigned int core_pos;
+ struct amu_ctx *ctx;
+
+ uint64_t id_aa64pfr0_el1_amu; /* AMU version */
+
+ uint64_t hcr_el2_amvoffen; /* AMU virtual offsets enabled */
+
+ uint64_t amcfgr_el0_ncg; /* Number of counter groups */
+ uint64_t amcgcr_el0_cg0nc; /* Number of group 0 counters */
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ uint64_t amcgcr_el0_cg1nc; /* Number of group 1 counters */
+ uint64_t amcg1idr_el0_voff; /* Auxiliary counters with virtual offsets */
+#endif
+
+ id_aa64pfr0_el1_amu = read_id_aa64pfr0_el1_amu();
+ if (id_aa64pfr0_el1_amu == ID_AA64PFR0_AMU_NOT_SUPPORTED) {
+ return (void *)0;
+ }
+
+ core_pos = plat_my_core_pos();
+ ctx = &amu_ctxs_[core_pos];
+
+ amcfgr_el0_ncg = read_amcfgr_el0_ncg();
+ amcgcr_el0_cg0nc = read_amcgcr_el0_cg0nc();
+
+ hcr_el2_amvoffen = (id_aa64pfr0_el1_amu >= ID_AA64PFR0_AMU_V1P1) ?
+ read_hcr_el2_amvoffen() : 0U;
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ amcgcr_el0_cg1nc = (amcfgr_el0_ncg > 0U) ? read_amcgcr_el0_cg1nc() : 0U;
+ amcg1idr_el0_voff = (hcr_el2_amvoffen != 0U) ? read_amcg1idr_el0_voff() : 0U;
+#endif
+
+ /*
+ * Sanity check that all counters were disabled when the context was
+ * previously saved.
+ */
+
+ assert(read_amcntenset0_el0_px() == 0U);
+
+ if (amcfgr_el0_ncg > 0U) {
+ assert(read_amcntenset1_el0_px() == 0U);
+ }
+
+ /*
+ * Restore the counter values from the local context.
+ */
+
+ for (i = 0U; i < amcgcr_el0_cg0nc; i++) {
+ amu_group0_cnt_write(i, ctx->group0_cnts[i]);
+ }
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ for (i = 0U; i < amcgcr_el0_cg1nc; i++) {
+ amu_group1_cnt_write(i, ctx->group1_cnts[i]);
+ }
+#endif
+
+ /*
+ * Restore virtual offsets for counters that offer them.
+ */
+
+ if (hcr_el2_amvoffen != 0U) {
+ for (i = 0U, j = 0U; i < amcgcr_el0_cg0nc; i++) {
+ if (!amu_group0_voffset_supported(i)) {
+ continue; /* No virtual offset */
+ }
+
+ amu_group0_voffset_write(i, ctx->group0_voffsets[j++]);
+ }
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ for (i = 0U, j = 0U; i < amcgcr_el0_cg1nc; i++) {
+ if ((amcg1idr_el0_voff >> i) & 1U) {
+ continue; /* No virtual offset */
+ }
+
+ amu_group1_voffset_write(i, ctx->group1_voffsets[j++]);
+ }
+#endif
+ }
+
+ /*
+ * Re-enable counters that were disabled during context save.
+ */
+
+ write_amcntenset0_el0_px(ctx->group0_enable);
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+ if (amcfgr_el0_ncg > 0) {
+ write_amcntenset1_el0_px(ctx->group1_enable);
+ }
+#endif
+
+#if ENABLE_MPMM
+ mpmm_enable();
+#endif
+
+ return (void *)0;
+}
+
+SUBSCRIBE_TO_EVENT(psci_suspend_pwrdown_start, amu_context_save);
+SUBSCRIBE_TO_EVENT(psci_suspend_pwrdown_finish, amu_context_restore);
diff --git a/lib/extensions/amu/aarch64/amu_helpers.S b/lib/extensions/amu/aarch64/amu_helpers.S
new file mode 100644
index 0000000..0f6d799
--- /dev/null
+++ b/lib/extensions/amu/aarch64/amu_helpers.S
@@ -0,0 +1,389 @@
+/*
+ * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <assert_macros.S>
+#include <asm_macros.S>
+
+ .globl amu_group0_cnt_read_internal
+ .globl amu_group0_cnt_write_internal
+ .globl amu_group1_cnt_read_internal
+ .globl amu_group1_cnt_write_internal
+ .globl amu_group1_set_evtype_internal
+
+ /* FEAT_AMUv1p1 virtualisation offset register functions */
+ .globl amu_group0_voffset_read_internal
+ .globl amu_group0_voffset_write_internal
+ .globl amu_group1_voffset_read_internal
+ .globl amu_group1_voffset_write_internal
+
+/*
+ * uint64_t amu_group0_cnt_read_internal(int idx);
+ *
+ * Given `idx`, read the corresponding AMU counter
+ * and return it in `x0`.
+ */
+func amu_group0_cnt_read_internal
+ adr x1, 1f
+#if ENABLE_ASSERTIONS
+ /*
+ * It can be dangerous to call this function with an
+ * out of bounds index. Ensure `idx` is valid.
+ */
+ tst x0, #~3
+ ASM_ASSERT(eq)
+#endif
+ /*
+ * Given `idx` calculate address of mrs/ret instruction pair
+ * in the table below.
+ */
+ add x1, x1, x0, lsl #3 /* each mrs/ret sequence is 8 bytes */
+#if ENABLE_BTI
+ add x1, x1, x0, lsl #2 /* + "bti j" instruction */
+#endif
+ br x1
+
+1: read AMEVCNTR00_EL0 /* index 0 */
+ read AMEVCNTR01_EL0 /* index 1 */
+ read AMEVCNTR02_EL0 /* index 2 */
+ read AMEVCNTR03_EL0 /* index 3 */
+endfunc amu_group0_cnt_read_internal
+
+/*
+ * void amu_group0_cnt_write_internal(int idx, uint64_t val);
+ *
+ * Given `idx`, write `val` to the corresponding AMU counter.
+ */
+func amu_group0_cnt_write_internal
+ adr x2, 1f
+#if ENABLE_ASSERTIONS
+ /*
+ * It can be dangerous to call this function with an
+ * out of bounds index. Ensure `idx` is valid.
+ */
+ tst x0, #~3
+ ASM_ASSERT(eq)
+#endif
+ /*
+ * Given `idx` calculate address of mrs/ret instruction pair
+ * in the table below.
+ */
+ add x2, x2, x0, lsl #3 /* each msr/ret sequence is 8 bytes */
+#if ENABLE_BTI
+ add x2, x2, x0, lsl #2 /* + "bti j" instruction */
+#endif
+ br x2
+
+1: write AMEVCNTR00_EL0 /* index 0 */
+ write AMEVCNTR01_EL0 /* index 1 */
+ write AMEVCNTR02_EL0 /* index 2 */
+ write AMEVCNTR03_EL0 /* index 3 */
+endfunc amu_group0_cnt_write_internal
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+/*
+ * uint64_t amu_group1_cnt_read_internal(int idx);
+ *
+ * Given `idx`, read the corresponding AMU counter
+ * and return it in `x0`.
+ */
+func amu_group1_cnt_read_internal
+ adr x1, 1f
+#if ENABLE_ASSERTIONS
+ /*
+ * It can be dangerous to call this function with an
+ * out of bounds index. Ensure `idx` is valid.
+ */
+ tst x0, #~0xF
+ ASM_ASSERT(eq)
+#endif
+ /*
+ * Given `idx` calculate address of mrs/ret instruction pair
+ * in the table below.
+ */
+ add x1, x1, x0, lsl #3 /* each mrs/ret sequence is 8 bytes */
+#if ENABLE_BTI
+ add x1, x1, x0, lsl #2 /* + "bti j" instruction */
+#endif
+ br x1
+
+1: read AMEVCNTR10_EL0 /* index 0 */
+ read AMEVCNTR11_EL0 /* index 1 */
+ read AMEVCNTR12_EL0 /* index 2 */
+ read AMEVCNTR13_EL0 /* index 3 */
+ read AMEVCNTR14_EL0 /* index 4 */
+ read AMEVCNTR15_EL0 /* index 5 */
+ read AMEVCNTR16_EL0 /* index 6 */
+ read AMEVCNTR17_EL0 /* index 7 */
+ read AMEVCNTR18_EL0 /* index 8 */
+ read AMEVCNTR19_EL0 /* index 9 */
+ read AMEVCNTR1A_EL0 /* index 10 */
+ read AMEVCNTR1B_EL0 /* index 11 */
+ read AMEVCNTR1C_EL0 /* index 12 */
+ read AMEVCNTR1D_EL0 /* index 13 */
+ read AMEVCNTR1E_EL0 /* index 14 */
+ read AMEVCNTR1F_EL0 /* index 15 */
+endfunc amu_group1_cnt_read_internal
+
+/*
+ * void amu_group1_cnt_write_internal(int idx, uint64_t val);
+ *
+ * Given `idx`, write `val` to the corresponding AMU counter.
+ */
+func amu_group1_cnt_write_internal
+ adr x2, 1f
+#if ENABLE_ASSERTIONS
+ /*
+ * It can be dangerous to call this function with an
+ * out of bounds index. Ensure `idx` is valid.
+ */
+ tst x0, #~0xF
+ ASM_ASSERT(eq)
+#endif
+ /*
+ * Given `idx` calculate address of mrs/ret instruction pair
+ * in the table below.
+ */
+ add x2, x2, x0, lsl #3 /* each msr/ret sequence is 8 bytes */
+#if ENABLE_BTI
+ add x2, x2, x0, lsl #2 /* + "bti j" instruction */
+#endif
+ br x2
+
+1: write AMEVCNTR10_EL0 /* index 0 */
+ write AMEVCNTR11_EL0 /* index 1 */
+ write AMEVCNTR12_EL0 /* index 2 */
+ write AMEVCNTR13_EL0 /* index 3 */
+ write AMEVCNTR14_EL0 /* index 4 */
+ write AMEVCNTR15_EL0 /* index 5 */
+ write AMEVCNTR16_EL0 /* index 6 */
+ write AMEVCNTR17_EL0 /* index 7 */
+ write AMEVCNTR18_EL0 /* index 8 */
+ write AMEVCNTR19_EL0 /* index 9 */
+ write AMEVCNTR1A_EL0 /* index 10 */
+ write AMEVCNTR1B_EL0 /* index 11 */
+ write AMEVCNTR1C_EL0 /* index 12 */
+ write AMEVCNTR1D_EL0 /* index 13 */
+ write AMEVCNTR1E_EL0 /* index 14 */
+ write AMEVCNTR1F_EL0 /* index 15 */
+endfunc amu_group1_cnt_write_internal
+
+/*
+ * void amu_group1_set_evtype_internal(int idx, unsigned int val);
+ *
+ * Program the AMU event type register indexed by `idx`
+ * with the value `val`.
+ */
+func amu_group1_set_evtype_internal
+ adr x2, 1f
+#if ENABLE_ASSERTIONS
+ /*
+ * It can be dangerous to call this function with an
+ * out of bounds index. Ensure `idx` is valid.
+ */
+ tst x0, #~0xF
+ ASM_ASSERT(eq)
+
+ /* val should be between [0, 65535] */
+ tst x1, #~0xFFFF
+ ASM_ASSERT(eq)
+#endif
+ /*
+ * Given `idx` calculate address of msr/ret instruction pair
+ * in the table below.
+ */
+ add x2, x2, x0, lsl #3 /* each msr/ret sequence is 8 bytes */
+#if ENABLE_BTI
+ add x2, x2, x0, lsl #2 /* + "bti j" instruction */
+#endif
+ br x2
+
+1: write AMEVTYPER10_EL0 /* index 0 */
+ write AMEVTYPER11_EL0 /* index 1 */
+ write AMEVTYPER12_EL0 /* index 2 */
+ write AMEVTYPER13_EL0 /* index 3 */
+ write AMEVTYPER14_EL0 /* index 4 */
+ write AMEVTYPER15_EL0 /* index 5 */
+ write AMEVTYPER16_EL0 /* index 6 */
+ write AMEVTYPER17_EL0 /* index 7 */
+ write AMEVTYPER18_EL0 /* index 8 */
+ write AMEVTYPER19_EL0 /* index 9 */
+ write AMEVTYPER1A_EL0 /* index 10 */
+ write AMEVTYPER1B_EL0 /* index 11 */
+ write AMEVTYPER1C_EL0 /* index 12 */
+ write AMEVTYPER1D_EL0 /* index 13 */
+ write AMEVTYPER1E_EL0 /* index 14 */
+ write AMEVTYPER1F_EL0 /* index 15 */
+endfunc amu_group1_set_evtype_internal
+#endif
+
+/*
+ * Accessor functions for virtual offset registers added with FEAT_AMUv1p1
+ */
+
+/*
+ * uint64_t amu_group0_voffset_read_internal(int idx);
+ *
+ * Given `idx`, read the corresponding AMU virtual offset register
+ * and return it in `x0`.
+ */
+func amu_group0_voffset_read_internal
+ adr x1, 1f
+#if ENABLE_ASSERTIONS
+ /*
+ * It can be dangerous to call this function with an
+ * out of bounds index. Ensure `idx` is valid.
+ */
+ tst x0, #~3
+ ASM_ASSERT(eq)
+ /* Make sure idx != 1 since AMEVCNTVOFF01_EL2 does not exist */
+ cmp x0, #1
+ ASM_ASSERT(ne)
+#endif
+ /*
+ * Given `idx` calculate address of mrs/ret instruction pair
+ * in the table below.
+ */
+ add x1, x1, x0, lsl #3 /* each mrs/ret sequence is 8 bytes */
+#if ENABLE_BTI
+ add x1, x1, x0, lsl #2 /* + "bti j" instruction */
+#endif
+ br x1
+
+1: read AMEVCNTVOFF00_EL2 /* index 0 */
+ .skip 8 /* AMEVCNTVOFF01_EL2 does not exist */
+#if ENABLE_BTI
+ .skip 4
+#endif
+ read AMEVCNTVOFF02_EL2 /* index 2 */
+ read AMEVCNTVOFF03_EL2 /* index 3 */
+endfunc amu_group0_voffset_read_internal
+
+/*
+ * void amu_group0_voffset_write_internal(int idx, uint64_t val);
+ *
+ * Given `idx`, write `val` to the corresponding AMU virtual offset register.
+ */
+func amu_group0_voffset_write_internal
+ adr x2, 1f
+#if ENABLE_ASSERTIONS
+ /*
+ * It can be dangerous to call this function with an
+ * out of bounds index. Ensure `idx` is valid.
+ */
+ tst x0, #~3
+ ASM_ASSERT(eq)
+ /* Make sure idx != 1 since AMEVCNTVOFF01_EL2 does not exist */
+ cmp x0, #1
+ ASM_ASSERT(ne)
+#endif
+ /*
+ * Given `idx` calculate address of mrs/ret instruction pair
+ * in the table below.
+ */
+ add x2, x2, x0, lsl #3 /* each msr/ret sequence is 8 bytes */
+#if ENABLE_BTI
+ add x2, x2, x0, lsl #2 /* + "bti j" instruction */
+#endif
+ br x2
+
+1: write AMEVCNTVOFF00_EL2 /* index 0 */
+ .skip 8 /* AMEVCNTVOFF01_EL2 does not exist */
+#if ENABLE_BTI
+ .skip 4
+#endif
+ write AMEVCNTVOFF02_EL2 /* index 2 */
+ write AMEVCNTVOFF03_EL2 /* index 3 */
+endfunc amu_group0_voffset_write_internal
+
+#if ENABLE_AMU_AUXILIARY_COUNTERS
+/*
+ * uint64_t amu_group1_voffset_read_internal(int idx);
+ *
+ * Given `idx`, read the corresponding AMU virtual offset register
+ * and return it in `x0`.
+ */
+func amu_group1_voffset_read_internal
+ adr x1, 1f
+#if ENABLE_ASSERTIONS
+ /*
+ * It can be dangerous to call this function with an
+ * out of bounds index. Ensure `idx` is valid.
+ */
+ tst x0, #~0xF
+ ASM_ASSERT(eq)
+#endif
+ /*
+ * Given `idx` calculate address of mrs/ret instruction pair
+ * in the table below.
+ */
+ add x1, x1, x0, lsl #3 /* each mrs/ret sequence is 8 bytes */
+#if ENABLE_BTI
+ add x1, x1, x0, lsl #2 /* + "bti j" instruction */
+#endif
+ br x1
+
+1: read AMEVCNTVOFF10_EL2 /* index 0 */
+ read AMEVCNTVOFF11_EL2 /* index 1 */
+ read AMEVCNTVOFF12_EL2 /* index 2 */
+ read AMEVCNTVOFF13_EL2 /* index 3 */
+ read AMEVCNTVOFF14_EL2 /* index 4 */
+ read AMEVCNTVOFF15_EL2 /* index 5 */
+ read AMEVCNTVOFF16_EL2 /* index 6 */
+ read AMEVCNTVOFF17_EL2 /* index 7 */
+ read AMEVCNTVOFF18_EL2 /* index 8 */
+ read AMEVCNTVOFF19_EL2 /* index 9 */
+ read AMEVCNTVOFF1A_EL2 /* index 10 */
+ read AMEVCNTVOFF1B_EL2 /* index 11 */
+ read AMEVCNTVOFF1C_EL2 /* index 12 */
+ read AMEVCNTVOFF1D_EL2 /* index 13 */
+ read AMEVCNTVOFF1E_EL2 /* index 14 */
+ read AMEVCNTVOFF1F_EL2 /* index 15 */
+endfunc amu_group1_voffset_read_internal
+
+/*
+ * void amu_group1_voffset_write_internal(int idx, uint64_t val);
+ *
+ * Given `idx`, write `val` to the corresponding AMU virtual offset register.
+ */
+func amu_group1_voffset_write_internal
+ adr x2, 1f
+#if ENABLE_ASSERTIONS
+ /*
+ * It can be dangerous to call this function with an
+ * out of bounds index. Ensure `idx` is valid.
+ */
+ tst x0, #~0xF
+ ASM_ASSERT(eq)
+#endif
+ /*
+ * Given `idx` calculate address of mrs/ret instruction pair
+ * in the table below.
+ */
+ add x2, x2, x0, lsl #3 /* each msr/ret sequence is 8 bytes */
+#if ENABLE_BTI
+ add x2, x2, x0, lsl #2 /* + "bti j" instruction */
+#endif
+ br x2
+
+1: write AMEVCNTVOFF10_EL2 /* index 0 */
+ write AMEVCNTVOFF11_EL2 /* index 1 */
+ write AMEVCNTVOFF12_EL2 /* index 2 */
+ write AMEVCNTVOFF13_EL2 /* index 3 */
+ write AMEVCNTVOFF14_EL2 /* index 4 */
+ write AMEVCNTVOFF15_EL2 /* index 5 */
+ write AMEVCNTVOFF16_EL2 /* index 6 */
+ write AMEVCNTVOFF17_EL2 /* index 7 */
+ write AMEVCNTVOFF18_EL2 /* index 8 */
+ write AMEVCNTVOFF19_EL2 /* index 9 */
+ write AMEVCNTVOFF1A_EL2 /* index 10 */
+ write AMEVCNTVOFF1B_EL2 /* index 11 */
+ write AMEVCNTVOFF1C_EL2 /* index 12 */
+ write AMEVCNTVOFF1D_EL2 /* index 13 */
+ write AMEVCNTVOFF1E_EL2 /* index 14 */
+ write AMEVCNTVOFF1F_EL2 /* index 15 */
+endfunc amu_group1_voffset_write_internal
+#endif
diff --git a/lib/extensions/amu/amu.mk b/lib/extensions/amu/amu.mk
new file mode 100644
index 0000000..0d203cb
--- /dev/null
+++ b/lib/extensions/amu/amu.mk
@@ -0,0 +1,24 @@
+#
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+include lib/fconf/fconf.mk
+
+AMU_SOURCES := lib/extensions/amu/${ARCH}/amu.c \
+ lib/extensions/amu/${ARCH}/amu_helpers.S
+
+ifneq (${ENABLE_AMU_AUXILIARY_COUNTERS},0)
+ ifeq (${ENABLE_AMU},0)
+ $(error AMU auxiliary counter support (`ENABLE_AMU_AUXILIARY_COUNTERS`) requires AMU support (`ENABLE_AMU`))
+ endif
+endif
+
+ifneq (${ENABLE_AMU_FCONF},0)
+ ifeq (${ENABLE_AMU_AUXILIARY_COUNTERS},0)
+ $(error AMU FCONF support (`ENABLE_AMU_FCONF`) is not necessary when auxiliary counter support (`ENABLE_AMU_AUXILIARY_COUNTERS`) is disabled)
+ endif
+
+ AMU_SOURCES += ${FCONF_AMU_SOURCES}
+endif
diff --git a/lib/extensions/amu/amu_private.h b/lib/extensions/amu/amu_private.h
new file mode 100644
index 0000000..eb7ff0e
--- /dev/null
+++ b/lib/extensions/amu/amu_private.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef AMU_PRIVATE_H
+#define AMU_PRIVATE_H
+
+#include <stdint.h>
+
+#include <lib/cassert.h>
+#include <lib/extensions/amu.h>
+#include <lib/utils_def.h>
+
+#include <platform_def.h>
+
+#define AMU_GROUP0_MAX_COUNTERS U(16)
+#define AMU_GROUP1_MAX_COUNTERS U(16)
+
+#define AMU_AMCGCR_CG0NC_MAX U(16)
+
+uint64_t amu_group0_cnt_read_internal(unsigned int idx);
+void amu_group0_cnt_write_internal(unsigned int idx, uint64_t val);
+
+uint64_t amu_group1_cnt_read_internal(unsigned int idx);
+void amu_group1_cnt_write_internal(unsigned int idx, uint64_t val);
+void amu_group1_set_evtype_internal(unsigned int idx, unsigned int val);
+
+#if __aarch64__
+uint64_t amu_group0_voffset_read_internal(unsigned int idx);
+void amu_group0_voffset_write_internal(unsigned int idx, uint64_t val);
+
+uint64_t amu_group1_voffset_read_internal(unsigned int idx);
+void amu_group1_voffset_write_internal(unsigned int idx, uint64_t val);
+#endif
+
+#endif /* AMU_PRIVATE_H */
diff --git a/lib/extensions/brbe/brbe.c b/lib/extensions/brbe/brbe.c
new file mode 100644
index 0000000..1982619
--- /dev/null
+++ b/lib/extensions/brbe/brbe.c
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_features.h>
+#include <arch_helpers.h>
+
+void brbe_enable(void)
+{
+ uint64_t val;
+
+ if (is_feat_brbe_present()) {
+ /*
+ * MDCR_EL3.SBRBE = 0b01
+ *
+ * Allows BRBE usage in non-secure world and prohibited in
+ * secure world.
+ */
+ val = read_mdcr_el3();
+ val &= ~(MDCR_SBRBE_MASK << MDCR_SBRBE_SHIFT);
+ val |= (0x1UL << MDCR_SBRBE_SHIFT);
+ write_mdcr_el3(val);
+ }
+}
diff --git a/lib/extensions/mpam/mpam.c b/lib/extensions/mpam/mpam.c
new file mode 100644
index 0000000..884d480
--- /dev/null
+++ b/lib/extensions/mpam/mpam.c
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <lib/extensions/mpam.h>
+
+void mpam_enable(bool el2_unused)
+{
+ /* Check if MPAM is implemented */
+ if (get_mpam_version() == 0U) {
+ return;
+ }
+
+ /*
+ * Enable MPAM, and disable trapping to EL3 when lower ELs access their
+ * own MPAM registers.
+ */
+ write_mpam3_el3(MPAM3_EL3_MPAMEN_BIT);
+
+ /*
+ * If EL2 is implemented but unused, disable trapping to EL2 when lower
+ * ELs access their own MPAM registers.
+ */
+ if (el2_unused) {
+ write_mpam2_el2(0ULL);
+
+ if ((read_mpamidr_el1() & MPAMIDR_HAS_HCR_BIT) != 0U) {
+ write_mpamhcr_el2(0ULL);
+ }
+ }
+}
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
diff --git a/lib/extensions/mtpmu/aarch64/mtpmu.S b/lib/extensions/mtpmu/aarch64/mtpmu.S
new file mode 100644
index 0000000..0a1d57b
--- /dev/null
+++ b/lib/extensions/mtpmu/aarch64/mtpmu.S
@@ -0,0 +1,96 @@
+/*
+ * 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: x0, x1
+ */
+func mtpmu_supported
+ mrs x0, id_aa64dfr0_el1
+ mov_imm x1, ID_AA64DFR0_MTPMU_MASK
+ and x0, x1, x0, LSR #ID_AA64DFR0_MTPMU_SHIFT
+ cmp x0, ID_AA64DFR0_MTPMU_SUPPORTED
+ cset x0, eq
+ ret
+endfunc mtpmu_supported
+
+/*
+ * bool el_implemented(unsigned int el_shift)
+ *
+ * Return a boolean indicating if the specified EL is implemented.
+ * The EL is represented as the bitmask shift on id_aa64pfr0_el1 register.
+ *
+ * Trash registers: x0, x1
+ */
+func el_implemented
+ mrs x1, id_aa64pfr0_el1
+ lsr x1, x1, x0
+ cmp x1, #ID_AA64PFR0_ELX_MASK
+ cset x0, eq
+ ret
+endfunc el_implemented
+
+/*
+ * void mtpmu_disable(void)
+ *
+ * Disable mtpmu feature if supported.
+ *
+ * Trash register: x0, x1, x30
+ */
+func mtpmu_disable
+ mov x10, x30
+ bl mtpmu_supported
+ cbz x0, exit_disable
+
+ /* FEAT_MTMPU Supported */
+ mov_imm x0, ID_AA64PFR0_EL3_SHIFT
+ bl el_implemented
+ cbz x0, 1f
+
+ /* EL3 implemented */
+ mrs x0, mdcr_el3
+ mov_imm x1, MDCR_MTPME_BIT
+ bic x0, x0, x1
+ msr mdcr_el3, x0
+
+ /*
+ * If EL3 is implemented, MDCR_EL2.MTPME is implemented as Res0 and
+ * FEAT_MTPMU is controlled only from EL3, so no need to perform
+ * any operations for EL2.
+ */
+ isb
+exit_disable:
+ ret x10
+1:
+ /* EL3 not implemented */
+ mov_imm x0, ID_AA64PFR0_EL2_SHIFT
+ bl el_implemented
+ cbz x0, exit_disable
+
+ /* EL2 implemented */
+ mrs x0, mdcr_el2
+ mov_imm x1, MDCR_EL2_MTPME
+ bic x0, x0, x1
+ msr mdcr_el2, x0
+ isb
+ ret x10
+endfunc mtpmu_disable
diff --git a/lib/extensions/pauth/pauth_helpers.S b/lib/extensions/pauth/pauth_helpers.S
new file mode 100644
index 0000000..d483c7d
--- /dev/null
+++ b/lib/extensions/pauth/pauth_helpers.S
@@ -0,0 +1,141 @@
+/*
+ * Copyright (c) 2019, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <lib/el3_runtime/cpu_data.h>
+
+ .global pauth_init_enable_el1
+ .global pauth_disable_el1
+ .global pauth_init_enable_el3
+ .global pauth_disable_el3
+ .globl pauth_load_bl31_apiakey
+ .globl pauth_load_bl1_apiakey_enable
+
+/* -------------------------------------------------------------
+ * Program APIAKey_EL1 and enable pointer authentication in EL1
+ * -------------------------------------------------------------
+ */
+func pauth_init_enable_el1
+ stp x29, x30, [sp, #-16]!
+
+ /* Initialize platform key */
+ bl plat_init_apkey
+
+ /* Program instruction key A used by the Trusted Firmware */
+ msr APIAKeyLo_EL1, x0
+ msr APIAKeyHi_EL1, x1
+
+ /* Enable pointer authentication */
+ mrs x0, sctlr_el1
+ orr x0, x0, #SCTLR_EnIA_BIT
+
+#if ENABLE_BTI
+ /* Enable PAC branch type compatibility */
+ bic x0, x0, #(SCTLR_BT0_BIT | SCTLR_BT1_BIT)
+#endif
+ msr sctlr_el1, x0
+ isb
+
+ ldp x29, x30, [sp], #16
+ ret
+endfunc pauth_init_enable_el1
+
+/* -------------------------------------------------------------
+ * Disable pointer authentication in EL3
+ * -------------------------------------------------------------
+ */
+func pauth_disable_el1
+ mrs x0, sctlr_el1
+ bic x0, x0, #SCTLR_EnIA_BIT
+ msr sctlr_el1, x0
+ isb
+ ret
+endfunc pauth_disable_el1
+
+/* -------------------------------------------------------------
+ * Program APIAKey_EL1 and enable pointer authentication in EL3
+ * -------------------------------------------------------------
+ */
+func pauth_init_enable_el3
+ stp x29, x30, [sp, #-16]!
+
+ /* Initialize platform key */
+ bl plat_init_apkey
+
+ /* Program instruction key A used by the Trusted Firmware */
+ msr APIAKeyLo_EL1, x0
+ msr APIAKeyHi_EL1, x1
+
+ /* Enable pointer authentication */
+ mrs x0, sctlr_el3
+ orr x0, x0, #SCTLR_EnIA_BIT
+
+#if ENABLE_BTI
+ /* Enable PAC branch type compatibility */
+ bic x0, x0, #SCTLR_BT_BIT
+#endif
+ msr sctlr_el3, x0
+ isb
+
+ ldp x29, x30, [sp], #16
+ ret
+endfunc pauth_init_enable_el3
+
+/* -------------------------------------------------------------
+ * Disable pointer authentication in EL3
+ * -------------------------------------------------------------
+ */
+func pauth_disable_el3
+ mrs x0, sctlr_el3
+ bic x0, x0, #SCTLR_EnIA_BIT
+ msr sctlr_el3, x0
+ isb
+ ret
+endfunc pauth_disable_el3
+
+/* -------------------------------------------------------------
+ * The following functions strictly follow the AArch64 PCS
+ * to use x9-x17 (temporary caller-saved registers) to load
+ * the APIAKey_EL1 and enable pointer authentication.
+ * -------------------------------------------------------------
+ */
+func pauth_load_bl31_apiakey
+ /* tpidr_el3 contains the address of cpu_data structure */
+ mrs x9, tpidr_el3
+
+ /* Load apiakey from cpu_data */
+ ldp x10, x11, [x9, #CPU_DATA_APIAKEY_OFFSET]
+
+ /* Program instruction key A */
+ msr APIAKeyLo_EL1, x10
+ msr APIAKeyHi_EL1, x11
+ isb
+ ret
+endfunc pauth_load_bl31_apiakey
+
+func pauth_load_bl1_apiakey_enable
+ /* Load instruction key A used by the Trusted Firmware */
+ adrp x9, bl1_apiakey
+ add x9, x9, :lo12:bl1_apiakey
+ ldp x10, x11, [x9]
+
+ /* Program instruction key A */
+ msr APIAKeyLo_EL1, x10
+ msr APIAKeyHi_EL1, x11
+
+ /* Enable pointer authentication */
+ mrs x9, sctlr_el3
+ orr x9, x9, #SCTLR_EnIA_BIT
+
+#if ENABLE_BTI
+ /* Enable PAC branch type compatibility */
+ bic x9, x9, #SCTLR_BT_BIT
+#endif
+ msr sctlr_el3, x9
+ isb
+ ret
+endfunc pauth_load_bl1_apiakey_enable
diff --git a/lib/extensions/ras/ras_common.c b/lib/extensions/ras/ras_common.c
new file mode 100644
index 0000000..622879e
--- /dev/null
+++ b/lib/extensions/ras/ras_common.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020, NVIDIA Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch_helpers.h>
+#include <bl31/ea_handle.h>
+#include <bl31/ehf.h>
+#include <common/debug.h>
+#include <lib/extensions/ras.h>
+#include <lib/extensions/ras_arch.h>
+#include <plat/common/platform.h>
+
+#ifndef PLAT_RAS_PRI
+# error Platform must define RAS priority value
+#endif
+
+/*
+ * Function to convert architecturally-defined primary error code SERR,
+ * bits[7:0] from ERR<n>STATUS to its corresponding error string.
+ */
+const char *ras_serr_to_str(unsigned int serr)
+{
+ const char *str[ERROR_STATUS_NUM_SERR] = {
+ "No error",
+ "IMPLEMENTATION DEFINED error",
+ "Data value from (non-associative) internal memory",
+ "IMPLEMENTATION DEFINED pin",
+ "Assertion failure",
+ "Error detected on internal data path",
+ "Data value from associative memory",
+ "Address/control value from associative memory",
+ "Data value from a TLB",
+ "Address/control value from a TLB",
+ "Data value from producer",
+ "Address/control value from producer",
+ "Data value from (non-associative) external memory",
+ "Illegal address (software fault)",
+ "Illegal access (software fault)",
+ "Illegal state (software fault)",
+ "Internal data register",
+ "Internal control register",
+ "Error response from slave",
+ "External timeout",
+ "Internal timeout",
+ "Deferred error from slave not supported at master"
+ };
+
+ /*
+ * All other values are reserved. Reserved values might be defined
+ * in a future version of the architecture
+ */
+ if (serr >= ERROR_STATUS_NUM_SERR)
+ return "unknown SERR";
+
+ return str[serr];
+}
+
+/* Handler that receives External Aborts on RAS-capable systems */
+int ras_ea_handler(unsigned int ea_reason, uint64_t syndrome, void *cookie,
+ void *handle, uint64_t flags)
+{
+ unsigned int i, n_handled = 0;
+ int probe_data, ret;
+ struct err_record_info *info;
+
+ const struct err_handler_data err_data = {
+ .version = ERR_HANDLER_VERSION,
+ .ea_reason = ea_reason,
+ .interrupt = 0,
+ .syndrome = (uint32_t) syndrome,
+ .flags = flags,
+ .cookie = cookie,
+ .handle = handle
+ };
+
+ for_each_err_record_info(i, info) {
+ assert(info->probe != NULL);
+ assert(info->handler != NULL);
+
+ /* Continue probing until the record group signals no error */
+ while (true) {
+ if (info->probe(info, &probe_data) == 0)
+ break;
+
+ /* Handle error */
+ ret = info->handler(info, probe_data, &err_data);
+ if (ret != 0)
+ return ret;
+
+ n_handled++;
+ }
+ }
+
+ return (n_handled != 0U) ? 1 : 0;
+}
+
+#if ENABLE_ASSERTIONS
+static void assert_interrupts_sorted(void)
+{
+ unsigned int i, last;
+ struct ras_interrupt *start = ras_interrupt_mappings.intrs;
+
+ if (ras_interrupt_mappings.num_intrs == 0UL)
+ return;
+
+ last = start[0].intr_number;
+ for (i = 1; i < ras_interrupt_mappings.num_intrs; i++) {
+ assert(start[i].intr_number > last);
+ last = start[i].intr_number;
+ }
+}
+#endif
+
+/*
+ * Given an RAS interrupt number, locate the registered handler and call it. If
+ * no handler was found for the interrupt number, this function panics.
+ */
+static int ras_interrupt_handler(uint32_t intr_raw, uint32_t flags,
+ void *handle, void *cookie)
+{
+ struct ras_interrupt *ras_inrs = ras_interrupt_mappings.intrs;
+ struct ras_interrupt *selected = NULL;
+ int probe_data = 0;
+ int start, end, mid, ret __unused;
+
+ const struct err_handler_data err_data = {
+ .version = ERR_HANDLER_VERSION,
+ .interrupt = intr_raw,
+ .flags = flags,
+ .cookie = cookie,
+ .handle = handle
+ };
+
+ assert(ras_interrupt_mappings.num_intrs > 0UL);
+
+ start = 0;
+ end = (int)ras_interrupt_mappings.num_intrs - 1;
+ while (start <= end) {
+ mid = ((end + start) / 2);
+ if (intr_raw == ras_inrs[mid].intr_number) {
+ selected = &ras_inrs[mid];
+ break;
+ } else if (intr_raw < ras_inrs[mid].intr_number) {
+ /* Move left */
+ end = mid - 1;
+ } else {
+ /* Move right */
+ start = mid + 1;
+ }
+ }
+
+ if (selected == NULL) {
+ ERROR("RAS interrupt %u has no handler!\n", intr_raw);
+ panic();
+ }
+
+ if (selected->err_record->probe != NULL) {
+ ret = selected->err_record->probe(selected->err_record, &probe_data);
+ assert(ret != 0);
+ }
+
+ /* Call error handler for the record group */
+ assert(selected->err_record->handler != NULL);
+ (void) selected->err_record->handler(selected->err_record, probe_data,
+ &err_data);
+
+ return 0;
+}
+
+void __init ras_init(void)
+{
+#if ENABLE_ASSERTIONS
+ /* Check RAS interrupts are sorted */
+ assert_interrupts_sorted();
+#endif
+
+ /* Register RAS priority handler */
+ ehf_register_priority_handler(PLAT_RAS_PRI, ras_interrupt_handler);
+}
diff --git a/lib/extensions/ras/std_err_record.c b/lib/extensions/ras/std_err_record.c
new file mode 100644
index 0000000..c03fbbe
--- /dev/null
+++ b/lib/extensions/ras/std_err_record.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/extensions/ras_arch.h>
+#include <lib/utils_def.h>
+
+/*
+ * Probe for error in memory-mapped registers containing error records
+ * implemented Standard Error Record format. Upon detecting an error, set probe
+ * data to the index of the record in error, and return 1; otherwise, return 0.
+ */
+int ser_probe_memmap(uintptr_t base, unsigned int size_num_k, int *probe_data)
+{
+ unsigned int num_records, num_group_regs, i;
+ uint64_t gsr;
+
+ assert(base != 0UL);
+
+ /* Only 4K supported for now */
+ assert(size_num_k == STD_ERR_NODE_SIZE_NUM_K);
+
+ num_records = (unsigned int)
+ (mmio_read_32(ERR_DEVID(base, size_num_k)) & ERR_DEVID_MASK);
+
+ /* A group register shows error status for 2^6 error records */
+ num_group_regs = (num_records >> 6U) + 1U;
+
+ /* Iterate through group registers to find a record in error */
+ for (i = 0; i < num_group_regs; i++) {
+ gsr = mmio_read_64(ERR_GSR(base, size_num_k, i));
+ if (gsr == 0ULL)
+ continue;
+
+ /* Return the index of the record in error */
+ if (probe_data != NULL)
+ *probe_data = (((int) (i << 6U)) + __builtin_ctzll(gsr));
+
+ return 1;
+ }
+
+ return 0;
+}
+
+/*
+ * Probe for error in System Registers where error records are implemented in
+ * Standard Error Record format. Upon detecting an error, set probe data to the
+ * index of the record in error, and return 1; otherwise, return 0.
+ */
+int ser_probe_sysreg(unsigned int idx_start, unsigned int num_idx, int *probe_data)
+{
+ unsigned int i;
+ uint64_t status;
+ unsigned int max_idx __unused =
+ ((unsigned int) read_erridr_el1()) & ERRIDR_MASK;
+
+ assert(idx_start < max_idx);
+ assert(check_u32_overflow(idx_start, num_idx) == 0);
+ assert((idx_start + num_idx - 1U) < max_idx);
+
+ for (i = 0; i < num_idx; i++) {
+ /* Select the error record */
+ ser_sys_select_record(idx_start + i);
+
+ /* Retrieve status register from the error record */
+ status = read_erxstatus_el1();
+
+ /* Check for valid field in status */
+ if (ERR_STATUS_GET_FIELD(status, V) != 0U) {
+ if (probe_data != NULL)
+ *probe_data = (int) i;
+ return 1;
+ }
+ }
+
+ return 0;
+}
diff --git a/lib/extensions/sme/sme.c b/lib/extensions/sme/sme.c
new file mode 100644
index 0000000..ec8cca8
--- /dev/null
+++ b/lib/extensions/sme/sme.c
@@ -0,0 +1,109 @@
+/*
+ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/extensions/sme.h>
+#include <lib/extensions/sve.h>
+
+static bool feat_sme_supported(void)
+{
+ uint64_t features;
+
+ features = read_id_aa64pfr1_el1() >> ID_AA64PFR1_EL1_SME_SHIFT;
+ return (features & ID_AA64PFR1_EL1_SME_MASK) != 0U;
+}
+
+static bool feat_sme_fa64_supported(void)
+{
+ uint64_t features;
+
+ features = read_id_aa64smfr0_el1();
+ return (features & ID_AA64SMFR0_EL1_FA64_BIT) != 0U;
+}
+
+void sme_enable(cpu_context_t *context)
+{
+ u_register_t reg;
+ u_register_t cptr_el3;
+ el3_state_t *state;
+
+ /* Make sure SME is implemented in hardware before continuing. */
+ if (!feat_sme_supported()) {
+ /* Perhaps the hardware supports SVE only */
+ sve_enable(context);
+ return;
+ }
+
+ /* Get the context state. */
+ state = get_el3state_ctx(context);
+
+ /* Enable SME in CPTR_EL3. */
+ reg = read_ctx_reg(state, CTX_CPTR_EL3);
+ reg |= ESM_BIT;
+ write_ctx_reg(state, CTX_CPTR_EL3, reg);
+
+ /* Set the ENTP2 bit in SCR_EL3 to enable access to TPIDR2_EL0. */
+ reg = read_ctx_reg(state, CTX_SCR_EL3);
+ reg |= SCR_ENTP2_BIT;
+ write_ctx_reg(state, CTX_SCR_EL3, reg);
+
+ /* Set CPTR_EL3.ESM bit so we can write SMCR_EL3 without trapping. */
+ cptr_el3 = read_cptr_el3();
+ write_cptr_el3(cptr_el3 | ESM_BIT);
+ isb();
+
+ /*
+ * Set the max LEN value and FA64 bit. This register is set up globally
+ * to be the least restrictive, then lower ELs can restrict as needed
+ * using SMCR_EL2 and SMCR_EL1.
+ */
+ reg = SMCR_ELX_LEN_MASK;
+ if (feat_sme_fa64_supported()) {
+ VERBOSE("[SME] FA64 enabled\n");
+ reg |= SMCR_ELX_FA64_BIT;
+ }
+ write_smcr_el3(reg);
+
+ /* Reset CPTR_EL3 value. */
+ write_cptr_el3(cptr_el3);
+ isb();
+
+ /* Enable SVE/FPU in addition to SME. */
+ sve_enable(context);
+}
+
+void sme_disable(cpu_context_t *context)
+{
+ u_register_t reg;
+ el3_state_t *state;
+
+ /* Make sure SME is implemented in hardware before continuing. */
+ if (!feat_sme_supported()) {
+ /* Perhaps the hardware supports SVE only */
+ sve_disable(context);
+ return;
+ }
+
+ /* Get the context state. */
+ state = get_el3state_ctx(context);
+
+ /* Disable SME, SVE, and FPU since they all share registers. */
+ reg = read_ctx_reg(state, CTX_CPTR_EL3);
+ reg &= ~ESM_BIT; /* Trap SME */
+ reg &= ~CPTR_EZ_BIT; /* Trap SVE */
+ reg |= TFP_BIT; /* Trap FPU/SIMD */
+ write_ctx_reg(state, CTX_CPTR_EL3, reg);
+
+ /* Disable access to TPIDR2_EL0. */
+ reg = read_ctx_reg(state, CTX_SCR_EL3);
+ reg &= ~SCR_ENTP2_BIT;
+ write_ctx_reg(state, CTX_SCR_EL3, reg);
+}
diff --git a/lib/extensions/spe/spe.c b/lib/extensions/spe/spe.c
new file mode 100644
index 0000000..d747efc
--- /dev/null
+++ b/lib/extensions/spe/spe.c
@@ -0,0 +1,98 @@
+/*
+ * Copyright (c) 2017-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <lib/el3_runtime/pubsub.h>
+#include <lib/extensions/spe.h>
+
+static inline void psb_csync(void)
+{
+ /*
+ * The assembler does not yet understand the psb csync mnemonic
+ * so use the equivalent hint instruction.
+ */
+ __asm__ volatile("hint #17");
+}
+
+bool spe_supported(void)
+{
+ uint64_t features;
+
+ features = read_id_aa64dfr0_el1() >> ID_AA64DFR0_PMS_SHIFT;
+ return (features & ID_AA64DFR0_PMS_MASK) > 0ULL;
+}
+
+void spe_enable(bool el2_unused)
+{
+ uint64_t v;
+
+ if (!spe_supported())
+ return;
+
+ if (el2_unused) {
+ /*
+ * MDCR_EL2.TPMS (ARM v8.2): Do not trap statistical
+ * profiling controls to EL2.
+ *
+ * MDCR_EL2.E2PB (ARM v8.2): SPE enabled in Non-secure
+ * state. Accesses to profiling buffer controls at
+ * Non-secure EL1 are not trapped to EL2.
+ */
+ v = read_mdcr_el2();
+ v &= ~MDCR_EL2_TPMS;
+ v |= MDCR_EL2_E2PB(MDCR_EL2_E2PB_EL1);
+ write_mdcr_el2(v);
+ }
+
+ /*
+ * MDCR_EL2.NSPB (ARM v8.2): SPE enabled in Non-secure state
+ * and disabled in secure state. Accesses to SPE registers at
+ * S-EL1 generate trap exceptions to EL3.
+ *
+ * MDCR_EL3.EnPMSN (ARM v8.7): Do not trap access to PMSNEVFR_EL1
+ * register at NS-EL1 or NS-EL2 to EL3 if FEAT_SPEv1p2 is implemented.
+ * Setting this bit to 1 doesn't have any effect on it when
+ * FEAT_SPEv1p2 not implemented.
+ */
+ v = read_mdcr_el3();
+ v |= MDCR_NSPB(MDCR_NSPB_EL1) | MDCR_EnPMSN_BIT;
+ write_mdcr_el3(v);
+}
+
+void spe_disable(void)
+{
+ uint64_t v;
+
+ if (!spe_supported())
+ return;
+
+ /* Drain buffered data */
+ psb_csync();
+ dsbnsh();
+
+ /* Disable profiling buffer */
+ v = read_pmblimitr_el1();
+ v &= ~(1ULL << 0);
+ write_pmblimitr_el1(v);
+ isb();
+}
+
+static void *spe_drain_buffers_hook(const void *arg)
+{
+ if (!spe_supported())
+ return (void *)-1;
+
+ /* Drain buffered data */
+ psb_csync();
+ dsbnsh();
+
+ return (void *)0;
+}
+
+SUBSCRIBE_TO_EVENT(cm_entering_secure_world, spe_drain_buffers_hook);
diff --git a/lib/extensions/sve/sve.c b/lib/extensions/sve/sve.c
new file mode 100644
index 0000000..f7dcc76
--- /dev/null
+++ b/lib/extensions/sve/sve.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <lib/cassert.h>
+#include <lib/el3_runtime/pubsub.h>
+#include <lib/extensions/sve.h>
+
+CASSERT(SVE_VECTOR_LEN <= 2048, assert_sve_vl_too_long);
+CASSERT(SVE_VECTOR_LEN >= 128, assert_sve_vl_too_short);
+CASSERT((SVE_VECTOR_LEN % 128) == 0, assert_sve_vl_granule);
+
+/*
+ * Converts SVE vector size restriction in bytes to LEN according to ZCR_EL3 documentation.
+ * VECTOR_SIZE = (LEN+1) * 128
+ */
+#define CONVERT_SVE_LENGTH(x) (((x / 128) - 1))
+
+static bool sve_supported(void)
+{
+ uint64_t features;
+
+ features = read_id_aa64pfr0_el1() >> ID_AA64PFR0_SVE_SHIFT;
+ return (features & ID_AA64PFR0_SVE_MASK) == 1U;
+}
+
+void sve_enable(cpu_context_t *context)
+{
+ u_register_t cptr_el3;
+
+ if (!sve_supported()) {
+ return;
+ }
+
+ cptr_el3 = read_ctx_reg(get_el3state_ctx(context), CTX_CPTR_EL3);
+
+ /* Enable access to SVE functionality for all ELs. */
+ cptr_el3 = (cptr_el3 | CPTR_EZ_BIT) & ~(TFP_BIT);
+ write_ctx_reg(get_el3state_ctx(context), CTX_CPTR_EL3, cptr_el3);
+
+ /* Restrict maximum SVE vector length (SVE_VECTOR_LEN+1) * 128. */
+ write_ctx_reg(get_el3state_ctx(context), CTX_ZCR_EL3,
+ (ZCR_EL3_LEN_MASK & CONVERT_SVE_LENGTH(SVE_VECTOR_LEN)));
+}
+
+void sve_disable(cpu_context_t *context)
+{
+ u_register_t reg;
+ el3_state_t *state;
+
+ /* Make sure SME is implemented in hardware before continuing. */
+ if (!sve_supported()) {
+ return;
+ }
+
+ /* Get the context state. */
+ state = get_el3state_ctx(context);
+
+ /* Disable SVE and FPU since they share registers. */
+ reg = read_ctx_reg(state, CTX_CPTR_EL3);
+ reg &= ~CPTR_EZ_BIT; /* Trap SVE */
+ reg |= TFP_BIT; /* Trap FPU/SIMD */
+ write_ctx_reg(state, CTX_CPTR_EL3, reg);
+}
diff --git a/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c b/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
new file mode 100644
index 0000000..89b8029
--- /dev/null
+++ b/lib/extensions/sys_reg_trace/aarch32/sys_reg_trace.c
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <lib/extensions/sys_reg_trace.h>
+
+static bool sys_reg_trace_supported(void)
+{
+ uint32_t features;
+
+ features = read_id_dfr0() >> ID_DFR0_COPTRC_SHIFT;
+ return ((features & ID_DFR0_COPTRC_MASK) ==
+ ID_DFR0_COPTRC_SUPPORTED);
+}
+
+void sys_reg_trace_enable(void)
+{
+ uint32_t val;
+
+ if (sys_reg_trace_supported()) {
+ /*
+ * NSACR.NSTRCDIS = b0
+ * enable NS system register access to implemented trace
+ * registers.
+ */
+ val = read_nsacr();
+ val &= ~NSTRCDIS_BIT;
+ write_nsacr(val);
+ }
+}
diff --git a/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c b/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
new file mode 100644
index 0000000..960d698
--- /dev/null
+++ b/lib/extensions/sys_reg_trace/aarch64/sys_reg_trace.c
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <lib/extensions/sys_reg_trace.h>
+
+static bool sys_reg_trace_supported(void)
+{
+ uint64_t features;
+
+ features = read_id_aa64dfr0_el1() >> ID_AA64DFR0_TRACEVER_SHIFT;
+ return ((features & ID_AA64DFR0_TRACEVER_MASK) ==
+ ID_AA64DFR0_TRACEVER_SUPPORTED);
+}
+
+void sys_reg_trace_enable(cpu_context_t *ctx)
+{
+ uint64_t val;
+
+ if (sys_reg_trace_supported()) {
+ /* Retrieve CPTR_EL3 value from the given context 'ctx',
+ * and update CPTR_EL3.TTA bit to 0.
+ * This function is called while switching context to NS to
+ * allow system trace register access to NS-EL2 and NS-EL1
+ * when NS-EL2 is implemented but not used.
+ */
+ val = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
+ val &= ~TTA_BIT;
+ write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, val);
+ }
+}
diff --git a/lib/extensions/trbe/trbe.c b/lib/extensions/trbe/trbe.c
new file mode 100644
index 0000000..b346387
--- /dev/null
+++ b/lib/extensions/trbe/trbe.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_features.h>
+#include <arch_helpers.h>
+#include <lib/el3_runtime/pubsub.h>
+#include <lib/extensions/trbe.h>
+
+static void tsb_csync(void)
+{
+ /*
+ * The assembler does not yet understand the tsb csync mnemonic
+ * so use the equivalent hint instruction.
+ */
+ __asm__ volatile("hint #18");
+}
+
+void trbe_enable(void)
+{
+ uint64_t val;
+
+ if (is_feat_trbe_present()) {
+ /*
+ * MDCR_EL3.NSTB = 0b11
+ * Allow access of trace buffer control registers from NS-EL1
+ * and NS-EL2, tracing is prohibited in Secure and Realm state
+ * (if implemented).
+ */
+ val = read_mdcr_el3();
+ val |= MDCR_NSTB(MDCR_NSTB_EL1);
+ write_mdcr_el3(val);
+ }
+}
+
+static void *trbe_drain_trace_buffers_hook(const void *arg __unused)
+{
+ if (is_feat_trbe_present()) {
+ /*
+ * Before switching from normal world to secure world
+ * the trace buffers need to be drained out to memory. This is
+ * required to avoid an invalid memory access when TTBR is switched
+ * for entry to S-EL1.
+ */
+ tsb_csync();
+ dsbnsh();
+ }
+
+ return (void *)0;
+}
+
+SUBSCRIBE_TO_EVENT(cm_entering_secure_world, trbe_drain_trace_buffers_hook);
diff --git a/lib/extensions/trf/aarch32/trf.c b/lib/extensions/trf/aarch32/trf.c
new file mode 100644
index 0000000..834092d
--- /dev/null
+++ b/lib/extensions/trf/aarch32/trf.c
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <lib/extensions/trf.h>
+
+static bool trf_supported(void)
+{
+ uint32_t features;
+
+ features = read_id_dfr0() >> ID_DFR0_TRACEFILT_SHIFT;
+ return ((features & ID_DFR0_TRACEFILT_MASK) ==
+ ID_DFR0_TRACEFILT_SUPPORTED);
+}
+
+void trf_enable(void)
+{
+ uint32_t val;
+
+ if (trf_supported()) {
+ /*
+ * Allow access of trace filter control registers from
+ * non-monitor mode
+ */
+ val = read_sdcr();
+ val &= ~SDCR_TTRF_BIT;
+ write_sdcr(val);
+ }
+}
diff --git a/lib/extensions/trf/aarch64/trf.c b/lib/extensions/trf/aarch64/trf.c
new file mode 100644
index 0000000..1da5dce
--- /dev/null
+++ b/lib/extensions/trf/aarch64/trf.c
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <lib/extensions/trf.h>
+
+static bool trf_supported(void)
+{
+ uint64_t features;
+
+ features = read_id_aa64dfr0_el1() >> ID_AA64DFR0_TRACEFILT_SHIFT;
+ return ((features & ID_AA64DFR0_TRACEFILT_MASK) ==
+ ID_AA64DFR0_TRACEFILT_SUPPORTED);
+}
+
+void trf_enable(void)
+{
+ uint64_t val;
+
+ if (trf_supported()) {
+ /*
+ * MDCR_EL3.TTRF = b0
+ * Allow access of trace filter control registers from NS-EL2
+ * and NS-EL1 when NS-EL2 is implemented but not used
+ */
+ val = read_mdcr_el3();
+ val &= ~MDCR_TTRF_BIT;
+ write_mdcr_el3(val);
+ }
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-27 05:48:00 +0000