diff --git a/include/arch/aarch64/arch_helpers.h b/include/arch/aarch64/arch_helpers.h
index 6fdc7e8..2d97018 100644
--- a/include/arch/aarch64/arch_helpers.h
+++ b/include/arch/aarch64/arch_helpers.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2023, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2024, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -464,6 +464,9 @@
 DEFINE_SYSREG_RW_FUNCS(cntp_cval_el0)
 DEFINE_SYSREG_READ_FUNC(cntpct_el0)
 DEFINE_SYSREG_RW_FUNCS(cnthctl_el2)
+DEFINE_SYSREG_RW_FUNCS(cntv_ctl_el0)
+DEFINE_SYSREG_RW_FUNCS(cntv_cval_el0)
+DEFINE_SYSREG_RW_FUNCS(cntkctl_el1)
 
 DEFINE_SYSREG_RW_FUNCS(vtcr_el2)
 
@@ -480,6 +483,9 @@
 #define clr_cntp_ctl_enable(x)  ((x) &= ~(U(1) << CNTP_CTL_ENABLE_SHIFT))
 #define clr_cntp_ctl_imask(x)   ((x) &= ~(U(1) << CNTP_CTL_IMASK_SHIFT))
 
+DEFINE_SYSREG_RW_FUNCS(tpidr_el0)
+DEFINE_SYSREG_RW_FUNCS(tpidr_el1)
+DEFINE_SYSREG_RW_FUNCS(tpidr_el2)
 DEFINE_SYSREG_RW_FUNCS(tpidr_el3)
 
 DEFINE_SYSREG_RW_FUNCS(cntvoff_el2)
@@ -489,7 +495,7 @@
 
 DEFINE_SYSREG_RW_FUNCS(hacr_el2)
 DEFINE_SYSREG_RW_FUNCS(hpfar_el2)
-DEFINE_SYSREG_RW_FUNCS(tpidr_el2)
+
 DEFINE_SYSREG_RW_FUNCS(dbgvcr32_el2)
 DEFINE_RENAME_SYSREG_RW_FUNCS(ich_hcr_el2, ICH_HCR_EL2)
 DEFINE_RENAME_SYSREG_RW_FUNCS(ich_vmcr_el2, ICH_VMCR_EL2)
@@ -501,6 +507,16 @@
 DEFINE_SYSREG_RW_FUNCS(hstr_el2)
 DEFINE_SYSREG_RW_FUNCS(pmcr_el0)
 
+DEFINE_SYSREG_RW_FUNCS(csselr_el1)
+DEFINE_SYSREG_RW_FUNCS(tpidrro_el0)
+DEFINE_SYSREG_RW_FUNCS(contextidr_el1)
+DEFINE_SYSREG_RW_FUNCS(spsr_abt)
+DEFINE_SYSREG_RW_FUNCS(spsr_und)
+DEFINE_SYSREG_RW_FUNCS(spsr_irq)
+DEFINE_SYSREG_RW_FUNCS(spsr_fiq)
+DEFINE_SYSREG_RW_FUNCS(dacr32_el2)
+DEFINE_SYSREG_RW_FUNCS(ifsr32_el2)
+
 /* GICv3 System Registers */
 
 DEFINE_RENAME_SYSREG_RW_FUNCS(icc_sre_el1, ICC_SRE_EL1)
diff --git a/include/lib/el3_runtime/aarch64/context.h b/include/lib/el3_runtime/aarch64/context.h
index 910026e..d5bd890 100644
--- a/include/lib/el3_runtime/aarch64/context.h
+++ b/include/lib/el3_runtime/aarch64/context.h
@@ -574,9 +574,6 @@
 /*******************************************************************************
  * Function prototypes
  ******************************************************************************/
-void el1_sysregs_context_save(el1_sysregs_t *regs);
-void el1_sysregs_context_restore(el1_sysregs_t *regs);
-
 #if CTX_INCLUDE_FPREGS
 void fpregs_context_save(fp_regs_t *regs);
 void fpregs_context_restore(fp_regs_t *regs);
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index 176308e..76aebf9 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -10,8 +10,6 @@
 #include <context.h>
 #include <el3_common_macros.S>
 
-	.global	el1_sysregs_context_save
-	.global	el1_sysregs_context_restore
 #if CTX_INCLUDE_FPREGS
 	.global	fpregs_context_save
 	.global	fpregs_context_restore
@@ -21,220 +19,6 @@
 	.global save_and_update_ptw_el1_sys_regs
 	.global	el3_exit
 
-
-/* ------------------------------------------------------------------
- * The following function strictly follows the AArch64 PCS to use
- * x9-x17 (temporary caller-saved registers) to save EL1 system
- * register context. It assumes that 'x0' is pointing to a
- * 'el1_sys_regs' structure where the register context will be saved.
- * ------------------------------------------------------------------
- */
-func el1_sysregs_context_save
-
-	mrs	x9, spsr_el1
-	mrs	x10, elr_el1
-	stp	x9, x10, [x0, #CTX_SPSR_EL1]
-
-#if !ERRATA_SPECULATIVE_AT
-	mrs	x15, sctlr_el1
-	mrs	x16, tcr_el1
-	stp	x15, x16, [x0, #CTX_SCTLR_EL1]
-#endif /* ERRATA_SPECULATIVE_AT */
-
-	mrs	x17, cpacr_el1
-	mrs	x9, csselr_el1
-	stp	x17, x9, [x0, #CTX_CPACR_EL1]
-
-	mrs	x10, sp_el1
-	mrs	x11, esr_el1
-	stp	x10, x11, [x0, #CTX_SP_EL1]
-
-	mrs	x12, ttbr0_el1
-	mrs	x13, ttbr1_el1
-	stp	x12, x13, [x0, #CTX_TTBR0_EL1]
-
-	mrs	x14, mair_el1
-	mrs	x15, amair_el1
-	stp	x14, x15, [x0, #CTX_MAIR_EL1]
-
-	mrs	x16, actlr_el1
-	mrs	x17, tpidr_el1
-	stp	x16, x17, [x0, #CTX_ACTLR_EL1]
-
-	mrs	x9, tpidr_el0
-	mrs	x10, tpidrro_el0
-	stp	x9, x10, [x0, #CTX_TPIDR_EL0]
-
-	mrs	x13, par_el1
-	mrs	x14, far_el1
-	stp	x13, x14, [x0, #CTX_PAR_EL1]
-
-	mrs	x15, afsr0_el1
-	mrs	x16, afsr1_el1
-	stp	x15, x16, [x0, #CTX_AFSR0_EL1]
-
-	mrs	x17, contextidr_el1
-	mrs	x9, vbar_el1
-	stp	x17, x9, [x0, #CTX_CONTEXTIDR_EL1]
-
-	/* Save AArch32 system registers if the build has instructed so */
-#if CTX_INCLUDE_AARCH32_REGS
-	mrs	x11, spsr_abt
-	mrs	x12, spsr_und
-	stp	x11, x12, [x0, #CTX_SPSR_ABT]
-
-	mrs	x13, spsr_irq
-	mrs	x14, spsr_fiq
-	stp	x13, x14, [x0, #CTX_SPSR_IRQ]
-
-	mrs	x15, dacr32_el2
-	mrs	x16, ifsr32_el2
-	stp	x15, x16, [x0, #CTX_DACR32_EL2]
-#endif /* CTX_INCLUDE_AARCH32_REGS */
-
-	/* Save NS timer registers if the build has instructed so */
-#if NS_TIMER_SWITCH
-	mrs	x10, cntp_ctl_el0
-	mrs	x11, cntp_cval_el0
-	stp	x10, x11, [x0, #CTX_CNTP_CTL_EL0]
-
-	mrs	x12, cntv_ctl_el0
-	mrs	x13, cntv_cval_el0
-	stp	x12, x13, [x0, #CTX_CNTV_CTL_EL0]
-
-	mrs	x14, cntkctl_el1
-	str	x14, [x0, #CTX_CNTKCTL_EL1]
-#endif /* NS_TIMER_SWITCH */
-
-	/* Save MTE system registers if the build has instructed so */
-#if ENABLE_FEAT_MTE
-#if ENABLE_FEAT_MTE == 2
-	mrs x8, id_aa64pfr1_el1
-	and x8, x8, #(ID_AA64PFR1_EL1_MTE_MASK << ID_AA64PFR1_EL1_MTE_SHIFT)
-	cbz x8, no_mte_save
-#endif
-	mrs	x15, TFSRE0_EL1
-	mrs	x16, TFSR_EL1
-	stp	x15, x16, [x0, #CTX_TFSRE0_EL1]
-
-	mrs	x9, RGSR_EL1
-	mrs	x10, GCR_EL1
-	stp	x9, x10, [x0, #CTX_RGSR_EL1]
-
-no_mte_save:
-#endif /* ENABLE_FEAT_MTE */
-
-	ret
-endfunc el1_sysregs_context_save
-
-/* ------------------------------------------------------------------
- * The following function strictly follows the AArch64 PCS to use
- * x9-x17 (temporary caller-saved registers) to restore EL1 system
- * register context.  It assumes that 'x0' is pointing to a
- * 'el1_sys_regs' structure from where the register context will be
- * restored
- * ------------------------------------------------------------------
- */
-func el1_sysregs_context_restore
-
-	ldp	x9, x10, [x0, #CTX_SPSR_EL1]
-	msr	spsr_el1, x9
-	msr	elr_el1, x10
-
-#if !ERRATA_SPECULATIVE_AT
-	ldp	x15, x16, [x0, #CTX_SCTLR_EL1]
-	msr	sctlr_el1, x15
-	msr	tcr_el1, x16
-#endif /* ERRATA_SPECULATIVE_AT */
-
-	ldp	x17, x9, [x0, #CTX_CPACR_EL1]
-	msr	cpacr_el1, x17
-	msr	csselr_el1, x9
-
-	ldp	x10, x11, [x0, #CTX_SP_EL1]
-	msr	sp_el1, x10
-	msr	esr_el1, x11
-
-	ldp	x12, x13, [x0, #CTX_TTBR0_EL1]
-	msr	ttbr0_el1, x12
-	msr	ttbr1_el1, x13
-
-	ldp	x14, x15, [x0, #CTX_MAIR_EL1]
-	msr	mair_el1, x14
-	msr	amair_el1, x15
-
-	ldp 	x16, x17, [x0, #CTX_ACTLR_EL1]
-	msr	actlr_el1, x16
-	msr	tpidr_el1, x17
-
-	ldp	x9, x10, [x0, #CTX_TPIDR_EL0]
-	msr	tpidr_el0, x9
-	msr	tpidrro_el0, x10
-
-	ldp	x13, x14, [x0, #CTX_PAR_EL1]
-	msr	par_el1, x13
-	msr	far_el1, x14
-
-	ldp	x15, x16, [x0, #CTX_AFSR0_EL1]
-	msr	afsr0_el1, x15
-	msr	afsr1_el1, x16
-
-	ldp	x17, x9, [x0, #CTX_CONTEXTIDR_EL1]
-	msr	contextidr_el1, x17
-	msr	vbar_el1, x9
-
-	/* Restore AArch32 system registers if the build has instructed so */
-#if CTX_INCLUDE_AARCH32_REGS
-	ldp	x11, x12, [x0, #CTX_SPSR_ABT]
-	msr	spsr_abt, x11
-	msr	spsr_und, x12
-
-	ldp	x13, x14, [x0, #CTX_SPSR_IRQ]
-	msr	spsr_irq, x13
-	msr	spsr_fiq, x14
-
-	ldp	x15, x16, [x0, #CTX_DACR32_EL2]
-	msr	dacr32_el2, x15
-	msr	ifsr32_el2, x16
-#endif /* CTX_INCLUDE_AARCH32_REGS */
-
-	/* Restore NS timer registers if the build has instructed so */
-#if NS_TIMER_SWITCH
-	ldp	x10, x11, [x0, #CTX_CNTP_CTL_EL0]
-	msr	cntp_ctl_el0, x10
-	msr	cntp_cval_el0, x11
-
-	ldp	x12, x13, [x0, #CTX_CNTV_CTL_EL0]
-	msr	cntv_ctl_el0, x12
-	msr	cntv_cval_el0, x13
-
-	ldr	x14, [x0, #CTX_CNTKCTL_EL1]
-	msr	cntkctl_el1, x14
-#endif /* NS_TIMER_SWITCH */
-
-	/* Restore MTE system registers if the build has instructed so */
-#if ENABLE_FEAT_MTE
-#if ENABLE_FEAT_MTE == 2
-	mrs x8, id_aa64pfr1_el1
-	and x8, x8, #(ID_AA64PFR1_EL1_MTE_MASK << ID_AA64PFR1_EL1_MTE_SHIFT)
-	cbz x8, no_mte_restore
-#endif
-
-	ldp	x11, x12, [x0, #CTX_TFSRE0_EL1]
-	msr	TFSRE0_EL1, x11
-	msr	TFSR_EL1, x12
-
-	ldp	x13, x14, [x0, #CTX_RGSR_EL1]
-	msr	RGSR_EL1, x13
-	msr	GCR_EL1, x14
-
-no_mte_restore:
-#endif /* ENABLE_FEAT_MTE */
-
-	/* No explict ISB required here as ERET covers it */
-	ret
-endfunc el1_sysregs_context_restore
-
 /* ------------------------------------------------------------------
  * The following function follows the aapcs_64 strictly to use
  * x9-x17 (temporary caller-saved registers according to AArch64 PCS)
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index c3b7e78..52e917e 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -1429,6 +1429,116 @@
 #endif /* CTX_INCLUDE_EL2_REGS */
 }
 
+static void el1_sysregs_context_save(el1_sysregs_t *ctx)
+{
+	write_ctx_reg(ctx, CTX_SPSR_EL1, read_spsr_el1());
+	write_ctx_reg(ctx, CTX_ELR_EL1, read_elr_el1());
+
+#if !ERRATA_SPECULATIVE_AT
+	write_ctx_reg(ctx, CTX_SCTLR_EL1, read_sctlr_el1());
+	write_ctx_reg(ctx, CTX_TCR_EL1, read_tcr_el1());
+#endif /* (!ERRATA_SPECULATIVE_AT) */
+
+	write_ctx_reg(ctx, CTX_CPACR_EL1, read_cpacr_el1());
+	write_ctx_reg(ctx, CTX_CSSELR_EL1, read_csselr_el1());
+	write_ctx_reg(ctx, CTX_SP_EL1, read_sp_el1());
+	write_ctx_reg(ctx, CTX_ESR_EL1, read_esr_el1());
+	write_ctx_reg(ctx, CTX_TTBR0_EL1, read_ttbr0_el1());
+	write_ctx_reg(ctx, CTX_TTBR1_EL1, read_ttbr1_el1());
+	write_ctx_reg(ctx, CTX_MAIR_EL1, read_mair_el1());
+	write_ctx_reg(ctx, CTX_AMAIR_EL1, read_amair_el1());
+	write_ctx_reg(ctx, CTX_ACTLR_EL1, read_actlr_el1());
+	write_ctx_reg(ctx, CTX_TPIDR_EL1, read_tpidr_el1());
+	write_ctx_reg(ctx, CTX_TPIDR_EL0, read_tpidr_el0());
+	write_ctx_reg(ctx, CTX_TPIDRRO_EL0, read_tpidrro_el0());
+	write_ctx_reg(ctx, CTX_PAR_EL1, read_par_el1());
+	write_ctx_reg(ctx, CTX_FAR_EL1, read_far_el1());
+	write_ctx_reg(ctx, CTX_AFSR0_EL1, read_afsr0_el1());
+	write_ctx_reg(ctx, CTX_AFSR1_EL1, read_afsr1_el1());
+	write_ctx_reg(ctx, CTX_CONTEXTIDR_EL1, read_contextidr_el1());
+	write_ctx_reg(ctx, CTX_VBAR_EL1, read_vbar_el1());
+
+#if CTX_INCLUDE_AARCH32_REGS
+	write_ctx_reg(ctx, CTX_SPSR_ABT, read_spsr_abt());
+	write_ctx_reg(ctx, CTX_SPSR_UND, read_spsr_und());
+	write_ctx_reg(ctx, CTX_SPSR_IRQ, read_spsr_irq());
+	write_ctx_reg(ctx, CTX_SPSR_FIQ, read_spsr_fiq());
+	write_ctx_reg(ctx, CTX_DACR32_EL2, read_dacr32_el2());
+	write_ctx_reg(ctx, CTX_IFSR32_EL2, read_ifsr32_el2());
+#endif /* CTX_INCLUDE_AARCH32_REGS */
+
+#if NS_TIMER_SWITCH
+	write_ctx_reg(ctx, CTX_CNTP_CTL_EL0, read_cntp_ctl_el0());
+	write_ctx_reg(ctx, CTX_CNTP_CVAL_EL0, read_cntp_cval_el0());
+	write_ctx_reg(ctx, CTX_CNTV_CTL_EL0, read_cntv_ctl_el0());
+	write_ctx_reg(ctx, CTX_CNTV_CVAL_EL0, read_cntv_cval_el0());
+	write_ctx_reg(ctx, CTX_CNTKCTL_EL1, read_cntkctl_el1());
+#endif /* NS_TIMER_SWITCH */
+
+#if ENABLE_FEAT_MTE
+	write_ctx_reg(ctx, CTX_TFSRE0_EL1, read_tfsre0_el1());
+	write_ctx_reg(ctx, CTX_TFSR_EL1, read_tfsr_el1());
+	write_ctx_reg(ctx, CTX_RGSR_EL1, read_rgsr_el1());
+	write_ctx_reg(ctx, CTX_GCR_EL1, read_gcr_el1());
+#endif /* ENABLE_FEAT_MTE */
+
+}
+
+static void el1_sysregs_context_restore(el1_sysregs_t *ctx)
+{
+	write_spsr_el1(read_ctx_reg(ctx, CTX_SPSR_EL1));
+	write_elr_el1(read_ctx_reg(ctx, CTX_ELR_EL1));
+
+#if !ERRATA_SPECULATIVE_AT
+	write_sctlr_el1(read_ctx_reg(ctx, CTX_SCTLR_EL1));
+	write_tcr_el1(read_ctx_reg(ctx, CTX_TCR_EL1));
+#endif /* (!ERRATA_SPECULATIVE_AT) */
+
+	write_cpacr_el1(read_ctx_reg(ctx, CTX_CPACR_EL1));
+	write_csselr_el1(read_ctx_reg(ctx, CTX_CSSELR_EL1));
+	write_sp_el1(read_ctx_reg(ctx, CTX_SP_EL1));
+	write_esr_el1(read_ctx_reg(ctx, CTX_ESR_EL1));
+	write_ttbr0_el1(read_ctx_reg(ctx, CTX_TTBR0_EL1));
+	write_ttbr1_el1(read_ctx_reg(ctx, CTX_TTBR1_EL1));
+	write_mair_el1(read_ctx_reg(ctx, CTX_MAIR_EL1));
+	write_amair_el1(read_ctx_reg(ctx, CTX_AMAIR_EL1));
+	write_actlr_el1(read_ctx_reg(ctx, CTX_ACTLR_EL1));
+	write_tpidr_el1(read_ctx_reg(ctx, CTX_TPIDR_EL1));
+	write_tpidr_el0(read_ctx_reg(ctx, CTX_TPIDR_EL0));
+	write_tpidrro_el0(read_ctx_reg(ctx, CTX_TPIDRRO_EL0));
+	write_par_el1(read_ctx_reg(ctx, CTX_PAR_EL1));
+	write_far_el1(read_ctx_reg(ctx, CTX_FAR_EL1));
+	write_afsr0_el1(read_ctx_reg(ctx, CTX_AFSR0_EL1));
+	write_afsr1_el1(read_ctx_reg(ctx, CTX_AFSR1_EL1));
+	write_contextidr_el1(read_ctx_reg(ctx, CTX_CONTEXTIDR_EL1));
+	write_vbar_el1(read_ctx_reg(ctx, CTX_VBAR_EL1));
+
+#if CTX_INCLUDE_AARCH32_REGS
+	write_spsr_abt(read_ctx_reg(ctx, CTX_SPSR_ABT));
+	write_spsr_und(read_ctx_reg(ctx, CTX_SPSR_UND));
+	write_spsr_irq(read_ctx_reg(ctx, CTX_SPSR_IRQ));
+	write_spsr_fiq(read_ctx_reg(ctx, CTX_SPSR_FIQ));
+	write_dacr32_el2(read_ctx_reg(ctx, CTX_DACR32_EL2));
+	write_ifsr32_el2(read_ctx_reg(ctx, CTX_IFSR32_EL2));
+#endif /* CTX_INCLUDE_AARCH32_REGS */
+
+#if NS_TIMER_SWITCH
+	write_cntp_ctl_el0(read_ctx_reg(ctx, CTX_CNTP_CTL_EL0));
+	write_cntp_cval_el0(read_ctx_reg(ctx, CTX_CNTP_CVAL_EL0));
+	write_cntv_ctl_el0(read_ctx_reg(ctx, CTX_CNTV_CTL_EL0));
+	write_cntv_cval_el0(read_ctx_reg(ctx, CTX_CNTV_CVAL_EL0));
+	write_cntkctl_el1(read_ctx_reg(ctx, CTX_CNTKCTL_EL1));
+#endif /* NS_TIMER_SWITCH */
+
+#if ENABLE_FEAT_MTE
+	write_tfsre0_el1(read_ctx_reg(ctx, CTX_TFSRE0_EL1));
+	write_tfsr_el1(read_ctx_reg(ctx, CTX_TFSR_EL1));
+	write_rgsr_el1(read_ctx_reg(ctx, CTX_RGSR_EL1));
+	write_gcr_el1(read_ctx_reg(ctx, CTX_GCR_EL1));
+#endif /* ENABLE_FEAT_MTE */
+
+}
+
 /*******************************************************************************
  * The next four functions are used by runtime services to save and restore
  * EL1 context on the 'cpu_context' structure for the specified security