Finish implementing SMP

STIVALE2.md

@@ -392,7 +392,7 @@ struct stivale2_smp_info {
                                 // This MUST point to a valid stack of at least
                                 // 256 bytes in size, and 16-byte aligned.
     uint64_t goto_address;      // This address is polled by the started APs
-                                // until the kernel on the BSP performs an
+                                // until the kernel on another CPU performs an
                                 // atomic write to this field.
                                 // When that happens, bootloader code will
                                 // load up ESP/RSP with the stack value as

stage2/lib/blib.h

@@ -11,7 +11,6 @@ int cpuid(uint32_t leaf, uint32_t subleaf,
 
 __attribute__((noreturn)) void panic(const char *fmt, ...);
 
-void pit_sleep(uint32_t pit_ticks);
 int pit_sleep_and_quit_on_keypress(uint32_t pit_ticks);
 
 void brewind(size_t count);

stage2/lib/sleep.asm

@@ -9,83 +9,6 @@ int_08_isr:
     iret
     bits 32
 
-global pit_sleep
-pit_sleep:
-    ; Hook int 0x08
-    mov edx, dword [0x08*4]
-    mov dword [0x80*4], edx
-    mov edx, int_08_isr
-    mov dword [0x08*4], int_08_isr
-
-    ; pit_ticks in edx
-    mov edx, dword [esp+4]
-
-    mov dword [int_08_ticks_counter], 0
-
-    ; Save non-scratch GPRs
-    push ebx
-    push esi
-    push edi
-    push ebp
-
-    ; Jump to real mode
-    jmp 0x08:.bits16
-  .bits16:
-    bits 16
-    mov ax, 0x10
-    mov ds, ax
-    mov es, ax
-    mov fs, ax
-    mov gs, ax
-    mov ss, ax
-    mov eax, cr0
-    and al, 0xfe
-    mov cr0, eax
-    jmp 0x00:.cszero
-  .cszero:
-    xor ax, ax
-    mov ds, ax
-    mov es, ax
-    mov fs, ax
-    mov gs, ax
-    mov ss, ax
-
-    sti
-
-  .loop:
-    cmp dword [int_08_ticks_counter], edx
-    je .done
-    jmp .loop
-
-  .done:
-    cli
-
-    ; Jump back to pmode
-    mov eax, cr0
-    or al, 1
-    mov cr0, eax
-    jmp 0x18:.bits32
-  .bits32:
-    bits 32
-    mov ax, 0x20
-    mov ds, ax
-    mov es, ax
-    mov fs, ax
-    mov gs, ax
-    mov ss, ax
-
-    ; Restore non-scratch GPRs
-    pop ebp
-    pop edi
-    pop esi
-    pop ebx
-
-    ; Dehook int 0x08
-    mov edx, dword [0x80*4]
-    mov dword [0x08*4], edx
-
-    ret
-
 global pit_sleep_and_quit_on_keypress
 pit_sleep_and_quit_on_keypress:
     ; Hook int 0x08

stage2/lib/smp.c

@@ -4,6 +4,7 @@
 #include <lib/acpi.h>
 #include <lib/cio.h>
 #include <lib/blib.h>
+#include <lib/print.h>
 #include <lib/smp.h>
 #include <drivers/lapic.h>
 #include <mm/vmm64.h>
@@ -32,6 +33,11 @@ struct gdtr {
     uint32_t ptr;
 } __attribute__((packed));
 
+static void delay(uint32_t cycles) {
+    for (uint32_t i = 0; i < cycles; i++)
+        port_in_b(0x80);
+}
+
 void     smp_trampoline(void);
 extern   struct gdtr smp_tpl_gdt;
 struct   smp_information *smp_tpl_info_struct;
@@ -52,17 +58,17 @@ static bool smp_start_ap(uint8_t lapic_id, struct gdtr *gdtr,
     // Send the INIT IPI
     lapic_write(LAPIC_REG_ICR1, lapic_id << 24);
     lapic_write(LAPIC_REG_ICR0, 0x500);
-    pit_sleep(1);
+    delay(5000);
 
     // Send the Startup IPI
     lapic_write(LAPIC_REG_ICR1, lapic_id << 24);
     lapic_write(LAPIC_REG_ICR0, ((size_t)smp_trampoline / 4096) | 0x600);
 
-    for (int i = 0; i < 20; i++) {
+    for (int i = 0; i < 100; i++) {
-        pit_sleep(1);
         if (locked_read(&smp_tpl_booted_flag) == 1) {
             return true;
         }
+        delay(10000);
     }
 
     return false;

stage2/lib/smp.h

@@ -9,6 +9,7 @@
 struct smp_information {
     uint32_t processor_id;
     uint32_t lapic_id;
+    uint64_t stack_addr;
     uint64_t goto_address;
 } __attribute__((packed));
 

stage2/lib/smp_trampoline.asm

@@ -77,16 +77,62 @@ section .text
 
 bits 32
 parking32:
-    mov ecx, dword [smp_tpl_info_struct]
+    mov edi, dword [smp_tpl_info_struct]
     mov eax, 1
     lock xchg dword [smp_tpl_booted_flag], eax
-    mov eax, 0xcafebabe
+
-    jmp $
+    xor eax, eax
+  .loop:
+    lock xadd dword [edi + 16], eax
+    test eax, eax
+    jnz .out
+    pause
+    jmp .loop
+
+  .out:
+    mov esp, dword [edi + 8]
+    push 0
+    push eax
+    push edi
+    xor eax, eax
+    xor ebx, ebx
+    xor ecx, ecx
+    xor edx, edx
+    xor esi, esi
+    xor edi, edi
+    xor ebp, ebp
+    ret
 
 bits 64
 parking64:
-    mov ecx, dword [smp_tpl_info_struct]
+    mov edi, dword [smp_tpl_info_struct]
     mov eax, 1
     lock xchg dword [smp_tpl_booted_flag], eax
-    mov eax, 0xdeadbeef
+
-    jmp $
+    xor eax, eax
+  .loop:
+    lock xadd qword [rdi + 16], rax
+    test rax, rax
+    jnz .out
+    pause
+    jmp .loop
+
+  .out:
+    mov rsp, qword [rdi + 8]
+    push 0
+    push rax
+    xor rax, rax
+    xor rbx, rbx
+    xor rcx, rcx
+    xor rdx, rdx
+    xor rsi, rsi
+    xor rbp, rbp
+    xor r8,  r8
+    xor r9,  r9
+    xor r10, r10
+    xor r11, r11
+    xor r12, r12
+    xor r13, r13
+    xor r14, r14
+    xor r15, r15
+    ret

stage2/limine.h

@@ -1,6 +1,6 @@
 #ifndef __LIMINE_H__
 #define __LIMINE_H__
 
-#define LIMINE_VERSION "0.4"
+#define LIMINE_VERSION "0.5"
 
 #endif