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