From 93634d08e3a9bac21806a65e2098961f7adab8e7 Mon Sep 17 00:00:00 2001
From: TuxSH <tuxsh@sfr.fr>
Date: Wed, 28 Feb 2018 02:37:08 +0100
Subject: [PATCH] Implement __set_memory_registers

---
 exosphere/linker.ld   |  2 ++
 exosphere/src/start.s | 25 +++++++++++++++++++++++++
 2 files changed, 27 insertions(+)

diff --git a/exosphere/linker.ld b/exosphere/linker.ld
index cf4fe2ff2..9d8123482 100644
--- a/exosphere/linker.ld
+++ b/exosphere/linker.ld
@@ -27,6 +27,7 @@ SECTIONS
     .cold_crt0 :
     {
         . = ALIGN(64);
+        KEEP (*(.cold_crt0.text.start)) /* MUST be first */
         KEEP (*(.cold_crt0.text*))
         KEEP (build/coldboot_init.o(.text*))
         *(.cold_crt0.rodata*)
@@ -44,6 +45,7 @@ SECTIONS
         . = ALIGN(64);
         __warmboot_crt0_lma__   = LOADADDR(.warm_crt0);
         __warmboot_crt0_start__ = ABSOLUTE(.);
+        KEEP (*(.warm_crt0.text.start)) /* Should be first */
         KEEP (*(.warm_crt0.text*))
         KEEP (build/warmboot_init.o(.text*))
         *(.warm_crt0.rodata*)
diff --git a/exosphere/src/start.s b/exosphere/src/start.s
index 3e3956a58..bde91e455 100644
--- a/exosphere/src/start.s
+++ b/exosphere/src/start.s
@@ -97,6 +97,31 @@ __start_warm:
     ldr  x16, =__jump_to_main_warm
     br   x16
 
+/* Used by coldboot as well */
+.section    .warm_crt0.text.__set_memory_registers, "ax", %progbits
+.global     __set_memory_registers
+.type       __set_memory_registers, %function
+__set_memory_registers:
+    msr  cpuactlr_el1, x2
+    isb
+    msr  scr_el3, x3
+    msr  ttbr0_el3, x0
+    msr  tcr_el3, x4
+    msr  cptr_el3, x5
+    msr  mair_el3, x6
+    msr  vbar_el3, x1
+
+    /* Invalidate the entire TLB on the Inner Shareable domain */
+    isb
+    dsb  ish
+    tlbi alle3is
+    dsb  ish
+    isb
+
+    msr  sctlr_el3, x7
+    isb
+    ret
+
 .align      4
 .section    .text.__jump_to_main_cold, "ax", %progbits
 __jump_to_main_cold: