usb hubs, hotplug support, unplug support, saving log files to disk, smt
This commit is contained in:
1 parent
b838f8d83d
commit
27b31f62b6
39 files changed
+6294
-735
No files matched your search
+202
-70
@@ -82,11 +82,13 @@ static uintptr_t gIoapicPhys = 0xFEC00000;
|
||||
static uintptr_t gHpetPhys = 0xFED00000;
|
||||
static HAL_CPU_TOPOLOGY gCpuTopology;
|
||||
|
||||
// ACPI power-off state (filled in by ParseFadt / ScanDsdtForS5)
|
||||
// ACPI power-off state (filled in by ParseFadt / ScanDsdtForS5).
|
||||
// Do not guess SLP_TYP on real hardware: these values are firmware-specific.
|
||||
static uint16_t gPm1aCntBlk = 0;
|
||||
static uint16_t gPm1bCntBlk = 0;
|
||||
static uint8_t gSlpTypA = 5; // S5 SLP_TYP for PM1a; 5 is correct for QEMU/most VMs
|
||||
static uint8_t gSlpTypB = 5; // S5 SLP_TYP for PM1b
|
||||
static uint8_t gSlpTypA = 0;
|
||||
static uint8_t gSlpTypB = 0;
|
||||
static int gS5Found = 0;
|
||||
|
||||
int debugEnabled = 0;
|
||||
int _fltused; // MSVC floating-point helper anchor
|
||||
@@ -106,73 +108,183 @@ static void *HalPhysToVirt(uint64_t phys) {
|
||||
return (void *)(phys + gPhysBase);
|
||||
}
|
||||
|
||||
// FADT (Fixed ACPI Description Table) - partial layout up to PM1b_CNT_BLK.
|
||||
// We only need offsets 40 (DSDT ptr), 64 (PM1a_CNT_BLK), 68 (PM1b_CNT_BLK).
|
||||
// The full table is ~276 bytes (ACPI 2.0+); we never access beyond offset 68.
|
||||
// FADT (Fixed ACPI Description Table) offsets used here. ACPI 2.0+
|
||||
// systems may put the real DSDT and PM1 control registers in the extended
|
||||
// X_* fields, so do not rely only on the legacy 32-bit fields.
|
||||
#define FADT_DSDT_OFFSET 40u
|
||||
#define FADT_PM1A_CNT_BLK_OFFSET 64u
|
||||
#define FADT_PM1B_CNT_BLK_OFFSET 68u
|
||||
#define FADT_X_DSDT_OFFSET 140u
|
||||
#define FADT_X_PM1A_CNT_BLK_OFFSET 172u
|
||||
#define FADT_X_PM1B_CNT_BLK_OFFSET 184u
|
||||
|
||||
#pragma pack(push, 1)
|
||||
typedef struct {
|
||||
ACPISDTHeader Header; // 0 (36 bytes)
|
||||
uint32_t FirmwareCtrl; // 36
|
||||
uint32_t Dsdt; // 40 - physical addr of DSDT
|
||||
uint8_t Ignored44; // 44
|
||||
uint8_t PreferredPmProfile; // 45
|
||||
uint16_t SciInt; // 46
|
||||
uint32_t SmiCmd; // 48
|
||||
uint8_t AcpiEnable; // 52
|
||||
uint8_t AcpiDisable; // 53
|
||||
uint8_t S4BiosReq; // 54
|
||||
uint8_t PstateCnt; // 55
|
||||
uint32_t Pm1aEvtBlk; // 56
|
||||
uint32_t Pm1bEvtBlk; // 60
|
||||
uint32_t Pm1aCntBlk; // 64 - I/O port for PM1a control register
|
||||
uint32_t Pm1bCntBlk; // 68 - I/O port for PM1b control register
|
||||
} FADT;
|
||||
uint8_t AddressSpaceId;
|
||||
uint8_t RegisterBitWidth;
|
||||
uint8_t RegisterBitOffset;
|
||||
uint8_t AccessSize;
|
||||
uint64_t Address;
|
||||
} ACPI_GAS;
|
||||
#pragma pack(pop)
|
||||
|
||||
// Scan DSDT AML for the _S5_ Name object and extract SLP_TYP values.
|
||||
// The encoding looks like:
|
||||
// 0x08 '_' 'S' '5' '_' ; NameOp + "_S5_"
|
||||
// 0x12 <PkgLen> ; PackageOp + encoded length
|
||||
// <NumElements> ; usually 0x04
|
||||
// 0x0A <slp_typ_a> ; BytePrefix + value (or 0x00 for zero)
|
||||
// 0x0A <slp_typ_b>
|
||||
static void ScanDsdtForS5(const uint8_t* aml, uint32_t len) {
|
||||
for (uint32_t i = 0; i + 9 < len; i++) {
|
||||
// Match NameOp + "_S5_" + PackageOp
|
||||
if (aml[i] != 0x08) continue;
|
||||
if (aml[i+1] != 0x5F || aml[i+2] != 0x53 ||
|
||||
aml[i+3] != 0x35 || aml[i+4] != 0x5F) continue;
|
||||
if (aml[i+5] != 0x12) continue;
|
||||
static uint32_t AcpiReadU32(const void* table, uint32_t tableLen, uint32_t off) {
|
||||
uint32_t value = 0;
|
||||
if (off + sizeof(value) <= tableLen)
|
||||
memcpy(&value, (const uint8_t*)table + off, sizeof(value));
|
||||
return value;
|
||||
}
|
||||
|
||||
// PkgLength encoding: low 6 bits = byte count (0 → only this byte);
|
||||
// bits [7:6] = number of following bytes in the length field.
|
||||
uint8_t pkgByte = aml[i+6];
|
||||
uint32_t pkgExtra = pkgByte >> 6; // 0..3 extra bytes
|
||||
uint32_t j = i + 7 + pkgExtra; // skip PkgLen field
|
||||
if (j + 2 >= len) break;
|
||||
j++; // skip NumElements byte
|
||||
static uint64_t AcpiReadU64(const void* table, uint32_t tableLen, uint32_t off) {
|
||||
uint64_t value = 0;
|
||||
if (off + sizeof(value) <= tableLen)
|
||||
memcpy(&value, (const uint8_t*)table + off, sizeof(value));
|
||||
return value;
|
||||
}
|
||||
|
||||
// SLP_TYP_A
|
||||
if (aml[j] == 0x0A && j + 1 < len) {
|
||||
gSlpTypA = aml[j + 1]; j += 2;
|
||||
} else if (aml[j] == 0x00) {
|
||||
gSlpTypA = 0; j++;
|
||||
} else {
|
||||
break; // unexpected encoding - keep defaults
|
||||
static uint16_t AcpiReadGasIoPort(const void* table, uint32_t tableLen, uint32_t off) {
|
||||
ACPI_GAS gas;
|
||||
if (off + sizeof(gas) > tableLen) return 0;
|
||||
|
||||
memcpy(&gas, (const uint8_t*)table + off, sizeof(gas));
|
||||
|
||||
// 0x01 == System I/O. HalShutdown currently writes PM1_CNT with outw(),
|
||||
// so ignore MMIO GAS entries until the HAL grows MMIO register access here.
|
||||
if (gas.AddressSpaceId != 0x01 || gas.Address == 0 || gas.Address > 0xFFFF)
|
||||
return 0;
|
||||
|
||||
return (uint16_t)gas.Address;
|
||||
}
|
||||
|
||||
static int AmlParsePkgLength(const uint8_t* p, const uint8_t* end,
|
||||
const uint8_t** body, const uint8_t** pkgEnd) {
|
||||
if (p >= end) return 0;
|
||||
|
||||
const uint8_t* start = p;
|
||||
uint8_t first = *p++;
|
||||
uint32_t follow = first >> 6;
|
||||
uint32_t length;
|
||||
|
||||
if (follow == 0) {
|
||||
length = first & 0x3F;
|
||||
} else {
|
||||
length = first & 0x0F;
|
||||
for (uint32_t i = 0; i < follow; i++) {
|
||||
if (p >= end) return 0;
|
||||
length |= ((uint32_t)*p++) << (4 + i * 8);
|
||||
}
|
||||
}
|
||||
|
||||
// SLP_TYP_B
|
||||
if (j < len && aml[j] == 0x0A && j + 1 < len)
|
||||
gSlpTypB = aml[j + 1];
|
||||
else if (j < len && aml[j] == 0x00)
|
||||
gSlpTypB = 0;
|
||||
if (start + length < p || start + length > end) return 0;
|
||||
*body = p;
|
||||
*pkgEnd = start + length;
|
||||
return 1;
|
||||
}
|
||||
|
||||
static const uint8_t* AmlParseNameString(const uint8_t* p, const uint8_t* end,
|
||||
int* lastSegmentIsS5) {
|
||||
uint32_t segments = 1;
|
||||
*lastSegmentIsS5 = 0;
|
||||
|
||||
// Optional root or parent prefixes: '\\', '^', '^^', ...
|
||||
while (p < end && (*p == 0x5C || *p == 0x5E)) p++;
|
||||
if (p >= end) return NULL;
|
||||
|
||||
if (*p == 0x00) { // NullName
|
||||
return p + 1;
|
||||
} else if (*p == 0x2E) { // DualNamePrefix
|
||||
p++;
|
||||
segments = 2;
|
||||
} else if (*p == 0x2F) { // MultiNamePrefix
|
||||
p++;
|
||||
if (p >= end) return NULL;
|
||||
segments = *p++;
|
||||
}
|
||||
|
||||
for (uint32_t s = 0; s < segments; s++) {
|
||||
if (p + 4 > end) return NULL;
|
||||
if (s == segments - 1 && memcmp(p, "_S5_", 4) == 0)
|
||||
*lastSegmentIsS5 = 1;
|
||||
p += 4;
|
||||
}
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
static int AmlReadInteger(const uint8_t** pp, const uint8_t* end, uint8_t* out) {
|
||||
const uint8_t* p = *pp;
|
||||
if (p >= end) return 0;
|
||||
|
||||
switch (*p++) {
|
||||
case 0x00: // ZeroOp
|
||||
*out = 0;
|
||||
break;
|
||||
case 0x01: // OneOp
|
||||
*out = 1;
|
||||
break;
|
||||
case 0x0A: // BytePrefix
|
||||
if (p >= end) return 0;
|
||||
*out = *p++;
|
||||
break;
|
||||
case 0x0B: // WordPrefix
|
||||
if (p + 2 > end) return 0;
|
||||
*out = p[0];
|
||||
p += 2;
|
||||
break;
|
||||
case 0x0C: // DWordPrefix
|
||||
if (p + 4 > end) return 0;
|
||||
*out = p[0];
|
||||
p += 4;
|
||||
break;
|
||||
case 0x0E: // QWordPrefix
|
||||
if (p + 8 > end) return 0;
|
||||
*out = p[0];
|
||||
p += 8;
|
||||
break;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
|
||||
*pp = p;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Scan DSDT AML for the _S5_ Name object and extract SLP_TYP values. Real
|
||||
// firmware commonly encodes the name as "\\_S5_" instead of just "_S5_", and
|
||||
// package integers may be ZeroOp/OneOp/Byte/Word/DWord/QWord constants.
|
||||
static void ScanDsdtForS5(const uint8_t* aml, uint32_t len) {
|
||||
const uint8_t* end = aml + len;
|
||||
|
||||
for (const uint8_t* cur = aml; cur < end; cur++) {
|
||||
if (*cur != 0x08) continue; // NameOp
|
||||
|
||||
const uint8_t* p = cur + 1;
|
||||
int isS5 = 0;
|
||||
p = AmlParseNameString(p, end, &isS5);
|
||||
if (!p || !isS5 || p >= end || *p != 0x12) continue; // PackageOp
|
||||
|
||||
const uint8_t* body;
|
||||
const uint8_t* pkgEnd;
|
||||
if (!AmlParsePkgLength(p + 1, end, &body, &pkgEnd)) continue;
|
||||
if (body >= pkgEnd) continue;
|
||||
|
||||
uint8_t elements = *body++;
|
||||
if (elements < 2) continue;
|
||||
|
||||
uint8_t typA, typB;
|
||||
if (!AmlReadInteger(&body, pkgEnd, &typA)) continue;
|
||||
if (!AmlReadInteger(&body, pkgEnd, &typB)) continue;
|
||||
|
||||
gSlpTypA = typA & 0x07;
|
||||
gSlpTypB = typB & 0x07;
|
||||
gS5Found = 1;
|
||||
|
||||
HalDbgPrint("HAL: _S5_ SLP_TYP_A=0x%02x SLP_TYP_B=0x%02x\n",
|
||||
gSlpTypA, gSlpTypB);
|
||||
return;
|
||||
}
|
||||
HalDbgPrint("HAL: _S5_ not found in DSDT, using defaults (A=0x%02x B=0x%02x)\n",
|
||||
gSlpTypA, gSlpTypB);
|
||||
|
||||
HalDbgPrint("HAL: _S5_ not found in DSDT; ACPI shutdown disabled\n");
|
||||
}
|
||||
|
||||
static int validate_checksum(void* table, uint32_t length) {
|
||||
@@ -184,19 +296,33 @@ static int validate_checksum(void* table, uint32_t length) {
|
||||
return sum == 0;
|
||||
}
|
||||
|
||||
static void ParseFadt(const FADT* fadt) {
|
||||
gPm1aCntBlk = (uint16_t)fadt->Pm1aCntBlk;
|
||||
gPm1bCntBlk = (uint16_t)fadt->Pm1bCntBlk;
|
||||
HalDbgPrint("HAL: FADT PM1a_CNT=0x%04x PM1b_CNT=0x%04x DSDT=0x%08x\n",
|
||||
gPm1aCntBlk, gPm1bCntBlk, fadt->Dsdt);
|
||||
static void ParseFadt(const ACPISDTHeader* header) {
|
||||
const void* fadt = header;
|
||||
uint32_t len = header->Length;
|
||||
|
||||
if (fadt->Dsdt) {
|
||||
const ACPISDTHeader* dsdt =
|
||||
(const ACPISDTHeader*)((uintptr_t)fadt->Dsdt + gPhysBase);
|
||||
gPm1aCntBlk = (uint16_t)AcpiReadU32(fadt, len, FADT_PM1A_CNT_BLK_OFFSET);
|
||||
gPm1bCntBlk = (uint16_t)AcpiReadU32(fadt, len, FADT_PM1B_CNT_BLK_OFFSET);
|
||||
|
||||
if (!gPm1aCntBlk)
|
||||
gPm1aCntBlk = AcpiReadGasIoPort(fadt, len, FADT_X_PM1A_CNT_BLK_OFFSET);
|
||||
if (!gPm1bCntBlk)
|
||||
gPm1bCntBlk = AcpiReadGasIoPort(fadt, len, FADT_X_PM1B_CNT_BLK_OFFSET);
|
||||
|
||||
uint64_t dsdtPhys = AcpiReadU64(fadt, len, FADT_X_DSDT_OFFSET);
|
||||
if (!dsdtPhys)
|
||||
dsdtPhys = AcpiReadU32(fadt, len, FADT_DSDT_OFFSET);
|
||||
|
||||
HalDbgPrint("HAL: FADT PM1a_CNT=0x%04x PM1b_CNT=0x%04x DSDT=0x%016llx\n",
|
||||
gPm1aCntBlk, gPm1bCntBlk, dsdtPhys);
|
||||
|
||||
if (dsdtPhys) {
|
||||
const ACPISDTHeader* dsdt = (const ACPISDTHeader*)(dsdtPhys + gPhysBase);
|
||||
if (memcmp(dsdt->Signature, "DSDT", 4) == 0 &&
|
||||
validate_checksum((void*)dsdt, dsdt->Length)) {
|
||||
const uint8_t* aml = (const uint8_t*)(dsdt + 1);
|
||||
ScanDsdtForS5(aml, dsdt->Length - (uint32_t)sizeof(ACPISDTHeader));
|
||||
} else {
|
||||
HalDbgPrint("HAL: DSDT invalid or checksum failed\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -319,7 +445,7 @@ static void WalkAcpi(uintptr_t rsdpPhys) {
|
||||
}
|
||||
// Parse FADT (signature "FACP") for PM1 control ports and DSDT pointer
|
||||
else if (memcmp(header->Signature, "FACP", 4) == 0) {
|
||||
ParseFadt((const FADT*)header);
|
||||
ParseFadt(header);
|
||||
}
|
||||
// Parse HPET
|
||||
else if (memcmp(header->Signature, "HPET", 4) == 0) {
|
||||
@@ -337,11 +463,17 @@ static void HalShutdown(void) {
|
||||
// Silence! Stop the world!
|
||||
_disable();
|
||||
|
||||
// SLP_EN (bit 13) | SLP_TYPx (bits 12:10)
|
||||
uint16_t valA = (uint16_t)((gSlpTypA << 10) | (1 << 13));
|
||||
uint16_t valB = (uint16_t)((gSlpTypB << 10) | (1 << 13));
|
||||
if (!gS5Found || !gPm1aCntBlk) {
|
||||
// Refuse to guess. Guessing SLP_TYP is exactly how real hardware gets
|
||||
// put into S3/S4 sleep instead of S5 soft-off.
|
||||
while (true) __halt();
|
||||
}
|
||||
|
||||
if (gPm1aCntBlk) __outword(gPm1aCntBlk, valA);
|
||||
// SLP_EN (bit 13) | SLP_TYPx (bits 12:10)
|
||||
uint16_t valA = (uint16_t)(((uint16_t)gSlpTypA << 10) | (1 << 13));
|
||||
uint16_t valB = (uint16_t)(((uint16_t)gSlpTypB << 10) | (1 << 13));
|
||||
|
||||
__outword(gPm1aCntBlk, valA);
|
||||
if (gPm1bCntBlk) __outword(gPm1bCntBlk, valB);
|
||||
|
||||
// Die!
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
// Kernel and user code open files through IoOpenFile.
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <stdbool.h>
|
||||
#include <ntlite/status.h>
|
||||
#include <kernel/ob_object.h>
|
||||
#include <kernel/ke_spinlock.h>
|
||||
@@ -143,6 +144,9 @@ typedef struct IO_VOLUME_PARAMS {
|
||||
// ---------------------------------------------------------------------------
|
||||
// driveIndex matches the index passed to IoRegisterDiskDevice.
|
||||
__declspec(dllexport) KSTATUS IoRegisterFilesystem(uint8_t driveIndex, IO_FILESYSTEM* fs, const IO_VOLUME_PARAMS *vol);
|
||||
__declspec(dllexport) KSTATUS IoUnregisterFilesystem(uint8_t driveIndex);
|
||||
__declspec(dllexport) KSTATUS IoUnregisterFilesystemsForDisk(uint8_t diskIndex);
|
||||
__declspec(dllexport) bool IoFilesystemPresent(uint8_t driveIndex);
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Kernel-side API
|
||||
|
||||
@@ -30,6 +30,24 @@ typedef struct KCPU {
|
||||
uint8_t Online;
|
||||
uint8_t IsBsp;
|
||||
|
||||
// CPUID/MADT-derived SMT topology. PackageId/CoreId/ThreadId are decoded
|
||||
// from the APIC ID using CPUID leaf 0x1F/0x0B when available, with a
|
||||
// conservative CPUID(1)/CPUID(4) fallback. PhysicalCoreIndex is a compact
|
||||
// kernel-local core number used by the scheduler. CoreSiblingMask contains
|
||||
// all logical CPUs that share this physical core. PackageMask contains all
|
||||
// logical CPUs in the same package. IsPrimaryThread is the scheduler's
|
||||
// preferred logical CPU for a physical core, currently the lowest CpuIndex
|
||||
// in that core.
|
||||
uint32_t PackageId;
|
||||
uint32_t CoreId;
|
||||
uint32_t ThreadId;
|
||||
uint32_t PhysicalCoreIndex;
|
||||
uint64_t CoreSiblingMask;
|
||||
uint64_t PackageMask;
|
||||
uint8_t IsPrimaryThread;
|
||||
uint8_t SmtThreadCount;
|
||||
uint16_t TopologyFlags;
|
||||
|
||||
struct THREAD* CurrentThread;
|
||||
struct THREAD* IdleThread;
|
||||
struct THREAD* OutgoingThread;
|
||||
|
||||
@@ -3,10 +3,18 @@
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <ntlite/status.h>
|
||||
#include <ntlite/string_view.h>
|
||||
|
||||
void KeInitDebug(void (*PrintPtr)(char, uint32_t, uint32_t), int v);
|
||||
#define KE_DEBUG_CONSOLE 0x00000001u
|
||||
#define KE_DEBUG_SELFTEST 0x00000002u
|
||||
#define KE_DEBUG_FILE_LOG 0x00000004u
|
||||
|
||||
void KeInitDebug(void (*PrintPtr)(char, uint32_t, uint32_t), uint32_t flags);
|
||||
uint32_t KeGetDebugFlags(void);
|
||||
__declspec(dllexport) void DbgPrint(const char* fmt, ...);
|
||||
__declspec(dllexport) void KePrintConsole(const char* buf, size_t len);
|
||||
__declspec(dllexport) KSTATUS KeFlushDebugLogToFile(SYS_STRING_VIEW path);
|
||||
|
||||
typedef enum KE_PANIC_CODE {
|
||||
KE_PANIC_CODE_GENERIC = 0,
|
||||
|
||||
@@ -1,83 +1,157 @@
|
||||
#pragma once
|
||||
// ke_irp.h - I/O Request Packet
|
||||
// ke_irp.h - I/O Request Packet and remove-lock primitives
|
||||
//
|
||||
// This header intentionally includes only ke_list.h, ke_spinlock.h, and
|
||||
// ke_status.h - all of which are pure-inline / macro headers with no
|
||||
// external symbol references. ke_event.h and ke_semaphore.h are forward-
|
||||
// declared here so the HAL layer (hal_disk.c, io_driver.h) can include this
|
||||
// header without dragging in kernel sync-object link dependencies.
|
||||
//
|
||||
// Files that actually call KeEventCreate / KeSignalEvent / KeWaitForEvent
|
||||
// (ke_irp.c, ahci.c) include ke_event.h and ke_semaphore.h themselves.
|
||||
// IRPs are stack-based. A caller builds an IRP with one or more
|
||||
// IO_STACK_LOCATION entries, submits it to a DEVICE_OBJECT, and the I/O
|
||||
// manager/driver stack advances CurrentLocation as the IRP is forwarded.
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <kernel/ke_list.h>
|
||||
#include <kernel/ke_spinlock.h>
|
||||
#include <kernel/ke_irql.h>
|
||||
#include <ntlite/status.h>
|
||||
|
||||
// Forward declarations - full types live in ke_event.h / ke_semaphore.h.
|
||||
// We only ever hold pointers in IRP and IRP_QUEUE, so a forward decl suffices.
|
||||
struct EVENT;
|
||||
struct SEMAPHORE;
|
||||
struct KAPC;
|
||||
typedef struct DEVICE_OBJECT DEVICE_OBJECT;
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// IRP major/minor functions
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
typedef enum IO_MAJOR_FUNCTION {
|
||||
IRP_MJ_CREATE = 0,
|
||||
IRP_MJ_CLOSE = 1,
|
||||
IRP_MJ_READ = 2,
|
||||
IRP_MJ_WRITE = 3,
|
||||
IRP_MJ_DEVICE_CONTROL = 4,
|
||||
IRP_MJ_INTERNAL_DEVICE_CONTROL = 5,
|
||||
IRP_MJ_PNP = 6,
|
||||
IRP_MJ_POWER = 7,
|
||||
IRP_MJ_MAXIMUM_FUNCTION = 8,
|
||||
} IO_MAJOR_FUNCTION;
|
||||
|
||||
typedef enum IO_PNP_MINOR_FUNCTION {
|
||||
IRP_MN_START_DEVICE = 0,
|
||||
IRP_MN_QUERY_REMOVE_DEVICE = 1,
|
||||
IRP_MN_CANCEL_REMOVE_DEVICE = 2,
|
||||
IRP_MN_SURPRISE_REMOVAL = 3,
|
||||
IRP_MN_REMOVE_DEVICE = 4,
|
||||
IRP_MN_QUERY_DEVICE_RELATIONS = 5,
|
||||
IRP_MN_QUERY_ID = 6,
|
||||
IRP_MN_QUERY_CAPABILITIES = 7,
|
||||
} IO_PNP_MINOR_FUNCTION;
|
||||
|
||||
typedef enum IO_DEVICE_RELATION_TYPE {
|
||||
IoBusRelations = 0,
|
||||
IoRemovalRelations = 1,
|
||||
} IO_DEVICE_RELATION_TYPE;
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// IRP flags
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
#define IRP_FLAG_WRITE (1u << 0) // direction: write to device
|
||||
#define IRP_FLAG_PRIORITY (1u << 1) // insert at head of queue (elevated)
|
||||
#define IRP_FLAG_PRIORITY (1u << 0) // insert at head of a driver queue
|
||||
#define IRP_FLAG_CANCELLED (1u << 1)
|
||||
#define IRP_FLAG_COMPLETED (1u << 2)
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Forward declaration (IRP is self-referential via CompletionFn)
|
||||
// IRP stack parameters
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
typedef struct IO_STACK_LOCATION {
|
||||
IO_MAJOR_FUNCTION MajorFunction;
|
||||
uint8_t MinorFunction;
|
||||
DEVICE_OBJECT* DeviceObject;
|
||||
|
||||
union {
|
||||
struct {
|
||||
uint64_t ByteOffset;
|
||||
uint32_t Length;
|
||||
} ReadWrite;
|
||||
|
||||
struct {
|
||||
uint32_t IoControlCode;
|
||||
void* InputBuffer;
|
||||
uint32_t InputLength;
|
||||
void* OutputBuffer;
|
||||
uint32_t OutputLength;
|
||||
} DeviceIoControl;
|
||||
|
||||
struct {
|
||||
IO_DEVICE_RELATION_TYPE RelationType;
|
||||
} QueryDeviceRelations;
|
||||
|
||||
struct {
|
||||
uint32_t IdType;
|
||||
} QueryId;
|
||||
} Parameters;
|
||||
} IO_STACK_LOCATION;
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Forward declaration / callbacks
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
typedef struct IRP IRP;
|
||||
|
||||
// Completion callback: invoked by IoCompleteRequest() in the completing
|
||||
// context. Driver worker, synchronous fallback, DPC, and APC completion paths
|
||||
// all centralize policy here.
|
||||
typedef void (*IRP_COMPLETION_FN)(IRP *irp, void *ctx);
|
||||
typedef void (*IRP_COMPLETION_FN)(IRP* irp, void* ctx);
|
||||
typedef void (*IRP_CANCEL_ROUTINE)(DEVICE_OBJECT* device, IRP* irp);
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// I/O Request Packet
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
typedef struct IRP {
|
||||
// Embedded list node - driver-internal; do not touch after submit.
|
||||
LIST_ENTRY Link;
|
||||
|
||||
// ---- Request parameters (caller fills before IoSubmitIrp) ------------
|
||||
|
||||
uint8_t DriveIndex;
|
||||
uint32_t Flags; // IRP_FLAG_*
|
||||
uint64_t Lba;
|
||||
uint32_t SectorCount;
|
||||
void* Buffer; // kernel VA; valid until completion
|
||||
|
||||
// ---- Completion (caller fills before submit; driver writes Status) ----
|
||||
|
||||
volatile KSTATUS Status;
|
||||
volatile size_t Information; // bytes transferred on success
|
||||
volatile size_t Information;
|
||||
|
||||
// Forward-declared pointer: callers that create events include ke_event.h.
|
||||
struct EVENT *CompletionEvent;
|
||||
|
||||
// Optional kernel APC. If set, IoCompleteRequest queues it to its
|
||||
// target thread with arg1=irp and arg2=(void*)(uintptr_t)status.
|
||||
struct KAPC *CompletionApc;
|
||||
uint32_t Flags;
|
||||
volatile bool Cancel;
|
||||
SPINLOCK CancelLock;
|
||||
KIRQL CancelIrql;
|
||||
IRP_CANCEL_ROUTINE CancelRoutine;
|
||||
|
||||
struct EVENT* CompletionEvent;
|
||||
struct KAPC* CompletionApc;
|
||||
IRP_COMPLETION_FN CompletionFn;
|
||||
void* CompletionCtx;
|
||||
|
||||
// ---- Driver-internal --------------------------------------------------
|
||||
void* UserBuffer;
|
||||
void* SystemBuffer;
|
||||
|
||||
uint8_t StackCount;
|
||||
uint8_t CurrentLocation;
|
||||
|
||||
// Driver-owned scratch fields. They are deliberately not interpreted by
|
||||
// the I/O manager and are preserved across queueing/completion.
|
||||
uint8_t HwSlot;
|
||||
void* DriverCtx;
|
||||
|
||||
IO_STACK_LOCATION Stack[1];
|
||||
} IRP;
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Remove lock
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
typedef struct IO_REMOVE_LOCK {
|
||||
SPINLOCK Lock;
|
||||
volatile long IoCount;
|
||||
bool Removing;
|
||||
struct EVENT* RemovedEvent;
|
||||
} IO_REMOVE_LOCK;
|
||||
|
||||
__declspec(dllexport) KSTATUS IoInitializeRemoveLock(IO_REMOVE_LOCK* lock);
|
||||
__declspec(dllexport) void IoDestroyRemoveLock(IO_REMOVE_LOCK* lock);
|
||||
__declspec(dllexport) KSTATUS IoAcquireRemoveLock(IO_REMOVE_LOCK* lock, IRP* irp);
|
||||
__declspec(dllexport) void IoReleaseRemoveLock(IO_REMOVE_LOCK* lock, IRP* irp);
|
||||
__declspec(dllexport) void IoReleaseRemoveLockAndWait(IO_REMOVE_LOCK* lock, IRP* removeIrp);
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// IRP_QUEUE (per-device, driver-internal)
|
||||
// -------------------------------------------------------------------------
|
||||
@@ -85,24 +159,56 @@ typedef struct IRP {
|
||||
typedef struct IRP_QUEUE {
|
||||
SPINLOCK Lock;
|
||||
LIST_ENTRY Head;
|
||||
struct SEMAPHORE *Semaphore; // forward-declared pointer
|
||||
struct SEMAPHORE* Semaphore;
|
||||
} IRP_QUEUE;
|
||||
|
||||
// Non-inline: defined in ke_irp.c, which can pull in ke_semaphore.h safely.
|
||||
__declspec(dllexport) void IrpQueueInit (IRP_QUEUE *q, struct SEMAPHORE *sem);
|
||||
__declspec(dllexport) void IrpQueueEnqueue (IRP_QUEUE *q, IRP *irp);
|
||||
__declspec(dllexport) IRP *IrpQueueDequeue (IRP_QUEUE *q);
|
||||
__declspec(dllexport) void IrpQueueInit (IRP_QUEUE* q, struct SEMAPHORE* sem);
|
||||
__declspec(dllexport) void IrpQueueEnqueue (IRP_QUEUE* q, IRP* irp);
|
||||
__declspec(dllexport) IRP* IrpQueueDequeue (IRP_QUEUE* q);
|
||||
typedef void (*IRP_QUEUE_CANCEL_CALLBACK)(IRP* irp, void* context);
|
||||
__declspec(dllexport) void IrpQueueCancelAllEx(IRP_QUEUE* q,
|
||||
KSTATUS status,
|
||||
IRP_QUEUE_CANCEL_CALLBACK callback,
|
||||
void* context);
|
||||
__declspec(dllexport) void IrpQueueCancelAll(IRP_QUEUE* q, KSTATUS status);
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Public dispatch API - implemented in kernel/ke_irp.c
|
||||
// IRP allocation / stack helpers
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
__declspec(dllexport) KSTATUS IoAllocateIrp(uint8_t stackCount, IRP** outIrp);
|
||||
__declspec(dllexport) void IoFreeIrp(IRP* irp);
|
||||
__declspec(dllexport) void IoInitializeIrp(IRP* irp, uint8_t stackCount);
|
||||
|
||||
__declspec(dllexport) IO_STACK_LOCATION* IoGetCurrentIrpStackLocation(IRP* irp);
|
||||
__declspec(dllexport) IO_STACK_LOCATION* IoGetNextIrpStackLocation(IRP* irp);
|
||||
__declspec(dllexport) void IoSkipCurrentIrpStackLocation(IRP* irp);
|
||||
__declspec(dllexport) void IoCopyCurrentIrpStackLocationToNext(IRP* irp);
|
||||
__declspec(dllexport) void IoSetCompletionRoutine(IRP* irp, IRP_COMPLETION_FN fn, void* ctx);
|
||||
__declspec(dllexport) IRP_CANCEL_ROUTINE IoSetCancelRoutine(IRP* irp, IRP_CANCEL_ROUTINE routine);
|
||||
__declspec(dllexport) void IoCancelIrp(IRP* irp);
|
||||
|
||||
__declspec(dllexport) void IoBuildReadWriteIrp(IRP* irp,
|
||||
IO_MAJOR_FUNCTION major,
|
||||
uint64_t byteOffset,
|
||||
uint32_t length,
|
||||
void* buffer);
|
||||
__declspec(dllexport) void IoBuildPnpIrp(IRP* irp,
|
||||
IO_PNP_MINOR_FUNCTION minor);
|
||||
__declspec(dllexport) void IoBuildDeviceIoControlIrp(IRP* irp,
|
||||
IO_MAJOR_FUNCTION major,
|
||||
uint32_t ioControlCode,
|
||||
void* inputBuffer,
|
||||
uint32_t inputLength,
|
||||
void* outputBuffer,
|
||||
uint32_t outputLength);
|
||||
|
||||
// Complete an IRP exactly once: publish status/information, signal the
|
||||
// optional completion event, then invoke the optional completion callback.
|
||||
__declspec(dllexport) void IoCompleteRequest(IRP *irp, KSTATUS status, size_t information);
|
||||
// optional completion event, queue the optional APC, then invoke the optional
|
||||
// completion callback.
|
||||
__declspec(dllexport) KSTATUS IoCompleteRequest(IRP* irp, KSTATUS status, size_t information);
|
||||
|
||||
// Submit asynchronously. Returns STATUS_PENDING on success.
|
||||
__declspec(dllexport) KSTATUS IoSubmitIrp (IRP *irp);
|
||||
|
||||
// Submit and block until complete. Returns irp->Status.
|
||||
__declspec(dllexport) KSTATUS IoSubmitIrpSync(IRP *irp);
|
||||
// Submit to a specific top-of-stack device object. The current stack location
|
||||
// is stamped with the target device before dispatch.
|
||||
__declspec(dllexport) KSTATUS IoSubmitIrp(DEVICE_OBJECT* device, IRP* irp);
|
||||
__declspec(dllexport) KSTATUS IoSubmitIrpSync(DEVICE_OBJECT* device, IRP* irp);
|
||||
@@ -23,6 +23,8 @@ typedef struct KE_SCHED_STATS {
|
||||
uint64_t PriorityPreemptions;
|
||||
uint64_t VoluntaryBlocks;
|
||||
uint64_t CpuBurstDemotions;
|
||||
uint64_t SmtSiblingDeferrals;
|
||||
uint64_t SmtPrimaryPokes;
|
||||
} KE_SCHED_STATS;
|
||||
|
||||
// Called once by KernelMain before any threads are created.
|
||||
|
||||
@@ -13,6 +13,12 @@ KSTATUS KeStartApplicationProcessors(void);
|
||||
// park loop into their idle threads.
|
||||
KSTATUS KeAdmitApplicationProcessorsToScheduler(void);
|
||||
|
||||
|
||||
// Decode CPUID topology and build per-KCPU package/core/SMT sibling masks.
|
||||
// KeStartApplicationProcessors calls this after MADT enumeration; it is also
|
||||
// safe to call again after failed AP startup disables a CPU record.
|
||||
void KeBuildProcessorTopology(void);
|
||||
|
||||
// Run explicit AP-affine kernel workers that exercise the global scheduler,
|
||||
// reschedule IPIs, yield, and event wait/wake. Normal kernel/user/driver
|
||||
// threads remain BSP-affine unless their affinity is explicitly widened.
|
||||
|
||||
@@ -17,6 +17,10 @@ typedef enum OBJECT_TYPE {
|
||||
ObjectTypeDir = 8,
|
||||
ObjectTypeAsyncIo = 9,
|
||||
ObjectTypeSocket = 10,
|
||||
ObjectTypeDevice = 11,
|
||||
ObjectTypeDriver = 12,
|
||||
ObjectTypeUsbDevice = 13,
|
||||
ObjectTypeUsbPipe = 14,
|
||||
} OBJECT_TYPE;
|
||||
|
||||
typedef struct OBJECT {
|
||||
|
||||
+559
-78
@@ -1,13 +1,423 @@
|
||||
// io_device.c - kernel driver/device dispatch and disk registry
|
||||
// io_device.c - I/O manager: driver/device objects, stacks, PnP skeleton, disk namespace
|
||||
#include <string.h>
|
||||
#include <intrin.h>
|
||||
#include <ntlite/ddi/io_driver.h>
|
||||
#include <kernel/ke_irp.h>
|
||||
#include <kernel/mm_virt.h>
|
||||
#include <kernel/ke_debug.h>
|
||||
|
||||
#include <kernel/ob_object.h>
|
||||
|
||||
#define MAX_PUBLISHED_DEVICES 64
|
||||
#define MAX_DRIVER_MATCHERS 32
|
||||
#define MAX_DRIVER_MATCHERS 32
|
||||
#define MAX_DRIVER_OBJECTS 64
|
||||
#define MAX_PDO_OBJECTS 128
|
||||
|
||||
static SYS_STRING_VIEW IoSvOrUnnamed(SYS_STRING_VIEW name) {
|
||||
return SysSvIsEmpty(name) ? SYS_SV_LITERAL("<unnamed>") : name;
|
||||
}
|
||||
|
||||
static const char* IoBusTypeName(IO_BUS_TYPE busType) {
|
||||
switch (busType) {
|
||||
case IoBusPci: return "pci";
|
||||
case IoBusUsb: return "usb";
|
||||
default: return "unknown";
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Driver object registry / PDO AddDevice binding
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static DRIVER_OBJECT* gDriverObjects[MAX_DRIVER_OBJECTS];
|
||||
static uint32_t gDriverObjectCount;
|
||||
static DEVICE_OBJECT* gPhysicalDevices[MAX_PDO_OBJECTS];
|
||||
static uint32_t gPhysicalDeviceCount;
|
||||
|
||||
static bool IoDriverMatchesPdo(const DRIVER_OBJECT* driver, const DEVICE_OBJECT* pdo) {
|
||||
if (!driver || !driver->IdTable || !pdo || pdo->Type != DeviceObjectPhysical)
|
||||
return false;
|
||||
|
||||
for (size_t d = 0; d < driver->IdTable->Count; d++) {
|
||||
const IO_DEVICE_ID* want = &driver->IdTable->Ids[d];
|
||||
if (want->BusType != IoBusUnknown && want->BusType != pdo->u.Pdo.BusType)
|
||||
continue;
|
||||
for (uint8_t p = 0; p < pdo->u.Pdo.IdCount; p++) {
|
||||
if (SysSvEqual(want->Id, pdo->u.Pdo.Ids[p].Id))
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool IoTryAddDeviceForDriver(DRIVER_OBJECT* driver, DEVICE_OBJECT* pdo) {
|
||||
if (!driver || !pdo || !driver->DriverAddDevice)
|
||||
return false;
|
||||
if (!IoDriverMatchesPdo(driver, pdo))
|
||||
return false;
|
||||
|
||||
KSTATUS st = driver->DriverAddDevice(driver, pdo);
|
||||
if (KSUCCESS(st)) {
|
||||
DbgPrint("io: AddDevice driver=%v pdo=%v bus=%s\n",
|
||||
IoSvOrUnnamed(driver->Name), IoSvOrUnnamed(pdo->Name), IoBusTypeName(pdo->u.Pdo.BusType));
|
||||
|
||||
KSTATUS start = IoSendPnpIrpSync(pdo, IRP_MN_START_DEVICE);
|
||||
if (!KSUCCESS(start) && start != STATUS_NOT_SUPPORTED) {
|
||||
DbgPrint("io: START_DEVICE failed driver=%v pdo=%v st=0x%x\n",
|
||||
IoSvOrUnnamed(driver->Name), IoSvOrUnnamed(pdo->Name), start);
|
||||
}
|
||||
return true;
|
||||
} else {
|
||||
DbgPrint("io: AddDevice failed driver=%v pdo=%v st=0x%x\n",
|
||||
IoSvOrUnnamed(driver->Name), IoSvOrUnnamed(pdo->Name), st);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoInitializeDriverObject(DRIVER_OBJECT* driver,
|
||||
SYS_STRING_VIEW name) {
|
||||
if (!driver) return;
|
||||
memset(driver, 0, sizeof(*driver));
|
||||
driver->Header.Type = ObjectTypeDriver;
|
||||
driver->Header.RefCount = 1;
|
||||
driver->Header.Destroy = (void*)0;
|
||||
driver->Name = name;
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoRegisterDriverObject(DRIVER_OBJECT* driver) {
|
||||
if (!driver) return STATUS_INVALID_PARAMETER;
|
||||
if (gDriverObjectCount >= MAX_DRIVER_OBJECTS)
|
||||
return STATUS_NO_MEMORY;
|
||||
|
||||
if (driver->Header.Type != ObjectTypeDriver) {
|
||||
driver->Header.Type = ObjectTypeDriver;
|
||||
driver->Header.RefCount = 1;
|
||||
}
|
||||
|
||||
gDriverObjects[gDriverObjectCount++] = driver;
|
||||
DbgPrint("io: registered driver object %v\n", IoSvOrUnnamed(driver->Name));
|
||||
|
||||
for (uint32_t i = 0; i < gPhysicalDeviceCount; i++)
|
||||
(void)IoTryAddDeviceForDriver(driver, gPhysicalDevices[i]);
|
||||
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Device object lifetime
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static void IoDeviceDestroy(OBJECT* object) {
|
||||
DEVICE_OBJECT* dev = (DEVICE_OBJECT*)object;
|
||||
|
||||
for (uint32_t i = 0; i < gPhysicalDeviceCount; i++) {
|
||||
if (gPhysicalDevices[i] == dev) {
|
||||
gPhysicalDevices[i] = gPhysicalDevices[gPhysicalDeviceCount - 1u];
|
||||
gPhysicalDevices[gPhysicalDeviceCount - 1u] = (DEVICE_OBJECT*)0;
|
||||
gPhysicalDeviceCount--;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (dev->LowerDevice)
|
||||
IoDetachDevice(dev);
|
||||
|
||||
if (dev->Parent) {
|
||||
DEVICE_OBJECT* parent = dev->Parent;
|
||||
uintptr_t flags = KeAcquireSpinlock(&parent->Lock);
|
||||
DEVICE_OBJECT** link = &parent->FirstChild;
|
||||
while (*link) {
|
||||
if (*link == dev) {
|
||||
*link = dev->NextSibling;
|
||||
break;
|
||||
}
|
||||
link = &(*link)->NextSibling;
|
||||
}
|
||||
KeReleaseSpinlock(&parent->Lock, flags);
|
||||
dev->Parent = (DEVICE_OBJECT*)0;
|
||||
dev->NextSibling = (DEVICE_OBJECT*)0;
|
||||
IoDereferenceDeviceObject(parent);
|
||||
}
|
||||
}
|
||||
|
||||
static KSTATUS IoCreateDeviceInternal(DRIVER_OBJECT* driver,
|
||||
DEVICE_OBJECT_TYPE type,
|
||||
size_t extensionSize,
|
||||
DEVICE_OBJECT** outDevice) {
|
||||
if (!driver || !outDevice) return STATUS_INVALID_PARAMETER;
|
||||
|
||||
size_t total = sizeof(DEVICE_OBJECT) + extensionSize;
|
||||
DEVICE_OBJECT* dev = (DEVICE_OBJECT*)ObAllocateObject(ObjectTypeDevice, total);
|
||||
if (!dev) return STATUS_NO_MEMORY;
|
||||
|
||||
dev->Header.Destroy = IoDeviceDestroy;
|
||||
dev->DriverObject = driver;
|
||||
dev->Type = type;
|
||||
dev->PnpState = DevicePnpNotStarted;
|
||||
dev->AlignmentRequirement = 1;
|
||||
dev->Lock = (SPINLOCK)SPINLOCK_INIT;
|
||||
|
||||
// The per-device union is interpreted differently for PDOs and disk FDOs.
|
||||
// Do not write disk defaults into a PDO: u.Disk.DiskIndex aliases the low
|
||||
// byte of u.Pdo.BusType, which prevents ID-table AddDevice matching.
|
||||
if (type != DeviceObjectPhysical) {
|
||||
dev->u.Disk.DiskIndex = 0xFF;
|
||||
dev->u.Disk.SectorSize = 512;
|
||||
}
|
||||
|
||||
if (extensionSize)
|
||||
dev->DeviceExtension = (void*)((uint8_t*)dev + sizeof(DEVICE_OBJECT));
|
||||
|
||||
*outDevice = dev;
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoCreateDevice(DRIVER_OBJECT* driver,
|
||||
size_t extensionSize,
|
||||
DEVICE_OBJECT** outDevice) {
|
||||
return IoCreateDeviceInternal(driver, DeviceObjectFunction, extensionSize, outDevice);
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoCreatePhysicalDevice(DRIVER_OBJECT* busDriver,
|
||||
DEVICE_OBJECT* parent,
|
||||
const IO_DEVICE_ID* ids,
|
||||
size_t idCount,
|
||||
size_t extensionSize,
|
||||
DEVICE_OBJECT** outPdo) {
|
||||
if (!busDriver || !outPdo) return STATUS_INVALID_PARAMETER;
|
||||
if (idCount > IO_MAX_DEVICE_IDS) return STATUS_INVALID_PARAMETER;
|
||||
|
||||
DEVICE_OBJECT* pdo = (DEVICE_OBJECT*)0;
|
||||
KSTATUS st = IoCreateDeviceInternal(busDriver, DeviceObjectPhysical, extensionSize, &pdo);
|
||||
if (!KSUCCESS(st)) return st;
|
||||
|
||||
if (idCount && ids) {
|
||||
for (size_t i = 0; i < idCount; i++) {
|
||||
if (!SysSvIsValid(ids[i].Id) || ids[i].Id.Length >= IO_MAX_DEVICE_ID_CHARS) {
|
||||
IoDeleteDevice(pdo);
|
||||
return STATUS_INVALID_PARAMETER;
|
||||
}
|
||||
|
||||
char* dst = pdo->u.Pdo.IdStorage[i];
|
||||
if (ids[i].Id.Length)
|
||||
memcpy(dst, ids[i].Id.Data, ids[i].Id.Length);
|
||||
dst[ids[i].Id.Length] = '\0';
|
||||
|
||||
pdo->u.Pdo.Ids[i].BusType = ids[i].BusType;
|
||||
pdo->u.Pdo.Ids[i].Id = SysSvFromParts(dst, ids[i].Id.Length);
|
||||
if (pdo->u.Pdo.BusType == IoBusUnknown)
|
||||
pdo->u.Pdo.BusType = ids[i].BusType;
|
||||
}
|
||||
pdo->u.Pdo.IdCount = (uint8_t)idCount;
|
||||
}
|
||||
|
||||
if (parent) {
|
||||
IoReferenceDeviceObject(parent);
|
||||
pdo->Parent = parent;
|
||||
uintptr_t flags = KeAcquireSpinlock(&parent->Lock);
|
||||
pdo->NextSibling = parent->FirstChild;
|
||||
parent->FirstChild = pdo;
|
||||
KeReleaseSpinlock(&parent->Lock, flags);
|
||||
}
|
||||
|
||||
*outPdo = pdo;
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoRegisterPhysicalDevice(DEVICE_OBJECT* pdo) {
|
||||
if (!pdo || pdo->Type != DeviceObjectPhysical)
|
||||
return STATUS_INVALID_PARAMETER;
|
||||
|
||||
for (uint32_t i = 0; i < gPhysicalDeviceCount; i++) {
|
||||
if (gPhysicalDevices[i] == pdo)
|
||||
return STATUS_ALREADY_EXISTS;
|
||||
}
|
||||
|
||||
if (gPhysicalDeviceCount >= MAX_PDO_OBJECTS)
|
||||
return STATUS_NO_MEMORY;
|
||||
|
||||
gPhysicalDevices[gPhysicalDeviceCount++] = pdo;
|
||||
|
||||
bool anyMatched = false;
|
||||
for (uint32_t i = 0; i < gDriverObjectCount; i++) {
|
||||
if (IoTryAddDeviceForDriver(gDriverObjects[i], pdo))
|
||||
anyMatched = true;
|
||||
}
|
||||
|
||||
if (!anyMatched && pdo->u.Pdo.BusType == IoBusUsb) {
|
||||
SYS_STRING_VIEW id = pdo->u.Pdo.IdCount ? pdo->u.Pdo.Ids[0].Id : SYS_SV_LITERAL("<no-id>");
|
||||
DbgPrint("io: no function driver for pdo=%v id=%v bus=%s\n",
|
||||
IoSvOrUnnamed(pdo->Name), id, IoBusTypeName(pdo->u.Pdo.BusType));
|
||||
}
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
__declspec(dllexport) KSTATUS IoSetDeviceName(DEVICE_OBJECT* device,
|
||||
SYS_STRING_VIEW name) {
|
||||
if (!device || !SysSvIsValid(name))
|
||||
return STATUS_INVALID_PARAMETER;
|
||||
if (name.Length >= IO_MAX_DEVICE_NAME_CHARS)
|
||||
return STATUS_BUFFER_TOO_SMALL;
|
||||
|
||||
if (name.Length)
|
||||
memcpy(device->NameStorage, name.Data, name.Length);
|
||||
device->NameStorage[name.Length] = '\0';
|
||||
device->Name = SysSvFromParts(device->NameStorage, name.Length);
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoReferenceDeviceObject(DEVICE_OBJECT* device) {
|
||||
if (device) ObReferenceObject(&device->Header);
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoDereferenceDeviceObject(DEVICE_OBJECT* device) {
|
||||
if (device) ObDereferenceObject(&device->Header);
|
||||
}
|
||||
|
||||
static void IoRemovePhysicalDeviceRegistryEntry(DEVICE_OBJECT* device) {
|
||||
if (!device || device->Type != DeviceObjectPhysical)
|
||||
return;
|
||||
|
||||
for (uint32_t i = 0; i < gPhysicalDeviceCount; i++) {
|
||||
if (gPhysicalDevices[i] == device) {
|
||||
for (uint32_t j = i + 1; j < gPhysicalDeviceCount; j++)
|
||||
gPhysicalDevices[j - 1] = gPhysicalDevices[j];
|
||||
gPhysicalDeviceCount--;
|
||||
if (gPhysicalDeviceCount < MAX_PDO_OBJECTS)
|
||||
gPhysicalDevices[gPhysicalDeviceCount] = (DEVICE_OBJECT*)0;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void IoUnlinkDeviceFromParent(DEVICE_OBJECT* device) {
|
||||
if (!device || !device->Parent)
|
||||
return;
|
||||
|
||||
DEVICE_OBJECT* parent = device->Parent;
|
||||
uintptr_t flags = KeAcquireSpinlock(&parent->Lock);
|
||||
DEVICE_OBJECT** link = &parent->FirstChild;
|
||||
while (*link) {
|
||||
if (*link == device) {
|
||||
*link = device->NextSibling;
|
||||
break;
|
||||
}
|
||||
link = &(*link)->NextSibling;
|
||||
}
|
||||
device->Parent = (DEVICE_OBJECT*)0;
|
||||
device->NextSibling = (DEVICE_OBJECT*)0;
|
||||
KeReleaseSpinlock(&parent->Lock, flags);
|
||||
|
||||
IoDereferenceDeviceObject(parent);
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoDeleteDevice(DEVICE_OBJECT* device) {
|
||||
if (!device) return;
|
||||
if (device->DeletePending)
|
||||
return;
|
||||
|
||||
device->DeletePending = true;
|
||||
device->PnpState = DevicePnpDeleted;
|
||||
IoUnlinkDeviceFromParent(device);
|
||||
IoRemovePhysicalDeviceRegistryEntry(device);
|
||||
IoDereferenceDeviceObject(device);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Device stack management
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
__declspec(dllexport) DEVICE_OBJECT* IoAttachDeviceToDeviceStack(DEVICE_OBJECT* source,
|
||||
DEVICE_OBJECT* target) {
|
||||
if (!source || !target) return (DEVICE_OBJECT*)0;
|
||||
|
||||
DEVICE_OBJECT* top = target;
|
||||
while (top->AttachedDevice)
|
||||
top = top->AttachedDevice;
|
||||
|
||||
top->AttachedDevice = source;
|
||||
source->LowerDevice = top;
|
||||
IoReferenceDeviceObject(top);
|
||||
return top;
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoDetachDevice(DEVICE_OBJECT* source) {
|
||||
if (!source || !source->LowerDevice) return;
|
||||
|
||||
DEVICE_OBJECT* lower = source->LowerDevice;
|
||||
if (lower->AttachedDevice == source)
|
||||
lower->AttachedDevice = (DEVICE_OBJECT*)0;
|
||||
source->LowerDevice = (DEVICE_OBJECT*)0;
|
||||
IoDereferenceDeviceObject(lower);
|
||||
}
|
||||
|
||||
__declspec(dllexport) DEVICE_OBJECT* IoGetAttachedDeviceReference(DEVICE_OBJECT* device) {
|
||||
if (!device) return (DEVICE_OBJECT*)0;
|
||||
DEVICE_OBJECT* top = device;
|
||||
while (top->AttachedDevice)
|
||||
top = top->AttachedDevice;
|
||||
IoReferenceDeviceObject(top);
|
||||
return top;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Device-stack helpers / PnP dispatch
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
__declspec(dllexport) uint8_t IoGetDeviceStackSize(DEVICE_OBJECT* device) {
|
||||
uint8_t count = 0;
|
||||
for (DEVICE_OBJECT* cur = device; cur && count < 0xFEu; cur = cur->LowerDevice)
|
||||
count++;
|
||||
return count ? count : 1;
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoForwardIrpToLower(DEVICE_OBJECT* device, IRP* irp) {
|
||||
if (!device || !irp || !device->LowerDevice)
|
||||
return STATUS_NOT_SUPPORTED;
|
||||
|
||||
IO_STACK_LOCATION* cur = IoGetCurrentIrpStackLocation(irp);
|
||||
IO_STACK_LOCATION* next = IoGetNextIrpStackLocation(irp);
|
||||
if (!cur || !next)
|
||||
return STATUS_INVALID_PARAMETER;
|
||||
|
||||
DEVICE_OBJECT* currentDevice = cur->DeviceObject;
|
||||
memcpy(next, cur, sizeof(*next));
|
||||
|
||||
if (currentDevice == device) {
|
||||
cur->DeviceObject = (DEVICE_OBJECT*)0;
|
||||
_InterlockedDecrement(&device->OutstandingIrps);
|
||||
IoDereferenceDeviceObject(device);
|
||||
}
|
||||
|
||||
irp->CurrentLocation++;
|
||||
return IoCallDriver(device->LowerDevice, irp);
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoSendPnpIrpSync(DEVICE_OBJECT* device,
|
||||
IO_PNP_MINOR_FUNCTION minor) {
|
||||
if (!device) return STATUS_INVALID_PARAMETER;
|
||||
|
||||
DEVICE_OBJECT* top = IoGetAttachedDeviceReference(device);
|
||||
if (!top) return STATUS_INVALID_PARAMETER;
|
||||
|
||||
uint8_t stackCount = IoGetDeviceStackSize(top);
|
||||
IRP* irp = (IRP*)0;
|
||||
KSTATUS st = IoAllocateIrp(stackCount, &irp);
|
||||
if (!KSUCCESS(st)) {
|
||||
IoDereferenceDeviceObject(top);
|
||||
return st;
|
||||
}
|
||||
|
||||
IoBuildPnpIrp(irp, minor);
|
||||
st = IoSubmitIrpSync(top, irp);
|
||||
IoFreeIrp(irp);
|
||||
IoDereferenceDeviceObject(top);
|
||||
return st;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Legacy publish/matcher path
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
typedef struct IO_DRIVER_MATCHER_ENTRY {
|
||||
bool Used;
|
||||
@@ -23,14 +433,6 @@ static uint32_t gPublishedDeviceCount;
|
||||
static IO_DRIVER_MATCHER_ENTRY gDriverMatchers[MAX_DRIVER_MATCHERS];
|
||||
static uint32_t gDriverMatcherCount;
|
||||
|
||||
static const char* IoBusTypeName(IO_BUS_TYPE busType) {
|
||||
switch (busType) {
|
||||
case IoBusPci: return "pci";
|
||||
case IoBusUsb: return "usb";
|
||||
default: return "unknown";
|
||||
}
|
||||
}
|
||||
|
||||
static bool IoTryBindDevice(IO_PUBLISHED_DEVICE* device,
|
||||
IO_DRIVER_MATCHER_ENTRY* matcher) {
|
||||
if (!device || !matcher || !matcher->Used || device->Bound)
|
||||
@@ -80,12 +482,10 @@ __declspec(dllexport) KSTATUS IoRegisterDriverMatcher(const char* driverName,
|
||||
entry->Bind = bind;
|
||||
entry->Context = context;
|
||||
|
||||
DbgPrint("io: registered %s matcher %s\n",
|
||||
IoBusTypeName(busType), driverName);
|
||||
DbgPrint("io: registered %s matcher %s\n", IoBusTypeName(busType), driverName);
|
||||
|
||||
for (uint32_t i = 0; i < gPublishedDeviceCount; i++) {
|
||||
for (uint32_t i = 0; i < gPublishedDeviceCount; i++)
|
||||
IoTryBindDevice(&gPublishedDevices[i], entry);
|
||||
}
|
||||
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
@@ -101,7 +501,6 @@ __declspec(dllexport) KSTATUS IoPublishDevice(const IO_PUBLISHED_DEVICE* device)
|
||||
stored->Bound = false;
|
||||
stored->BoundDriverName = (const char*)0;
|
||||
|
||||
|
||||
for (uint32_t i = 0; i < gDriverMatcherCount; i++) {
|
||||
if (IoTryBindDevice(stored, &gDriverMatchers[i]))
|
||||
break;
|
||||
@@ -114,50 +513,44 @@ __declspec(dllexport) uint32_t IoPublishedDeviceCount(void) {
|
||||
return gPublishedDeviceCount;
|
||||
}
|
||||
|
||||
#define MAX_DISK_DEVICES 8
|
||||
#define MAX_DISK_ARRIVAL_CALLBACKS 16
|
||||
// -------------------------------------------------------------------------
|
||||
// Disk namespace
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
typedef struct IO_DISK_CALLBACK_ENTRY {
|
||||
#define MAX_DISK_DEVICES 8
|
||||
#define MAX_DISK_CALLBACKS 16
|
||||
|
||||
typedef struct IO_DISK_CHANGE_CALLBACK_ENTRY {
|
||||
bool Used;
|
||||
IO_DISK_CHANGE_CALLBACK Callback;
|
||||
void* Context;
|
||||
} IO_DISK_CHANGE_CALLBACK_ENTRY;
|
||||
|
||||
typedef struct IO_DISK_ARRIVAL_CALLBACK_ENTRY {
|
||||
bool Used;
|
||||
IO_DISK_ARRIVAL_CALLBACK Callback;
|
||||
void* Context;
|
||||
} IO_DISK_CALLBACK_ENTRY;
|
||||
} IO_DISK_ARRIVAL_CALLBACK_ENTRY;
|
||||
|
||||
static DEVICE_OBJECT* gDiskDevices[MAX_DISK_DEVICES];
|
||||
static IO_DISK_CALLBACK_ENTRY gDiskArrivalCallbacks[MAX_DISK_ARRIVAL_CALLBACKS];
|
||||
static IO_DISK_CHANGE_CALLBACK_ENTRY gDiskChangeCallbacks[MAX_DISK_CALLBACKS];
|
||||
static uint32_t gDiskChangeCallbackCount;
|
||||
static IO_DISK_ARRIVAL_CALLBACK_ENTRY gDiskArrivalCallbacks[MAX_DISK_CALLBACKS];
|
||||
static uint32_t gDiskArrivalCallbackCount;
|
||||
|
||||
static void IoNotifyDiskArrived(uint8_t driveIndex, DEVICE_OBJECT* device) {
|
||||
for (uint32_t i = 0; i < gDiskArrivalCallbackCount; i++) {
|
||||
IO_DISK_CALLBACK_ENTRY* entry = &gDiskArrivalCallbacks[i];
|
||||
static void IoNotifyDiskChange(uint8_t driveIndex, DEVICE_OBJECT* device, IO_DEVICE_CHANGE change) {
|
||||
for (uint32_t i = 0; i < gDiskChangeCallbackCount; i++) {
|
||||
IO_DISK_CHANGE_CALLBACK_ENTRY* entry = &gDiskChangeCallbacks[i];
|
||||
if (entry->Used && entry->Callback)
|
||||
entry->Callback(driveIndex, device, entry->Context);
|
||||
entry->Callback(driveIndex, device, change, entry->Context);
|
||||
}
|
||||
if (change == IoDeviceArrived) {
|
||||
for (uint32_t i = 0; i < gDiskArrivalCallbackCount; i++) {
|
||||
IO_DISK_ARRIVAL_CALLBACK_ENTRY* entry = &gDiskArrivalCallbacks[i];
|
||||
if (entry->Used && entry->Callback)
|
||||
entry->Callback(driveIndex, device, entry->Context);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoCreateDevice(DRIVER_OBJECT* driver,
|
||||
size_t extensionSize,
|
||||
DEVICE_OBJECT** outDevice) {
|
||||
if (!driver || !outDevice) return STATUS_INVALID_PARAMETER;
|
||||
|
||||
size_t total = sizeof(DEVICE_OBJECT) + extensionSize;
|
||||
DEVICE_OBJECT* dev = (DEVICE_OBJECT*)MmAllocatePool(total);
|
||||
if (!dev) return STATUS_NO_MEMORY;
|
||||
|
||||
memset(dev, 0, total);
|
||||
dev->DriverObject = driver;
|
||||
if (extensionSize)
|
||||
dev->DeviceExtension = (void*)((uint8_t*)dev + sizeof(DEVICE_OBJECT));
|
||||
dev->AlignmentRequirement = 1;
|
||||
dev->DiskIndex = 0xFF;
|
||||
|
||||
*outDevice = dev;
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoDeleteDevice(DEVICE_OBJECT* device) {
|
||||
if (!device) return;
|
||||
MmFreePool(device);
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoRegisterDiskDevice(uint8_t driveIndex,
|
||||
@@ -168,9 +561,57 @@ __declspec(dllexport) KSTATUS IoRegisterDiskDevice(uint8_t driveIndex,
|
||||
return STATUS_ALREADY_EXISTS;
|
||||
|
||||
device->Flags |= DEVICE_OBJECT_DISK;
|
||||
device->DiskIndex = driveIndex;
|
||||
device->u.Disk.DiskIndex = driveIndex;
|
||||
if (device->u.Disk.SectorSize == 0)
|
||||
device->u.Disk.SectorSize = 512;
|
||||
|
||||
KSTATUS pnpStatus = IoSendPnpIrpSync(device, IRP_MN_START_DEVICE);
|
||||
if (!KSUCCESS(pnpStatus) && pnpStatus != STATUS_NOT_SUPPORTED)
|
||||
return pnpStatus;
|
||||
if (pnpStatus == STATUS_NOT_SUPPORTED)
|
||||
device->PnpState = DevicePnpStarted;
|
||||
|
||||
IoReferenceDeviceObject(device);
|
||||
gDiskDevices[driveIndex] = device;
|
||||
IoNotifyDiskArrived(driveIndex, device);
|
||||
IoNotifyDiskChange(driveIndex, device, IoDeviceArrived);
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoNotifyDiskSurpriseRemoved(uint8_t driveIndex) {
|
||||
if (driveIndex >= MAX_DISK_DEVICES)
|
||||
return STATUS_INVALID_PARAMETER;
|
||||
DEVICE_OBJECT* dev = gDiskDevices[driveIndex];
|
||||
if (!dev) return STATUS_NOT_FOUND;
|
||||
|
||||
dev->PnpState = DevicePnpSurpriseRemoved;
|
||||
IoNotifyDiskChange(driveIndex, dev, IoDeviceSurpriseRemoved);
|
||||
(void)IoSendPnpIrpSync(dev, IRP_MN_SURPRISE_REMOVAL);
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoUnregisterDiskDevice(uint8_t driveIndex) {
|
||||
if (driveIndex >= MAX_DISK_DEVICES)
|
||||
return STATUS_INVALID_PARAMETER;
|
||||
DEVICE_OBJECT* dev = gDiskDevices[driveIndex];
|
||||
if (!dev) return STATUS_NOT_FOUND;
|
||||
|
||||
if (dev->PnpState != DevicePnpSurpriseRemoved) {
|
||||
IoNotifyDiskChange(driveIndex, dev, IoDeviceQueryRemove);
|
||||
KSTATUS queryStatus = IoSendPnpIrpSync(dev, IRP_MN_QUERY_REMOVE_DEVICE);
|
||||
if (!KSUCCESS(queryStatus) && queryStatus != STATUS_NOT_SUPPORTED)
|
||||
return queryStatus;
|
||||
|
||||
dev->PnpState = DevicePnpSurpriseRemoved;
|
||||
IoNotifyDiskChange(driveIndex, dev, IoDeviceSurpriseRemoved);
|
||||
(void)IoSendPnpIrpSync(dev, IRP_MN_SURPRISE_REMOVAL);
|
||||
}
|
||||
|
||||
dev->PnpState = DevicePnpRemovePending;
|
||||
(void)IoSendPnpIrpSync(dev, IRP_MN_REMOVE_DEVICE);
|
||||
|
||||
gDiskDevices[driveIndex] = (DEVICE_OBJECT*)0;
|
||||
IoNotifyDiskChange(driveIndex, dev, IoDeviceRemoved);
|
||||
IoDereferenceDeviceObject(dev);
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
@@ -178,10 +619,10 @@ __declspec(dllexport) KSTATUS IoRegisterDiskArrivalCallback(IO_DISK_ARRIVAL_CALL
|
||||
void* context) {
|
||||
if (!callback)
|
||||
return STATUS_INVALID_PARAMETER;
|
||||
if (gDiskArrivalCallbackCount >= MAX_DISK_ARRIVAL_CALLBACKS)
|
||||
if (gDiskArrivalCallbackCount >= MAX_DISK_CALLBACKS)
|
||||
return STATUS_NO_MEMORY;
|
||||
|
||||
IO_DISK_CALLBACK_ENTRY* entry = &gDiskArrivalCallbacks[gDiskArrivalCallbackCount++];
|
||||
IO_DISK_ARRIVAL_CALLBACK_ENTRY* entry = &gDiskArrivalCallbacks[gDiskArrivalCallbackCount++];
|
||||
memset(entry, 0, sizeof(*entry));
|
||||
entry->Used = true;
|
||||
entry->Callback = callback;
|
||||
@@ -191,7 +632,21 @@ __declspec(dllexport) KSTATUS IoRegisterDiskArrivalCallback(IO_DISK_ARRIVAL_CALL
|
||||
if (gDiskDevices[i])
|
||||
callback(i, gDiskDevices[i], context);
|
||||
}
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoRegisterDiskChangeCallback(IO_DISK_CHANGE_CALLBACK callback,
|
||||
void* context) {
|
||||
if (!callback)
|
||||
return STATUS_INVALID_PARAMETER;
|
||||
if (gDiskChangeCallbackCount >= MAX_DISK_CALLBACKS)
|
||||
return STATUS_NO_MEMORY;
|
||||
|
||||
IO_DISK_CHANGE_CALLBACK_ENTRY* entry = &gDiskChangeCallbacks[gDiskChangeCallbackCount++];
|
||||
memset(entry, 0, sizeof(*entry));
|
||||
entry->Used = true;
|
||||
entry->Callback = callback;
|
||||
entry->Context = context;
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
@@ -204,7 +659,6 @@ __declspec(dllexport) KSTATUS IoEnumerateDiskDevices(IO_DISK_ARRIVAL_CALLBACK ca
|
||||
if (gDiskDevices[i])
|
||||
callback(i, gDiskDevices[i], context);
|
||||
}
|
||||
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
@@ -213,28 +667,59 @@ __declspec(dllexport) DEVICE_OBJECT* IoGetDiskDevice(uint8_t driveIndex) {
|
||||
return gDiskDevices[driveIndex];
|
||||
}
|
||||
|
||||
__declspec(dllexport) DEVICE_OBJECT* IoReferenceDiskDevice(uint8_t driveIndex) {
|
||||
DEVICE_OBJECT* dev = IoGetDiskDevice(driveIndex);
|
||||
if (dev) IoReferenceDeviceObject(dev);
|
||||
return dev;
|
||||
}
|
||||
|
||||
__declspec(dllexport) bool IoDiskDevicePresent(uint8_t driveIndex) {
|
||||
return IoGetDiskDevice(driveIndex) != (DEVICE_OBJECT*)0;
|
||||
}
|
||||
|
||||
__declspec(dllexport) uint64_t IoDiskSectorCount(uint8_t driveIndex) {
|
||||
DEVICE_OBJECT* dev = IoGetDiskDevice(driveIndex);
|
||||
return dev ? dev->SectorCount : 0;
|
||||
return dev ? dev->u.Disk.SectorCount : 0;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Dispatch
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
__declspec(dllexport) KSTATUS IoCallDriver(DEVICE_OBJECT* device, IRP* irp) {
|
||||
if (!device || !device->DriverObject || !irp)
|
||||
return STATUS_INVALID_PARAMETER;
|
||||
|
||||
IO_MAJOR_FUNCTION major = (irp->Flags & IRP_FLAG_WRITE) ? IRP_MJ_WRITE : IRP_MJ_READ;
|
||||
IO_STACK_LOCATION* ios = IoGetCurrentIrpStackLocation(irp);
|
||||
if (!ios)
|
||||
return STATUS_INVALID_PARAMETER;
|
||||
|
||||
IO_MAJOR_FUNCTION major = ios->MajorFunction;
|
||||
if (major >= IRP_MJ_MAXIMUM_FUNCTION)
|
||||
return STATUS_INVALID_PARAMETER;
|
||||
|
||||
if (major != IRP_MJ_PNP &&
|
||||
(device->DeletePending || device->PnpState == DevicePnpSurpriseRemoved ||
|
||||
device->PnpState == DevicePnpRemovePending || device->PnpState == DevicePnpDeleted))
|
||||
return STATUS_OBJECT_DEAD;
|
||||
|
||||
PDRIVER_DISPATCH dispatch = device->DriverObject->MajorFunction[major];
|
||||
if (!dispatch)
|
||||
return STATUS_NOT_SUPPORTED;
|
||||
|
||||
return dispatch(device, irp);
|
||||
ios->DeviceObject = device;
|
||||
IoReferenceDeviceObject(device);
|
||||
_InterlockedIncrement(&device->OutstandingIrps);
|
||||
|
||||
KSTATUS st = dispatch(device, irp);
|
||||
if (st != STATUS_PENDING) {
|
||||
if (ios->DeviceObject == device) {
|
||||
ios->DeviceObject = (DEVICE_OBJECT*)0;
|
||||
_InterlockedDecrement(&device->OutstandingIrps);
|
||||
IoDereferenceDeviceObject(device);
|
||||
}
|
||||
}
|
||||
return st;
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoReadDiskSectors(uint8_t driveIndex,
|
||||
@@ -243,17 +728,15 @@ __declspec(dllexport) KSTATUS IoReadDiskSectors(uint8_t driveIndex,
|
||||
void* buffer) {
|
||||
if (!buffer || count == 0) return STATUS_INVALID_PARAMETER;
|
||||
|
||||
IRP irp;
|
||||
memset(&irp, 0, sizeof(irp));
|
||||
irp.DriveIndex = driveIndex;
|
||||
irp.Flags = 0;
|
||||
irp.Lba = lba;
|
||||
irp.SectorCount = count;
|
||||
irp.Buffer = buffer;
|
||||
irp.Status = STATUS_FAILURE;
|
||||
irp.Information = 0;
|
||||
DEVICE_OBJECT* dev = IoReferenceDiskDevice(driveIndex);
|
||||
if (!dev) return STATUS_NOT_FOUND;
|
||||
|
||||
return IoSubmitIrpSync(&irp);
|
||||
IRP irp;
|
||||
IoBuildReadWriteIrp(&irp, IRP_MJ_READ, lba * 512ull, count * 512u, buffer);
|
||||
KSTATUS st = IoSubmitIrpSync(dev, &irp);
|
||||
|
||||
IoDereferenceDeviceObject(dev);
|
||||
return st;
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoWriteDiskSectors(uint8_t driveIndex,
|
||||
@@ -262,15 +745,13 @@ __declspec(dllexport) KSTATUS IoWriteDiskSectors(uint8_t driveIndex,
|
||||
const void* buffer) {
|
||||
if (!buffer || count == 0) return STATUS_INVALID_PARAMETER;
|
||||
|
||||
IRP irp;
|
||||
memset(&irp, 0, sizeof(irp));
|
||||
irp.DriveIndex = driveIndex;
|
||||
irp.Flags = IRP_FLAG_WRITE;
|
||||
irp.Lba = lba;
|
||||
irp.SectorCount = count;
|
||||
irp.Buffer = (void*)buffer;
|
||||
irp.Status = STATUS_FAILURE;
|
||||
irp.Information = 0;
|
||||
DEVICE_OBJECT* dev = IoReferenceDiskDevice(driveIndex);
|
||||
if (!dev) return STATUS_NOT_FOUND;
|
||||
|
||||
return IoSubmitIrpSync(&irp);
|
||||
IRP irp;
|
||||
IoBuildReadWriteIrp(&irp, IRP_MJ_WRITE, lba * 512ull, count * 512u, (void*)buffer);
|
||||
KSTATUS st = IoSubmitIrpSync(dev, &irp);
|
||||
|
||||
IoDereferenceDeviceObject(dev);
|
||||
return st;
|
||||
}
|
||||
@@ -586,6 +586,33 @@ __declspec(dllexport) KSTATUS IoRegisterFilesystem(uint8_t volumeIndex,
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoUnregisterFilesystem(uint8_t volumeIndex) {
|
||||
if (volumeIndex >= MAX_DRIVES)
|
||||
return STATUS_INVALID_PARAMETER;
|
||||
if (!gVolumes[volumeIndex].Fs)
|
||||
return STATUS_NOT_FOUND;
|
||||
|
||||
memset(&gVolumes[volumeIndex], 0, sizeof(gVolumes[volumeIndex]));
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoUnregisterFilesystemsForDisk(uint8_t diskIndex) {
|
||||
KSTATUS last = STATUS_NOT_FOUND;
|
||||
for (uint8_t i = 0; i < MAX_DRIVES; i++) {
|
||||
if (gVolumes[i].Fs && gVolumes[i].Vol.DiskIndex == diskIndex) {
|
||||
memset(&gVolumes[i], 0, sizeof(gVolumes[i]));
|
||||
last = STATUS_SUCCESS;
|
||||
}
|
||||
}
|
||||
return last;
|
||||
}
|
||||
|
||||
__declspec(dllexport) bool IoFilesystemPresent(uint8_t volumeIndex) {
|
||||
if (volumeIndex >= MAX_DRIVES)
|
||||
return false;
|
||||
return gVolumes[volumeIndex].Fs != (IO_FILESYSTEM*)0;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Destructor
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
@@ -48,6 +48,15 @@ void KeInitializeBootstrapCpu(void) {
|
||||
cpu->Parked = 0;
|
||||
cpu->Online = 1;
|
||||
cpu->IsBsp = 1;
|
||||
cpu->PackageId = 0;
|
||||
cpu->CoreId = 0;
|
||||
cpu->ThreadId = 0;
|
||||
cpu->PhysicalCoreIndex = 0;
|
||||
cpu->CoreSiblingMask = 1ULL;
|
||||
cpu->PackageMask = 1ULL;
|
||||
cpu->IsPrimaryThread = 1;
|
||||
cpu->SmtThreadCount = 1;
|
||||
cpu->TopologyFlags = 0;
|
||||
cpu->CurrentIrql = PASSIVE_LEVEL;
|
||||
cpu->InterruptDepth = 0;
|
||||
cpu->ReschedulePending = 0;
|
||||
|
||||
+124
-7
@@ -13,12 +13,53 @@
|
||||
#include <kernel/ke_thread.h>
|
||||
#include <kernel/ke_sched.h>
|
||||
#include <kernel/mm_virt.h>
|
||||
#include <kernel/io_file.h>
|
||||
#include <common/printf.h>
|
||||
int _fltused; // MSVC floating-point helper anchor
|
||||
|
||||
int gDebugEnabled = 0;
|
||||
static uint32_t gDebugFlags = 0;
|
||||
void (*PrintChar)(char, uint32_t, uint32_t);
|
||||
|
||||
#define KE_DEBUG_LOG_BUFFER_SIZE (50u * 1024u)
|
||||
#define KE_DEBUG_LOG_LINE_SIZE 1024u
|
||||
|
||||
static SPINLOCK gDebugPrintLock = SPINLOCK_INIT;
|
||||
static char* gDebugLogBuffer;
|
||||
static size_t gDebugLogSize;
|
||||
static size_t gDebugLogWrite;
|
||||
static size_t gDebugLogUsed;
|
||||
static volatile long gDebugLogFlushing;
|
||||
|
||||
static void KiDebugLogAppendLocked(const char* text, size_t len) {
|
||||
if (!gDebugLogBuffer || gDebugLogSize == 0 || !text || len == 0)
|
||||
return;
|
||||
|
||||
if (len >= gDebugLogSize) {
|
||||
text += len - (gDebugLogSize - 1u);
|
||||
len = gDebugLogSize - 1u;
|
||||
}
|
||||
|
||||
size_t first = gDebugLogSize - gDebugLogWrite;
|
||||
if (first > len) first = len;
|
||||
memcpy(gDebugLogBuffer + gDebugLogWrite, text, first);
|
||||
if (len > first)
|
||||
memcpy(gDebugLogBuffer, text + first, len - first);
|
||||
|
||||
gDebugLogWrite = (gDebugLogWrite + len) % gDebugLogSize;
|
||||
if (gDebugLogUsed + len >= gDebugLogSize)
|
||||
gDebugLogUsed = gDebugLogSize;
|
||||
else
|
||||
gDebugLogUsed += len;
|
||||
}
|
||||
|
||||
static void KiDebugConsoleWrite(const char* text, size_t len) {
|
||||
if (!PrintChar || !text)
|
||||
return;
|
||||
for (size_t i = 0; i < len; i++)
|
||||
PrintChar(text[i], 0x00FF00, 0x000000);
|
||||
}
|
||||
|
||||
static volatile long gPanicInProgress = 0;
|
||||
static volatile long gPanicStoppedCpuCount = 0;
|
||||
|
||||
@@ -50,22 +91,98 @@ void KiKernelPutChar(char c) {
|
||||
PrintChar(c, 0x00FF00, 0x000000);
|
||||
}
|
||||
|
||||
void KeInitDebug(void (*PrintPtr)(char, uint32_t, uint32_t), int v) {
|
||||
gDebugEnabled = v;
|
||||
void KeInitDebug(void (*PrintPtr)(char, uint32_t, uint32_t), uint32_t flags) {
|
||||
gDebugFlags = flags;
|
||||
gDebugEnabled = (flags & (KE_DEBUG_CONSOLE | KE_DEBUG_FILE_LOG)) != 0;
|
||||
PrintChar = PrintPtr;
|
||||
printf_set_putchar(KiKernelPutChar);
|
||||
|
||||
if ((flags & (KE_DEBUG_CONSOLE | KE_DEBUG_FILE_LOG)) && !gDebugLogBuffer) {
|
||||
gDebugLogBuffer = (char*)MmAllocatePool(KE_DEBUG_LOG_BUFFER_SIZE);
|
||||
if (gDebugLogBuffer) {
|
||||
gDebugLogSize = KE_DEBUG_LOG_BUFFER_SIZE;
|
||||
gDebugLogWrite = 0;
|
||||
gDebugLogUsed = 0;
|
||||
memset(gDebugLogBuffer, 0, gDebugLogSize);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t KeGetDebugFlags(void) {
|
||||
return gDebugFlags;
|
||||
}
|
||||
|
||||
__declspec(dllexport) void DbgPrint(const char* fmt, ...) {
|
||||
if (!gDebugEnabled) return;
|
||||
if (!gDebugEnabled || !fmt) return;
|
||||
|
||||
static SPINLOCK sl = {0};
|
||||
uintptr_t flags = KeAcquireSpinlock(&sl);
|
||||
char line[KE_DEBUG_LOG_LINE_SIZE];
|
||||
va_list va;
|
||||
va_start(va, fmt);
|
||||
vprintf(fmt, va);
|
||||
int n = vsnprintf_(line, sizeof(line), fmt, va);
|
||||
va_end(va);
|
||||
KeReleaseSpinlock(&sl, flags);
|
||||
|
||||
size_t len = 0;
|
||||
if (n > 0) {
|
||||
len = (size_t)n;
|
||||
if (len >= sizeof(line))
|
||||
len = sizeof(line) - 1u;
|
||||
}
|
||||
|
||||
uintptr_t flags = KeAcquireSpinlock(&gDebugPrintLock);
|
||||
if (!_InterlockedCompareExchange((volatile long*)&gDebugLogFlushing, 0, 0))
|
||||
KiDebugLogAppendLocked(line, len);
|
||||
if (gDebugFlags & KE_DEBUG_CONSOLE)
|
||||
KiDebugConsoleWrite(line, len);
|
||||
KeReleaseSpinlock(&gDebugPrintLock, flags);
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS KeFlushDebugLogToFile(SYS_STRING_VIEW path) {
|
||||
if (!gDebugLogBuffer || gDebugLogSize == 0 || !SysSvIsValid(path) || path.Length == 0)
|
||||
return STATUS_INVALID_PARAMETER;
|
||||
if (!KeCanWait())
|
||||
return STATUS_INVALID_IRQL;
|
||||
|
||||
if (_InterlockedCompareExchange((volatile long*)&gDebugLogFlushing, 1, 0) != 0)
|
||||
return STATUS_PENDING;
|
||||
|
||||
size_t used;
|
||||
size_t start;
|
||||
uintptr_t flags = KeAcquireSpinlock(&gDebugPrintLock);
|
||||
used = gDebugLogUsed;
|
||||
start = (gDebugLogUsed == gDebugLogSize) ? gDebugLogWrite : 0;
|
||||
KeReleaseSpinlock(&gDebugPrintLock, flags);
|
||||
|
||||
if (used == 0) {
|
||||
_InterlockedExchange((volatile long*)&gDebugLogFlushing, 0);
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
(void)IoDeleteFile(path);
|
||||
|
||||
FILE_OBJECT* file = (FILE_OBJECT*)0;
|
||||
KSTATUS st = IoCreateFile(path, &file);
|
||||
if (!KSUCCESS(st)) {
|
||||
_InterlockedExchange((volatile long*)&gDebugLogFlushing, 0);
|
||||
return st;
|
||||
}
|
||||
|
||||
size_t written = 0;
|
||||
size_t totalWritten = 0;
|
||||
size_t first = gDebugLogSize - start;
|
||||
if (first > used) first = used;
|
||||
st = IoWriteFile(file, gDebugLogBuffer + start, first, &written);
|
||||
totalWritten += written;
|
||||
|
||||
if (KSUCCESS(st) && used > first) {
|
||||
written = 0;
|
||||
st = IoWriteFile(file, gDebugLogBuffer, used - first, &written);
|
||||
totalWritten += written;
|
||||
}
|
||||
|
||||
IoCloseFile(file);
|
||||
|
||||
_InterlockedExchange((volatile long*)&gDebugLogFlushing, 0);
|
||||
return (KSUCCESS(st) && totalWritten == used) ? STATUS_SUCCESS : st;
|
||||
}
|
||||
|
||||
// KePanic
|
||||
|
||||
+259
-68
@@ -1,130 +1,321 @@
|
||||
// ke_irp.c - I/O Request Packet dispatch and queue helpers
|
||||
//
|
||||
// This file lives in the kernel layer and is the ONLY place that bridges the
|
||||
// IRP abstraction to kernel sync objects. All calls to KeEventCreate,
|
||||
// KeSignalEvent, KeWaitForEvent, KeSignalSemaphore, and ObDereferenceObject
|
||||
// are confined here; hal_disk.c and io_driver.h remain free of those symbols.
|
||||
//
|
||||
// Dependency graph (→ = includes / calls):
|
||||
// ke_irp.c → ke_irp.h (IRP type, IRP_QUEUE, forward decls)
|
||||
// → ke_event.h (KeEventCreate, KeSignalEvent, KeWaitForEvent)
|
||||
// → ke_semaphore.h (KeSignalSemaphore)
|
||||
// → ob_object.h (ObDereferenceObject)
|
||||
// → io_driver.h (DEVICE_OBJECT / IoCallDriver)
|
||||
// ke_irp.c - I/O Request Packet dispatch, queue, cancellation, remove-locks
|
||||
|
||||
#include <string.h>
|
||||
#include <intrin.h>
|
||||
#include <kernel/ke_irp.h>
|
||||
#include <kernel/ke_event.h>
|
||||
#include <kernel/ke_semaphore.h>
|
||||
#include <kernel/ke_apc.h>
|
||||
#include <kernel/ob_object.h>
|
||||
#include <kernel/mm_virt.h>
|
||||
#include <ntlite/ddi/io_driver.h>
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// IRP_QUEUE operations
|
||||
// -------------------------------------------------------------------------
|
||||
// These are defined here (not as static inlines in ke_irp.h) so that
|
||||
// ke_irp.h can avoid including ke_semaphore.h, keeping the header safe
|
||||
// for inclusion from the HAL layer.
|
||||
|
||||
__declspec(dllexport) void IrpQueueInit(IRP_QUEUE *q, struct SEMAPHORE *sem) {
|
||||
__declspec(dllexport) void IrpQueueInit(IRP_QUEUE* q, struct SEMAPHORE* sem) {
|
||||
q->Lock = (SPINLOCK)SPINLOCK_INIT;
|
||||
q->Semaphore = sem;
|
||||
LIST_INIT(&q->Head);
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IrpQueueEnqueue(IRP_QUEUE *q, IRP *irp) {
|
||||
__declspec(dllexport) void IrpQueueEnqueue(IRP_QUEUE* q, IRP* irp) {
|
||||
uintptr_t flags = KeAcquireSpinlock(&q->Lock);
|
||||
if (irp->Flags & IRP_FLAG_PRIORITY)
|
||||
ListInsertHead(&q->Head, &irp->Link);
|
||||
else
|
||||
ListInsertTail(&q->Head, &irp->Link);
|
||||
KeReleaseSpinlock(&q->Lock, flags);
|
||||
// Signal outside the spinlock - safe, and avoids priority-inversion risk.
|
||||
KeSignalSemaphore((SEMAPHORE *)q->Semaphore);
|
||||
KeSignalSemaphore((SEMAPHORE*)q->Semaphore);
|
||||
}
|
||||
|
||||
__declspec(dllexport) IRP *IrpQueueDequeue(IRP_QUEUE *q) {
|
||||
__declspec(dllexport) IRP* IrpQueueDequeue(IRP_QUEUE* q) {
|
||||
uintptr_t flags = KeAcquireSpinlock(&q->Lock);
|
||||
LIST_ENTRY *node = ListPopHead(&q->Head);
|
||||
LIST_ENTRY* node = ListPopHead(&q->Head);
|
||||
KeReleaseSpinlock(&q->Lock, flags);
|
||||
if (!node) return (IRP *)0;
|
||||
if (!node) return (IRP*)0;
|
||||
return CONTAINING_RECORD(node, IRP, Link);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// IoCompleteRequest
|
||||
// -------------------------------------------------------------------------
|
||||
__declspec(dllexport) void IrpQueueCancelAllEx(IRP_QUEUE* q,
|
||||
KSTATUS status,
|
||||
IRP_QUEUE_CANCEL_CALLBACK callback,
|
||||
void* context) {
|
||||
if (!q) return;
|
||||
for (;;) {
|
||||
uintptr_t flags = KeAcquireSpinlock(&q->Lock);
|
||||
LIST_ENTRY* node = ListPopHead(&q->Head);
|
||||
KeReleaseSpinlock(&q->Lock, flags);
|
||||
if (!node) break;
|
||||
|
||||
static size_t IoIrpTransferBytes(const IRP *irp, KSTATUS status) {
|
||||
if (!irp || !KSUCCESS(status)) return 0;
|
||||
return (size_t)irp->SectorCount * 512u;
|
||||
IRP* irp = CONTAINING_RECORD(node, IRP, Link);
|
||||
irp->Cancel = true;
|
||||
irp->Flags |= IRP_FLAG_CANCELLED;
|
||||
if (callback)
|
||||
callback(irp, context);
|
||||
IoCompleteRequest(irp, status, 0);
|
||||
}
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoCompleteRequest(IRP *irp, KSTATUS status, size_t information) {
|
||||
__declspec(dllexport) void IrpQueueCancelAll(IRP_QUEUE* q, KSTATUS status) {
|
||||
IrpQueueCancelAllEx(q, status, (IRP_QUEUE_CANCEL_CALLBACK)0, (void*)0);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Remove locks
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
__declspec(dllexport) KSTATUS IoInitializeRemoveLock(IO_REMOVE_LOCK* lock) {
|
||||
if (!lock) return STATUS_INVALID_PARAMETER;
|
||||
memset(lock, 0, sizeof(*lock));
|
||||
lock->Lock = (SPINLOCK)SPINLOCK_INIT;
|
||||
lock->IoCount = 1; // sentinel reference released by IoReleaseRemoveLockAndWait
|
||||
KSTATUS st = KeEventCreate(EventManualReset, 0, &lock->RemovedEvent);
|
||||
if (!KSUCCESS(st)) return st;
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoDestroyRemoveLock(IO_REMOVE_LOCK* lock) {
|
||||
if (!lock) return;
|
||||
if (lock->RemovedEvent) {
|
||||
ObDereferenceObject(&lock->RemovedEvent->Header);
|
||||
lock->RemovedEvent = (EVENT*)0;
|
||||
}
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoAcquireRemoveLock(IO_REMOVE_LOCK* lock, IRP* irp) {
|
||||
(void)irp;
|
||||
if (!lock) return STATUS_INVALID_PARAMETER;
|
||||
uintptr_t flags = KeAcquireSpinlock(&lock->Lock);
|
||||
if (lock->Removing) {
|
||||
KeReleaseSpinlock(&lock->Lock, flags);
|
||||
return STATUS_OBJECT_DEAD;
|
||||
}
|
||||
_InterlockedIncrement(&lock->IoCount);
|
||||
KeReleaseSpinlock(&lock->Lock, flags);
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoReleaseRemoveLock(IO_REMOVE_LOCK* lock, IRP* irp) {
|
||||
(void)irp;
|
||||
if (!lock) return;
|
||||
long left = _InterlockedDecrement(&lock->IoCount);
|
||||
if (left == 0 && lock->Removing && lock->RemovedEvent)
|
||||
KeSignalEvent(lock->RemovedEvent);
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoReleaseRemoveLockAndWait(IO_REMOVE_LOCK* lock, IRP* removeIrp) {
|
||||
(void)removeIrp;
|
||||
if (!lock) return;
|
||||
|
||||
uintptr_t flags = KeAcquireSpinlock(&lock->Lock);
|
||||
lock->Removing = true;
|
||||
long left = _InterlockedDecrement(&lock->IoCount); // release sentinel
|
||||
bool done = (left == 0);
|
||||
KeReleaseSpinlock(&lock->Lock, flags);
|
||||
|
||||
if (done) {
|
||||
if (lock->RemovedEvent) KeSignalEvent(lock->RemovedEvent);
|
||||
return;
|
||||
}
|
||||
|
||||
if (lock->RemovedEvent)
|
||||
KeWaitForEvent(lock->RemovedEvent, 0);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// IRP helpers
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static size_t IoIrpSize(uint8_t stackCount) {
|
||||
if (stackCount == 0) stackCount = 1;
|
||||
return sizeof(IRP) + ((size_t)stackCount - 1u) * sizeof(IO_STACK_LOCATION);
|
||||
}
|
||||
|
||||
__declspec(dllexport) KSTATUS IoAllocateIrp(uint8_t stackCount, IRP** outIrp) {
|
||||
if (!outIrp) return STATUS_INVALID_PARAMETER;
|
||||
if (stackCount == 0) stackCount = 1;
|
||||
IRP* irp = (IRP*)MmAllocatePool(IoIrpSize(stackCount));
|
||||
if (!irp) return STATUS_NO_MEMORY;
|
||||
IoInitializeIrp(irp, stackCount);
|
||||
*outIrp = irp;
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoFreeIrp(IRP* irp) {
|
||||
if (irp) MmFreePool(irp);
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoInitializeIrp(IRP* irp, uint8_t stackCount) {
|
||||
if (!irp) return;
|
||||
if (stackCount == 0) stackCount = 1;
|
||||
memset(irp, 0, IoIrpSize(stackCount));
|
||||
LIST_INIT(&irp->Link);
|
||||
irp->Status = STATUS_FAILURE;
|
||||
irp->Information = 0;
|
||||
irp->CancelLock = (SPINLOCK)SPINLOCK_INIT;
|
||||
irp->StackCount = stackCount;
|
||||
irp->CurrentLocation = 0;
|
||||
}
|
||||
|
||||
__declspec(dllexport) IO_STACK_LOCATION* IoGetCurrentIrpStackLocation(IRP* irp) {
|
||||
if (!irp || irp->CurrentLocation >= irp->StackCount) return (IO_STACK_LOCATION*)0;
|
||||
return &irp->Stack[irp->CurrentLocation];
|
||||
}
|
||||
|
||||
__declspec(dllexport) IO_STACK_LOCATION* IoGetNextIrpStackLocation(IRP* irp) {
|
||||
if (!irp || irp->CurrentLocation + 1u >= irp->StackCount) return (IO_STACK_LOCATION*)0;
|
||||
return &irp->Stack[irp->CurrentLocation + 1u];
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoSkipCurrentIrpStackLocation(IRP* irp) {
|
||||
if (!irp) return;
|
||||
if (irp->CurrentLocation + 1u < irp->StackCount)
|
||||
irp->CurrentLocation++;
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoCopyCurrentIrpStackLocationToNext(IRP* irp) {
|
||||
IO_STACK_LOCATION* cur = IoGetCurrentIrpStackLocation(irp);
|
||||
IO_STACK_LOCATION* next = IoGetNextIrpStackLocation(irp);
|
||||
if (!cur || !next) return;
|
||||
memcpy(next, cur, sizeof(*next));
|
||||
irp->CurrentLocation++;
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoSetCompletionRoutine(IRP* irp, IRP_COMPLETION_FN fn, void* ctx) {
|
||||
if (!irp) return;
|
||||
irp->CompletionFn = fn;
|
||||
irp->CompletionCtx = ctx;
|
||||
}
|
||||
|
||||
__declspec(dllexport) IRP_CANCEL_ROUTINE IoSetCancelRoutine(IRP* irp, IRP_CANCEL_ROUTINE routine) {
|
||||
if (!irp) return (IRP_CANCEL_ROUTINE)0;
|
||||
uintptr_t flags = KeAcquireSpinlock(&irp->CancelLock);
|
||||
IRP_CANCEL_ROUTINE old = irp->CancelRoutine;
|
||||
irp->CancelRoutine = routine;
|
||||
KeReleaseSpinlock(&irp->CancelLock, flags);
|
||||
return old;
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoCancelIrp(IRP* irp) {
|
||||
if (!irp) return;
|
||||
|
||||
uintptr_t flags = KeAcquireSpinlock(&irp->CancelLock);
|
||||
irp->Cancel = true;
|
||||
irp->Flags |= IRP_FLAG_CANCELLED;
|
||||
IRP_CANCEL_ROUTINE routine = irp->CancelRoutine;
|
||||
irp->CancelRoutine = (IRP_CANCEL_ROUTINE)0;
|
||||
IO_STACK_LOCATION* ios = IoGetCurrentIrpStackLocation(irp);
|
||||
DEVICE_OBJECT* dev = ios ? ios->DeviceObject : (DEVICE_OBJECT*)0;
|
||||
KeReleaseSpinlock(&irp->CancelLock, flags);
|
||||
|
||||
if (routine)
|
||||
routine(dev, irp);
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoBuildReadWriteIrp(IRP* irp,
|
||||
IO_MAJOR_FUNCTION major,
|
||||
uint64_t byteOffset,
|
||||
uint32_t length,
|
||||
void* buffer) {
|
||||
if (!irp) return;
|
||||
IoInitializeIrp(irp, 1);
|
||||
irp->UserBuffer = buffer;
|
||||
IO_STACK_LOCATION* ios = IoGetCurrentIrpStackLocation(irp);
|
||||
if (!ios) return;
|
||||
ios->MajorFunction = major;
|
||||
ios->MinorFunction = 0;
|
||||
ios->Parameters.ReadWrite.ByteOffset = byteOffset;
|
||||
ios->Parameters.ReadWrite.Length = length;
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoBuildPnpIrp(IRP* irp,
|
||||
IO_PNP_MINOR_FUNCTION minor) {
|
||||
if (!irp) return;
|
||||
uint8_t stackCount = irp->StackCount ? irp->StackCount : 1;
|
||||
IoInitializeIrp(irp, stackCount);
|
||||
IO_STACK_LOCATION* ios = IoGetCurrentIrpStackLocation(irp);
|
||||
if (!ios) return;
|
||||
ios->MajorFunction = IRP_MJ_PNP;
|
||||
ios->MinorFunction = (uint8_t)minor;
|
||||
}
|
||||
|
||||
__declspec(dllexport) void IoBuildDeviceIoControlIrp(IRP* irp,
|
||||
IO_MAJOR_FUNCTION major,
|
||||
uint32_t ioControlCode,
|
||||
void* inputBuffer,
|
||||
uint32_t inputLength,
|
||||
void* outputBuffer,
|
||||
uint32_t outputLength) {
|
||||
if (!irp) return;
|
||||
uint8_t stackCount = irp->StackCount ? irp->StackCount : 1;
|
||||
IoInitializeIrp(irp, stackCount);
|
||||
IO_STACK_LOCATION* ios = IoGetCurrentIrpStackLocation(irp);
|
||||
if (!ios) return;
|
||||
ios->MajorFunction = major;
|
||||
ios->MinorFunction = 0;
|
||||
ios->Parameters.DeviceIoControl.IoControlCode = ioControlCode;
|
||||
ios->Parameters.DeviceIoControl.InputBuffer = inputBuffer;
|
||||
ios->Parameters.DeviceIoControl.InputLength = inputLength;
|
||||
ios->Parameters.DeviceIoControl.OutputBuffer = outputBuffer;
|
||||
ios->Parameters.DeviceIoControl.OutputLength = outputLength;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Completion/submission
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
__declspec(dllexport) KSTATUS IoCompleteRequest(IRP* irp, KSTATUS status, size_t information) {
|
||||
if (!irp) return STATUS_INVALID_PARAMETER;
|
||||
if (irp->Flags & IRP_FLAG_COMPLETED)
|
||||
return STATUS_ALREADY_EXISTS;
|
||||
|
||||
irp->Flags |= IRP_FLAG_COMPLETED;
|
||||
irp->Status = status;
|
||||
irp->Information = information;
|
||||
|
||||
// Queue the APC before waking event waiters so a thread that waits on the
|
||||
// completion event will see and deliver the APC on its wait-return path.
|
||||
IO_STACK_LOCATION* ios = IoGetCurrentIrpStackLocation(irp);
|
||||
DEVICE_OBJECT* dev = ios ? ios->DeviceObject : (DEVICE_OBJECT*)0;
|
||||
if (dev) {
|
||||
_InterlockedDecrement(&dev->OutstandingIrps);
|
||||
IoDereferenceDeviceObject(dev);
|
||||
ios->DeviceObject = (DEVICE_OBJECT*)0;
|
||||
}
|
||||
|
||||
if (irp->CompletionApc)
|
||||
KeInsertQueueApc((KAPC *)irp->CompletionApc, irp, (void *)(uintptr_t)status);
|
||||
KeInsertQueueApc((KAPC*)irp->CompletionApc, irp, (void*)(uintptr_t)status);
|
||||
if (irp->CompletionEvent)
|
||||
KeSignalEvent((EVENT *)irp->CompletionEvent);
|
||||
KeSignalEvent((EVENT*)irp->CompletionEvent);
|
||||
if (irp->CompletionFn)
|
||||
irp->CompletionFn(irp, irp->CompletionCtx);
|
||||
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// IoSubmitIrp
|
||||
// -------------------------------------------------------------------------
|
||||
// Dispatches through the registered DEVICE_OBJECT.
|
||||
// Accepted IRPs are always completed by the target driver via IoCompleteRequest().
|
||||
|
||||
__declspec(dllexport) KSTATUS IoSubmitIrp(IRP *irp) {
|
||||
if (!irp) return STATUS_INVALID_PARAMETER;
|
||||
|
||||
__declspec(dllexport) KSTATUS IoSubmitIrp(DEVICE_OBJECT* device, IRP* irp) {
|
||||
if (!device || !irp) return STATUS_INVALID_PARAMETER;
|
||||
irp->Status = STATUS_FAILURE;
|
||||
irp->Information = 0;
|
||||
|
||||
DEVICE_OBJECT *device = IoGetDiskDevice(irp->DriveIndex);
|
||||
if (!device) return STATUS_NOT_FOUND;
|
||||
|
||||
KSTATUS st = IoCallDriver(device, irp);
|
||||
|
||||
// Definitive driver contract:
|
||||
// STATUS_PENDING means the IRP was accepted and will be completed via
|
||||
// IoCompleteRequest(), possibly inline before IoCallDriver returns.
|
||||
// Any other status means the IRP was rejected and no completion occurs.
|
||||
return st;
|
||||
irp->Flags &= ~IRP_FLAG_COMPLETED;
|
||||
return IoCallDriver(device, irp);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// IoSubmitIrpSync
|
||||
// -------------------------------------------------------------------------
|
||||
// Creates a stack-allocated auto-reset event, attaches it to the IRP,
|
||||
// submits, and blocks. Callers that already provided a CompletionEvent
|
||||
// should use IoSubmitIrp() directly and wait on their own event instead.
|
||||
__declspec(dllexport) KSTATUS IoSubmitIrpSync(DEVICE_OBJECT* device, IRP* irp) {
|
||||
if (!device || !irp) return STATUS_INVALID_PARAMETER;
|
||||
|
||||
__declspec(dllexport) KSTATUS IoSubmitIrpSync(IRP *irp) {
|
||||
if (!irp) return STATUS_INVALID_PARAMETER;
|
||||
|
||||
EVENT *ev = (EVENT *)0;
|
||||
EVENT* ev = (EVENT*)0;
|
||||
KSTATUS st = KeEventCreate(EventAutoReset, 0, &ev);
|
||||
if (!KSUCCESS(st)) return st;
|
||||
|
||||
// Attach our event; preserve any APC/callback the caller may have set.
|
||||
irp->CompletionEvent = ev;
|
||||
|
||||
st = IoSubmitIrp(irp);
|
||||
st = IoSubmitIrp(device, irp);
|
||||
if (st == STATUS_PENDING) {
|
||||
KeWaitForEvent(ev, 0);
|
||||
st = irp->Status;
|
||||
}
|
||||
|
||||
// Release our reference to the temporary event object.
|
||||
ObDereferenceObject(&ev->Header);
|
||||
|
||||
return st;
|
||||
}
|
||||
+73
-2
@@ -36,6 +36,44 @@ static int KiThreadCanRunOnCpuLocked(const THREAD* thread, const KCPU* cpu) {
|
||||
return (thread->AffinityMask & (1ULL << cpu->CpuIndex)) != 0;
|
||||
}
|
||||
|
||||
static int KiCpuIsIdleLocked(const KCPU* cpu) {
|
||||
if (!cpu || !cpu->Online)
|
||||
return 0;
|
||||
THREAD* cur = cpu->CurrentThread;
|
||||
return (!cur || cur == cpu->IdleThread || cur->State != ThreadRunning);
|
||||
}
|
||||
|
||||
static int KiAnyIdlePrimaryForThreadLocked(const THREAD* thread) {
|
||||
if (!thread || thread->IsIdleThread)
|
||||
return 0;
|
||||
|
||||
for (uint32_t i = 0; i < gCpuCount; i++) {
|
||||
KCPU* cpu = &gCpus[i];
|
||||
if (!cpu->Present || !cpu->Started || !cpu->Online)
|
||||
continue;
|
||||
if (!cpu->IsPrimaryThread)
|
||||
continue;
|
||||
if (!KiCpuIsIdleLocked(cpu))
|
||||
continue;
|
||||
if (KiThreadCanRunOnCpuLocked(thread, cpu))
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int KiShouldDeferToIdlePrimaryLocked(KCPU* cpu, const THREAD* thread) {
|
||||
if (!cpu || !thread || thread->IsIdleThread)
|
||||
return 0;
|
||||
if (cpu->IsPrimaryThread || cpu->SmtThreadCount <= 1)
|
||||
return 0;
|
||||
|
||||
// Do not fill a secondary SMT sibling while any primary hardware thread
|
||||
// that can run this thread is idle. KiPokeRunnableCpusLocked targets those
|
||||
// primary CPUs first, so this CPU can safely remain idle and wait for more
|
||||
// work or for physical cores to fill.
|
||||
return KiAnyIdlePrimaryForThreadLocked(thread);
|
||||
}
|
||||
|
||||
static void KiRequestRescheduleCpuLocked(KCPU* target) {
|
||||
if (!target || !target->Online)
|
||||
return;
|
||||
@@ -52,6 +90,9 @@ static void KiPokeRunnableCpusLocked(THREAD* thread) {
|
||||
if (!thread || thread->IsIdleThread)
|
||||
return;
|
||||
|
||||
int haveIdlePrimary = KiAnyIdlePrimaryForThreadLocked(thread);
|
||||
int requestedIdle = 0;
|
||||
|
||||
for (uint32_t i = 0; i < gCpuCount; i++) {
|
||||
KCPU* cpu = &gCpus[i];
|
||||
if (!cpu->Present || !cpu->Started || !cpu->Online)
|
||||
@@ -61,12 +102,35 @@ static void KiPokeRunnableCpusLocked(THREAD* thread) {
|
||||
|
||||
THREAD* cur = cpu->CurrentThread;
|
||||
if (!cur || cur == cpu->IdleThread) {
|
||||
if (haveIdlePrimary && !cpu->IsPrimaryThread)
|
||||
continue;
|
||||
if (cpu->IsPrimaryThread)
|
||||
gSchedStats.SmtPrimaryPokes++;
|
||||
KiRequestRescheduleCpuLocked(cpu);
|
||||
requestedIdle = 1;
|
||||
} else if (cur->State == ThreadRunning && thread->Priority > cur->Priority) {
|
||||
gSchedStats.PriorityPreemptions++;
|
||||
KiRequestRescheduleCpuLocked(cpu);
|
||||
}
|
||||
}
|
||||
|
||||
// If no preferred idle primary was actually targetable, allow an idle SMT
|
||||
// sibling to take the work. This covers explicit affinity to a secondary
|
||||
// logical CPU and partially-online systems.
|
||||
if (!requestedIdle && haveIdlePrimary == 0) {
|
||||
for (uint32_t i = 0; i < gCpuCount; i++) {
|
||||
KCPU* cpu = &gCpus[i];
|
||||
if (!cpu->Present || !cpu->Started || !cpu->Online)
|
||||
continue;
|
||||
if (!KiThreadCanRunOnCpuLocked(thread, cpu))
|
||||
continue;
|
||||
THREAD* cur = cpu->CurrentThread;
|
||||
if (!cur || cur == cpu->IdleThread) {
|
||||
KiRequestRescheduleCpuLocked(cpu);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static uint32_t KiBaseQuantumForPriority(THREAD_PRIORITY priority) {
|
||||
@@ -369,8 +433,11 @@ static int KiAnyRunnableForCpuLocked(KCPU* cpu) {
|
||||
LIST_ENTRY* head = &gRunQueue[p];
|
||||
for (LIST_ENTRY* node = head->Fwd; node != head; node = node->Fwd) {
|
||||
THREAD* thread = CONTAINING_RECORD(node, THREAD, SchedulerLink);
|
||||
if (KiThreadCanRunOnCpuLocked(thread, cpu))
|
||||
return 1;
|
||||
if (!KiThreadCanRunOnCpuLocked(thread, cpu))
|
||||
continue;
|
||||
if (KiShouldDeferToIdlePrimaryLocked(cpu, thread))
|
||||
continue;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
@@ -385,6 +452,10 @@ static THREAD* KiPickNextForCpu(KCPU* cpu) {
|
||||
THREAD* thread = CONTAINING_RECORD(node, THREAD, SchedulerLink);
|
||||
if (!KiThreadCanRunOnCpuLocked(thread, cpu))
|
||||
continue;
|
||||
if (KiShouldDeferToIdlePrimaryLocked(cpu, thread)) {
|
||||
gSchedStats.SmtSiblingDeferrals++;
|
||||
continue;
|
||||
}
|
||||
|
||||
ListRemove(node);
|
||||
thread->Queued = 0;
|
||||
|
||||
@@ -352,6 +352,7 @@ static void KiRunKernelApcSelfTest(void) {
|
||||
|
||||
typedef struct KI_IRP_APC_SELFTEST_STATE {
|
||||
EVENT* CompletionEvent;
|
||||
DEVICE_OBJECT* DiskDevice;
|
||||
IRP Irp;
|
||||
KAPC Apc;
|
||||
uint8_t* Buffer;
|
||||
@@ -430,23 +431,18 @@ static void KiRunIrpKernelApcSelfTest(const HAL_API* hal) {
|
||||
KiIrpApcSelfTestNormalRoutine,
|
||||
(void*)&st);
|
||||
|
||||
LIST_INIT(&st.Irp.Link);
|
||||
st.Irp.DriveIndex = 0;
|
||||
st.Irp.Flags = 0;
|
||||
st.Irp.Lba = 0;
|
||||
st.Irp.SectorCount = 1;
|
||||
st.Irp.Buffer = st.Buffer;
|
||||
st.Irp.Status = STATUS_FAILURE;
|
||||
st.Irp.Information = 0;
|
||||
st.DiskDevice = IoReferenceDiskDevice(0);
|
||||
if (!st.DiskDevice)
|
||||
KePanic(KE_PANIC_CODE_SELFTEST, "IRP/APC selftest: drive 0 disappeared before submit\n");
|
||||
|
||||
IoBuildReadWriteIrp(&st.Irp, IRP_MJ_READ, 0, 512, st.Buffer);
|
||||
st.Irp.CompletionEvent = st.CompletionEvent;
|
||||
st.Irp.CompletionApc = &st.Apc;
|
||||
st.Irp.CompletionFn = (IRP_COMPLETION_FN)0;
|
||||
st.Irp.CompletionCtx = (void*)0;
|
||||
st.Irp.HwSlot = 0;
|
||||
st.Irp.DriverCtx = (void*)0;
|
||||
|
||||
DbgPrint("IRP/APC selftest: submitting drive=0 lba=0 sectors=1\n");
|
||||
s = IoSubmitIrp(&st.Irp);
|
||||
s = IoSubmitIrp(st.DiskDevice, &st.Irp);
|
||||
if (s != STATUS_PENDING)
|
||||
KePanic(KE_PANIC_CODE_SELFTEST, "IRP/APC selftest: IoSubmitIrp returned %u\n", s);
|
||||
|
||||
@@ -472,6 +468,7 @@ static void KiRunIrpKernelApcSelfTest(const HAL_API* hal) {
|
||||
st.Buffer[510], st.Buffer[511]);
|
||||
|
||||
MmFreePool(st.Buffer);
|
||||
if (st.DiskDevice) IoDereferenceDeviceObject(st.DiskDevice);
|
||||
ObDereferenceObject(&st.CompletionEvent->Header);
|
||||
|
||||
DbgPrint("IRP/APC selftest: PASS\n");
|
||||
|
||||
@@ -321,6 +321,15 @@ static void KiPrepareCpuRecord(uint32_t cpuIndex, const HAL_CPU_INFO* info) {
|
||||
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;
|
||||
@@ -328,6 +337,223 @@ static void KiPrepareCpuRecord(uint32_t cpuIndex, const HAL_CPU_INFO* info) {
|
||||
cpu->SchedulerActive = 0;
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
@@ -510,6 +736,7 @@ KSTATUS KeStartApplicationProcessors(void) {
|
||||
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;
|
||||
}
|
||||
|
||||
+7
-2
@@ -341,7 +341,7 @@ void SystemInitThread(void* arg) {
|
||||
KiResolveSystemImports(args->bootInfo);
|
||||
KiReclaimBootloaderMemory(args->bootInfo);
|
||||
|
||||
if (args->bootInfo->Debug & 2)
|
||||
if (args->bootInfo->Debug & KE_DEBUG_SELFTEST)
|
||||
KiRunKernelSelfTests(args->hal);
|
||||
|
||||
// Launch User-Mode init.exe and restart it until it exits cleanly.
|
||||
@@ -363,6 +363,11 @@ void SystemInitThread(void* arg) {
|
||||
}
|
||||
|
||||
DbgPrint("SystemInitThread: init.exe completed successfully; shutting down.\n");
|
||||
if (args->bootInfo->Debug & (KE_DEBUG_CONSOLE | KE_DEBUG_FILE_LOG)) {
|
||||
KSTATUS logStatus = KeFlushDebugLogToFile(SYS_SV_LITERAL("0:/kernel.log"));
|
||||
if (!KSUCCESS(logStatus))
|
||||
DbgPrint("debuglog: shutdown flush to 0:/kernel.log failed st=0x%x\n", logStatus);
|
||||
}
|
||||
gKmainHal->Power.Shutdown();
|
||||
MmFreePool(args);
|
||||
}
|
||||
@@ -416,7 +421,7 @@ extern void KernelMain(BOOT_INFO* bootInfo) {
|
||||
const HAL_API* hal = HalGetApi();
|
||||
gKmainHal = hal;
|
||||
|
||||
KeInitDebug(hal->Screen.PrintChar, bootInfo->Debug & 1);
|
||||
KeInitDebug(hal->Screen.PrintChar, bootInfo->Debug);
|
||||
|
||||
// CPU startup intentionally lives after HAL initialization but before IRQs
|
||||
// and scheduler startup. HAL owns ACPI/MADT discovery and LAPIC INIT/SIPI;
|
||||
|
||||
Reference in new issue
Block a user