diff options
| -rw-r--r-- | bl1/aarch64/bl1_arch_setup.c | 9 | ||||
| -rw-r--r-- | bl1/aarch64/early_exceptions.S | 6 | ||||
| -rw-r--r-- | bl1/bl1_main.c | 2 | ||||
| -rw-r--r-- | bl31/aarch64/bl31_arch_setup.c | 9 | ||||
| -rw-r--r-- | common/bl_common.c | 4 | ||||
| -rw-r--r-- | include/aarch64/arch_helpers.h | 30 | ||||
| -rw-r--r-- | lib/arch/aarch64/sysreg_helpers.S | 318 | ||||
| -rw-r--r-- | plat/fvp/aarch64/plat_common.c | 46 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_afflvl_off.c | 5 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_afflvl_suspend.c | 5 |
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 |