kprobe/arch/arm/swap-asm/swap_kprobes.c

   1 /**
   2  * kprobe/arch/asm-arm/swap_kprobes.c
   3  * @author Ekaterina Gorelkina <e.gorelkina@samsung.com>: initial implementation for ARM/MIPS
   4  * @author Alexey Gerenkov <a.gerenkov@samsung.com> User-Space Probes initial implementation; Support x86.
   5  * @author Ekaterina Gorelkina <e.gorelkina@samsung.com>: redesign module for separating core and arch parts
   6  * @author Alexander Shirshikov <a.shirshikov@samsung.com>: initial implementation for Thumb
   7  * @author Stanislav Andreev <s.andreev@samsung.com>: added time debug profiling support; BUG() message fix
   8  * @author Stanislav Andreev <s.andreev@samsung.com>: redesign of kprobe functionality -
   9  * kprobe_handler() now called via undefined instruction hooks
  10  * @author Stanislav Andreev <s.andreev@samsung.com>: hash tables search implemented for uprobes
  11  *
  12  * @section LICENSE
  13  *
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License as published by
  16  * the Free Software Foundation; either version 2 of the License, or
  17  * (at your option) any later version.
  18  *
  19  * This program is distributed in the hope that it will be useful,
  20  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  21  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  22  * GNU General Public License for more details.
  23  *
  24  * You should have received a copy of the GNU General Public License
  25  * along with this program; if not, write to the Free Software
  26  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  27  *
  28  * @section COPYRIGHT
  29  *
  30  * Copyright (C) Samsung Electronics, 2006-2014
  31  *
  32  * @section DESCRIPTION
  33  *
  34  * SWAP kprobe implementation for ARM architecture.
  35  */
  36
  37 #include <linux/module.h>
  38 #include <linux/mm.h>
  39
  40 #include "swap_kprobes.h"
  41 #include "trampoline_arm.h"
  42 #include <kprobe/swap_kprobes.h>
  43
  44 #include <kprobe/swap_kdebug.h>
  45 #include <kprobe/swap_slots.h>
  46 #include <kprobe/swap_kprobes_deps.h>
  47 #include <ksyms/ksyms.h>
  48
  49 #include <asm/cacheflush.h>
  50 #include <asm/traps.h>
  51 #include <asm/ptrace.h>
  52 #include <linux/list.h>
  53 #include <linux/hash.h>
  54
  55 #define SUPRESS_BUG_MESSAGES            /**< Debug-off definition */
  56
  57 #define sign_extend(x, signbit) ((x) | (0 - ((x) & (1 << (signbit)))))
  58 #define branch_displacement(insn) sign_extend(((insn) & 0xffffff) << 2, 25)
  59
  60
  61 static void (*__swap_register_undef_hook)(struct undef_hook *hook);
  62 static void (*__swap_unregister_undef_hook)(struct undef_hook *hook);
  63
  64 static unsigned long get_addr_b(unsigned long insn, unsigned long addr)
  65 {
  66         /* real position less then PC by 8 */
  67         return (kprobe_opcode_t)((long)addr + 8 + branch_displacement(insn));
  68 }
  69
  70 static int prep_pc_dep_insn_execbuf(kprobe_opcode_t *insns,
  71                                     kprobe_opcode_t insn, int uregs)
  72 {
  73         int i;
  74
  75         if (uregs & 0x10) {
  76                 int reg_mask = 0x1;
  77                 //search in reg list
  78                 for (i = 0; i < 13; i++, reg_mask <<= 1) {
  79                         if (!(insn & reg_mask))
  80                                 break;
  81                 }
  82         } else {
  83                 for (i = 0; i < 13; i++) {
  84                         if ((uregs & 0x1) && (ARM_INSN_REG_RN(insn) == i))
  85                                 continue;
  86                         if ((uregs & 0x2) && (ARM_INSN_REG_RD(insn) == i))
  87                                 continue;
  88                         if ((uregs & 0x4) && (ARM_INSN_REG_RS(insn) == i))
  89                                 continue;
  90                         if ((uregs & 0x8) && (ARM_INSN_REG_RM(insn) == i))
  91                                 continue;
  92                         break;
  93                 }
  94         }
  95
  96         if (i == 13) {
  97                 DBPRINTF ("there are no free register %x in insn %lx!", uregs, insn);
  98                 return -EINVAL;
  99         }
 100         DBPRINTF ("prep_pc_dep_insn_execbuf: using R%d, changing regs %x", i, uregs);
 101
 102         // set register to save
 103         ARM_INSN_REG_SET_RD(insns[0], i);
 104         // set register to load address to
 105         ARM_INSN_REG_SET_RD(insns[1], i);
 106         // set instruction to execute and patch it
 107         if (uregs & 0x10) {
 108                 ARM_INSN_REG_CLEAR_MR(insn, 15);
 109                 ARM_INSN_REG_SET_MR(insn, i);
 110         } else {
 111                 if ((uregs & 0x1) && (ARM_INSN_REG_RN(insn) == 15))
 112                         ARM_INSN_REG_SET_RN(insn, i);
 113                 if ((uregs & 0x2) && (ARM_INSN_REG_RD(insn) == 15))
 114                         ARM_INSN_REG_SET_RD(insn, i);
 115                 if ((uregs & 0x4) && (ARM_INSN_REG_RS(insn) == 15))
 116                         ARM_INSN_REG_SET_RS(insn, i);
 117                 if ((uregs & 0x8) && (ARM_INSN_REG_RM(insn) == 15))
 118                         ARM_INSN_REG_SET_RM(insn, i);
 119         }
 120
 121         insns[UPROBES_TRAMP_INSN_IDX] = insn;
 122         // set register to restore
 123         ARM_INSN_REG_SET_RD(insns[3], i);
 124
 125         return 0;
 126 }
 127
 128 static int arch_check_insn_arm(unsigned long insn)
 129 {
 130         /* check instructions that can change PC by nature */
 131         if (
 132          /* ARM_INSN_MATCH(UNDEF, insn) || */
 133             ARM_INSN_MATCH(AUNDEF, insn) ||
 134             ARM_INSN_MATCH(SWI, insn) ||
 135             ARM_INSN_MATCH(BREAK, insn) ||
 136             ARM_INSN_MATCH(BXJ, insn)) {
 137                 goto bad_insn;
 138 #ifndef CONFIG_CPU_V7
 139         /* check instructions that can write result to PC */
 140         } else if ((ARM_INSN_MATCH(DPIS, insn) ||
 141                     ARM_INSN_MATCH(DPRS, insn) ||
 142                     ARM_INSN_MATCH(DPI, insn) ||
 143                     ARM_INSN_MATCH(LIO, insn) ||
 144                     ARM_INSN_MATCH(LRO, insn)) &&
 145                    (ARM_INSN_REG_RD(insn) == 15)) {
 146                 goto bad_insn;
 147 #endif /* CONFIG_CPU_V7 */
 148         /* check special instruction loads store multiple registers */
 149         } else if ((ARM_INSN_MATCH(LM, insn) || ARM_INSN_MATCH(SM, insn)) &&
 150                         /* store PC or load to PC */
 151                    (ARM_INSN_REG_MR(insn, 15) ||
 152                          /* store/load with PC update */
 153                     ((ARM_INSN_REG_RN(insn) == 15) && (insn & 0x200000)))) {
 154                 goto bad_insn;
 155         }
 156
 157         return 0;
 158
 159 bad_insn:
 160         return -EFAULT;
 161 }
 162
 163 static int make_branch_tarmpoline(unsigned long addr, unsigned long insn,
 164                                   unsigned long *tramp)
 165 {
 166         int ok = 0;
 167
 168         /* B */
 169         if (ARM_INSN_MATCH(B, insn) &&
 170             !ARM_INSN_MATCH(BLX1, insn)) {
 171                 /* B check can be false positive on BLX1 instruction */
 172                 memcpy(tramp, b_cond_insn_execbuf, KPROBES_TRAMP_LEN);
 173                 tramp[KPROBES_TRAMP_RET_BREAK_IDX] = BREAKPOINT_INSTRUCTION;
 174                 tramp[0] |= insn & 0xf0000000;
 175                 tramp[6] = get_addr_b(insn, addr);
 176                 tramp[7] = addr + 4;
 177                 ok = 1;
 178         /* BX, BLX (Rm) */
 179         } else if (ARM_INSN_MATCH(BX, insn) ||
 180                    ARM_INSN_MATCH(BLX2, insn)) {
 181                 memcpy(tramp, b_r_insn_execbuf, KPROBES_TRAMP_LEN);
 182                 tramp[0] = insn;
 183                 tramp[KPROBES_TRAMP_RET_BREAK_IDX] = BREAKPOINT_INSTRUCTION;
 184                 tramp[7] = addr + 4;
 185                 ok = 1;
 186         /* BL, BLX (Off) */
 187         } else if (ARM_INSN_MATCH(BLX1, insn)) {
 188                 memcpy(tramp, blx_off_insn_execbuf, KPROBES_TRAMP_LEN);
 189                 tramp[0] |= 0xe0000000;
 190                 tramp[1] |= 0xe0000000;
 191                 tramp[KPROBES_TRAMP_RET_BREAK_IDX] = BREAKPOINT_INSTRUCTION;
 192                 tramp[6] = get_addr_b(insn, addr) +
 193                            2 * (insn & 01000000) + 1; /* jump to thumb */
 194                 tramp[7] = addr + 4;
 195                 ok = 1;
 196         /* BL */
 197         } else if (ARM_INSN_MATCH(BL, insn)) {
 198                 memcpy(tramp, blx_off_insn_execbuf, KPROBES_TRAMP_LEN);
 199                 tramp[0] |= insn & 0xf0000000;
 200                 tramp[1] |= insn & 0xf0000000;
 201                 tramp[KPROBES_TRAMP_RET_BREAK_IDX] = BREAKPOINT_INSTRUCTION;
 202                 tramp[6] = get_addr_b(insn, addr);
 203                 tramp[7] = addr + 4;
 204                 ok = 1;
 205         }
 206
 207         return ok;
 208 }
 209
 210 /**
 211  * @brief Creates ARM trampoline.
 212  *
 213  * @param addr Probe address.
 214  * @param insn Instuction at this address.
 215  * @param tramp Pointer to memory for trampoline.
 216  * @return 0 on success, error code on error.
 217  */
 218 int arch_make_trampoline_arm(unsigned long addr, unsigned long insn,
 219                              unsigned long *tramp)
 220 {
 221         int ret, uregs, pc_dep;
 222
 223         if (addr & 0x03) {
 224                 printk("Error in %s at %d: attempt to register uprobe "
 225                        "at an unaligned address\n", __FILE__, __LINE__);
 226                 return -EINVAL;
 227         }
 228
 229         ret = arch_check_insn_arm(insn);
 230         if (ret)
 231                 return ret;
 232
 233         if (make_branch_tarmpoline(addr, insn, tramp))
 234                 return 0;
 235
 236         uregs = pc_dep = 0;
 237         /* Rm */
 238         if (ARM_INSN_MATCH(CLZ, insn)) {
 239                 uregs = 0xa;
 240                 if (ARM_INSN_REG_RM(insn) == 15)
 241                         pc_dep = 1;
 242         /* Rn, Rm ,Rd */
 243         } else if (ARM_INSN_MATCH(DPIS, insn) || ARM_INSN_MATCH(LRO, insn) ||
 244             ARM_INSN_MATCH(SRO, insn)) {
 245                 uregs = 0xb;
 246                 if ((ARM_INSN_REG_RN(insn) == 15) ||
 247                     (ARM_INSN_REG_RM(insn) == 15) ||
 248                     (ARM_INSN_MATCH(SRO, insn) &&
 249                      (ARM_INSN_REG_RD(insn) == 15))) {
 250                         pc_dep = 1;
 251                 }
 252         /* Rn ,Rd */
 253         } else if (ARM_INSN_MATCH(DPI, insn) || ARM_INSN_MATCH(LIO, insn) ||
 254                    ARM_INSN_MATCH(SIO, insn)) {
 255                 uregs = 0x3;
 256                 if ((ARM_INSN_REG_RN(insn) == 15) ||
 257                     (ARM_INSN_MATCH(SIO, insn) &&
 258                     (ARM_INSN_REG_RD(insn) == 15))) {
 259                         pc_dep = 1;
 260                 }
 261         /* Rn, Rm, Rs */
 262         } else if (ARM_INSN_MATCH(DPRS, insn)) {
 263                 uregs = 0xd;
 264                 if ((ARM_INSN_REG_RN(insn) == 15) ||
 265                     (ARM_INSN_REG_RM(insn) == 15) ||
 266                     (ARM_INSN_REG_RS(insn) == 15)) {
 267                         pc_dep = 1;
 268                 }
 269         /* register list */
 270         } else if (ARM_INSN_MATCH(SM, insn)) {
 271                 uregs = 0x10;
 272                 if (ARM_INSN_REG_MR(insn, 15)) {
 273                         pc_dep = 1;
 274                 }
 275         }
 276
 277         /* check instructions that can write result to SP and uses PC */
 278         if (pc_dep && (ARM_INSN_REG_RD(insn) == 13)) {
 279                 printk("Error in %s at %d: instruction check failed (arm)\n",
 280                        __FILE__, __LINE__);
 281                 return -EFAULT;
 282         }
 283
 284         if (unlikely(uregs && pc_dep)) {
 285                 memcpy(tramp, pc_dep_insn_execbuf, KPROBES_TRAMP_LEN);
 286                 if (prep_pc_dep_insn_execbuf(tramp, insn, uregs) != 0) {
 287                         printk("Error in %s at %d: failed "
 288                                "to prepare exec buffer for insn %lx!",
 289                                __FILE__, __LINE__, insn);
 290                         return -EINVAL;
 291                 }
 292
 293                 tramp[6] = addr + 8;
 294         } else {
 295                 memcpy(tramp, gen_insn_execbuf, KPROBES_TRAMP_LEN);
 296                 tramp[KPROBES_TRAMP_INSN_IDX] = insn;
 297         }
 298
 299         /* TODO: remove for kprobe */
 300         tramp[KPROBES_TRAMP_RET_BREAK_IDX] = BREAKPOINT_INSTRUCTION;
 301         tramp[7] = addr + 4;
 302
 303         return 0;
 304 }
 305 EXPORT_SYMBOL_GPL(arch_make_trampoline_arm);
 306
 307 /**
 308  * @brief Creates trampoline for kprobe.
 309  *
 310  * @param p Pointer to kprobe.
 311  * @param sm Pointer to slot manager
 312  * @return 0 on success, error code on error.
 313  */
 314 int swap_arch_prepare_kprobe(struct kprobe *p, struct slot_manager *sm)
 315 {
 316         unsigned long addr = (unsigned long)p->addr;
 317         unsigned long insn = p->opcode = *p->addr;
 318         unsigned long *tramp;
 319         int ret;
 320
 321         tramp = swap_slot_alloc(sm);
 322         if (tramp == NULL)
 323                 return -ENOMEM;
 324
 325         ret = arch_make_trampoline_arm(addr, insn, tramp);
 326         if (ret) {
 327                 swap_slot_free(sm, tramp);
 328                 return ret;
 329         }
 330
 331         flush_icache_range((unsigned long)tramp,
 332                            (unsigned long)tramp + KPROBES_TRAMP_LEN);
 333
 334         p->ainsn.insn = tramp;
 335
 336         return 0;
 337 }
 338
 339 /**
 340  * @brief Prepares singlestep for current CPU.
 341  *
 342  * @param p Pointer to kprobe.
 343  * @param regs Pointer to CPU registers data.
 344  * @return Void.
 345  */
 346 void prepare_singlestep(struct kprobe *p, struct pt_regs *regs)
 347 {
 348         int cpu = smp_processor_id();
 349
 350         if (p->ss_addr[cpu]) {
 351                 regs->ARM_pc = (unsigned long)p->ss_addr[cpu];
 352                 p->ss_addr[cpu] = NULL;
 353         } else {
 354                 regs->ARM_pc = (unsigned long)p->ainsn.insn;
 355         }
 356 }
 357 EXPORT_SYMBOL_GPL(prepare_singlestep);
 358
 359 /**
 360  * @brief Saves previous kprobe.
 361  *
 362  * @param kcb Pointer to kprobe_ctlblk struct whereto save current kprobe.
 363  * @param p_run Pointer to kprobe.
 364  * @return Void.
 365  */
 366 void save_previous_kprobe(struct kprobe_ctlblk *kcb, struct kprobe *p_run)
 367 {
 368         kcb->prev_kprobe.kp = swap_kprobe_running();
 369         kcb->prev_kprobe.status = kcb->kprobe_status;
 370 }
 371
 372 /**
 373  * @brief Restores previous kprobe.
 374  *
 375  * @param kcb Pointer to kprobe_ctlblk which contains previous kprobe.
 376  * @return Void.
 377  */
 378 void restore_previous_kprobe(struct kprobe_ctlblk *kcb)
 379 {
 380         __get_cpu_var(swap_current_kprobe) = kcb->prev_kprobe.kp;
 381         kcb->kprobe_status = kcb->prev_kprobe.status;
 382 }
 383
 384 /**
 385  * @brief Sets currently running kprobe.
 386  *
 387  * @param p Pointer to currently running kprobe.
 388  * @param regs Pointer to CPU registers data.
 389  * @param kcb Pointer to kprobe_ctlblk.
 390  * @return Void.
 391  */
 392 void set_current_kprobe(struct kprobe *p, struct pt_regs *regs, struct kprobe_ctlblk *kcb)
 393 {
 394         __get_cpu_var(swap_current_kprobe) = p;
 395         DBPRINTF ("set_current_kprobe: p=%p addr=%p\n", p, p->addr);
 396 }
 397
 398 static int kprobe_handler(struct pt_regs *regs)
 399 {
 400         struct kprobe *p, *cur;
 401         struct kprobe_ctlblk *kcb;
 402
 403         kcb = swap_get_kprobe_ctlblk();
 404         cur = swap_kprobe_running();
 405         p = swap_get_kprobe((void *)regs->ARM_pc);
 406
 407         if (p) {
 408                 if (cur) {
 409                         /* Kprobe is pending, so we're recursing. */
 410                         switch (kcb->kprobe_status) {
 411                         case KPROBE_HIT_ACTIVE:
 412                         case KPROBE_HIT_SSDONE:
 413                                 /* A pre- or post-handler probe got us here. */
 414                                 swap_kprobes_inc_nmissed_count(p);
 415                                 save_previous_kprobe(kcb, NULL);
 416                                 set_current_kprobe(p, 0, 0);
 417                                 kcb->kprobe_status = KPROBE_REENTER;
 418                                 prepare_singlestep(p, regs);
 419                                 restore_previous_kprobe(kcb);
 420                                 break;
 421                         default:
 422                                 /* impossible cases */
 423                                 BUG();
 424                         }
 425                 } else {
 426                         set_current_kprobe(p, 0, 0);
 427                         kcb->kprobe_status = KPROBE_HIT_ACTIVE;
 428
 429                         if (!p->pre_handler || !p->pre_handler(p, regs)) {
 430                                 kcb->kprobe_status = KPROBE_HIT_SS;
 431                                 prepare_singlestep(p, regs);
 432                                 swap_reset_current_kprobe();
 433                         }
 434                 }
 435         } else {
 436                 goto no_kprobe;
 437         }
 438
 439         return 0;
 440
 441 no_kprobe:
 442         printk("no_kprobe: Not one of ours: let kernel handle it %p\n",
 443                         (unsigned long *)regs->ARM_pc);
 444         return 1;
 445 }
 446
 447 /**
 448  * @brief Trap handler.
 449  *
 450  * @param regs Pointer to CPU register data.
 451  * @param instr Instruction.
 452  * @return kprobe_handler result.
 453  */
 454 int kprobe_trap_handler(struct pt_regs *regs, unsigned int instr)
 455 {
 456         int ret;
 457         unsigned long flags;
 458
 459 #ifdef SUPRESS_BUG_MESSAGES
 460         int swap_oops_in_progress;
 461         /* oops_in_progress used to avoid BUG() messages
 462          * that slow down kprobe_handler() execution */
 463         swap_oops_in_progress = oops_in_progress;
 464         oops_in_progress = 1;
 465 #endif
 466
 467         local_irq_save(flags);
 468         preempt_disable();
 469         ret = kprobe_handler(regs);
 470         preempt_enable_no_resched();
 471         local_irq_restore(flags);
 472
 473 #ifdef SUPRESS_BUG_MESSAGES
 474         oops_in_progress = swap_oops_in_progress;
 475 #endif
 476
 477         return ret;
 478 }
 479
 480 /**
 481  * @brief Probe pre handler.
 482  *
 483  * @param p Pointer to fired kprobe.
 484  * @param regs Pointer to CPU registers data.
 485  * @return 0.
 486  */
 487 int swap_setjmp_pre_handler(struct kprobe *p, struct pt_regs *regs)
 488 {
 489         struct jprobe *jp = container_of(p, struct jprobe, kp);
 490         kprobe_pre_entry_handler_t pre_entry = (kprobe_pre_entry_handler_t)jp->pre_entry;
 491         entry_point_t entry = (entry_point_t)jp->entry;
 492         pre_entry = (kprobe_pre_entry_handler_t)jp->pre_entry;
 493
 494         if (pre_entry) {
 495                 p->ss_addr[smp_processor_id()] = (void *)
 496                                                  pre_entry(jp->priv_arg, regs);
 497         }
 498
 499         if (entry) {
 500                 entry(regs->ARM_r0, regs->ARM_r1, regs->ARM_r2,
 501                       regs->ARM_r3, regs->ARM_r4, regs->ARM_r5);
 502         } else {
 503                 swap_jprobe_return();
 504         }
 505
 506         return 0;
 507 }
 508
 509 /**
 510  * @brief Jprobe return stub.
 511  *
 512  * @return Void.
 513  */
 514 void swap_jprobe_return(void)
 515 {
 516 }
 517 EXPORT_SYMBOL_GPL(swap_jprobe_return);
 518
 519 /**
 520  * @brief Break handler stub.
 521  *
 522  * @param p Pointer to fired kprobe.
 523  * @param regs Pointer to CPU registers data.
 524  * @return 0.
 525  */
 526 int swap_longjmp_break_handler (struct kprobe *p, struct pt_regs *regs)
 527 {
 528         return 0;
 529 }
 530 EXPORT_SYMBOL_GPL(swap_longjmp_break_handler);
 531
 532 #ifdef CONFIG_STRICT_MEMORY_RWX
 533 #include "memory_rwx.h"
 534
 535 static void write_u32(unsigned long addr, unsigned long val)
 536 {
 537         mem_rwx_write_u32(addr, val);
 538 }
 539 #else /* CONFIG_STRICT_MEMORY_RWX */
 540 static void write_u32(unsigned long addr, unsigned long val)
 541 {
 542         *(long *)addr = val;
 543         flush_icache_range(addr, addr + sizeof(long));
 544 }
 545 #endif /* CONFIG_STRICT_MEMORY_RWX */
 546
 547 /**
 548  * @brief Arms kprobe.
 549  *
 550  * @param p Pointer to target kprobe.
 551  * @return Void.
 552  */
 553 void swap_arch_arm_kprobe(struct kprobe *p)
 554 {
 555         write_u32((long)p->addr, BREAKPOINT_INSTRUCTION);
 556 }
 557
 558 /**
 559  * @brief Disarms kprobe.
 560  *
 561  * @param p Pointer to target kprobe.
 562  * @return Void.
 563  */
 564 void swap_arch_disarm_kprobe(struct kprobe *p)
 565 {
 566         write_u32((long)p->addr, p->opcode);
 567 }
 568
 569 /**
 570  * @brief Kretprobe trampoline. Provides jumping to probe handler.
 571  *
 572  * @return Void.
 573  */
 574 void __naked swap_kretprobe_trampoline(void)
 575 {
 576         __asm__ __volatile__ (
 577                 "stmdb  sp!, {r0 - r11}         \n\t"
 578                 "mov    r1, sp                  \n\t"
 579                 "mov    r0, #0                  \n\t"
 580                 "bl     trampoline_probe_handler\n\t"
 581                 "mov    lr, r0                  \n\t"
 582                 "ldmia  sp!, {r0 - r11}         \n\t"
 583                 "bx     lr                      \n\t"
 584                 : : : "memory");
 585 }
 586
 587 /**
 588  * @brief Prepares kretprobes, saves ret address, makes function return to
 589  * trampoline.
 590  *
 591  * @param ri Pointer to kretprobe_instance.
 592  * @param regs Pointer to CPU registers data.
 593  * @return Void.
 594  */
 595 void swap_arch_prepare_kretprobe(struct kretprobe_instance *ri,
 596                                  struct pt_regs *regs)
 597 {
 598         unsigned long *ptr_ret_addr;
 599
 600         /* for __switch_to probe */
 601         if ((unsigned long)ri->rp->kp.addr == sched_addr) {
 602                 struct thread_info *tinfo = (struct thread_info *)regs->ARM_r2;
 603
 604                 ptr_ret_addr = (unsigned long *)&tinfo->cpu_context.pc;
 605                 ri->sp = NULL;
 606                 ri->task = tinfo->task;
 607         } else {
 608                 ptr_ret_addr = (unsigned long *)&regs->ARM_lr;
 609                 ri->sp = (unsigned long *)regs->ARM_sp;
 610         }
 611
 612         /* Save the return address */
 613         ri->ret_addr = (unsigned long *)*ptr_ret_addr;
 614
 615         /* Replace the return addr with trampoline addr */
 616         *ptr_ret_addr = (unsigned long)&swap_kretprobe_trampoline;
 617 }
 618
 619
 620
 621
 622
 623 /*
 624  ******************************************************************************
 625  *                                   kjumper                                  *
 626  ******************************************************************************
 627  */
 628 struct kj_cb_data {
 629         unsigned long ret_addr;
 630
 631         struct pt_regs regs;
 632
 633         jumper_cb_t cb;
 634         char data[0];
 635 };
 636
 637 static struct kj_cb_data * __used kjump_handler(struct kj_cb_data *data)
 638 {
 639         /* call callback */
 640         data->cb(data->data);
 641
 642         return data;
 643 }
 644
 645 /**
 646  * @brief Trampoline for kjump kprobes.
 647  *
 648  * @return Void.
 649  */
 650 void kjump_trampoline(void);
 651 __asm(
 652         "kjump_trampoline:              \n"
 653
 654         "mov    r0, r10                 \n"
 655         "bl     kjump_handler           \n"
 656         "nop                            \n"     /* for kjump_kprobe */
 657 );
 658
 659 /**
 660  * @brief Registers callback for kjump probes.
 661  *
 662  * @param regs Pointer to CPU registers data.
 663  * @param cb Kjump probe callback of jumper_cb_t type.
 664  * @param data Pointer to data that should be saved in kj_cb_data.
 665  * @param size Size of the data.
 666  * @return 0.
 667  */
 668 int set_kjump_cb(struct pt_regs *regs, jumper_cb_t cb, void *data, size_t size)
 669 {
 670         struct kprobe *p;
 671         struct kj_cb_data *cb_data;
 672
 673         cb_data = kmalloc(sizeof(*cb_data) + size, GFP_ATOMIC);
 674         if (cb_data == NULL)
 675                 return -ENOMEM;
 676
 677         p = swap_kprobe_running();
 678         p->ss_addr[smp_processor_id()] = (kprobe_opcode_t *)&kjump_trampoline;
 679
 680         cb_data->ret_addr = (unsigned long)p->ainsn.insn;
 681         cb_data->cb = cb;
 682
 683         /* save regs */
 684         memcpy(&cb_data->regs, regs, sizeof(*regs));
 685
 686         memcpy(cb_data->data, data, size);
 687
 688         /* save cb_data to r10 */
 689         regs->ARM_r10 = (long)cb_data;
 690
 691         return 0;
 692 }
 693 EXPORT_SYMBOL_GPL(set_kjump_cb);
 694
 695 static int kjump_pre_handler(struct kprobe *p, struct pt_regs *regs)
 696 {
 697         struct kj_cb_data *data = (struct kj_cb_data *)regs->ARM_r0;
 698
 699         /* restore regs */
 700         memcpy(regs, &data->regs, sizeof(*regs));
 701         p->ss_addr[smp_processor_id()] = (void *)data->ret_addr;
 702
 703         /* FIXME: potential memory leak, when process kill */
 704         kfree(data);
 705
 706         return 0;
 707 }
 708
 709 static struct kprobe kjump_kprobe = {
 710         .pre_handler = kjump_pre_handler,
 711         .addr = (unsigned long *)&kjump_trampoline + 2, /* nop */
 712 };
 713
 714 static int kjump_init(void)
 715 {
 716         int ret;
 717
 718         ret = swap_register_kprobe(&kjump_kprobe);
 719         if (ret)
 720                 printk("ERROR: kjump_init(), ret=%d\n", ret);
 721
 722         return ret;
 723 }
 724
 725 static void kjump_exit(void)
 726 {
 727         swap_unregister_kprobe(&kjump_kprobe);
 728 }
 729
 730
 731
 732
 733
 734 /*
 735  ******************************************************************************
 736  *                                   jumper                                   *
 737  ******************************************************************************
 738  */
 739 struct cb_data {
 740         unsigned long ret_addr;
 741         unsigned long r0;
 742
 743         jumper_cb_t cb;
 744         char data[0];
 745 };
 746
 747 static unsigned long __used get_r0(struct cb_data *data)
 748 {
 749         return data->r0;
 750 }
 751
 752 static unsigned long __used jump_handler(struct cb_data *data)
 753 {
 754         unsigned long ret_addr = data->ret_addr;
 755
 756         /* call callback */
 757         data->cb(data->data);
 758
 759         /* FIXME: potential memory leak, when process kill */
 760         kfree(data);
 761
 762         return ret_addr;
 763 }
 764
 765 /* FIXME: restore condition flags */
 766
 767 /**
 768  * @brief Jumper trampoline.
 769  *
 770  * @return Void.
 771  */
 772 void jump_trampoline(void);
 773 __asm(
 774         "jump_trampoline:               \n"
 775
 776         "push   {r0 - r12}              \n"
 777         "mov    r1, r0                  \n"     /* data --> r1 */
 778         "bl     get_r0                  \n"
 779         "str    r0, [sp]                \n"     /* restore r0 */
 780         "mov    r0, r1                  \n"     /* data --> r0 */
 781         "bl     jump_handler            \n"
 782         "mov    lr, r0                  \n"
 783         "pop    {r0 - r12}              \n"
 784         "bx     lr                      \n"
 785 );
 786
 787 /**
 788  * @brief Get jumper address.
 789  *
 790  * @return Jumper address.
 791  */
 792 unsigned long get_jump_addr(void)
 793 {
 794         return (unsigned long)&jump_trampoline;
 795 }
 796 EXPORT_SYMBOL_GPL(get_jump_addr);
 797
 798 /**
 799  * @brief Set jumper probe callback.
 800  *
 801  * @param ret_addr Jumper probe return address.
 802  * @param regs Pointer to CPU registers data.
 803  * @param cb Jumper callback of jumper_cb_t type.
 804  * @param data Data that should be stored in cb_data.
 805  * @param size Size of the data.
 806  * @return 0.
 807  */
 808 int set_jump_cb(unsigned long ret_addr, struct pt_regs *regs,
 809                 jumper_cb_t cb, void *data, size_t size)
 810 {
 811         struct cb_data *cb_data;
 812
 813         cb_data = kmalloc(sizeof(*cb_data) + size, GFP_ATOMIC);
 814
 815         /* save data */
 816         cb_data->ret_addr = ret_addr;
 817         cb_data->cb = cb;
 818         cb_data->r0 = regs->ARM_r0;
 819         memcpy(cb_data->data, data, size);
 820
 821         /* save cb_data to r0 */
 822         regs->ARM_r0 = (long)cb_data;
 823
 824         return 0;
 825 }
 826 EXPORT_SYMBOL_GPL(set_jump_cb);
 827
 828
 829
 830
 831 /**
 832  * @brief Registers hook on specified instruction.
 833  *
 834  * @param hook Pointer to struct undef_hook.
 835  * @return Void.
 836  */
 837 void swap_register_undef_hook(struct undef_hook *hook)
 838 {
 839         __swap_register_undef_hook(hook);
 840 }
 841 EXPORT_SYMBOL_GPL(swap_register_undef_hook);
 842
 843 /**
 844  * @brief Unregisters hook.
 845  *
 846  * @param hook Pointer to struct undef_hook.
 847  * @return Void.
 848  */
 849 void swap_unregister_undef_hook(struct undef_hook *hook)
 850 {
 851         __swap_unregister_undef_hook(hook);
 852 }
 853 EXPORT_SYMBOL_GPL(swap_unregister_undef_hook);
 854
 855 // kernel probes hook
 856 static struct undef_hook undef_ho_k = {
 857         .instr_mask     = 0xffffffff,
 858         .instr_val      = BREAKPOINT_INSTRUCTION,
 859         .cpsr_mask      = MODE_MASK,
 860         .cpsr_val       = SVC_MODE,
 861         .fn             = kprobe_trap_handler
 862 };
 863
 864 /**
 865  * @brief Initializes kprobes module for ARM arch.
 866  *
 867  * @return 0 on success, error code on error.
 868  */
 869 int swap_arch_init_kprobes(void)
 870 {
 871         int ret;
 872
 873 #ifdef CONFIG_STRICT_MEMORY_RWX
 874         ret = mem_rwx_init();
 875         if (ret)
 876                 return ret;
 877 #endif /* CONFIG_STRICT_MEMORY_RWX */
 878
 879         // Register hooks (kprobe_handler)
 880         __swap_register_undef_hook = (void *)swap_ksyms("register_undef_hook");
 881         if (__swap_register_undef_hook == NULL) {
 882                 printk("no register_undef_hook symbol found!\n");
 883                 return -1;
 884         }
 885
 886         // Unregister hooks (kprobe_handler)
 887         __swap_unregister_undef_hook = (void *)swap_ksyms("unregister_undef_hook");
 888         if (__swap_unregister_undef_hook == NULL) {
 889                 printk("no unregister_undef_hook symbol found!\n");
 890                 return -1;
 891         }
 892
 893         swap_register_undef_hook(&undef_ho_k);
 894
 895         ret = kjump_init();
 896         if (ret) {
 897                 swap_unregister_undef_hook(&undef_ho_k);
 898                 return ret;
 899         }
 900
 901         return 0;
 902 }
 903
 904 /**
 905  * @brief Uninitializes kprobe module.
 906  *
 907  * @return Void.
 908  */
 909 void swap_arch_exit_kprobes(void)
 910 {
 911         kjump_exit();
 912         swap_unregister_undef_hook(&undef_ho_k);
 913
 914 #ifdef CONFIG_STRICT_MEMORY_RWX
 915         mem_rwx_exit();
 916 #endif /* CONFIG_STRICT_MEMORY_RWX */
 917 }
 918
 919 /* export symbol for trampoline_arm.h */
 920 EXPORT_SYMBOL_GPL(gen_insn_execbuf);
 921 EXPORT_SYMBOL_GPL(pc_dep_insn_execbuf);
 922 EXPORT_SYMBOL_GPL(b_r_insn_execbuf);
 923 EXPORT_SYMBOL_GPL(b_cond_insn_execbuf);
 924 EXPORT_SYMBOL_GPL(blx_off_insn_execbuf);