kernel/swap/kprobe/swap_kprobes.c

   1 /**
   2  * kprobe/swap_kprobes.c
   3  * @author Ekaterina Gorelkina <e.gorelkina@samsung.com>: initial implementation for ARM and MIPS
   4  * @author Alexey Gerenkov <a.gerenkov@samsung.com> User-Space Probes initial implementation;
   5  * Support x86/ARM/MIPS for both user and kernel spaces.
   6  * @author Ekaterina Gorelkina <e.gorelkina@samsung.com>: redesign module for separating core and arch parts
   7  *
   8  * @section LICENSE
   9  *
  10  * This program is free software; you can redistribute it and/or modify
  11  * it under the terms of the GNU General Public License as published by
  12  * the Free Software Foundation; either version 2 of the License, or
  13  * (at your option) any later version.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  *
  20  * You should have received a copy of the GNU General Public License
  21  * along with this program; if not, write to the Free Software
  22  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  23  *
  24  * @section COPYRIGHT
  25  *
  26  * Copyright (C) IBM Corporation, 2002, 2004
  27  * Copyright (C) Samsung Electronics, 2006-2010
  28  *
  29  * @section DESCRIPTION
  30  *
  31  * SWAP kprobe implementation. Dynamic kernel functions instrumentation.
  32  */
  33
  34 #include <linux/version.h>
  35
  36 #include <linux/hash.h>
  37 #include <linux/module.h>
  38 #include <linux/mm.h>
  39 #include <linux/pagemap.h>
  40 #include <linux/stop_machine.h>
  41
  42 #include <ksyms/ksyms.h>
  43 #include <master/swap_initializer.h>
  44 #include <swap-asm/swap_kprobes.h>
  45
  46 #include "swap_slots.h"
  47 #include "swap_kdebug.h"
  48 #include "swap_kprobes.h"
  49 #include "swap_kprobes_deps.h"
  50
  51 /**
  52  * @var sched_addr
  53  * @brief Scheduler address.
  54  */
  55 unsigned long sched_addr;
  56 static unsigned long exit_addr;
  57 static unsigned long do_group_exit_addr;
  58 static unsigned long sys_exit_group_addr;
  59 static unsigned long sys_exit_addr;
  60
  61 /**
  62  * @var sm
  63  * @brief Current slot manager. Slots are the places where trampolines are
  64  * located.
  65  */
  66 struct slot_manager sm;
  67
  68 DEFINE_PER_CPU(struct kprobe *, swap_current_kprobe);
  69 static DEFINE_PER_CPU(struct kprobe_ctlblk, kprobe_ctlblk);
  70
  71 static DEFINE_SPINLOCK(kretprobe_lock); /* Protects kretprobe_inst_table */
  72 static DEFINE_PER_CPU(struct kprobe *, kprobe_instance);
  73
  74 struct hlist_head kprobe_table[KPROBE_TABLE_SIZE];
  75 static struct hlist_head kretprobe_inst_table[KPROBE_TABLE_SIZE];
  76
  77 /**
  78  * @var kprobe_count
  79  * @brief Count of kprobes.
  80  */
  81 atomic_t kprobe_count;
  82 EXPORT_SYMBOL_GPL(kprobe_count);
  83
  84
  85 static void *(*module_alloc)(unsigned long size);
  86 static void *(*module_free)(struct module *mod, void *module_region);
  87
  88 static void *__wrapper_module_alloc(unsigned long size)
  89 {
  90         return module_alloc(size);
  91 }
  92
  93 static void *__wrapper_module_free(void *module_region)
  94 {
  95         return module_free(NULL, module_region);
  96 }
  97
  98 static void *sm_alloc(struct slot_manager *sm)
  99 {
 100         return __wrapper_module_alloc(PAGE_SIZE);
 101 }
 102
 103 static void sm_free(struct slot_manager *sm, void *ptr)
 104 {
 105         __wrapper_module_free(ptr);
 106 }
 107
 108 static void init_sm(void)
 109 {
 110         sm.slot_size = KPROBES_TRAMP_LEN;
 111         sm.alloc = sm_alloc;
 112         sm.free = sm_free;
 113         INIT_HLIST_HEAD(&sm.page_list);
 114 }
 115
 116 static void exit_sm(void)
 117 {
 118         /* FIXME: free */
 119 }
 120
 121 static void kretprobe_assert(struct kretprobe_instance *ri,
 122                              unsigned long orig_ret_address,
 123                              unsigned long trampoline_address)
 124 {
 125         if (!orig_ret_address || (orig_ret_address == trampoline_address)) {
 126                 struct task_struct *task;
 127                 if (ri == NULL)
 128                         panic("kretprobe BUG!: ri = NULL\n");
 129
 130                 task = ri->task;
 131
 132                 if (task == NULL)
 133                         panic("kretprobe BUG!: task = NULL\n");
 134
 135                 if (ri->rp == NULL)
 136                         panic("kretprobe BUG!: ri->rp = NULL\n");
 137
 138                 panic("kretprobe BUG!: "
 139                       "Processing kretprobe %p @ %p (%d/%d - %s)\n",
 140                       ri->rp, ri->rp->kp.addr, ri->task->tgid,
 141                       ri->task->pid, ri->task->comm);
 142         }
 143 }
 144
 145 /* We have preemption disabled.. so it is safe to use __ versions */
 146 static inline void set_kprobe_instance(struct kprobe *kp)
 147 {
 148         __get_cpu_var(kprobe_instance) = kp;
 149 }
 150
 151 static inline void reset_kprobe_instance(void)
 152 {
 153         __get_cpu_var(kprobe_instance) = NULL;
 154 }
 155
 156 /**
 157  * @brief Gets the current kprobe on this CPU.
 158  *
 159  * @return Pointer to the current kprobe.
 160  */
 161 struct kprobe *swap_kprobe_running(void)
 162 {
 163         return __get_cpu_var(swap_current_kprobe);
 164 }
 165
 166 /**
 167  * @brief Sets the current kprobe to NULL.
 168  *
 169  * @return Void.
 170  */
 171 void swap_reset_current_kprobe(void)
 172 {
 173         __get_cpu_var(swap_current_kprobe) = NULL;
 174 }
 175
 176 /**
 177  * @brief Gets kprobe_ctlblk for the current CPU.
 178  *
 179  * @return Current CPU struct kprobe_ctlblk.
 180  */
 181 struct kprobe_ctlblk *swap_get_kprobe_ctlblk(void)
 182 {
 183         return &__get_cpu_var(kprobe_ctlblk);
 184 }
 185
 186 /*
 187  * This routine is called either:
 188  *      - under the kprobe_mutex - during kprobe_[un]register()
 189  *                              OR
 190  *      - with preemption disabled - from arch/xxx/kernel/kprobes.c
 191  */
 192
 193 /**
 194  * @brief Gets kprobe.
 195  *
 196  * @param addr Probe address.
 197  * @return Kprobe for addr.
 198  */
 199 struct kprobe *swap_get_kprobe(void *addr)
 200 {
 201         struct hlist_head *head;
 202         struct kprobe *p;
 203         DECLARE_NODE_PTR_FOR_HLIST(node);
 204
 205         head = &kprobe_table[hash_ptr(addr, KPROBE_HASH_BITS)];
 206         swap_hlist_for_each_entry_rcu(p, node, head, hlist) {
 207                 if (p->addr == addr)
 208                         return p;
 209         }
 210
 211         return NULL;
 212 }
 213
 214 /*
 215  * Aggregate handlers for multiple kprobes support - these handlers
 216  * take care of invoking the individual kprobe handlers on p->list
 217  */
 218 static int aggr_pre_handler(struct kprobe *p, struct pt_regs *regs)
 219 {
 220         struct kprobe *kp;
 221         int ret;
 222
 223         list_for_each_entry_rcu(kp, &p->list, list) {
 224                 if (kp->pre_handler) {
 225                         set_kprobe_instance(kp);
 226                         ret = kp->pre_handler(kp, regs);
 227                         if (ret)
 228                                 return ret;
 229                 }
 230                 reset_kprobe_instance();
 231         }
 232
 233         return 0;
 234 }
 235
 236 static void aggr_post_handler(struct kprobe *p,
 237                               struct pt_regs *regs,
 238                               unsigned long flags)
 239 {
 240         struct kprobe *kp;
 241
 242         list_for_each_entry_rcu(kp, &p->list, list) {
 243                 if (kp->post_handler) {
 244                         set_kprobe_instance(kp);
 245                         kp->post_handler(kp, regs, flags);
 246                         reset_kprobe_instance();
 247                 }
 248         }
 249 }
 250
 251 static int aggr_fault_handler(struct kprobe *p,
 252                               struct pt_regs *regs,
 253                               int trapnr)
 254 {
 255         struct kprobe *cur = __get_cpu_var(kprobe_instance);
 256
 257         /*
 258          * if we faulted "during" the execution of a user specified
 259          * probe handler, invoke just that probe's fault handler
 260          */
 261         if (cur && cur->fault_handler) {
 262                 if (cur->fault_handler(cur, regs, trapnr))
 263                         return 1;
 264         }
 265
 266         return 0;
 267 }
 268
 269 static int aggr_break_handler(struct kprobe *p, struct pt_regs *regs)
 270 {
 271         struct kprobe *cur = __get_cpu_var(kprobe_instance);
 272         int ret = 0;
 273         DBPRINTF("cur = 0x%p\n", cur);
 274         if (cur)
 275                 DBPRINTF("cur = 0x%p cur->break_handler = 0x%p\n",
 276                          cur, cur->break_handler);
 277
 278         if (cur && cur->break_handler) {
 279                 if (cur->break_handler(cur, regs))
 280                         ret = 1;
 281         }
 282         reset_kprobe_instance();
 283
 284         return ret;
 285 }
 286
 287 /**
 288  * @brief Walks the list and increments nmissed count for multiprobe case.
 289  *
 290  * @param p Pointer to the missed kprobe.
 291  * @return Void.
 292  */
 293 void swap_kprobes_inc_nmissed_count(struct kprobe *p)
 294 {
 295         struct kprobe *kp;
 296         if (p->pre_handler != aggr_pre_handler) {
 297                 p->nmissed++;
 298         } else {
 299                 list_for_each_entry_rcu(kp, &p->list, list) {
 300                         ++kp->nmissed;
 301                 }
 302         }
 303 }
 304
 305 static int alloc_nodes_kretprobe(struct kretprobe *rp);
 306
 307 /* Called with kretprobe_lock held */
 308 static struct kretprobe_instance *get_free_rp_inst(struct kretprobe *rp)
 309 {
 310         struct kretprobe_instance *ri;
 311         DECLARE_NODE_PTR_FOR_HLIST(node);
 312
 313         swap_hlist_for_each_entry(ri, node, &rp->free_instances, uflist) {
 314                 return ri;
 315         }
 316
 317         if (!alloc_nodes_kretprobe(rp)) {
 318                 swap_hlist_for_each_entry(ri, node, &rp->free_instances,
 319                                           uflist) {
 320                         return ri;
 321                 }
 322         }
 323
 324         return NULL;
 325 }
 326
 327 /* Called with kretprobe_lock held */
 328 static struct kretprobe_instance *
 329 get_free_rp_inst_no_alloc(struct kretprobe *rp)
 330 {
 331         struct kretprobe_instance *ri;
 332         DECLARE_NODE_PTR_FOR_HLIST(node);
 333
 334         swap_hlist_for_each_entry(ri, node, &rp->free_instances, uflist) {
 335                 return ri;
 336         }
 337
 338         return NULL;
 339 }
 340
 341 /* Called with kretprobe_lock held */
 342 static struct kretprobe_instance *get_used_rp_inst(struct kretprobe *rp)
 343 {
 344         struct kretprobe_instance *ri;
 345         DECLARE_NODE_PTR_FOR_HLIST(node);
 346
 347         swap_hlist_for_each_entry(ri, node, &rp->used_instances, uflist) {
 348                 return ri;
 349         }
 350
 351         return NULL;
 352 }
 353
 354 /* Called with kretprobe_lock held */
 355 static void add_rp_inst(struct kretprobe_instance *ri)
 356 {
 357         /*
 358          * Remove rp inst off the free list -
 359          * Add it back when probed function returns
 360          */
 361         hlist_del(&ri->uflist);
 362
 363         /* Add rp inst onto table */
 364         INIT_HLIST_NODE(&ri->hlist);
 365
 366         hlist_add_head(&ri->hlist,
 367                        &kretprobe_inst_table[hash_ptr(ri->task,
 368                                                       KPROBE_HASH_BITS)]);
 369
 370         /* Also add this rp inst to the used list. */
 371         INIT_HLIST_NODE(&ri->uflist);
 372         hlist_add_head(&ri->uflist, &ri->rp->used_instances);
 373 }
 374
 375 /* Called with kretprobe_lock held */
 376 static void recycle_rp_inst(struct kretprobe_instance *ri)
 377 {
 378         if (ri->rp) {
 379                 hlist_del(&ri->hlist);
 380                 /* remove rp inst off the used list */
 381                 hlist_del(&ri->uflist);
 382                 /* put rp inst back onto the free list */
 383                 INIT_HLIST_NODE(&ri->uflist);
 384                 hlist_add_head(&ri->uflist, &ri->rp->free_instances);
 385         }
 386 }
 387
 388 static struct hlist_head *kretprobe_inst_table_head(void *hash_key)
 389 {
 390         return &kretprobe_inst_table[hash_ptr(hash_key, KPROBE_HASH_BITS)];
 391 }
 392
 393 static void free_rp_inst(struct kretprobe *rp)
 394 {
 395         struct kretprobe_instance *ri;
 396         while ((ri = get_free_rp_inst_no_alloc(rp)) != NULL) {
 397                 hlist_del(&ri->uflist);
 398                 kfree(ri);
 399         }
 400 }
 401
 402 /*
 403  * Keep all fields in the kprobe consistent
 404  */
 405 static inline void copy_kprobe(struct kprobe *old_p, struct kprobe *p)
 406 {
 407         memcpy(&p->opcode, &old_p->opcode, sizeof(kprobe_opcode_t));
 408         memcpy(&p->ainsn, &old_p->ainsn, sizeof(struct arch_specific_insn));
 409 }
 410
 411 /*
 412  * Add the new probe to old_p->list. Fail if this is the
 413  * second jprobe at the address - two jprobes can't coexist
 414  */
 415 static int add_new_kprobe(struct kprobe *old_p, struct kprobe *p)
 416 {
 417         if (p->break_handler) {
 418                 if (old_p->break_handler)
 419                         return -EEXIST;
 420
 421                 list_add_tail_rcu(&p->list, &old_p->list);
 422                 old_p->break_handler = aggr_break_handler;
 423         } else {
 424                 list_add_rcu(&p->list, &old_p->list);
 425         }
 426
 427         if (p->post_handler && !old_p->post_handler)
 428                 old_p->post_handler = aggr_post_handler;
 429
 430         return 0;
 431 }
 432
 433 /**
 434  * hlist_replace_rcu - replace old entry by new one
 435  * @old : the element to be replaced
 436  * @new : the new element to insert
 437  *
 438  * The @old entry will be replaced with the @new entry atomically.
 439  */
 440 inline void swap_hlist_replace_rcu(struct hlist_node *old,
 441                                    struct hlist_node *new)
 442 {
 443         struct hlist_node *next = old->next;
 444
 445         new->next = next;
 446         new->pprev = old->pprev;
 447         smp_wmb();
 448         if (next)
 449                 new->next->pprev = &new->next;
 450         if (new->pprev)
 451                 *new->pprev = new;
 452         old->pprev = LIST_POISON2;
 453 }
 454
 455 /*
 456  * Fill in the required fields of the "manager kprobe". Replace the
 457  * earlier kprobe in the hlist with the manager kprobe
 458  */
 459 static inline void add_aggr_kprobe(struct kprobe *ap, struct kprobe *p)
 460 {
 461         copy_kprobe(p, ap);
 462         ap->addr = p->addr;
 463         ap->pre_handler = aggr_pre_handler;
 464         ap->fault_handler = aggr_fault_handler;
 465         if (p->post_handler)
 466                 ap->post_handler = aggr_post_handler;
 467         if (p->break_handler)
 468                 ap->break_handler = aggr_break_handler;
 469
 470         INIT_LIST_HEAD(&ap->list);
 471         list_add_rcu(&p->list, &ap->list);
 472
 473         swap_hlist_replace_rcu(&p->hlist, &ap->hlist);
 474 }
 475
 476 /*
 477  * This is the second or subsequent kprobe at the address - handle
 478  * the intricacies
 479  */
 480 static int register_aggr_kprobe(struct kprobe *old_p, struct kprobe *p)
 481 {
 482         int ret = 0;
 483         struct kprobe *ap;
 484         DBPRINTF("start\n");
 485
 486         DBPRINTF("p = %p old_p = %p\n", p, old_p);
 487         if (old_p->pre_handler == aggr_pre_handler) {
 488                 DBPRINTF("aggr_pre_handler\n");
 489
 490                 copy_kprobe(old_p, p);
 491                 ret = add_new_kprobe(old_p, p);
 492         } else {
 493                 DBPRINTF("kzalloc\n");
 494 #ifdef kzalloc
 495                 ap = kzalloc(sizeof(struct kprobe), GFP_KERNEL);
 496 #else
 497                 ap = kmalloc(sizeof(struct kprobe), GFP_KERNEL);
 498                 if (ap)
 499                         memset(ap, 0, sizeof(struct kprobe));
 500 #endif
 501                 if (!ap)
 502                         return -ENOMEM;
 503                 add_aggr_kprobe(ap, old_p);
 504                 copy_kprobe(ap, p);
 505                 DBPRINTF("ap = %p p = %p old_p = %p\n", ap, p, old_p);
 506                 ret = add_new_kprobe(ap, p);
 507         }
 508
 509         return ret;
 510 }
 511
 512 static void remove_kprobe(struct kprobe *p)
 513 {
 514         /* TODO: check boostable for x86 and MIPS */
 515         swap_slot_free(&sm, p->ainsn.insn);
 516 }
 517
 518 /**
 519  * @brief Registers kprobe.
 520  *
 521  * @param p Pointer to the target kprobe.
 522  * @return 0 on success, error code on error.
 523  */
 524 int swap_register_kprobe(struct kprobe *p)
 525 {
 526         struct kprobe *old_p;
 527         int ret = 0;
 528         /*
 529          * If we have a symbol_name argument look it up,
 530          * and add it to the address.  That way the addr
 531          * field can either be global or relative to a symbol.
 532          */
 533         if (p->symbol_name) {
 534                 if (p->addr)
 535                         return -EINVAL;
 536                 p->addr = (kprobe_opcode_t *)swap_ksyms(p->symbol_name);
 537         }
 538
 539         if (!p->addr)
 540                 return -EINVAL;
 541         DBPRINTF("p->addr = 0x%p\n", p->addr);
 542         p->addr = (kprobe_opcode_t *)(((char *)p->addr) + p->offset);
 543         DBPRINTF("p->addr = 0x%p p = 0x%p\n", p->addr, p);
 544
 545 #ifdef KPROBES_PROFILE
 546         p->start_tm.tv_sec = p->start_tm.tv_usec = 0;
 547         p->hnd_tm_sum.tv_sec = p->hnd_tm_sum.tv_usec = 0;
 548         p->count = 0;
 549 #endif
 550         p->mod_refcounted = 0;
 551         p->nmissed = 0;
 552         INIT_LIST_HEAD(&p->list);
 553
 554         old_p = swap_get_kprobe(p->addr);
 555         if (old_p) {
 556                 ret = register_aggr_kprobe(old_p, p);
 557                 if (!ret)
 558                         atomic_inc(&kprobe_count);
 559                 goto out;
 560         }
 561         ret = swap_arch_prepare_kprobe(p, &sm);
 562         if (ret != 0)
 563                 goto out;
 564
 565         DBPRINTF("before out ret = 0x%x\n", ret);
 566         INIT_HLIST_NODE(&p->hlist);
 567         hlist_add_head_rcu(&p->hlist,
 568                            &kprobe_table[hash_ptr(p->addr, KPROBE_HASH_BITS)]);
 569         swap_arch_arm_kprobe(p);
 570
 571 out:
 572         DBPRINTF("out ret = 0x%x\n", ret);
 573         return ret;
 574 }
 575 EXPORT_SYMBOL_GPL(swap_register_kprobe);
 576
 577 static void swap_unregister_valid_kprobe(struct kprobe *p, struct kprobe *old_p)
 578 {
 579         struct kprobe *list_p;
 580
 581         if ((old_p == p) || ((old_p->pre_handler == aggr_pre_handler) &&
 582             (p->list.next == &old_p->list) && (p->list.prev == &old_p->list))) {
 583                 /* Only probe on the hash list */
 584                 swap_arch_disarm_kprobe(p);
 585
 586                 /* FIXME: move sync out from atomic context */
 587                 if (!in_atomic())
 588                         synchronize_sched();
 589
 590                 hlist_del_rcu(&old_p->hlist);
 591                 remove_kprobe(old_p);
 592
 593                 if (p != old_p) {
 594                         list_del_rcu(&old_p->list);
 595                         kfree(old_p);
 596                 }
 597                 /* Synchronize and remove probe in bottom */
 598         } else {
 599                 list_del_rcu(&p->list);
 600
 601                 if (p->break_handler)
 602                         old_p->break_handler = NULL;
 603                 if (p->post_handler) {
 604                         list_for_each_entry_rcu(list_p, &old_p->list, list)
 605                                 if (list_p->post_handler)
 606                                         return;
 607
 608                         old_p->post_handler = NULL;
 609                 }
 610         }
 611         /* Set NULL addr for reusability if symbol_name is used */
 612         if (p->symbol_name)
 613                 p->addr = NULL;
 614 }
 615
 616 /**
 617  * @brief Unregistes kprobe.
 618  *
 619  * @param kp Pointer to the target kprobe.
 620  * @return Void.
 621  */
 622 void swap_unregister_kprobe(struct kprobe *kp)
 623 {
 624         struct kprobe *old_p, *list_p;
 625
 626         old_p = swap_get_kprobe(kp->addr);
 627         if (unlikely(!old_p))
 628                 return;
 629
 630         if (kp != old_p) {
 631                 list_for_each_entry_rcu(list_p, &old_p->list, list)
 632                         if (list_p == kp)
 633                                 goto unreg_valid_kprobe;
 634                 /* kprobe invalid */
 635                 return;
 636         }
 637
 638 unreg_valid_kprobe:
 639         swap_unregister_valid_kprobe(kp, old_p);
 640 }
 641 EXPORT_SYMBOL_GPL(swap_unregister_kprobe);
 642
 643 /**
 644  * @brief Registers jprobe.
 645  *
 646  * @param jp Pointer to the target jprobe.
 647  * @return swap_register_kprobe result.
 648  */
 649 int swap_register_jprobe(struct jprobe *jp)
 650 {
 651         /* Todo: Verify probepoint is a function entry point */
 652         jp->kp.pre_handler = swap_setjmp_pre_handler;
 653         jp->kp.break_handler = swap_longjmp_break_handler;
 654
 655         return swap_register_kprobe(&jp->kp);
 656 }
 657 EXPORT_SYMBOL_GPL(swap_register_jprobe);
 658
 659 /**
 660  * @brief Unregisters jprobe.
 661  *
 662  * @param jp Pointer to the target jprobe.
 663  * @return Void.
 664  */
 665 void swap_unregister_jprobe(struct jprobe *jp)
 666 {
 667         swap_unregister_kprobe(&jp->kp);
 668 }
 669 EXPORT_SYMBOL_GPL(swap_unregister_jprobe);
 670
 671 /*
 672  * This kprobe pre_handler is registered with every kretprobe. When probe
 673  * hits it will set up the return probe.
 674  */
 675 static int pre_handler_kretprobe(struct kprobe *p, struct pt_regs *regs)
 676 {
 677         struct kretprobe *rp = container_of(p, struct kretprobe, kp);
 678         struct kretprobe_instance *ri;
 679         unsigned long flags = 0;
 680
 681         /* TODO: consider to only swap the RA
 682          * after the last pre_handler fired */
 683         spin_lock_irqsave(&kretprobe_lock, flags);
 684
 685         /* TODO: test - remove retprobe after func entry but before its exit */
 686         ri = get_free_rp_inst(rp);
 687         if (ri != NULL) {
 688                 int skip = 0;
 689
 690                 ri->rp = rp;
 691                 ri->task = current;
 692
 693                 if (rp->entry_handler)
 694                         skip = rp->entry_handler(ri, regs);
 695
 696                 if (skip) {
 697                         add_rp_inst(ri);
 698                         recycle_rp_inst(ri);
 699                 } else {
 700                         swap_arch_prepare_kretprobe(ri, regs);
 701                         add_rp_inst(ri);
 702                 }
 703         } else {
 704                 ++rp->nmissed;
 705         }
 706
 707         spin_unlock_irqrestore(&kretprobe_lock, flags);
 708
 709         return 0;
 710 }
 711
 712 /**
 713  * @brief Trampoline probe handler.
 714  *
 715  * @param p Pointer to the fired kprobe.
 716  * @param regs Pointer to CPU registers data.
 717  * @return orig_ret_address
 718  */
 719 int trampoline_probe_handler(struct kprobe *p, struct pt_regs *regs)
 720 {
 721         struct kretprobe_instance *ri = NULL;
 722         struct hlist_head *head;
 723         unsigned long flags, orig_ret_address = 0;
 724         unsigned long trampoline_address;
 725
 726         struct kprobe_ctlblk *kcb;
 727
 728         struct hlist_node *tmp;
 729         DECLARE_NODE_PTR_FOR_HLIST(node);
 730
 731         trampoline_address = (unsigned long)&swap_kretprobe_trampoline;
 732
 733         preempt_disable();
 734         kcb = swap_get_kprobe_ctlblk();
 735
 736         spin_lock_irqsave(&kretprobe_lock, flags);
 737
 738         /*
 739          * We are using different hash keys (current and mm) for finding kernel
 740          * space and user space probes.  Kernel space probes can change mm field
 741          * in task_struct.  User space probes can be shared between threads of
 742          * one process so they have different current but same mm.
 743          */
 744         head = kretprobe_inst_table_head(current);
 745
 746 #ifdef CONFIG_X86
 747         regs->XREG(cs) = __KERNEL_CS | get_kernel_rpl();
 748         regs->EREG(ip) = trampoline_address;
 749         regs->ORIG_EAX_REG = 0xffffffff;
 750 #endif
 751
 752         /*
 753          * It is possible to have multiple instances associated with a given
 754          * task either because an multiple functions in the call path
 755          * have a return probe installed on them, and/or more then one
 756          * return probe was registered for a target function.
 757          *
 758          * We can handle this because:
 759          *     - instances are always inserted at the head of the list
 760          *     - when multiple return probes are registered for the same
 761          *       function, the first instance's ret_addr will point to the
 762          *       real return address, and all the rest will point to
 763          *       kretprobe_trampoline
 764          */
 765         swap_hlist_for_each_entry_safe(ri, node, tmp, head, hlist) {
 766                 if (ri->task != current)
 767                         /* another task is sharing our hash bucket */
 768                         continue;
 769                 if (ri->rp && ri->rp->handler) {
 770                         __get_cpu_var(swap_current_kprobe) = &ri->rp->kp;
 771                         swap_get_kprobe_ctlblk()->kprobe_status =
 772                                 KPROBE_HIT_ACTIVE;
 773                         ri->rp->handler(ri, regs);
 774                         __get_cpu_var(swap_current_kprobe) = NULL;
 775                 }
 776
 777                 orig_ret_address = (unsigned long)ri->ret_addr;
 778                 recycle_rp_inst(ri);
 779                 if (orig_ret_address != trampoline_address)
 780                         /*
 781                          * This is the real return address. Any other
 782                          * instances associated with this task are for
 783                          * other calls deeper on the call stack
 784                          */
 785                         break;
 786         }
 787         kretprobe_assert(ri, orig_ret_address, trampoline_address);
 788
 789         if (kcb->kprobe_status == KPROBE_REENTER)
 790                 restore_previous_kprobe(kcb);
 791         else
 792                 swap_reset_current_kprobe();
 793
 794         spin_unlock_irqrestore(&kretprobe_lock, flags);
 795         swap_preempt_enable_no_resched();
 796
 797         /*
 798          * By returning a non-zero value, we are telling
 799          * kprobe_handler() that we don't want the post_handler
 800          * to run (and have re-enabled preemption)
 801          */
 802
 803         return (int)orig_ret_address;
 804 }
 805
 806 #define SCHED_RP_NR 200
 807 #define COMMON_RP_NR 10
 808
 809 static int alloc_nodes_kretprobe(struct kretprobe *rp)
 810 {
 811         int alloc_nodes;
 812         struct kretprobe_instance *inst;
 813         int i;
 814
 815         DBPRINTF("Alloc aditional mem for retprobes");
 816
 817         if ((unsigned long)rp->kp.addr == sched_addr) {
 818                 rp->maxactive += SCHED_RP_NR; /* max (100, 2 * NR_CPUS); */
 819                 alloc_nodes = SCHED_RP_NR;
 820         } else {
 821 #if 1/* def CONFIG_PREEMPT */
 822                 rp->maxactive += max(COMMON_RP_NR, 2 * NR_CPUS);
 823 #else
 824                 rp->maxacpptive += NR_CPUS;
 825 #endif
 826                 alloc_nodes = COMMON_RP_NR;
 827         }
 828
 829         for (i = 0; i < alloc_nodes; i++) {
 830                 inst = kmalloc(sizeof(*inst) + rp->data_size, GFP_ATOMIC);
 831                 if (inst == NULL) {
 832                         free_rp_inst(rp);
 833                         return -ENOMEM;
 834                 }
 835                 INIT_HLIST_NODE(&inst->uflist);
 836                 hlist_add_head(&inst->uflist, &rp->free_instances);
 837         }
 838
 839         DBPRINTF("addr=%p, *addr=[%lx %lx %lx]", rp->kp.addr,
 840                   (unsigned long) (*(rp->kp.addr)),
 841                   (unsigned long) (*(rp->kp.addr + 1)),
 842                   (unsigned long) (*(rp->kp.addr + 2)));
 843         return 0;
 844 }
 845
 846 /**
 847  * @brief Registers kretprobes.
 848  *
 849  * @param rp Pointer to the target kretprobe.
 850  * @return 0 on success, error code on error.
 851  */
 852 int swap_register_kretprobe(struct kretprobe *rp)
 853 {
 854         int ret = 0;
 855         struct kretprobe_instance *inst;
 856         int i;
 857         DBPRINTF("START");
 858
 859         rp->kp.pre_handler = pre_handler_kretprobe;
 860         rp->kp.post_handler = NULL;
 861         rp->kp.fault_handler = NULL;
 862         rp->kp.break_handler = NULL;
 863
 864         /* Pre-allocate memory for max kretprobe instances */
 865         if ((unsigned long)rp->kp.addr == exit_addr) {
 866                 rp->kp.pre_handler = NULL; /* not needed for do_exit */
 867                 rp->maxactive = 0;
 868         } else if ((unsigned long)rp->kp.addr == do_group_exit_addr) {
 869                 rp->kp.pre_handler = NULL;
 870                 rp->maxactive = 0;
 871         } else if ((unsigned long)rp->kp.addr == sys_exit_group_addr) {
 872                 rp->kp.pre_handler = NULL;
 873                 rp->maxactive = 0;
 874         } else if ((unsigned long)rp->kp.addr == sys_exit_addr) {
 875                 rp->kp.pre_handler = NULL;
 876                 rp->maxactive = 0;
 877         } else if (rp->maxactive <= 0) {
 878 #if 1/* def CONFIG_PREEMPT */
 879                 rp->maxactive = max(COMMON_RP_NR, 2 * NR_CPUS);
 880 #else
 881                 rp->maxactive = NR_CPUS;
 882 #endif
 883         }
 884         INIT_HLIST_HEAD(&rp->used_instances);
 885         INIT_HLIST_HEAD(&rp->free_instances);
 886         for (i = 0; i < rp->maxactive; i++) {
 887                 inst = kmalloc(sizeof(*inst) + rp->data_size, GFP_KERNEL);
 888                 if (inst == NULL) {
 889                         free_rp_inst(rp);
 890                         return -ENOMEM;
 891                 }
 892                 INIT_HLIST_NODE(&inst->uflist);
 893                 hlist_add_head(&inst->uflist, &rp->free_instances);
 894         }
 895
 896         DBPRINTF("addr=%p, *addr=[%lx %lx %lx]", rp->kp.addr,
 897                   (unsigned long) (*(rp->kp.addr)),
 898                   (unsigned long) (*(rp->kp.addr + 1)),
 899                   (unsigned long) (*(rp->kp.addr + 2)));
 900         rp->nmissed = 0;
 901         /* Establish function entry probe point */
 902         ret = swap_register_kprobe(&rp->kp);
 903         if (ret != 0)
 904                 free_rp_inst(rp);
 905
 906         DBPRINTF("addr=%p, *addr=[%lx %lx %lx]", rp->kp.addr,
 907                   (unsigned long) (*(rp->kp.addr)),
 908                   (unsigned long) (*(rp->kp.addr + 1)),
 909                   (unsigned long) (*(rp->kp.addr + 2)));
 910
 911         return ret;
 912 }
 913 EXPORT_SYMBOL_GPL(swap_register_kretprobe);
 914
 915 static int swap_disarm_krp_inst(struct kretprobe_instance *ri);
 916
 917 static void swap_disarm_krp(struct kretprobe *rp)
 918 {
 919         struct kretprobe_instance *ri;
 920         DECLARE_NODE_PTR_FOR_HLIST(node);
 921
 922         swap_hlist_for_each_entry(ri, node, &rp->used_instances, uflist) {
 923                 if (swap_disarm_krp_inst(ri) != 0) {
 924                         printk(KERN_INFO "%s (%d/%d): cannot disarm "
 925                                "krp instance (%08lx)\n",
 926                                ri->task->comm, ri->task->tgid, ri->task->pid,
 927                                (unsigned long)rp->kp.addr);
 928                 }
 929         }
 930 }
 931
 932
 933 struct unreg_krp_args {
 934         struct kretprobe **rps;
 935         size_t size;
 936         int rp_disarm;
 937 };
 938
 939 static int __swap_unregister_kretprobes_top(void *data)
 940 {
 941         struct unreg_krp_args *args = data;
 942         struct kretprobe **rps = args->rps;
 943         size_t size = args->size;
 944         int rp_disarm = args->rp_disarm;
 945         unsigned long flags;
 946         const size_t end = ((size_t) 0) - 1;
 947
 948         for (--size; size != end; --size) {
 949                 swap_unregister_kprobe(&rps[size]->kp);
 950                 if (rp_disarm) {
 951                         spin_lock_irqsave(&kretprobe_lock, flags);
 952                         swap_disarm_krp(rps[size]);
 953                         spin_unlock_irqrestore(&kretprobe_lock, flags);
 954                 }
 955         }
 956
 957         return 0;
 958 }
 959
 960 /**
 961  * @brief Kretprobes unregister top. Unregisters kprobes.
 962  *
 963  * @param rps Pointer to the array of pointers to the target kretprobes.
 964  * @param size Size of rps array.
 965  * @param rp_disarm Disarm flag. If set kretprobe is disarmed.
 966  * @return Void.
 967  */
 968 void swap_unregister_kretprobes_top(struct kretprobe **rps, size_t size,
 969                                    int rp_disarm)
 970 {
 971         struct unreg_krp_args args = {
 972                 .rps = rps,
 973                 .size = size,
 974                 .rp_disarm = rp_disarm,
 975         };
 976
 977         if (rp_disarm) {
 978                 int ret;
 979
 980                 ret = stop_machine(__swap_unregister_kretprobes_top,
 981                                    &args, NULL);
 982                 if (ret)
 983                         pr_err("%s failed (%d)\n", __func__, ret);
 984         } else {
 985                 __swap_unregister_kretprobes_top(&args);
 986         }
 987 }
 988 EXPORT_SYMBOL_GPL(swap_unregister_kretprobes_top);
 989
 990 /**
 991  * @brief swap_unregister_kretprobes_top wrapper for a single kretprobe.
 992  *
 993  * @param rp Pointer to the target kretprobe.
 994  * @param rp_disarm Disarm flag.
 995  * @return Void.
 996  */
 997 void swap_unregister_kretprobe_top(struct kretprobe *rp, int rp_disarm)
 998 {
 999         swap_unregister_kretprobes_top(&rp, 1, rp_disarm);
1000 }
1001 EXPORT_SYMBOL_GPL(swap_unregister_kretprobe_top);
1002
1003 /**
1004  * @brief Kretprobe unregister bottom. Here is kretprobe memory is released.
1005  *
1006  * @param rp Pointer to the target kretprobe.
1007  * @return Void.
1008  */
1009 void swap_unregister_kretprobe_bottom(struct kretprobe *rp)
1010 {
1011         unsigned long flags;
1012         struct kretprobe_instance *ri;
1013
1014         spin_lock_irqsave(&kretprobe_lock, flags);
1015
1016         while ((ri = get_used_rp_inst(rp)) != NULL)
1017                 recycle_rp_inst(ri);
1018         free_rp_inst(rp);
1019
1020         spin_unlock_irqrestore(&kretprobe_lock, flags);
1021 }
1022 EXPORT_SYMBOL_GPL(swap_unregister_kretprobe_bottom);
1023
1024 /**
1025  * @brief swap_unregister_kretprobe_bottom wrapper for several kretprobes.
1026  *
1027  * @param rps Pointer to the array of the target kretprobes pointers.
1028  * @param size Size of rps array.
1029  * @return Void.
1030  */
1031 void swap_unregister_kretprobes_bottom(struct kretprobe **rps, size_t size)
1032 {
1033         const size_t end = ((size_t) 0) - 1;
1034
1035         for (--size; size != end; --size)
1036                 swap_unregister_kretprobe_bottom(rps[size]);
1037 }
1038 EXPORT_SYMBOL_GPL(swap_unregister_kretprobes_bottom);
1039
1040 /**
1041  * @brief Unregisters kretprobes.
1042  *
1043  * @param rpp Pointer to the array of the target kretprobes pointers.
1044  * @param size Size of rpp array.
1045  * @return Void.
1046  */
1047 void swap_unregister_kretprobes(struct kretprobe **rpp, size_t size)
1048 {
1049         swap_unregister_kretprobes_top(rpp, size, 1);
1050
1051         if (!in_atomic())
1052                 synchronize_sched();
1053
1054         swap_unregister_kretprobes_bottom(rpp, size);
1055 }
1056 EXPORT_SYMBOL_GPL(swap_unregister_kretprobes);
1057
1058 /**
1059  * @brief swap_unregister_kretprobes wrapper for a single kretprobe.
1060  *
1061  * @param rp Pointer to the target kretprobe.
1062  * @return Void.
1063  */
1064 void swap_unregister_kretprobe(struct kretprobe *rp)
1065 {
1066         swap_unregister_kretprobes(&rp, 1);
1067 }
1068 EXPORT_SYMBOL_GPL(swap_unregister_kretprobe);
1069
1070 static inline void rm_task_trampoline(struct task_struct *p,
1071                                       struct kretprobe_instance *ri)
1072 {
1073         arch_set_task_pc(p, (unsigned long)ri->ret_addr);
1074 }
1075
1076 static int swap_disarm_krp_inst(struct kretprobe_instance *ri)
1077 {
1078         unsigned long *tramp = (unsigned long *)&swap_kretprobe_trampoline;
1079         unsigned long *sp = ri->sp;
1080         unsigned long *found = NULL;
1081         int retval = -ENOENT;
1082
1083         if (!sp) {
1084                 unsigned long pc = arch_get_task_pc(ri->task);
1085
1086                 printk(KERN_INFO "---> [%d] %s (%d/%d): pc = %08lx, ra = %08lx, tramp= %08lx (%08lx)\n",
1087                        task_cpu(ri->task),
1088                        ri->task->comm, ri->task->tgid, ri->task->pid,
1089                        pc, (long unsigned int)ri->ret_addr,
1090                        (long unsigned int)tramp,
1091                        (long unsigned int)(ri->rp ? ri->rp->kp.addr : NULL));
1092
1093                 /* __switch_to retprobe handling */
1094                 if (pc == (unsigned long)tramp) {
1095                         rm_task_trampoline(ri->task, ri);
1096                         return 0;
1097                 }
1098
1099                 return -EINVAL;
1100         }
1101
1102         while (sp > ri->sp - RETPROBE_STACK_DEPTH) {
1103                 if (*sp == (unsigned long)tramp) {
1104                         found = sp;
1105                         break;
1106                 }
1107                 sp--;
1108         }
1109
1110         if (found) {
1111                 printk(KERN_INFO "---> [%d] %s (%d/%d): tramp (%08lx) "
1112                        "found at %08lx (%08lx /%+d) - %p\n",
1113                        task_cpu(ri->task),
1114                        ri->task->comm, ri->task->tgid, ri->task->pid,
1115                        (long unsigned int)tramp,
1116                        (long unsigned int)found, (long unsigned int)ri->sp,
1117                        found - ri->sp, ri->rp ? ri->rp->kp.addr : NULL);
1118                 *found = (unsigned long)ri->ret_addr;
1119                 retval = 0;
1120         } else {
1121                 printk(KERN_INFO "---> [%d] %s (%d/%d): tramp (%08lx) "
1122                        "NOT found at sp = %08lx - %p\n",
1123                        task_cpu(ri->task),
1124                        ri->task->comm, ri->task->tgid, ri->task->pid,
1125                        (long unsigned int)tramp,
1126                        (long unsigned int)ri->sp,
1127                        ri->rp ? ri->rp->kp.addr : NULL);
1128         }
1129
1130         return retval;
1131 }
1132
1133 static void krp_inst_flush(struct task_struct *task)
1134 {
1135         unsigned long flags;
1136         struct kretprobe_instance *ri;
1137         struct hlist_node *tmp;
1138         struct hlist_head *head;
1139         DECLARE_NODE_PTR_FOR_HLIST(node);
1140
1141         spin_lock_irqsave(&kretprobe_lock, flags);
1142         head = kretprobe_inst_table_head(task);
1143         swap_hlist_for_each_entry_safe(ri, node, tmp, head, hlist) {
1144                 if (ri->task == task) {
1145                         printk("task[%u %u %s]: flush krp_inst, ret_addr=%p\n",
1146                                 task->tgid, task->pid, task->comm,
1147                                 ri->ret_addr);
1148                         recycle_rp_inst(ri);
1149                 }
1150         }
1151         spin_unlock_irqrestore(&kretprobe_lock, flags);
1152 }
1153
1154 static int put_task_handler(struct kprobe *p, struct pt_regs *regs)
1155 {
1156         struct task_struct *t = (struct task_struct *)swap_get_karg(regs, 0);
1157
1158         /* task has died */
1159         krp_inst_flush(t);
1160
1161         return 0;
1162 }
1163
1164 static struct kprobe put_task_kp = {
1165         .pre_handler = put_task_handler,
1166 };
1167
1168 static int init_module_deps(void)
1169 {
1170         int ret;
1171
1172         sched_addr = swap_ksyms("__switch_to");
1173         exit_addr = swap_ksyms("do_exit");
1174         sys_exit_group_addr = swap_ksyms("sys_exit_group");
1175         do_group_exit_addr = swap_ksyms("do_group_exit");
1176         sys_exit_addr = swap_ksyms("sys_exit");
1177
1178         if (sched_addr == 0 ||
1179             exit_addr == 0 ||
1180             sys_exit_group_addr == 0 ||
1181             do_group_exit_addr == 0 ||
1182             sys_exit_addr == 0) {
1183                 return -ESRCH;
1184         }
1185
1186         ret = init_module_dependencies();
1187         if (ret)
1188                 return ret;
1189
1190         return arch_init_module_deps();
1191 }
1192
1193 static int once(void)
1194 {
1195         int i, ret;
1196         const char *sym;
1197
1198         sym = "module_alloc";
1199         module_alloc = (void *)swap_ksyms(sym);
1200         if (module_alloc == NULL)
1201                 goto not_found;
1202
1203         sym = "module_free";
1204         module_free = (void *)swap_ksyms(sym);
1205         if (module_alloc == NULL)
1206                 goto not_found;
1207
1208         sym = "__put_task_struct";
1209         put_task_kp.addr = (void *)swap_ksyms(sym);
1210         if (put_task_kp.addr == NULL)
1211                 goto not_found;
1212
1213         ret = init_module_deps();
1214         if (ret)
1215                 return ret;
1216
1217         /*
1218          * FIXME allocate the probe table, currently defined statically
1219          * initialize all list heads
1220          */
1221         for (i = 0; i < KPROBE_TABLE_SIZE; ++i) {
1222                 INIT_HLIST_HEAD(&kprobe_table[i]);
1223                 INIT_HLIST_HEAD(&kretprobe_inst_table[i]);
1224         }
1225
1226         return 0;
1227
1228 not_found:
1229         printk(KERN_INFO "ERROR: symbol '%s' not found\n", sym);
1230         return -ESRCH;
1231 }
1232
1233 static int init_kprobes(void)
1234 {
1235         int ret;
1236
1237         init_sm();
1238         atomic_set(&kprobe_count, 0);
1239
1240         ret = swap_arch_init_kprobes();
1241         if (ret)
1242                 return ret;
1243
1244         ret = swap_register_kprobe(&put_task_kp);
1245         if (ret) {
1246                 swap_arch_exit_kprobes();
1247                 return ret;
1248         }
1249
1250         return 0;
1251 }
1252
1253 static void exit_kprobes(void)
1254 {
1255         swap_unregister_kprobe(&put_task_kp);
1256         swap_arch_exit_kprobes();
1257         exit_sm();
1258 }
1259
1260 SWAP_LIGHT_INIT_MODULE(once, init_kprobes, exit_kprobes, NULL, NULL);
1261
1262 MODULE_LICENSE("GPL");