Use provided __libc_init_array and __libc_fini_array, bugfixes

exosphere/src/coldboot_init.c

@@ -2,12 +2,6 @@
 #include "mmu.h"
 #include "memory_map.h"
 
-extern void (*const __preinit_array_start[])(void);
-extern void (*const __preinit_array_end[])(void);
-extern void ( *const __init_array_start[])(void);
-extern void (*const __init_array_end[])(void);
-extern void _init(void);
-
 extern const uint8_t __start_cold[];
 
 extern const uint8_t __warmboot_crt0_start__[], __warmboot_crt0_end__[], __warmboot_crt0_lma__[];
@@ -126,15 +120,6 @@ __attribute__((target("cmodel=large"), noinline)) static void copy_other_section
     copy_lma_to_vma(__vectors_start__, __vectors_lma__, __vectors_end__ - __vectors_start__);
 }
 
-__attribute__((target("cmodel=large"), noinline)) static void __libc_init_array(void) {
-    /* On the stock secmon the init array lives in crt0 rodata, but whatever... */
-    for (size_t i = 0; i < __preinit_array_end - __preinit_array_start; i++)
-        __preinit_array_start[i]();
-    //_init(); /* FIXME: do we have this gcc-provided symbol if we build with -nostartfiles? */
-    for (size_t i = 0; i < __init_array_end - __init_array_start; i++)
-        __init_array_start[i]();
-}
-
 void coldboot_init(void) {
     /* TODO: Set NX BOOTLOADER clock time field */
     copy_warmboot_crt0();
@@ -150,5 +135,4 @@ void coldboot_init(void) {
     /* At this point we can access the mapped segments */
     /* TODO: zero-initialize the cpu context */
     /* Nintendo clears the (emtpy) pk2ldr's BSS section, but we embed it 0-filled in the binary */
-    __libc_init_array();  /* construct global objects */
 }

exosphere/src/coldboot_main.c

@@ -4,28 +4,24 @@
 #include "memory_map.h"
 #include "cache.h"
 
-/*
-extern void (*__fini_array_start[])(void);
-extern void (*__fini_array_end[])(void);
-extern void _fini(void);*/
-
 extern uint8_t __pk2ldr_start__[], __pk2ldr_end__[];
 
 extern void __jump_to_lower_el(uint64_t arg, uintptr_t ep, unsigned int el);
 
+void _init(void);
+void _fini(void);
+
 void coldboot_main(void);
 
-#if 0
+void _init(void) {
-/* Needs to be called for EL3->EL3 chainloading (and only in that case). TODO: use it */
+    /* Stubbed. Should have been provided by gcc but we have set -nostartfiles */
-__attribute__((used)) static void __libc_fini_array(void) {
+}
-    for (size_t i = __fini_array_end - __fini_array_start; i > 0; i--)
+
-        __fini_array_start[i - 1]();
+void _fini(void) {
-    _fini(); /* FIXME: do we have this gcc-provided symbol if we build with -nostartfiles? */
+    /* Stubbed. Should have been provided by gcc but we have set -nostartfiles */
 }
-#endif
 
 void coldboot_main(void) {
-    #if 0
     uintptr_t *mmu_l3_table = (uintptr_t *)TZRAM_GET_SEGMENT_ADDRESS(TZRAM_SEGMENT_ID_L3_TRANSLATION_TABLE);
     uintptr_t pk2ldr = TZRAM_GET_SEGMENT_ADDRESS(TZRAM_SEGMENT_ID_PK2LDR);
 
@@ -33,7 +29,6 @@ void coldboot_main(void) {
     memset((void *)pk2ldr, 0, __pk2ldr_end__ - __pk2ldr_start__);
     mmu_unmap_range(3, mmu_l3_table, pk2ldr, __pk2ldr_end__ - __pk2ldr_start__);
     tlb_invalidate_all_inner_shareable();
-    #endif
 
     /* TODO: stuff & jump to lower EL */
 }

exosphere/src/memory_map.h

@@ -54,7 +54,7 @@
 #define MMIO_BASE                       0x1F0080000ull
 #define LP0_ENTRY_RAM_SEGMENT_BASE      (MMIO_BASE + 0x000100000ull)
 #define WARMBOOT_RAM_SEGMENT_BASE       (LP0_ENTRY_RAM_SEGMENT_BASE + 0x000047000ull) /* increment seems to be arbitrary ? */
-#define TZRAM_SEGMENT_BASE              (MMIO_BASE + 0x0001E0000ull)
+#define TZRAM_SEGMENT_BASE              (MMIO_BASE + 0x000160000ull)
 
 #define IDENTITY_MAPPING_IRAM_CD        0
 #define IDENTITY_MAPPING_TZRAM          1

exosphere/src/mmu.h

@@ -151,21 +151,21 @@ static inline void mmu_unmap(unsigned int level, uintptr_t *tbl, uintptr_t base_
 
 static inline void mmu_map_block_range(unsigned int level, uintptr_t *tbl, uintptr_t base_addr, uintptr_t phys_addr, size_t size, uint64_t attrs) {
     size = ((size + (BITL(MMU_Lx_SHIFT(level)) - 1)) >> MMU_Lx_SHIFT(level)) << MMU_Lx_SHIFT(level);
-    for(size_t offset = 0; offset < size; offset += MMU_Lx_SHIFT(level)) {
+    for(size_t offset = 0; offset < size; offset += BITL(MMU_Lx_SHIFT(level))) {
         mmu_map_block(level, tbl, base_addr + offset, phys_addr + offset, attrs);
     }
 }
 
 static inline void mmu_map_page_range(uintptr_t *tbl, uintptr_t base_addr, uintptr_t phys_addr, size_t size, uint64_t attrs) {
     size = ((size + (BITL(MMU_Lx_SHIFT(3)) - 1)) >> MMU_Lx_SHIFT(3)) << MMU_Lx_SHIFT(3);
-    for(size_t offset = 0; offset < size; offset += MMU_Lx_SHIFT(3)) {
+    for(size_t offset = 0; offset < size; offset += BITL(MMU_Lx_SHIFT(3))) {
         mmu_map_page(tbl, base_addr + offset, phys_addr + offset, attrs);
     }
 }
 
 static inline void mmu_unmap_range(unsigned int level, uintptr_t *tbl, uintptr_t base_addr, size_t size) {
     size = ((size + (BITL(MMU_Lx_SHIFT(level)) - 1)) >> MMU_Lx_SHIFT(level)) << MMU_Lx_SHIFT(level);
-    for(size_t offset = 0; offset < size; offset += MMU_Lx_SHIFT(level)) {
+    for(size_t offset = 0; offset < size; offset += BITL(MMU_Lx_SHIFT(level))) {
         mmu_unmap(level, tbl, base_addr + offset);
     }
 }

exosphere/src/start.s

@@ -1,5 +1,19 @@
 /* For some reason GAS doesn't know about it, even with .cpu cortex-a57 */
 #define cpuactlr_el1 s3_1_c15_c2_0
+.macro      RESET_CORE
+    mov  x0, #(1 << 63)
+    msr  cpuactlr_el1, x0 /* disable regional clock gating */
+    isb
+    mov  x0, #3
+    msr  rmr_el3, x0
+    isb
+    dsb  sy
+    /* Nintendo forgot to copy-paste the branch instruction below. */
+    1:
+        wfi
+        b 1b
+.endm
+
 .macro      ERRATUM_INVALIDATE_BTB_AT_BOOT
 /* Nintendo copy-pasted https://github.com/ARM-software/arm-trusted-firmware/blob/master/plat/nvidia/tegra/common/aarch64/tegra_helpers.S#L312 */
         /*
@@ -42,17 +56,8 @@
     mov  x0, xzr
     msr  oslar_el1, x0
 
-    mov  x0, #(1 << 63)
+    RESET_CORE
-    msr  cpuactlr_el1, x0 /* disable regional clock gating */
+
-    isb
-    mov  x0, #3
-    msr  rmr_el3, x0
-    isb
-    dsb  sy
-    /* Nintendo forgot to copy-paste the branch instruction below. */
-    1:
-        wfi
-        b 1b
 .rept 65
     nop                     /* guard against speculative excecution */
 .endr
@@ -70,7 +75,7 @@ __start_cold:
     ERRATUM_INVALIDATE_BTB_AT_BOOT
 
     msr  spsel, #0
-    bl   get_coldboot_crt0_stack_address /* should be optimized so it doesn't make function calls */
+    bl   get_coldboot_crt0_stack_address
     mov  sp, x0
     bl   coldboot_init
     ldr  x16, =__jump_to_main_cold
@@ -84,7 +89,7 @@ __start_warm:
 
     /* For some reasons, Nintendo uses spsel, #1 here, causing issues if an exception occurs */
     msr  spsel, #0
-    bl   get_warmboot_crt0_stack_address /* should be optimized so it doesn't make function calls */
+    bl   get_warmboot_crt0_stack_address
     mov  sp, x0
     bl   warmboot_init
     ldr  x16, =__jump_to_main_warm
@@ -93,16 +98,28 @@ __start_warm:
 .align      4
 .section    .text.__jump_to_main_cold, "ax", %progbits
 __jump_to_main_cold:
+    /* This is inspired by Nintendo's code but significantly different */
     bl   __set_exception_entry_stack_pointer
 
     bl   get_pk2ldr_stack_address
     mov  sp, x0
+    /*
+        Normally Nintendo calls it in crt0, but it's fine to do that here
+        note that package2.c shouldn't have constructed objects, because we
+        call __libc_fini_array after load_package2 has been cleared, on EL3
+        to EL3 chainload.
+    */
+    bl   __libc_init_array
     bl   load_package2
 
     mov  w0, #3 /* use core3 stack temporarily */
     bl   get_exception_entry_stack_address
     mov  sp, x0
-    b    coldboot_main
+    bl   coldboot_main
+    /* If we ever return, it's to chainload an EL3 payload */
+    bl   __libc_fini_array
+    /* Reset the core (only one is running on coldboot) */
+    RESET_CORE
 
 .section    .text.__jump_to_main_warm, "ax", %progbits
 __jump_to_main_warm:
@@ -127,7 +144,7 @@ __set_exception_entry_stack_pointer:
     mrs  x17, spsel
     mrs  x0, mpidr_el1
     and  w0, w0, #3
-    bl   get_exception_entry_stack_address /* should be optimized so it doesn't make function calls */
+    bl   get_exception_entry_stack_address
     msr  spsel, #1
     mov  sp, x0
     msr  spsel, x17

exosphere/src/warmboot_init.c

@@ -12,7 +12,7 @@ void invalidate_icache_all_tzram_pa(void) {
 }
 
 uintptr_t get_warmboot_crt0_stack_address(void) {
-    return TZRAM_GET_SEGMENT_ADDRESS(TZRAM_SEGMENT_ID_CORE3_STACK) + 0x800;
+    return TZRAM_GET_SEGMENT_PA(TZRAM_SEGMENT_ID_CORE012_STACK) + 0x800;
 }
 
 void warmboot_init(void) {