Add most of warmboot_main

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;
 }
 

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);

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();
-}
+}
+
+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();
+}

exosphere/src/bootup.h

@@ -9,4 +9,10 @@ void setup_4x_mmio(void);
 
 void setup_current_core_state(void);
 
-#endif
+void identity_unmap_iram_cd_tzram(void);
+
+void secure_additional_devices(void);
+
+void set_extabt_serror_taken_to_el3(bool taken_to_el3);
+
+#endif

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;
 }

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);

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();
 }

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 {
+    set_extabt_serror_taken_to_el3(bootconfig_take_extabt_serror_to_el3());
-        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);
 }

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

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");
 }

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();
     }
 

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) {
+#include "se.h"
-    /* TODO: lots of stuff */
+#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();
 }