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 #include #include +#include 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 #include #include +#include #include #include #include @@ -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 #include #include +#include + +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 #include #include @@ -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