diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index 771fcdc..f47e779 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -10,15 +10,6 @@
 #include <context.h>
 #include <el3_common_macros.S>
 
-#if CTX_INCLUDE_EL2_REGS
-	.global	el2_sysregs_context_save_common
-	.global	el2_sysregs_context_restore_common
-#if CTX_INCLUDE_MTE_REGS
-	.global	el2_sysregs_context_save_mte
-	.global	el2_sysregs_context_restore_mte
-#endif /* CTX_INCLUDE_MTE_REGS */
-#endif /* CTX_INCLUDE_EL2_REGS */
-
 	.global	el1_sysregs_context_save
 	.global	el1_sysregs_context_restore
 #if CTX_INCLUDE_FPREGS
@@ -30,183 +21,6 @@
 	.global save_and_update_ptw_el1_sys_regs
 	.global	el3_exit
 
-#if CTX_INCLUDE_EL2_REGS
-
-/* -----------------------------------------------------
- * The following functions strictly follow the AArch64
- * PCS to use x9-x16 (temporary caller-saved registers)
- * to save/restore EL2 system register context.
- * el2_sysregs_context_save/restore_common functions
- * save and restore registers that are common to all
- * configurations. The rest of the functions save and
- * restore EL2 system registers that are present when a
- * particular feature is enabled. All functions assume
- * that 'x0' is pointing to a 'el2_sys_regs' structure
- * where the register context will be saved/restored.
- *
- * The following registers are not added.
- * AMEVCNTVOFF0<n>_EL2
- * AMEVCNTVOFF1<n>_EL2
- * ICH_AP0R<n>_EL2
- * ICH_AP1R<n>_EL2
- * ICH_LR<n>_EL2
- * -----------------------------------------------------
- */
-func el2_sysregs_context_save_common
-	mrs	x9, actlr_el2
-	mrs	x10, afsr0_el2
-	stp	x9, x10, [x0, #CTX_ACTLR_EL2]
-
-	mrs	x11, afsr1_el2
-	mrs	x12, amair_el2
-	stp	x11, x12, [x0, #CTX_AFSR1_EL2]
-
-	mrs	x13, cnthctl_el2
-	mrs	x14, cntvoff_el2
-	stp	x13, x14, [x0, #CTX_CNTHCTL_EL2]
-
-	mrs	x15, cptr_el2
-	str	x15, [x0, #CTX_CPTR_EL2]
-
-#if CTX_INCLUDE_AARCH32_REGS
-	mrs	x16, dbgvcr32_el2
-	str	x16, [x0, #CTX_DBGVCR32_EL2]
-#endif /* CTX_INCLUDE_AARCH32_REGS */
-
-	mrs	x9, elr_el2
-	mrs	x10, esr_el2
-	stp	x9, x10, [x0, #CTX_ELR_EL2]
-
-	mrs	x11, far_el2
-	mrs	x12, hacr_el2
-	stp	x11, x12, [x0, #CTX_FAR_EL2]
-
-	mrs	x13, hcr_el2
-	mrs	x14, hpfar_el2
-	stp	x13, x14, [x0, #CTX_HCR_EL2]
-
-	mrs	x15, hstr_el2
-	mrs	x16, ICC_SRE_EL2
-	stp	x15, x16, [x0, #CTX_HSTR_EL2]
-
-	mrs	x9, ICH_HCR_EL2
-	mrs	x10, ICH_VMCR_EL2
-	stp	x9, x10, [x0, #CTX_ICH_HCR_EL2]
-
-	mrs	x11, mair_el2
-	mrs	x12, mdcr_el2
-	stp	x11, x12, [x0, #CTX_MAIR_EL2]
-
-	mrs	x14, sctlr_el2
-	str	x14, [x0, #CTX_SCTLR_EL2]
-
-	mrs	x15, spsr_el2
-	mrs	x16, sp_el2
-	stp	x15, x16, [x0, #CTX_SPSR_EL2]
-
-	mrs	x9, tcr_el2
-	mrs	x10, tpidr_el2
-	stp	x9, x10, [x0, #CTX_TCR_EL2]
-
-	mrs	x11, ttbr0_el2
-	mrs	x12, vbar_el2
-	stp	x11, x12, [x0, #CTX_TTBR0_EL2]
-
-	mrs	x13, vmpidr_el2
-	mrs	x14, vpidr_el2
-	stp	x13, x14, [x0, #CTX_VMPIDR_EL2]
-
-	mrs	x15, vtcr_el2
-	mrs	x16, vttbr_el2
-	stp	x15, x16, [x0, #CTX_VTCR_EL2]
-	ret
-endfunc el2_sysregs_context_save_common
-
-func el2_sysregs_context_restore_common
-	ldp	x9, x10, [x0, #CTX_ACTLR_EL2]
-	msr	actlr_el2, x9
-	msr	afsr0_el2, x10
-
-	ldp	x11, x12, [x0, #CTX_AFSR1_EL2]
-	msr	afsr1_el2, x11
-	msr	amair_el2, x12
-
-	ldp	x13, x14, [x0, #CTX_CNTHCTL_EL2]
-	msr	cnthctl_el2, x13
-	msr	cntvoff_el2, x14
-
-	ldr	x15, [x0, #CTX_CPTR_EL2]
-	msr	cptr_el2, x15
-
-#if CTX_INCLUDE_AARCH32_REGS
-	ldr	x16, [x0, #CTX_DBGVCR32_EL2]
-	msr	dbgvcr32_el2, x16
-#endif /* CTX_INCLUDE_AARCH32_REGS */
-
-	ldp	x9, x10, [x0, #CTX_ELR_EL2]
-	msr	elr_el2, x9
-	msr	esr_el2, x10
-
-	ldp	x11, x12, [x0, #CTX_FAR_EL2]
-	msr	far_el2, x11
-	msr	hacr_el2, x12
-
-	ldp	x13, x14, [x0, #CTX_HCR_EL2]
-	msr	hcr_el2, x13
-	msr	hpfar_el2, x14
-
-	ldp	x15, x16, [x0, #CTX_HSTR_EL2]
-	msr	hstr_el2, x15
-	msr	ICC_SRE_EL2, x16
-
-	ldp	x9, x10, [x0, #CTX_ICH_HCR_EL2]
-	msr	ICH_HCR_EL2, x9
-	msr	ICH_VMCR_EL2, x10
-
-	ldp	x11, x12, [x0, #CTX_MAIR_EL2]
-	msr	mair_el2, x11
-	msr	mdcr_el2, x12
-
-	ldr	x14, [x0, #CTX_SCTLR_EL2]
-	msr	sctlr_el2, x14
-
-	ldp	x15, x16, [x0, #CTX_SPSR_EL2]
-	msr	spsr_el2, x15
-	msr	sp_el2, x16
-
-	ldp	x9, x10, [x0, #CTX_TCR_EL2]
-	msr	tcr_el2, x9
-	msr	tpidr_el2, x10
-
-	ldp	x11, x12, [x0, #CTX_TTBR0_EL2]
-	msr	ttbr0_el2, x11
-	msr	vbar_el2, x12
-
-	ldp	x13, x14, [x0, #CTX_VMPIDR_EL2]
-	msr	vmpidr_el2, x13
-	msr	vpidr_el2, x14
-
-	ldp	x15, x16, [x0, #CTX_VTCR_EL2]
-	msr	vtcr_el2, x15
-	msr	vttbr_el2, x16
-	ret
-endfunc el2_sysregs_context_restore_common
-
-#if CTX_INCLUDE_MTE_REGS
-func el2_sysregs_context_save_mte
-	mrs	x9, TFSR_EL2
-	str	x9, [x0, #CTX_TFSR_EL2]
-	ret
-endfunc el2_sysregs_context_save_mte
-
-func el2_sysregs_context_restore_mte
-	ldr	x9, [x0, #CTX_TFSR_EL2]
-	msr	TFSR_EL2, x9
-	ret
-endfunc el2_sysregs_context_restore_mte
-#endif /* CTX_INCLUDE_MTE_REGS */
-
-#endif /* CTX_INCLUDE_EL2_REGS */
 
 /* ------------------------------------------------------------------
  * The following function strictly follows the AArch64 PCS to use
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 9d717bb..0ac2d6e 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -971,6 +971,89 @@
 	}
 }
 
+/* -----------------------------------------------------
+ * The following registers are not added:
+ * AMEVCNTVOFF0<n>_EL2
+ * AMEVCNTVOFF1<n>_EL2
+ * ICH_AP0R<n>_EL2
+ * ICH_AP1R<n>_EL2
+ * ICH_LR<n>_EL2
+ * -----------------------------------------------------
+ */
+static void el2_sysregs_context_save_common(el2_sysregs_t *ctx)
+{
+	write_ctx_reg(ctx, CTX_ACTLR_EL2, read_actlr_el2());
+	write_ctx_reg(ctx, CTX_AFSR0_EL2, read_afsr0_el2());
+	write_ctx_reg(ctx, CTX_AFSR1_EL2, read_afsr1_el2());
+	write_ctx_reg(ctx, CTX_AMAIR_EL2, read_amair_el2());
+	write_ctx_reg(ctx, CTX_CNTHCTL_EL2, read_cnthctl_el2());
+	write_ctx_reg(ctx, CTX_CNTVOFF_EL2, read_cntvoff_el2());
+	write_ctx_reg(ctx, CTX_CPTR_EL2, read_cptr_el2());
+	if (CTX_INCLUDE_AARCH32_REGS) {
+		write_ctx_reg(ctx, CTX_DBGVCR32_EL2, read_dbgvcr32_el2());
+	}
+	write_ctx_reg(ctx, CTX_ELR_EL2, read_elr_el2());
+	write_ctx_reg(ctx, CTX_ESR_EL2, read_esr_el2());
+	write_ctx_reg(ctx, CTX_FAR_EL2, read_far_el2());
+	write_ctx_reg(ctx, CTX_HACR_EL2, read_hacr_el2());
+	write_ctx_reg(ctx, CTX_HCR_EL2, read_hcr_el2());
+	write_ctx_reg(ctx, CTX_HPFAR_EL2, read_hpfar_el2());
+	write_ctx_reg(ctx, CTX_HSTR_EL2, read_hstr_el2());
+	write_ctx_reg(ctx, CTX_ICC_SRE_EL2, read_icc_sre_el2());
+	write_ctx_reg(ctx, CTX_ICH_HCR_EL2, read_ich_hcr_el2());
+	write_ctx_reg(ctx, CTX_ICH_VMCR_EL2, read_ich_vmcr_el2());
+	write_ctx_reg(ctx, CTX_MAIR_EL2, read_mair_el2());
+	write_ctx_reg(ctx, CTX_MDCR_EL2, read_mdcr_el2());
+	write_ctx_reg(ctx, CTX_SCTLR_EL2, read_sctlr_el2());
+	write_ctx_reg(ctx, CTX_SPSR_EL2, read_spsr_el2());
+	write_ctx_reg(ctx, CTX_SP_EL2, read_sp_el2());
+	write_ctx_reg(ctx, CTX_TCR_EL2, read_tcr_el2());
+	write_ctx_reg(ctx, CTX_TPIDR_EL2, read_tpidr_el2());
+	write_ctx_reg(ctx, CTX_TTBR0_EL2, read_ttbr0_el2());
+	write_ctx_reg(ctx, CTX_VBAR_EL2, read_vbar_el2());
+	write_ctx_reg(ctx, CTX_VMPIDR_EL2, read_vmpidr_el2());
+	write_ctx_reg(ctx, CTX_VPIDR_EL2, read_vpidr_el2());
+	write_ctx_reg(ctx, CTX_VTCR_EL2, read_vtcr_el2());
+	write_ctx_reg(ctx, CTX_VTTBR_EL2, read_vttbr_el2());
+}
+
+static void el2_sysregs_context_restore_common(el2_sysregs_t *ctx)
+{
+	write_actlr_el2(read_ctx_reg(ctx, CTX_ACTLR_EL2));
+	write_afsr0_el2(read_ctx_reg(ctx, CTX_AFSR0_EL2));
+	write_afsr1_el2(read_ctx_reg(ctx, CTX_AFSR1_EL2));
+	write_amair_el2(read_ctx_reg(ctx, CTX_AMAIR_EL2));
+	write_cnthctl_el2(read_ctx_reg(ctx, CTX_CNTHCTL_EL2));
+	write_cntvoff_el2(read_ctx_reg(ctx, CTX_CNTVOFF_EL2));
+	write_cptr_el2(read_ctx_reg(ctx, CTX_CPTR_EL2));
+	if (CTX_INCLUDE_AARCH32_REGS) {
+		write_dbgvcr32_el2(read_ctx_reg(ctx, CTX_DBGVCR32_EL2));
+	}
+	write_elr_el2(read_ctx_reg(ctx, CTX_ELR_EL2));
+	write_esr_el2(read_ctx_reg(ctx, CTX_ESR_EL2));
+	write_far_el2(read_ctx_reg(ctx, CTX_FAR_EL2));
+	write_hacr_el2(read_ctx_reg(ctx, CTX_HACR_EL2));
+	write_hcr_el2(read_ctx_reg(ctx, CTX_HCR_EL2));
+	write_hpfar_el2(read_ctx_reg(ctx, CTX_HPFAR_EL2));
+	write_hstr_el2(read_ctx_reg(ctx, CTX_HSTR_EL2));
+	write_icc_sre_el2(read_ctx_reg(ctx, CTX_ICC_SRE_EL2));
+	write_ich_hcr_el2(read_ctx_reg(ctx, CTX_ICH_HCR_EL2));
+	write_ich_vmcr_el2(read_ctx_reg(ctx, CTX_ICH_VMCR_EL2));
+	write_mair_el2(read_ctx_reg(ctx, CTX_MAIR_EL2));
+	write_mdcr_el2(read_ctx_reg(ctx, CTX_MDCR_EL2));
+	write_sctlr_el2(read_ctx_reg(ctx, CTX_SCTLR_EL2));
+	write_spsr_el2(read_ctx_reg(ctx, CTX_SPSR_EL2));
+	write_sp_el2(read_ctx_reg(ctx, CTX_SP_EL2));
+	write_tcr_el2(read_ctx_reg(ctx, CTX_TCR_EL2));
+	write_tpidr_el2(read_ctx_reg(ctx, CTX_TPIDR_EL2));
+	write_ttbr0_el2(read_ctx_reg(ctx, CTX_TTBR0_EL2));
+	write_vbar_el2(read_ctx_reg(ctx, CTX_VBAR_EL2));
+	write_vmpidr_el2(read_ctx_reg(ctx, CTX_VMPIDR_EL2));
+	write_vpidr_el2(read_ctx_reg(ctx, CTX_VPIDR_EL2));
+	write_vtcr_el2(read_ctx_reg(ctx, CTX_VTCR_EL2));
+	write_vttbr_el2(read_ctx_reg(ctx, CTX_VTTBR_EL2));
+}
+
 /*******************************************************************************
  * Save EL2 sysreg context
  ******************************************************************************/
@@ -994,7 +1077,7 @@
 
 		el2_sysregs_context_save_common(el2_sysregs_ctx);
 #if CTX_INCLUDE_MTE_REGS
-		el2_sysregs_context_save_mte(el2_sysregs_ctx);
+		write_ctx_reg(el2_sysregs_ctx, CTX_TFSR_EL2, read_tfsr_el2());
 #endif
 		if (is_feat_mpam_supported()) {
 			el2_sysregs_context_save_mpam(el2_sysregs_ctx);
@@ -1083,7 +1166,7 @@
 
 		el2_sysregs_context_restore_common(el2_sysregs_ctx);
 #if CTX_INCLUDE_MTE_REGS
-		el2_sysregs_context_restore_mte(el2_sysregs_ctx);
+		write_tfsr_el2(read_ctx_reg(el2_sysregs_ctx, CTX_TFSR_EL2));
 #endif
 		if (is_feat_mpam_supported()) {
 			el2_sysregs_context_restore_mpam(el2_sysregs_ctx);