x86_64: acpi: add minimal SMP core startup

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;
 }

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

arch/x86_64/acpi/smp.c

@@ -0,0 +1,123 @@
+#include <arch/acpi.h>
+#include <socks/printk.h>
+#include <socks/vm.h>
+
+#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;
+}

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))

arch/x86_64/include/arch/acpi.h

@@ -5,8 +5,13 @@
 #include <socks/status.h>
 #include <stdint.h>
 
+#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);