1 files changed, 79 insertions, 0 deletions
diff --git a/kernel/src/apic/apic.c b/kernel/src/apic/apic.c
index e69de29..b947317 100644
--- a/kernel/src/apic/apic.c
+++ b/kernel/src/apic/apic.c
@@ -0,0 +1,79 @@
+#include <atomic.h>
+#include <paging.h>
+#include <libk/stdio.h>
+#include <apic.h>
+#include <ioapic.h>
+#include <madt.h>
+#include <timer.h>
+
+mutex_t cnt_lock;
+uint32_t cnt = 0;
+uint32_t ioapic_addr = 0;
+uint64_t lapic_addr = 0;
+uint32_t numcores = 0;
+uint8_t cpu_apic_ids[256];
+
+uint8_t curr_cpu_apic_id()
+{
+	uint8_t apic_id = 0;
+	__asm__ __volatile__ ("movl $1, %%eax; cpuid; shrl $24, %%ebx;": "=b"(apic_id));
+	return apic_id;
+}
+
+void init_ap_cpus()
+{
+	uint8_t bspid = curr_cpu_apic_id();
+	uint8_t* bspdone = (uint8_t*)0x3000100;
+	uint8_t* aprunning = (uint8_t*)0x3000200;
+	*bspdone = 0;
+	*aprunning = 0;
+
+	init_mutex(&cnt_lock);
+	map_addr(lapic_addr, lapic_addr, FLAG_PRESENT);
+
+	for(size_t i = 0; i < numcores; i++) {
+		// do not start BSP, that's already running this code
+		if(cpu_apic_ids[i] == bspid)
+			continue;
+
+		printf("initializing cpu with apic id 0x%x\n", cpu_apic_ids[i]);
+		// send INIT IPI
+
+		// clear APIC errors
+		*((volatile uint32_t*)(lapic_addr + 0x280)) = 0;
+		// select AP
+		*((volatile uint32_t*)(lapic_addr + 0x310)) = (*((volatile uint32_t*)(lapic_addr + 0x310)) & 0x00ffffff) | ((uint32_t)cpu_apic_ids[i] << 24);
+		// trigger INIT IPI
+		*((volatile uint32_t*)(lapic_addr + 0x300)) = (*((volatile uint32_t*)(lapic_addr + 0x300)) & 0xfff00000) | 0x00C500;
+		// wait for delivery
+		do { __asm__ __volatile__ ("pause" : : : "memory"); }while(*((volatile uint32_t*)(uint64_t)(lapic_addr + 0x300)) & (1 << 12));
+		// select AP
+		*((volatile uint32_t*)(lapic_addr + 0x310)) = (*((volatile uint32_t*)(lapic_addr + 0x310)) & 0x00ffffff) | ((uint32_t)cpu_apic_ids[i] << 24);
+		// deassert
+		*((volatile uint32_t*)(lapic_addr + 0x300)) = (*((volatile uint32_t*)(lapic_addr + 0x300)) & 0xfff00000) | 0x008500;
+		// wait for delivery
+		do { __asm__ __volatile__ ("pause" : : : "memory"); }while(*((volatile uint32_t*)(uint64_t)(lapic_addr + 0x300)) & (1 << 12));
+		// wait 10 msec
+		wait(10);
+
+		// send STARTUP IPI (twice)
+
+		for(size_t j = 0; j < 2; j++) {
+			// clear APIC errors
+			*((volatile uint32_t*)(lapic_addr + 0x280)) = 0;
+			// select AP
+			*((volatile uint32_t*)(lapic_addr + 0x310)) = (*((volatile uint32_t*)(lapic_addr + 0x310)) & 0x00ffffff) | ((uint32_t)cpu_apic_ids[i] << 24);
+			// trigger STARTUP IPI for 0800:0000
+			*((volatile uint32_t*)(lapic_addr + 0x300)) = (*((volatile uint32_t*)(lapic_addr + 0x300)) & 0xfff0f800) | 0x000608;
+			// wait 200 usec
+			wait(1);
+			// wait for delivery
+			do { __asm__ __volatile__ ("pause" : : : "memory"); }while(*((volatile uint32_t*)(uint64_t)(lapic_addr + 0x300)) & (1 << 12));
+		}
+	}
+
+	*bspdone = 1;
+	wait(100);
+	printf("aprunning: %d\n", *aprunning);
+	printf("cnt: %d\n", cnt);
+}