static int isr_hooker_init(void)
{
gate_desc *old_idt, *new_idt;
unsigned long new_idt_page;
pr_info("%s\n", __func__);
/* obtain IDT descriptor */
store_idt(&old_idtr);
old_idt = (gate_desc *)old_idtr.address;
/* prepare new IDT */
new_idt_page = __get_free_page(GFP_KERNEL);
if(!new_idt_page)
return -ENOMEM;
new_idtr.address = new_idt_page;
new_idtr.size = old_idtr.size;
new_idt = (gate_desc *)new_idtr.address;
memcpy(new_idt, old_idt, old_idtr.size);
/* modify the target entry */
orig_isr = gate_offset(new_idt[TRAP_NR]);
pr_info("orig_isr@%p\n", (void*)orig_isr);
pack_gate(&new_idt[TRAP_NR], GATE_INTERRUPT, (unsigned long)stub, 0, 0, __KERNEL_CS);
/* setup new entry */
load_idt((void *)&new_idtr);
smp_call_function((smp_call_func_t)load_idt, &new_idtr, 1);
return 0;
}
/*!
* @fn void cpumon_Set_IDT_Func(idt, func)
*
* @param GATE_STRUCT* - address of the idt vector
* @param PVOID - function to set in IDT
*
* @return None No return needed
*
* @brief Set up the interrupt handler.
* @brief Save the old handler for restoration when done
*
*/
static VOID
cpumon_Set_IDT_Func (
GATE_STRUCT *idt,
PVOID func
)
{
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,25)
_set_gate(&idt[CPU_PERF_VECTOR], GATE_INTERRUPT, (unsigned long) func, 3, 0);
#else
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0)
unsigned long cr0_value;
#endif
GATE_STRUCT local;
// _set_gate() cannot be used because the IDT table is not exported.
pack_gate(&local, GATE_INTERRUPT, (unsigned long)func, 3, 0, __KERNEL_CS);
// From 3.10 kernel, the IDT memory has been moved to a read-only location
// which is controlled by the bit 16 in the CR0 register.
// The write protection should be temporarily released to update the IDT.
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0)
cr0_value = read_cr0();
write_cr0(cr0_value & ~X86_CR0_WP);
#endif
write_idt_entry((idt), CPU_PERF_VECTOR, &local);
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0)
write_cr0(cr0_value);
#endif
#endif
return;
}
/*!
* @fn void cpumon_Set_IDT_Func(idt, func)
*
* @param GATE_STRUCT* - address of the idt vector
* @param PVOID - function to set in IDT
*
* @return None No return needed
*
* @brief Set up the interrupt handler.
* @brief Save the old handler for restoration when done
*
*/
static VOID
cpumon_Set_IDT_Func (
GATE_STRUCT *idt,
PVOID func
)
{
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,25)
_set_gate(&idt[CPU_PERF_VECTOR], GATE_INTERRUPT, (unsigned long) func, 3, 0);
#else
GATE_STRUCT local;
// _set_gate() cannot be used because the IDT table is not exported.
pack_gate(&local, GATE_INTERRUPT, (unsigned long)func, 3, 0, __KERNEL_CS);
write_idt_entry(idt, CPU_PERF_VECTOR, &local);
#endif
return;
}
int registerPageFaultListener(void){
struct desc_ptr idtr;
gate_desc *old_idt, *new_idt;
int retval;
//first, do some initialization work.
retval = initMyFault();
if(retval)
return retval;
//record the default idtr
store_idt(&defaultIDTR);
//read the content of idtr register and get the address of old IDT table
old_idt = (gate_desc *)defaultIDTR.address; //'defaultIDTR' is initialized in 'my_virt_drv_init'
//allocate a page to store the new IDT table
printk(KERN_INFO "Page fault Listener: alloc a page to store new idt table.\n");
newIDTTablePage = __get_free_page(GFP_KERNEL);
if(!newIDTTablePage)
return -ENOMEM;
idtr.address = newIDTTablePage;
idtr.size = defaultIDTR.size;
//copy the old idt table to the new one
new_idt = (gate_desc *)idtr.address;
memcpy(new_idt, old_idt, idtr.size);
pack_gate(&new_idt[PGFAULT_NR], GATE_INTERRUPT, (unsigned long)customPageFault, 0, 0, __KERNEL_CS);
//load idt for all the processors
printk(KERN_INFO "Page fault Listener: Load the new idt table.\n");
load_idt(&idtr);
printk(KERN_INFO "Page fault Listener: new idt table loaded.\n");
smp_call_function(loadMyIDTTable, (void *)&idtr, 1); //wait till all are finished
printk(KERN_INFO "Page fault Listener: all CPUs have loaded the new idt table.\n");
return 0;
}
static inline void _set_gate(int gate, unsigned int type, void *addr, unsigned short seg)
{
__u32 a, b;
pack_gate(&a, &b, (unsigned long)addr, seg, type, 0);
write_idt_entry(idt_table, gate, a, b);
}
