1046 lines
39 KiB
C
1046 lines
39 KiB
C
// SMP bring-up and CPU topology management. This file owns AP startup, CPU admission, and topology-derived scheduling metadata.
|
|
|
|
// 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>
|
|
#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_pfn.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;
|
|
|
|
// Implements the emit8 operation.
|
|
static void KiEmit8(uint8_t** p, uint8_t v) { *(*p)++ = v; }
|
|
// Implements the emit16 operation.
|
|
static void KiEmit16(uint8_t** p, uint16_t v) { memcpy(*p, &v, sizeof(v)); *p += sizeof(v); }
|
|
// Implements the emit32 operation.
|
|
static void KiEmit32(uint8_t** p, uint32_t v) { memcpy(*p, &v, sizeof(v)); *p += sizeof(v); }
|
|
// Implements the emit64 operation.
|
|
static void KiEmit64(uint8_t** p, uint64_t v) { memcpy(*p, &v, sizeof(v)); *p += sizeof(v); }
|
|
|
|
// Implements the patch16 operation.
|
|
static void KiPatch16(uint8_t* base, size_t off, uint16_t v) { memcpy(base + off, &v, sizeof(v)); }
|
|
// Implements the patch32 operation.
|
|
static void KiPatch32(uint8_t* base, size_t off, uint32_t v) { memcpy(base + off, &v, sizeof(v)); }
|
|
// Implements the patch64 operation.
|
|
static void KiPatch64(uint8_t* base, size_t off, uint64_t v) { memcpy(base + off, &v, sizeof(v)); }
|
|
|
|
// Implements the gdt Descriptor operation.
|
|
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);
|
|
}
|
|
|
|
// Updates application Processor Trampoline.
|
|
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;
|
|
}
|
|
}
|
|
|
|
// Builds ap Trampoline.
|
|
static KSTATUS KiBuildApTrampoline(void) {
|
|
if (gApTrampolinePhys)
|
|
return NTLSTATUS_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 = MmPfnAllocateInRange(AP_TRAMPOLINE_PREFERRED_MIN,
|
|
AP_TRAMPOLINE_MAX_PHYS,
|
|
MmPfnUseKernel);
|
|
}
|
|
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 = MmPfnAllocateInRange(AP_TRAMPOLINE_FALLBACK_MIN,
|
|
AP_TRAMPOLINE_PREFERRED_MIN,
|
|
MmPfnUseKernel);
|
|
}
|
|
if (!frame) {
|
|
DbgPrint("SMP: no free PMM frame available below 0x%016llX for AP trampoline\n",
|
|
(uint64_t)AP_TRAMPOLINE_MAX_PHYS);
|
|
return NTLSTATUS_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 NTLSTATUS_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 NTLSTATUS_SUCCESS;
|
|
}
|
|
|
|
_Noreturn void KiApKernelEntry(KCPU* cpu) {
|
|
(void)HalDisableInterrupts();
|
|
|
|
if (!cpu)
|
|
HalStopProcessor();
|
|
|
|
// 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;
|
|
|
|
// Do not arm the AP's periodic LAPIC timer while it is still in
|
|
// the raw park loop. Timer IRQs are scheduler-owned events;
|
|
// the AP may take them only after it has a current idle thread.
|
|
HalTimerConfigureCurrentCpu();
|
|
|
|
_InterlockedExchange(&cpu->SchedulerActive, 1);
|
|
HalEnableInterrupts();
|
|
KeStartInitialThread(idle);
|
|
}
|
|
}
|
|
|
|
// The BSP admits parked APs by storing SchedulerCommand and sending a
|
|
// wake IPI. Avoid the classic lost wakeup where the command appears
|
|
// between the poll and HLT: check once with interrupts disabled, then
|
|
// STI;HLT as a single sleep transition.
|
|
uintptr_t interruptFlags = HalDisableInterrupts();
|
|
if (_InterlockedCompareExchange(&cpu->SchedulerCommand, 0, 0) != 0) {
|
|
HalRestoreInterrupts(interruptFlags);
|
|
continue;
|
|
}
|
|
HalWaitForInterrupt();
|
|
}
|
|
}
|
|
|
|
// Prepares cpu Record.
|
|
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->PackageId = 0;
|
|
cpu->CoreId = cpuIndex;
|
|
cpu->ThreadId = 0;
|
|
cpu->PhysicalCoreIndex = cpuIndex;
|
|
cpu->CoreSiblingMask = (cpuIndex < 64) ? (1ULL << cpuIndex) : 0ULL;
|
|
cpu->PackageMask = cpu->CoreSiblingMask;
|
|
cpu->IsPrimaryThread = 1;
|
|
cpu->SmtThreadCount = 1;
|
|
cpu->TopologyFlags = 0;
|
|
cpu->CurrentIrql = PASSIVE_LEVEL;
|
|
cpu->InterruptDepth = 0;
|
|
cpu->ReschedulePending = 0;
|
|
cpu->SchedulerCommand = 0;
|
|
cpu->SchedulerActive = 0;
|
|
}
|
|
|
|
// Implements the ceil Log2 U32 operation.
|
|
static uint32_t KiCeilLog2U32(uint32_t value) {
|
|
if (value <= 1u)
|
|
return 0u;
|
|
uint32_t shift = 0;
|
|
uint32_t n = value - 1u;
|
|
while (n) {
|
|
shift++;
|
|
n >>= 1;
|
|
}
|
|
return shift;
|
|
}
|
|
|
|
// Implements the mask Width operation.
|
|
static uint32_t KiMaskWidth(uint32_t shift) {
|
|
if (shift >= 32u)
|
|
return 0xFFFFFFFFu;
|
|
if (shift == 0u)
|
|
return 0u;
|
|
return (1u << shift) - 1u;
|
|
}
|
|
|
|
typedef struct KI_CPUID_TOPOLOGY_PARAMS {
|
|
uint32_t SmtShift;
|
|
uint32_t CoreShift;
|
|
uint32_t ThreadsPerCore;
|
|
uint32_t LogicalPerPackage;
|
|
const char* Source;
|
|
} KI_CPUID_TOPOLOGY_PARAMS;
|
|
|
|
// Returns cpuid Topology Leaf.
|
|
static void KiReadCpuidTopologyLeaf(uint32_t leaf, KI_CPUID_TOPOLOGY_PARAMS* out) {
|
|
if (!out)
|
|
return;
|
|
|
|
for (uint32_t sub = 0; sub < 32u; sub++) {
|
|
int r[4] = {0, 0, 0, 0};
|
|
__cpuidex(r, (int)leaf, (int)sub);
|
|
|
|
uint32_t levelShift = (uint32_t)r[0] & 0x1Fu;
|
|
uint32_t logicals = (uint32_t)r[1] & 0xFFFFu;
|
|
uint32_t levelType = ((uint32_t)r[2] >> 8) & 0xFFu;
|
|
|
|
if (levelType == 0u || logicals == 0u)
|
|
break;
|
|
|
|
if (levelType == 1u) { // SMT level
|
|
out->SmtShift = levelShift;
|
|
out->ThreadsPerCore = logicals ? logicals : 1u;
|
|
} else if (levelType == 2u) { // Core/package level
|
|
out->CoreShift = levelShift;
|
|
out->LogicalPerPackage = logicals ? logicals : 1u;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Implements the detect Cpuid Topology Params operation.
|
|
static KI_CPUID_TOPOLOGY_PARAMS KiDetectCpuidTopologyParams(void) {
|
|
KI_CPUID_TOPOLOGY_PARAMS topo;
|
|
memset(&topo, 0, sizeof(topo));
|
|
topo.ThreadsPerCore = 1;
|
|
topo.LogicalPerPackage = 1;
|
|
topo.Source = "flat";
|
|
|
|
int maxRegs[4] = {0, 0, 0, 0};
|
|
__cpuid(maxRegs, 0);
|
|
uint32_t maxLeaf = (uint32_t)maxRegs[0];
|
|
|
|
if (maxLeaf >= 0x1Fu) {
|
|
KiReadCpuidTopologyLeaf(0x1Fu, &topo);
|
|
if (topo.CoreShift >= topo.SmtShift && topo.CoreShift != 0u) {
|
|
topo.Source = "cpuid.1f";
|
|
return topo;
|
|
}
|
|
memset(&topo, 0, sizeof(topo));
|
|
topo.ThreadsPerCore = 1;
|
|
topo.LogicalPerPackage = 1;
|
|
topo.Source = "flat";
|
|
}
|
|
|
|
if (maxLeaf >= 0x0Bu) {
|
|
KiReadCpuidTopologyLeaf(0x0Bu, &topo);
|
|
if (topo.CoreShift >= topo.SmtShift && topo.CoreShift != 0u) {
|
|
topo.Source = "cpuid.0b";
|
|
return topo;
|
|
}
|
|
memset(&topo, 0, sizeof(topo));
|
|
topo.ThreadsPerCore = 1;
|
|
topo.LogicalPerPackage = 1;
|
|
topo.Source = "flat";
|
|
}
|
|
|
|
int r1[4] = {0, 0, 0, 0};
|
|
__cpuid(r1, 1);
|
|
uint32_t logicalPerPackage = ((uint32_t)r1[1] >> 16) & 0xFFu;
|
|
if (logicalPerPackage == 0u)
|
|
logicalPerPackage = 1u;
|
|
|
|
uint32_t coresPerPackage = 1u;
|
|
if (maxLeaf >= 4u) {
|
|
int r4[4] = {0, 0, 0, 0};
|
|
__cpuidex(r4, 4, 0);
|
|
coresPerPackage = (((uint32_t)r4[0] >> 26) & 0x3Fu) + 1u;
|
|
if (coresPerPackage == 0u)
|
|
coresPerPackage = 1u;
|
|
}
|
|
|
|
uint32_t threadsPerCore = logicalPerPackage / coresPerPackage;
|
|
if (threadsPerCore == 0u)
|
|
threadsPerCore = 1u;
|
|
|
|
topo.ThreadsPerCore = threadsPerCore;
|
|
topo.LogicalPerPackage = logicalPerPackage;
|
|
topo.SmtShift = KiCeilLog2U32(threadsPerCore);
|
|
topo.CoreShift = KiCeilLog2U32(logicalPerPackage);
|
|
if (topo.CoreShift < topo.SmtShift)
|
|
topo.CoreShift = topo.SmtShift;
|
|
topo.Source = "cpuid.1/4";
|
|
return topo;
|
|
}
|
|
|
|
// Builds processor Topology.
|
|
void KeBuildProcessorTopology(void) {
|
|
KI_CPUID_TOPOLOGY_PARAMS topo = KiDetectCpuidTopologyParams();
|
|
|
|
uint32_t smtMask = KiMaskWidth(topo.SmtShift);
|
|
uint32_t coreBits = (topo.CoreShift > topo.SmtShift) ?
|
|
(topo.CoreShift - topo.SmtShift) : 0u;
|
|
uint32_t coreMask = KiMaskWidth(coreBits);
|
|
|
|
for (uint32_t i = 0; i < gCpuCount && i < KE_MAX_CPUS; i++) {
|
|
KCPU* cpu = &gCpus[i];
|
|
if (!cpu->Present)
|
|
continue;
|
|
|
|
uint32_t apic = cpu->LapicId;
|
|
cpu->ThreadId = smtMask ? (apic & smtMask) : 0u;
|
|
cpu->CoreId = coreBits ? ((apic >> topo.SmtShift) & coreMask) :
|
|
(apic >> topo.SmtShift);
|
|
cpu->PackageId = (topo.CoreShift < 32u) ? (apic >> topo.CoreShift) : 0u;
|
|
cpu->PhysicalCoreIndex = KE_INVALID_CPU;
|
|
cpu->CoreSiblingMask = 0;
|
|
cpu->PackageMask = 0;
|
|
cpu->IsPrimaryThread = 0;
|
|
cpu->SmtThreadCount = 1;
|
|
cpu->TopologyFlags = (topo.ThreadsPerCore > 1u) ? 1u : 0u;
|
|
}
|
|
|
|
uint32_t nextCoreIndex = 0;
|
|
for (uint32_t i = 0; i < gCpuCount && i < KE_MAX_CPUS; i++) {
|
|
KCPU* cpu = &gCpus[i];
|
|
if (!cpu->Present)
|
|
continue;
|
|
if (cpu->PhysicalCoreIndex != KE_INVALID_CPU)
|
|
continue;
|
|
|
|
uint32_t coreIndex = nextCoreIndex++;
|
|
uint64_t coreMaskBits = 0;
|
|
uint64_t packageMaskBits = 0;
|
|
uint32_t primaryCpuIndex = i;
|
|
uint32_t siblingCount = 0;
|
|
|
|
for (uint32_t j = 0; j < gCpuCount && j < KE_MAX_CPUS; j++) {
|
|
KCPU* peer = &gCpus[j];
|
|
if (!peer->Present)
|
|
continue;
|
|
if (peer->PackageId == cpu->PackageId) {
|
|
if (j < 64)
|
|
packageMaskBits |= (1ULL << j);
|
|
}
|
|
if (peer->PackageId == cpu->PackageId && peer->CoreId == cpu->CoreId) {
|
|
peer->PhysicalCoreIndex = coreIndex;
|
|
if (j < 64)
|
|
coreMaskBits |= (1ULL << j);
|
|
if (j < primaryCpuIndex)
|
|
primaryCpuIndex = j;
|
|
siblingCount++;
|
|
}
|
|
}
|
|
|
|
if (siblingCount == 0)
|
|
siblingCount = 1;
|
|
|
|
for (uint32_t j = 0; j < gCpuCount && j < KE_MAX_CPUS; j++) {
|
|
KCPU* peer = &gCpus[j];
|
|
if (!peer->Present)
|
|
continue;
|
|
if (peer->PackageId == cpu->PackageId && peer->CoreId == cpu->CoreId) {
|
|
peer->CoreSiblingMask = coreMaskBits;
|
|
peer->PackageMask = packageMaskBits;
|
|
peer->SmtThreadCount = (uint8_t)((siblingCount > 255u) ? 255u : siblingCount);
|
|
peer->IsPrimaryThread = (j == primaryCpuIndex) ? 1u : 0u;
|
|
}
|
|
}
|
|
}
|
|
|
|
DbgPrint("SMP: SMT topology source=%s smtShift=%u coreShift=%u threads/core=%u logical/package=%u physicalCores=%u\n",
|
|
topo.Source ? topo.Source : "?",
|
|
topo.SmtShift,
|
|
topo.CoreShift,
|
|
topo.ThreadsPerCore,
|
|
topo.LogicalPerPackage,
|
|
nextCoreIndex);
|
|
|
|
for (uint32_t i = 0; i < gCpuCount && i < KE_MAX_CPUS; i++) {
|
|
KCPU* cpu = &gCpus[i];
|
|
if (!cpu->Present)
|
|
continue;
|
|
DbgPrint("SMP: cpu%u lapic=%u pkg=%u core=%u thread=%u primary=%u siblings=0x%llx package=0x%llx\n",
|
|
i,
|
|
cpu->LapicId,
|
|
cpu->PackageId,
|
|
cpu->CoreId,
|
|
cpu->ThreadId,
|
|
cpu->IsPrimaryThread,
|
|
(unsigned long long)cpu->CoreSiblingMask,
|
|
(unsigned long long)cpu->PackageMask);
|
|
}
|
|
}
|
|
|
|
// Creates ap Stack.
|
|
static KSTATUS KiAllocateApStack(KCPU* cpu) {
|
|
// AP kernel stacks do not need physically contiguous memory. Requiring
|
|
// MmPfnAllocateContiguous(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 NTLSTATUS_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 NTLSTATUS_SUCCESS;
|
|
}
|
|
|
|
// Implements the ap Stage Name operation.
|
|
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";
|
|
}
|
|
}
|
|
|
|
// Fills boot Block For Cpu.
|
|
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;
|
|
}
|
|
|
|
// Starts application Processors.
|
|
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 NTLSTATUS_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 NTLSTATUS_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 NTLSTATUS_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 NTLSTATUS_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");
|
|
|
|
KeBuildProcessorTopology();
|
|
DbgPrint("SMP: kernel claimed %u APs\n", claimed);
|
|
return NTLSTATUS_SUCCESS;
|
|
}
|
|
|
|
// Implements the admit Application Processors To Scheduler operation.
|
|
KSTATUS KeAdmitApplicationProcessorsToScheduler(void) {
|
|
if (gCpuCount <= 1) {
|
|
DbgPrint("SMP: no AP scheduler admission needed\n");
|
|
return NTLSTATUS_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);
|
|
|
|
// Parked APs sleep with HLT and have no LAPIC timer armed yet. The
|
|
// command store alone is not sufficient; send a fixed-vector IPI only
|
|
// to wake the parked loop. ReschedulePending is intentionally left
|
|
// clear, so the interrupt exit path will not try to schedule before
|
|
// the AP has installed its idle thread.
|
|
HalApicSendRescheduleIpi(cpu->LapicId);
|
|
|
|
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 NTLSTATUS_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];
|
|
};
|
|
|
|
// Implements the smp Ap Controlled Worker operation.
|
|
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);
|
|
}
|
|
|
|
// Waits for for Worker Counter.
|
|
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;
|
|
}
|
|
|
|
// Runs application Processor Scheduler Self Tests.
|
|
KSTATUS KeRunApplicationProcessorSchedulerSelfTests(void) {
|
|
if (gCpuCount <= 1) {
|
|
DbgPrint("SMP scheduler selftest: SKIP, no APs\n");
|
|
return NTLSTATUS_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 NTLSTATUS_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);
|
|
}
|
|
|
|
st = KeCreateKernelThreadWithAffinity((uintptr_t)KiSmpApControlledWorker,
|
|
(uintptr_t)arg,
|
|
PriorityNormal,
|
|
(1ULL << cpuIndex),
|
|
(THREAD**)0);
|
|
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 NTLSTATUS_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 NTLSTATUS_SUCCESS;
|
|
}
|