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