776 lines
30 KiB
C
776 lines
30 KiB
C
// ke_smp.c - SMP application-processor startup and scheduler admission
|
|
//
|
|
// The bootloader is intentionally not involved in AP ownership. The kernel
|
|
// asks the HAL for MADT-discovered LAPIC IDs, copies a small low-memory x86 AP
|
|
// startup blob, sends INIT/SIPI through the local APIC, and waits for each AP
|
|
// to enter KiApKernelEntry() and park in kernel-owned state.
|
|
#include <stdint.h>
|
|
#include <stddef.h>
|
|
#include <string.h>
|
|
#include <intrin.h>
|
|
#pragma intrinsic(_disable)
|
|
#pragma intrinsic(_enable)
|
|
#include <ntlite/status.h>
|
|
#include <kernel/ke_smp.h>
|
|
#include <kernel/ke_cpu.h>
|
|
#include <kernel/ke_debug.h>
|
|
#include <kernel/ke_idt.h>
|
|
#include <kernel/ke_sched.h>
|
|
#include <kernel/ke_thread.h>
|
|
#include <kernel/ke_event.h>
|
|
#include <kernel/ob_object.h>
|
|
#include <kernel/mm_phys.h>
|
|
#include <kernel/mm_virt.h>
|
|
#include <kernel/hal/hal_apic.h>
|
|
|
|
#define AP_TRAMPOLINE_MAX_PHYS 0x000A0000ULL
|
|
#define AP_TRAMPOLINE_PREFERRED_MIN 0x00070000ULL
|
|
#define AP_TRAMPOLINE_FALLBACK_MIN 0x00010000ULL
|
|
#define AP_TRAMPOLINE_BOOT_OFF 0x0800u
|
|
#define AP_STACK_PAGES 8u
|
|
#define AP_START_TIMEOUT_SPINS 20000000u
|
|
#define AP_SCHED_TIMEOUT_SPINS 20000000u
|
|
|
|
#define AP_CR4_PAE 0x00000020u
|
|
#define AP_CR4_OSFXSR 0x00000200u
|
|
#define AP_CR4_OSXMMEXCPT 0x00000400u
|
|
#define AP_CR4_REQUIRED (AP_CR4_PAE | AP_CR4_OSFXSR | AP_CR4_OSXMMEXCPT)
|
|
#define AP_CR0_PE 0x00000001u
|
|
#define AP_CR0_MP 0x00000002u
|
|
#define AP_CR0_EM 0x00000004u
|
|
#define AP_CR0_TS 0x00000008u
|
|
#define AP_CR0_NE 0x00000020u
|
|
#define AP_CR0_PG 0x80000000u
|
|
|
|
#define AP_BOOT_MAGIC 0x3436544F4F425041ULL /* 'APBOOT64' */
|
|
#define AP_STATE_EMPTY 0u
|
|
#define AP_STATE_READY 1u
|
|
#define AP_STATE_LONG_MODE 2u
|
|
#define AP_STATE_KERNEL_ENTRY 3u
|
|
#define AP_STATE_KERNEL_PARKED 4u
|
|
#define AP_STATE_FAILED 0xFFFFFFFFu
|
|
|
|
#define AP_STAGE_NONE 0x00000000u
|
|
#define AP_STAGE_LONG64 0x4C4D3634u /* 'LM64' */
|
|
#define AP_STAGE_ENTRY 0x454E5452u /* 'ENTR' */
|
|
#define AP_STAGE_GS 0x47534241u /* 'GSBA' */
|
|
#define AP_STAGE_GDT 0x47445421u /* 'GDT!' */
|
|
#define AP_STAGE_IDT 0x49445421u /* 'IDT!' */
|
|
#define AP_STAGE_PARK 0x5041524Bu /* 'PARK' */
|
|
|
|
#pragma pack(push, 1)
|
|
typedef struct AP_BOOT_BLOCK {
|
|
uint64_t Magic;
|
|
uint64_t KernelCr3;
|
|
uint64_t LongModeEntry;
|
|
uint64_t Kcpu;
|
|
uint64_t StackTop;
|
|
volatile uint64_t Heartbeat;
|
|
volatile uint32_t State;
|
|
volatile uint32_t Stage;
|
|
volatile uint32_t ApIndex;
|
|
} AP_BOOT_BLOCK;
|
|
#pragma pack(pop)
|
|
|
|
_Static_assert(offsetof(AP_BOOT_BLOCK, KernelCr3) == 8, "AP_BOOT_BLOCK.KernelCr3 offset");
|
|
_Static_assert(offsetof(AP_BOOT_BLOCK, StackTop) == 32, "AP_BOOT_BLOCK.StackTop offset");
|
|
_Static_assert(offsetof(AP_BOOT_BLOCK, State) == 48, "AP_BOOT_BLOCK.State offset");
|
|
_Static_assert(offsetof(AP_BOOT_BLOCK, Stage) == 52, "AP_BOOT_BLOCK.Stage offset");
|
|
|
|
static PHYSICAL_ADDRESS gApTrampolinePhys;
|
|
static PHYSICAL_ADDRESS gReservedApTrampolinePhys;
|
|
static AP_BOOT_BLOCK* gApBootBlock;
|
|
|
|
static void KiEmit8(uint8_t** p, uint8_t v) { *(*p)++ = v; }
|
|
static void KiEmit16(uint8_t** p, uint16_t v) { memcpy(*p, &v, sizeof(v)); *p += sizeof(v); }
|
|
static void KiEmit32(uint8_t** p, uint32_t v) { memcpy(*p, &v, sizeof(v)); *p += sizeof(v); }
|
|
static void KiEmit64(uint8_t** p, uint64_t v) { memcpy(*p, &v, sizeof(v)); *p += sizeof(v); }
|
|
|
|
static void KiPatch16(uint8_t* base, size_t off, uint16_t v) { memcpy(base + off, &v, sizeof(v)); }
|
|
static void KiPatch32(uint8_t* base, size_t off, uint32_t v) { memcpy(base + off, &v, sizeof(v)); }
|
|
static void KiPatch64(uint8_t* base, size_t off, uint64_t v) { memcpy(base + off, &v, sizeof(v)); }
|
|
|
|
static uint64_t KiGdtDescriptor(uint32_t base, uint32_t limit, uint8_t access, uint8_t flags) {
|
|
return ((uint64_t)(limit & 0xFFFF)) |
|
|
((uint64_t)(base & 0xFFFFFF) << 16) |
|
|
((uint64_t)access << 40) |
|
|
((uint64_t)((limit >> 16) & 0xF) << 48) |
|
|
((uint64_t)(flags & 0xF) << 52) |
|
|
((uint64_t)((base >> 24) & 0xFF) << 56);
|
|
}
|
|
|
|
void KeSetApplicationProcessorTrampoline(uint64_t trampolinePhys) {
|
|
if (trampolinePhys >= PAGE_SIZE &&
|
|
trampolinePhys < AP_TRAMPOLINE_MAX_PHYS &&
|
|
!(trampolinePhys & (PAGE_SIZE - 1))) {
|
|
gReservedApTrampolinePhys = (PHYSICAL_ADDRESS)trampolinePhys;
|
|
} else {
|
|
gReservedApTrampolinePhys = 0;
|
|
}
|
|
}
|
|
|
|
static KSTATUS KiBuildApTrampoline(void) {
|
|
if (gApTrampolinePhys)
|
|
return STATUS_SUCCESS;
|
|
|
|
PHYSICAL_ADDRESS frame = gReservedApTrampolinePhys;
|
|
if (frame) {
|
|
if (frame < AP_TRAMPOLINE_FALLBACK_MIN ||
|
|
frame >= AP_TRAMPOLINE_MAX_PHYS ||
|
|
(frame & (PAGE_SIZE - 1))) {
|
|
DbgPrint("SMP: rejecting bootloader AP trampoline frame 0x%016llX\n",
|
|
(uint64_t)frame);
|
|
frame = 0;
|
|
} else {
|
|
DbgPrint("SMP: using bootloader-reserved AP trampoline frame 0x%016llX\n",
|
|
(uint64_t)frame);
|
|
}
|
|
}
|
|
|
|
if (!frame) {
|
|
DbgPrint("SMP: trying PMM AP trampoline range 0x%016llX-0x%016llX\n",
|
|
(uint64_t)AP_TRAMPOLINE_PREFERRED_MIN,
|
|
(uint64_t)AP_TRAMPOLINE_MAX_PHYS);
|
|
frame = MmAllocateFrameInRange(AP_TRAMPOLINE_PREFERRED_MIN,
|
|
AP_TRAMPOLINE_MAX_PHYS);
|
|
}
|
|
if (!frame) {
|
|
DbgPrint("SMP: trying PMM AP trampoline range 0x%016llX-0x%016llX\n",
|
|
(uint64_t)AP_TRAMPOLINE_FALLBACK_MIN,
|
|
(uint64_t)AP_TRAMPOLINE_PREFERRED_MIN);
|
|
frame = MmAllocateFrameInRange(AP_TRAMPOLINE_FALLBACK_MIN,
|
|
AP_TRAMPOLINE_PREFERRED_MIN);
|
|
}
|
|
if (!frame) {
|
|
DbgPrint("SMP: no free PMM frame available below 0x%016llX for AP trampoline\n",
|
|
(uint64_t)AP_TRAMPOLINE_MAX_PHYS);
|
|
return STATUS_NO_MEMORY;
|
|
}
|
|
|
|
DbgPrint("SMP: AP trampoline frame 0x%016llX selected\n", (uint64_t)frame);
|
|
|
|
uint8_t* page = (uint8_t*)(PHYS_MAP_BASE + frame);
|
|
memset(page, 0, PAGE_SIZE);
|
|
|
|
uint32_t physBase = (uint32_t)frame;
|
|
uint8_t* p = page;
|
|
|
|
// --- 16-bit real-mode SIPI entry ---
|
|
KiEmit8(&p, 0xFA); // cli
|
|
KiEmit8(&p, 0x8C); KiEmit8(&p, 0xC8); // mov ax, cs
|
|
KiEmit8(&p, 0x8E); KiEmit8(&p, 0xD8); // mov ds, ax
|
|
KiEmit8(&p, 0x0F); KiEmit8(&p, 0x01); KiEmit8(&p, 0x16);
|
|
size_t gdtr16Patch = (size_t)(p - page); KiEmit16(&p, 0); // lgdt [gdtr]
|
|
KiEmit8(&p, 0x0F); KiEmit8(&p, 0x20); KiEmit8(&p, 0xC0); // mov eax, cr0
|
|
KiEmit8(&p, 0x66); KiEmit8(&p, 0x83); KiEmit8(&p, 0xC8); KiEmit8(&p, 0x01); // or eax, 1
|
|
KiEmit8(&p, 0x0F); KiEmit8(&p, 0x22); KiEmit8(&p, 0xC0); // mov cr0, eax
|
|
KiEmit8(&p, 0x66); KiEmit8(&p, 0xEA); // jmp far 32-bit offset
|
|
size_t prot32Patch = (size_t)(p - page); KiEmit32(&p, 0);
|
|
KiEmit16(&p, 0x0008); // selector 32-bit code
|
|
|
|
while (((uintptr_t)(p - page)) & 0x0F) KiEmit8(&p, 0x90);
|
|
uint32_t prot32Off = (uint32_t)(p - page);
|
|
|
|
// --- 32-bit protected-mode transition to long mode ---
|
|
KiEmit8(&p, 0x66); KiEmit8(&p, 0xB8); KiEmit16(&p, 0x0010); // mov ax, 0x10
|
|
KiEmit8(&p, 0x8E); KiEmit8(&p, 0xD8); // mov ds, ax
|
|
KiEmit8(&p, 0x8E); KiEmit8(&p, 0xC0); // mov es, ax
|
|
KiEmit8(&p, 0x8E); KiEmit8(&p, 0xD0); // mov ss, ax
|
|
KiEmit8(&p, 0x8E); KiEmit8(&p, 0xE0); // mov fs, ax
|
|
KiEmit8(&p, 0x8E); KiEmit8(&p, 0xE8); // mov gs, ax
|
|
KiEmit8(&p, 0x0F); KiEmit8(&p, 0x20); KiEmit8(&p, 0xE0); // mov eax, cr4
|
|
KiEmit8(&p, 0x0D); KiEmit32(&p, AP_CR4_REQUIRED); // or eax, PAE|OSFXSR|OSXMMEXCPT
|
|
KiEmit8(&p, 0x0F); KiEmit8(&p, 0x22); KiEmit8(&p, 0xE0); // mov cr4, eax
|
|
KiEmit8(&p, 0xB9); KiEmit32(&p, 0xC0000080u); // mov ecx, IA32_EFER
|
|
KiEmit8(&p, 0x0F); KiEmit8(&p, 0x32); // rdmsr
|
|
KiEmit8(&p, 0x0D); KiEmit32(&p, 0x00000900u); // or eax, LME|NXE
|
|
KiEmit8(&p, 0x0F); KiEmit8(&p, 0x30); // wrmsr
|
|
KiEmit8(&p, 0xA1); size_t cr3Patch = (size_t)(p - page); KiEmit32(&p, 0); // mov eax,[cr3]
|
|
KiEmit8(&p, 0x0F); KiEmit8(&p, 0x22); KiEmit8(&p, 0xD8); // mov cr3, eax
|
|
KiEmit8(&p, 0x0F); KiEmit8(&p, 0x20); KiEmit8(&p, 0xC0); // mov eax, cr0
|
|
KiEmit8(&p, 0x25); KiEmit32(&p, ~(AP_CR0_EM | AP_CR0_TS)); // and eax, ~(EM|TS)
|
|
KiEmit8(&p, 0x0D); KiEmit32(&p, AP_CR0_PG | AP_CR0_PE | AP_CR0_MP | AP_CR0_NE); // or eax, PG|PE|MP|NE
|
|
KiEmit8(&p, 0x0F); KiEmit8(&p, 0x22); KiEmit8(&p, 0xC0); // mov cr0, eax
|
|
KiEmit8(&p, 0xEA); size_t long64Patch = (size_t)(p - page); KiEmit32(&p, 0);
|
|
KiEmit16(&p, 0x0018); // selector 64-bit code
|
|
|
|
while (((uintptr_t)(p - page)) & 0x0F) KiEmit8(&p, 0x90);
|
|
uint32_t long64Off = (uint32_t)(p - page);
|
|
|
|
// --- 64-bit entry. Jump into high-half KiApKernelEntry(KCPU*). ---
|
|
KiEmit8(&p, 0x48); KiEmit8(&p, 0xBB); size_t bootBlockPatch = (size_t)(p - page); KiEmit64(&p, 0); // mov rbx, bootblock
|
|
KiEmit8(&p, 0xC7); KiEmit8(&p, 0x83); KiEmit32(&p, (uint32_t)offsetof(AP_BOOT_BLOCK, Stage)); KiEmit32(&p, AP_STAGE_LONG64);
|
|
KiEmit8(&p, 0xC7); KiEmit8(&p, 0x83); KiEmit32(&p, (uint32_t)offsetof(AP_BOOT_BLOCK, State)); KiEmit32(&p, AP_STATE_LONG_MODE);
|
|
KiEmit8(&p, 0x48); KiEmit8(&p, 0x8B); KiEmit8(&p, 0xA3); KiEmit32(&p, (uint32_t)offsetof(AP_BOOT_BLOCK, StackTop)); // mov rsp,[rbx+StackTop]
|
|
KiEmit8(&p, 0x48); KiEmit8(&p, 0x83); KiEmit8(&p, 0xE4); KiEmit8(&p, 0xF0); // and rsp,-16
|
|
KiEmit8(&p, 0x48); KiEmit8(&p, 0x83); KiEmit8(&p, 0xEC); KiEmit8(&p, 0x28); // sub rsp,28h
|
|
KiEmit8(&p, 0x48); KiEmit8(&p, 0x8B); KiEmit8(&p, 0x8B); KiEmit32(&p, (uint32_t)offsetof(AP_BOOT_BLOCK, Kcpu)); // mov rcx,[rbx+Kcpu]
|
|
KiEmit8(&p, 0x48); KiEmit8(&p, 0x8B); KiEmit8(&p, 0x83); KiEmit32(&p, (uint32_t)offsetof(AP_BOOT_BLOCK, LongModeEntry)); // mov rax,[rbx+LongModeEntry]
|
|
KiEmit8(&p, 0xFF); KiEmit8(&p, 0xE0); // jmp rax
|
|
|
|
while (((uintptr_t)(p - page)) & 0x07) KiEmit8(&p, 0);
|
|
uint32_t gdtOff = (uint32_t)(p - page);
|
|
uint64_t* gdt = (uint64_t*)p;
|
|
gdt[0] = 0;
|
|
gdt[1] = KiGdtDescriptor(0, 0xFFFFF, 0x9A, 0xC); // 32-bit code
|
|
gdt[2] = KiGdtDescriptor(0, 0xFFFFF, 0x92, 0xC); // 32-bit data
|
|
gdt[3] = KiGdtDescriptor(0, 0xFFFFF, 0x9A, 0xA); // 64-bit code
|
|
gdt[4] = KiGdtDescriptor(0, 0xFFFFF, 0x92, 0xC); // 64-bit data
|
|
p += 5 * sizeof(uint64_t);
|
|
|
|
uint32_t gdtrOff = (uint32_t)(p - page);
|
|
KiEmit16(&p, (uint16_t)(5 * sizeof(uint64_t) - 1));
|
|
KiEmit32(&p, physBase + gdtOff);
|
|
|
|
if ((uintptr_t)(p - page) >= AP_TRAMPOLINE_BOOT_OFF)
|
|
return STATUS_DEVICE_ERROR;
|
|
|
|
AP_BOOT_BLOCK* boot = (AP_BOOT_BLOCK*)(page + AP_TRAMPOLINE_BOOT_OFF);
|
|
memset(boot, 0, sizeof(*boot));
|
|
boot->Magic = AP_BOOT_MAGIC;
|
|
|
|
KiPatch16(page, gdtr16Patch, (uint16_t)gdtrOff);
|
|
KiPatch32(page, prot32Patch, physBase + prot32Off);
|
|
KiPatch32(page, cr3Patch, physBase + AP_TRAMPOLINE_BOOT_OFF + (uint32_t)offsetof(AP_BOOT_BLOCK, KernelCr3));
|
|
KiPatch32(page, long64Patch, physBase + long64Off);
|
|
KiPatch64(page, bootBlockPatch, (uint64_t)(physBase + AP_TRAMPOLINE_BOOT_OFF));
|
|
|
|
gApTrampolinePhys = frame;
|
|
gApBootBlock = boot;
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
_Noreturn void KiApKernelEntry(KCPU* cpu) {
|
|
_disable();
|
|
|
|
if (!cpu) {
|
|
while (1) __halt();
|
|
}
|
|
|
|
// APs start from the SIPI reset state, so do not assume the firmware/BSP
|
|
// CR4 feature bits are present. The scheduler and context-switch paths
|
|
// use FXSAVE/FXRSTOR on every CPU; without OSFXSR, those instructions
|
|
// raise #UD as soon as an AP enters its idle thread.
|
|
uintptr_t cr4 = __readcr4();
|
|
if ((cr4 & AP_CR4_REQUIRED) != AP_CR4_REQUIRED)
|
|
__writecr4(cr4 | AP_CR4_REQUIRED);
|
|
|
|
if (gApBootBlock) {
|
|
gApBootBlock->Stage = AP_STAGE_ENTRY;
|
|
gApBootBlock->State = AP_STATE_KERNEL_ENTRY;
|
|
gApBootBlock->Heartbeat++;
|
|
}
|
|
|
|
cpu->Self = cpu;
|
|
KeLoadKernelGsBase(cpu);
|
|
KeSetUserGsBase(0);
|
|
if (gApBootBlock) gApBootBlock->Stage = AP_STAGE_GS;
|
|
|
|
HalApicEnableCurrentCpu();
|
|
|
|
KeInitializeGdtForCpu(cpu);
|
|
KeLoadGdtForCurrentCpu(cpu);
|
|
if (gApBootBlock) gApBootBlock->Stage = AP_STAGE_GDT;
|
|
|
|
KeLoadIdtForCurrentCpu();
|
|
if (gApBootBlock) gApBootBlock->Stage = AP_STAGE_IDT;
|
|
|
|
cpu->Started = 1;
|
|
cpu->Parked = 1;
|
|
cpu->Online = 0;
|
|
cpu->CurrentIrql = PASSIVE_LEVEL;
|
|
cpu->InterruptDepth = 0;
|
|
cpu->ReschedulePending = 0;
|
|
|
|
if (gApBootBlock) {
|
|
gApBootBlock->Stage = AP_STAGE_PARK;
|
|
gApBootBlock->State = AP_STATE_KERNEL_PARKED;
|
|
}
|
|
|
|
for (;;) {
|
|
cpu->SchedulerTicks++;
|
|
|
|
if (_InterlockedCompareExchange(&cpu->SchedulerCommand, 0, 0) != 0) {
|
|
THREAD* idle = cpu->IdleThread;
|
|
if (idle) {
|
|
cpu->CurrentThread = idle;
|
|
idle->State = ThreadRunning;
|
|
idle->RunningCpu = cpu->CpuIndex;
|
|
idle->Queued = 0;
|
|
cpu->Parked = 0;
|
|
_InterlockedExchange(&cpu->SchedulerActive, 1);
|
|
_enable();
|
|
KeStartInitialThread(idle);
|
|
}
|
|
}
|
|
|
|
_mm_pause();
|
|
}
|
|
}
|
|
|
|
static void KiPrepareCpuRecord(uint32_t cpuIndex, const HAL_CPU_INFO* info) {
|
|
KCPU* cpu = &gCpus[cpuIndex];
|
|
memset(cpu, 0, sizeof(*cpu));
|
|
cpu->Self = cpu;
|
|
cpu->CpuIndex = cpuIndex;
|
|
cpu->LapicId = info->LapicId;
|
|
cpu->AcpiProcessorId = info->ProcessorUid;
|
|
cpu->BootProcessorIndex = cpuIndex;
|
|
cpu->Present = info->Enabled ? 1 : 0;
|
|
cpu->Started = 0;
|
|
cpu->Parked = 0;
|
|
cpu->Online = 0;
|
|
cpu->IsBsp = info->IsBsp ? 1 : 0;
|
|
cpu->CurrentIrql = PASSIVE_LEVEL;
|
|
cpu->InterruptDepth = 0;
|
|
cpu->ReschedulePending = 0;
|
|
cpu->SchedulerCommand = 0;
|
|
cpu->SchedulerActive = 0;
|
|
}
|
|
|
|
static KSTATUS KiAllocateApStack(KCPU* cpu) {
|
|
// AP kernel stacks do not need physically contiguous memory. Requiring
|
|
// MmAllocateContiguous(8) made SMP startup depend on a DMA-style run being
|
|
// available, which is fragile on real machines and unrelated to AP startup.
|
|
size_t stackBytes = (size_t)AP_STACK_PAGES * PAGE_SIZE;
|
|
void* stack = MmAllocatePool(stackBytes);
|
|
if (!stack) {
|
|
DbgPrint("SMP: AP%u stack allocation failed bytes=%llu\n",
|
|
cpu ? cpu->CpuIndex : 0xFFFFFFFFu, (uint64_t)stackBytes);
|
|
return STATUS_NO_MEMORY;
|
|
}
|
|
|
|
cpu->KernelStack = stack;
|
|
cpu->KernelStackTop = (uintptr_t)stack + stackBytes;
|
|
memset(cpu->KernelStack, 0, stackBytes);
|
|
memset(&cpu->Tss, 0, sizeof(cpu->Tss));
|
|
cpu->Tss.IoMapBase = sizeof(TSS64);
|
|
cpu->Tss.RSP0 = cpu->KernelStackTop;
|
|
KeInitializeGdtForCpu(cpu);
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
|
|
static const char* KiApStageName(uint32_t stage) {
|
|
switch (stage) {
|
|
case AP_STAGE_NONE: return "none";
|
|
case AP_STAGE_LONG64: return "long64";
|
|
case AP_STAGE_ENTRY: return "entry";
|
|
case AP_STAGE_GS: return "gs";
|
|
case AP_STAGE_GDT: return "gdt";
|
|
case AP_STAGE_IDT: return "idt";
|
|
case AP_STAGE_PARK: return "park";
|
|
default: return "unknown";
|
|
}
|
|
}
|
|
|
|
static void KiFillBootBlockForCpu(KCPU* cpu, uint32_t apIndex) {
|
|
memset(gApBootBlock, 0, sizeof(*gApBootBlock));
|
|
gApBootBlock->Magic = AP_BOOT_MAGIC;
|
|
gApBootBlock->KernelCr3 = (uint64_t)__readcr3();
|
|
gApBootBlock->LongModeEntry = (uint64_t)(uintptr_t)KiApKernelEntry;
|
|
gApBootBlock->Kcpu = (uint64_t)(uintptr_t)cpu;
|
|
gApBootBlock->StackTop = cpu->KernelStackTop;
|
|
gApBootBlock->State = AP_STATE_READY;
|
|
gApBootBlock->Stage = AP_STAGE_NONE;
|
|
gApBootBlock->ApIndex = apIndex;
|
|
}
|
|
|
|
KSTATUS KeStartApplicationProcessors(void) {
|
|
HAL_CPU_TOPOLOGY topo;
|
|
KSTATUS st = HalGetProcessorTopology(&topo);
|
|
if (!KSUCCESS(st)) return st;
|
|
|
|
if (topo.CpuCount == 0 || topo.CpuCount > KE_MAX_CPUS)
|
|
return STATUS_DEVICE_ERROR;
|
|
|
|
uint32_t bspLapic = HalGetBspLapicId();
|
|
KCPU* bsp = KeGetCurrentCpu();
|
|
bsp->LapicId = bspLapic;
|
|
bsp->IsBsp = 1;
|
|
bsp->Present = 1;
|
|
bsp->Started = 1;
|
|
bsp->Online = 1;
|
|
bsp->Parked = 0;
|
|
bsp->SchedulerCommand = 0;
|
|
bsp->SchedulerActive = 1;
|
|
|
|
DbgPrint("SMP: APIC mode=%s BSP LAPIC=%u MADT CPUs=%u\n",
|
|
HalApicUsingX2Apic() ? "x2APIC" : "xAPIC",
|
|
bspLapic, topo.CpuCount);
|
|
|
|
uint32_t count = 1;
|
|
for (uint32_t i = 0; i < topo.CpuCount && count < KE_MAX_CPUS; i++) {
|
|
HAL_CPU_INFO* info = &topo.Cpus[i];
|
|
DbgPrint("SMP: topology[%u] uid=%u lapic=%u flags=0x%x enabled=%u bsp=%u addressable=%u\n",
|
|
i, info->ProcessorUid, info->LapicId, info->Flags,
|
|
info->Enabled, info->IsBsp, HalApicCanAddressId(info->LapicId));
|
|
|
|
if (!info->Enabled) continue;
|
|
if (info->LapicId == bspLapic) {
|
|
bsp->AcpiProcessorId = info->ProcessorUid;
|
|
continue;
|
|
}
|
|
if (!HalApicCanAddressId(info->LapicId)) {
|
|
DbgPrint("SMP: skipping AP uid=%u lapic=%u; current APIC mode cannot address it\n",
|
|
info->ProcessorUid, info->LapicId);
|
|
continue;
|
|
}
|
|
KiPrepareCpuRecord(count, info);
|
|
count++;
|
|
}
|
|
gCpuCount = count;
|
|
|
|
DbgPrint("SMP: discovered %u addressable CPU(s), BSP LAPIC=%u\n", gCpuCount, bspLapic);
|
|
if (gCpuCount <= 1) {
|
|
DbgPrint("SMP: no application processors present\n");
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
if ((__readcr3() >> 32) != 0) {
|
|
DbgPrint("SMP: disabled; bootstrap CR3 is above 4GiB (cr3=0x%016llX), AP trampoline cannot load it yet\n",
|
|
(uint64_t)__readcr3());
|
|
gCpuCount = 1;
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
DbgPrint("SMP: bootloader AP trampoline phys=0x%016llX\n",
|
|
(uint64_t)gReservedApTrampolinePhys);
|
|
st = KiBuildApTrampoline();
|
|
if (!KSUCCESS(st)) {
|
|
DbgPrint("SMP: disabled; AP trampoline allocation/build failed status=0x%x\n", st);
|
|
gCpuCount = 1;
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
uint8_t sipiVector = (uint8_t)(gApTrampolinePhys >> 12);
|
|
uint32_t claimed = 0;
|
|
|
|
for (uint32_t i = 1; i < gCpuCount; i++) {
|
|
KCPU* cpu = &gCpus[i];
|
|
if (!cpu->Present) continue;
|
|
|
|
st = KiAllocateApStack(cpu);
|
|
if (!KSUCCESS(st)) {
|
|
DbgPrint("SMP: AP%u LAPIC=%u stack allocation failed status=0x%x; disabling AP and continuing\n",
|
|
i, cpu->LapicId, st);
|
|
cpu->Present = 0;
|
|
cpu->Started = 0;
|
|
cpu->Parked = 0;
|
|
cpu->Online = 0;
|
|
continue;
|
|
}
|
|
|
|
KiFillBootBlockForCpu(cpu, i);
|
|
DbgPrint("SMP: starting AP%u LAPIC=%u vector=0x%x\n", i, cpu->LapicId, sipiVector);
|
|
|
|
st = HalStartApplicationProcessor(cpu->LapicId, sipiVector);
|
|
if (!KSUCCESS(st)) {
|
|
DbgPrint("SMP: AP%u LAPIC=%u startup IPI failed status=0x%x; disabling AP and continuing\n",
|
|
i, cpu->LapicId, st);
|
|
cpu->Present = 0;
|
|
cpu->Started = 0;
|
|
cpu->Parked = 0;
|
|
cpu->Online = 0;
|
|
continue;
|
|
}
|
|
|
|
uint64_t lastHeartbeat = gApBootBlock->Heartbeat;
|
|
int parked = 0;
|
|
for (uint32_t spin = 0; spin < AP_START_TIMEOUT_SPINS; spin++) {
|
|
if (gApBootBlock->State == AP_STATE_KERNEL_PARKED) {
|
|
parked = 1;
|
|
break;
|
|
}
|
|
if ((spin & 0xFFFFu) == 0) {
|
|
uint64_t hb = gApBootBlock->Heartbeat;
|
|
lastHeartbeat = hb;
|
|
}
|
|
_mm_pause();
|
|
}
|
|
|
|
if (!parked) {
|
|
DbgPrint("SMP: AP%u failed to reach kernel park state lapic=%u state=%u stage=0x%x(%s) heartbeat=%llu last=%llu; disabling AP and continuing\n",
|
|
i, cpu->LapicId, gApBootBlock->State, gApBootBlock->Stage,
|
|
KiApStageName(gApBootBlock->Stage), gApBootBlock->Heartbeat, lastHeartbeat);
|
|
cpu->Present = 0;
|
|
cpu->Started = 0;
|
|
cpu->Parked = 0;
|
|
cpu->Online = 0;
|
|
continue;
|
|
}
|
|
|
|
cpu->Started = 1;
|
|
cpu->Parked = 1;
|
|
cpu->Online = 0;
|
|
claimed++;
|
|
DbgPrint("SMP: AP%u reached kernel park\n", i);
|
|
}
|
|
|
|
if (claimed == 0 && gCpuCount > 1)
|
|
DbgPrint("SMP: no APs reached kernel-owned state; continuing BSP-only\n");
|
|
|
|
DbgPrint("SMP: kernel claimed %u APs\n", claimed);
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
KSTATUS KeAdmitApplicationProcessorsToScheduler(void) {
|
|
if (gCpuCount <= 1) {
|
|
DbgPrint("SMP: no AP scheduler admission needed\n");
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
uint32_t admitted = 0;
|
|
for (uint32_t i = 1; i < gCpuCount; i++) {
|
|
KCPU* cpu = &gCpus[i];
|
|
if (!cpu->Present) continue;
|
|
|
|
if (!cpu->Started || !cpu->Parked || cpu->Online) {
|
|
KePanic(KE_PANIC_CODE_INIT,
|
|
"SMP: AP%u cannot enter scheduler P/S/Park/O=%u/%u/%u/%u\n",
|
|
i, cpu->Present, cpu->Started, cpu->Parked, cpu->Online);
|
|
}
|
|
|
|
KSTATUS st = KiCreateIdleThreadForCpu(cpu);
|
|
if (!KSUCCESS(st))
|
|
return st;
|
|
|
|
cpu->Online = 1;
|
|
_ReadWriteBarrier();
|
|
_InterlockedExchange(&cpu->SchedulerCommand, 1);
|
|
|
|
int active = 0;
|
|
for (uint32_t spin = 0; spin < AP_SCHED_TIMEOUT_SPINS; spin++) {
|
|
if (_InterlockedCompareExchange(&cpu->SchedulerActive, 0, 0) != 0) {
|
|
active = 1;
|
|
break;
|
|
}
|
|
_mm_pause();
|
|
}
|
|
|
|
if (!active) {
|
|
KePanic(KE_PANIC_CODE_INIT,
|
|
"SMP: AP%u failed scheduler admission lapic=%u parked=%u online=%u command=%ld active=%ld ticks=%llu\n",
|
|
i, cpu->LapicId, cpu->Parked, cpu->Online,
|
|
cpu->SchedulerCommand, cpu->SchedulerActive, cpu->SchedulerTicks);
|
|
}
|
|
|
|
admitted++;
|
|
DbgPrint("SMP: AP%u entered scheduler idle thread\n", i);
|
|
}
|
|
|
|
DbgPrint("SMP: scheduler admitted %u APs\n", admitted);
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
#define SMP_AP_WORKER_ROUNDS 64u
|
|
#define SMP_AP_WORKER_TIMEOUT_SPINS 4000000u
|
|
#define SMP_AP_TEST_MAGIC 0x524B575041504D53ULL /* 'SMPAPWKR' */
|
|
|
|
typedef struct SMP_AP_WORK_TEST SMP_AP_WORK_TEST;
|
|
|
|
typedef struct SMP_AP_WORKER_ARG {
|
|
uint64_t Magic;
|
|
uint32_t CpuIndex;
|
|
uint32_t WorkerIndex;
|
|
struct SMP_AP_WORK_TEST* Test;
|
|
EVENT* StepEvent;
|
|
volatile long Ready;
|
|
volatile long WaitArmed;
|
|
volatile long Progress;
|
|
volatile long Done;
|
|
volatile long Failure;
|
|
uint32_t WrongCpu;
|
|
uint32_t WrongRunningCpu;
|
|
} SMP_AP_WORKER_ARG;
|
|
|
|
struct SMP_AP_WORK_TEST {
|
|
uint64_t Magic;
|
|
uint32_t WorkerCount;
|
|
volatile long ReadyCount;
|
|
volatile long DoneCount;
|
|
volatile long FailureCount;
|
|
volatile long CpuHits[KE_MAX_CPUS];
|
|
SMP_AP_WORKER_ARG Workers[KE_MAX_CPUS];
|
|
};
|
|
|
|
static void KiSmpApControlledWorker(void* context) {
|
|
SMP_AP_WORKER_ARG* arg = (SMP_AP_WORKER_ARG*)context;
|
|
if (!arg || arg->Magic != SMP_AP_TEST_MAGIC || !arg->StepEvent) {
|
|
KePanic(KE_PANIC_CODE_SELFTEST, "SMP AP worker received bad context 0x%016llX\n", (uint64_t)arg);
|
|
}
|
|
|
|
_InterlockedExchange(&arg->Ready, 1);
|
|
_InterlockedIncrement(&arg->Test->ReadyCount);
|
|
|
|
for (uint32_t round = 0; round < SMP_AP_WORKER_ROUNDS; round++) {
|
|
_InterlockedIncrement(&arg->WaitArmed);
|
|
KSTATUS waitStatus = KeWaitForEvent(arg->StepEvent, 0);
|
|
if (!KSUCCESS(waitStatus)) {
|
|
_InterlockedExchange(&arg->Failure, 1);
|
|
break;
|
|
}
|
|
|
|
KCPU* cpu = KeGetCurrentCpu();
|
|
THREAD* self = KeGetCurrentThread();
|
|
if (!cpu || cpu->CpuIndex != arg->CpuIndex) {
|
|
arg->WrongCpu = cpu ? cpu->CpuIndex : KE_INVALID_CPU;
|
|
_InterlockedExchange(&arg->Failure, 1);
|
|
break;
|
|
}
|
|
if (!self || self->RunningCpu != cpu->CpuIndex) {
|
|
arg->WrongRunningCpu = self ? self->RunningCpu : KE_INVALID_CPU;
|
|
_InterlockedExchange(&arg->Failure, 1);
|
|
break;
|
|
}
|
|
if (self && (self->AffinityMask & (1ULL << cpu->CpuIndex)) == 0) {
|
|
arg->WrongCpu = cpu->CpuIndex;
|
|
_InterlockedExchange(&arg->Failure, 1);
|
|
break;
|
|
}
|
|
|
|
if (cpu->CpuIndex < KE_MAX_CPUS)
|
|
_InterlockedIncrement(&arg->Test->CpuHits[cpu->CpuIndex]);
|
|
|
|
_InterlockedIncrement(&arg->Progress);
|
|
if ((round & 3u) == 0)
|
|
KeYield();
|
|
}
|
|
|
|
if (_InterlockedCompareExchange(&arg->Failure, 0, 0) != 0) {
|
|
_InterlockedIncrement(&arg->Test->FailureCount);
|
|
}
|
|
|
|
_InterlockedExchange(&arg->Done, 1);
|
|
_InterlockedIncrement(&arg->Test->DoneCount);
|
|
}
|
|
|
|
static int KiWaitForWorkerCounter(volatile long* counter, long expected, const char* what) {
|
|
for (uint32_t spin = 0; spin < SMP_AP_WORKER_TIMEOUT_SPINS; spin++) {
|
|
if (_InterlockedCompareExchange(counter, 0, 0) >= expected)
|
|
return 1;
|
|
if ((spin & 0x3FFu) == 0)
|
|
KeYield();
|
|
else
|
|
_mm_pause();
|
|
}
|
|
DbgPrint("SMP scheduler selftest timeout waiting for %s expected=%ld got=%ld\n",
|
|
what ? what : "counter", expected, _InterlockedCompareExchange(counter, 0, 0));
|
|
return 0;
|
|
}
|
|
|
|
KSTATUS KeRunApplicationProcessorSchedulerSelfTests(void) {
|
|
if (gCpuCount <= 1) {
|
|
DbgPrint("SMP scheduler selftest: SKIP, no APs\n");
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
DbgPrint("SMP scheduler selftest: begin AP-affine workers\n");
|
|
|
|
SMP_AP_WORK_TEST* test = (SMP_AP_WORK_TEST*)MmAllocatePool(sizeof(SMP_AP_WORK_TEST));
|
|
if (!test) return STATUS_NO_MEMORY;
|
|
memset(test, 0, sizeof(*test));
|
|
test->Magic = SMP_AP_TEST_MAGIC;
|
|
|
|
uint32_t workerCount = 0;
|
|
for (uint32_t cpuIndex = 1; cpuIndex < gCpuCount; cpuIndex++) {
|
|
KCPU* cpu = &gCpus[cpuIndex];
|
|
if (!cpu->Present || !cpu->Started || !cpu->Online || cpu->Parked)
|
|
continue;
|
|
if (cpuIndex >= 64)
|
|
continue;
|
|
|
|
SMP_AP_WORKER_ARG* arg = &test->Workers[workerCount];
|
|
arg->Magic = SMP_AP_TEST_MAGIC;
|
|
arg->CpuIndex = cpuIndex;
|
|
arg->WorkerIndex = workerCount;
|
|
arg->Test = test;
|
|
|
|
KSTATUS st = KeEventCreate(EventAutoReset, 0, &arg->StepEvent);
|
|
if (!KSUCCESS(st)) {
|
|
KePanic(KE_PANIC_CODE_SELFTEST,
|
|
"SMP scheduler selftest: AP%u step event create failed status=0x%x\n",
|
|
cpuIndex, st);
|
|
}
|
|
|
|
THREAD* thread = (THREAD*)0;
|
|
st = KeCreateKernelThreadWithAffinity((uintptr_t)KiSmpApControlledWorker,
|
|
(uintptr_t)arg,
|
|
PriorityNormal,
|
|
(1ULL << cpuIndex),
|
|
&thread);
|
|
if (!KSUCCESS(st)) {
|
|
KePanic(KE_PANIC_CODE_SELFTEST,
|
|
"SMP scheduler selftest: AP%u worker create failed status=0x%x\n",
|
|
cpuIndex, st);
|
|
}
|
|
|
|
workerCount++;
|
|
}
|
|
test->WorkerCount = workerCount;
|
|
|
|
if (workerCount == 0) {
|
|
MmFreePool(test);
|
|
DbgPrint("SMP scheduler selftest: SKIP, no online AP workers\n");
|
|
return STATUS_SUCCESS;
|
|
}
|
|
|
|
if (!KiWaitForWorkerCounter(&test->ReadyCount, (long)workerCount, "AP worker ready")) {
|
|
KePanic(KE_PANIC_CODE_SELFTEST, "SMP scheduler selftest: workers failed to become ready ready=%ld workers=%u\n",
|
|
test->ReadyCount, workerCount);
|
|
}
|
|
|
|
for (uint32_t round = 0; round < SMP_AP_WORKER_ROUNDS; round++) {
|
|
for (uint32_t w = 0; w < workerCount; w++) {
|
|
SMP_AP_WORKER_ARG* arg = &test->Workers[w];
|
|
if (!KiWaitForWorkerCounter(&arg->WaitArmed, (long)(round + 1), "AP worker wait-armed")) {
|
|
KePanic(KE_PANIC_CODE_SELFTEST,
|
|
"SMP scheduler selftest: AP%u did not arm wait for round %u armed=%ld progress=%ld\n",
|
|
arg->CpuIndex, round, arg->WaitArmed, arg->Progress);
|
|
}
|
|
KeSignalEvent(arg->StepEvent);
|
|
}
|
|
|
|
for (uint32_t w = 0; w < workerCount; w++) {
|
|
SMP_AP_WORKER_ARG* arg = &test->Workers[w];
|
|
if (!KiWaitForWorkerCounter(&arg->Progress, (long)(round + 1), "AP worker progress")) {
|
|
KePanic(KE_PANIC_CODE_SELFTEST,
|
|
"SMP scheduler selftest: AP%u no progress round=%u progress=%ld failure=%ld wrongCpu=%u wrongRunningCpu=%u\n",
|
|
arg->CpuIndex, round, arg->Progress, arg->Failure,
|
|
arg->WrongCpu, arg->WrongRunningCpu);
|
|
}
|
|
if (_InterlockedCompareExchange(&arg->Failure, 0, 0) != 0) {
|
|
KePanic(KE_PANIC_CODE_SELFTEST,
|
|
"SMP scheduler selftest: AP%u worker failed round=%u wrongCpu=%u wrongRunningCpu=%u progress=%ld\n",
|
|
arg->CpuIndex, round, arg->WrongCpu, arg->WrongRunningCpu, arg->Progress);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!KiWaitForWorkerCounter(&test->DoneCount, (long)workerCount, "AP worker done")) {
|
|
KePanic(KE_PANIC_CODE_SELFTEST, "SMP scheduler selftest: workers did not finish done=%ld workers=%u\n",
|
|
test->DoneCount, workerCount);
|
|
}
|
|
|
|
if (_InterlockedCompareExchange(&test->FailureCount, 0, 0) != 0) {
|
|
KePanic(KE_PANIC_CODE_SELFTEST, "SMP scheduler selftest: failure count=%ld\n",
|
|
test->FailureCount);
|
|
}
|
|
|
|
for (uint32_t w = 0; w < workerCount; w++) {
|
|
SMP_AP_WORKER_ARG* arg = &test->Workers[w];
|
|
long hits = (arg->CpuIndex < KE_MAX_CPUS) ? test->CpuHits[arg->CpuIndex] : 0;
|
|
if (hits <= 0) {
|
|
KePanic(KE_PANIC_CODE_SELFTEST,
|
|
"SMP scheduler selftest: AP%u recorded no CPU hits\n", arg->CpuIndex);
|
|
}
|
|
if (arg->StepEvent)
|
|
ObDereferenceObject(&arg->StepEvent->Header);
|
|
}
|
|
|
|
DbgPrint("SMP scheduler selftest: PASS workers=%u rounds=%u\n",
|
|
workerCount, SMP_AP_WORKER_ROUNDS);
|
|
MmFreePool(test);
|
|
return STATUS_SUCCESS;
|
|
}
|