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