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

10 files changed, 52 insertions, 382 deletions

diff --git a/bl1/aarch64/bl1_arch_setup.c b/bl1/aarch64/bl1_arch_setup.c
index e82ad09b..f9443aa1 100644
--- a/bl1/aarch64/bl1_arch_setup.c
+++ b/bl1/aarch64/bl1_arch_setup.c
@@ -41,10 +41,10 @@ void bl1_arch_setup(void)
 	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 @@ void bl1_arch_setup(void)
  * 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 d06e8543..ee91a89c 100644
--- a/bl1/aarch64/early_exceptions.S
+++ b/bl1/aarch64/early_exceptions.S
@@ -195,7 +195,7 @@ process_exception:
 	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 @@ process_exception:
 	 * ---------------------------------------------
 	 */
 	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 401a525b..dbf6c9d0 100644
--- a/bl1/bl1_main.c
+++ b/bl1/bl1_main.c
@@ -48,7 +48,7 @@ void bl1_arch_next_el_setup(void);
 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 faeccf31..2a424f86 100644
--- a/bl31/aarch64/bl31_arch_setup.c
+++ b/bl31/aarch64/bl31_arch_setup.c
@@ -42,10 +42,10 @@ void bl31_arch_setup(void)
 	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_arch_setup(void)
 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 da027b66..b03165ba 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -78,8 +78,8 @@ void __dead2 drop_el(aapcs64_params *args,
 		     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 fe96b4d1..f55c0033 100644
--- a/include/aarch64/arch_helpers.h
+++ b/include/aarch64/arch_helpers.h
@@ -97,21 +97,17 @@ extern unsigned long read_id_pfr1_el1(void);
 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_mpidr(void);
 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(void);
 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_cpacr(unsigned long);
 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_el1(unsigned long);
 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 66504194..dfd0f2f1 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 @@ read_id_aa64pfr0_el1: ; .type read_id_aa64pfr0_el1, %function
 	 * 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 @@ read_vbar_el3: ; .type read_vbar_el3, %function
 	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 @@ write_vbar_el3: ; .type write_vbar_el3, %function
 	 * 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 @@ read_afsr0_el3: ; .type read_afsr0_el3, %function
 	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 @@ write_afsr0_el3: ; .type write_afsr0_el3, %function
 	 * 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 @@ read_far_el3: ; .type read_far_el3, %function
 	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 @@ write_far_el3: ; .type write_far_el3, %function
 	 * 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 @@ read_mair_el3: ; .type read_mair_el3, %function
 	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 @@ write_mair_el3: ; .type write_mair_el3, %function
 	 * 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 @@ read_amair_el3: ; .type read_amair_el3, %function
 	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 @@ write_amair_el3: ; .type write_amair_el3, %function
 	 * 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 @@ read_rvbar_el3: ; .type read_rvbar_el3, %function
 	 * 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 @@ read_rmr_el3: ; .type read_rmr_el3, %function
 	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 @@ write_rmr_el3: ; .type write_rmr_el3, %function
 	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 @@ read_afsr1_el3: ; .type read_afsr1_el3, %function
 	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 @@ write_afsr1_el3: ; .type write_afsr1_el3, %function
 	 * 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 @@ read_sctlr_el3: ; .type read_sctlr_el3, %function
 	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 @@ write_sctlr_el3: ; .type write_sctlr_el3, %function
 	 * 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 @@ read_actlr_el3: ; .type read_actlr_el3, %function
 	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 @@ write_actlr_el3: ; .type write_actlr_el3, %function
 	 * 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 @@ read_esr_el3: ; .type read_esr_el3, %function
 	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 @@ write_esr_el3: ; .type write_esr_el3, %function
 	 * 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 @@ read_tcr_el3: ; .type read_tcr_el3, %function
 	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 @@ write_tcr_el3: ; .type write_tcr_el3, %function
 	 * 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 @@ read_cptr_el3: ; .type read_cptr_el3, %function
 	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 @@ write_cptr_el3: ; .type write_cptr_el3, %function
 	 * 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 @@ read_ttbr0_el3: ; .type read_ttbr0_el3, %function
 	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 @@ write_ttbr0_el3: ; .type write_ttbr0_el3, %function
 	 * 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 @@ read_ttbr1_el3: ; .type read_ttbr1_el3, %function
 	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 1de6c03e..d44ccb6e 100644
--- a/plat/fvp/aarch64/plat_common.c
+++ b/plat/fvp/aarch64/plat_common.c
@@ -59,32 +59,43 @@ void enable_mmu()
 	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 enable_mmu()
 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 3763f6fe..f6bd40c8 100644
--- a/services/std_svc/psci/psci_afflvl_off.c
+++ b/services/std_svc/psci/psci_afflvl_off.c
@@ -48,7 +48,7 @@ static int psci_afflvl0_off(unsigned long mpidr, aff_map_node *cpu_node)
 {
 	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 @@ static int psci_afflvl0_off(unsigned long mpidr, aff_map_node *cpu_node)
 	 * 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 138d0331..ca521ff3 100644
--- a/services/std_svc/psci/psci_afflvl_suspend.c
+++ b/services/std_svc/psci/psci_afflvl_suspend.c
@@ -87,7 +87,7 @@ static int psci_afflvl0_suspend(unsigned long mpidr,
 				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 @@ static int psci_afflvl0_suspend(unsigned long mpidr,
 	 * 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