diff --git a/bl1/aarch64/bl1_arch_setup.c b/bl1/aarch64/bl1_arch_setup.c
index e82ad09..f9443aa 100644
--- a/bl1/aarch64/bl1_arch_setup.c
+++ b/bl1/aarch64/bl1_arch_setup.c
@@ -41,10 +41,10 @@
 	unsigned long tmp_reg = 0;
 
 	/* Enable alignment checks and set the exception endianess to LE */
-	tmp_reg = read_sctlr();
+	tmp_reg = read_sctlr_el3();
 	tmp_reg |= (SCTLR_A_BIT | SCTLR_SA_BIT);
 	tmp_reg &= ~SCTLR_EE_BIT;
-	write_sctlr(tmp_reg);
+	write_sctlr_el3(tmp_reg);
 
 	/*
 	 * Enable HVCs, route FIQs to EL3, set the next EL to be AArch64, route
@@ -67,11 +67,10 @@
  * Set the Secure EL1 required architectural state
  ******************************************************************************/
 void bl1_arch_next_el_setup(void) {
-	unsigned long current_sctlr, next_sctlr;
+	unsigned long next_sctlr;
 
 	/* Use the same endianness than the current BL */
-	current_sctlr = read_sctlr();
-	next_sctlr = (current_sctlr & SCTLR_EE_BIT);
+	next_sctlr = (read_sctlr_el3() & SCTLR_EE_BIT);
 
 	/* Set SCTLR Secure EL1 */
 	next_sctlr |= SCTLR_EL1_RES1;
diff --git a/bl1/aarch64/early_exceptions.S b/bl1/aarch64/early_exceptions.S
index d06e854..ee91a89 100644
--- a/bl1/aarch64/early_exceptions.S
+++ b/bl1/aarch64/early_exceptions.S
@@ -195,7 +195,7 @@
 	mov	x0, #SYNC_EXCEPTION_AARCH64
 	bl	plat_report_exception
 
-	bl	read_esr
+	bl	read_esr_el3
 	ubfx	x1, x0, #ESR_EC_SHIFT, #ESR_EC_LENGTH
 	cmp	x1, #EC_AARCH64_SMC
 	b.ne	panic
@@ -224,9 +224,9 @@
 	 * ---------------------------------------------
 	 */
 	mov	x1, #(SCTLR_M_BIT | SCTLR_C_BIT | SCTLR_I_BIT)
-	bl	read_sctlr
+	bl	read_sctlr_el3
 	bic	x0, x0, x1
-	bl	write_sctlr
+	bl	write_sctlr_el3
 	mov	x0, #DCCISW
 	bl	dcsw_op_all
 	bl	tlbialle3
diff --git a/bl1/bl1_main.c b/bl1/bl1_main.c
index 401a525..dbf6c9d 100644
--- a/bl1/bl1_main.c
+++ b/bl1/bl1_main.c
@@ -48,7 +48,7 @@
 void bl1_main(void)
 {
 #if DEBUG
-	unsigned long sctlr_el3 = read_sctlr();
+	unsigned long sctlr_el3 = read_sctlr_el3();
 #endif
 	unsigned long bl2_base;
 	unsigned int load_type = TOP_LOAD, spsr;
diff --git a/bl31/aarch64/bl31_arch_setup.c b/bl31/aarch64/bl31_arch_setup.c
index faeccf3..2a424f8 100644
--- a/bl31/aarch64/bl31_arch_setup.c
+++ b/bl31/aarch64/bl31_arch_setup.c
@@ -42,10 +42,10 @@
 	unsigned long tmp_reg = 0;
 
 	/* Enable alignment checks and set the exception endianness to LE */
-	tmp_reg = read_sctlr();
+	tmp_reg = read_sctlr_el3();
 	tmp_reg |= (SCTLR_A_BIT | SCTLR_SA_BIT);
 	tmp_reg &= ~SCTLR_EE_BIT;
-	write_sctlr(tmp_reg);
+	write_sctlr_el3(tmp_reg);
 
 	/*
 	 * Enable HVCs, route FIQs to EL3, set the next EL to be AArch64, route
@@ -72,13 +72,12 @@
 void bl31_next_el_arch_setup(uint32_t security_state)
 {
 	unsigned long id_aa64pfr0 = read_id_aa64pfr0_el1();
-	unsigned long current_sctlr, next_sctlr;
+	unsigned long next_sctlr;
 	unsigned long el_status;
 	unsigned long scr = read_scr();
 
 	/* Use the same endianness than the current BL */
-	current_sctlr = read_sctlr();
-	next_sctlr = (current_sctlr & SCTLR_EE_BIT);
+	next_sctlr = (read_sctlr_el3() & SCTLR_EE_BIT);
 
 	/* Find out which EL we are going to */
 	el_status = (id_aa64pfr0 >> ID_AA64PFR0_EL2_SHIFT) & ID_AA64PFR0_ELX_MASK;
diff --git a/common/bl_common.c b/common/bl_common.c
index da027b6..b03165b 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -78,8 +78,8 @@
 		     unsigned long spsr,
 		     unsigned long entrypoint)
 {
-	write_spsr(spsr);
-	write_elr(entrypoint);
+	write_spsr_el3(spsr);
+	write_elr_el3(entrypoint);
 	eret(args->arg0,
 	     args->arg1,
 	     args->arg2,
diff --git a/include/aarch64/arch_helpers.h b/include/aarch64/arch_helpers.h
index fe96b4d..f55c003 100644
--- a/include/aarch64/arch_helpers.h
+++ b/include/aarch64/arch_helpers.h
@@ -97,21 +97,17 @@
 extern unsigned long read_id_aa64pfr0_el1(void);
 extern unsigned long read_current_el(void);
 extern unsigned long read_daif(void);
-extern unsigned long read_spsr(void);
 extern unsigned long read_spsr_el1(void);
 extern unsigned long read_spsr_el2(void);
 extern unsigned long read_spsr_el3(void);
-extern unsigned long read_elr(void);
 extern unsigned long read_elr_el1(void);
 extern unsigned long read_elr_el2(void);
 extern unsigned long read_elr_el3(void);
 
 extern void write_daif(unsigned long);
-extern void write_spsr(unsigned long);
 extern void write_spsr_el1(unsigned long);
 extern void write_spsr_el2(unsigned long);
 extern void write_spsr_el3(unsigned long);
-extern void write_elr(unsigned long);
 extern void write_elr_el1(unsigned long);
 extern void write_elr_el2(unsigned long);
 extern void write_elr_el3(unsigned long);
@@ -145,67 +141,54 @@
 extern unsigned long read_scr(void);
 extern unsigned long read_hcr(void);
 
-extern unsigned long read_vbar(void);
 extern unsigned long read_vbar_el1(void);
 extern unsigned long read_vbar_el2(void);
 extern unsigned long read_vbar_el3(void);
 
-extern unsigned long read_sctlr(void);
 extern unsigned long read_sctlr_el1(void);
 extern unsigned long read_sctlr_el2(void);
 extern unsigned long read_sctlr_el3(void);
 
-extern unsigned long read_actlr(void);
 extern unsigned long read_actlr_el1(void);
 extern unsigned long read_actlr_el2(void);
 extern unsigned long read_actlr_el3(void);
 
-extern unsigned long read_esr(void);
 extern unsigned long read_esr_el1(void);
 extern unsigned long read_esr_el2(void);
 extern unsigned long read_esr_el3(void);
 
-extern unsigned long read_afsr0(void);
 extern unsigned long read_afsr0_el1(void);
 extern unsigned long read_afsr0_el2(void);
 extern unsigned long read_afsr0_el3(void);
 
-extern unsigned long read_afsr1(void);
 extern unsigned long read_afsr1_el1(void);
 extern unsigned long read_afsr1_el2(void);
 extern unsigned long read_afsr1_el3(void);
 
-extern unsigned long read_far(void);
 extern unsigned long read_far_el1(void);
 extern unsigned long read_far_el2(void);
 extern unsigned long read_far_el3(void);
 
-extern unsigned long read_mair(void);
 extern unsigned long read_mair_el1(void);
 extern unsigned long read_mair_el2(void);
 extern unsigned long read_mair_el3(void);
 
-extern unsigned long read_amair(void);
 extern unsigned long read_amair_el1(void);
 extern unsigned long read_amair_el2(void);
 extern unsigned long read_amair_el3(void);
 
-extern unsigned long read_rvbar(void);
 extern unsigned long read_rvbar_el1(void);
 extern unsigned long read_rvbar_el2(void);
 extern unsigned long read_rvbar_el3(void);
 
-extern unsigned long read_rmr(void);
 extern unsigned long read_rmr_el1(void);
 extern unsigned long read_rmr_el2(void);
 extern unsigned long read_rmr_el3(void);
 
-extern unsigned long read_tcr(void);
 extern unsigned long read_tcr_el1(void);
 extern unsigned long read_tcr_el2(void);
 extern unsigned long read_tcr_el3(void);
 
-extern unsigned long read_ttbr0(void);
 extern unsigned long read_ttbr0_el1(void);
 extern unsigned long read_ttbr0_el2(void);
 extern unsigned long read_ttbr0_el3(void);
@@ -214,7 +197,6 @@
 extern unsigned long read_ttbr1_el1(void);
 extern unsigned long read_ttbr1_el2(void);
 
-extern unsigned long read_cptr(void);
 extern unsigned long read_cptr_el2(void);
 extern unsigned long read_cptr_el3(void);
 
@@ -229,22 +211,18 @@
 extern void write_cntfrq_el0(unsigned int);
 extern void write_cnthctl_el2(unsigned long);
 
-extern void write_vbar(unsigned long);
 extern void write_vbar_el1(unsigned long);
 extern void write_vbar_el2(unsigned long);
 extern void write_vbar_el3(unsigned long);
 
-extern void write_sctlr(unsigned long);
 extern void write_sctlr_el1(unsigned long);
 extern void write_sctlr_el2(unsigned long);
 extern void write_sctlr_el3(unsigned long);
 
-extern void write_actlr(unsigned long);
 extern void write_actlr_el1(unsigned long);
 extern void write_actlr_el2(unsigned long);
 extern void write_actlr_el3(unsigned long);
 
-extern void write_esr(unsigned long);
 extern void write_esr_el1(unsigned long);
 extern void write_esr_el2(unsigned long);
 extern void write_esr_el3(unsigned long);
@@ -259,41 +237,33 @@
 extern void write_afsr1_el2(unsigned long);
 extern void write_afsr1_el3(unsigned long);
 
-extern void write_far(unsigned long);
 extern void write_far_el1(unsigned long);
 extern void write_far_el2(unsigned long);
 extern void write_far_el3(unsigned long);
 
-extern void write_mair(unsigned long);
 extern void write_mair_el1(unsigned long);
 extern void write_mair_el2(unsigned long);
 extern void write_mair_el3(unsigned long);
 
-extern void write_amair(unsigned long);
 extern void write_amair_el1(unsigned long);
 extern void write_amair_el2(unsigned long);
 extern void write_amair_el3(unsigned long);
 
-extern void write_rmr(unsigned long);
 extern void write_rmr_el1(unsigned long);
 extern void write_rmr_el2(unsigned long);
 extern void write_rmr_el3(unsigned long);
 
-extern void write_tcr(unsigned long);
 extern void write_tcr_el1(unsigned long);
 extern void write_tcr_el2(unsigned long);
 extern void write_tcr_el3(unsigned long);
 
-extern void write_ttbr0(unsigned long);
 extern void write_ttbr0_el1(unsigned long);
 extern void write_ttbr0_el2(unsigned long);
 extern void write_ttbr0_el3(unsigned long);
 
-extern void write_ttbr1(unsigned long);
 extern void write_ttbr1_el1(unsigned long);
 extern void write_ttbr1_el2(unsigned long);
 
-extern void write_cptr(unsigned long);
 extern void write_cpuectlr(unsigned long);
 extern void write_cptr_el2(unsigned long);
 extern void write_cptr_el3(unsigned long);
diff --git a/lib/arch/aarch64/sysreg_helpers.S b/lib/arch/aarch64/sysreg_helpers.S
index 6650419..dfd0f2f 100644
--- a/lib/arch/aarch64/sysreg_helpers.S
+++ b/lib/arch/aarch64/sysreg_helpers.S
@@ -30,127 +30,99 @@
 
 #include <arch_helpers.h>
 
-	.globl	read_vbar
 	.globl	read_vbar_el1
 	.globl	read_vbar_el2
 	.globl	read_vbar_el3
-	.globl	write_vbar
 	.globl	write_vbar_el1
 	.globl	write_vbar_el2
 	.globl	write_vbar_el3
 
-	.globl	read_sctlr
 	.globl	read_sctlr_el1
 	.globl	read_sctlr_el2
 	.globl	read_sctlr_el3
-	.globl	write_sctlr
 	.globl	write_sctlr_el1
 	.globl	write_sctlr_el2
 	.globl	write_sctlr_el3
 
-	.globl	read_actlr
 	.globl	read_actlr_el1
 	.globl	read_actlr_el2
 	.globl	read_actlr_el3
-	.globl	write_actlr
 	.globl	write_actlr_el1
 	.globl	write_actlr_el2
 	.globl	write_actlr_el3
 
-	.globl	read_esr
 	.globl	read_esr_el1
 	.globl	read_esr_el2
 	.globl	read_esr_el3
-	.globl	write_esr
 	.globl	write_esr_el1
 	.globl	write_esr_el2
 	.globl	write_esr_el3
 
-	.globl	read_afsr0
 	.globl	read_afsr0_el1
 	.globl	read_afsr0_el2
 	.globl	read_afsr0_el3
-	.globl	write_afsr0
 	.globl	write_afsr0_el1
 	.globl	write_afsr0_el2
 	.globl	write_afsr0_el3
 
-	.globl	read_afsr1
 	.globl	read_afsr1_el1
 	.globl	read_afsr1_el2
 	.globl	read_afsr1_el3
-	.globl	write_afsr1
 	.globl	write_afsr1_el1
 	.globl	write_afsr1_el2
 	.globl	write_afsr1_el3
 
-	.globl	read_far
 	.globl	read_far_el1
 	.globl	read_far_el2
 	.globl	read_far_el3
-	.globl	write_far
 	.globl	write_far_el1
 	.globl	write_far_el2
 	.globl	write_far_el3
 
-	.globl	read_mair
 	.globl	read_mair_el1
 	.globl	read_mair_el2
 	.globl	read_mair_el3
-	.globl	write_mair
 	.globl	write_mair_el1
 	.globl	write_mair_el2
 	.globl	write_mair_el3
 
-	.globl	read_amair
 	.globl	read_amair_el1
 	.globl	read_amair_el2
 	.globl	read_amair_el3
-	.globl	write_amair
 	.globl	write_amair_el1
 	.globl	write_amair_el2
 	.globl	write_amair_el3
 
-	.globl	read_rvbar
 	.globl	read_rvbar_el1
 	.globl	read_rvbar_el2
 	.globl	read_rvbar_el3
 
-	.globl	read_rmr
 	.globl	read_rmr_el1
 	.globl	read_rmr_el2
 	.globl	read_rmr_el3
-	.globl	write_rmr
 	.globl	write_rmr_el1
 	.globl	write_rmr_el2
 	.globl	write_rmr_el3
 
-	.globl	read_tcr
 	.globl	read_tcr_el1
 	.globl	read_tcr_el2
 	.globl	read_tcr_el3
-	.globl	write_tcr
 	.globl	write_tcr_el1
 	.globl	write_tcr_el2
 	.globl	write_tcr_el3
 
-	.globl	read_cptr
 	.globl	read_cptr_el2
 	.globl	read_cptr_el3
-	.globl	write_cptr
 	.globl	write_cptr_el2
 	.globl	write_cptr_el3
 
-	.globl	read_ttbr0
 	.globl	read_ttbr0_el1
 	.globl	read_ttbr0_el2
 	.globl	read_ttbr0_el3
-	.globl	write_ttbr0
 	.globl	write_ttbr0_el1
 	.globl	write_ttbr0_el2
 	.globl	write_ttbr0_el3
 
-	.globl	read_ttbr1
 	.globl	read_ttbr1_el1
 	.globl	read_ttbr1_el2
 	.globl	write_ttbr1
@@ -213,16 +185,6 @@
 	 * VBAR accessors
 	 * -----------------------------------------------------
 	 */
-read_vbar: ; .type read_vbar, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_vbar_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_vbar_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_vbar_el3
-
-
 read_vbar_el1: ; .type read_vbar_el1, %function
 	mrs	x0, vbar_el1
 	ret
@@ -238,16 +200,6 @@
 	ret
 
 
-write_vbar: ; .type write_vbar, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_vbar_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_vbar_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_vbar_el3
-
-
 write_vbar_el1: ; .type write_vbar_el1, %function
 	msr	vbar_el1, x0
 	isb
@@ -270,16 +222,6 @@
 	 * AFSR0 accessors
 	 * -----------------------------------------------------
 	 */
-read_afsr0: ; .type read_afsr0, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_afsr0_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_afsr0_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_afsr0_el3
-
-
 read_afsr0_el1: ; .type read_afsr0_el1, %function
 	mrs	x0, afsr0_el1
 	ret
@@ -295,16 +237,6 @@
 	ret
 
 
-write_afsr0: ; .type write_afsr0, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_afsr0_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_afsr0_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_afsr0_el3
-
-
 write_afsr0_el1: ; .type write_afsr0_el1, %function
 	msr	afsr0_el1, x0
 	isb
@@ -327,16 +259,6 @@
 	 * FAR accessors
 	 * -----------------------------------------------------
 	 */
-read_far: ; .type read_far, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_far_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_far_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_far_el3
-
-
 read_far_el1: ; .type read_far_el1, %function
 	mrs	x0, far_el1
 	ret
@@ -352,16 +274,6 @@
 	ret
 
 
-write_far: ; .type write_far, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_far_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_far_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_far_el3
-
-
 write_far_el1: ; .type write_far_el1, %function
 	msr	far_el1, x0
 	isb
@@ -384,16 +296,6 @@
 	 * MAIR accessors
 	 * -----------------------------------------------------
 	 */
-read_mair: ; .type read_mair, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_mair_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_mair_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_mair_el3
-
-
 read_mair_el1: ; .type read_mair_el1, %function
 	mrs	x0, mair_el1
 	ret
@@ -409,16 +311,6 @@
 	ret
 
 
-write_mair: ; .type write_mair, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_mair_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_mair_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_mair_el3
-
-
 write_mair_el1: ; .type write_mair_el1, %function
 	msr	mair_el1, x0
 	isb
@@ -441,16 +333,6 @@
 	 * AMAIR accessors
 	 * -----------------------------------------------------
 	 */
-read_amair: ; .type read_amair, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_amair_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_amair_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_amair_el3
-
-
 read_amair_el1: ; .type read_amair_el1, %function
 	mrs	x0, amair_el1
 	ret
@@ -466,16 +348,6 @@
 	ret
 
 
-write_amair: ; .type write_amair, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_amair_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_amair_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_amair_el3
-
-
 write_amair_el1: ; .type write_amair_el1, %function
 	msr	amair_el1, x0
 	isb
@@ -498,16 +370,6 @@
 	 * RVBAR accessors
 	 * -----------------------------------------------------
 	 */
-read_rvbar: ; .type read_rvbar, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_rvbar_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_rvbar_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_rvbar_el3
-
-
 read_rvbar_el1: ; .type read_rvbar_el1, %function
 	mrs	x0, rvbar_el1
 	ret
@@ -527,16 +389,6 @@
 	 * RMR accessors
 	 * -----------------------------------------------------
 	 */
-read_rmr: ; .type read_rmr, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_rmr_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_rmr_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_rmr_el3
-
-
 read_rmr_el1: ; .type read_rmr_el1, %function
 	mrs	x0, rmr_el1
 	ret
@@ -552,16 +404,6 @@
 	ret
 
 
-write_rmr: ; .type write_rmr, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_rmr_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_rmr_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_rmr_el3
-
-
 write_rmr_el1: ; .type write_rmr_el1, %function
 	msr	rmr_el1, x0
 	isb
@@ -580,16 +422,6 @@
 	ret
 
 
-read_afsr1: ; .type read_afsr1, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_afsr1_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_afsr1_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_afsr1_el3
-
-
 	/* -----------------------------------------------------
 	 * AFSR1 accessors
 	 * -----------------------------------------------------
@@ -609,16 +441,6 @@
 	ret
 
 
-write_afsr1: ; .type write_afsr1, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_afsr1_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_afsr1_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_afsr1_el3
-
-
 write_afsr1_el1: ; .type write_afsr1_el1, %function
 	msr	afsr1_el1, x0
 	isb
@@ -641,16 +463,6 @@
 	 * SCTLR accessors
 	 * -----------------------------------------------------
 	 */
-read_sctlr: ; .type read_sctlr, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_sctlr_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_sctlr_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_sctlr_el3
-
-
 read_sctlr_el1: ; .type read_sctlr_el1, %function
 	mrs	x0, sctlr_el1
 	ret
@@ -666,16 +478,6 @@
 	ret
 
 
-write_sctlr: ; .type write_sctlr, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_sctlr_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_sctlr_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_sctlr_el3
-
-
 write_sctlr_el1: ; .type write_sctlr_el1, %function
 	msr	sctlr_el1, x0
 	dsb	sy
@@ -701,16 +503,6 @@
 	 * ACTLR accessors
 	 * -----------------------------------------------------
 	 */
-read_actlr: ; .type read_actlr, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_actlr_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_actlr_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_actlr_el3
-
-
 read_actlr_el1: ; .type read_actlr_el1, %function
 	mrs	x0, actlr_el1
 	ret
@@ -726,16 +518,6 @@
 	ret
 
 
-write_actlr: ; .type write_actlr, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_actlr_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_actlr_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_actlr_el3
-
-
 write_actlr_el1: ; .type write_actlr_el1, %function
 	msr	actlr_el1, x0
 	dsb	sy
@@ -761,16 +543,6 @@
 	 * ESR accessors
 	 * -----------------------------------------------------
 	 */
-read_esr: ; .type read_esr, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_esr_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_esr_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_esr_el3
-
-
 read_esr_el1: ; .type read_esr_el1, %function
 	mrs	x0, esr_el1
 	ret
@@ -786,16 +558,6 @@
 	ret
 
 
-write_esr: ; .type write_esr, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_esr_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_esr_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_esr_el3
-
-
 write_esr_el1: ; .type write_esr_el1, %function
 	msr	esr_el1, x0
 	dsb	sy
@@ -821,16 +583,6 @@
 	 * TCR accessors
 	 * -----------------------------------------------------
 	 */
-read_tcr: ; .type read_tcr, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_tcr_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_tcr_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_tcr_el3
-
-
 read_tcr_el1: ; .type read_tcr_el1, %function
 	mrs	x0, tcr_el1
 	ret
@@ -846,16 +598,6 @@
 	ret
 
 
-write_tcr: ; .type write_tcr, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_tcr_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_tcr_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_tcr_el3
-
-
 write_tcr_el1: ; .type write_tcr_el1, %function
 	msr	tcr_el1, x0
 	dsb	sy
@@ -881,16 +623,6 @@
 	 * CPTR accessors
 	 * -----------------------------------------------------
 	 */
-read_cptr: ; .type read_cptr, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_cptr_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_cptr_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_cptr_el3
-
-
 read_cptr_el1: ; .type read_cptr_el1, %function
 	b	read_cptr_el1
 	ret
@@ -906,16 +638,6 @@
 	ret
 
 
-write_cptr: ; .type write_cptr, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_cptr_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_cptr_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_cptr_el3
-
-
 write_cptr_el1: ; .type write_cptr_el1, %function
 	b	write_cptr_el1
 
@@ -938,16 +660,6 @@
 	 * TTBR0 accessors
 	 * -----------------------------------------------------
 	 */
-read_ttbr0: ; .type read_ttbr0, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_ttbr0_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_ttbr0_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_ttbr0_el3
-
-
 read_ttbr0_el1: ; .type read_ttbr0_el1, %function
 	mrs	x0, ttbr0_el1
 	ret
@@ -963,16 +675,6 @@
 	ret
 
 
-write_ttbr0: ; .type write_ttbr0, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_ttbr0_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_ttbr0_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_ttbr0_el3
-
-
 write_ttbr0_el1: ; .type write_ttbr0_el1, %function
 	msr	ttbr0_el1, x0
 	isb
@@ -995,16 +697,6 @@
 	 * TTBR1 accessors
 	 * -----------------------------------------------------
 	 */
-read_ttbr1: ; .type read_ttbr1, %function
-	mrs	x0, CurrentEl
-	cmp	x0, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	read_ttbr1_el1
-	cmp	x0, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	read_ttbr1_el2
-	cmp	x0, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	read_ttbr1_el3
-
-
 read_ttbr1_el1: ; .type read_ttbr1_el1, %function
 	mrs	x0, ttbr1_el1
 	ret
@@ -1018,16 +710,6 @@
 	b	read_ttbr1_el3
 
 
-write_ttbr1: ; .type write_ttbr1, %function
-	mrs	x1, CurrentEl
-	cmp	x1, #(MODE_EL1 << MODE_EL_SHIFT)
-	b.eq	write_ttbr1_el1
-	cmp	x1, #(MODE_EL2 << MODE_EL_SHIFT)
-	b.eq	write_ttbr1_el2
-	cmp	x1, #(MODE_EL3 << MODE_EL_SHIFT)
-	b.eq	write_ttbr1_el3
-
-
 write_ttbr1_el1: ; .type write_ttbr1_el1, %function
 	msr	ttbr1_el1, x0
 	isb
diff --git a/plat/fvp/aarch64/plat_common.c b/plat/fvp/aarch64/plat_common.c
index 1de6c03..d44ccb6 100644
--- a/plat/fvp/aarch64/plat_common.c
+++ b/plat/fvp/aarch64/plat_common.c
@@ -59,32 +59,43 @@
 	mair = MAIR_ATTR_SET(ATTR_DEVICE, ATTR_DEVICE_INDEX);
 	mair |= MAIR_ATTR_SET(ATTR_IWBWA_OWBWA_NTR,
 				  ATTR_IWBWA_OWBWA_NTR_INDEX);
-	write_mair(mair);
 
 	/*
 	 * Set TCR bits as well. Inner & outer WBWA & shareable + T0SZ = 32
 	 */
 	tcr = TCR_SH_INNER_SHAREABLE | TCR_RGN_OUTER_WBA |
 		  TCR_RGN_INNER_WBA | TCR_T0SZ_4GB;
-	if (GET_EL(current_el) == MODE_EL3) {
-		tcr |= TCR_EL3_RES1;
-		/* Invalidate EL3 TLBs */
-		tlbialle3();
-	} else {
-		/* Invalidate EL1 TLBs */
-		tlbivmalle1();
-	}
-
-	write_tcr(tcr);
 
 	/* Set TTBR bits as well */
 	ttbr = (unsigned long) l1_xlation_table;
-	write_ttbr0(ttbr);
 
-	sctlr = read_sctlr();
-	sctlr |= SCTLR_WXN_BIT | SCTLR_M_BIT | SCTLR_I_BIT;
-	sctlr |= SCTLR_A_BIT | SCTLR_C_BIT;
-	write_sctlr(sctlr);
+	if (GET_EL(current_el) == MODE_EL3) {
+		write_mair_el3(mair);
+		tcr |= TCR_EL3_RES1;
+		/* Invalidate EL3 TLBs */
+		tlbialle3();
+
+		write_tcr_el3(tcr);
+		write_ttbr0_el3(ttbr);
+
+		sctlr = read_sctlr_el3();
+		sctlr |= SCTLR_WXN_BIT | SCTLR_M_BIT | SCTLR_I_BIT;
+		sctlr |= SCTLR_A_BIT | SCTLR_C_BIT;
+		write_sctlr_el3(sctlr);
+	} else {
+
+		write_mair_el1(mair);
+		/* Invalidate EL1 TLBs */
+		tlbivmalle1();
+
+		write_tcr_el1(tcr);
+		write_ttbr0_el1(ttbr);
+
+		sctlr = read_sctlr_el1();
+		sctlr |= SCTLR_WXN_BIT | SCTLR_M_BIT | SCTLR_I_BIT;
+		sctlr |= SCTLR_A_BIT | SCTLR_C_BIT;
+		write_sctlr_el1(sctlr);
+	}
 
 	return;
 }
@@ -92,10 +103,17 @@
 void disable_mmu(void)
 {
 	unsigned long sctlr;
+	unsigned long current_el = read_current_el();
 
-	sctlr = read_sctlr();
-	sctlr = sctlr & ~(SCTLR_M_BIT | SCTLR_C_BIT);
-	write_sctlr(sctlr);
+	if (GET_EL(current_el) == MODE_EL3) {
+		sctlr = read_sctlr_el3();
+		sctlr = sctlr & ~(SCTLR_M_BIT | SCTLR_C_BIT);
+		write_sctlr_el3(sctlr);
+	} else {
+		sctlr = read_sctlr_el1();
+		sctlr = sctlr & ~(SCTLR_M_BIT | SCTLR_C_BIT);
+		write_sctlr_el1(sctlr);
+	}
 
 	/* Flush the caches */
 	dcsw_op_all(DCCISW);
diff --git a/services/std_svc/psci/psci_afflvl_off.c b/services/std_svc/psci/psci_afflvl_off.c
index 3763f6f..f6bd40c 100644
--- a/services/std_svc/psci/psci_afflvl_off.c
+++ b/services/std_svc/psci/psci_afflvl_off.c
@@ -48,7 +48,7 @@
 {
 	unsigned int index, plat_state;
 	int rc = PSCI_E_SUCCESS;
-	unsigned long sctlr = read_sctlr();
+	unsigned long sctlr;
 
 	assert(cpu_node->level == MPIDR_AFFLVL0);
 
@@ -83,8 +83,9 @@
 	 * Do the bare minimal for the time being. Fix this before porting to
 	 * Cortex models.
 	 */
+	sctlr = read_sctlr_el3();
 	sctlr &= ~SCTLR_C_BIT;
-	write_sctlr(sctlr);
+	write_sctlr_el3(sctlr);
 
 	/*
 	 * CAUTION: This flush to the level of unification makes an assumption
diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c
index 138d033..ca521ff 100644
--- a/services/std_svc/psci/psci_afflvl_suspend.c
+++ b/services/std_svc/psci/psci_afflvl_suspend.c
@@ -87,7 +87,7 @@
 				unsigned int power_state)
 {
 	unsigned int index, plat_state;
-	unsigned long psci_entrypoint, sctlr = read_sctlr();
+	unsigned long psci_entrypoint, sctlr;
 	el3_state *saved_el3_state;
 	int rc = PSCI_E_SUCCESS;
 
@@ -146,8 +146,9 @@
 	 * Do the bare minimal for the time being. Fix this before porting to
 	 * Cortex models.
 	 */
+	sctlr = read_sctlr_el3();
 	sctlr &= ~SCTLR_C_BIT;
-	write_sctlr(sctlr);
+	write_sctlr_el3(sctlr);
 
 	/*
 	 * CAUTION: This flush to the level of unification makes an assumption