embedded/arch/arm/generic/libs/misc/undef_support.c

   1 #include <cpu_def.h>
   2 #include <irq_def.h>
   3
   4 static int undef_initialized = 0;
   5
   6 static unsigned long cpu_undef_stack[256];
   7
   8 struct undef_hook *undef_hook_chain = NULL;
   9
  10 static void undef_exception_handler(int excptnum, struct pt_regs *regs)
  11 {
  12   /*unsigned int correction = thumb_mode(regs) ? 2 : 4;*/
  13   unsigned int correction = 0;
  14   struct undef_hook *hook;
  15   void *pc;
  16   unsigned long instr;
  17
  18   regs->ARM_pc -= correction;
  19
  20   pc = (void *)regs->ARM_pc;
  21
  22   instr = *(unsigned long *)pc;
  23
  24   for(hook = undef_hook_chain; hook; hook = hook->next) {
  25     if (((instr & hook->instr_mask) == hook->instr_val) &&
  26       ((regs->ARM_cpsr & hook->cpsr_mask) == hook->cpsr_val)) {
  27       if (hook->fn(regs, instr) == 0) {
  28         return;
  29       }
  30     }
  31   }
  32
  33   for(;;){
  34     /* Fatal error */
  35   }
  36 }
  37
  38 int register_undef_hook(struct undef_hook *hook)
  39 {
  40   unsigned long flags;
  41
  42   save_and_cli(flags);
  43
  44   if(!undef_initialized) {
  45     set_cpu_exception_handler(ARM_EXCEPTION_UNDEF, (long)undef_exception_handler);
  46     set_cpu_exception_stack(ARM_EXCEPTION_UNDEF, (long)((char*)cpu_undef_stack+sizeof(cpu_undef_stack)-8));
  47     undef_initialized = 1;
  48   }
  49
  50   hook->next = undef_hook_chain;
  51   undef_hook_chain = hook;
  52   restore_flags(flags);
  53
  54   return 0;
  55 }