SMP WORKING

6 files changed, 129 insertions, 22 deletions

diff --git a/kernel/Makefile b/kernel/Makefile
index 266eea8..8e9e158 100644
--- a/kernel/Makefile
+++ b/kernel/Makefile
@@ -34,6 +34,8 @@ OBJS = \
 	src/mem/pmm.o \
 	src/misc/graphics.o \
 	src/misc/stdbuff.o \
+	src/scheduler/ap_init.o \
+	src/scheduler/ap_startup.o \
 	src/scheduler/atomic.o \
 	src/scheduler/scheduler.o \
 	src/sys/syscall_asm.o \
diff --git a/kernel/src/apic/madt.c b/kernel/src/apic/madt.c
index d133f2d..6d701b7 100644
--- a/kernel/src/apic/madt.c
+++ b/kernel/src/apic/madt.c
@@ -4,6 +4,7 @@
 #include <libk/string.h>
 #include <libk/stdio.h>
 #include <apic.h>
+#include <timer.h>
 
 uint8_t curr_cpu_apic_id(void);
 uint8_t curr_cpu_apic_id()
@@ -14,7 +15,7 @@ uint8_t curr_cpu_apic_id()
 }
 
 uint32_t ioapic_addr = 0;
-uint32_t lapic_addr = 0;
+uint64_t lapic_addr = 0;
 uint32_t numcores = 0;
 uint8_t cpu_apic_ids[256];
 
@@ -147,6 +148,11 @@ void parse_madt()
 
 
 	uint8_t bspid = curr_cpu_apic_id();
+	uint8_t* bspdone = (uint8_t*)0x3000100;
+	uint8_t* aprunning = (uint8_t*)0x3000200;
+	*bspdone = 0;
+	*aprunning = 0;
+
 
 	map_addr(lapic_addr, lapic_addr, FLAG_PRESENT);
 	map_addr(ioapic_addr, ioapic_addr, FLAG_PRESENT);
@@ -155,27 +161,47 @@ void parse_madt()
 	ioapic_set_irq(0x1, bspid, 0x21); // keyboard
 	__asm__ volatile ("sti;");
 
-	/*
 	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;
+		if(cpu_apic_ids[i] == bspid)
+			continue;
+
+		printf("initializing cpu with apic id 0x%x\n", cpu_apic_ids[i]);
 		// send INIT IPI
-		*((volatile uint32_t*)(lapic_addr + 0x280)) = 0;                                                                             // clear APIC errors
-		*((volatile uint32_t*)(lapic_addr + 0x310)) = (*((volatile uint32_t*)(lapic_addr + 0x310)) & 0x00ffffff) | (i << 24);         // select AP
-		*((volatile uint32_t*)(lapic_addr + 0x300)) = (*((volatile uint32_t*)(lapic_addr + 0x300)) & 0xfff00000) | 0x00C500;          // trigger INIT IPI
-		do { __asm__ __volatile__ ("pause" : : : "memory"); }while(*((volatile uint32_t*)(lapic_addr + 0x300)) & (1 << 12));         // wait for delivery
-		*((volatile uint32_t*)(lapic_addr + 0x310)) = (*((volatile uint32_t*)(lapic_addr + 0x310)) & 0x00ffffff) | (i << 24);         // select AP
-		*((volatile uint32_t*)(lapic_addr + 0x300)) = (*((volatile uint32_t*)(lapic_addr + 0x300)) & 0xfff00000) | 0x008500;          // deassert
-		do { __asm__ __volatile__ ("pause" : : : "memory"); }while(*((volatile uint32_t*)(lapic_addr + 0x300)) & (1 << 12));         // wait for delivery
-		mdelay(10);                                                                                                                 // wait 10 msec
+
+		// 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++) {
-			*((volatile uint32_t*)(lapic_addr + 0x280)) = 0;                                                                     // clear APIC errors
-			*((volatile uint32_t*)(lapic_addr + 0x310)) = (*((volatile uint32_t*)(lapic_addr + 0x310)) & 0x00ffffff) | (i << 24); // select AP
-			*((volatile uint32_t*)(lapic_addr + 0x300)) = (*((volatile uint32_t*)(lapic_addr + 0x300)) & 0xfff0f800) | 0x000608;  // trigger STARTUP IPI for 0800:0000
-			udelay(200);                                                                                                        // wait 200 usec
-			do { __asm__ __volatile__ ("pause" : : : "memory"); }while(*((volatile uint32_t*)(lapic_addr + 0x300)) & (1 << 12)); // wait for delivery
+			// 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(1);
+	printf("aprunning: %d\n", *aprunning);
 }
diff --git a/kernel/src/kernel.ld b/kernel/src/kernel.ld
index f0247b2..33aa87d 100644
--- a/kernel/src/kernel.ld
+++ b/kernel/src/kernel.ld
@@ -1,10 +1,18 @@
 ENTRY(_start)
 
+KERNEL_AP_PMA = 0x0000000000008000;
 KERNEL_PMA = 0x0000000000100000;
 KERNEL_VMA = 0xffffffff80000000;
 
 SECTIONS
 {
+	. = KERNEL_AP_PMA;
+
+	.apinit BLOCK(4K) : ALIGN(4K)
+	{
+		*(.apinit)
+	}
+
 	. = KERNEL_PMA;
 
 	.boot BLOCK(4K) : ALIGN(4K)
diff --git a/kernel/src/main.c b/kernel/src/main.c
index 6a98068..5061515 100644
--- a/kernel/src/main.c
+++ b/kernel/src/main.c
@@ -43,11 +43,6 @@ int kernel_main(mb2_tag_header* multiboot_bootinfo, uint32_t multiboot_magic)
 	init_tss();
 	list_sys_tables();
 	parse_madt();
-
-	printf("before wait\n");
-	wait(10000);
-	printf("\nafter wait\n");
-
 	jump_userspace();
 
 	for(;;) {
diff --git a/kernel/src/scheduler/ap_init.S b/kernel/src/scheduler/ap_init.S
new file mode 100644
index 0000000..8cb4314
--- /dev/null
+++ b/kernel/src/scheduler/ap_init.S
@@ -0,0 +1,71 @@
+.section .apinit, "a"
+
+
+.SET stack_top, 0x3008000
+.SET stack_off, 0x8000
+.SET bspdone, 0x3000100
+.SET aprunning, 0x3000200
+
+# this code will be relocated to 0x8000, sets up environment for calling a C function
+.code16
+ap_trampoline:
+	cli
+	cld
+	ljmp $0, $0x8040
+.align 16
+_L8010_GDT_table:
+	.long 0, 0
+	.long 0x0000FFFF, 0x00CF9A00 # flat code
+	.long 0x0000FFFF, 0x008F9200 # flat data
+	.long 0x00000068, 0x00CF8900 # tss
+_L8030_GDT_value:
+	.word _L8030_GDT_value - _L8010_GDT_table - 1
+	.long 0x8010
+	.long 0, 0
+	.align 64
+_L8040:
+	xorw %ax, %ax
+	movw %ax, %ds
+	lgdtl 0x8030
+	movl %cr0, %eax
+	orl $1, %eax
+	movl %eax, %cr0
+	ljmp $8, $0x8060
+	.align 32
+	.code32
+_L8060:
+	movw $16, %ax
+	movw %ax, %ds
+	movw %ax, %ss
+	# get our Local APIC ID
+	mov $1, %eax
+	cpuid
+	shrl $24, %ebx
+	movl %ebx, %edi
+
+
+	# set up 32k stack, one for each core. It is important that all core must have its own stack
+	movl $stack_top, %esp
+	mov %edi, %ecx
+2:
+	addl $stack_off, %esp
+	decl %ecx
+	cmp $0, %ecx
+	jne 2b
+
+	shll $15, %ebx
+	subl %ebx, %esp
+	pushl %edi
+	# spinlock, wait for the BSP to finish
+1:
+	pause
+	cmpb $0, bspdone
+	jz 1b
+	lock incb aprunning
+
+3:
+	hlt
+	jmp 3b
+
+	# jump into C code (should never return)
+	# ljmp $8, $ap_startup
diff --git a/kernel/src/scheduler/ap_startup.c b/kernel/src/scheduler/ap_startup.c
new file mode 100644
index 0000000..43b1a51
--- /dev/null
+++ b/kernel/src/scheduler/ap_startup.c
@@ -0,0 +1,5 @@
+// this C code can be anywhere you want it, no relocation needed
+void ap_startup(int apicid) {
+	// do what you want to do on the AP
+	while(1);
+}