From 6a2bef55170fcef2aaab63ef9c1f92bd014cbccc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Thomas=20Lov=C3=A9n?= <thomasloven@gmail.com>
Date: Wed, 23 Nov 2016 16:16:33 +0100
Subject: [PATCH] [SMP] Boot Application Processors

---
 kernel/cpu/cpu.c        |  17 +++++++
 kernel/cpu/smp.c        |  50 ++++++++++++++++++
 kernel/cpu/trampoline.S | 110 ++++++++++++++++++++++++++++++++++++++++
 kernel/include/cpu.h    |  16 ++++++
 4 files changed, 193 insertions(+)
 create mode 100644 kernel/cpu/smp.c
 create mode 100644 kernel/cpu/trampoline.S

diff --git a/kernel/cpu/cpu.c b/kernel/cpu/cpu.c
index 89b9369..c238e79 100644
--- a/kernel/cpu/cpu.c
+++ b/kernel/cpu/cpu.c
@@ -4,6 +4,9 @@
 #include <msr.h>
 #include <sse.h>
 #include <scheduler.h>
+#include <mem.h>
+#include <gdt.h>
+#include <string.h>
 
 cpu_t cpus[MAX_NUMCPU];
 unsigned int num_cpu = 0;
@@ -19,6 +22,11 @@ void cpu_add(uint64_t id, uint64_t apic)
   cpu->id = id;
   cpu->apic_id = apic;
   cpu->is_bsp = (num_cpu)?0:1;
+
+  cpu->current_thread = 0;
+  cpu->current_state = (num_cpu)?CPU_STOPPED:CPU_STARTED;
+  cpu->kernel_stack = (num_cpu)?kmalloc(PAGE_SIZE) + PAGE_SIZE:0;
+
   num_cpu++;
 }
 
@@ -44,6 +52,15 @@ void cpu_init()
   asm("swapgs");
   init_cpu();
 
+  vmm_set_page(0, TRAMPOLINE_ADDR, TRAMPOLINE_ADDR, PAGE_PRESENT | PAGE_WRITE);
+  memcpy((void *)TRAMPOLINE_ADDR, &trampoline, PAGE_SIZE);
+  vmm_set_page(0, V2P(&BootGDT), V2P(&BootGDT), PAGE_PRESENT);
+  for(unsigned int i = 0; i < num_cpu; i++)
+  {
+    ap_init(&cpus[i]);
+  }
+  vmm_set_page(0, TRAMPOLINE_ADDR, 0, 0);
+
   debug_info("CPU - Status\n");
   for(unsigned int i = 0; i < num_cpu; i++)
   {
diff --git a/kernel/cpu/smp.c b/kernel/cpu/smp.c
new file mode 100644
index 0000000..975daa9
--- /dev/null
+++ b/kernel/cpu/smp.c
@@ -0,0 +1,50 @@
+#include <stdint.h>
+#include <int.h>
+#include <mem.h>
+#include <debug.h>
+#include <cpu.h>
+#include <string.h>
+#include <apic.h>
+#include <pit.h>
+
+extern uint64_t smp_stack_top;
+extern uint64_t ap_gs_base;
+
+void ap_init(cpu_t *cpu)
+{
+  debug_info("SMP - Starting up processor %x\n", cpu->id);
+  if(!cpu->current_state == CPU_STOPPED)
+    return;
+  // Send INIT command
+  apic_interrupt(cpu->id, APIC_INT_LEVEL_ASSERT, APIC_INT_TYPE_INIT, 0);
+  // Wait 10 ms
+  pit_delay(10);
+
+  // Write cpu data position to trampoline
+  ap_gs_base = (uint64_t) cpu;
+
+  cpu->current_state = CPU_SIPI_SENT;
+  apic_interrupt(cpu->id, APIC_INT_LEVEL_ASSERT, APIC_INT_TYPE_STARTUP, TRAMPOLINE_ADDR/PAGE_SIZE);
+  pit_delay(10);
+
+  if(cpu->current_state != CPU_SIPI_REC)
+  {
+    cpu->current_state = CPU_SIPI_SENT;
+    apic_interrupt(cpu->id, APIC_INT_LEVEL_ASSERT, APIC_INT_TYPE_STARTUP, TRAMPOLINE_ADDR/PAGE_SIZE);
+    pit_delay(100);
+  }
+
+  if(cpu->current_state != CPU_SIPI_REC)
+  {
+    debug_error("SMP - Starting cpu %x failed\n", cpu->id);
+    cpu->current_state = CPU_FAILED;
+    return;
+  }
+  cpu->current_state = CPU_STARTED;
+}
+
+void ap_start()
+{
+  debug_ok("STARTED CPU:%x\n", get_cpu()->id);
+  for(;;)asm("hlt");
+}
diff --git a/kernel/cpu/trampoline.S b/kernel/cpu/trampoline.S
new file mode 100644
index 0000000..12231a8
--- /dev/null
+++ b/kernel/cpu/trampoline.S
@@ -0,0 +1,110 @@
+#include <mem.h>
+#include <cpu.h>
+#include <msr.h>
+#define TRAMPOLINE_OFFSET TRAMPOLINE_ADDR - trampoline
+
+.intel_syntax noprefix
+.global trampoline
+.section .text
+.code16
+trampoline:
+
+  cli
+  mov ax, 0x0
+  mov ds, ax
+  mov es, ax
+  mov fs, ax
+  mov gs, ax
+  mov ss, ax
+
+  lidt [ap_boot_idt + TRAMPOLINE_OFFSET]
+  lgdt [ap_boot_gdtp + TRAMPOLINE_OFFSET]
+
+  mov eax, cr0
+  or al, 0x1
+  mov cr0, eax
+
+  jmp 0x8:start_32 + TRAMPOLINE_OFFSET
+
+.code32
+start_32:
+  mov ax, 0x10
+  mov ds, ax
+  mov es, ax
+  mov fs, ax
+  mov gs, ax
+  mov ss, ax
+
+  mov eax, cr4
+  or eax, 1<<5
+  mov cr4, eax
+
+  mov eax, offset V2P(BootP4)
+  mov cr3, eax
+
+  mov ecx, 0xC0000080
+  rdmsr
+  or eax, 1<<8
+  wrmsr
+
+  mov eax, cr0
+  or eax, 1<<31
+  mov cr0, eax
+
+  lgdt V2P(BootGDTp)
+
+  mov ax, 0x10
+  mov ds, ax
+  mov es, ax
+  mov ss, ax
+
+  jmp 0x8:start_64 + TRAMPOLINE_OFFSET
+.code64
+start_64:
+  movabs rax, offset start_longmode
+  jmp rax
+start_longmode:
+
+  movabs rax, ap_gs_base
+  mov rdx, rax
+  shr rdx, 32
+  mov ecx, MSR_REG_KERNEL_GS
+  wrmsr
+  swapgs
+
+  movq [gs:GS_OFFSET_STATE], CPU_SIPI_REC
+.ap_sync_loop:
+  cmpq [gs:GS_OFFSET_STATE], CPU_SIPI_REC
+  je .ap_sync_loop
+
+  mov rsp, gs:GS_OFFSET_STACK
+  push 0
+  mov rbp, rsp
+  push 0
+  popf
+
+  .extern ap_start
+  call ap_start
+
+
+.align 16
+ap_boot_gdt:
+  .long 0x00000000, 0x00000000
+  .long 0x0000FFFF, 0x00CF9A00
+  .long 0x0000FFFF, 0x00CF9200
+.align 16
+ap_boot_gdtp:
+  .short 3*8-1
+  .long ap_boot_gdt + TRAMPOLINE_OFFSET
+
+.align 16
+ap_boot_idt:
+  .short 0
+  .long 0
+  .long 0
+
+.global ap_gs_base
+ap_gs_base:
+  .long 0x00000000, 0x00000000
+trampoline_end:
+  nop
diff --git a/kernel/include/cpu.h b/kernel/include/cpu.h
index c1935cb..3fe17f8 100644
--- a/kernel/include/cpu.h
+++ b/kernel/include/cpu.h
@@ -2,7 +2,17 @@
 
 #define MAX_NUMCPU 64
 
+#define CPU_STOPPED 0
+#define CPU_SIPI_SENT 1
+#define CPU_SIPI_REC 2
+#define CPU_STARTED 3
+#define CPU_FAILED 4
+
+#define TRAMPOLINE_ADDR 0x1000
+
 #define GS_OFFSET_CPU 0
+#define GS_OFFSET_STATE 40
+#define GS_OFFSET_STACK 48
 
 #ifndef __ASSEMBLER__
 #include <stdint.h>
@@ -17,6 +27,8 @@ typedef struct cpu_t
   uint64_t apic_id;
   uint64_t apic_ticks_per_us;
   uint64_t is_bsp;
+  uint64_t current_state; // 40
+  void *kernel_stack; // 48
   thread_t *current_thread;
   thread_t *last_thread;
   process_t *current_process;
@@ -31,7 +43,11 @@ extern unsigned int num_cpu;
 void acpi_init();
 void cpu_add(uint64_t id, uint64_t apic);
 void cpu_init();
+void init_cpu();
 cpu_t *get_cpu();
+void ap_init(cpu_t *cpu);
+
+void trampoline();
 #endif