From 992e08a93ac940ecbf1ff488f728301f289dcb97 Mon Sep 17 00:00:00 2001 From: Mark Date: Wed, 9 Oct 2019 23:45:19 +0300 Subject: [PATCH] Added AP initialization code --- include/sys/amd64/hw/gdt.h | 2 + include/sys/amd64/hw/idt.h | 8 +++ sys/amd64/conf.mk | 13 ++++- sys/amd64/entry.S | 8 +++ sys/amd64/hw/ap_code.nasm | 111 ++++++++++++++++++++++++++++++++++++ sys/amd64/hw/ap_code_blob.S | 8 +++ sys/amd64/hw/apic.c | 105 ++++++++++++++++++++++++++++++++-- sys/amd64/hw/gdt.c | 4 +- sys/amd64/hw/idt.c | 7 +-- sys/amd64/hw/irq0.S | 14 +---- sys/amd64/mm/mm.c | 4 +- 11 files changed, 258 insertions(+), 26 deletions(-) create mode 100644 sys/amd64/hw/ap_code.nasm create mode 100644 sys/amd64/hw/ap_code_blob.S diff --git a/include/sys/amd64/hw/gdt.h b/include/sys/amd64/hw/gdt.h index 5825881..00b604b 100644 --- a/include/sys/amd64/hw/gdt.h +++ b/include/sys/amd64/hw/gdt.h @@ -34,4 +34,6 @@ typedef struct { uintptr_t offset; } __attribute__((packed)) amd64_gdt_ptr_t; +extern amd64_gdt_ptr_t amd64_gdtr; + void amd64_gdt_init(void); diff --git a/include/sys/amd64/hw/idt.h b/include/sys/amd64/hw/idt.h index b580ca3..672ea69 100644 --- a/include/sys/amd64/hw/idt.h +++ b/include/sys/amd64/hw/idt.h @@ -1,3 +1,11 @@ #pragma once +#include "sys/types.h" + +struct amd64_idtr { + uint16_t size; + uintptr_t offset; +} __attribute__((packed)); + +extern const struct amd64_idtr amd64_idtr; void amd64_idt_init(void); diff --git a/sys/amd64/conf.mk b/sys/amd64/conf.mk index 928ffcd..13b8760 100644 --- a/sys/amd64/conf.mk +++ b/sys/amd64/conf.mk @@ -1,5 +1,7 @@ include sys/amd64/compiler.mk +.PHONY+=$(O)/sys/amd64/hw/ap_code_blob.o + all: ### Kernel build DEFINES+=-DARCH_AMD64 @@ -12,7 +14,8 @@ OBJS+=$(O)/sys/amd64/hw/rs232.o \ $(O)/sys/amd64/hw/apic.o \ $(O)/sys/amd64/hw/idt.o \ $(O)/sys/amd64/hw/exc_s.o \ - $(O)/sys/amd64/hw/irq0.o + $(O)/sys/amd64/hw/irq0.o \ + $(O)/sys/amd64/hw/ap_code_blob.o kernel_OBJS=$(O)/sys/amd64/entry.o \ $(OBJS) kernel_LINKER=sys/amd64/link.ld @@ -59,6 +62,14 @@ $(O)/sys/%.o: sys/%.c $(HEADERS) @printf " CC\t%s\n" $(@:$(O)/%=%) @$(CC64) $(kernel_CFLAGS) -c -o $@ $< +$(O)/sys/amd64/hw/ap_code_blob.o: $(O)/sys/amd64/hw/ap_code.bin + @printf " AS\t%s\n" $(@:$(O)/%=%) + @$(CC64) $(kernel_CFLAGS) -c -DAP_CODE_BIN='"$<"' -o $@ sys/amd64/hw/ap_code_blob.S + +$(O)/sys/amd64/hw/ap_code.bin: sys/amd64/hw/ap_code.nasm + @printf " NASM\t%s\n" $(@:$(O)/%=%) + @nasm -f bin -o $@ $< + ### Kernel loader build TARGETS+=$(O)/sys/amd64/image.iso DIRS+=$(O)/sys/amd64/loader diff --git a/sys/amd64/entry.S b/sys/amd64/entry.S index c7d9be2..d83ff53 100644 --- a/sys/amd64/entry.S +++ b/sys/amd64/entry.S @@ -13,7 +13,15 @@ _start: hlt jmp 1b +// Entrypoint for AP bootstrap code +.global ap_kernel_stacks_top + .section .bss kernel_stack_bottom: .skip 65536 kernel_stack_top: + +.set MAX_CPUS, 4 +ap_kernel_stacks_bottom: + .skip 65536 * (MAX_CPUS - 1) +ap_kernel_stacks_top: diff --git a/sys/amd64/hw/ap_code.nasm b/sys/amd64/hw/ap_code.nasm new file mode 100644 index 0000000..442f69a --- /dev/null +++ b/sys/amd64/hw/ap_code.nasm @@ -0,0 +1,111 @@ +; vi: ft=asm : +org 0x7000 +bits 16 + +; Application processor bootstrapping code +; -------------------------------------------- +; Parameters passed to us from BSP: +; 0x7FC0 Kernel PML4 physical address +; 0x7FC8 GDTR physical address +; 0x7FD0 IDTR +; 0x7FD8 Kernel AP core entrypoint +; 0x7FE0 Kernel AP stack +; -------------------------------------------- + +ap_startup: + cli + + mov ax, 0 + mov ds, ax + + ; Disable NMI + in al, 0x70 + or al, 0x80 + out 0x70, al + + ; Load protected-mode GDT + lgdt [ap_prot_gdtr] + mov eax, cr0 + or al, 1 + mov cr0, eax + + jmp 0x08:ap_startup_prot + +bits 32 +ap_startup_prot: + cli + + mov eax, 0x10 + mov ds, eax + + ; Enable PAE, PSE + mov eax, cr4 + or eax, (1 << 5) | (1 << 4) + mov cr4, eax + + ; Load cr3 = PML4 + mov eax, dword [0x7FC0] + mov cr3, eax + + ; Enable EFER.LME + mov ecx, 0xC0000080 + rdmsr + or eax, 1 << 8 + wrmsr + + ; Enable paging + mov eax, cr0 + or eax, 1 << 31 + mov cr0, eax + + ; Load long-mode GDT (will be reloaded with upper address) + mov eax, [0x7FC8] + lgdt [eax] + + jmp 0x08:ap_startup_long + +bits 64 +align 8 +ap_startup_long: + mov rax, 0x10 + mov ds, rax + + ; Reload GDT from upper memory pointer + mov rax, [0x7FC8] + mov rcx, 0xFFFFFF0000000000 + add rax, rcx + + lgdt [rax] + + mov rax, 0x10 + mov ds, rax + mov ss, rax + mov es, rax + mov fs, rax + mov gs, rax + + ; Load IDT + mov rax, qword [0x7FD0] + lidt [rax] + + ; Load stack + mov rsp, qword [0x7FE0] + + ; Jump to kernel entry + mov rax, qword [0x7FD8] + jmp rax + +; Protected-mode GDT +align 4 +ap_prot_gdt: + ; 0x00 - Null + dq 0 + ; 0x08 - Code: 32-bit, ex, pr, sys + dq 0xCF98000000FFFF + ; 0x10 - Data: 32-bit, rw, pr, sys + dq 0xCF92000000FFFF +ap_prot_gdt_end: + +ap_prot_gdtr: + dw ap_prot_gdt_end - ap_prot_gdt - 1 + dd ap_prot_gdt diff --git a/sys/amd64/hw/ap_code_blob.S b/sys/amd64/hw/ap_code_blob.S new file mode 100644 index 0000000..32f079f --- /dev/null +++ b/sys/amd64/hw/ap_code_blob.S @@ -0,0 +1,8 @@ +.section .rodata +.global amd64_ap_code_start +.global amd64_ap_code_end + +.align 8 +amd64_ap_code_start: + .incbin AP_CODE_BIN +amd64_ap_code_end: diff --git a/sys/amd64/hw/apic.c b/sys/amd64/hw/apic.c index 4ad4363..f6c0fb3 100644 --- a/sys/amd64/hw/apic.c +++ b/sys/amd64/hw/apic.c @@ -1,4 +1,8 @@ #include "sys/amd64/hw/apic.h" +#include "sys/amd64/hw/gdt.h" +#include "sys/amd64/hw/idt.h" +#include "sys/amd64/mm/mm.h" +#include "sys/string.h" #include "sys/debug.h" #define IA32_APIC_BASE_MSR 0x1B @@ -27,12 +31,6 @@ static uint64_t rdmsr(uint32_t addr) { return v; } -// static void wrmsr(uint32_t addr, uint64_t v) { -// uint32_t low = v & 0xFFFFFFFF; -// uint32_t high = v >> 32; -// asm volatile ("wrmsr"::"c"(addr),"d"(high),"a"(low)); -// } - ///// struct acpi_apic_field_type { @@ -65,6 +63,10 @@ static uint32_t bsp_lapic_id; // (mapped to per-CPU LAPICs, while the address is the same) static uintptr_t lapic_base; +extern char ap_kernel_stacks_top[]; +// TODO: use mutual exclusion for this +static size_t started_up_aps = 0; + ///// static uintptr_t amd64_apic_base(void) { @@ -86,6 +88,76 @@ static void amd64_pic8259_disable(void) { ); } +static void amd64_ap_code_entry(void) { + // Can do this as core should've bootstrapped BEFORE BSP checks this value again + ++started_up_aps; + + while (1) { + asm ("cli; hlt"); + } +} + +static void amd64_load_ap_code(void) { + extern const char amd64_ap_code_start[]; + extern const char amd64_ap_code_end[]; + //extern void amd64_ap_code_entry(void); + size_t ap_code_size = (uintptr_t) amd64_ap_code_end - (uintptr_t) amd64_ap_code_start; + + // Startup vector physical address, below 1M boundary + uintptr_t physical_address = 0x7000; + + // Load code at 0x7000 + memcpy((void *) (0xFFFFFF0000000000 + 0x7000), amd64_ap_code_start, ap_code_size); + + // These parameters are shared and may be loaded only once + // Write AP code startup parameters + extern mm_space_t mm_kernel; + uintptr_t mm_kernel_phys = (uintptr_t) mm_kernel - 0xFFFFFF0000000000; + uintptr_t amd64_gdtr_phys = (uintptr_t) &amd64_gdtr - 0xFFFFFF0000000000; + + // 0x7FC0 - MM_PHYS(mm_kernel) + *((uint64_t *) 0xFFFFFF0000007FC0) = mm_kernel_phys; + // 0x7FC8 - MM_PHYS(amd64_gdtr) + *((uint64_t *) 0xFFFFFF0000007FC8) = amd64_gdtr_phys; + // 0x7FD0 - amd64_idtr + *((uint64_t *) 0xFFFFFF0000007FD0) = (uintptr_t) &amd64_idtr; + // 0x7FD8 - amd64_core_entry + *((uint64_t *) 0xFFFFFF0000007FD8) = (uintptr_t) amd64_ap_code_entry; +} + +static void amd64_set_ap_params(void) { + // Allocate a new AP kernel stack + uintptr_t stack_ptr = (uintptr_t) ap_kernel_stacks_top - started_up_aps * 65536; + // 0x7FE0 - stack_ptr + *((uint64_t *) 0xFFFFFF0000007FE0) = stack_ptr; +} + +static void amd64_core_wakeup(uint8_t core_id) { + amd64_set_ap_params(); + + uint8_t entry_vector = 0x7000 >> 12; + + *(uint32_t *) (lapic_base + IA32_LAPIC_REG_CMD1) = ((uint32_t) core_id) << 24; + *(uint32_t *) (lapic_base + IA32_LAPIC_REG_CMD0) = entry_vector | (5 << 8) | (1 << 14); + + for (uint64_t i = 0; i < 1000000; ++i); + + size_t old_ap_count = started_up_aps; + + *(uint32_t *) (lapic_base + IA32_LAPIC_REG_CMD1) = ((uint32_t) core_id) << 24; + *(uint32_t *) (lapic_base + IA32_LAPIC_REG_CMD0) = entry_vector | (6 << 8) | (1 << 14); + + + for (uint64_t i = 0; i < 10000000; ++i); + + if (started_up_aps == old_ap_count) { + kdebug("AP failed to start: LAPIC ID %d\n", core_id); + while (1) { + asm ("cli; hlt"); + }; + } +} + void amd64_apic_init(struct acpi_madt *madt) { // Get LAPIC base lapic_base = amd64_apic_base() + 0xFFFFFF0000000000; @@ -107,4 +179,25 @@ void amd64_apic_init(struct acpi_madt *madt) { *(uint32_t *) (lapic_base + IA32_LAPIC_REG_LVTT) = 32 | 0x20000; *(uint32_t *) (lapic_base + IA32_LAPIC_REG_TMRDIV) = 0x3; *(uint32_t *) (lapic_base + IA32_LAPIC_REG_TMRINITCNT) = 100000; + + // Load the code APs are expected to run + amd64_load_ap_code(); + // Get other LAPICs from MADT + size_t offset = 0; + while (offset < madt->hdr.length - sizeof(struct acpi_madt)) { + struct acpi_apic_field_type *ent_hdr = (struct acpi_apic_field_type *) &madt->entry[offset]; + + if (ent_hdr->type == 0) { + // LAPIC entry + struct acpi_lapic_entry *ent = (struct acpi_lapic_entry *) ent_hdr; + + // It's not us + if (ent->apic_id != bsp_lapic_id) { + // Initiate wakeup sequence + amd64_core_wakeup(ent->apic_id); + } + } + + offset += ent_hdr->length; + } } diff --git a/sys/amd64/hw/gdt.c b/sys/amd64/hw/gdt.c index ebe0153..2dd3220 100644 --- a/sys/amd64/hw/gdt.c +++ b/sys/amd64/hw/gdt.c @@ -4,7 +4,7 @@ extern void amd64_gdt_load(void *p); static amd64_gdt_entry_t gdt[GDT_SIZE] = { 0 }; -static amd64_gdt_ptr_t gdtr = { +amd64_gdt_ptr_t amd64_gdtr = { sizeof(gdt) - 1, (uintptr_t) gdt }; @@ -42,5 +42,5 @@ void amd64_gdt_init(void) { 0, GDT_ACC_PR | GDT_ACC_S | GDT_ACC_RW); - amd64_gdt_load(&gdtr); + amd64_gdt_load(&amd64_gdtr); } diff --git a/sys/amd64/hw/idt.c b/sys/amd64/hw/idt.c index 7a04ebf..f9a7790 100644 --- a/sys/amd64/hw/idt.c +++ b/sys/amd64/hw/idt.c @@ -27,10 +27,7 @@ static struct amd64_idt_entry { uint32_t zero1; } __attribute__((packed)) idt[256] __attribute__((aligned(0x10))); -static const struct { - uint16_t size; - uintptr_t offset; -} __attribute__((packed)) idtr __attribute__((aligned(0x10))) = { +const struct amd64_idtr amd64_idtr = { .size = sizeof(idt) - 1, .offset = (uintptr_t) idt }; @@ -52,5 +49,5 @@ void amd64_idt_init(void) { } amd64_idt_set(32, (uintptr_t) amd64_irq0, 0x08, IDT_FLG_P | IDT_FLG_R0 | IDT_FLG_INT32); - asm volatile ("lidt idtr(%rip)"); + asm volatile ("lidt amd64_idtr(%rip)"); } diff --git a/sys/amd64/hw/irq0.S b/sys/amd64/hw/irq0.S index dc2a965..183b0f0 100644 --- a/sys/amd64/hw/irq0.S +++ b/sys/amd64/hw/irq0.S @@ -1,11 +1,6 @@ .section .text .global amd64_irq0 amd64_irq0: - pushq %rax - movq $0xFFFFFF00FEE000B0, %rax - movl $0, (%rax) - popq %rax - pushq %rax pushq %rcx pushq %rdx @@ -14,9 +9,9 @@ amd64_irq0: pushq %rdi pushq %rsi - xorq %rdi, %rdi - leaq _tmr_msg(%rip), %rsi - call debugs + // EOI + movq $0xFFFFFF00FEE000B0, %rax + movl $0, (%rax) popq %rsi popq %rdi @@ -27,6 +22,3 @@ amd64_irq0: popq %rax iretq - -_tmr_msg: - .string "T!\n" diff --git a/sys/amd64/mm/mm.c b/sys/amd64/mm/mm.c index 054066d..9688277 100644 --- a/sys/amd64/mm/mm.c +++ b/sys/amd64/mm/mm.c @@ -27,6 +27,8 @@ void amd64_mm_init(struct amd64_loader_data *data) { memset((void *) (0x200000 - 2 * 0x1000), 0, 0x1000 * 2); + // 0x0000000000000000 -> 0 Mapping for AP bootstrapping + pml4[0] = ((uintptr_t) pdpt) | 1 | 2 | 4; // 0xFFFFFF0000000000 -> 0 (512GiB) mapping pml4[AMD64_MM_STRIPSX(KERNEL_VIRT_BASE) >> 39] = ((uintptr_t) pdpt) | 1 | 2 | 4; for (uint64_t i = 0; i < 4; ++i) { @@ -40,7 +42,7 @@ void amd64_mm_init(struct amd64_loader_data *data) { // Create a pool located right after kernel image // amd64_mm_pool_init((uintptr_t) &_kernel_end, MM_POOL_SIZE); - // mm_kernel = (mm_space_t) (MM_VIRTUALIZE(pml4)); + mm_kernel = (mm_space_t) (MM_VIRTUALIZE(pml4)); // // Allocate some pages for kernel heap (base size: 16MiB) // uintptr_t heap_base_phys = amd64_phys_alloc_contiguous(KERNEL_HEAP >> 12);