fix MmAllocateContiguous, implement polling AHCI driver.
This commit is contained in:
maksw committed
1 parent
84d6265410
commit
5cf7a20ea2
8 files changed
+990
-27
No files matched your search
+944
-6
@@ -1,9 +1,947 @@
|
||||
#include <hal_api.h>
|
||||
#include <ke_status.h>
|
||||
#include <ke_debug.h>
|
||||
// ahci.c - AHCI driver, polling mode.
|
||||
// Supports SATA drives only (no ATAPI). Registers each found drive with the
|
||||
// HAL disk subsystem via IoRegisterDisk().
|
||||
//
|
||||
// Memory model:
|
||||
// MmAllocateContiguous(n) returns a physical address for n*4K pages.
|
||||
// hal->PhysToVirt(phys) converts to kernel virtual address.
|
||||
// DMA buffers are bounce-buffered (allocated via MmAllocateContiguous) to
|
||||
// avoid needing a VirtToPhys conversion for arbitrary kernel pointers.
|
||||
|
||||
KSTATUS DriverEntry(const HAL_API* hal) {
|
||||
(void)hal;
|
||||
DbgPrint("AHCI: TODO.\n");
|
||||
#include <io_driver.h>
|
||||
#include <mm_phys.h>
|
||||
#include <mm_virt.h>
|
||||
#include <ke_debug.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// FIS types
|
||||
// ---------------------------------------------------------------------------
|
||||
typedef enum {
|
||||
FIS_TYPE_REG_H2D = 0x27,
|
||||
FIS_TYPE_REG_D2H = 0x34,
|
||||
FIS_TYPE_DMA_ACT = 0x39,
|
||||
FIS_TYPE_DMA_SETUP = 0x41,
|
||||
FIS_TYPE_DATA = 0x46,
|
||||
FIS_TYPE_BIST = 0x58,
|
||||
FIS_TYPE_PIO_SETUP = 0x5F,
|
||||
FIS_TYPE_DEV_BITS = 0xA1,
|
||||
} FIS_TYPE;
|
||||
|
||||
#pragma pack(push, 1)
|
||||
// Register FIS - Host to Device (H2D)
|
||||
typedef struct {
|
||||
uint8_t fis_type; // FIS_TYPE_REG_H2D
|
||||
uint8_t pmport : 4;
|
||||
uint8_t rsv0 : 3;
|
||||
uint8_t c : 1; // 1 = Command, 0 = Control
|
||||
uint8_t command;
|
||||
uint8_t featurel;
|
||||
// DW1
|
||||
uint8_t lba0, lba1, lba2;
|
||||
uint8_t device;
|
||||
// DW2
|
||||
uint8_t lba3, lba4, lba5;
|
||||
uint8_t featureh;
|
||||
// DW3
|
||||
uint8_t countl, counth;
|
||||
uint8_t icc;
|
||||
uint8_t control;
|
||||
// DW4
|
||||
uint8_t rsv1[4];
|
||||
} FIS_REG_H2D;
|
||||
|
||||
// Register FIS - Device to Host (D2H)
|
||||
typedef struct {
|
||||
uint8_t fis_type;
|
||||
uint8_t pmport : 4;
|
||||
uint8_t rsv0 : 2;
|
||||
uint8_t i : 1;
|
||||
uint8_t rsv1 : 1;
|
||||
uint8_t status;
|
||||
uint8_t error;
|
||||
uint8_t lba0, lba1, lba2;
|
||||
uint8_t device;
|
||||
uint8_t lba3, lba4, lba5;
|
||||
uint8_t rsv2;
|
||||
uint8_t countl, counth;
|
||||
uint8_t rsv3[2];
|
||||
uint8_t rsv4[4];
|
||||
} FIS_REG_D2H;
|
||||
|
||||
// DMA Setup FIS
|
||||
typedef struct {
|
||||
uint8_t fis_type;
|
||||
uint8_t pmport : 4;
|
||||
uint8_t rsv0 : 1;
|
||||
uint8_t d : 1;
|
||||
uint8_t i : 1;
|
||||
uint8_t a : 1;
|
||||
uint8_t rsved[2];
|
||||
uint64_t DMAbufferID;
|
||||
uint32_t rsvd;
|
||||
uint32_t DMAbufOffset;
|
||||
uint32_t TransferCount;
|
||||
uint32_t resvd;
|
||||
} FIS_DMA_SETUP;
|
||||
|
||||
// PIO Setup FIS
|
||||
typedef struct {
|
||||
uint8_t fis_type;
|
||||
uint8_t pmport : 4;
|
||||
uint8_t rsv0 : 1;
|
||||
uint8_t d : 1;
|
||||
uint8_t i : 1;
|
||||
uint8_t rsv1 : 1;
|
||||
uint8_t status;
|
||||
uint8_t error;
|
||||
uint8_t lba0, lba1, lba2;
|
||||
uint8_t device;
|
||||
uint8_t lba3, lba4, lba5;
|
||||
uint8_t rsv2;
|
||||
uint8_t countl, counth;
|
||||
uint8_t rsv3;
|
||||
uint8_t e_status;
|
||||
uint16_t tc;
|
||||
uint8_t rsv4[2];
|
||||
} FIS_PIO_SETUP;
|
||||
|
||||
// Set Device Bits FIS
|
||||
typedef struct {
|
||||
uint8_t fis_type;
|
||||
uint8_t pmport : 4;
|
||||
uint8_t rsv0 : 2;
|
||||
uint8_t i : 1;
|
||||
uint8_t n : 1;
|
||||
uint8_t statusl : 4;
|
||||
uint8_t rsv1 : 4;
|
||||
uint8_t statush : 4;
|
||||
uint8_t rsv2 : 4;
|
||||
uint8_t error;
|
||||
uint32_t protocol;
|
||||
} FIS_DEV_BITS;
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// HBA structures (volatile - MMIO)
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
// Received FIS structure in host memory
|
||||
typedef volatile struct {
|
||||
FIS_DMA_SETUP dsfis; // 0x00 DMA Setup FIS
|
||||
uint8_t pad0[4];
|
||||
FIS_PIO_SETUP psfis; // 0x20 PIO Setup FIS
|
||||
uint8_t pad1[12];
|
||||
FIS_REG_D2H rfis; // 0x40 Register D2H FIS
|
||||
uint8_t pad2[4];
|
||||
FIS_DEV_BITS sdbfis; // 0x58 Set Device Bits FIS
|
||||
uint8_t ufis[64]; // 0x60 Unknown FIS
|
||||
uint8_t rsv[0x100-0xA0];
|
||||
} HBA_FIS;
|
||||
|
||||
// Command List header (one entry = one slot)
|
||||
typedef struct {
|
||||
// DW0
|
||||
uint8_t cfl : 5; // Command FIS length in DWORDs (2..16)
|
||||
uint8_t a : 1; // ATAPI
|
||||
uint8_t w : 1; // Write (H2D=1, D2H=0)
|
||||
uint8_t p : 1; // Prefetchable
|
||||
uint8_t r : 1; // Reset
|
||||
uint8_t b : 1; // BIST
|
||||
uint8_t c : 1; // Clear busy on R_OK
|
||||
uint8_t rsv0 : 1;
|
||||
uint8_t pmp : 4; // Port multiplier port
|
||||
uint16_t prdtl; // PRDT length in entries
|
||||
// DW1
|
||||
volatile uint32_t prdbc; // PRDT byte count transferred
|
||||
// DW2,3
|
||||
uint32_t ctba; // Command table base address (128-byte aligned)
|
||||
uint32_t ctbau; // Upper 32 bits
|
||||
// DW4-7
|
||||
uint32_t rsv1[4];
|
||||
} HBA_CMD_HEADER;
|
||||
|
||||
// Physical Region Descriptor Table entry
|
||||
typedef struct {
|
||||
uint32_t dba; // Data base address
|
||||
uint32_t dbau; // Upper 32 bits
|
||||
uint32_t rsv0;
|
||||
uint32_t dbc : 22; // Byte count (0-based, so actual = dbc+1)
|
||||
uint32_t rsv1 : 9;
|
||||
uint32_t i : 1; // Interrupt on completion
|
||||
} HBA_PRDT_ENTRY;
|
||||
|
||||
// Command Table
|
||||
typedef struct {
|
||||
uint8_t cfis[64]; // Command FIS (up to 64 bytes)
|
||||
uint8_t acmd[16]; // ATAPI command (12 or 16 bytes)
|
||||
uint8_t rsv[48]; // Reserved
|
||||
HBA_PRDT_ENTRY prdt_entry[1]; // PRDT entries (variable length)
|
||||
} HBA_CMD_TBL;
|
||||
#pragma pack(pop)
|
||||
|
||||
// Port registers
|
||||
typedef volatile struct {
|
||||
uint32_t clb; // 0x00 Command list base (1K-aligned)
|
||||
uint32_t clbu; // 0x04 Command list base upper
|
||||
uint32_t fb; // 0x08 FIS base (256-byte aligned)
|
||||
uint32_t fbu; // 0x0C FIS base upper
|
||||
uint32_t is; // 0x10 Interrupt status
|
||||
uint32_t ie; // 0x14 Interrupt enable
|
||||
uint32_t cmd; // 0x18 Command and status
|
||||
uint32_t rsv0; // 0x1C Reserved
|
||||
uint32_t tfd; // 0x20 Task file data
|
||||
uint32_t sig; // 0x24 Signature
|
||||
uint32_t ssts; // 0x28 SATA status
|
||||
uint32_t sctl; // 0x2C SATA control
|
||||
uint32_t serr; // 0x30 SATA error
|
||||
uint32_t sact; // 0x34 SATA active
|
||||
uint32_t ci; // 0x38 Command issue
|
||||
uint32_t sntf; // 0x3C SATA notification
|
||||
uint32_t fbs; // 0x40 FIS-based switch control
|
||||
uint32_t rsv1[11];
|
||||
uint32_t vendor[4];
|
||||
} HBA_PORT;
|
||||
|
||||
// Generic Host Control registers (starts at ABAR)
|
||||
typedef volatile struct {
|
||||
uint32_t cap; // 0x00 Host capability
|
||||
uint32_t ghc; // 0x04 Global host control
|
||||
uint32_t is; // 0x08 Interrupt status
|
||||
uint32_t pi; // 0x0C Ports implemented
|
||||
uint32_t vs; // 0x10 Version
|
||||
uint32_t ccc_ctl; // 0x14 CCC control
|
||||
uint32_t ccc_pts; // 0x18 CCC ports
|
||||
uint32_t em_loc; // 0x1C Enclosure mgmt location
|
||||
uint32_t em_ctl; // 0x20 Enclosure mgmt control
|
||||
uint32_t cap2; // 0x24 Extended capabilities
|
||||
uint32_t bohc; // 0x28 BIOS/OS handoff control
|
||||
uint8_t rsv[0xA0-0x2C];
|
||||
uint8_t vendor[0x100-0xA0];
|
||||
HBA_PORT ports[32]; // 0x100 Port control registers
|
||||
} HBA_MEM;
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Constants
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
// PCI
|
||||
#define PCI_AHCI_CLASS 0x01
|
||||
#define PCI_AHCI_SUBCLASS 0x06
|
||||
#define PCI_CMD_REG 0x04
|
||||
#define PCI_BAR5_REG 0x24
|
||||
#define PCI_CMD_BUSMASTER (1 << 2)
|
||||
#define PCI_CMD_MEMSPACE (1 << 1)
|
||||
|
||||
// GHC register bits
|
||||
#define GHC_AE (1u << 31) // AHCI Enable
|
||||
#define GHC_IE (1u << 1) // Interrupt Enable
|
||||
#define GHC_HR (1u << 0) // HBA Reset
|
||||
|
||||
// BOHC register bits
|
||||
#define BOHC_BOS (1u << 0) // BIOS-owned semaphore
|
||||
#define BOHC_OOS (1u << 1) // OS-owned semaphore
|
||||
#define BOHC_BB (1u << 4) // BIOS Busy
|
||||
|
||||
// CAP2 bits
|
||||
#define CAP2_BOH (1u << 0) // BIOS/OS handoff capable
|
||||
|
||||
// Port CMD register bits
|
||||
#define PORT_CMD_ST (1u << 0) // Start
|
||||
#define PORT_CMD_FRE (1u << 4) // FIS Receive Enable
|
||||
#define PORT_CMD_FR (1u << 14) // FIS Receive Running
|
||||
#define PORT_CMD_CR (1u << 15) // Command List Running
|
||||
#define PORT_CMD_POD (1u << 2) // Power On Device
|
||||
#define PORT_CMD_SUD (1u << 1) // Spin-Up Device
|
||||
#define PORT_CMD_ICC_ACTIVE (0x1u << 28)
|
||||
|
||||
// Port IS register bits (interrupt status)
|
||||
#define PORT_IS_TFES (1u << 30) // Task File Error Status
|
||||
#define PORT_IS_HBFS (1u << 29) // Host Bus Fatal Error
|
||||
#define PORT_IS_HBDS (1u << 28) // Host Bus Data Error
|
||||
#define PORT_IS_IFS (1u << 27) // Interface Fatal Error
|
||||
|
||||
// Port SSTS register
|
||||
#define PORT_DET_PRESENT 0x3
|
||||
#define PORT_IPM_ACTIVE 0x1
|
||||
|
||||
// Port signatures
|
||||
#define SATA_SIG_ATA 0x00000101
|
||||
#define SATA_SIG_ATAPI 0xEB140101
|
||||
#define SATA_SIG_SEMB 0xC33C0101
|
||||
#define SATA_SIG_PM 0x96690101
|
||||
|
||||
// ATA commands
|
||||
#define ATA_CMD_READ_DMA_EX 0x25
|
||||
#define ATA_CMD_WRITE_DMA_EX 0x35
|
||||
#define ATA_CMD_IDENTIFY 0xEC
|
||||
#define ATA_CMD_FLUSH_EXT 0xEA
|
||||
|
||||
// ATA status bits
|
||||
#define ATA_SR_BUSY 0x80
|
||||
#define ATA_SR_DRQ 0x08
|
||||
#define ATA_SR_ERR 0x01
|
||||
|
||||
// IDENTIFY word offsets
|
||||
#define ATA_IDENT_W83 83 // bit 10 = 48-bit LBA supported
|
||||
#define ATA_IDENT_W100 100 // 48-bit LBA sector count (4 words)
|
||||
#define ATA_IDENT_W60 60 // 28-bit LBA sector count (2 words)
|
||||
|
||||
// Layout constants (per port, single contiguous physical allocation)
|
||||
// Offset 0: Command List 1024 bytes (32 × 32-byte headers)
|
||||
// Offset 1024: Received FIS 256 bytes
|
||||
// Offset 2048: Command Tables 32 × 256 bytes = 8192 bytes
|
||||
// Total: 10240 bytes → 3 pages (12288 bytes)
|
||||
#define PORT_PAGES_STRUCT 3
|
||||
#define PORT_OFF_CMDLIST 0
|
||||
#define PORT_OFF_FIS 1024
|
||||
#define PORT_OFF_CMDTBLS 2048
|
||||
#define PORT_CMDTBL_SIZE 256 // 64 CFIS + 16 ACMD + 48 rsv + 8×16 PRDT
|
||||
#define PORT_PRDT_COUNT 8 // PRDT entries per command table
|
||||
|
||||
// Bounce buffer: 128 sectors × 512 bytes = 64 KB = 16 pages
|
||||
#define BOUNCE_SECTORS 128
|
||||
#define BOUNCE_BYTES (BOUNCE_SECTORS * 512)
|
||||
#define BOUNCE_PAGES (BOUNCE_BYTES / 4096)
|
||||
|
||||
// Polling timeout (spin iterations)
|
||||
#define POLL_TIMEOUT 1000000
|
||||
|
||||
#define MAX_AHCI_DRIVES 8
|
||||
#define NUM_CMD_SLOTS 32
|
||||
|
||||
// Map MMIO physical range into the physical window as uncacheable.
|
||||
// Must be called before any pointer dereference via PhysToVirt().
|
||||
// AHCI MMIO layout: 0x100 GHC + 32*0x80 ports = 0x1100 bytes → 2 pages minimum.
|
||||
// We map 8 pages (32KB) without probing the BAR size.
|
||||
#define AHCI_MMIO_PAGES 8
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Driver-global state
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
static const HAL_API *g_hal;
|
||||
|
||||
// Per-drive context
|
||||
typedef struct {
|
||||
HBA_PORT *port; // MMIO port registers (virtual)
|
||||
uint64_t sectorCount; // Total LBA sectors on device
|
||||
|
||||
// Structures block (command list + FIS + cmd tables)
|
||||
uint64_t structPhys; // Physical base
|
||||
void *structVirt; // Virtual base
|
||||
|
||||
// Bounce buffer for DMA data
|
||||
uint64_t bouncePhys;
|
||||
void *bounceVirt;
|
||||
} AHCI_DRIVE;
|
||||
|
||||
static AHCI_DRIVE g_drives[MAX_AHCI_DRIVES];
|
||||
static int g_driveCount;
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Helpers
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
static inline void *phys_to_virt(uint64_t phys) {
|
||||
return g_hal->PhysToVirt(phys);
|
||||
}
|
||||
|
||||
static KSTATUS ahci_map_mmio(uint64_t physBase) {
|
||||
for (int i = 0; i < AHCI_MMIO_PAGES; i++) {
|
||||
uint64_t phys = physBase + (uint64_t)i * 4096;
|
||||
uintptr_t virt = (uintptr_t)g_hal->PhysToVirt(phys);
|
||||
uint64_t flags = PAGE_PRESENT | PAGE_WRITE | PAGE_NOCACHE;
|
||||
KSTATUS st = MmMapPage(virt, phys, flags);
|
||||
// Ignore STATUS_ALREADY_EXISTS — a prior mapping with different flags
|
||||
// is possible for the first page if the bootloader was overly generous.
|
||||
// Use MmSetPageProtect to force NOCACHE in that case.
|
||||
if (st == STATUS_ALREADY_EXISTS) {
|
||||
st = MmSetPageProtect(virt, 4096, flags);
|
||||
}
|
||||
if (!KSUCCESS(st)) {
|
||||
DbgPrint("AHCI: MmMapPage failed for page %d (phys=0x%llx, st=0x%x)\n",
|
||||
i, (unsigned long long)phys, st);
|
||||
return st;
|
||||
}
|
||||
}
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
// Accessors for port structure memory at known offsets from structVirt/Phys
|
||||
static inline HBA_CMD_HEADER *cmd_list(AHCI_DRIVE *drv) {
|
||||
return (HBA_CMD_HEADER *)((uint8_t *)drv->structVirt + PORT_OFF_CMDLIST);
|
||||
}
|
||||
|
||||
static inline uint64_t cmd_list_phys(AHCI_DRIVE *drv) {
|
||||
return drv->structPhys + PORT_OFF_CMDLIST;
|
||||
}
|
||||
|
||||
static inline uint64_t fis_phys(AHCI_DRIVE *drv) {
|
||||
return drv->structPhys + PORT_OFF_FIS;
|
||||
}
|
||||
|
||||
static inline HBA_CMD_TBL *cmd_tbl(AHCI_DRIVE *drv, int slot) {
|
||||
return (HBA_CMD_TBL *)((uint8_t *)drv->structVirt
|
||||
+ PORT_OFF_CMDTBLS
|
||||
+ (uint32_t)slot * PORT_CMDTBL_SIZE);
|
||||
}
|
||||
|
||||
static inline uint64_t cmd_tbl_phys(AHCI_DRIVE *drv, int slot) {
|
||||
return drv->structPhys + PORT_OFF_CMDTBLS
|
||||
+ (uint64_t)slot * PORT_CMDTBL_SIZE;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Port engine start / stop (polling safe)
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
static void port_stop(HBA_PORT *port) {
|
||||
// Clear ST (bit 0)
|
||||
port->cmd &= ~PORT_CMD_ST;
|
||||
// Wait for CR (bit 15) to clear (command list DMA stops)
|
||||
for (int i = 0; i < POLL_TIMEOUT; i++) {
|
||||
if (!(port->cmd & PORT_CMD_CR)) break;
|
||||
}
|
||||
// Clear FRE (bit 4)
|
||||
port->cmd &= ~PORT_CMD_FRE;
|
||||
// Wait for FR (bit 14) to clear
|
||||
for (int i = 0; i < POLL_TIMEOUT; i++) {
|
||||
if (!(port->cmd & PORT_CMD_FR)) break;
|
||||
}
|
||||
}
|
||||
|
||||
static void port_start(HBA_PORT *port) {
|
||||
// Wait until CR is clear before setting ST
|
||||
for (int i = 0; i < POLL_TIMEOUT; i++) {
|
||||
if (!(port->cmd & PORT_CMD_CR)) break;
|
||||
}
|
||||
port->cmd |= PORT_CMD_FRE;
|
||||
port->cmd |= PORT_CMD_ST;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Port memory initialisation
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
static KSTATUS port_init_memory(AHCI_DRIVE *drv) {
|
||||
uint64_t flags = PAGE_PRESENT | PAGE_WRITE | PAGE_NOCACHE;
|
||||
|
||||
drv->structPhys = MmAllocateContiguous(PORT_PAGES_STRUCT);
|
||||
if (!drv->structPhys) return STATUS_NO_MEMORY;
|
||||
drv->structVirt = g_hal->PhysToVirt(drv->structPhys);
|
||||
// Force NOCACHE on DMA descriptor memory
|
||||
MmSetPageProtect((uintptr_t)drv->structVirt,
|
||||
PORT_PAGES_STRUCT * 4096, flags);
|
||||
memset(drv->structVirt, 0, PORT_PAGES_STRUCT * 4096);
|
||||
|
||||
drv->bouncePhys = MmAllocateContiguous(BOUNCE_PAGES);
|
||||
if (!drv->bouncePhys) return STATUS_NO_MEMORY;
|
||||
drv->bounceVirt = g_hal->PhysToVirt(drv->bouncePhys);
|
||||
// Bounce buffer: NOCACHE to ensure CPU writes reach physical memory
|
||||
// before the HBA's DMA engine reads them
|
||||
MmSetPageProtect((uintptr_t)drv->bounceVirt,
|
||||
BOUNCE_PAGES * 4096, flags);
|
||||
memset(drv->bounceVirt, 0, BOUNCE_BYTES);
|
||||
|
||||
HBA_PORT *port = drv->port;
|
||||
|
||||
port_stop(port);
|
||||
|
||||
// Set command list base
|
||||
port->clb = (uint32_t)(cmd_list_phys(drv) & 0xFFFFFFFF);
|
||||
port->clbu = (uint32_t)(cmd_list_phys(drv) >> 32);
|
||||
|
||||
// Set FIS base
|
||||
port->fb = (uint32_t)(fis_phys(drv) & 0xFFFFFFFF);
|
||||
port->fbu = (uint32_t)(fis_phys(drv) >> 32);
|
||||
|
||||
// Initialise all 32 command headers, pointing each to its command table
|
||||
HBA_CMD_HEADER *hdrs = cmd_list(drv);
|
||||
for (int i = 0; i < NUM_CMD_SLOTS; i++) {
|
||||
hdrs[i].prdtl = PORT_PRDT_COUNT;
|
||||
uint64_t tbl = cmd_tbl_phys(drv, i);
|
||||
hdrs[i].ctba = (uint32_t)(tbl & 0xFFFFFFFF);
|
||||
hdrs[i].ctbau = (uint32_t)(tbl >> 32);
|
||||
}
|
||||
|
||||
// Clear any stale interrupts then start the port engine
|
||||
port->serr = (uint32_t)-1;
|
||||
port->is = (uint32_t)-1;
|
||||
|
||||
port_start(port);
|
||||
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Issue a command (polling, always uses slot 0)
|
||||
// Returns STATUS_SUCCESS or STATUS_DEVICE_ERROR / STATUS_TIMEOUT.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
static KSTATUS port_issue_cmd(AHCI_DRIVE *drv) {
|
||||
HBA_PORT *port = drv->port;
|
||||
|
||||
// Clear pending interrupts before issuing
|
||||
port->is = (uint32_t)-1;
|
||||
|
||||
// Wait for port to be idle (not BSY or DRQ)
|
||||
int spin = 0;
|
||||
while ((port->tfd & (ATA_SR_BUSY | ATA_SR_DRQ)) && spin < POLL_TIMEOUT)
|
||||
spin++;
|
||||
if (spin == POLL_TIMEOUT) {
|
||||
DbgPrint("AHCI: port hung (pre-issue)\n");
|
||||
return STATUS_TIMEOUT;
|
||||
}
|
||||
|
||||
// Issue slot 0
|
||||
port->ci = (1u << 0);
|
||||
|
||||
// Poll until slot 0 clears
|
||||
for (;;) {
|
||||
if (!(port->ci & (1u << 0))) break;
|
||||
if (port->is & (PORT_IS_TFES | PORT_IS_HBFS | PORT_IS_HBDS | PORT_IS_IFS)) {
|
||||
DbgPrint("AHCI: error IS=0x%x TFD=0x%x SERR=0x%x\n",
|
||||
port->is, port->tfd, port->serr);
|
||||
// Reset error bits
|
||||
port->serr = (uint32_t)-1;
|
||||
port->is = (uint32_t)-1;
|
||||
return STATUS_DEVICE_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
// One final error check after CI cleared
|
||||
if (port->is & PORT_IS_TFES) {
|
||||
DbgPrint("AHCI: TFES after completion TFD=0x%x\n", port->tfd);
|
||||
port->is = (uint32_t)-1;
|
||||
return STATUS_DEVICE_ERROR;
|
||||
}
|
||||
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Build a standard DMA read or write command in slot 0
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
static void build_rw_cmd(AHCI_DRIVE *drv, uint64_t lba, uint32_t sectors,
|
||||
uint64_t bufPhys, bool write) {
|
||||
HBA_CMD_HEADER *hdr = cmd_list(drv); // slot 0
|
||||
HBA_CMD_TBL *tbl = cmd_tbl(drv, 0);
|
||||
|
||||
memset(tbl, 0, PORT_CMDTBL_SIZE);
|
||||
|
||||
// How many PRDT entries we need (each covers up to 8K = 16 sectors)
|
||||
uint32_t prdtl = (sectors + 15) / 16;
|
||||
if (prdtl > PORT_PRDT_COUNT) prdtl = PORT_PRDT_COUNT; // capped (see caller)
|
||||
|
||||
hdr->cfl = sizeof(FIS_REG_H2D) / 4;
|
||||
hdr->w = write ? 1 : 0;
|
||||
hdr->prdtl = (uint16_t)prdtl;
|
||||
hdr->prdbc = 0;
|
||||
|
||||
// Fill PRDT entries
|
||||
uint32_t remaining = sectors;
|
||||
uint64_t buf = bufPhys;
|
||||
for (uint32_t i = 0; i < prdtl; i++) {
|
||||
uint32_t sects = (remaining > 16) ? 16 : remaining;
|
||||
uint32_t bytes = sects * 512;
|
||||
|
||||
tbl->prdt_entry[i].dba = (uint32_t)(buf & 0xFFFFFFFF);
|
||||
tbl->prdt_entry[i].dbau = (uint32_t)(buf >> 32);
|
||||
tbl->prdt_entry[i].dbc = bytes - 1; // hardware adds 1
|
||||
tbl->prdt_entry[i].i = (i == prdtl - 1) ? 1 : 0;
|
||||
|
||||
buf += bytes;
|
||||
remaining -= sects;
|
||||
}
|
||||
|
||||
// Build H2D FIS
|
||||
FIS_REG_H2D *fis = (FIS_REG_H2D *)tbl->cfis;
|
||||
memset(fis, 0, sizeof(*fis));
|
||||
fis->fis_type = FIS_TYPE_REG_H2D;
|
||||
fis->c = 1;
|
||||
fis->command = write ? ATA_CMD_WRITE_DMA_EX : ATA_CMD_READ_DMA_EX;
|
||||
fis->device = 1u << 6; // LBA mode
|
||||
|
||||
fis->lba0 = (uint8_t)(lba);
|
||||
fis->lba1 = (uint8_t)(lba >> 8);
|
||||
fis->lba2 = (uint8_t)(lba >> 16);
|
||||
fis->lba3 = (uint8_t)(lba >> 24);
|
||||
fis->lba4 = (uint8_t)(lba >> 32);
|
||||
fis->lba5 = (uint8_t)(lba >> 40);
|
||||
|
||||
fis->countl = (uint8_t)(sectors & 0xFF);
|
||||
fis->counth = (uint8_t)(sectors >> 8);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// IDENTIFY command - fills a 512-byte buffer and returns it
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
static KSTATUS port_identify(AHCI_DRIVE *drv) {
|
||||
// Reuse bounce buffer for IDENTIFY data (512 bytes)
|
||||
memset(drv->bounceVirt, 0, 512);
|
||||
|
||||
HBA_CMD_HEADER *hdr = cmd_list(drv);
|
||||
HBA_CMD_TBL *tbl = cmd_tbl(drv, 0);
|
||||
|
||||
memset(tbl, 0, PORT_CMDTBL_SIZE);
|
||||
|
||||
hdr->cfl = sizeof(FIS_REG_H2D) / 4;
|
||||
hdr->w = 0;
|
||||
hdr->prdtl = 1;
|
||||
hdr->prdbc = 0;
|
||||
|
||||
tbl->prdt_entry[0].dba = (uint32_t)(drv->bouncePhys & 0xFFFFFFFF);
|
||||
tbl->prdt_entry[0].dbau = (uint32_t)(drv->bouncePhys >> 32);
|
||||
tbl->prdt_entry[0].dbc = 512 - 1;
|
||||
tbl->prdt_entry[0].i = 1;
|
||||
|
||||
FIS_REG_H2D *fis = (FIS_REG_H2D *)tbl->cfis;
|
||||
memset(fis, 0, sizeof(*fis));
|
||||
fis->fis_type = FIS_TYPE_REG_H2D;
|
||||
fis->c = 1;
|
||||
fis->command = ATA_CMD_IDENTIFY;
|
||||
fis->device = 0;
|
||||
|
||||
KSTATUS st = port_issue_cmd(drv);
|
||||
if (!KSUCCESS(st)) return st;
|
||||
|
||||
// Parse sector count from IDENTIFY data
|
||||
const uint16_t *id = (const uint16_t *)drv->bounceVirt;
|
||||
|
||||
if (id[ATA_IDENT_W83] & (1u << 10)) {
|
||||
// 48-bit LBA supported
|
||||
drv->sectorCount = (uint64_t)id[ATA_IDENT_W100]
|
||||
| ((uint64_t)id[ATA_IDENT_W100 + 1] << 16)
|
||||
| ((uint64_t)id[ATA_IDENT_W100 + 2] << 32)
|
||||
| ((uint64_t)id[ATA_IDENT_W100 + 3] << 48);
|
||||
} else {
|
||||
// 28-bit LBA
|
||||
drv->sectorCount = (uint64_t)id[ATA_IDENT_W60]
|
||||
| ((uint64_t)id[ATA_IDENT_W60 + 1] << 16);
|
||||
}
|
||||
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// HAL_DISK callbacks (forwarded through per-drive context)
|
||||
// We need one closure per drive. Since C has no closures, we use a small
|
||||
// trampoline table indexed by drive number.
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
// Thin wrappers; the drive index is baked in at registration time by using
|
||||
// separately named static functions. We generate MAX_AHCI_DRIVES of them with
|
||||
// a macro to keep the code DRY without requiring heap-allocated closures.
|
||||
|
||||
// Core read logic
|
||||
static KSTATUS do_read(AHCI_DRIVE *drv, uint64_t lba,
|
||||
uint32_t count, void *buf) {
|
||||
uint8_t *out = (uint8_t *)buf;
|
||||
|
||||
while (count > 0) {
|
||||
uint32_t batch = (count > BOUNCE_SECTORS) ? BOUNCE_SECTORS : count;
|
||||
|
||||
build_rw_cmd(drv, lba, batch, drv->bouncePhys, false);
|
||||
KSTATUS st = port_issue_cmd(drv);
|
||||
if (!KSUCCESS(st)) return st;
|
||||
|
||||
memcpy(out, drv->bounceVirt, (size_t)batch * 512);
|
||||
out += (size_t)batch * 512;
|
||||
lba += batch;
|
||||
count -= batch;
|
||||
}
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
// Core write logic
|
||||
static KSTATUS do_write(AHCI_DRIVE *drv, uint64_t lba,
|
||||
uint32_t count, const void *buf) {
|
||||
const uint8_t *in = (const uint8_t *)buf;
|
||||
|
||||
while (count > 0) {
|
||||
uint32_t batch = (count > BOUNCE_SECTORS) ? BOUNCE_SECTORS : count;
|
||||
|
||||
memcpy(drv->bounceVirt, in, (size_t)batch * 512);
|
||||
build_rw_cmd(drv, lba, batch, drv->bouncePhys, true);
|
||||
KSTATUS st = port_issue_cmd(drv);
|
||||
if (!KSUCCESS(st)) return st;
|
||||
|
||||
in += (size_t)batch * 512;
|
||||
lba += batch;
|
||||
count -= batch;
|
||||
}
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
// Macro-generate per-slot trampoline pairs (read + write)
|
||||
#define AHCI_TRAMPOLINE(N) \
|
||||
static KSTATUS AhciRead##N(uint64_t lba, uint32_t n, void *b) \
|
||||
{ return do_read (&g_drives[N], lba, n, b); } \
|
||||
static KSTATUS AhciWrite##N(uint64_t lba, uint32_t n, const void *b) \
|
||||
{ return do_write(&g_drives[N], lba, n, b); }
|
||||
|
||||
AHCI_TRAMPOLINE(0)
|
||||
AHCI_TRAMPOLINE(1)
|
||||
AHCI_TRAMPOLINE(2)
|
||||
AHCI_TRAMPOLINE(3)
|
||||
AHCI_TRAMPOLINE(4)
|
||||
AHCI_TRAMPOLINE(5)
|
||||
AHCI_TRAMPOLINE(6)
|
||||
AHCI_TRAMPOLINE(7)
|
||||
|
||||
typedef KSTATUS (*RW_FN_R)(uint64_t, uint32_t, void*);
|
||||
typedef KSTATUS (*RW_FN_W)(uint64_t, uint32_t, const void*);
|
||||
|
||||
static const RW_FN_R kReadFns[MAX_AHCI_DRIVES] = {
|
||||
AhciRead0, AhciRead1, AhciRead2, AhciRead3,
|
||||
AhciRead4, AhciRead5, AhciRead6, AhciRead7 };
|
||||
static const RW_FN_W kWriteFns[MAX_AHCI_DRIVES] = {
|
||||
AhciWrite0, AhciWrite1, AhciWrite2, AhciWrite3,
|
||||
AhciWrite4, AhciWrite5, AhciWrite6, AhciWrite7 };
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// AHCI controller initialisation
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
static KSTATUS hba_reset(HBA_MEM *hba) {
|
||||
// BIOS/OS handoff (spec §10.6.3)
|
||||
if (hba->cap2 & CAP2_BOH) {
|
||||
hba->bohc |= BOHC_OOS;
|
||||
// Wait for BIOS to clear BOS
|
||||
for (int i = 0; i < POLL_TIMEOUT; i++) {
|
||||
if (!(hba->bohc & BOHC_BOS)) break;
|
||||
}
|
||||
// Give the BIOS another moment to clear Busy
|
||||
for (int i = 0; i < 800000; i++) {
|
||||
if (!(hba->bohc & BOHC_BB)) break;
|
||||
}
|
||||
}
|
||||
|
||||
// Enable AHCI mode before reset so the reset actually takes effect
|
||||
hba->ghc |= GHC_AE;
|
||||
|
||||
// HBA reset
|
||||
hba->ghc |= GHC_HR;
|
||||
for (int i = 0; i < POLL_TIMEOUT; i++) {
|
||||
if (!(hba->ghc & GHC_HR)) break;
|
||||
}
|
||||
if (hba->ghc & GHC_HR) {
|
||||
DbgPrint("AHCI: HBA reset timed out\n");
|
||||
return STATUS_TIMEOUT;
|
||||
}
|
||||
|
||||
// Re-enable AHCI mode (reset cleared it)
|
||||
hba->ghc |= GHC_AE;
|
||||
|
||||
// Leave GHC.IE clear - we use polling only
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
static bool port_has_sata_drive(HBA_PORT *port) {
|
||||
uint32_t det = port->ssts & 0x0F;
|
||||
uint32_t ipm = (port->ssts >> 8) & 0x0F;
|
||||
if (det != PORT_DET_PRESENT) return false;
|
||||
if (ipm != PORT_IPM_ACTIVE) return false;
|
||||
return port->sig == SATA_SIG_ATA; // SATA disk only (no ATAPI for now)
|
||||
}
|
||||
|
||||
// Before memory init: only check physical link. Signature is not yet valid.
|
||||
static bool port_link_up(HBA_PORT *port) {
|
||||
uint32_t det = port->ssts & 0x0F;
|
||||
uint32_t ipm = (port->ssts >> 8) & 0x0F;
|
||||
return (det == PORT_DET_PRESENT && ipm == PORT_IPM_ACTIVE);
|
||||
}
|
||||
|
||||
// After COMRESET + FIS receive: check signature for SATA disk.
|
||||
static bool port_is_sata_disk(HBA_PORT *port) {
|
||||
return port->sig == SATA_SIG_ATA;
|
||||
}
|
||||
|
||||
static void ahci_delay_ms(uint32_t ms) {
|
||||
for (uint32_t i = 0; i < ms * 1000; i++)
|
||||
__outbyte(0x80, 0);
|
||||
}
|
||||
|
||||
// Issue a COMRESET on the port, wait for link re-establishment and D2H FIS.
|
||||
// FRE must already be set (port_init_memory does this) so the HBA can
|
||||
// capture the signature FIS the device sends after the reset.
|
||||
static KSTATUS port_comreset(AHCI_DRIVE *drv) {
|
||||
HBA_PORT *port = drv->port;
|
||||
|
||||
port->cmd &= ~PORT_CMD_ST;
|
||||
for (int i = 0; i < POLL_TIMEOUT; i++)
|
||||
if (!(port->cmd & PORT_CMD_CR)) break;
|
||||
|
||||
// Assert COMRESET: SControl.DET = 1, hold for ≥ 1ms
|
||||
port->sctl = (port->sctl & ~0x0Fu) | 0x1u;
|
||||
ahci_delay_ms(1);
|
||||
|
||||
// Deassert: DET = 0
|
||||
port->sctl = port->sctl & ~0x0Fu;
|
||||
|
||||
// Poll for DET = 3, up to 1 second
|
||||
bool link_ok = false;
|
||||
for (int i = 0; i < 1000; i++) {
|
||||
if ((port->ssts & 0x0Fu) == PORT_DET_PRESENT) { link_ok = true; break; }
|
||||
ahci_delay_ms(1);
|
||||
}
|
||||
if (!link_ok) {
|
||||
DbgPrint("AHCI: COMRESET: link did not come back (ssts=0x%x)\n", port->ssts);
|
||||
return STATUS_TIMEOUT;
|
||||
}
|
||||
|
||||
port->serr = (uint32_t)-1;
|
||||
port->is = (uint32_t)-1;
|
||||
port->cmd |= PORT_CMD_ST;
|
||||
|
||||
// Wait up to 500ms for a valid signature (D2H FIS from device)
|
||||
for (int i = 0; i < 500; i++) {
|
||||
uint32_t sig = port->sig;
|
||||
if (sig != 0xFFFFFFFFu && sig != 0x00000000u) break;
|
||||
ahci_delay_ms(1);
|
||||
}
|
||||
|
||||
DbgPrint("AHCI: port COMRESET done: sig=0x%08x ssts=0x%08x\n",
|
||||
port->sig, port->ssts);
|
||||
return STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
KSTATUS AhciInitialize(const HAL_API *hal) {
|
||||
g_hal = hal;
|
||||
g_driveCount = 0;
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 1. Locate AHCI controller via PCI class scan
|
||||
// -----------------------------------------------------------------------
|
||||
uint8_t pciBus, pciDev, pciFn;
|
||||
if (!hal->Pci.FindClass(PCI_AHCI_CLASS, PCI_AHCI_SUBCLASS,
|
||||
&pciBus, &pciDev, &pciFn)) {
|
||||
DbgPrint("AHCI: no AHCI controller found\n");
|
||||
return STATUS_NOT_FOUND;
|
||||
}
|
||||
DbgPrint("AHCI: controller at PCI %02x:%02x.%x\n", pciBus, pciDev, pciFn);
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 2. Enable Bus Master + Memory Space in PCI command register
|
||||
// -----------------------------------------------------------------------
|
||||
uint32_t cmd = hal->Pci.ConfigRead32(pciBus, pciDev, pciFn, PCI_CMD_REG);
|
||||
cmd |= (PCI_CMD_BUSMASTER | PCI_CMD_MEMSPACE);
|
||||
hal->Pci.ConfigWrite32(pciBus, pciDev, pciFn, PCI_CMD_REG, cmd);
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 3. Read ABAR (BAR5) and map as uncacheable
|
||||
// The physical address is already identity-mapped via PhysToVirt.
|
||||
// NOTE: real drivers should mark the mapping WC or UC; here we rely
|
||||
// on the kernel not caching MMIO ranges (caller's responsibility).
|
||||
// -----------------------------------------------------------------------
|
||||
uint32_t bar5 = hal->Pci.ConfigRead32(pciBus, pciDev, pciFn, PCI_BAR5_REG);
|
||||
uint64_t abarPhys = (uint64_t)(bar5 & ~0xFFFu); // strip flags
|
||||
KSTATUS st = ahci_map_mmio(abarPhys); // establish PTEs
|
||||
if (!KSUCCESS(st)) return st;
|
||||
|
||||
HBA_MEM *hba = (HBA_MEM *)hal->PhysToVirt(abarPhys);
|
||||
|
||||
DbgPrint("AHCI: ABAR phys=0x%llx version=0x%08x\n",
|
||||
(unsigned long long)abarPhys, hba->vs);
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 4. HBA reset + BIOS handoff
|
||||
// -----------------------------------------------------------------------
|
||||
st = hba_reset(hba);
|
||||
if (!KSUCCESS(st)) return st;
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 5. Log capability summary
|
||||
// -----------------------------------------------------------------------
|
||||
uint32_t cap = hba->cap;
|
||||
uint32_t nPorts = (cap & 0x1F) + 1; // bits 4:0 + 1
|
||||
uint32_t nSlots = ((cap >> 8) & 0x1F) + 1;
|
||||
bool s64a = !!(cap & (1u << 31));
|
||||
DbgPrint("AHCI: cap=0x%08x ports=%u slots=%u 64bit=%d\n",
|
||||
cap, nPorts, nSlots, (int)s64a);
|
||||
|
||||
// -----------------------------------------------------------------------
|
||||
// 6. Enumerate implemented ports
|
||||
// -----------------------------------------------------------------------
|
||||
uint32_t pi = hba->pi;
|
||||
for (uint32_t portno = 0; portno < 32 && g_driveCount < MAX_AHCI_DRIVES; portno++) {
|
||||
if (!(pi & (1u << portno))) continue;
|
||||
|
||||
HBA_PORT *port = &hba->ports[portno];
|
||||
|
||||
// Phase 1: check physical link only — sig is garbage until after COMRESET
|
||||
if (!port_link_up(port)) {
|
||||
DbgPrint("AHCI: port %u: no link (ssts=0x%08x)\n", portno, port->ssts);
|
||||
continue;
|
||||
}
|
||||
|
||||
DbgPrint("AHCI: port %u: link up, initializing...\n", portno);
|
||||
|
||||
int driveIdx = g_driveCount;
|
||||
AHCI_DRIVE *drv = &g_drives[driveIdx];
|
||||
memset(drv, 0, sizeof(*drv));
|
||||
drv->port = port;
|
||||
|
||||
// Phase 2: set up command list + FIS buffer (enables FRE)
|
||||
st = port_init_memory(drv);
|
||||
if (!KSUCCESS(st)) {
|
||||
DbgPrint("AHCI: port %u: memory init failed (0x%x)\n", portno, st);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Phase 3: COMRESET to flush stale HBA state and force device to
|
||||
// re-send D2H FIS so the SIG register gets a valid value
|
||||
st = port_comreset(drv);
|
||||
if (!KSUCCESS(st)) {
|
||||
DbgPrint("AHCI: port %u: COMRESET failed\n", portno);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Phase 4: now sig is valid — filter to SATA disks only
|
||||
if (!port_is_sata_disk(port)) {
|
||||
DbgPrint("AHCI: port %u: not a SATA disk (sig=0x%08x), skipping\n",
|
||||
portno, port->sig);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Phase 5: IDENTIFY to get sector count
|
||||
st = port_identify(drv);
|
||||
if (!KSUCCESS(st)) {
|
||||
DbgPrint("AHCI: port %u: IDENTIFY failed (0x%x)\n", portno, st);
|
||||
continue;
|
||||
}
|
||||
|
||||
DbgPrint("AHCI: drive %d: %llu sectors (%llu MB)\n",
|
||||
driveIdx,
|
||||
(unsigned long long)drv->sectorCount,
|
||||
(unsigned long long)(drv->sectorCount / 2048));
|
||||
|
||||
DISK_DRIVER dd = {
|
||||
.ReadSectors = kReadFns[driveIdx],
|
||||
.WriteSectors = kWriteFns[driveIdx],
|
||||
.SectorCount = drv->sectorCount,
|
||||
};
|
||||
st = IoRegisterDisk((uint8_t)driveIdx, &dd);
|
||||
if (KSUCCESS(st)) {
|
||||
DbgPrint("AHCI: drive %d registered\n", driveIdx);
|
||||
g_driveCount++; // only increment on full success
|
||||
} else {
|
||||
DbgPrint("AHCI: IoRegisterDisk(%d) failed (0x%x)\n", driveIdx, st);
|
||||
}
|
||||
}
|
||||
|
||||
DbgPrint("AHCI: initialised %d drive(s)\n", g_driveCount);
|
||||
return (g_driveCount > 0) ? STATUS_SUCCESS : STATUS_NOT_FOUND;
|
||||
}
|
||||
|
||||
KSTATUS DriverEntry(const HAL_API *hal) {
|
||||
DbgPrint("AHCI: driver loading...\n");
|
||||
KSTATUS st = AhciInitialize(hal);
|
||||
if (!KSUCCESS(st))
|
||||
DbgPrint("AHCI: init failed: 0x%x\n", st);
|
||||
return st;
|
||||
}
|
||||
@@ -1,5 +1,6 @@
|
||||
[boot]
|
||||
ide.sys
|
||||
ahci.sys
|
||||
fat32.sys
|
||||
|
||||
[runtime]
|
||||
|
||||
@@ -39,7 +39,15 @@ static uint64_t QueryTicks(void) { return gTicks; }
|
||||
static void Stall(uint32_t us) {
|
||||
uint64_t start = gTicks;
|
||||
uint64_t ms = (us + 999) / 1000;
|
||||
while ((gTicks - start) < ms) _mm_pause();
|
||||
|
||||
// Spin up to 100ms worth of IO delays waiting for ticks to advance.
|
||||
// If gTicks doesn't move (interrupts off), fall back to pure IO delay.
|
||||
uint32_t io_iters = (uint32_t)(ms * 1000);
|
||||
for (uint32_t i = 0; i < io_iters; i++) {
|
||||
if ((gTicks - start) >= ms) return; // interrupt-driven path
|
||||
__outbyte(0x80, 0);
|
||||
}
|
||||
// If we exhausted the IO delay loop we've waited long enough anyway.
|
||||
}
|
||||
|
||||
// Returns HPET ticks/second, or 0 if HPET is unavailable.
|
||||
|
||||
+33
-19
@@ -115,43 +115,57 @@ PHYSICAL_ADDRESS MmAllocateFrame(void) {
|
||||
}
|
||||
|
||||
void MmFreeFrame(PHYSICAL_ADDRESS frame) {
|
||||
if (!frame || (frame & (PAGE_SIZE - 1))) return; // must be page-aligned
|
||||
PushFrame(frame);
|
||||
if (!frame || (frame & (PAGE_SIZE - 1))) return;
|
||||
KeAcquireSpinlock(&gFrameStack.Lock);
|
||||
if (gFrameStack.Top < gFrameStack.Capacity)
|
||||
gFrameStack.Stack[gFrameStack.Top++] = frame;
|
||||
// else: stack full — leak rather than corrupt
|
||||
KeReleaseSpinlock(&gFrameStack.Lock);
|
||||
}
|
||||
|
||||
PHYSICAL_ADDRESS MmAllocateContiguous(size_t count) {
|
||||
if (!count) return (PHYSICAL_ADDRESS)0;
|
||||
|
||||
// Brute-force: scan the stack for 'count' consecutive frames.
|
||||
// Acceptable for small DMA buffers; not for general use.
|
||||
KeAcquireSpinlock(&gFrameStack.Lock);
|
||||
|
||||
// Build a temporary sorted view - too expensive; instead do a simple
|
||||
// linear scan for a contiguous run already on the stack (lucky case),
|
||||
// or fall back to pulling frames one by one and checking adjacency.
|
||||
// For now: pull 'count' frames and verify they are contiguous.
|
||||
// If not, return them all and report failure.
|
||||
if (gFrameStack.Top < count) {
|
||||
KeReleaseSpinlock(&gFrameStack.Lock);
|
||||
return (PHYSICAL_ADDRESS)0;
|
||||
}
|
||||
|
||||
// Attempt: take the top 'count' entries and check.
|
||||
PHYSICAL_ADDRESS base = gFrameStack.Stack[gFrameStack.Top - 1];
|
||||
int ok = 1;
|
||||
for (size_t i = 0; i < count; i++) {
|
||||
PHYSICAL_ADDRESS f = gFrameStack.Stack[gFrameStack.Top - 1 - i];
|
||||
if (f != base + i * PAGE_SIZE) { ok = 0; break; }
|
||||
// Frames are pushed in ascending address order by MmInitializePhysical,
|
||||
// so physically consecutive pages sit at consecutive stack indices.
|
||||
// Scan from bottom upward for a run of 'count' adjacent frames.
|
||||
size_t run = (size_t)-1;
|
||||
for (size_t i = 0; i + count <= gFrameStack.Top; i++) {
|
||||
int ok = 1;
|
||||
for (size_t j = 1; j < count; j++) {
|
||||
if (gFrameStack.Stack[i + j] != gFrameStack.Stack[i] + j * PAGE_SIZE) {
|
||||
ok = 0;
|
||||
// Skip past this broken run; the next candidate can't start
|
||||
// before the frame that broke the sequence.
|
||||
i += j - 1; // outer loop adds 1 more
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (ok) { run = i; break; }
|
||||
}
|
||||
|
||||
if (ok) {
|
||||
gFrameStack.Top -= count;
|
||||
if (run == (size_t)-1) {
|
||||
KeReleaseSpinlock(&gFrameStack.Lock);
|
||||
return base;
|
||||
return (PHYSICAL_ADDRESS)0;
|
||||
}
|
||||
|
||||
PHYSICAL_ADDRESS base = gFrameStack.Stack[run];
|
||||
|
||||
// Splice out the 'count' entries by sliding the tail down.
|
||||
size_t tail = gFrameStack.Top - run - count;
|
||||
for (size_t i = 0; i < tail; i++)
|
||||
gFrameStack.Stack[run + i] = gFrameStack.Stack[run + count + i];
|
||||
gFrameStack.Top -= count;
|
||||
|
||||
KeReleaseSpinlock(&gFrameStack.Lock);
|
||||
return (PHYSICAL_ADDRESS)0; // caller should retry or fail gracefully
|
||||
return base;
|
||||
}
|
||||
|
||||
size_t MmFreeFrameCount(void) {
|
||||
|
||||
Reference in new issue
Block a user