From 5488c8818b52a95b8205fd483be38f8416960c88 Mon Sep 17 00:00:00 2001
From: mintsuki <mintsuki@protonmail.com>
Date: Wed, 14 Apr 2021 11:06:14 +0200
Subject: [PATCH] stivale2: Implement unmap NULL feature

---
 stage23/protos/stivale.c  | 13 ++++++++++---
 stage23/protos/stivale.h  |  2 +-
 stage23/protos/stivale2.c | 31 ++++++++++++++++---------------
 3 files changed, 27 insertions(+), 19 deletions(-)

diff --git a/stage23/protos/stivale.c b/stage23/protos/stivale.c
index e73a033c..5f910ea6 100644
--- a/stage23/protos/stivale.c
+++ b/stage23/protos/stivale.c
@@ -201,7 +201,7 @@ void stivale_load(char *config, char *cmdline) {
 #endif
 
     bool want_5lv = level5pg && (stivale_hdr.flags & (1 << 1));
-    pagemap_t pagemap = stivale_build_pagemap(want_5lv);
+    pagemap_t pagemap = stivale_build_pagemap(want_5lv, false);
 
     size_t memmap_entries;
     struct e820_entry_t *memmap = get_memmap(&memmap_entries);
@@ -213,7 +213,7 @@ void stivale_load(char *config, char *cmdline) {
                    entry_point, &stivale_struct, stivale_hdr.stack);
 }
 
-pagemap_t stivale_build_pagemap(bool level5pg) {
+pagemap_t stivale_build_pagemap(bool level5pg, bool unmap_null) {
     pagemap_t pagemap = new_pagemap(level5pg ? 5 : 4);
     uint64_t higher_half_base = level5pg ? 0xff00000000000000 : 0xffff800000000000;
 
@@ -224,7 +224,8 @@ pagemap_t stivale_build_pagemap(bool level5pg) {
 
     // Map 0 to 4GiB at higher half base and 0
     for (uint64_t i = 0; i < 0x100000000; i += PAGE_SIZE) {
-        map_page(pagemap, i, i, 0x03);
+        if (!(i == 0 && unmap_null))
+            map_page(pagemap, i, i, 0x03);
         map_page(pagemap, higher_half_base + i, i, 0x03);
     }
 
@@ -240,6 +241,12 @@ pagemap_t stivale_build_pagemap(bool level5pg) {
         uint64_t length = _memmap[i].length;
         uint64_t top    = base + length;
 
+        if (base < 0x100000000)
+            base = 0x100000000;
+
+        if (base >= top)
+            continue;
+
         uint64_t aligned_base   = ALIGN_DOWN(base, PAGE_SIZE);
         uint64_t aligned_top    = ALIGN_UP(top, PAGE_SIZE);
         uint64_t aligned_length = aligned_top - aligned_base;
diff --git a/stage23/protos/stivale.h b/stage23/protos/stivale.h
index 367413a3..45cd8fef 100644
--- a/stage23/protos/stivale.h
+++ b/stage23/protos/stivale.h
@@ -8,7 +8,7 @@
 
 void stivale_load(char *config, char *cmdline);
 
-pagemap_t stivale_build_pagemap(bool level5pg);
+pagemap_t stivale_build_pagemap(bool level5pg, bool unmap_null);
 __attribute__((noreturn)) void stivale_spinup(
                  int bits, bool level5pg, pagemap_t *pagemap,
                  uint64_t entry_point, void *stivale_struct, uint64_t stack);
diff --git a/stage23/protos/stivale2.c b/stage23/protos/stivale2.c
index 56bfe37e..a2547687 100644
--- a/stage23/protos/stivale2.c
+++ b/stage23/protos/stivale2.c
@@ -370,6 +370,20 @@ skip_modeset:;
     }
     }
 
+#if defined (bios)
+    //////////////////////////////////////////////
+    // Create PXE struct tag
+    //////////////////////////////////////////////
+    if (pxe) {
+        struct stivale2_struct_tag_pxe_server_info *tag = ext_mem_alloc(sizeof(struct stivale2_struct_tag_pxe_server_info));
+        tag->tag.identifier = STIVALE2_STRUCT_TAG_PXE_SERVER_INFO;
+        tag->server_ip = get_boot_server_info();
+        append_tag(&stivale2_struct, (struct stivale2_tag *)tag);
+    }
+#else
+    (void)pxe;
+#endif
+
     //////////////////////////////////////////////
     // Create EFI system table struct tag
     //////////////////////////////////////////////
@@ -386,10 +400,11 @@ skip_modeset:;
 
     // Check if 5-level paging tag is requesting support
     bool level5pg_requested = get_tag(&stivale2_hdr, STIVALE2_HEADER_TAG_5LV_PAGING_ID) ? true : false;
+    bool unmap_null = get_tag(&stivale2_hdr, STIVALE2_HEADER_TAG_UNMAP_NULL_ID) ? true : false;
 
     pagemap_t pagemap = {0};
     if (bits == 64)
-        pagemap = stivale_build_pagemap(level5pg && level5pg_requested);
+        pagemap = stivale_build_pagemap(level5pg && level5pg_requested, unmap_null);
 
 #if defined (uefi)
     efi_exit_boot_services();
@@ -421,20 +436,6 @@ skip_modeset:;
     }
     }
 
-#if defined (bios)
-    //////////////////////////////////////////////
-    // Create PXE struct tag
-    //////////////////////////////////////////////
-    if (pxe) {
-        struct stivale2_struct_tag_pxe_server_info *tag = ext_mem_alloc(sizeof(struct stivale2_struct_tag_pxe_server_info));
-        tag->tag.identifier = STIVALE2_STRUCT_TAG_PXE_SERVER_INFO;
-        tag->server_ip = get_boot_server_info();
-        append_tag(&stivale2_struct, (struct stivale2_tag *)tag);
-    }
-#else
-    (void)pxe;
-#endif
-
     //////////////////////////////////////////////
     // Create memmap struct tag
     //////////////////////////////////////////////