From a99a0c003d14a7ff1e03a49034f8736ee94d3396 Mon Sep 17 00:00:00 2001 From: Nick Smallbone Date: Sat, 2 May 2015 21:43:33 +0200 Subject: [PATCH] EFI: add support for SMP. Signed-off-by: Jessica Hamilton --- src/system/boot/platform/efi/Jamfile | 2 + src/system/boot/platform/efi/entry.S | 4 +- src/system/boot/platform/efi/smp.cpp | 457 ++++++++++++++++++ src/system/boot/platform/efi/smp.h | 30 ++ src/system/boot/platform/efi/smp_trampoline.S | 129 +++++ src/system/boot/platform/efi/start.cpp | 6 + 6 files changed, 626 insertions(+), 2 deletions(-) create mode 100644 src/system/boot/platform/efi/smp.cpp create mode 100644 src/system/boot/platform/efi/smp.h create mode 100644 src/system/boot/platform/efi/smp_trampoline.S diff --git a/src/system/boot/platform/efi/Jamfile b/src/system/boot/platform/efi/Jamfile index 19ee053356..8356f7674f 100644 --- a/src/system/boot/platform/efi/Jamfile +++ b/src/system/boot/platform/efi/Jamfile @@ -32,6 +32,8 @@ local platform_src = devices.cpp hpet.cpp cpu.cpp + smp.cpp + smp_trampoline.S support.S ; diff --git a/src/system/boot/platform/efi/entry.S b/src/system/boot/platform/efi/entry.S index 5c5180df7a..75717080d8 100644 --- a/src/system/boot/platform/efi/entry.S +++ b/src/system/boot/platform/efi/entry.S @@ -26,7 +26,7 @@ FUNCTION(efi_enter_kernel): movq %rdi, %cr3 // Load 64-bit enabled GDT - lgdtq long_gdtr(%rip) + lgdtq gLongGDTR(%rip) // Jump into the 64-bit code segment. push $KERNEL_CODE_SELECTOR @@ -63,7 +63,7 @@ FUNCTION(efi_enter_kernel): .data -long_gdtr: +SYMBOL(gLongGDTR): .word BOOT_GDT_SEGMENT_COUNT * 8 - 1 SYMBOL(gLongGDT): .quad 0 diff --git a/src/system/boot/platform/efi/smp.cpp b/src/system/boot/platform/efi/smp.cpp new file mode 100644 index 0000000000..d597ea5fde --- /dev/null +++ b/src/system/boot/platform/efi/smp.cpp @@ -0,0 +1,457 @@ +/* + * Copyright 2008, Dustin Howett, dustin.howett@gmail.com. All rights reserved. + * Copyright 2004-2010, Axel Dörfler, axeld@pinc-software.de. + * Distributed under the terms of the MIT License. + * + * Copyright 2001, Travis Geiselbrecht. All rights reserved. + * Distributed under the terms of the NewOS License. +*/ + + +#include "smp.h" + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mmu.h" +#include "acpi.h" + + +#define NO_SMP 0 + +//#define TRACE_SMP +#ifdef TRACE_SMP +# define TRACE(x) dprintf x +#else +# define TRACE(x) ; +#endif + + +extern "C" void execute_n_instructions(int count); + +extern "C" void smp_trampoline(void); +extern "C" void smp_trampoline_args(void); +extern "C" void smp_trampoline_end(void); + +struct gdtr { + uint16 limit; + uint32 base; + unsigned char null[8]; + unsigned char code[8]; + unsigned char data[8]; +} __attribute__((packed)); + +// Arguments passed to the SMP trampoline. +struct trampoline_args { + uint32 trampoline; // Trampoline address + uint32 gdt32; // 32-bit GDTR + uint32 pml4; // 64-bit PML4 + uint32 gdt64; // 64-bit GDTR + uint64 kernel_entry; // Kernel entry point + uint64 kernel_args; // Kernel arguments + uint64 current_cpu; // CPU number + uint64 stack_top; // Kernel stack + volatile uint64 sentinel; // Sentinel, AP sets to 0 when finished + + // smp_boot_other_cpus puts the GDTR here. + struct gdtr gdtr; +}; + + +static uint32 +apic_read(uint32 offset) +{ + return *(volatile uint32 *)((addr_t)gKernelArgs.arch_args.apic_phys + offset); +} + + +static void +apic_write(uint32 offset, uint32 data) +{ + *(volatile uint32 *)((addr_t)gKernelArgs.arch_args.apic_phys + offset) = data; +} + + +static status_t +smp_do_acpi_config(void) +{ + TRACE(("smp: using ACPI to detect MP configuration\n")); + + // reset CPU count + gKernelArgs.num_cpus = 0; + + acpi_madt *madt = (acpi_madt *)acpi_find_table(ACPI_MADT_SIGNATURE); + + if (madt == NULL) { + TRACE(("smp: Failed to find MADT!\n")); + return B_ERROR; + } + + gKernelArgs.arch_args.apic_phys = madt->local_apic_address; + TRACE(("smp: local apic address is 0x%" B_PRIx32 "\n", madt->local_apic_address)); + + acpi_apic *apic = (acpi_apic *)((uint8 *)madt + sizeof(acpi_madt)); + acpi_apic *end = (acpi_apic *)((uint8 *)madt + madt->header.length); + while (apic < end) { + switch (apic->type) { + case ACPI_MADT_LOCAL_APIC: + { + if (gKernelArgs.num_cpus == SMP_MAX_CPUS) { + TRACE(("smp: already reached maximum CPUs (%d)\n", + SMP_MAX_CPUS)); + break; + } + + acpi_local_apic *localApic = (acpi_local_apic *)apic; + TRACE(("smp: found local APIC with id %u\n", + localApic->apic_id)); + if ((localApic->flags & ACPI_LOCAL_APIC_ENABLED) == 0) { + TRACE(("smp: APIC is disabled and will not be used\n")); + break; + } + + gKernelArgs.arch_args.cpu_apic_id[gKernelArgs.num_cpus] + = localApic->apic_id; + // TODO: how to find out? putting 0x10 in to indicate a local apic + gKernelArgs.arch_args.cpu_apic_version[gKernelArgs.num_cpus] + = 0x10; + gKernelArgs.num_cpus++; + break; + } + + case ACPI_MADT_IO_APIC: { + acpi_io_apic *ioApic = (acpi_io_apic *)apic; + TRACE(("smp: found io APIC with id %" B_PRIu32 " and address 0x%" B_PRIx32 "\n", + ioApic->io_apic_id, ioApic->io_apic_address)); + if (gKernelArgs.arch_args.ioapic_phys == 0) + gKernelArgs.arch_args.ioapic_phys = ioApic->io_apic_address; + break; + } + default: + break; + } + + apic = (acpi_apic *)((uint8 *)apic + apic->length); + } + + return gKernelArgs.num_cpus > 0 ? B_OK : B_ERROR; +} + + +static void +calculate_apic_timer_conversion_factor(void) +{ + int64 t1, t2; + uint32 config; + uint32 count; + + TRACE(("calculating apic timer conversion factor\n")); + + // setup the timer + config = apic_read(APIC_LVT_TIMER); + config = (config & APIC_LVT_TIMER_MASK) + APIC_LVT_MASKED; + // timer masked, vector 0 + apic_write(APIC_LVT_TIMER, config); + + config = (apic_read(APIC_TIMER_DIVIDE_CONFIG) & ~0x0000000f); + apic_write(APIC_TIMER_DIVIDE_CONFIG, config | APIC_TIMER_DIVIDE_CONFIG_1); + // divide clock by one + + t1 = system_time(); + apic_write(APIC_INITIAL_TIMER_COUNT, 0xffffffff); // start the counter + + execute_n_instructions(128 * 20000); + + count = apic_read(APIC_CURRENT_TIMER_COUNT); + t2 = system_time(); + + count = 0xffffffff - count; + + gKernelArgs.arch_args.apic_time_cv_factor + = (uint32)((1000000.0/(t2 - t1)) * count); + + TRACE(("APIC ticks/sec = %" B_PRId32 "\n", + gKernelArgs.arch_args.apic_time_cv_factor)); +} + + +// #pragma mark - + + +int +smp_get_current_cpu(void) +{ + if (gKernelArgs.arch_args.apic == NULL) + return 0; + + uint8 apicID = apic_read(APIC_ID) >> 24; + for (uint32 i = 0; i < gKernelArgs.num_cpus; i++) { + if (gKernelArgs.arch_args.cpu_apic_id[i] == apicID) + return i; + } + + return 0; +} + + +void +smp_init_other_cpus(void) +{ + if (get_safemode_boolean(B_SAFEMODE_DISABLE_SMP, false)) { + // SMP has been disabled! + TRACE(("smp disabled per safemode setting\n")); + gKernelArgs.num_cpus = 1; + } + + if (get_safemode_boolean(B_SAFEMODE_DISABLE_APIC, false)) { + TRACE(("local apic disabled per safemode setting, disabling smp\n")); + gKernelArgs.arch_args.apic_phys = 0; + gKernelArgs.num_cpus = 1; + } + + if (gKernelArgs.arch_args.apic_phys == 0) + return; + + TRACE(("smp: found %" B_PRId32 " cpu%s\n", gKernelArgs.num_cpus, + gKernelArgs.num_cpus != 1 ? "s" : "")); + TRACE(("smp: apic_phys = %lx\n", (addr_t)gKernelArgs.arch_args.apic_phys)); + TRACE(("smp: ioapic_phys = %lx\n", + (addr_t)gKernelArgs.arch_args.ioapic_phys)); + + // map in the apic + gKernelArgs.arch_args.apic = (void *)mmu_map_physical_memory( + gKernelArgs.arch_args.apic_phys, B_PAGE_SIZE, kDefaultPageFlags); + + TRACE(("smp: apic (mapped) = %lx\n", (addr_t)gKernelArgs.arch_args.apic.Pointer())); + + // calculate how fast the apic timer is + calculate_apic_timer_conversion_factor(); + + if (gKernelArgs.num_cpus < 2) + return; + + for (uint32 i = 1; i < gKernelArgs.num_cpus; i++) { + // create a final stack the trampoline code will put the ap processor on + void * stack = NULL; + const size_t size = KERNEL_STACK_SIZE + KERNEL_STACK_GUARD_PAGES * B_PAGE_SIZE; + if (platform_allocate_region(&stack, size, 0, false) != B_OK) { + panic("Unable to allocate AP stack"); + } + memset(stack, 0, size); + gKernelArgs.cpu_kstack[i].start = fix_address((uint64_t)stack); + gKernelArgs.cpu_kstack[i].size = size; + } +} + + +void +smp_boot_other_cpus(uint32 pml4, uint32 gdtr64, uint64 kernel_entry) +{ + if (gKernelArgs.num_cpus < 2) + return; + + TRACE(("trampolining other cpus\n")); + + // allocate a stack and a code area for the smp trampoline + // (these have to be < 1M physical, 0xa0000-0xfffff is reserved by the BIOS) + uint64 trampolineCode = 0x9000; + uint64 trampolineStack = 0x8000; + + // copy the trampoline code over + TRACE(("copying the trampoline code to %p from %p\n", (char*)trampolineCode, (const void*)&smp_trampoline)); + TRACE(("size of trampoline code = %lu bytes\n", (uint64)&smp_trampoline_end - (uint64)&smp_trampoline)); + memcpy((char *)trampolineCode, (const void*)&smp_trampoline, + (uint64)&smp_trampoline_end - (uint64)&smp_trampoline); + + // boot the cpus + TRACE(("we have %d CPUs to boot...\n", gKernelArgs.num_cpus - 1)); + for (uint32 i = 1; i < gKernelArgs.num_cpus; i++) { + TRACE(("trampolining CPU %d\n", i)); + uint32 config; + uint64 numStartups; + uint32 j; + trampoline_args * args = (trampoline_args *)trampolineStack; + args->trampoline = trampolineCode; + args->gdt32 = (uint64) &args->gdtr; + args->gdtr.limit = 23; + args->gdtr.base = (uint32)(uint64)args->gdtr.null; + #define COPY_ARRAY(A, X0, X1, X2, X3, X4, X5, X6, X7) \ + { A[0] = X0; A[1] = X1; A[2] = X2; A[3] = X3; A[4] = X4; A[5] = X5; A[6] = X6; A[7] = X7; } + COPY_ARRAY(args->gdtr.null, 0, 0, 0, 0, 0, 0, 0, 0); + COPY_ARRAY(args->gdtr.code, 0xff, 0xff, 0, 0, 0, 0x9a, 0xcf, 0); + COPY_ARRAY(args->gdtr.data, 0xff, 0xff, 0, 0, 0, 0x92, 0xcf, 0); + #undef COPY_ARRAY + args->pml4 = pml4; + args->gdt64 = gdtr64; + args->kernel_entry = kernel_entry; + args->kernel_args = (uint64)&gKernelArgs; + args->current_cpu = i; + args->stack_top = gKernelArgs.cpu_kstack[i].start + gKernelArgs.cpu_kstack[i].size; + args->sentinel = 1; + + // put the args in the right place + uint32 * args_ptr = + (uint32 *)(trampolineCode + (uint64)smp_trampoline_args - (uint64)smp_trampoline); + *args_ptr = (uint32)(uint64)args; + + /* clear apic errors */ + if (gKernelArgs.arch_args.cpu_apic_version[i] & 0xf0) { + apic_write(APIC_ERROR_STATUS, 0); + apic_read(APIC_ERROR_STATUS); + } + + /* send (aka assert) INIT IPI */ + config = (apic_read(APIC_INTR_COMMAND_2) & APIC_INTR_COMMAND_2_MASK) + | (gKernelArgs.arch_args.cpu_apic_id[i] << 24); + apic_write(APIC_INTR_COMMAND_2, config); /* set target pe */ + config = (apic_read(APIC_INTR_COMMAND_1) & 0xfff00000) + | APIC_TRIGGER_MODE_LEVEL | APIC_INTR_COMMAND_1_ASSERT + | APIC_DELIVERY_MODE_INIT; + apic_write(APIC_INTR_COMMAND_1, config); + + // wait for pending to end + while ((apic_read(APIC_INTR_COMMAND_1) & APIC_DELIVERY_STATUS) != 0) + asm volatile ("pause;"); + + /* deassert INIT */ + config = (apic_read(APIC_INTR_COMMAND_2) & APIC_INTR_COMMAND_2_MASK) + | (gKernelArgs.arch_args.cpu_apic_id[i] << 24); + apic_write(APIC_INTR_COMMAND_2, config); + config = (apic_read(APIC_INTR_COMMAND_1) & 0xfff00000) + | APIC_TRIGGER_MODE_LEVEL | APIC_DELIVERY_MODE_INIT; + apic_write(APIC_INTR_COMMAND_1, config); + + // wait for pending to end + while ((apic_read(APIC_INTR_COMMAND_1) & APIC_DELIVERY_STATUS) != 0) + asm volatile ("pause;"); + + /* wait 10ms */ + spin(10000); + /* is this a local apic or an 82489dx ? */ + numStartups = (gKernelArgs.arch_args.cpu_apic_version[i] & 0xf0) + ? 2 : 0; + for (j = 0; j < numStartups; j++) { + /* it's a local apic, so send STARTUP IPIs */ + apic_write(APIC_ERROR_STATUS, 0); + + /* set target pe */ + config = (apic_read(APIC_INTR_COMMAND_2) & APIC_INTR_COMMAND_2_MASK) + | (gKernelArgs.arch_args.cpu_apic_id[i] << 24); + apic_write(APIC_INTR_COMMAND_2, config); + + /* send the IPI */ + config = (apic_read(APIC_INTR_COMMAND_1) & 0xfff0f800) + | APIC_DELIVERY_MODE_STARTUP | (trampolineCode >> 12); + apic_write(APIC_INTR_COMMAND_1, config); + + /* wait */ + spin(200); + + while ((apic_read(APIC_INTR_COMMAND_1) & APIC_DELIVERY_STATUS) != 0) + asm volatile ("pause;"); + } + + // Wait for the trampoline code to clear the final stack location. + // This serves as a notification for us that it has loaded the address + // and it is safe for us to overwrite it to trampoline the next CPU. + while (args->sentinel != 0) + spin(1000); + } + + TRACE(("done trampolining\n")); +} + + +void +smp_add_safemode_menus(Menu *menu) +{ + MenuItem *item; + + if (gKernelArgs.arch_args.ioapic_phys != 0) { + menu->AddItem(item = new(nothrow) MenuItem("Disable IO-APIC")); + item->SetType(MENU_ITEM_MARKABLE); + item->SetData(B_SAFEMODE_DISABLE_IOAPIC); + item->SetHelpText("Disables using the IO APIC for interrupt routing, " + "forcing the use of the legacy PIC instead."); + } + + if (gKernelArgs.arch_args.apic_phys != 0) { + menu->AddItem(item = new(nothrow) MenuItem("Disable local APIC")); + item->SetType(MENU_ITEM_MARKABLE); + item->SetData(B_SAFEMODE_DISABLE_APIC); + item->SetHelpText("Disables using the local APIC, also disables SMP."); + + cpuid_info info; + if (get_current_cpuid(&info, 1, 0) == B_OK + && (info.regs.ecx & IA32_FEATURE_EXT_X2APIC) != 0) { +#if 0 + menu->AddItem(item = new(nothrow) MenuItem("Disable X2APIC")); + item->SetType(MENU_ITEM_MARKABLE); + item->SetData(B_SAFEMODE_DISABLE_X2APIC); + item->SetHelpText("Disables using X2APIC."); +#else + menu->AddItem(item = new(nothrow) MenuItem("Enable X2APIC")); + item->SetType(MENU_ITEM_MARKABLE); + item->SetData(B_SAFEMODE_ENABLE_X2APIC); + item->SetHelpText("Enables using X2APIC."); +#endif + } + } + + if (gKernelArgs.num_cpus < 2) + return; + + item = new(nothrow) MenuItem("Disable SMP"); + menu->AddItem(item); + item->SetData(B_SAFEMODE_DISABLE_SMP); + item->SetType(MENU_ITEM_MARKABLE); + item->SetHelpText("Disables all but one CPU core."); +} + + +void +smp_init(void) +{ +#if NO_SMP + gKernelArgs.num_cpus = 1; + return; +#endif + + cpuid_info info; + if (get_current_cpuid(&info, 1, 0) != B_OK) + return; + + if ((info.eax_1.features & IA32_FEATURE_APIC) == 0) { + // Local APICs aren't present; As they form the basis for all inter CPU + // communication and therefore SMP, we don't need to go any further. + TRACE(("no local APIC present, not attempting SMP init\n")); + return; + } + + // first try to find ACPI tables to get MP configuration as it handles + // physical as well as logical MP configurations as in multiple cpus, + // multiple cores or hyper threading. + if (smp_do_acpi_config() == B_OK) { + TRACE(("smp init success\n")); + return; + } + + // Everything failed or we are not running an SMP system, reset anything + // that might have been set through an incomplete configuration attempt. + gKernelArgs.arch_args.apic_phys = 0; + gKernelArgs.arch_args.ioapic_phys = 0; + gKernelArgs.num_cpus = 1; +} diff --git a/src/system/boot/platform/efi/smp.h b/src/system/boot/platform/efi/smp.h new file mode 100644 index 0000000000..b2eda62368 --- /dev/null +++ b/src/system/boot/platform/efi/smp.h @@ -0,0 +1,30 @@ +/* + * Copyright 2005, Axel Dörfler, axeld@pinc-software.de. All rights reserved. + * Distributed under the terms of the MIT License. + */ +#ifndef SMP_H +#define SMP_H + + +#include + + +#ifdef __cplusplus +// this is only available in C++ +# include +extern void smp_add_safemode_menus(Menu *menu); + +extern "C" { +#endif + +extern void smp_init(void); +extern void smp_init_other_cpus(void); +extern void smp_boot_other_cpus(uint32 pml4, uint32 gdt64, uint64 kernel_entry); + +extern int smp_get_current_cpu(void); + +#ifdef __cplusplus +} +#endif + +#endif /* SMP_H */ diff --git a/src/system/boot/platform/efi/smp_trampoline.S b/src/system/boot/platform/efi/smp_trampoline.S new file mode 100644 index 0000000000..48cebf493f --- /dev/null +++ b/src/system/boot/platform/efi/smp_trampoline.S @@ -0,0 +1,129 @@ +/* +** Copyright 2001, Travis Geiselbrecht. All rights reserved. +** Copyright 2012, Alex Smith, alex@alex-smith.me.uk. +** Distributed under the terms of the NewOS License. +*/ + +// Relocatable. Before calling, smp_trampoline_args should point +// to a struct trampoline_args (see smp.cpp). This pointer should +// be 16-byte aligned, below 1MB and identity-mapped. +.globl smp_trampoline +.globl smp_trampoline_end +.globl smp_trampoline_args + +#include + +#include +#include "mmu.h" + +.code16 +smp_trampoline: + cli + +// movl 0xdeadbeef, esi + .byte 0x66 + .byte 0xbe +smp_trampoline_args: + .long 0xdeadbeef + + // Load the trampoline args into ss. + movl %esi, %eax + shrl $4, %eax + movw %ax, %ss + xorw %sp, %sp + + // Switch to protected mode. + popl %ebx + popl %edx + lgdt (%edx) + + movl %cr0,%eax + orl $0x01,%eax + movl %eax,%cr0 + + pushl $8 + leal (trampoline_32 - smp_trampoline)(%ebx), %eax + pushl %eax + .byte 0x66 +.code32 + retf + +trampoline_32: + mov $0x10, %ax + mov %ax, %ds + mov %ax, %es + mov %ax, %fs + mov %ax, %gs + mov %ax, %ss + // Put the trampoline args on the stack. + movl %esi, %esp + addl $8, %esp + + // Enable PAE and PGE + movl %cr4, %eax + orl $(1 << 5) | (1 << 7), %eax + movl %eax, %cr4 + + // Point CR3 to the kernel's PML4. + popl %eax + movl %eax, %cr3 + + // Enable long mode by setting EFER.LME. + movl $0xc0000080, %ecx + rdmsr + orl $(1 << 8), %eax + wrmsr + + // Re-enable paging, which will put us in compatibility mode as we are + // currently in a 32-bit code segment. + movl %cr0, %ecx + orl $(1 << 31), %ecx + movl %ecx, %cr0 + + // Load 64-bit enabled GDT + popl %eax + lgdtl (%eax) + + // Jump into the 64-bit code segment. + pushl $KERNEL_CODE_SELECTOR + leal (.Llmode - smp_trampoline)(%ebx), %eax + pushl %eax + retf +.align 8 +.code64 +.Llmode: + // Set data segments. + mov $KERNEL_DATA_SELECTOR, %ax + mov %ax, %ss + xor %ax, %ax + mov %ax, %ds + mov %ax, %es + mov %ax, %fs + mov %ax, %gs + + // Initialisation that comes from long_smp_start_kernel. + movq %cr0, %rax + orq $0x10000, %rax + andq $(~6), %rax + movq %rax, %cr0 + fninit + movq %cr4, %rax + orq $0x600, %rax + movq %rax, %cr4 + + // Get kernel arguments. + popq %rax + popq %rdi + popq %rsi + + // Set the stack pointer, write to the sentinel and clear the stack frame/RFLAGS. + popq %rbp + movq $0, (%rsp) + movq %rbp, %rsp + xorq %rbp, %rbp + push $0 + popf + + // Call the entry point. + call *%rax +smp_trampoline_end: diff --git a/src/system/boot/platform/efi/start.cpp b/src/system/boot/platform/efi/start.cpp index 02a5920857..1c035c713c 100644 --- a/src/system/boot/platform/efi/start.cpp +++ b/src/system/boot/platform/efi/start.cpp @@ -25,6 +25,7 @@ #include "hpet.h" #include "cpu.h" #include "mmu.h" +#include "smp.h" extern void (*__ctor_list)(void); @@ -40,6 +41,7 @@ EFI_HANDLE kImage; static uint32 sBootOptions; static uint64 gLongKernelEntry; extern uint64 gLongGDT; +extern uint64 gLongGDTR; segment_descriptor gBootGDT[BOOT_GDT_SEGMENT_COUNT]; @@ -147,6 +149,8 @@ platform_start_kernel(void) cpu_init(); acpi_init(); hpet_init(); + smp_init(); + smp_init_other_cpus(); preloaded_elf64_image *image = static_cast( gKernelArgs.kernel_image.Pointer()); @@ -235,6 +239,8 @@ platform_start_kernel(void) // Update EFI, generate final kernel physical memory map, etc. mmu_post_efi_setup(memory_map_size, memory_map, descriptor_size, descriptor_version); + smp_boot_other_cpus(final_pml4, (uint32_t)(uint64_t)&gLongGDTR, gLongKernelEntry); + // Enter the kernel! efi_enter_kernel(final_pml4, gLongKernelEntry,