From 813418f7c91bef16f61aa8ff0d5725ae279c1054 Mon Sep 17 00:00:00 2001
From: mintsuki <mintsuki@protonmail.com>
Date: Fri, 29 Oct 2021 02:15:17 +0200
Subject: [PATCH] stivale2: Introduce fully virtual kernel mappings (bug fixes)

---
 stage23/lib/elf.c         | 23 +++++++++--------------
 stage23/protos/stivale.c  |  6 +++---
 stage23/protos/stivale.h  |  2 +-
 stage23/protos/stivale2.c |  2 +-
 4 files changed, 14 insertions(+), 19 deletions(-)

diff --git a/stage23/lib/elf.c b/stage23/lib/elf.c
index 80af3823..db12643f 100644
--- a/stage23/lib/elf.c
+++ b/stage23/lib/elf.c
@@ -457,8 +457,6 @@ int elf64_load(uint8_t *elf, uint64_t *entry_point, uint64_t *top, uint64_t *_sl
 
     uint64_t max_align = elf64_max_align(elf);
 
-    uint64_t base_load_addr;
-
     if (fully_virtual) {
         simulation = false;
 
@@ -486,9 +484,7 @@ int elf64_load(uint8_t *elf, uint64_t *entry_point, uint64_t *top, uint64_t *_sl
 
         uint64_t image_size = max_vaddr - min_vaddr;
 
-        base_load_addr = (uintptr_t)ext_mem_alloc_type_aligned(image_size, alloc_type, max_align);
-
-        *physical_base = base_load_addr;
+        *physical_base = (uintptr_t)ext_mem_alloc_type_aligned(image_size, alloc_type, max_align);
         *virtual_base = min_vaddr;
     }
 
@@ -520,23 +516,22 @@ final:
         if (use_paddr) {
             load_addr = phdr.p_paddr;
         } else {
-            load_addr = phdr.p_vaddr;
-
-            if (load_addr & ((uint64_t)1 << 63)) {
+            if (phdr.p_vaddr >= 0xffffffff80000000) {
                 higher_half = true;
-                load_addr -= FIXED_HIGHER_HALF_OFFSET_64;
 
                 if (fully_virtual) {
-                    load_addr += base_load_addr;
+                    load_addr = *physical_base + (phdr.p_vaddr - *virtual_base);
+                } else {
+                    load_addr = phdr.p_vaddr - FIXED_HIGHER_HALF_OFFSET_64;
                 }
             }
         }
 
-        if (higher_half == true && load_addr + phdr.p_memsz > 0x80000000) {
-            panic("elf: Higher half executable trying to load too high");
-        }
-
         if (!fully_virtual) {
+            if (higher_half == true && load_addr + phdr.p_memsz > 0x80000000) {
+                panic("elf: Higher half executable trying to load too high");
+            }
+
             load_addr += slide;
         }
 
diff --git a/stage23/protos/stivale.c b/stage23/protos/stivale.c
index c302401d..e14e9b7b 100644
--- a/stage23/protos/stivale.c
+++ b/stage23/protos/stivale.c
@@ -300,7 +300,7 @@ void stivale_load(char *config, char *cmdline) {
 
     pagemap_t pagemap = {0};
     if (bits == 64)
-        pagemap = stivale_build_pagemap(want_5lv, false, NULL, 0, false, 0);
+        pagemap = stivale_build_pagemap(want_5lv, false, NULL, 0, false, 0, 0);
 
     // Reserve 32K at 0x70000
     memmap_alloc_range(0x70000, 0x8000, MEMMAP_USABLE, true, true, false, false);
@@ -325,7 +325,7 @@ void stivale_load(char *config, char *cmdline) {
 }
 
 pagemap_t stivale_build_pagemap(bool level5pg, bool unmap_null, struct elf_range *ranges, size_t ranges_count,
-                                bool want_fully_virtual, uint64_t physical_base) {
+                                bool want_fully_virtual, uint64_t physical_base, uint64_t virtual_base) {
     pagemap_t pagemap = new_pagemap(level5pg ? 5 : 4);
     uint64_t higher_half_base = level5pg ? 0xff00000000000000 : 0xffff800000000000;
 
@@ -341,7 +341,7 @@ pagemap_t stivale_build_pagemap(bool level5pg, bool unmap_null, struct elf_range
 
             if (virt & ((uint64_t)1 << 63)) {
                 if (want_fully_virtual) {
-                    phys = physical_base + (virt - FIXED_HIGHER_HALF_OFFSET_64);
+                    phys = physical_base + (virt - virtual_base);
                 } else {
                     phys = virt - FIXED_HIGHER_HALF_OFFSET_64;
                 }
diff --git a/stage23/protos/stivale.h b/stage23/protos/stivale.h
index b4da17e0..c6123843 100644
--- a/stage23/protos/stivale.h
+++ b/stage23/protos/stivale.h
@@ -11,7 +11,7 @@ void stivale_load(char *config, char *cmdline);
 bool stivale_load_by_anchor(void **_anchor, const char *magic,
                             uint8_t *file, uint64_t filesize);
 pagemap_t stivale_build_pagemap(bool level5pg, bool unmap_null, struct elf_range *ranges, size_t ranges_count,
-                                bool want_fully_virtual, uint64_t physical_base);
+                                bool want_fully_virtual, uint64_t physical_base, uint64_t virtual_base);
 __attribute__((noreturn)) void stivale_spinup(
                  int bits, bool level5pg, pagemap_t *pagemap,
                  uint64_t entry_point, uint64_t stivale_struct, uint64_t stack,
diff --git a/stage23/protos/stivale2.c b/stage23/protos/stivale2.c
index d91aa686..d7e834d5 100644
--- a/stage23/protos/stivale2.c
+++ b/stage23/protos/stivale2.c
@@ -639,7 +639,7 @@ have_tm_tag:;
         pagemap = stivale_build_pagemap(want_5lv, unmap_null,
                                         want_pmrs ? ranges : NULL,
                                         want_pmrs ? ranges_count : 0,
-                                        want_fully_virtual, physical_base);
+                                        want_fully_virtual, physical_base, virtual_base);
 
 #if uefi == 1
     efi_exit_boot_services();