kprobe/arch/asm-mips/dbi_kprobes.c

   1 /*
   2  *  Dynamic Binary Instrumentation Module based on KProbes
   3  *  modules/kprobe/arch/asm-mips/dbi_kprobes.c
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation; either version 2 of the License, or
   8  * (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program; if not, write to the Free Software
  17  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  18  *
  19  * Copyright (C) Samsung Electronics, 2006-2010
  20  *
  21  * 2006-2007    Ekaterina Gorelkina <e.gorelkina@samsung.com>: initial implementation for ARM/MIPS
  22  * 2008-2009    Alexey Gerenkov <a.gerenkov@samsung.com> User-Space
  23  *              Probes initial implementation; Support x86/ARM/MIPS for both user-space and kernel space.
  24  * 2010         Ekaterina Gorelkina <e.gorelkina@samsung.com>: redesign module for separating core and arch parts
  25  *
  26  */
  27
  28 #include "dbi_kprobes.h"
  29 #include "../dbi_kprobes.h"
  30 #include "../../dbi_kprobes.h"
  31
  32 #include "../../dbi_kdebug.h"
  33 #include "../../dbi_insn_slots.h"
  34 #include "../../dbi_kprobes_deps.h"
  35
  36 /*
  37  * Function return probe trampoline:
  38  *      - init_kprobes() establishes a probepoint here
  39  *      - When the probed function returns, this probe
  40  *              causes the handlers to fire
  41  */
  42 void kretprobe_trampoline_holder (void)
  43 {
  44         asm volatile (".global kretprobe_trampoline\n"
  45                         "kretprobe_trampoline:\n"
  46                         "nop\n"
  47                         "nop\n");
  48 }
  49
  50
  51 struct kprobe trampoline_p =
  52 {
  53         .addr = (kprobe_opcode_t *) & kretprobe_trampoline,
  54         .pre_handler = trampoline_probe_handler
  55 };
  56
  57 void gen_insn_execbuf_holder (void)
  58 {
  59         asm volatile (".global gen_insn_execbuf\n"
  60                         "gen_insn_execbuf:\n"
  61                         "nop\n"                 // original instruction
  62                         "nop\n"                 //ssbreak
  63                         "nop\n");               //retbreak
  64 }
  65
  66
  67 int arch_check_insn (struct arch_specific_insn *ainsn)
  68 {
  69         int ret = 0;
  70
  71         switch (MIPS_INSN_OPCODE (ainsn->insn[0]))
  72         {
  73                 case MIPS_BEQ_OPCODE:   //B, BEQ
  74                 case MIPS_BEQL_OPCODE:  //BEQL
  75                 case MIPS_BNE_OPCODE:   //BNE
  76                 case MIPS_BNEL_OPCODE:  //BNEL
  77                 case MIPS_BGTZ_OPCODE:  //BGTZ
  78                 case MIPS_BGTZL_OPCODE: //BGTZL
  79                 case MIPS_BLEZ_OPCODE:  //BLEZ
  80                 case MIPS_BLEZL_OPCODE: //BLEZL
  81                 case MIPS_J_OPCODE:     //J
  82                 case MIPS_JAL_OPCODE:   //JAL
  83                         DBPRINTF ("arch_check_insn: opcode");
  84                         ret = -EFAULT;
  85                         break;
  86                 case MIPS_REGIMM_OPCODE:
  87                         //BAL, BGEZ, BGEZAL, BGEZALL, BGEZL, BLTZ, BLTZAL, BLTZALL, BLTZL
  88                         switch (MIPS_INSN_RT (ainsn->insn[0]))
  89                         {
  90                                 case MIPS_BLTZ_RT:
  91                                 case MIPS_BGEZ_RT:
  92                                 case MIPS_BLTZL_RT:
  93                                 case MIPS_BGEZL_RT:
  94                                 case MIPS_BLTZAL_RT:
  95                                 case MIPS_BGEZAL_RT:
  96                                 case MIPS_BLTZALL_RT:
  97                                 case MIPS_BGEZALL_RT:
  98                                         DBPRINTF ("arch_check_insn: REGIMM opcode\n");
  99                                         ret = -EFAULT;
 100                                         break;
 101                         }
 102                         break;
 103                         //BC1F, BC1FL, BC1T, BC1TL
 104                 case MIPS_COP1_OPCODE:
 105                         //BC2F, BC2FL, BC2T, BC2TL
 106                 case MIPS_COP2_OPCODE:
 107                         if (MIPS_INSN_RS (ainsn->insn[0]) == MIPS_BC_RS)
 108                         {
 109                                 DBPRINTF ("arch_check_insn: COP1 opcode\n");
 110                                 ret = -EFAULT;
 111                         }
 112                         break;
 113                 case MIPS_SPECIAL_OPCODE:
 114                         //BREAK, JALR, JALR.HB, JR, JR.HB
 115                         switch (MIPS_INSN_FUNC (ainsn->insn[0]))
 116                         {
 117                                 case MIPS_JR_FUNC:
 118                                 case MIPS_JALR_FUNC:
 119                                 case MIPS_BREAK_FUNC:
 120                                 case MIPS_SYSCALL_FUNC:
 121                                         DBPRINTF ("arch_check_insn: SPECIAL opcode\n");
 122                                         ret = -EFAULT;
 123                                         break;
 124                         }
 125                         break;
 126         }
 127         return ret;
 128 }
 129
 130 int arch_prepare_kretprobe (struct kretprobe *p)
 131 {
 132         DPRINF("Warrning: arch_prepare_kretprobe is not implemented\n");
 133         return 0;
 134 }
 135
 136 int arch_prepare_kprobe (struct kprobe *p)
 137 {
 138         int ret = 0;
 139         if (!ret)
 140         {
 141                 kprobe_opcode_t insn[MAX_INSN_SIZE];
 142                 struct arch_specific_insn ainsn;
 143                 /* insn: must be on special executable page on i386. */
 144                 p->ainsn.insn = get_insn_slot (NULL, 0);
 145                 if (!p->ainsn.insn)
 146                         return -ENOMEM;
 147                 memcpy (insn, p->addr, MAX_INSN_SIZE * sizeof (kprobe_opcode_t));
 148                 ainsn.insn = insn;
 149                 ret = arch_check_insn (&ainsn);
 150                 if (!ret)
 151                 {
 152                         p->opcode = *p->addr;
 153                         p->ainsn.boostable = 0;
 154                         memcpy (insns, gen_insn_execbuf, sizeof (insns));
 155                         insns[KPROBES_TRAMP_INSN_IDX] = insn[0];
 156                         insns[KPROBES_TRAMP_SS_BREAK_IDX] = BREAKPOINT_INSTRUCTION;
 157                         insns[KPROBES_TRAMP_RET_BREAK_IDX] = UNDEF_INSTRUCTION;
 158                         DBPRINTF ("arch_prepare_kprobe: insn %lx", insn[0]);
 159                         DBPRINTF ("arch_prepare_kprobe: to %p - %lx %lx %lx",
 160                                         p->ainsn.insn, insns[0], insns[1], insns[2]);
 161                         memcpy (p->ainsn.insn, insns, sizeof(insns));
 162                 }
 163                 else
 164                 {
 165                         free_insn_slot (&kprobe_insn_pages, NULL, p->ainsn.insn, 0);
 166                 }
 167         }
 168
 169         return ret;
 170 }
 171
 172 int arch_prepare_uprobe (struct kprobe *p, struct task_struct *task, int atomic)
 173 {
 174         int ret = 0;
 175         kprobe_opcode_t insns[UPROBES_TRAMP_LEN];
 176
 177         if ((unsigned long) p->addr & 0x01)
 178         {
 179                 DBPRINTF ("Attempt to register kprobe at an unaligned address");
 180                 ret = -EINVAL;
 181         }
 182
 183         if (!ret)
 184         {
 185                 kprobe_opcode_t insn[MAX_INSN_SIZE];
 186                 struct arch_specific_insn ainsn;
 187
 188                 if (!read_proc_vm_atomic (task, (unsigned long) p->addr, &insn, MAX_INSN_SIZE * sizeof(kprobe_opcode_t)))
 189                         panic ("failed to read memory %p!\n", p->addr);
 190                 ainsn.insn = insn;
 191                 ret = arch_check_insn (&ainsn);
 192                 if (!ret)
 193                 {
 194                         p->opcode = insn[0];
 195                         p->ainsn.insn = get_insn_slot(task, atomic);
 196                         if (!p->ainsn.insn)
 197                                 return -ENOMEM;
 198                         p->ainsn.boostable = 0;
 199                         memcpy (insns, gen_insn_execbuf, sizeof (insns));
 200                         insns[UPROBES_TRAMP_INSN_IDX] = insn[0];
 201                         insns[UPROBES_TRAMP_SS_BREAK_IDX] = BREAKPOINT_INSTRUCTION;
 202                         insns[UPROBES_TRAMP_RET_BREAK_IDX] = UNDEF_INSTRUCTION;
 203                         DBPRINTF ("arch_prepare_uprobe: insn %lx", insn[0]);
 204                         DBPRINTF ("arch_prepare_uprobe: to %p - %lx %lx %lx",
 205                                         p->ainsn.insn, insns[0], insns[1], insns[2]);
 206
 207                         if (!write_proc_vm_atomic (task, (unsigned long) p->ainsn.insn, insns, sizeof (insns)))
 208                         {
 209                                 panic("failed to write memory %p!\n", p->ainsn.insn);
 210                                 DBPRINTF ("failed to write insn slot to process memory: insn %p, addr %p, probe %p!", insn, p->ainsn.insn, p->addr);
 211                                 /*printk ("failed to write insn slot to process memory: %p/%d insn %lx, addr %p, probe %p!\n",
 212                                   task, task->pid, insn, p->ainsn.insn, p->addr);*/
 213                                 free_insn_slot (&uprobe_insn_pages, task, p->ainsn.insn, 0);
 214                                 return -EINVAL;
 215                         }
 216                 }
 217         }
 218
 219         return ret;
 220 }
 221
 222 int arch_prepare_uretprobe (struct kretprobe *p, struct task_struct *task)
 223 {
 224         DPRINF("Warrning: arch_prepare_uretprobe is not implemented\n");
 225         return 0;
 226 }
 227
 228 void prepare_singlestep (struct kprobe *p, struct pt_regs *regs)
 229 {
 230         if(p->ss_addr)
 231         {
 232                 regs->cp0_epc = (unsigned long) p->ss_addr;
 233                 p->ss_addr = NULL;
 234         }
 235         else
 236                 regs->cp0_epc = (unsigned long) p->ainsn.insn;
 237 }
 238
 239
 240 void save_previous_kprobe (struct kprobe_ctlblk *kcb, struct kprobe *cur_p)
 241 {
 242         if (kcb->prev_kprobe.kp != NULL)
 243         {
 244                 panic ("no space to save new probe[%lu]: task = %d/%s, prev %d/%p, current %d/%p, new %d/%p,",
 245                                 nCount, current->pid, current->comm, kcb->prev_kprobe.kp->tgid, kcb->prev_kprobe.kp->addr,
 246                                 kprobe_running()->tgid, kprobe_running()->addr, cur_p->tgid, cur_p->addr);
 247         }
 248
 249         kcb->prev_kprobe.kp = kprobe_running ();
 250         kcb->prev_kprobe.status = kcb->kprobe_status;
 251 }
 252
 253 void restore_previous_kprobe (struct kprobe_ctlblk *kcb)
 254 {
 255         __get_cpu_var (current_kprobe) = kcb->prev_kprobe.kp;
 256         kcb->kprobe_status = kcb->prev_kprobe.status;
 257         kcb->prev_kprobe.kp = NULL;
 258         kcb->prev_kprobe.status = 0;
 259 }
 260
 261 void set_current_kprobe (struct kprobe *p, struct pt_regs *regs, struct kprobe_ctlblk *kcb)
 262 {
 263         __get_cpu_var (current_kprobe) = p;
 264         DBPRINTF ("set_current_kprobe[%lu]: p=%p addr=%p\n", nCount, p, p->addr);
 265 }
 266
 267 int kprobe_handler (struct pt_regs *regs)
 268 {
 269         struct kprobe *p = 0;
 270         int ret = 0, pid = 0, retprobe = 0, reenter = 0;
 271         kprobe_opcode_t *addr = NULL, *ssaddr = 0;
 272         struct kprobe_ctlblk *kcb;
 273
 274         nCount++;
 275         /* We're in an interrupt, but this is clear and BUG()-safe. */
 276
 277         addr = (kprobe_opcode_t *) regs->cp0_epc;
 278         DBPRINTF ("regs->regs[ 31 ] = 0x%lx\n", regs->regs[31]);
 279
 280         preempt_disable ();
 281
 282         kcb = get_kprobe_ctlblk ();
 283
 284         if (user_mode (regs))
 285         {
 286                 //DBPRINTF("exception[%lu] from user mode %s/%u addr %p (%lx).", nCount, current->comm, current->pid, addr, regs->uregs[14]);
 287                 pid = current->tgid;
 288         }
 289
 290         /* Check we're not actually recursing */
 291         if (kprobe_running ())
 292         {
 293                 DBPRINTF ("lock???");
 294                 p = get_kprobe (addr, pid, current);
 295                 if (p)
 296                 {
 297                         if(!pid && (addr == (kprobe_opcode_t *)kretprobe_trampoline)){
 298                                 save_previous_kprobe (kcb, p);
 299                                 kcb->kprobe_status = KPROBE_REENTER;
 300                                 reenter = 1;
 301                         }
 302                         else {
 303                                 /* We have reentered the kprobe_handler(), since
 304                                  * another probe was hit while within the handler.
 305                                  * We here save the original kprobes variables and
 306                                  * just single step on the instruction of the new probe
 307                                  * without calling any user handlers.
 308                                  */
 309                                 if(!p->ainsn.boostable){
 310                                         save_previous_kprobe (kcb, p);
 311                                         set_current_kprobe (p, regs, kcb);
 312                                 }
 313                                 kprobes_inc_nmissed_count (p);
 314                                 prepare_singlestep (p, regs);
 315                                 if(!p->ainsn.boostable)
 316                                         kcb->kprobe_status = KPROBE_REENTER;
 317                                 preempt_enable_no_resched ();
 318                                 return 1;
 319                         }
 320                 }
 321                 else
 322                 {
 323                         if(pid) { //we can reenter probe upon uretprobe exception
 324                                 DBPRINTF ("check for UNDEF_INSTRUCTION %p\n", addr);
 325                                 // UNDEF_INSTRUCTION from user space
 326                                 p = get_kprobe_by_insn_slot (addr-UPROBES_TRAMP_RET_BREAK_IDX, pid, current);
 327                                 if (p) {
 328                                         save_previous_kprobe (kcb, p);
 329                                         kcb->kprobe_status = KPROBE_REENTER;
 330                                         reenter = 1;
 331                                         retprobe = 1;
 332                                         DBPRINTF ("uretprobe %p\n", addr);
 333                                 }
 334                         }
 335                         if(!p) {
 336                                 p = __get_cpu_var (current_kprobe);
 337                                 DBPRINTF ("kprobe_running !!! p = 0x%p p->break_handler = 0x%p", p, p->break_handler);
 338                                 /*if (p->break_handler && p->break_handler(p, regs)) {
 339                                   DBPRINTF("kprobe_running !!! goto ss");
 340                                   goto ss_probe;
 341                                   } */
 342                                 DBPRINTF ("unknown uprobe at %p cur at %p/%p\n", addr, p->addr, p->ainsn.insn);
 343                                 if(pid)
 344                                         ssaddr = p->ainsn.insn + UPROBES_TRAMP_SS_BREAK_IDX;
 345                                 else
 346                                         ssaddr = p->ainsn.insn + KPROBES_TRAMP_SS_BREAK_IDX;
 347                                 if (addr == ssaddr)
 348                                 {
 349                                         regs->cp0_epc = (unsigned long) (p->addr + 1);
 350                                         DBPRINTF ("finish step at %p cur at %p/%p, redirect to %lx\n", addr, p->addr, p->ainsn.insn, regs->cp0_epc);
 351
 352                                         if (kcb->kprobe_status == KPROBE_REENTER) {
 353                                                 restore_previous_kprobe (kcb);
 354                                         }
 355                                         else {
 356                                                 reset_current_kprobe ();
 357                                         }
 358                                 }
 359                                 DBPRINTF ("kprobe_running !!! goto no");
 360                                 ret = 1;
 361                                 /* If it's not ours, can't be delete race, (we hold lock). */
 362                                 DBPRINTF ("no_kprobe");
 363                                 goto no_kprobe;
 364                         }
 365                 }
 366         }
 367
 368         //if(einsn != UNDEF_INSTRUCTION) {
 369         DBPRINTF ("get_kprobe %p-%d", addr, pid);
 370         if (!p)
 371                 p = get_kprobe (addr, pid, current);
 372         if (!p)
 373         {
 374                 if(pid) {
 375                         DBPRINTF ("search UNDEF_INSTRUCTION %p\n", addr);
 376                         // UNDEF_INSTRUCTION from user space
 377                         p = get_kprobe_by_insn_slot (addr-UPROBES_TRAMP_RET_BREAK_IDX, pid, current);
 378                         if (!p) {
 379                                 /* Not one of ours: let kernel handle it */
 380                                 DBPRINTF ("no_kprobe");
 381                                 //printk("no_kprobe2 ret = %d\n", ret);
 382                                 goto no_kprobe;
 383                         }
 384                         retprobe = 1;
 385                         DBPRINTF ("uretprobe %p\n", addr);
 386                 }
 387                 else {
 388                         /* Not one of ours: let kernel handle it */
 389                         DBPRINTF ("no_kprobe");
 390                         //printk("no_kprobe2 ret = %d\n", ret);
 391                         goto no_kprobe;
 392                 }
 393         }
 394
 395         set_current_kprobe (p, regs, kcb);
 396         if(!reenter)
 397                 kcb->kprobe_status = KPROBE_HIT_ACTIVE;
 398
 399         if (retprobe)           //(einsn == UNDEF_INSTRUCTION)
 400                 ret = trampoline_probe_handler (p, regs);
 401         else if (p->pre_handler)
 402         {
 403                 ret = p->pre_handler (p, regs);
 404                 if(!p->ainsn.boostable)
 405                         kcb->kprobe_status = KPROBE_HIT_SS;
 406                 else if(p->pre_handler != trampoline_probe_handler)
 407                         reset_current_kprobe ();
 408         }
 409
 410         if (ret)
 411         {
 412                 DBPRINTF ("p->pre_handler[%lu] 1", nCount);
 413                 /* handler has already set things up, so skip ss setup */
 414                 return 1;
 415         }
 416         DBPRINTF ("p->pre_handler 0");
 417
 418 no_kprobe:
 419         preempt_enable_no_resched ();
 420         return ret;
 421 }
 422
 423 void patch_suspended_task_ret_addr(struct task_struct *p, struct kretprobe *rp)
 424 {
 425         DPRINTF("patch_suspended_task_ret_addr is not implemented");
 426 }
 427
 428 int setjmp_pre_handler (struct kprobe *p, struct pt_regs *regs)
 429 {
 430         struct jprobe *jp = container_of (p, struct jprobe, kp);
 431         kprobe_pre_entry_handler_t pre_entry;
 432         entry_point_t entry;
 433
 434 # ifdef REENTER
 435         p = __get_cpu_var (current_kprobe);
 436 # endif
 437
 438         DBPRINTF ("pjp = 0x%p jp->entry = 0x%p", jp, jp->entry);
 439         entry = (entry_point_t) jp->entry;
 440         pre_entry = (kprobe_pre_entry_handler_t) jp->pre_entry;
 441         //if(!entry)
 442         //      DIE("entry NULL", regs)
 443         DBPRINTF ("entry = 0x%p jp->entry = 0x%p", entry, jp->entry);
 444
 445         //call handler for all kernel probes and user space ones which belong to current tgid
 446         if (!p->tgid || (p->tgid == current->tgid))
 447         {
 448                 if(!p->tgid && (p->addr == sched_addr) && sched_rp){
 449                         struct task_struct *p, *g;
 450                         rcu_read_lock();
 451                         //swapper task
 452                         if(current != &init_task)
 453                                 patch_suspended_task_ret_addr(&init_task, sched_rp);
 454                         // other tasks
 455                         do_each_thread(g, p){
 456                                 if(p == current)
 457                                         continue;
 458                                 patch_suspended_task_ret_addr(p, sched_rp);
 459                         } while_each_thread(g, p);
 460                         rcu_read_unlock();
 461                 }
 462                 if (pre_entry)
 463                         p->ss_addr = (void *)pre_entry (jp->priv_arg, regs);
 464                 if (entry){
 465                         entry (regs->regs[4], regs->regs[5], regs->regs[6], regs->regs[7], regs->regs[8], regs->regs[9]);
 466                 }
 467                 else {
 468                         if (p->tgid)
 469                                 arch_uprobe_return ();
 470                         else
 471                                 jprobe_return ();
 472                 }
 473         }
 474         else if (p->tgid)
 475                 arch_uprobe_return ();
 476
 477         prepare_singlestep (p, regs);
 478
 479         return 1;
 480 }
 481
 482
 483 void __kprobes jprobe_return (void)
 484 {
 485         preempt_enable_no_resched();
 486 }
 487
 488 void __kprobes arch_uprobe_return (void)
 489 {
 490         preempt_enable_no_resched();
 491 }
 492
 493 int longjmp_break_handler (struct kprobe *p, struct pt_regs *regs)
 494 {
 495 # ifndef REENTER
 496         //kprobe_opcode_t insn = BREAKPOINT_INSTRUCTION;
 497         kprobe_opcode_t insns[2];
 498
 499         if (p->pid)
 500         {
 501                 insns[0] = BREAKPOINT_INSTRUCTION;
 502                 insns[1] = p->opcode;
 503                 //p->opcode = *p->addr;
 504                 if (read_proc_vm_atomic (current, (unsigned long) (p->addr), &(p->opcode), sizeof (p->opcode)) < sizeof (p->opcode))
 505                 {
 506                         printk ("ERROR[%lu]: failed to read vm of proc %s/%u addr %p.", nCount, current->comm, current->pid, p->addr);
 507                         return -1;
 508                 }
 509                 //*p->addr = BREAKPOINT_INSTRUCTION;
 510                 //*(p->addr+1) = p->opcode;
 511                 if (write_proc_vm_atomic (current, (unsigned long) (p->addr), insns, sizeof (insns)) < sizeof (insns))
 512                 {
 513                         printk ("ERROR[%lu]: failed to write vm of proc %s/%u addr %p.", nCount, current->comm, current->pid, p->addr);
 514                         return -1;
 515                 }
 516         }
 517         else
 518         {
 519                 DBPRINTF ("p->opcode = 0x%lx *p->addr = 0x%lx p->addr = 0x%p\n", p->opcode, *p->addr, p->addr);
 520                 *(p->addr + 1) = p->opcode;
 521                 p->opcode = *p->addr;
 522                 *p->addr = BREAKPOINT_INSTRUCTION;
 523                 flush_icache_range ((unsigned int) p->addr, (unsigned int) (((unsigned int) p->addr) + (sizeof (kprobe_opcode_t) * 2)));
 524         }
 525
 526         reset_current_kprobe ();
 527         return 0;
 528 }
 529
 530 void __kprobes arch_arm_kprobe (struct kprobe *p)
 531 {
 532         *p->addr = BREAKPOINT_INSTRUCTION;
 533         flush_icache_range ((unsigned long) p->addr, (unsigned long) p->addr + sizeof (kprobe_opcode_t));
 534 }
 535
 536 void __kprobes arch_disarm_kprobe (struct kprobe *p)
 537 {
 538         *p->addr = p->opcode;
 539         flush_icache_range ((unsigned long) p->addr, (unsigned long) p->addr + sizeof (kprobe_opcode_t));
 540 }
 541
 542 int __kprobes trampoline_probe_handler (struct kprobe *p, struct pt_regs *regs)
 543 {
 544         struct kretprobe_instance *ri = NULL;
 545         struct hlist_head *head, empty_rp;
 546         struct hlist_node *node, *tmp;
 547         unsigned long flags, orig_ret_address = 0;
 548         unsigned long trampoline_address = (unsigned long) &kretprobe_trampoline;
 549         struct kretprobe *crp = NULL;
 550         struct kprobe_ctlblk *kcb = get_kprobe_ctlblk ();
 551
 552         DBPRINTF ("start");
 553
 554         if (p && p->tgid){
 555                 // in case of user space retprobe trampoline is at the Nth instruction of US tramp
 556                 trampoline_address = (unsigned long)(p->ainsn.insn + UPROBES_TRAMP_RET_BREAK_IDX);
 557         }
 558
 559         INIT_HLIST_HEAD (&empty_rp);
 560         spin_lock_irqsave (&kretprobe_lock, flags);
 561         head = kretprobe_inst_table_head (current);
 562
 563         /*
 564          * It is possible to have multiple instances associated with a given
 565          * task either because an multiple functions in the call path
 566          * have a return probe installed on them, and/or more then one
 567          * return probe was registered for a target function.
 568          *
 569          * We can handle this because:
 570          *     - instances are always inserted at the head of the list
 571          *     - when multiple return probes are registered for the same
 572          *       function, the first instance's ret_addr will point to the
 573          *       real return address, and all the rest will point to
 574          *       kretprobe_trampoline
 575          */
 576         hlist_for_each_entry_safe (ri, node, tmp, head, hlist)
 577         {
 578                 if (ri->task != current)
 579                         /* another task is sharing our hash bucket */
 580                         continue;
 581                 if (ri->rp && ri->rp->handler){
 582                         ri->rp->handler (ri, regs, ri->rp->priv_arg);
 583
 584                 }
 585
 586                 orig_ret_address = (unsigned long) ri->ret_addr;
 587                 recycle_rp_inst (ri, &empty_rp);
 588                 if (orig_ret_address != trampoline_address)
 589                         /*
 590                          * This is the real return address. Any other
 591                          * instances associated with this task are for
 592                          * other calls deeper on the call stack
 593                          */
 594                         break;
 595         }
 596         kretprobe_assert (ri, orig_ret_address, trampoline_address);
 597         //BUG_ON(!orig_ret_address || (orig_ret_address == trampoline_address));
 598         if (trampoline_address != (unsigned long) &kretprobe_trampoline){
 599                 if (ri->rp2) BUG_ON (ri->rp2->kp.tgid == 0);
 600                 if (ri->rp) BUG_ON (ri->rp->kp.tgid == 0);
 601                 else if (ri->rp2) BUG_ON (ri->rp2->kp.tgid == 0);
 602         }
 603         if ((ri->rp && ri->rp->kp.tgid) || (ri->rp2 && ri->rp2->kp.tgid))
 604                 BUG_ON (trampoline_address == (unsigned long) &kretprobe_trampoline);
 605
 606         regs->regs[31] = orig_ret_address;
 607         DBPRINTF ("regs->cp0_epc = 0x%lx", regs->cp0_epc);
 608         if (trampoline_address != (unsigned long) &kretprobe_trampoline)
 609                 regs->cp0_epc = orig_ret_address;
 610         else
 611                 regs->cp0_epc = regs->cp0_epc + 4;
 612         DBPRINTF ("regs->cp0_epc = 0x%lx", regs->cp0_epc);
 613         DBPRINTF ("regs->cp0_status = 0x%lx", regs->cp0_status);
 614
 615         if(p){ // ARM, MIPS, X86 user space
 616                 if (kcb->kprobe_status == KPROBE_REENTER)
 617                         restore_previous_kprobe (kcb);
 618                 else
 619                         reset_current_kprobe ();
 620
 621                 //TODO: test - enter function, delete us retprobe, exit function
 622                 // for user space retprobes only - deferred deletion
 623                 if (trampoline_address != (unsigned long) &kretprobe_trampoline)
 624                 {
 625                         // if we are not at the end of the list and current retprobe should be disarmed
 626                         if (node && ri->rp2)
 627                         {
 628                                 crp = ri->rp2;
 629                                 /*sprintf(die_msg, "deferred disarm p->addr = %p [%lx %lx %lx]\n",
 630                                   crp->kp.addr, *kaddrs[0], *kaddrs[1], *kaddrs[2]);
 631                                   DIE(die_msg, regs); */
 632                                 // look for other instances for the same retprobe
 633                                 hlist_for_each_entry_continue (ri, node, hlist)
 634                                 {
 635                                         if (ri->task != current)
 636                                                 continue;       /* another task is sharing our hash bucket */
 637                                         if (ri->rp2 == crp)     //if instance belong to the same retprobe
 638                                                 break;
 639                                 }
 640                                 if (!node)
 641                                 {       // if there are no more instances for this retprobe
 642                                         // delete retprobe
 643                                         DBPRINTF ("defered retprobe deletion p->addr = %p", crp->kp.addr);
 644                                         unregister_uprobe (&crp->kp, current, 1);
 645                                         kfree (crp);
 646                                 }
 647                         }
 648                 }
 649         }
 650
 651         spin_unlock_irqrestore (&kretprobe_lock, flags);
 652         hlist_for_each_entry_safe (ri, node, tmp, &empty_rp, hlist)
 653         {
 654                 hlist_del (&ri->hlist);
 655                 kfree (ri);
 656         }
 657         preempt_enable_no_resched ();
 658         /*
 659          * By returning a non-zero value, we are telling
 660          * kprobe_handler() that we don't want the post_handler
 661          * to run (and have re-enabled preemption)
 662          */
 663         return 1;
 664 }
 665
 666 void __kprobes __arch_prepare_kretprobe (struct kretprobe *rp, struct pt_regs *regs)
 667 {
 668
 669         struct kretprobe_instance *ri;
 670
 671         DBPRINTF ("start\n");
 672         //TODO: test - remove retprobe after func entry but before its exit
 673         if ((ri = get_free_rp_inst (rp)) != NULL)
 674         {
 675                 ri->rp = rp;
 676                 ri->rp2 = NULL;
 677                 ri->task = current;
 678 #if defined(CONFIG_MIPS)
 679                 ri->ret_addr = (kprobe_opcode_t *) regs->regs[31];
 680                 if (rp->kp.tgid)
 681                         regs->regs[31] = (unsigned long) (rp->kp.ainsn.insn + UPROBES_TRAMP_RET_BREAK_IDX);
 682                 else    /* Replace the return addr with trampoline addr */
 683                         regs->regs[31] = (unsigned long) &kretprobe_trampoline;
 684                 add_rp_inst (ri);
 685         }
 686         else {
 687                 DBPRINTF ("WARNING: missed retprobe %p\n", rp->kp.addr);
 688                 rp->nmissed++;
 689         }
 690 }
 691
 692 int asm_init_module_dependencies()
 693 {
 694         INIT_MOD_DEP_CB(flush_icache_range, r4k_flush_icache_range);
 695         INIT_MOD_DEP_CB(flush_icache_page, r4k_flush_icache_page);
 696         INIT_MOD_DEP_CB(flush_cache_page, r4k_flush_cache_page);
 697
 698         return 0;
 699 }
 700
 701
 702 int __init arch_init_kprobes (void)
 703 {
 704         unsigned int do_bp_handler;
 705         unsigned int kprobe_handler_addr;
 706
 707         unsigned int num_insns = 0;
 708         unsigned int code_size = 0;
 709
 710         unsigned int reg_hi;
 711         unsigned int reg_lo;
 712
 713         if (!arch_init_module_dependencies())
 714         {
 715                 DBPRINTF ("Unable to init module dependencies\n");
 716                 return -1;
 717         }
 718
 719         do_bp_handler = (unsigned int) kallsyms_search ("do_bp");
 720
 721         kprobe_handler_addr = (unsigned int) &kprobe_handler;
 722         insns_num = sizeof (arr_traps_template) / sizeof (arr_traps_template[0]);
 723         code_size = insns_num * sizeof (unsigned int);
 724         DBPRINTF ("insns_num = %d\n", insns_num);
 725         // Save original code
 726         arr_traps_original = kmalloc (code_size, GFP_KERNEL);
 727         if (!arr_traps_original)
 728         {
 729                 DBPRINTF ("Unable to allocate space for original code of <do_bp>!\n");
 730                 return -1;
 731         }
 732         memcpy (arr_traps_original, (void *) do_bp_handler, code_size);
 733
 734         xRegHi = HIWORD (xKProbeHandler);
 735         xRegLo = LOWORD (xKProbeHandler);
 736         if (xRegLo >= 0x8000)
 737                 xRegHi += 0x0001;
 738         arrTrapsTemplate[REG_HI_INDEX] |= xRegHi;
 739         arrTrapsTemplate[REG_LO_INDEX] |= xRegLo;
 740
 741         // Insert new code
 742         memcpy ((void *) do_bp_handler, arr_traps_template, code_size);
 743         flush_icache_range (do_bp_handler, do_bp_handler + code_size);
 744         if((ret = register_kprobe (&trampoline_p, 0)) != 0){
 745                 //unregister_jprobe(&do_exit_p, 0);
 746                 return ret;
 747         }
 748 }
 749
 750 void __exit arch_exit_kprobes (void)
 751 {
 752         unsigned int do_bp_handler;
 753
 754         unsigned int insns_num = 0;
 755         unsigned int code_size = 0;
 756
 757         // Get instruction address
 758         do_bp_handler = (unsigned int) kallsyms_search ("do_undefinstr");
 759
 760         //unregister_jprobe(&do_exit_p, 0);
 761
 762         // Replace back the original code
 763
 764         insns_num = sizeof (arr_traps_template) / sizeof (arr_traps_template[0]);
 765         code_size = insns_num * sizeof (unsigned int);
 766         memcpy ((void *) do_bp_handler, arr_traps_original, code_size);
 767         flush_icache_range (do_bp_handler, do_bp_handler + code_size);
 768         kfree (arr_traps_original);
 769         arr_traps_original = NULL;
 770 }
 771
 772 EXPORT_SYMBOL_GPL (arch_uprobe_return);
 773 EXPORT_SYMBOL_GPL (arch_exit_kprobes);
 774