1 files changed, 98 insertions, 13 deletions

diff --git a/sys/arch/amd64/amd64/machdep.c b/sys/arch/amd64/amd64/machdep.c
index 07d6cdd..8258f8e 100644
--- a/sys/arch/amd64/amd64/machdep.c
+++ b/sys/arch/amd64/amd64/machdep.c
@@ -42,24 +42,32 @@
 #include <machine/uart.h>
 #include <machine/sync.h>
 #include <machine/intr.h>
+#include <machine/cdefs.h>
 #include <machine/isa/i8042var.h>
 
+#define pr_trace(fmt, ...) kprintf("cpu: " fmt, ##__VA_ARGS__)
+#define pr_error(...) pr_trace(__VA_ARGS__)
+#define pr_trace_bsp(...)       \
+    if (!bsp_init) {            \
+        pr_trace(__VA_ARGS__);  \
+    }
+
+#define HALT_VECTOR 0x21
+#define TLB_VECTOR  0x22
+
 #if defined(__SPECTRE_IBRS)
 #define SPECTRE_IBRS  __SPECTRE_IBRS
 #else
 #define SPECTRE_IBRS 0
 #endif
 
-static uint8_t halt_vector = 0;
-
 int ibrs_enable(void);
+int simd_init(void);
 void syscall_isr(void);
+void pin_isr_load(void);
 
 struct cpu_info g_bsp_ci = {0};
-static struct gdtr bsp_gdtr = {
-    .limit = sizeof(struct gdt_entry) * 256 - 1,
-    .offset = (uintptr_t)&g_gdt_data[0]
-};
+static bool bsp_init = false;
 
 __attribute__((__interrupt__))
 static void
@@ -69,13 +77,34 @@ cpu_halt_isr(void *p)
     __builtin_unreachable();
 }
 
+__attribute__((__interrupt__))
 static void
-setup_vectors(void)
+tlb_shootdown_isr(void *p)
 {
-    if (halt_vector == 0) {
-        halt_vector = intr_alloc_vector("cpu-halt", IPL_HIGH);
+    struct cpu_info *ci;
+    int ipl;
+
+    /*
+     * Get the current CPU and check if we even
+     * need a shootdown. If `tlb_shootdown' is
+     * unset, this is not for us.
+     */
+    ci = this_cpu();
+    if (!ci->tlb_shootdown) {
+        return;
     }
 
+    ipl = splraise(IPL_HIGH);
+    __invlpg(ci->shootdown_va);
+
+    ci->shootdown_va = 0;
+    ci->tlb_shootdown = 0;
+    splx(ipl);
+}
+
+static void
+setup_vectors(void)
+{
     idt_set_desc(0x0, IDT_TRAP_GATE, ISR(arith_err), 0);
     idt_set_desc(0x2, IDT_TRAP_GATE, ISR(nmi), 0);
     idt_set_desc(0x3, IDT_TRAP_GATE, ISR(breakpoint_handler), 0);
@@ -89,7 +118,9 @@ setup_vectors(void)
     idt_set_desc(0xD, IDT_TRAP_GATE, ISR(general_prot), 0);
     idt_set_desc(0xE, IDT_TRAP_GATE, ISR(page_fault), 0);
     idt_set_desc(0x80, IDT_USER_INT_GATE, ISR(syscall_isr), 0);
-    idt_set_desc(halt_vector, IDT_INT_GATE, ISR(cpu_halt_isr), 0);
+    idt_set_desc(HALT_VECTOR, IDT_INT_GATE, ISR(cpu_halt_isr), 0);
+    idt_set_desc(TLB_VECTOR, IDT_INT_GATE, ISR(tlb_shootdown_isr), 0);
+    pin_isr_load();
 }
 
 static inline void
@@ -97,7 +128,7 @@ init_tss(struct cpu_info *ci)
 {
     struct tss_desc *desc;
 
-    desc = (struct tss_desc *)&g_gdt_data[GDT_TSS];
+    desc = (struct tss_desc *)&g_gdt_data[GDT_TSS_INDEX];
     write_tss(ci, desc);
     tss_load();
 }
@@ -133,6 +164,40 @@ backtrace_addr_to_name(uintptr_t addr, off_t *off)
     return NULL;
 }
 
+static void
+enable_simd(void)
+{
+    int retval;
+
+    if ((retval = simd_init()) < 0) {
+        pr_trace_bsp("SIMD not supported\n");
+    }
+
+    if (retval == 1) {
+        pr_trace_bsp("SSE enabled but not AVX\n");
+    }
+}
+
+void
+cpu_shootdown_tlb(vaddr_t va)
+{
+    uint32_t ncpu = cpu_count();
+    struct cpu_info *cip;
+
+    for (uint32_t i = 0; i < ncpu; ++i) {
+        cip = cpu_get(i);
+        if (cip == NULL) {
+            break;
+        }
+
+        spinlock_acquire(&cip->lock);
+        cip->shootdown_va = va;
+        cip->tlb_shootdown = 1;
+        lapic_send_ipi(cip->apicid, IPI_SHORTHAND_NONE, TLB_VECTOR);
+        spinlock_release(&cip->lock);
+    }
+}
+
 void
 md_backtrace(void)
 {
@@ -170,10 +235,25 @@ cpu_halt_all(void)
     }
 
     /* Send IPI to all cores */
-    lapic_send_ipi(0, IPI_SHORTHAND_ALL, halt_vector);
+    lapic_send_ipi(0, IPI_SHORTHAND_ALL, HALT_VECTOR);
     for (;;);
 }
 
+/*
+ * Same as cpu_halt_all() but for all other
+ * cores but ourselves.
+ */
+void
+cpu_halt_others(void)
+{
+    if (rdmsr(IA32_GS_BASE) == 0) {
+        __ASMV("cli; hlt");
+    }
+
+    /* Send IPI to all cores */
+    lapic_send_ipi(0, IPI_SHORTHAND_OTHERS, HALT_VECTOR);
+}
+
 void
 serial_init(void)
 {
@@ -224,7 +304,7 @@ void
 cpu_startup(struct cpu_info *ci)
 {
     ci->self = ci;
-    gdt_load(&bsp_gdtr);
+    gdt_load();
     idt_load();
 
     setup_vectors();
@@ -233,5 +313,10 @@ cpu_startup(struct cpu_info *ci)
     init_tss(ci);
     try_mitigate_spectre();
 
+    enable_simd();
     lapic_init();
+
+    if (!bsp_init) {
+        bsp_init = true;
+    }
 }