armv8: New MMU setup code allowing to use 48+ bits PA/VA

[people/ms/u-boot.git] / arch / arm / cpu / armv8 / start.S

diff --git a/arch/arm/cpu/armv8/start.S b/arch/arm/cpu/armv8/start.S
index da45d984d01aa8880e36f9e4f538c57298438441..2ee60d60f123b9e5f86729c16088a225b086a4fa 100644 (file)
--- a/arch/arm/cpu/armv8/start.S
+++ b/arch/arm/cpu/armv8/start.S
@@ -43,6 +43,9 @@ _bss_end_ofs:
        .quad   __bss_end - _start
 
 reset:
+#ifdef CONFIG_SYS_RESET_SCTRL
+       bl reset_sctrl
+#endif
        /*
         * Could be EL3/EL2/EL1, Initial State:
         * Little Endian, MMU Disabled, i/dCache Disabled
@@ -99,6 +102,39 @@ master_cpu:
 
        bl      _main
 
+#ifdef CONFIG_SYS_RESET_SCTRL
+reset_sctrl:
+       switch_el x1, 3f, 2f, 1f
+3:
+       mrs     x0, sctlr_el3
+       b       0f
+2:
+       mrs     x0, sctlr_el2
+       b       0f
+1:
+       mrs     x0, sctlr_el1
+
+0:
+       ldr     x1, =0xfdfffffa
+       and     x0, x0, x1
+
+       switch_el x1, 6f, 5f, 4f
+6:
+       msr     sctlr_el3, x0
+       b       7f
+5:
+       msr     sctlr_el2, x0
+       b       7f
+4:
+       msr     sctlr_el1, x0
+
+7:
+       dsb     sy
+       isb
+       b       __asm_invalidate_tlb_all
+       ret
+#endif
+
 /*-----------------------------------------------------------------------*/
 
 WEAK(apply_core_errata)