Remove partially qualified asm helper functions

Each ARM Trusted Firmware image should know in which EL it is running
and it should use the corresponding register directly instead of reading
currentEL and knowing which asm register to read/write

Change-Id: Ief35630190b6f07c8fbb7ba6cb20db308f002945
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
@@ -845,16 +597,6 @@
 	mrs	x0, tcr_el3
 	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
@@ -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
@@ -905,16 +637,6 @@
 	mrs	x0, cptr_el3
 	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;
+
+	/* Set TTBR bits as well */
+	ttbr = (unsigned long) l1_xlation_table;
+
 	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(tcr);
+		write_tcr_el1(tcr);
+		write_ttbr0_el1(ttbr);
 
-	/* 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);
+		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