Get SMP to work on UEFI

stage23/mm/mtrr.32.c

@@ -0,0 +1,28 @@
+#include <stdint.h>
+#include <stddef.h>
+#include <mm/mtrr.h>
+#include <sys/cpu.h>
+#include <lib/blib.h>
+
+static bool mtrr_supported(void) {
+    uint32_t eax, ebx, ecx, edx;
+
+    if (!cpuid(1, 0, &eax, &ebx, &ecx, &edx))
+        return false;
+
+    return !!(edx & (1 << 12));
+}
+
+void mtrr_restore_32(struct mtrr *saved_mtrr) {
+    if (!mtrr_supported())
+        return;
+
+    uint64_t ia32_mtrrcap = rdmsr(0xfe);
+
+    uint8_t var_reg_count = ia32_mtrrcap & 0xff;
+
+    for (uint8_t i = 0; i < var_reg_count; i++) {
+        wrmsr(0x200 + i * 2,     saved_mtrr[i].base);
+        wrmsr(0x200 + i * 2 + 1, saved_mtrr[i].mask);
+    }
+}

stage23/mm/mtrr.c

@@ -6,11 +6,6 @@
 #include <lib/print.h>
 #include <lib/blib.h>
 
-struct mtrr {
-    uint64_t base;
-    uint64_t mask;
-};
-
 static bool mtrr_supported(void) {
     uint32_t eax, ebx, ecx, edx;
 
@@ -107,7 +102,7 @@ bool mtrr_set_range(uint64_t base, uint64_t size, uint8_t memory_type) {
     return false;
 }
 
-static struct mtrr *saved_mtrr = NULL;
+struct mtrr *saved_mtrr = NULL;
 
 void mtrr_save(void) {
     if (!mtrr_supported())

stage23/mm/mtrr.h

@@ -10,8 +10,16 @@
 #define MTRR_MEMORY_TYPE_WP 0x05
 #define MTRR_MEMORY_TYPE_WB 0x06
 
+struct mtrr {
+    uint64_t base;
+    uint64_t mask;
+};
+
+extern struct mtrr *saved_mtrr;
+
 void mtrr_save(void);
 void mtrr_restore(void);
+void mtrr_restore_32(struct mtrr *saved_mtrr);
 bool mtrr_set_range(uint64_t base, uint64_t size, uint8_t caching_type);
 
 #endif

stage23/protos/stivale.c

@@ -200,6 +200,10 @@ void stivale_load(char *config, char *cmdline) {
     stivale_struct.memory_map_entries = (uint64_t)memmap_entries;
     stivale_struct.memory_map_addr    = (uint64_t)(size_t)memmap;
 
+#if defined (uefi)
+    efi_exit_boot_services();
+#endif
+
     stivale_spinup(bits, want_5lv, &pagemap,
                    entry_point, &stivale_struct, stivale_hdr.stack);
 }
@@ -271,10 +275,6 @@ __attribute__((noreturn)) void stivale_spinup(
     }
 #endif
 
-#if defined (uefi)
-    efi_exit_boot_services();
-#endif
-
     pic_mask_all();
     pic_flush();
 

stage23/protos/stivale2.c

@@ -299,6 +299,10 @@ void stivale2_load(char *config, char *cmdline, bool pxe) {
     if (bits == 64)
         pagemap = stivale_build_pagemap(level5pg && level5pg_requested);
 
+#if defined (uefi)
+    efi_exit_boot_services();
+#endif
+
     //////////////////////////////////////////////
     // Create SMP struct tag
     //////////////////////////////////////////////

stage23/sys/gdt.h

@@ -4,8 +4,8 @@
 #include <stdint.h>
 
 struct gdtr {
-    uint16_t  limit;
-    uintptr_t ptr;
+    uint16_t limit;
+    uint64_t ptr;
 } __attribute__((packed));
 
 struct gdt_desc {

stage23/sys/smp.c

@@ -52,8 +52,9 @@ struct trampoline_passed_info {
     uint8_t  smp_tpl_booted_flag;
     uint8_t  smp_tpl_target_mode;
     uint32_t smp_tpl_pagemap;
-    void *mtrr_restore_vector;
-    struct smp_information *smp_tpl_info_struct;
+    uint32_t mtrr_restore_vector;
+    uint32_t saved_mtrr_ptr;
+    uint32_t smp_tpl_info_struct;
     struct gdtr smp_tpl_gdt;
 } __attribute__((packed));
 
@@ -78,14 +79,19 @@ static bool smp_start_ap(uint32_t lapic_id, struct gdtr *gdtr,
                                - sizeof(struct trampoline_passed_info));
     }
 
-    passed_info->smp_tpl_info_struct = info_struct;
+    passed_info->smp_tpl_info_struct = (uint32_t)(uintptr_t)info_struct;
     passed_info->smp_tpl_booted_flag = 0;
     passed_info->smp_tpl_pagemap     = pagemap;
     passed_info->smp_tpl_target_mode = ((uint32_t)x2apic << 2)
                         | ((uint32_t)lv5 << 1)
                         | (uint32_t)longmode;
     passed_info->smp_tpl_gdt         = *gdtr;
-    passed_info->mtrr_restore_vector = mtrr_restore;
+#if defined (bios)
+    passed_info->mtrr_restore_vector = (uint32_t)(uintptr_t)mtrr_restore;
+#elif defined (uefi)
+    passed_info->mtrr_restore_vector = (uint32_t)(uintptr_t)mtrr_restore_32;
+#endif
+    passed_info->saved_mtrr_ptr      = (uint32_t)(uintptr_t)saved_mtrr;
     passed_info->smp_tpl_booted_flag = 0;
 
     asm volatile ("" ::: "memory");
@@ -135,8 +141,7 @@ struct smp_information *init_smp(size_t    header_hack_size,
     if (madt == NULL)
         return NULL;
 
-    struct gdtr gdtr;
-    asm volatile ("sgdt %0" :: "m"(gdtr) : "memory");
+    struct gdtr gdtr = gdt;
 
     uint32_t eax, ebx, ecx, edx;
 

stage23/sys/smp_trampoline.real

@@ -5,11 +5,10 @@ smp_trampoline:
     cli
     cld
 
-jmp $
     mov ebx, cs
     shl ebx, 4
 
-    lgdt [cs:passed_info.gdtr]
+    o32 lgdt [cs:passed_info.gdtr]
 
     lea eax, [ebx + .mode32]
     mov [cs:.farjmp_off], eax
@@ -49,7 +48,9 @@ jmp $
   .nox2apic:
     lea esp, [ebx + temp_stack.top]
 
+    push dword [ebx + passed_info.saved_mtrr_ptr]
     call [ebx + passed_info.mtrr_restore_vector]
+    add esp, 4
 
     test dword [ebx + passed_info.target_mode], (1 << 0)
     jz parking32
@@ -169,7 +170,8 @@ passed_info:
     .target_mode db 0
     .pagemap dd 0
     .mtrr_restore_vector dd 0
+    .saved_mtrr_ptr dd 0
     .smp_info_struct dd 0
     .gdtr:
         dw 0
-        dd 0
+        dq 0