From 3d4dcdde13f606b7045b7a19858274292f2413fb Mon Sep 17 00:00:00 2001
From: TuxSH <tuxsh@sfr.fr>
Date: Wed, 28 Feb 2018 23:35:30 +0100
Subject: [PATCH] Fix ttbl init pointer arith bug, other fixes

---
 exosphere/src/coldboot_init.c | 6 +++---
 exosphere/src/mmu.h           | 8 ++++----
 exosphere/src/utils.h         | 7 ++++++-
 exosphere/src/warmboot_init.c | 4 ++--
 4 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/exosphere/src/coldboot_init.c b/exosphere/src/coldboot_init.c
index f7c7d431e..326b0fdad 100644
--- a/exosphere/src/coldboot_init.c
+++ b/exosphere/src/coldboot_init.c
@@ -111,17 +111,17 @@ uintptr_t get_coldboot_crt0_stack_address(void) {
     return TZRAM_GET_SEGMENT_PA(TZRAM_SEGMENT_ID_CORE3_STACK) + 0x800;
 }
 
-__attribute__((target("cmodel=large"), noinline)) static void copy_warmboot_crt0(void) {
+FAR_REACHING static void copy_warmboot_crt0(void) {
     copy_lma_to_vma(__warmboot_crt0_start__, __warmboot_crt0_lma__, __warmboot_crt0_end__ - __warmboot_crt0_start__);
 }
 
-__attribute__((target("cmodel=large"), noinline)) static void copy_other_sections(void) {
+FAR_REACHING static void copy_other_sections(void) {
     copy_lma_to_vma(__main_start__, __main_lma__, __main_end__ - __main_start__);
     copy_lma_to_vma(__pk2ldr_start__, __pk2ldr_lma__, __pk2ldr_end__ - __pk2ldr_start__);
     copy_lma_to_vma(__vectors_start__, __vectors_lma__, __vectors_end__ - __vectors_start__);
 }
 
-__attribute__((target("cmodel=large"), noinline)) static void clear_bss(void) {
+FAR_REACHING static void clear_bss(void) {
     memset((void *)__pk2ldr_bss_start__, 0, __pk2ldr_end__ - __pk2ldr_bss_start__);
     memset((void *)__main_bss_start__, 0, __main_end__ - __main_bss_start__);
 }
diff --git a/exosphere/src/mmu.h b/exosphere/src/mmu.h
index 5c32dcbc2..ed9b885bc 100644
--- a/exosphere/src/mmu.h
+++ b/exosphere/src/mmu.h
@@ -13,14 +13,14 @@
 
 #if MMU_GRANULE_TYPE == 0
 #define MMU_Lx_SHIFT(x) (12 + 9 * (3 - (x)))
-#define MMU_Lx_MASK(x)  (BITL(9) - 1)
+#define MMU_Lx_MASK(x)  MASKL(9)
 #elif MMU_GRANULE_TYPE == 1
 /* 64 KB, no L0 here */
 #define MMU_Lx_SHIFT(x) (16 + 13 * (3 - (x)))
-#define MMU_Lx_MASK(x)  ((x) == 1 ? (BITL(5) - 1) : (BITL(13) - 1))
+#define MMU_Lx_MASK(x)  ((x) == 1 ? MASKL(5) : MASKL(13))
 #elif MMU_GRANULE_TYPE == 2
 #define MMU_Lx_SHIFT(x) (14 + 11 * (3 - (x)))
-#define MMU_Lx_MASK(x)  ((x) == 0 ? 1 : (BITL(11) - 1))
+#define MMU_Lx_MASK(x)  ((x) == 0 ? 1 : MASKL(11))
 #endif
 
 /*
@@ -119,7 +119,7 @@
 #define TCR_EL3_RSVD        (BIT(31) | BIT(23))
 
 static inline void mmu_init_table(uintptr_t *tbl, size_t num_entries) {
-    for(size_t i = 0; i < num_entries; i++) {
+    for(size_t i = 0; i < num_entries / 8; i++) {
         tbl[i] = MMU_PTE_TYPE_FAULT;
     }
 }
diff --git a/exosphere/src/utils.h b/exosphere/src/utils.h
index 2eddff5cb..e1dad970a 100644
--- a/exosphere/src/utils.h
+++ b/exosphere/src/utils.h
@@ -7,11 +7,16 @@
 
 #define BIT(n)      (1u   << (n))
 #define BITL(n)     (1ull << (n))
+#define MASK(n)     (BIT(n) - 1)
+#define MASKL(n)    (BITL(n) - 1)
+#define MASK2(a,b)  (MASK(a) & ~MASK(b))
+#define MASK2L(a,b) (MASKL(a) & ~MASKL(b))
 
 #define ALIGN(m)        __attribute__((aligned(m)))
 #define PACKED          __attribute__((packed))
 
 #define ALINLINE        __attribute__((always_inline))
+#define FAR_REACHING    __attribute__((target("cmodel=large"), noinline))
 
 __attribute__ ((noreturn)) void panic(uint32_t code);
 __attribute__ ((noreturn)) void generic_panic(void);
@@ -21,7 +26,7 @@ static inline uintptr_t get_physical_address(const void *vaddr) {
     uintptr_t PAR;
     __asm__ __volatile__ ("at s1e3r, %0" :: "r"(vaddr));
     __asm__ __volatile__ ("mrs %0, par_el1" : "=r"(PAR));
-    return (PAR & 1) ? 0ull : (PAR & 0x00000FFFFFFFF000ull) | ((uintptr_t)vaddr & 0xFFF);
+    return (PAR & 1) ? 0ull : (PAR & MASK2L(40, 12)) | ((uintptr_t)vaddr & MASKL(12));
 }
 
 static inline uint32_t read32le(const volatile void *dword, size_t offset) {
diff --git a/exosphere/src/warmboot_init.c b/exosphere/src/warmboot_init.c
index f8287185f..4d3f4458b 100644
--- a/exosphere/src/warmboot_init.c
+++ b/exosphere/src/warmboot_init.c
@@ -8,14 +8,14 @@ extern const uint8_t __main_start__[];
 void __set_memory_registers(uintptr_t ttbr0, uintptr_t vbar, uint64_t cpuectlr, uint32_t scr,
                             uint32_t tcr, uint32_t cptr, uint64_t mair, uint32_t sctlr);
 
-__attribute__((target("cmodel=large"))) void flush_dcache_all_tzram_pa(void) {
+FAR_REACHING void flush_dcache_all_tzram_pa(void) {
     uintptr_t pa = TZRAM_GET_SEGMENT_PA(TZRAM_SEGMENT_ID_WARMBOOT_CRT0_AND_MAIN);
     uintptr_t main_pa = pa | ((uintptr_t)__main_start__ & 0xFFF);
     uintptr_t v = (uintptr_t)flush_dcache_all - (uintptr_t)__main_start__ + (uintptr_t)main_pa;
     ((void (*)(void))v)();
 }
 
-__attribute__((target("cmodel=large"))) void invalidate_icache_all_inner_shareable_tzram_pa(void) {
+FAR_REACHING void invalidate_icache_all_inner_shareable_tzram_pa(void) {
     uintptr_t pa = TZRAM_GET_SEGMENT_PA(TZRAM_SEGMENT_ID_WARMBOOT_CRT0_AND_MAIN);
     uintptr_t main_pa = pa | ((uintptr_t)__main_start__ & 0xFFF);
     uintptr_t v = (uintptr_t)invalidate_icache_all_inner_shareable - (uintptr_t)__main_start__ + (uintptr_t)main_pa;