35 lines
1.0 KiB
C
35 lines
1.0 KiB
C
// Pure DDK support library routines; no kernel or module-wide state.
|
|
|
|
#include <ntlite/ddk/pci_caps.h>
|
|
|
|
KSTATUS NtlPciFindCapability(const NTL_PCI_CONFIG_READER* reader,
|
|
uint8_t firstOffset,
|
|
uint8_t capabilityId,
|
|
uint8_t* outOffset) {
|
|
if (!reader || !reader->Read32 || !outOffset || !capabilityId)
|
|
return NTLSTATUS_INVALID_PARAMETER;
|
|
|
|
*outOffset = 0;
|
|
uint8_t current = firstOffset;
|
|
uint64_t visited = 0;
|
|
|
|
while (current) {
|
|
if (current < 0x40u || (current & 3u) != 0u)
|
|
return NTLSTATUS_DEVICE_ERROR;
|
|
|
|
uint64_t bit = 1ull << (current / 4u - 16u);
|
|
if (visited & bit)
|
|
return NTLSTATUS_DEVICE_ERROR;
|
|
visited |= bit;
|
|
|
|
uint32_t header = reader->Read32(reader->Context, current);
|
|
if ((uint8_t)header == capabilityId) {
|
|
*outOffset = current;
|
|
return NTLSTATUS_SUCCESS;
|
|
}
|
|
current = (uint8_t)((header >> 8) & 0xFCu);
|
|
}
|
|
|
|
return NTLSTATUS_NOT_FOUND;
|
|
}
|