x86_64: acpi: bring up other APs in long mode

arch/x86_64/acpi/smp.cpp

@@ -0,0 +1,172 @@
+#include <arch/acpi.h>
+#include <arch/acpi/local_apic.hpp>
+#include <socks/printk.h>
+#include <socks/pmap.h>
+#include <socks/vm.h>
+#include <socks/cpu.h>
+
+using namespace arch::acpi;
+
+extern "C" uint8_t acpi_bsp_lapic_id(void);
+extern "C" char ap_trampoline[];
+
+volatile uintptr_t __ap_stack_top = 0;
+volatile uint8_t __this_ap_ok = 0;
+volatile uint8_t __all_ap_ok = 0;
+
+/*
+static int __used 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;
+}
+*/
+
+extern "C" void ap_trampoline_exit(void)
+{
+	printk("OK!");
+	__this_ap_ok = 1;
+}
+
+static int init_ap(struct acpi_madt_record *rec, local_apic& lapic, uint8_t bsp_id)
+{
+	struct lapic_record *ap_lapic = (struct lapic_record *)(rec + 1);
+	if (!(ap_lapic->l_flags & 0x1) && !(ap_lapic->l_flags & 0x2)) {
+		return -1;
+	}
+
+	if (ap_lapic->l_apic_id == bsp_id) {
+		printk("acpi: core %u online [BSP]", ap_lapic->l_apic_id);
+		return 0;
+	}
+
+	struct vm_page *ap_stack = vm_page_alloc(VM_PAGE_4K, VM_NORMAL);
+	__ap_stack_top = (uintptr_t)vm_page_get_vaddr(ap_stack) + VM_PAGE_SIZE;
+
+	lapic.send_ipi(ap_lapic->l_apic_id, 0xC500);
+	lapic.send_ipi(ap_lapic->l_apic_id, 0x8500);
+	milli_sleep(10);                                                                                                                 // wait 10 msec
+	lapic.send_ipi(ap_lapic->l_apic_id, 0x4600 | (AP_TRAMPOLINE_PADDR >> VM_PAGE_SHIFT));
+
+	do {
+		ml_cpu_relax();
+	} while (__this_ap_ok == 0);
+
+	printk("acpi: core %u online [AP]", ap_lapic->l_apic_id);
+	return 0;
+}
+
+kern_status_t bring_all_ap_online(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 *ap_trampoline_dest = vm_phys_to_virt(AP_TRAMPOLINE_PADDR);
+	memcpy(ap_trampoline_dest, ap_trampoline, VM_PAGE_SIZE);
+
+	pmap_t kernel_pmap = get_kernel_pmap();
+	uint32_t *pmap_ptr = (uint32_t *)vm_phys_to_virt(0x8ff8);
+	*pmap_ptr = kernel_pmap;
+
+	pmap_add(kernel_pmap, (void *)0x8000, 8, (enum vm_prot)(VM_PROT_READ | VM_PROT_EXEC), PMAP_NORMAL);
+
+	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, local_apic::get(), bsp_id) == 0) {
+				nr_processors++;
+			}
+			break;
+		default:
+			break;
+		}
+
+		p += rec->r_length;
+	}
+
+	printk("acpi: found %u logical cores", nr_processors);
+	return KERN_OK;
+}
+
+static void no_smp_config(void)
+{
+	cpu_set_available(0);
+	cpu_set_online(0);
+
+	ml_cpu_block *self = ml_this_cpu();
+	self->c_cpu_id = 0;
+}
+
+kern_status_t acpi_scan_cpu_topology(void)
+{
+	struct acpi_madt *madt = (struct acpi_madt *)acpi_find_sdt(ACPI_SIG_MADT);
+	if (!madt) {
+		no_smp_config();
+		return KERN_UNSUPPORTED;
+	}
+
+	uint8_t bsp_id = acpi_bsp_lapic_id();
+
+	//void *bsp_lapic = find_lapic(madt);
+
+	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;
+
+		if (rec->r_type != ACPI_MADT_LAPIC) {
+			p += rec->r_length;
+			continue;
+		}
+
+		struct lapic_record *lapic = (struct lapic_record *)(rec + 1);
+
+		if (!(lapic->l_flags & 0x1) && !(lapic->l_flags & 0x2)) {
+			/* processor cannot be used */
+			continue;
+		}
+
+		cpu_set_available(lapic->l_apic_id);
+		nr_processors++;
+
+		p += rec->r_length;
+	}
+
+	cpu_set_online(bsp_id);
+
+	/* store the BSP ID in the current cpu block */
+	ml_cpu_block *self = ml_this_cpu();
+	self->c_cpu_id = bsp_id;
+
+	printk("acpi: found %u logical cores", nr_processors);
+	return KERN_OK;
+}
+
+kern_status_t smp_init(void)
+{
+	return bring_all_ap_online();
+}