:: commit b29a513e6c84db06e0ed79755a974319289887bf

diff --git a/stage23/entry.s3.c b/stage23/entry.s3.c
index f98b3dd6..0be9473f 100644
--- a/stage23/entry.s3.c
+++ b/stage23/entry.s3.c
@@ -21,6 +21,7 @@
 #include <pxe/pxe.h>
 #include <pxe/tftp.h>
 #include <drivers/disk.h>
+#include <sys/lapic.h>
 
 void stage3_common(void);
 
@@ -63,6 +64,8 @@ void uefi_entry(EFI_HANDLE ImageHandle, EFI_SYSTEM_TABLE *SystemTable) {
 
     disk_create_index();
 
+    init_io_apics();
+
     boot_volume = NULL;
 
     EFI_HANDLE current_handle = ImageHandle;
diff --git a/stage23/protos/stivale.c b/stage23/protos/stivale.c
index 13d3dbc4..2fe80404 100644
--- a/stage23/protos/stivale.c
+++ b/stage23/protos/stivale.c
@@ -16,6 +16,7 @@
 #include <sys/pic.h>
 #include <sys/cpu.h>
 #include <sys/gdt.h>
+#include <sys/lapic.h>
 #include <fs/file.h>
 #include <mm/vmm.h>
 #include <mm/pmm.h>
@@ -424,6 +425,8 @@ __attribute__((noreturn)) void stivale_spinup(
     pic_mask_all();
     pic_flush();
 
+    io_apic_mask_all();
+
     common_spinup(stivale_spinup_32, 9,
         bits, level5pg, enable_nx, (uint32_t)(uintptr_t)pagemap->top_level,
         (uint32_t)entry_point, (uint32_t)(entry_point >> 32),
diff --git a/stage23/sys/lapic.c b/stage23/sys/lapic.c
index ffe480d6..142fe3ad 100644
--- a/stage23/sys/lapic.c
+++ b/stage23/sys/lapic.c
@@ -5,6 +5,23 @@
 #include <sys/cpu.h>
 #include <lib/blib.h>
 #include <lib/acpi.h>
+#include <mm/pmm.h>
+
+struct madt {
+    struct sdt header;
+    uint32_t local_controller_addr;
+    uint32_t flags;
+    char     madt_entries_begin[];
+} __attribute__((packed));
+
+struct madt_io_apic {
+    uint8_t type;
+    uint8_t length;
+    uint8_t apic_id;
+    uint8_t reserved;
+    uint32_t address;
+    uint32_t gsib;
+} __attribute__((packed));
 
 struct dmar {
     struct sdt header;
@@ -75,3 +92,74 @@ uint64_t x2apic_read(uint32_t reg) {
 void x2apic_write(uint32_t reg, uint64_t data) {
     wrmsr(0x800 + (reg >> 4), data);
 }
+
+static struct madt_io_apic **io_apics = NULL;
+static size_t max_io_apics = 0;
+
+void init_io_apics(void) {
+    static bool already_inited = false;
+    if (already_inited) {
+        return;
+    }
+
+    struct madt *madt = acpi_get_table("APIC", 0);
+
+    if (madt == NULL) {
+        panic("IO APIC error");
+    }
+
+    for (uint8_t *madt_ptr = (uint8_t *)madt->madt_entries_begin;
+      (uintptr_t)madt_ptr < (uintptr_t)madt + madt->header.length;
+      madt_ptr += *(madt_ptr + 1)) {
+        switch (*madt_ptr) {
+            case 1: {
+                max_io_apics++;
+                continue;
+            }
+        }
+    }
+
+    io_apics = ext_mem_alloc(max_io_apics * sizeof(struct madt_io_apic *));
+    max_io_apics = 0;
+
+    // Try to start all APs
+    for (uint8_t *madt_ptr = (uint8_t *)madt->madt_entries_begin;
+      (uintptr_t)madt_ptr < (uintptr_t)madt + madt->header.length;
+      madt_ptr += *(madt_ptr + 1)) {
+        switch (*madt_ptr) {
+            case 1: {
+                io_apics[max_io_apics++] = (void *)madt_ptr;
+                continue;
+            }
+        }
+    }
+
+    already_inited = true;
+}
+
+uint32_t io_apic_read(size_t io_apic, uint32_t reg) {
+    uintptr_t base = (uintptr_t)io_apics[io_apic]->address;
+    mmoutd(base, reg);
+    return mmind(base + 16);
+}
+
+void io_apic_write(size_t io_apic, uint32_t reg, uint32_t value) {
+    uintptr_t base = (uintptr_t)io_apics[io_apic]->address;
+    mmoutd(base, reg);
+    mmoutd(base + 16, value);
+}
+
+uint32_t io_apic_gsi_count(size_t io_apic) {
+	return ((io_apic_read(io_apic, 1) & 0xff0000) >> 16) + 1;
+}
+
+void io_apic_mask_all(void) {
+    for (size_t i = 0; i < max_io_apics; i++) {
+        uint32_t gsi_count = io_apic_gsi_count(i);
+        for (uint32_t j = 0; j < gsi_count; j++) {
+            uintptr_t ioredtbl = j * 2 + 16;
+            io_apic_write(i, ioredtbl, (1 << 16)); // mask
+            io_apic_write(i, ioredtbl + 1, 0);
+        }
+    }
+}
diff --git a/stage23/sys/lapic.h b/stage23/sys/lapic.h
index caacd392..aba64ad3 100644
--- a/stage23/sys/lapic.h
+++ b/stage23/sys/lapic.h
@@ -1,6 +1,7 @@
 #ifndef __SYS__APIC_H__
 #define __SYS__APIC_H__
 
+#include <stddef.h>
 #include <stdint.h>
 #include <stdbool.h>
 
@@ -18,4 +19,10 @@ bool x2apic_enable(void);
 uint64_t x2apic_read(uint32_t reg);
 void x2apic_write(uint32_t reg, uint64_t data);
 
+void init_io_apics(void);
+uint32_t io_apic_read(size_t io_apic, uint32_t reg);
+void io_apic_write(size_t io_apic, uint32_t reg, uint32_t value);
+uint32_t io_apic_gsi_count(size_t io_apic);
+void io_apic_mask_all(void);
+
 #endif