diff --git a/stage23/mm/mtrr.32.c b/stage23/mm/mtrr.32.c new file mode 100644 index 00000000..8ea988cd --- /dev/null +++ b/stage23/mm/mtrr.32.c @@ -0,0 +1,28 @@ +#include +#include +#include +#include +#include + +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); + } +} diff --git a/stage23/mm/mtrr.c b/stage23/mm/mtrr.c index 4c900f20..bcd24346 100644 --- a/stage23/mm/mtrr.c +++ b/stage23/mm/mtrr.c @@ -6,11 +6,6 @@ #include #include -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()) diff --git a/stage23/mm/mtrr.h b/stage23/mm/mtrr.h index 9f76136d..7fae542f 100644 --- a/stage23/mm/mtrr.h +++ b/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 diff --git a/stage23/protos/stivale.c b/stage23/protos/stivale.c index 4b2a66d4..4386235a 100644 --- a/stage23/protos/stivale.c +++ b/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(); diff --git a/stage23/protos/stivale2.c b/stage23/protos/stivale2.c index bcfc0570..7b5044e1 100644 --- a/stage23/protos/stivale2.c +++ b/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 ////////////////////////////////////////////// diff --git a/stage23/sys/gdt.h b/stage23/sys/gdt.h index e8eaf4a2..35f36a04 100644 --- a/stage23/sys/gdt.h +++ b/stage23/sys/gdt.h @@ -4,8 +4,8 @@ #include struct gdtr { - uint16_t limit; - uintptr_t ptr; + uint16_t limit; + uint64_t ptr; } __attribute__((packed)); struct gdt_desc { diff --git a/stage23/sys/smp.c b/stage23/sys/smp.c index 138d602c..da56568d 100644 --- a/stage23/sys/smp.c +++ b/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; diff --git a/stage23/sys/smp_trampoline.real b/stage23/sys/smp_trampoline.real index fcc24dc9..c7c42ca9 100644 --- a/stage23/sys/smp_trampoline.real +++ b/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