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