diff options
Diffstat (limited to 'arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S')
| -rw-r--r-- | arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S | 61 |
1 files changed, 51 insertions, 10 deletions
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S b/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S index f7b49cb..72f2c11 100644 --- a/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S +++ b/arch/arm/cpu/armv8/fsl-layerscape/lowlevel.S @@ -17,6 +17,7 @@ #include <asm/arch-fsl-layerscape/immap_lsch3.h> #include <asm/arch-fsl-layerscape/soc.h> #endif +#include <asm/u-boot.h> ENTRY(lowlevel_init) mov x29, lr /* Save LR */ @@ -359,11 +360,6 @@ ENTRY(secondary_boot_func) gic_wait_for_interrupt_m x0, w1 #endif - bl secondary_switch_to_el2 -#ifdef CONFIG_ARMV8_SWITCH_TO_EL1 - bl secondary_switch_to_el1 -#endif - slave_cpu: wfe ldr x0, [x11] @@ -376,19 +372,64 @@ slave_cpu: tbz x1, #25, cpu_is_le rev x0, x0 /* BE to LE conversion */ cpu_is_le: - br x0 /* branch to the given address */ + ldr x5, [x11, #24] + ldr x6, =IH_ARCH_DEFAULT + cmp x6, x5 + b.eq 1f + +#ifdef CONFIG_ARMV8_SWITCH_TO_EL1 + adr x3, secondary_switch_to_el1 + ldr x4, =ES_TO_AARCH64 +#else + ldr x3, [x11] + ldr x4, =ES_TO_AARCH32 +#endif + bl secondary_switch_to_el2 + +1: +#ifdef CONFIG_ARMV8_SWITCH_TO_EL1 + adr x3, secondary_switch_to_el1 +#else + ldr x3, [x11] +#endif + ldr x4, =ES_TO_AARCH64 + bl secondary_switch_to_el2 + ENDPROC(secondary_boot_func) ENTRY(secondary_switch_to_el2) - switch_el x0, 1f, 0f, 0f + switch_el x5, 1f, 0f, 0f 0: ret -1: armv8_switch_to_el2_m x0 +1: armv8_switch_to_el2_m x3, x4, x5 ENDPROC(secondary_switch_to_el2) ENTRY(secondary_switch_to_el1) - switch_el x0, 0f, 1f, 0f + mrs x0, mpidr_el1 + ubfm x1, x0, #8, #15 + ubfm x2, x0, #0, #1 + orr x10, x2, x1, lsl #2 /* x10 has LPID */ + + lsl x1, x10, #6 + ldr x0, =__spin_table + /* physical address of this cpus spin table element */ + add x11, x1, x0 + + ldr x3, [x11] + + ldr x5, [x11, #24] + ldr x6, =IH_ARCH_DEFAULT + cmp x6, x5 + b.eq 2f + + ldr x4, =ES_TO_AARCH32 + bl switch_to_el1 + +2: ldr x4, =ES_TO_AARCH64 + +switch_to_el1: + switch_el x5, 0f, 1f, 0f 0: ret -1: armv8_switch_to_el1_m x0, x1 +1: armv8_switch_to_el1_m x3, x4, x5 ENDPROC(secondary_switch_to_el1) /* Ensure that the literals used by the secondary boot code are |