[SMP] Initialize APs

kernel/cpu/cpu.c

@@ -10,6 +10,7 @@
 
 cpu_t cpus[MAX_NUMCPU];
 unsigned int num_cpu = 0;
+unsigned int all_ap_started = 0;
 
 void cpu_add(uint64_t id, uint64_t apic)
 {
@@ -32,9 +33,6 @@ void cpu_add(uint64_t id, uint64_t apic)
 
 void init_cpu()
 {
-  cpu_t *cpu = get_cpu();
-  debug_info("CPU - Initializing CPU %x\n", cpu->id);
-
   interrupt_init();
   apic_init();
   gdt_init();
@@ -52,18 +50,24 @@ void cpu_init()
   asm("swapgs");
   init_cpu();
 
+  uintptr_t page = pmm_alloc();
   vmm_set_page(0, TRAMPOLINE_ADDR, TRAMPOLINE_ADDR, PAGE_PRESENT | PAGE_WRITE);
+  vmm_set_page(0, TRAMPOLINE_GDT, page, PAGE_PRESENT | PAGE_WRITE);
   memcpy((void *)TRAMPOLINE_ADDR, &trampoline, PAGE_SIZE);
-  vmm_set_page(0, V2P(&BootGDT), V2P(&BootGDT), PAGE_PRESENT);
+  memcpy((void *)TRAMPOLINE_GDT, &trampoline_GDT, PAGE_SIZE);
+
   for(unsigned int i = 0; i < num_cpu; i++)
-  {
     ap_init(&cpus[i]);
-  }
+
+  all_ap_started = 1;
+
   vmm_set_page(0, TRAMPOLINE_ADDR, 0, 0);
+  pmm_free(page);
 
   debug_info("CPU - Status\n");
   for(unsigned int i = 0; i < num_cpu; i++)
   {
     debug("  CPU id:%x lapic:%x\n", cpus[i].id, cpus[i].apic_id);
   }
+
 }

kernel/cpu/smp.c

@@ -45,6 +45,9 @@ void ap_init(cpu_t *cpu)
 
 void ap_start()
 {
-  debug_ok("STARTED CPU:%x\n", get_cpu()->id);
+  init_cpu();
+  while(!all_ap_started);
+  cpu_t *cpu = get_cpu();
+  debug_ok("STARTED CPU:%x\n", cpu->id);
   for(;;)asm("hlt");
 }

kernel/cpu/trampoline.S

@@ -1,6 +1,7 @@
 #include <mem.h>
 #include <cpu.h>
 #include <msr.h>
+#include <gdt.h>
 #define TRAMPOLINE_OFFSET TRAMPOLINE_ADDR - trampoline
 
 .intel_syntax noprefix
@@ -51,7 +52,7 @@ start_32:
   or eax, 1<<31
   mov cr0, eax
 
-  lgdt V2P(BootGDTp)
+  lgdt [trampoline_GDTp + TRAMPOLINE_OFFSET]
 
   mov ax, 0x10
   mov ds, ax
@@ -106,5 +107,14 @@ ap_boot_idt:
 .global ap_gs_base
 ap_gs_base:
   .long 0x00000000, 0x00000000
+
+.global trampoline_GDT
+trampoline_GDT:
+  .quad 0
+  .quad (GDT_PRESENT | GDT_CODEDATA | GDT_WRITE | GDT_EXECUTE | GDT_64BIT)
+  .quad (GDT_PRESENT | GDT_CODEDATA | GDT_WRITE)
+trampoline_GDTp:
+  .short 3*8-1
+  .quad TRAMPOLINE_GDT
 trampoline_end:
   nop

kernel/include/cpu.h

@@ -9,6 +9,7 @@
 #define CPU_FAILED 4
 
 #define TRAMPOLINE_ADDR 0x1000
+#define TRAMPOLINE_GDT 0x2000
 
 #define GS_OFFSET_CPU 0
 #define GS_OFFSET_STATE 40
@@ -39,6 +40,7 @@ typedef struct cpu_t
 
 extern cpu_t cpus[];
 extern unsigned int num_cpu;
+extern unsigned int all_ap_started;
 
 void acpi_init();
 void cpu_add(uint64_t id, uint64_t apic);
@@ -48,6 +50,7 @@ cpu_t *get_cpu();
 void ap_init(cpu_t *cpu);
 
 void trampoline();
+void trampoline_GDT();
 #endif
 
 

kernel/interrupts/interrupts.c

@@ -6,7 +6,7 @@
 #include <thread.h>
 
 struct int_gate_descriptor idt[NUM_INTERRUPTS];
-struct idtr idtr;
+struct idtr idtr = {0,0};
 int_handler_t int_handlers[NUM_INTERRUPTS];
 
 void idt_set(uint32_t num, void *vector, uint16_t cs, uint8_t ist, uint8_t flags)
@@ -21,6 +21,8 @@ void idt_set(uint32_t num, void *vector, uint16_t cs, uint8_t ist, uint8_t flags
 }
 
 void interrupt_init()
+{
+  if(!idtr.addr)
   {
     // Clear IDT and interrupt handler list
     memset(idt, 0, sizeof(idt));
@@ -37,6 +39,12 @@ void interrupt_init()
     // Setup pointer and load IDT
     idtr.addr = (uint64_t)idt;
     idtr.len = sizeof(idt)-1;
+  }
+  load_idt(&idtr);
+}
+
+void interrupt_init_smp()
+{
   load_idt(&idtr);
 }