1 files changed, 77 insertions, 3 deletions

diff --git a/sys/arch/amd64/amd64/machdep.c b/sys/arch/amd64/amd64/machdep.c
index 3381437..8258f8e 100644
--- a/sys/arch/amd64/amd64/machdep.c
+++ b/sys/arch/amd64/amd64/machdep.c
@@ -42,9 +42,18 @@
 #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
@@ -52,13 +61,13 @@
 #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 bool bsp_init = false;
 
 __attribute__((__interrupt__))
 static void
@@ -68,6 +77,31 @@ cpu_halt_isr(void *p)
     __builtin_unreachable();
 }
 
+__attribute__((__interrupt__))
+static void
+tlb_shootdown_isr(void *p)
+{
+    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)
 {
@@ -85,6 +119,7 @@ setup_vectors(void)
     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(TLB_VECTOR, IDT_INT_GATE, ISR(tlb_shootdown_isr), 0);
     pin_isr_load();
 }
 
@@ -129,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)
 {
@@ -166,7 +235,7 @@ 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 (;;);
 }
 
@@ -244,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;
+    }
 }