Adding upstream version 2.8.0+dfsg.upstream/2.8.0+dfsgupstream

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

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);
+	}
+}