//-------------------------------------------------------------------
//	hdtraps.c
//
//	This module sets a debug trap for i/o to the Hard Disk's
//	Command/Status Register and installs a trap-handler that
//	logs information about each access to that port-address.
//	It also creates a pseudo-file named '/proc/hdtraps' that
//	displays information about these logged entries.
//
//	NOTE: Written and tested with Linux kernel version 2.6.10
//
//	programmer: ALLAN CRUSE
//	written on: 02 MAR 2005
//-------------------------------------------------------------------

#include <linux/module.h>	// for init_module() 
#include <linux/proc_fs.h>	// for create_proc_info_entry() 

#define TRAP_ID		1
#define RESUME_FLAG	(1<<16)
#define IDE_COMMAND	0x1F7
#define MAX_INFO	256

typedef struct	{
		unsigned long	eip;
		unsigned long	eax;
		unsigned long	jif;
		unsigned short	opn;
		unsigned short	itm;
		} TRAPINFO;

char modname[] = "hdtraps";
unsigned short idtr[3];
unsigned long long *idt, gatesave;
unsigned long cr4save, quitting = 0;
int	head = 0, tail = 0;
TRAPINFO info[ MAX_INFO ];

static int my_get_info( char *buf, char **start, off_t off, int count )
{
	int	i, k, len = 0;
	
	for (k = 0; k < 32; k++)
		{
		if ( ( i = head ) == tail ) break;
		len += sprintf( buf+len, "%5u: ", info[i].itm );
		len += sprintf( buf+len, "jiffies=%08lX ", info[i].jif );
		len += sprintf( buf+len, "eip=%08lX ", info[i].eip );
		len += sprintf( buf+len, "eax=%08lX ", info[i].eax );
		len += sprintf( buf+len, "opn=%04X ", info[i].opn );
		len += sprintf( buf+len, "\n" );
		i = ( i+1 )%MAX_INFO;
		head = i;
		}
	return	len;
}

void trap_handler( unsigned long *tos )
{
	static unsigned short 	rep = 0;
	unsigned long	db_status;
	int		i = tail;

	asm(" movl %dr6, %eax ");
	asm(" movl %%eax, %0 " : "=m" (db_status) );
	asm(" xorl %eax, %eax ");
	asm(" movl %eax, %dr6 ");

	info[i].jif = jiffies;	
	info[i].eip = tos[ 10 ];
	info[i].eax = tos[  9 ];
	info[i].itm = ++rep;
	info[i].opn = *(unsigned short*)(tos[ 10 ] - 2);	
	i = (i+1)%MAX_INFO;
	if ( i != head ) tail = i;	// commited

	// if DB-bit is set, skip next instruction (unless quitting)
	if (( !quitting )&&( db_status & (1<<13) )) tos[10] += 3;	
		
	if ( db_status & 1 )	// breakpoint-matched
		{
		asm(" movl %dr7, %eax ");
		asm(" bts   $13, %eax ");
		asm(" movl %eax, %dr7 ");
		}

	tos[ 12 ] |= RESUME_FLAG;
}

//-----------------------------------------
asmlinkage void isr_debug( void );
asm("	.type	isr_debug, @function	");
asm("isr_debug:				");
asm("	pushal				");
asm("	pushl	%ds			");
asm("	pushl	%es			");
//
asm("	movl	%ss, %eax		");
asm("	movl	%eax, %ds		");
asm("	movl	%eax, %es		");
//
asm("	pushl	%esp			");
asm("	call	trap_handler		");
asm("	addl	$4, %esp		");
//
asm("	popl	%es			");
asm("	popl	%ds			");
asm("	popal				");
asm("	iret				");
//-----------------------------------------

MODULE_LICENSE("GPL"); 

int init_module( void )
{
	unsigned long long	desc;	

	// enable debug extensions
	asm(" movl %cr4, %eax ");
	asm(" movl %eax, cr4save ");
	asm(" bts $3, %eax ");
	asm(" movl %eax, %cr4 ");

	// preserve original trap-gate
	asm(" sidtl idtr ");
	idt = (unsigned long long *)(*(unsigned long*)(idtr+1));
	gatesave = idt[ TRAP_ID ];

	// install new gate-descriptor
	desc = (unsigned long long)isr_debug;
	desc &= 0x00000000FFFFFFFFLL;
	desc |= ( desc << 32 );
	desc &= 0xFFFF00000000FFFFLL;
	desc |= ( __KERNEL_CS << 16 );
	desc |= 0x00008F0000000000LL;	
	idt[ TRAP_ID ] = desc;

	// set i/o breakpoint
	asm(" movl %0, %%eax " : : "i" (IDE_COMMAND) );
	asm(" movl %eax, %dr0 ");
	asm(" movl $0x00020002, %eax ");
	asm(" bts  $13, %eax ");	// General Detect Enable
	asm(" movl %eax, %dr7 ");

	create_proc_info_entry( modname, 0, NULL, my_get_info );
	printk( "<1>\nInstalled \'%s\' module\n", modname );
	return	0;  //SUCCESS
}

void cleanup_module( void )
{
	printk( "<1>Removing \'%s\' module\n", modname );
	remove_proc_entry( modname, NULL );
	
	// disable our breakpoints
	quitting = 1;
	asm(" xorl %eax, %eax ");
	asm(" movl %eax, %dr7 ");
	asm(" movl %eax, %dr6 ");
	asm(" movl %eax, %dr0 ");

	// restore gate-descriptor
	idt[ TRAP_ID ] = gatesave;

	// restore original CR4 
	asm(" movl cr4save, %eax ");
	asm(" movl %eax, %cr4 ");
}