From 81ca98c95277273dd82cc648fb66409b120509fe Mon Sep 17 00:00:00 2001 From: Max Wash Date: Thu, 9 Feb 2023 21:39:20 +0000 Subject: [PATCH] x86_64: acpi: add minimal SMP core startup --- arch/x86_64/acpi/acpi.c | 7 +- arch/x86_64/acpi/ap_trampoline.S | 28 +++++++ arch/x86_64/acpi/smp.c | 123 +++++++++++++++++++++++++++++++ arch/x86_64/config.mk | 2 +- arch/x86_64/include/arch/acpi.h | 12 +++ 5 files changed, 169 insertions(+), 3 deletions(-) create mode 100644 arch/x86_64/acpi/ap_trampoline.S create mode 100644 arch/x86_64/acpi/smp.c diff --git a/arch/x86_64/acpi/acpi.c b/arch/x86_64/acpi/acpi.c index 948e4a2..47e149f 100644 --- a/arch/x86_64/acpi/acpi.c +++ b/arch/x86_64/acpi/acpi.c @@ -3,7 +3,10 @@ kern_status_t acpi_init(void) { - struct acpi_sdt *fadt = acpi_find_sdt(ACPI_SIG_FADT); - printk("acpi: FADT=%p", fadt); + kern_status_t status = acpi_smp_init(); + if (status != KERN_OK) { + return status; + } + return KERN_OK; } diff --git a/arch/x86_64/acpi/ap_trampoline.S b/arch/x86_64/acpi/ap_trampoline.S new file mode 100644 index 0000000..e24ca8d --- /dev/null +++ b/arch/x86_64/acpi/ap_trampoline.S @@ -0,0 +1,28 @@ + + .global acpi_bsp_lapic_id + .type acpi_bsp_lapic_id, @function + +acpi_bsp_lapic_id: + push %rbp + mov %rsp, %rbp + + push %rbx + + mov $0x00000001, %eax + cpuid + shrq $0x18, %rbx + + mov %rbx, %rax + + pop %rbx + pop %rbp + ret + + .global ap_trampoline + .type ap_trampoline, @function + .code16 + +ap_trampoline: + mov $0xFF, %ax + cli + hlt diff --git a/arch/x86_64/acpi/smp.c b/arch/x86_64/acpi/smp.c new file mode 100644 index 0000000..defc3a7 --- /dev/null +++ b/arch/x86_64/acpi/smp.c @@ -0,0 +1,123 @@ +#include +#include +#include + +#define AP_TRAMPOLINE_PADDR 0x8000 +#define LAPIC_IPI_STATUS_REG 0x0280 +#define LAPIC_IPI_DEST_REG 0x0310 +#define LAPIC_IPI_ICR_REG 0x0300 + +struct acpi_madt_record { + uint8_t r_type; + uint8_t r_length; +} __packed; + +struct lapic_record { + uint8_t l_cpu_id; + uint8_t l_apic_id; + uint32_t l_flags; +} __packed; + +struct lapic_override_record { + uint16_t l_reserved; + uint64_t l_lapic_ptr; +} __packed; + +extern uint8_t acpi_bsp_lapic_id(void); +extern char ap_trampoline[]; + +static int send_ipi(void *lapic, unsigned int target_id, uint32_t payload) +{ + uintptr_t lapic_ptr = (uintptr_t)lapic; + + *((volatile uint32_t*)(lapic_ptr + LAPIC_IPI_STATUS_REG)) = 0; + *((volatile uint32_t*)(lapic_ptr + LAPIC_IPI_DEST_REG)) = (*((volatile uint32_t*)(lapic_ptr + LAPIC_IPI_DEST_REG)) & 0x00ffffff) | (target_id << 24); + *((volatile uint32_t*)(lapic_ptr + LAPIC_IPI_ICR_REG)) = (*((volatile uint32_t*)(lapic_ptr + LAPIC_IPI_ICR_REG)) & 0xfff00000) | payload; + do { __asm__ __volatile__ ("pause" : : : "memory"); }while(*((volatile uint32_t*)(lapic_ptr + LAPIC_IPI_ICR_REG)) & (1 << 12)); + + return 0; +} + +static int init_ap(struct acpi_madt_record *rec, void *bsp_lapic, uint8_t bsp_id) +{ + struct lapic_record *lapic = (struct lapic_record *)(rec + 1); + if (!(lapic->l_flags & 0x1) && !(lapic->l_flags & 0x2)) { + return -1; + } + + if (lapic->l_apic_id == bsp_id) { + printk("acpi: core %u online [BSP]", lapic->l_apic_id); + return 0; + } + + /* send init IPI */ + send_ipi(bsp_lapic, lapic-> l_apic_id, 0xC500); + send_ipi(bsp_lapic, lapic-> l_apic_id, 0x8500); + /* send startup IPI */ + send_ipi(bsp_lapic, lapic-> l_apic_id, 0x4600 | (AP_TRAMPOLINE_PADDR >> VM_PAGE_SHIFT)); + + printk("acpi: core %u online [AP]", lapic->l_apic_id); + return 0; +} + +static void *find_lapic(struct acpi_madt *madt) +{ + phys_addr_t local_apic = madt->m_lapic_ptr; + + unsigned char *p = (unsigned char *)madt + sizeof *madt; + unsigned char *madt_end = (unsigned char *)madt + madt->m_base.s_length; + + while (p < madt_end) { + struct acpi_madt_record *rec = (struct acpi_madt_record *)p; + struct lapic_override_record *lapic; + + switch (rec->r_type) { + case ACPI_MADT_LAPIC_OVERRIDE: + lapic = (struct lapic_override_record *)(rec + 1); + return vm_phys_to_virt(lapic->l_lapic_ptr); + default: + break; + } + + p += rec->r_length; + } + + return vm_phys_to_virt(local_apic); +} + +kern_status_t acpi_smp_init(void) +{ + struct acpi_madt *madt = (struct acpi_madt *)acpi_find_sdt(ACPI_SIG_MADT); + if (!madt) { + return KERN_UNSUPPORTED; + } + + uint8_t bsp_id = acpi_bsp_lapic_id(); + void *bsp_lapic = find_lapic(madt); + void *ap_trampoline_dest = vm_phys_to_virt(AP_TRAMPOLINE_PADDR); + memcpy(ap_trampoline_dest, ap_trampoline, VM_PAGE_SIZE); + + unsigned char *p = (unsigned char *)madt + sizeof *madt; + unsigned char *madt_end = (unsigned char *)madt + madt->m_base.s_length; + + unsigned int nr_processors = 0; + + while (p < madt_end) { + struct acpi_madt_record *rec = (struct acpi_madt_record *)p; + + switch (rec->r_type) { + case ACPI_MADT_LAPIC: + if (init_ap(rec, bsp_lapic, bsp_id) == 0) { + nr_processors++; + } + break; + default: + break; + } + + p += rec->r_length; + } + + printk("acpi: initialised %u processors", nr_processors); + return KERN_OK; +} diff --git a/arch/x86_64/config.mk b/arch/x86_64/config.mk index 348b80d..308c1a7 100644 --- a/arch/x86_64/config.mk +++ b/arch/x86_64/config.mk @@ -4,5 +4,5 @@ ARCH_LDFLAGS := -z max-page-size=0x1000 -T arch/x86_64/layout.ld ARCH_DIR := arch/$(ARCH) ARCH_C_FILES := $(wildcard $(ARCH_DIR)/*.c) $(wildcard $(ARCH_DIR)/acpi/*.c) -ARCH_ASM_FILES := $(wildcard $(ARCH_DIR)/*.S) +ARCH_ASM_FILES := $(wildcard $(ARCH_DIR)/*.S) $(wildcard $(ARCH_DIR)/acpi/*.S) ARCH_OBJ := $(addprefix $(BUILD_DIR)/,$(ARCH_C_FILES:.c=.o) $(ARCH_ASM_FILES:.S=.o)) diff --git a/arch/x86_64/include/arch/acpi.h b/arch/x86_64/include/arch/acpi.h index 6da19d5..0e7fa4f 100644 --- a/arch/x86_64/include/arch/acpi.h +++ b/arch/x86_64/include/arch/acpi.h @@ -5,8 +5,13 @@ #include #include +#define ACPI_MADT_LAPIC 0x00 +#define ACPI_MADT_IOAPIC 0x01 +#define ACPI_MADT_LAPIC_OVERRIDE 0x05 + #define ACPI_SIG_RSDP 0x2052545020445352ULL #define ACPI_SIG_FADT 0x50434146 +#define ACPI_SIG_MADT 0x43495041 struct acpi_rsdp_10 { char r_sig[8]; @@ -54,7 +59,14 @@ struct acpi_xsdt { uint64_t x_tables[]; } __packed; +struct acpi_madt { + struct acpi_sdt m_base; + uint32_t m_lapic_ptr; + uint32_t m_flags; +} __packed; + extern kern_status_t acpi_init(void); +extern kern_status_t acpi_smp_init(void); extern struct acpi_sdt *acpi_find_sdt(uint32_t sig);