fault_handl_good.h~

#include <linux/module.h>
#include <linux/kernel.h>
#include "pg_flag_mod.h"
#include "hde28c/hde28.c"
#include "sidt.h"
#include "idt.h"

void* old_page_fault = 0;
uint32_t old_dr0 = 0;
void* old_debug = 0;

idt_entry* pf_ent_p = 0;

void new_debug(void)
{
  // TODO: here we check where did the exception came from
  
  // Clear the debug status register
  set_debugreg(0, 6);
}
void new_page_fault(void)
{
  // Saving all the registers before modifying them.
  __asm__("pushad");
  
  uint32_t eip = 0; // The pointer to the instruction.
  uint16_t cs; // The CS register (stored in stack).
  char* instruction_buf = kmalloc(8, GFP_KERNEL);

  // Here we get the required data from stack (note: after the
  // "pushad" instruction the offset in stack is 32 bytes).
  __asm__("movl 32(%%esp), %0\n\t"
          : "=r"(eip));

  __asm__("movw 36(%%esp), %0\n\t"
          : "=r"(cs));

  // And here we get the instruction itself (in two steps, copying 8
  // bytes).
  __asm__("movl %0:%1,     (%2)\n\t"
          "movl %0:4(%1), 4(%2)\n\t"
          : "=r"(instruction_buf)
          : "r"(cs),"r"(eip));
  
  // The disassembly is performed here in order to see the page to
  // which that instruction accessed
  
  hde32s instr_disasm;
  hde32_disasm(instruction_buf, &instr_disasm);

  // The query is from the kernel (not kernel modules),
  // everything OK.
  // Note: here I assume that the kernel itself doesn't
  // cause page faults.
  // TODO: change 0x1 by something more universal.
  if ((eip >> 24) == 0xc1)
    {
      // Here we set up the debugging register
      // to stop at the next instruction
      // in order to clear the P flag again
      // and save the previous value of DR0.
      get_debugreg(old_dr0, 0);
      set_debugreg(eip + instr_disasm.len, 0);
      
      // Now we open access to the requested page
      set_present(virt_to_pte(instr_disasm.imm32));
      
      // Here we return from the exception handler,
      // the command executes, the breakpoint is activated,
      // the P flag is cleared again
      __asm__("popad"
              "iret");
      
      __asm__("popad" // Restoring the registers for normal operation
              "jmpl *%0\n\t"
              : // No output
              : "m"(old_page_fault));
    }
}

int init_module()
{
  pf_ent_p = get_idt_entry(get_idtr(), 0x0e);
  old_page_fault = (void*)((pf_ent_p->offset_high << 16) +
                           pf_ent_p->offset_low);

  // Setting the new handler
  modify_idt_entry_addr(pf_ent_p, &new_page_fault);

  // Here we'll clear the P flag on some pages
  // TODO

  return 0;
}

void cleanup_module()
{
  // Here we'll set the P flag on some pages
  // TODO
  modify_idt_entry_addr(pf_ent_p, &old_page_fault);
}