// 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 #include #include #include #pragma intrinsic(_disable) #pragma intrinsic(_enable) #include #include #include #include #include #include #include #include #include #include #include #include #include #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 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 = 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 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; // 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); _enable(); 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. _disable(); if (_InterlockedCompareExchange(&cpu->SchedulerCommand, 0, 0) != 0) { _enable(); continue; } _enable(); __halt(); } } // 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 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; } // 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 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"); KeBuildProcessorTopology(); DbgPrint("SMP: kernel claimed %u APs\n", claimed); return STATUS_SUCCESS; } // Implements the admit Application Processors To Scheduler operation. 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); // 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 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]; }; // 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 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); } 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 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; }