From 03c1ad711972aa14b7e0fcf84bfee60acbd521f5 Mon Sep 17 00:00:00 2001
From: TuxSH <tuxsh@sfr.fr>
Date: Sat, 3 Mar 2018 19:31:22 +0100
Subject: [PATCH] Add most of warmboot_main

---
 exosphere/src/bootconfig.c      |  2 +-
 exosphere/src/bootconfig.h      |  2 +-
 exosphere/src/bootup.c          | 54 +++++++++++++++++++++++++++++----
 exosphere/src/bootup.h          |  8 ++++-
 exosphere/src/cpu_context.c     | 16 ++++++----
 exosphere/src/cpu_context.h     |  3 +-
 exosphere/src/lp0.c             |  2 +-
 exosphere/src/package2.c        | 40 ++++--------------------
 exosphere/src/start.s           |  2 +-
 exosphere/src/synchronization.h |  4 +++
 exosphere/src/warmboot_init.c   |  6 ++--
 exosphere/src/warmboot_main.c   | 53 ++++++++++++++++++++++++++++++--
 12 files changed, 135 insertions(+), 57 deletions(-)

diff --git a/exosphere/src/bootconfig.c b/exosphere/src/bootconfig.c
index f7352bd15..95fcacec8 100644
--- a/exosphere/src/bootconfig.c
+++ b/exosphere/src/bootconfig.c
@@ -64,7 +64,7 @@ bool bootconfig_is_debug_mode(void) {
     return (LOADED_BOOTCONFIG->unsigned_config.data[0x10] & 2) != 0;
 }
 
-bool bootconfig_should_set_scr_el3_bit(void) {
+bool bootconfig_take_extabt_serror_to_el3(void) {
     return (LOADED_BOOTCONFIG->unsigned_config.data[0x10] & 6) != 6;
 }
 
diff --git a/exosphere/src/bootconfig.h b/exosphere/src/bootconfig.h
index 593a1f0b1..1ceeb4b53 100644
--- a/exosphere/src/bootconfig.h
+++ b/exosphere/src/bootconfig.h
@@ -43,7 +43,7 @@ bool bootconfig_is_package2_unsigned(void);
 bool bootconfig_disable_program_verification(void);
 bool bootconfig_is_debug_mode(void);
 
-bool bootconfig_should_set_scr_el3_bit(void);
+bool bootconfig_take_extabt_serror_to_el3(void);
 
 uint64_t bootconfig_get_memory_arrangement(void);
 uint64_t bootconfig_get_kernel_memory_configuration(void);
diff --git a/exosphere/src/bootup.c b/exosphere/src/bootup.c
index 33f673655..b1641b92a 100644
--- a/exosphere/src/bootup.c
+++ b/exosphere/src/bootup.c
@@ -21,6 +21,11 @@
 #include "actmon.h"
 #include "syscrt0.h"
 
+#include "mmu.h"
+#include "arm.h"
+#include "memory_map.h"
+#include "synchronization.h"
+
 static bool g_has_booted_up = false;
 
 void bootup_misc_mmio(void) {
@@ -134,7 +139,7 @@ void bootup_misc_mmio(void) {
     APBDEV_PMC_SEC_DISABLE3_0 = 0x500000;
 
     /* Setup FIQs. */
-    
+
 
     /* And assign "se_operation_completed" to Interrupt 0x5A. */
     intr_set_priority(INTERRUPT_ID_SECURITY_ENGINE, 0);
@@ -180,13 +185,13 @@ void setup_current_core_state(void) {
     SET_SYSREG(dacr32_el2, 0xFFFFFFFFull);
     SET_SYSREG(sctlr_el1, 0xC50838ull);
     SET_SYSREG(sctlr_el2, 0x30C50838ull);
-    
-    do { __asm__ __volatile__ ("isb"); } while (false);
-    
+
+    __isb();
+
     SET_SYSREG(cntfrq_el0, MAKE_SYSCRT0_REG(0x20)); /* TODO: Reg name. */
     SET_SYSREG(cnthctl_el2, 3ull);
 
-    do { __asm__ __volatile__ ("isb"); } while (false);
+    __isb();
 
     /* Setup Interrupts, flow. */
     flow_clear_csr0_and_events(get_core_id());
@@ -197,4 +202,41 @@ void setup_current_core_state(void) {
 
     /* Restore current core context. */
     restore_current_core_context();
-}
\ No newline at end of file
+}
+
+void identity_unmap_iram_cd_tzram(void) {
+    /* See also: configure_ttbls (in coldboot_init.c). */
+    uintptr_t *mmu_l1_tbl = (uintptr_t *)(TZRAM_GET_SEGMENT_ADDRESS(TZRAM_SEGEMENT_ID_SECMON_EVT) + 0x800 - 64);
+    uintptr_t *mmu_l2_tbl = (uintptr_t *)TZRAM_GET_SEGMENT_ADDRESS(TZRAM_SEGMENT_ID_L2_TRANSLATION_TABLE);
+    uintptr_t *mmu_l3_tbl = (uintptr_t *)TZRAM_GET_SEGMENT_ADDRESS(TZRAM_SEGMENT_ID_L3_TRANSLATION_TABLE);
+
+    mmu_unmap_range(3, mmu_l3_tbl, IDENTITY_GET_MAPPING_ADDRESS(IDENTITY_MAPPING_IRAM_CD), IDENTITY_GET_MAPPING_SIZE(IDENTITY_MAPPING_IRAM_CD));
+    mmu_unmap_range(3, mmu_l3_tbl, IDENTITY_GET_MAPPING_ADDRESS(IDENTITY_MAPPING_TZRAM), IDENTITY_GET_MAPPING_SIZE(IDENTITY_MAPPING_TZRAM));
+
+    mmu_unmap(2, mmu_l2_tbl, 0x40000000);
+    mmu_unmap(2, mmu_l2_tbl, 0x7C000000);
+
+    mmu_unmap(1, mmu_l1_tbl, 0x40000000);
+
+    tlb_invalidate_all_inner_shareable();
+}
+
+void secure_additional_devices(void) {
+    if (mkey_get_revision() >= MASTERKEY_REVISION_400_CURRENT) {
+        APB_MISC_SECURE_REGS_APB_SLAVE_SECURITY_ENABLE_REG0_0 |= 0x2000; /* make PMC secure-only (2.x+ but see note below) */
+        APB_MISC_SECURE_REGS_APB_SLAVE_SECURITY_ENABLE_REG1_0 |= 0X510;  /* make MC0, MC1, MCB secure-only (4.x+) */
+    } else {
+        /* TODO: Detect 1.x */
+    }
+}
+
+void set_extabt_serror_taken_to_el3(bool taken_to_el3) {
+    uint64_t temp_scr_el3;
+    __asm__ __volatile__ ("mrs %0, scr_el3" : "=r"(temp_scr_el3) :: "memory");
+
+    temp_scr_el3 &= 0xFFFFFFF7;
+    temp_scr_el3 |= taken_to_el3 ? 8 : 0;
+
+    __asm__ __volatile__ ("msr scr_el3, %0" :: "r"(temp_scr_el3) : "memory");
+    __isb();
+}
diff --git a/exosphere/src/bootup.h b/exosphere/src/bootup.h
index 598cb5a60..bd6ef0a38 100644
--- a/exosphere/src/bootup.h
+++ b/exosphere/src/bootup.h
@@ -9,4 +9,10 @@ void setup_4x_mmio(void);
 
 void setup_current_core_state(void);
 
-#endif
\ No newline at end of file
+void identity_unmap_iram_cd_tzram(void);
+
+void secure_additional_devices(void);
+
+void set_extabt_serror_taken_to_el3(bool taken_to_el3);
+
+#endif
diff --git a/exosphere/src/cpu_context.c b/exosphere/src/cpu_context.c
index 9fdee9722..8e91bf36b 100644
--- a/exosphere/src/cpu_context.c
+++ b/exosphere/src/cpu_context.c
@@ -31,7 +31,7 @@ static saved_cpu_context_t g_cpu_contexts[NUM_CPU_CORES] = {0};
 void use_core_entrypoint_and_argument(uint32_t core, uintptr_t *entrypoint_addr, uint64_t *argument) {
     saved_cpu_context_t *ctx = &g_cpu_contexts[core];
     if(ctx->ELR_EL3 == 0 || ctx->is_active) {
-        panic(0xFA000007); /* invalid context */
+        panic(0xF7F00007); /* invalid context */
     }
 
     *entrypoint_addr = ctx->ELR_EL3;
@@ -39,7 +39,7 @@ void use_core_entrypoint_and_argument(uint32_t core, uintptr_t *entrypoint_addr,
 
     ctx->ELR_EL3 = 0;
     ctx->argument = 0;
-    ctx->is_active = true;
+    ctx->is_active = 1;
 }
 
 void set_core_entrypoint_and_argument(uint32_t core, uintptr_t entrypoint_addr, uint64_t argument) {
@@ -47,7 +47,7 @@ void set_core_entrypoint_and_argument(uint32_t core, uintptr_t entrypoint_addr,
     g_cpu_contexts[core].argument = argument;
 }
 
-void core_jump_to_lower_el(void) {
+void __attribute__((noreturn)) core_jump_to_lower_el(void) {
     uintptr_t ep;
     uint64_t arg;
     unsigned int core_id = get_core_id();
@@ -71,9 +71,9 @@ uint32_t cpu_on(uint32_t core, uintptr_t entrypoint_addr, uint64_t argument) {
     if (g_cpu_contexts[core].is_active) {
         return 0xFFFFFFFC;
     }
-    
+
     set_core_entrypoint_and_argument(core, entrypoint_addr, argument);
-    
+
     const uint32_t status_masks[NUM_CPU_CORES] = {0x4000, 0x200, 0x400, 0x800};
     const uint32_t toggle_vals[NUM_CPU_CORES] = {0xE, 0x9, 0xA, 0xB};
     
@@ -173,11 +173,15 @@ void restore_current_core_context(void) {
 
         EVAL(REPEAT(6, RESTORE_BP_REG, ~));
         EVAL(REPEAT(4, RESTORE_WP_REG, ~));
-        
+
         g_cpu_contexts[current_core].is_saved = 0;
     }
 }
 
+bool is_core_active(uint32_t core) {
+    return g_cpu_contexts[core].is_active != 0;
+}
+
 void set_core_is_active(uint32_t core, bool is_active) {
     g_cpu_contexts[core].is_active = (is_active) ? 1 : 0;
 }
diff --git a/exosphere/src/cpu_context.h b/exosphere/src/cpu_context.h
index dcbe0e9dd..adab83348 100644
--- a/exosphere/src/cpu_context.h
+++ b/exosphere/src/cpu_context.h
@@ -50,13 +50,14 @@ typedef struct {
 void save_current_core_context(void);
 void restore_current_core_context(void);
 
+bool is_core_active(uint32_t core);
 void set_core_is_active(uint32_t core, bool is_active);
 void set_current_core_active(void);
 void set_current_core_inactive(void);
 
 void use_core_entrypoint_and_argument(uint32_t core, uintptr_t *entrypoint_addr, uint64_t *argument);
 void set_core_entrypoint_and_argument(uint32_t core, uintptr_t entrypoint_addr, uint64_t argument);
-void core_jump_to_lower_el(void);
+void __attribute__((noreturn)) core_jump_to_lower_el(void);
 
 uint32_t cpu_on(uint32_t core, uintptr_t entrypoint_addr, uint64_t argument);
 uint32_t cpu_off(void);
diff --git a/exosphere/src/lp0.c b/exosphere/src/lp0.c
index cca81740a..b59e9425b 100644
--- a/exosphere/src/lp0.c
+++ b/exosphere/src/lp0.c
@@ -181,6 +181,6 @@ uint32_t cpu_suspend(uint64_t power_state, uint64_t entrypoint, uint64_t argumen
     save_current_core_context();
     set_current_core_inactive(); 
     call_with_stack_pointer(get_smc_core012_stack_address(), save_se_and_power_down_cpu);
-    
+
     generic_panic();
 }
diff --git a/exosphere/src/package2.c b/exosphere/src/package2.c
index 9ca9fbe1e..2a87041f4 100644
--- a/exosphere/src/package2.c
+++ b/exosphere/src/package2.c
@@ -14,6 +14,7 @@
 #include "pmc.h"
 #include "randomcache.h"
 #include "timers.h"
+#include "bootconfig.h"
 
 extern void *__start_cold_addr;
 extern size_t __bin_size;
@@ -25,7 +26,7 @@ static void setup_se(void) {
     /* Sanity check the Security Engine. */
     se_verify_flags_cleared();
 
-    /* Initialize Interrupts. */
+    /* Initialize interrupts. */
     intr_initialize_gic_nonsecure();
 
     /* Perform some sanity initialization. */
@@ -35,8 +36,6 @@ static void setup_se(void) {
     p_security_engine->RSA_KEY_READ_DISABLE_REG = 0;
     p_security_engine->_0x0 &= 0xFFFFFFFB;
 
-    
-
     /* Currently unknown what each flag does. */
     for (unsigned int i = 0; i < KEYSLOT_AES_MAX; i++) {
         set_aes_keyslot_flags(i, 0x15);
@@ -342,23 +341,6 @@ static void sync_with_nx_bootloader(int state) {
     }
 }
 
-static void identity_unmap_iram_cd_tzram(void) {
-    /* See also: configure_ttbls (in coldboot_init.c). */
-    uintptr_t *mmu_l1_tbl = (uintptr_t *)(TZRAM_GET_SEGMENT_ADDRESS(TZRAM_SEGEMENT_ID_SECMON_EVT) + 0x800 - 64);
-    uintptr_t *mmu_l2_tbl = (uintptr_t *)TZRAM_GET_SEGMENT_ADDRESS(TZRAM_SEGMENT_ID_L2_TRANSLATION_TABLE);
-    uintptr_t *mmu_l3_tbl = (uintptr_t *)TZRAM_GET_SEGMENT_ADDRESS(TZRAM_SEGMENT_ID_L3_TRANSLATION_TABLE);
-
-    mmu_unmap_range(3, mmu_l3_tbl, IDENTITY_GET_MAPPING_ADDRESS(IDENTITY_MAPPING_IRAM_CD), IDENTITY_GET_MAPPING_SIZE(IDENTITY_MAPPING_IRAM_CD));
-    mmu_unmap_range(3, mmu_l3_tbl, IDENTITY_GET_MAPPING_ADDRESS(IDENTITY_MAPPING_TZRAM), IDENTITY_GET_MAPPING_SIZE(IDENTITY_MAPPING_TZRAM));
-
-    mmu_unmap(2, mmu_l2_tbl, 0x40000000);
-    mmu_unmap(2, mmu_l2_tbl, 0x7C000000);
-
-    mmu_unmap(1, mmu_l1_tbl, 0x40000000);
-
-    tlb_invalidate_all_inner_shareable();
-}
-
 static void indentity_unmap_dram(void) {
     uintptr_t *mmu_l1_tbl = (uintptr_t *)(TZRAM_GET_SEGMENT_ADDRESS(TZRAM_SEGEMENT_ID_SECMON_EVT) + 0x800 - 64);
 
@@ -410,6 +392,9 @@ void load_package2(coldboot_crt0_reloc_list_t *reloc_list) {
         sync_with_nx_bootloader(NX_BOOTLOADER_STATE_LOADED_PACKAGE2);
     }
 
+    /* Make PMC (2.x+), MC (4.x+) registers secure-only */
+    secure_additional_devices();
+
     /* Remove the identity mapping for iRAM-C+D & TZRAM */
     identity_unmap_iram_cd_tzram();
 
@@ -449,18 +434,5 @@ void load_package2(coldboot_crt0_reloc_list_t *reloc_list) {
     }
 
     /* Update SCR_EL3 depending on value in Bootconfig. */
-    do {
-        uint64_t temp_scr_el3;
-        __asm__ __volatile__ ("mrs %0, scr_el3" : "=r"(temp_scr_el3) :: "memory");
-        
-        temp_scr_el3 &= 0xFFFFFFF7;
-        
-        if (bootconfig_should_set_scr_el3_bit()) {
-            temp_scr_el3 |= 8;
-        }
-        
-        __asm__ __volatile__ ("msr scr_el3, %0" :: "r"(temp_scr_el3) : "memory");
-        
-        __asm__ __volatile__("isb");
-    } while(false);
+    set_extabt_serror_taken_to_el3(bootconfig_take_extabt_serror_to_el3());
 }
diff --git a/exosphere/src/start.s b/exosphere/src/start.s
index 456862906..7ab35c7e3 100644
--- a/exosphere/src/start.s
+++ b/exosphere/src/start.s
@@ -188,7 +188,7 @@ __jump_to_main_warm:
     mov  w0, #0 /* use core0,1,2 stack bottom + 0x800 (VA of warmboot crt0 sp) temporarily */
     bl   get_exception_entry_stack_address
     add  sp, x0, #0x800
-    b    warmboot_main
+    bl   warmboot_main
 
 .section    .text.__set_exception_entry_stack, "ax", %progbits
 .type       __set_exception_entry_stack, %function
diff --git a/exosphere/src/synchronization.h b/exosphere/src/synchronization.h
index 0f16f5680..141713d96 100644
--- a/exosphere/src/synchronization.h
+++ b/exosphere/src/synchronization.h
@@ -21,6 +21,10 @@ static inline void __dmb_sy(void) {
     __asm__ __volatile__ ("dmb sy" ::: "memory");
 }
 
+static inline void __isb(void) {
+    __asm__ __volatile__ ("isb" ::: "memory");
+}
+
 static inline void __sev(void) {
     __asm__ __volatile__ ("sev");
 }
diff --git a/exosphere/src/warmboot_init.c b/exosphere/src/warmboot_init.c
index e1937c8f2..23d5d2fde 100644
--- a/exosphere/src/warmboot_init.c
+++ b/exosphere/src/warmboot_init.c
@@ -20,8 +20,7 @@ uintptr_t get_warmboot_crt0_stack_address_critsec_enter(void) {
 
     if (core_id) {
         return TZRAM_GET_SEGMENT_PA(TZRAM_SEGMENT_ID_CORE3_STACK) + 0x1000;
-    }
-    else {
+    } else {
         return TZRAM_GET_SEGMENT_PA(TZRAM_SEGEMENT_ID_SECMON_EVT) + 0x80 * (core_id + 1);
     }
 }
@@ -143,7 +142,8 @@ void warmboot_init(boot_func_list_t *func_list) {
     func_list->funcs.flush_dcache_all();
     func_list->funcs.invalidate_icache_all();
 
-    if(MC_SECURITY_CFG0_0 != 0) {
+    /* On warmboot (not cpu_on) only */
+    if (MC_SECURITY_CFG0_0 != 0) {
         init_dma_controllers();
     }
 
diff --git a/exosphere/src/warmboot_main.c b/exosphere/src/warmboot_main.c
index 4d0eb1f59..b8a0147ea 100644
--- a/exosphere/src/warmboot_main.c
+++ b/exosphere/src/warmboot_main.c
@@ -2,8 +2,57 @@
 #include "mmu.h"
 #include "memory_map.h"
 #include "cpu_context.h"
+#include "bootconfig.h"
+#include "configitem.h"
+#include "masterkey.h"
+#include "bootup.h"
+#include "smc_api.h"
 
-void warmboot_main(void) {
-    /* TODO: lots of stuff */
+#include "se.h"
+#include "mc.h"
+#include "interrupt.h"
+
+void __attribute__((noreturn)) warmboot_main(void) {
+    /*
+        This function and its callers are reached in either of the following events, under normal conditions:
+            - warmboot (core 3)
+            - cpu_on
+    */
+    if (is_core_active(get_core_id())) {
+        panic(0xF7F00007); /* invalid CPU context */
+    }
+
+    /* IRAM C+D identity mapping has actually been removed on coldboot but we don't really care */
+    identity_unmap_iram_cd_tzram();
+
+    /* On warmboot (not cpu_on) only */
+    if (MC_SECURITY_CFG0_0 != 0) {
+        if (!configitem_is_retail()) {
+            /* TODO: uart_log("OHAYO"); */
+        }
+
+        /* Sanity check the Security Engine. */
+        se_verify_flags_cleared();
+
+        /* Initialize interrupts. */
+        intr_initialize_gic_nonsecure();
+
+        bootup_misc_mmio();
+
+        /* Make PMC (2.x+), MC (4.x+) registers secure-only */
+        secure_additional_devices();
+
+        /* TODO: car+clkreset stuff, some other mmio (?) */
+
+        if (mkey_get_revision() >= MASTERKEY_REVISION_400_CURRENT) {
+            setup_4x_mmio(); /* TODO */
+        }
+    }
+
+    clear_priv_smc_in_progress();
+    setup_current_core_state();
+
+    /* Update SCR_EL3 depending on value in Bootconfig. */
+    set_extabt_serror_taken_to_el3(bootconfig_take_extabt_serror_to_el3());
     core_jump_to_lower_el();
 }