limine/common/sys/smp.c
2022-03-31 13:33:18 +02:00

284 lines
8.8 KiB
C

#include <stddef.h>
#include <stdint.h>
#include <stdbool.h>
#include <lib/libc.h>
#include <lib/acpi.h>
#include <sys/cpu.h>
#include <lib/blib.h>
#include <lib/print.h>
#include <sys/smp.h>
#include <sys/lapic.h>
#include <sys/gdt.h>
#include <mm/vmm.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_header {
uint8_t type;
uint8_t length;
} __attribute__((packed));
struct madt_lapic {
struct madt_header header;
uint8_t acpi_processor_uid;
uint8_t lapic_id;
uint32_t flags;
} __attribute__((packed));
struct madt_x2apic {
struct madt_header header;
uint8_t reserved[2];
uint32_t x2apic_id;
uint32_t flags;
uint32_t acpi_processor_uid;
} __attribute__((packed));
extern symbol _binary_smp_trampoline_bin_start;
extern symbol _binary_smp_trampoline_bin_end;
struct trampoline_passed_info {
uint8_t smp_tpl_booted_flag;
uint8_t smp_tpl_target_mode;
uint32_t smp_tpl_pagemap;
uint32_t smp_tpl_info_struct;
struct gdtr smp_tpl_gdt;
uint64_t smp_tpl_hhdm;
} __attribute__((packed));
static bool smp_start_ap(uint32_t lapic_id, struct gdtr *gdtr,
struct smp_information *info_struct,
bool longmode, bool lv5, uint32_t pagemap,
bool x2apic, bool nx, uint64_t hhdm, bool wp) {
size_t trampoline_size = (size_t)_binary_smp_trampoline_bin_end
- (size_t)_binary_smp_trampoline_bin_start;
// Prepare the trampoline
static void *trampoline = NULL;
if (trampoline == NULL) {
trampoline = conv_mem_alloc(trampoline_size);
memcpy(trampoline, _binary_smp_trampoline_bin_start, trampoline_size);
}
static struct trampoline_passed_info *passed_info = NULL;
if (passed_info == NULL) {
passed_info = (void *)(((uintptr_t)trampoline + trampoline_size)
- sizeof(struct trampoline_passed_info));
}
passed_info->smp_tpl_info_struct = (uint32_t)(uintptr_t)info_struct;
passed_info->smp_tpl_booted_flag = 0;
passed_info->smp_tpl_pagemap = pagemap;
passed_info->smp_tpl_target_mode = ((uint32_t)x2apic << 2)
| ((uint32_t)lv5 << 1)
| ((uint32_t)nx << 3)
| ((uint32_t)wp << 4)
| (uint32_t)longmode;
passed_info->smp_tpl_gdt = *gdtr;
passed_info->smp_tpl_booted_flag = 0;
passed_info->smp_tpl_hhdm = hhdm;
asm volatile ("" ::: "memory");
// Send the INIT IPI
if (x2apic) {
x2apic_write(LAPIC_REG_ICR0, ((uint64_t)lapic_id << 32) | 0x4500);
} else {
lapic_write(LAPIC_REG_ICR1, lapic_id << 24);
lapic_write(LAPIC_REG_ICR0, 0x4500);
}
delay(10000000);
// Send the Startup IPI
if (x2apic) {
x2apic_write(LAPIC_REG_ICR0, ((uint64_t)lapic_id << 32) |
((size_t)trampoline / 4096) | 0x4600);
} else {
lapic_write(LAPIC_REG_ICR1, lapic_id << 24);
lapic_write(LAPIC_REG_ICR0, ((size_t)trampoline / 4096) | 0x4600);
}
for (int i = 0; i < 100; i++) {
if (locked_read(&passed_info->smp_tpl_booted_flag) == 1) {
return true;
}
delay(10000000);
}
return false;
}
struct smp_information *init_smp(size_t header_hack_size,
void **header_ptr,
size_t *cpu_count,
uint32_t *_bsp_lapic_id,
bool longmode,
bool lv5,
pagemap_t pagemap,
bool x2apic,
bool nx,
uint64_t hhdm,
bool wp) {
if (!lapic_check())
return NULL;
// Search for MADT table
struct madt *madt = acpi_get_table("APIC", 0);
if (madt == NULL)
return NULL;
struct gdtr gdtr = gdt;
uint32_t eax, ebx, ecx, edx;
if (!cpuid(1, 0, &eax, &ebx, &ecx, &edx))
return NULL;
uint8_t bsp_lapic_id = ebx >> 24;
x2apic = x2apic && x2apic_enable();
uint32_t bsp_x2apic_id = 0;
if (x2apic) {
// The Intel manual recommends checking if leaf 0x1f exists first, and
// using that in place of 0xb if that's the case
if (!cpuid(0x1f, 0, &eax, &ebx, &ecx, &edx))
if (!cpuid(0xb, 0, &eax, &ebx, &ecx, &edx))
return NULL;
bsp_x2apic_id = edx;
*_bsp_lapic_id = bsp_x2apic_id;
} else {
*_bsp_lapic_id = bsp_lapic_id;
}
*cpu_count = 0;
// Count the MAX of startable APs and allocate accordingly
size_t max_cpus = 0;
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 0: {
// Processor local xAPIC
struct madt_lapic *lapic = (void *)madt_ptr;
// Check if we can actually try to start the AP
if ((lapic->flags & 1) ^ ((lapic->flags >> 1) & 1))
max_cpus++;
continue;
}
case 9: {
// Processor local x2APIC
if (!x2apic)
continue;
struct madt_x2apic *x2lapic = (void *)madt_ptr;
// Check if we can actually try to start the AP
if ((x2lapic->flags & 1) ^ ((x2lapic->flags >> 1) & 1))
max_cpus++;
continue;
}
}
}
*header_ptr = ext_mem_alloc(
header_hack_size + max_cpus * sizeof(struct smp_information));
struct smp_information *ret = *header_ptr + header_hack_size;
*cpu_count = 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 0: {
// Processor local xAPIC
struct madt_lapic *lapic = (void *)madt_ptr;
// Check if we can actually try to start the AP
if (!((lapic->flags & 1) ^ ((lapic->flags >> 1) & 1)))
continue;
struct smp_information *info_struct = &ret[*cpu_count];
info_struct->acpi_processor_uid = lapic->acpi_processor_uid;
info_struct->lapic_id = lapic->lapic_id;
// Do not try to restart the BSP
if (lapic->lapic_id == bsp_lapic_id) {
(*cpu_count)++;
continue;
}
printv("smp: [xAPIC] Found candidate AP for bring-up. LAPIC ID: %u\n", lapic->lapic_id);
// Try to start the AP
if (!smp_start_ap(lapic->lapic_id, &gdtr, info_struct,
longmode, lv5, (uintptr_t)pagemap.top_level,
x2apic, nx, hhdm, wp)) {
print("smp: FAILED to bring-up AP\n");
continue;
}
printv("smp: Successfully brought up AP\n");
(*cpu_count)++;
continue;
}
case 9: {
// Processor local x2APIC
if (!x2apic)
continue;
struct madt_x2apic *x2lapic = (void *)madt_ptr;
// Check if we can actually try to start the AP
if (!((x2lapic->flags & 1) ^ ((x2lapic->flags >> 1) & 1)))
continue;
struct smp_information *info_struct = &ret[*cpu_count];
info_struct->acpi_processor_uid = x2lapic->acpi_processor_uid;
info_struct->lapic_id = x2lapic->x2apic_id;
// Do not try to restart the BSP
if (x2lapic->x2apic_id == bsp_x2apic_id) {
(*cpu_count)++;
continue;
}
printv("smp: [x2APIC] Found candidate AP for bring-up. LAPIC ID: %u\n", x2lapic->x2apic_id);
// Try to start the AP
if (!smp_start_ap(x2lapic->x2apic_id, &gdtr, info_struct,
longmode, lv5, (uintptr_t)pagemap.top_level,
true, nx, hhdm, wp)) {
print("smp: FAILED to bring-up AP\n");
continue;
}
printv("smp: Successfully brought up AP\n");
(*cpu_count)++;
continue;
}
}
}
return ret;
}