kprobe/dbi_kprobes.c

   1 // src_kprobes.c
   2
   3 /*
   4  *  Kernel Probes (KProbes)
   5  *  kernel/kprobes.c
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 2 of the License, or
  10  * (at your option) any later version.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with this program; if not, write to the Free Software
  19  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  20  *
  21  * Copyright (C) IBM Corporation, 2002, 2004
  22  */
  23
  24 /*
  25  *  Dynamic Binary Instrumentation Module based on KProbes
  26  *  modules/kprobe/dbi_kprobes.h
  27  *
  28  * This program is free software; you can redistribute it and/or modify
  29  * it under the terms of the GNU General Public License as published by
  30  * the Free Software Foundation; either version 2 of the License, or
  31  * (at your option) any later version.
  32  *
  33  * This program is distributed in the hope that it will be useful,
  34  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  35  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  36  * GNU General Public License for more details.
  37  *
  38  * You should have received a copy of the GNU General Public License
  39  * along with this program; if not, write to the Free Software
  40  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  41  *
  42  * Copyright (C) Samsung Electronics, 2006-2010
  43  *
  44  * 2006-2007    Ekaterina Gorelkina <e.gorelkina@samsung.com>: initial implementation for ARM and MIPS
  45  * 2008-2009    Alexey Gerenkov <a.gerenkov@samsung.com> User-Space
  46  *              Probes initial implementation; Support x86/ARM/MIPS for both user and kernel spaces.
  47  * 2010         Ekaterina Gorelkina <e.gorelkina@samsung.com>: redesign module for separating core and arch parts
  48  *
  49  */
  50
  51 #include "dbi_kprobes.h"
  52 #include "arch/dbi_kprobes.h"
  53 #include "arch/asm/dbi_kprobes.h"
  54
  55 #include "dbi_kdebug.h"
  56 #include "dbi_kprobes_deps.h"
  57 #include "dbi_insn_slots.h"
  58 #include "dbi_uprobes.h"
  59 #include <ksyms.h>
  60
  61 #include <linux/version.h>
  62 #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19)
  63 #include <linux/config.h>
  64 #endif
  65
  66 #include <linux/hash.h>
  67 #include <linux/module.h>
  68 #include <linux/mm.h>
  69 #include <linux/pagemap.h>
  70
  71
  72
  73 extern unsigned long sched_addr;
  74 extern unsigned long fork_addr;
  75 extern struct hlist_head kprobe_insn_pages;
  76
  77 DEFINE_PER_CPU (struct kprobe *, current_kprobe) = NULL;
  78 DEFINE_PER_CPU (struct kprobe_ctlblk, kprobe_ctlblk);
  79
  80 DEFINE_SPINLOCK (kretprobe_lock);       /* Protects kretprobe_inst_table */
  81 DEFINE_PER_CPU (struct kprobe *, kprobe_instance) = NULL;
  82
  83 struct hlist_head kprobe_table[KPROBE_TABLE_SIZE];
  84 struct hlist_head kretprobe_inst_table[KPROBE_TABLE_SIZE];
  85
  86 atomic_t kprobe_count;
  87
  88
  89 void kretprobe_assert (struct kretprobe_instance *ri, unsigned long orig_ret_address, unsigned long trampoline_address)
  90 {
  91         if (!orig_ret_address || (orig_ret_address == trampoline_address))
  92                 panic ("kretprobe BUG!: Processing kretprobe %p @ %p (%d/%d - %s)\n",
  93                                 ri->rp, ri->rp->kp.addr, ri->task->tgid, ri->task->pid, ri->task->comm);
  94 }
  95
  96
  97 /* We have preemption disabled.. so it is safe to use __ versions */
  98 static inline
  99 void set_kprobe_instance (struct kprobe *kp)
 100 {
 101         __get_cpu_var (kprobe_instance) = kp;
 102 }
 103
 104 static inline
 105 void reset_kprobe_instance (void)
 106 {
 107         __get_cpu_var (kprobe_instance) = NULL;
 108 }
 109
 110 /* kprobe_running() will just return the current_kprobe on this CPU */
 111 struct kprobe *kprobe_running (void)
 112 {
 113         return (__get_cpu_var (current_kprobe));
 114 }
 115
 116 void reset_current_kprobe (void)
 117 {
 118         __get_cpu_var (current_kprobe) = NULL;
 119 }
 120
 121 struct kprobe_ctlblk *get_kprobe_ctlblk (void)
 122 {
 123         return (&__get_cpu_var (kprobe_ctlblk));
 124 }
 125
 126 /*
 127  * This routine is called either:
 128  *      - under the kprobe_mutex - during kprobe_[un]register()
 129  *                              OR
 130  *      - with preemption disabled - from arch/xxx/kernel/kprobes.c
 131  */
 132 struct kprobe *get_kprobe(kprobe_opcode_t *addr, pid_t tgid)
 133 {
 134         struct hlist_head *head;
 135         struct hlist_node *node;
 136         struct kprobe *p, *retVal = NULL;
 137
 138         head = &kprobe_table[hash_ptr (addr, KPROBE_HASH_BITS)];
 139         hlist_for_each_entry_rcu(p, node, head, hlist) {
 140                 if (p->addr == addr && p->tgid == tgid) {
 141                         retVal = p;
 142                         break;
 143                 }
 144         }
 145
 146         DBPRINTF ("get_kprobe: probe %p", retVal);
 147         return retVal;
 148 }
 149
 150 /*
 151  * Aggregate handlers for multiple kprobes support - these handlers
 152  * take care of invoking the individual kprobe handlers on p->list
 153  */
 154 static
 155 int aggr_pre_handler (struct kprobe *p, struct pt_regs *regs)
 156 {
 157         struct kprobe *kp;
 158         int ret;
 159
 160         list_for_each_entry_rcu (kp, &p->list, list)
 161         {
 162                 if (kp->pre_handler)
 163                 {
 164                         set_kprobe_instance (kp);
 165                         ret = kp->pre_handler (kp, regs);
 166                         if (ret)
 167                                 return ret;
 168                 }
 169                 reset_kprobe_instance ();
 170         }
 171         return 0;
 172 }
 173
 174 static
 175 void aggr_post_handler (struct kprobe *p, struct pt_regs *regs, unsigned long flags)
 176 {
 177         struct kprobe *kp;
 178
 179         list_for_each_entry_rcu (kp, &p->list, list)
 180         {
 181                 if (kp->post_handler)
 182                 {
 183                         set_kprobe_instance (kp);
 184                         kp->post_handler (kp, regs, flags);
 185                         reset_kprobe_instance ();
 186                 }
 187         }
 188 }
 189
 190 static
 191 int aggr_fault_handler (struct kprobe *p, struct pt_regs *regs, int trapnr)
 192 {
 193         struct kprobe *cur = __get_cpu_var (kprobe_instance);
 194
 195         /*
 196          * if we faulted "during" the execution of a user specified
 197          * probe handler, invoke just that probe's fault handler
 198          */
 199         if (cur && cur->fault_handler)
 200         {
 201                 if (cur->fault_handler (cur, regs, trapnr))
 202                         return 1;
 203         }
 204         return 0;
 205 }
 206
 207 static
 208 int aggr_break_handler (struct kprobe *p, struct pt_regs *regs)
 209 {
 210         struct kprobe *cur = __get_cpu_var (kprobe_instance);
 211         int ret = 0;
 212         DBPRINTF ("cur = 0x%p\n", cur);
 213         if (cur)
 214                 DBPRINTF ("cur = 0x%p cur->break_handler = 0x%p\n", cur, cur->break_handler);
 215
 216         if (cur && cur->break_handler)
 217         {
 218                 if (cur->break_handler (cur, regs /*, vma, page, kaddr */ ))
 219                         ret = 1;
 220         }
 221         reset_kprobe_instance ();
 222         return ret;
 223 }
 224
 225 /* Walks the list and increments nmissed count for multiprobe case */
 226 void kprobes_inc_nmissed_count (struct kprobe *p)
 227 {
 228         struct kprobe *kp;
 229         if (p->pre_handler != aggr_pre_handler)
 230         {
 231                 p->nmissed++;
 232         }
 233         else
 234         {
 235                 list_for_each_entry_rcu (kp, &p->list, list) kp->nmissed++;
 236         }
 237         return;
 238 }
 239
 240 /* Called with kretprobe_lock held */
 241 struct kretprobe_instance *get_free_rp_inst (struct kretprobe *rp)
 242 {
 243         struct hlist_node *node;
 244         struct kretprobe_instance *ri;
 245         hlist_for_each_entry (ri, node, &rp->free_instances, uflist)
 246                 return ri;
 247         if(!alloc_nodes_kretprobe(rp)){
 248              hlist_for_each_entry (ri, node, &rp->free_instances, uflist)
 249                   return ri;
 250         }
 251         return NULL;
 252 }
 253
 254 /* Called with kretprobe_lock held */
 255 struct kretprobe_instance *get_free_rp_inst_no_alloc (struct kretprobe *rp)
 256 {
 257         struct hlist_node *node;
 258         struct kretprobe_instance *ri;
 259         hlist_for_each_entry (ri, node, &rp->free_instances, uflist)
 260                 return ri;
 261         return NULL;
 262 }
 263
 264 /* Called with kretprobe_lock held */
 265 struct kretprobe_instance *get_used_rp_inst (struct kretprobe *rp)
 266 {
 267         struct hlist_node *node;
 268         struct kretprobe_instance *ri;
 269         hlist_for_each_entry (ri, node, &rp->used_instances, uflist) return ri;
 270         return NULL;
 271 }
 272
 273 /* Called with kretprobe_lock held */
 274 void add_rp_inst (struct kretprobe_instance *ri)
 275 {
 276         /*
 277          * Remove rp inst off the free list -
 278          * Add it back when probed function returns
 279          */
 280         hlist_del (&ri->uflist);
 281
 282         /* Add rp inst onto table */
 283         INIT_HLIST_NODE (&ri->hlist);
 284         /*
 285          * We are using different hash keys (task and mm) for finding kernel
 286          * space and user space probes.  Kernel space probes can change mm field in
 287          * task_struct.  User space probes can be shared between threads of one
 288          * process so they have different task but same mm.
 289          */
 290         if (ri->rp->kp.tgid) {
 291                 hlist_add_head (&ri->hlist, &kretprobe_inst_table[hash_ptr (ri->task->mm, KPROBE_HASH_BITS)]);
 292         } else {
 293                 hlist_add_head (&ri->hlist, &kretprobe_inst_table[hash_ptr (ri->task, KPROBE_HASH_BITS)]);
 294         }
 295
 296         /* Also add this rp inst to the used list. */
 297         INIT_HLIST_NODE (&ri->uflist);
 298         hlist_add_head (&ri->uflist, &ri->rp->used_instances);
 299 }
 300
 301 /* Called with kretprobe_lock held */
 302 void recycle_rp_inst (struct kretprobe_instance *ri)
 303 {
 304         if (ri->rp)
 305         {
 306                 hlist_del (&ri->hlist);
 307                 /* remove rp inst off the used list */
 308                 hlist_del (&ri->uflist);
 309                 /* put rp inst back onto the free list */
 310                 INIT_HLIST_NODE (&ri->uflist);
 311                 hlist_add_head (&ri->uflist, &ri->rp->free_instances);
 312         } else if (!ri->rp2) {
 313                 /*
 314                  * This is __switch_to retprobe instance.  It has neither rp nor rp2.
 315                  */
 316                 hlist_del (&ri->hlist);
 317         }
 318 }
 319
 320 struct hlist_head  * kretprobe_inst_table_head (void *hash_key)
 321 {
 322         return &kretprobe_inst_table[hash_ptr (hash_key, KPROBE_HASH_BITS)];
 323 }
 324
 325 void free_rp_inst (struct kretprobe *rp)
 326 {
 327         struct kretprobe_instance *ri;
 328         while ((ri = get_free_rp_inst_no_alloc (rp)) != NULL)
 329         {
 330                 hlist_del (&ri->uflist);
 331                 kfree (ri);
 332         }
 333 }
 334
 335 /*
 336  * Keep all fields in the kprobe consistent
 337  */
 338 static inline
 339 void copy_kprobe (struct kprobe *old_p, struct kprobe *p)
 340 {
 341         memcpy (&p->opcode, &old_p->opcode, sizeof (kprobe_opcode_t));
 342         memcpy (&p->ainsn, &old_p->ainsn, sizeof (struct arch_specific_insn));
 343         p->tgid = old_p->tgid;
 344         p->ss_addr = old_p->ss_addr;
 345 #ifdef CONFIG_ARM
 346         p->safe_arm = old_p->safe_arm;
 347         p->safe_thumb = old_p->safe_thumb;
 348 #endif
 349         //p->spid = old_p->spid;
 350 }
 351
 352 /*
 353  * Add the new probe to old_p->list. Fail if this is the
 354  * second jprobe at the address - two jprobes can't coexist
 355  */
 356 int add_new_kprobe (struct kprobe *old_p, struct kprobe *p)
 357 {
 358         if (p->break_handler)
 359         {
 360                 if (old_p->break_handler)
 361                         return -EEXIST;
 362                 list_add_tail_rcu (&p->list, &old_p->list);
 363                 old_p->break_handler = aggr_break_handler;
 364         }
 365         else
 366                 list_add_rcu (&p->list, &old_p->list);
 367         if (p->post_handler && !old_p->post_handler)
 368                 old_p->post_handler = aggr_post_handler;
 369         return 0;
 370 }
 371
 372 /**
 373  * hlist_replace_rcu - replace old entry by new one
 374  * @old : the element to be replaced
 375  * @new : the new element to insert
 376  *
 377  * The @old entry will be replaced with the @new entry atomically.
 378  */
 379 inline void dbi_hlist_replace_rcu (struct hlist_node *old, struct hlist_node *new)
 380 {
 381         struct hlist_node *next = old->next;
 382
 383         new->next = next;
 384         new->pprev = old->pprev;
 385         smp_wmb ();
 386         if (next)
 387                 new->next->pprev = &new->next;
 388         if (new->pprev)
 389                 *new->pprev = new;
 390         old->pprev = LIST_POISON2;
 391 }
 392
 393
 394 /*
 395  * Fill in the required fields of the "manager kprobe". Replace the
 396  * earlier kprobe in the hlist with the manager kprobe
 397  */
 398 static inline
 399 void add_aggr_kprobe (struct kprobe *ap, struct kprobe *p)
 400 {
 401         copy_kprobe (p, ap);
 402         //flush_insn_slot (ap);
 403         ap->addr = p->addr;
 404         ap->pre_handler = aggr_pre_handler;
 405         ap->fault_handler = aggr_fault_handler;
 406         if (p->post_handler)
 407                 ap->post_handler = aggr_post_handler;
 408         if (p->break_handler)
 409                 ap->break_handler = aggr_break_handler;
 410
 411         INIT_LIST_HEAD (&ap->list);
 412         list_add_rcu (&p->list, &ap->list);
 413
 414         dbi_hlist_replace_rcu (&p->hlist, &ap->hlist);
 415 }
 416
 417 /*
 418  * This is the second or subsequent kprobe at the address - handle
 419  * the intricacies
 420  */
 421 int register_aggr_kprobe (struct kprobe *old_p, struct kprobe *p)
 422 {
 423         int ret = 0;
 424         struct kprobe *ap;
 425         DBPRINTF ("start\n");
 426
 427         DBPRINTF ("p = %p old_p = %p \n", p, old_p);
 428         if (old_p->pre_handler == aggr_pre_handler)
 429         {
 430                 DBPRINTF ("aggr_pre_handler \n");
 431
 432                 copy_kprobe (old_p, p);
 433                 ret = add_new_kprobe (old_p, p);
 434         }
 435         else
 436         {
 437                 DBPRINTF ("kzalloc\n");
 438
 439 #ifdef kzalloc
 440                 ap = kzalloc (sizeof (struct kprobe), GFP_KERNEL);
 441 #else
 442                 ap = kmalloc (sizeof (struct kprobe), GFP_KERNEL);
 443                 if (ap)
 444                         memset (ap, 0, sizeof (struct kprobe));
 445 #endif
 446                 if (!ap)
 447                         return -ENOMEM;
 448                 add_aggr_kprobe (ap, old_p);
 449                 copy_kprobe (ap, p);
 450                 DBPRINTF ("ap = %p p = %p old_p = %p \n", ap, p, old_p);
 451                 ret = add_new_kprobe (ap, p);
 452         }
 453         return ret;
 454 }
 455
 456 int dbi_register_kprobe (struct kprobe *p)
 457 {
 458     struct kprobe *old_p;
 459     int ret = 0;
 460     /*
 461      * If we have a symbol_name argument look it up,
 462      * and add it to the address.  That way the addr
 463      * field can either be global or relative to a symbol.
 464      */
 465     if (p->symbol_name)
 466     {
 467         if (p->addr)
 468             return -EINVAL;
 469         p->addr = (kprobe_opcode_t *)swap_ksyms(p->symbol_name);
 470     }
 471
 472     if (!p->addr)
 473         return -EINVAL;
 474     DBPRINTF ("p->addr = 0x%p\n", p->addr);
 475     p->addr = (kprobe_opcode_t *) (((char *) p->addr) + p->offset);
 476     DBPRINTF ("p->addr = 0x%p p = 0x%p\n", p->addr, p);
 477
 478 #ifdef KPROBES_PROFILE
 479     p->start_tm.tv_sec = p->start_tm.tv_usec = 0;
 480     p->hnd_tm_sum.tv_sec = p->hnd_tm_sum.tv_usec = 0;
 481     p->count = 0;
 482 #endif
 483     p->mod_refcounted = 0;
 484     p->nmissed = 0;
 485
 486     old_p = get_kprobe(p->addr, 0);
 487     if (old_p)
 488     {
 489         ret = register_aggr_kprobe (old_p, p);
 490         if (!ret)
 491             atomic_inc (&kprobe_count);
 492         goto out;
 493     }
 494
 495     if ((ret = arch_prepare_kprobe (p)) != 0)
 496         goto out;
 497
 498     DBPRINTF ("before out ret = 0x%x\n", ret);
 499     INIT_HLIST_NODE (&p->hlist);
 500     hlist_add_head_rcu (&p->hlist, &kprobe_table[hash_ptr (p->addr, KPROBE_HASH_BITS)]);
 501     arch_arm_kprobe (p);
 502
 503 out:
 504     DBPRINTF ("out ret = 0x%x\n", ret);
 505     return ret;
 506 }
 507
 508 void dbi_unregister_kprobe (struct kprobe *p, struct task_struct *task)
 509 {
 510         struct kprobe *old_p, *list_p;
 511         int cleanup_p, pid = p->tgid;
 512
 513         old_p = get_kprobe(p->addr, pid);
 514         DBPRINTF ("dbi_unregister_kprobe p=%p old_p=%p", p, old_p);
 515         if (unlikely (!old_p))
 516                 return;
 517
 518         if (p != old_p)
 519         {
 520                 list_for_each_entry_rcu (list_p, &old_p->list, list)
 521                         if (list_p == p)
 522                                 /* kprobe p is a valid probe */
 523                                 goto valid_p;
 524                 return;
 525         }
 526
 527 valid_p:
 528         DBPRINTF ("dbi_unregister_kprobe valid_p");
 529         if ((old_p == p) || ((old_p->pre_handler == aggr_pre_handler) &&
 530                                 (p->list.next == &old_p->list) && (p->list.prev == &old_p->list)))
 531         {
 532                 /* Only probe on the hash list */
 533                 DBPRINTF ("dbi_unregister_kprobe disarm pid=%d", pid);
 534                 if (pid)
 535                         arch_disarm_uprobe (p, task);//vma, page, kaddr);
 536                 else
 537                         arch_disarm_kprobe (p);
 538                 hlist_del_rcu (&old_p->hlist);
 539                 cleanup_p = 1;
 540         }
 541         else
 542         {
 543                 list_del_rcu (&p->list);
 544                 cleanup_p = 0;
 545         }
 546         DBPRINTF ("dbi_unregister_kprobe cleanup_p=%d", cleanup_p);
 547
 548         if (cleanup_p)
 549         {
 550                 if (p != old_p)
 551                 {
 552                         list_del_rcu (&p->list);
 553                         kfree (old_p);
 554                 }
 555                 arch_remove_kprobe (p, task);
 556         }
 557         else
 558         {
 559                 if (p->break_handler)
 560                         old_p->break_handler = NULL;
 561                 if (p->post_handler)
 562                 {
 563                         list_for_each_entry_rcu (list_p, &old_p->list, list)
 564                         {
 565                                 if (list_p->post_handler)
 566                                 {
 567                                         cleanup_p = 2;
 568                                         break;
 569                                 }
 570                         }
 571                         if (cleanup_p == 0)
 572                                 old_p->post_handler = NULL;
 573                 }
 574         }
 575 }
 576
 577 int dbi_register_jprobe (struct jprobe *jp)
 578 {
 579         /* Todo: Verify probepoint is a function entry point */
 580         jp->kp.pre_handler = setjmp_pre_handler;
 581         jp->kp.break_handler = longjmp_break_handler;
 582
 583         return dbi_register_kprobe (&jp->kp);
 584 }
 585
 586 void dbi_unregister_jprobe (struct jprobe *jp)
 587 {
 588         dbi_unregister_kprobe (&jp->kp, 0);
 589 }
 590
 591 /*
 592  * This kprobe pre_handler is registered with every kretprobe. When probe
 593  * hits it will set up the return probe.
 594  */
 595 int pre_handler_kretprobe (struct kprobe *p, struct pt_regs *regs)
 596 {
 597         struct kretprobe *rp = container_of (p, struct kretprobe, kp);
 598         unsigned long flags = 0;
 599         DBPRINTF ("START\n");
 600
 601         /*TODO: consider to only swap the RA after the last pre_handler fired */
 602         spin_lock_irqsave (&kretprobe_lock, flags);
 603         if (!rp->disarm)
 604                 __arch_prepare_kretprobe (rp, regs);
 605         spin_unlock_irqrestore (&kretprobe_lock, flags);
 606         DBPRINTF ("END\n");
 607         return 0;
 608 }
 609
 610 struct kretprobe *sched_rp;
 611
 612 #define SCHED_RP_NR 200
 613 #define COMMON_RP_NR 10
 614
 615 int alloc_nodes_kretprobe(struct kretprobe *rp)
 616 {
 617      int alloc_nodes;
 618      struct kretprobe_instance *inst;
 619      int i;
 620
 621      DBPRINTF("Alloc aditional mem for retprobes");
 622
 623      if ((unsigned long)rp->kp.addr == sched_addr){
 624           rp->maxactive += SCHED_RP_NR;//max (100, 2 * NR_CPUS);
 625           alloc_nodes = SCHED_RP_NR;
 626      }
 627      else
 628      {
 629 #if 1//def CONFIG_PREEMPT
 630           rp->maxactive += max (COMMON_RP_NR, 2 * NR_CPUS);
 631 #else
 632           rp->maxacpptive += NR_CPUS;
 633 #endif
 634           alloc_nodes = COMMON_RP_NR;
 635      }
 636      /* INIT_HLIST_HEAD (&rp->used_instances); */
 637      /* INIT_HLIST_HEAD (&rp->free_instances); */
 638      for (i = 0; i < alloc_nodes; i++)
 639      {
 640           inst = kmalloc (sizeof (struct kretprobe_instance), GFP_ATOMIC);
 641           if (inst == NULL)
 642           {
 643                free_rp_inst (rp);
 644                return -ENOMEM;
 645           }
 646           INIT_HLIST_NODE (&inst->uflist);
 647           hlist_add_head (&inst->uflist, &rp->free_instances);
 648      }
 649
 650      DBPRINTF ("addr=%p, *addr=[%lx %lx %lx]", rp->kp.addr, (unsigned long) (*(rp->kp.addr)), (unsigned long) (*(rp->kp.addr + 1)), (unsigned long) (*(rp->kp.addr + 2)));
 651      return 0;
 652 }
 653
 654 int dbi_register_kretprobe (struct kretprobe *rp)
 655 {
 656         int ret = 0;
 657         struct kretprobe_instance *inst;
 658         int i;
 659         DBPRINTF ("START");
 660
 661         rp->kp.pre_handler = pre_handler_kretprobe;
 662         rp->kp.post_handler = NULL;
 663         rp->kp.fault_handler = NULL;
 664         rp->kp.break_handler = NULL;
 665
 666         rp->disarm = 0;
 667
 668         /* Pre-allocate memory for max kretprobe instances */
 669         if ((unsigned long)rp->kp.addr == sched_addr) {
 670                 rp->maxactive = SCHED_RP_NR;//max (100, 2 * NR_CPUS);
 671         } else if (rp->maxactive <= 0) {
 672 #if 1//def CONFIG_PREEMPT
 673                 rp->maxactive = max (COMMON_RP_NR, 2 * NR_CPUS);
 674 #else
 675                 rp->maxactive = NR_CPUS;
 676 #endif
 677         }
 678         INIT_HLIST_HEAD (&rp->used_instances);
 679         INIT_HLIST_HEAD (&rp->free_instances);
 680         for (i = 0; i < rp->maxactive; i++)
 681         {
 682                 inst = kmalloc (sizeof (struct kretprobe_instance), GFP_KERNEL);
 683                 if (inst == NULL)
 684                 {
 685                         free_rp_inst (rp);
 686                         return -ENOMEM;
 687                 }
 688                 INIT_HLIST_NODE (&inst->uflist);
 689                 hlist_add_head (&inst->uflist, &rp->free_instances);
 690         }
 691
 692         DBPRINTF ("addr=%p, *addr=[%lx %lx %lx]", rp->kp.addr, (unsigned long) (*(rp->kp.addr)), (unsigned long) (*(rp->kp.addr + 1)), (unsigned long) (*(rp->kp.addr + 2)));
 693         rp->nmissed = 0;
 694         /* Establish function entry probe point */
 695         if ((ret = dbi_register_kprobe (&rp->kp)) != 0)
 696                 free_rp_inst (rp);
 697
 698         DBPRINTF ("addr=%p, *addr=[%lx %lx %lx]", rp->kp.addr, (unsigned long) (*(rp->kp.addr)), (unsigned long) (*(rp->kp.addr + 1)), (unsigned long) (*(rp->kp.addr + 2)));
 699         if ((unsigned long)rp->kp.addr == sched_addr) {
 700                 sched_rp = rp;
 701         }
 702
 703         return ret;
 704 }
 705
 706 static void unpatch_suspended_all_task_ret_addr(struct kretprobe *rp);
 707
 708 void dbi_unregister_kretprobe (struct kretprobe *rp)
 709 {
 710         unsigned long flags;
 711         struct kretprobe_instance *ri;
 712
 713         dbi_unregister_kprobe (&rp->kp, 0);
 714
 715         if ((unsigned long)rp->kp.addr == sched_addr) {
 716                 unpatch_suspended_all_task_ret_addr(rp);
 717                 sched_rp = NULL;
 718         }
 719
 720         /* No race here */
 721         spin_lock_irqsave (&kretprobe_lock, flags);
 722         while ((ri = get_used_rp_inst (rp)) != NULL)
 723         {
 724                 ri->rp = NULL;
 725                 hlist_del (&ri->uflist);
 726         }
 727         spin_unlock_irqrestore (&kretprobe_lock, flags);
 728         free_rp_inst (rp);
 729 }
 730
 731 struct kretprobe * clone_kretprobe (struct kretprobe *rp)
 732 {
 733         struct kprobe *old_p;
 734         struct kretprobe *clone = NULL;
 735         int ret;
 736
 737         clone = kmalloc (sizeof (struct kretprobe), GFP_KERNEL);
 738         if (!clone)
 739         {
 740                 DBPRINTF ("failed to alloc memory for clone probe %p!", rp->kp.addr);
 741                 return NULL;
 742         }
 743         memcpy (clone, rp, sizeof (struct kretprobe));
 744         clone->kp.pre_handler = pre_handler_kretprobe;
 745         clone->kp.post_handler = NULL;
 746         clone->kp.fault_handler = NULL;
 747         clone->kp.break_handler = NULL;
 748         old_p = get_kprobe(rp->kp.addr, rp->kp.tgid);
 749         if (old_p)
 750         {
 751                 ret = register_aggr_kprobe (old_p, &clone->kp);
 752                 if (ret)
 753                 {
 754                         kfree (clone);
 755                         return NULL;
 756                 }
 757                 atomic_inc (&kprobe_count);
 758         }
 759
 760         return clone;
 761 }
 762
 763
 764 static void inline set_task_trampoline(struct task_struct *p, struct kretprobe_instance *ri, unsigned long tramp_addr)
 765 {
 766         ri->ret_addr = (kprobe_opcode_t *)arch_get_task_pc(p);
 767         arch_set_task_pc(p, tramp_addr);
 768 }
 769
 770 static void inline rm_task_trampoline(struct task_struct *p, struct kretprobe_instance *ri)
 771 {
 772         arch_set_task_pc(p, (unsigned long)ri->ret_addr);
 773 }
 774
 775 static struct kretprobe_instance* find_ri_pc_mod(struct task_struct *p, struct kretprobe *rp)
 776 {
 777         struct kretprobe_instance *ri;
 778         struct hlist_node *node, *tmp;
 779         struct hlist_head *head;
 780         unsigned long flags;
 781
 782         spin_lock_irqsave (&kretprobe_lock, flags);
 783         head = kretprobe_inst_table_head (p);
 784         hlist_for_each_entry_safe (ri, node, tmp, head, hlist) {
 785                 if ((ri->rp == rp) && (p == ri->task)) {
 786                         spin_unlock_irqrestore (&kretprobe_lock, flags);
 787                         return ri;
 788                 }
 789         }
 790         spin_unlock_irqrestore (&kretprobe_lock, flags);
 791
 792         return NULL;
 793 }
 794
 795 static void add_ri_pc_mod(struct task_struct *p, struct kretprobe *rp, unsigned long tramp_addr)
 796 {
 797         struct kretprobe_instance *ri;
 798         unsigned long flags;
 799
 800         spin_lock_irqsave(&kretprobe_lock, flags);
 801         if ((ri = get_free_rp_inst(rp)) != NULL) {
 802                 ri->rp = rp;
 803                 ri->rp2 = NULL;
 804                 ri->task = p;
 805                 // set PC
 806                 set_task_trampoline(p, ri, tramp_addr);
 807                 add_rp_inst(ri);
 808         } else {
 809                 printk("no ri for %d\n", p->pid);
 810                 BUG();
 811         }
 812         spin_unlock_irqrestore(&kretprobe_lock, flags);
 813 }
 814
 815 static void patch_suspended_task_ret_addr(struct task_struct *p, struct kretprobe *rp)
 816 {
 817         struct kretprobe_instance *ri = find_ri_pc_mod(p, rp);
 818
 819         if(ri) {
 820                 // update PC
 821                 if( arch_get_task_pc(p) != (unsigned long) &kretprobe_trampoline)
 822                         set_task_trampoline(p, ri, (unsigned long) &kretprobe_trampoline);
 823         } else {
 824                 add_ri_pc_mod(p, rp, (unsigned long) &kretprobe_trampoline);
 825         }
 826 }
 827
 828 static void unpatch_suspended_task_ret_addr(struct task_struct *p, struct kretprobe *rp)
 829 {
 830         struct kretprobe_instance *ri;
 831
 832         if( arch_get_task_pc(p) == (unsigned long)&kretprobe_trampoline )
 833         {
 834                 ri = find_ri_pc_mod(p, rp);
 835                 if(ri) {
 836                         rm_task_trampoline(p, ri);
 837                         recycle_rp_inst(ri);
 838                 }
 839         }
 840 }
 841
 842 void patch_suspended_all_task_ret_addr(struct kretprobe *rp)
 843 {
 844         struct task_struct *p, *g;
 845
 846         rcu_read_lock();
 847         // swapper task
 848         if(current != &init_task)
 849                 patch_suspended_task_ret_addr(&init_task, rp);
 850
 851         // other tasks
 852         do_each_thread(g, p) {
 853                 if(p == current)
 854                         continue;
 855                 patch_suspended_task_ret_addr(p, rp);
 856         } while_each_thread(g, p);
 857
 858 #ifdef CONFIG_X86
 859         /* workaround for do_exit probe on x86 targets */
 860         if ((current->flags & PF_EXITING) || (current->flags & PF_EXITPIDONE)) {
 861                 patch_suspended_task_ret_addr(current, rp);
 862         }
 863 #endif
 864         rcu_read_unlock();
 865 }
 866
 867 static void unpatch_suspended_all_task_ret_addr(struct kretprobe *rp)
 868 {
 869         struct task_struct *p, *g;
 870
 871         rcu_read_lock();
 872         // swapper task
 873         unpatch_suspended_task_ret_addr(&init_task, rp);
 874
 875         // other tasks
 876         do_each_thread(g, p) {
 877                 unpatch_suspended_task_ret_addr(p, rp);
 878         } while_each_thread(g, p);
 879         rcu_read_unlock();
 880 }
 881
 882 int __init init_kprobes (void)
 883 {
 884         int i, err = 0;
 885
 886         /* FIXME allocate the probe table, currently defined statically */
 887         /* initialize all list heads */
 888         for (i = 0; i < KPROBE_TABLE_SIZE; i++)
 889         {
 890                 INIT_HLIST_HEAD (&kprobe_table[i]);
 891                 INIT_HLIST_HEAD (&kretprobe_inst_table[i]);
 892
 893                 init_uprobes_insn_slots(i);
 894         }
 895         atomic_set (&kprobe_count, 0);
 896
 897         err = arch_init_kprobes ();
 898
 899         DBPRINTF ("init_kprobes: arch_init_kprobes - %d", err);
 900
 901         return err;
 902 }
 903
 904 void __exit exit_kprobes (void)
 905 {
 906         dbi_arch_exit_kprobes ();
 907 }
 908
 909 module_init (init_kprobes);
 910 module_exit (exit_kprobes);
 911
 912 EXPORT_SYMBOL_GPL (dbi_register_kprobe);
 913 EXPORT_SYMBOL_GPL (dbi_unregister_kprobe);
 914 EXPORT_SYMBOL_GPL (dbi_register_jprobe);
 915 EXPORT_SYMBOL_GPL (dbi_unregister_jprobe);
 916 EXPORT_SYMBOL_GPL (dbi_jprobe_return);
 917 EXPORT_SYMBOL_GPL (dbi_register_kretprobe);
 918 EXPORT_SYMBOL_GPL (dbi_unregister_kretprobe);
 919
 920 MODULE_LICENSE ("Dual BSD/GPL");
 921