uprobe/arch/asm-arm/swap_uprobes.c

   1 #include <dbi_kprobes.h>
   2 #include <asm/dbi_kprobes.h>
   3 #include <asm/traps.h>
   4 #include <swap_uprobes.h>
   5
   6 // FIXME:
   7 #include <dbi_kdebug.h>
   8 extern struct hlist_head uprobe_insn_pages;
   9 kprobe_opcode_t *get_insn_slot(struct task_struct *task, struct hlist_head *page_list, int atomic);
  10 int arch_check_insn_arm(struct arch_specific_insn *ainsn);
  11 int prep_pc_dep_insn_execbuf(kprobe_opcode_t *insns, kprobe_opcode_t insn, int uregs);
  12 void free_insn_slot(struct hlist_head *page_list, struct task_struct *task, kprobe_opcode_t *slot);
  13 void pc_dep_insn_execbuf(void);
  14 void gen_insn_execbuf(void);
  15 void gen_insn_execbuf_thumb(void);
  16 void pc_dep_insn_execbuf_thumb(void);
  17 int kprobe_trap_handler(struct pt_regs *regs, unsigned int instr);
  18 void add_rp_inst(struct kretprobe_instance *ri);
  19 struct kretprobe_instance *get_free_rp_inst (struct kretprobe *rp);
  20
  21
  22 #define sign_extend(x, signbit) ((x) | (0 - ((x) & (1 << (signbit)))))
  23 #define branch_displacement(insn) sign_extend(((insn) & 0xffffff) << 2, 25)
  24
  25 static kprobe_opcode_t get_addr_b(kprobe_opcode_t insn, kprobe_opcode_t *addr)
  26 {
  27         // real position less then PC by 8
  28         return (kprobe_opcode_t)((long)addr + 8 + branch_displacement(insn));
  29 }
  30
  31 /* is instruction Thumb2 and NOT a branch, etc... */
  32 static int is_thumb2(kprobe_opcode_t insn)
  33 {
  34         return ((insn & 0xf800) == 0xe800 ||
  35                 (insn & 0xf800) == 0xf000 ||
  36                 (insn & 0xf800) == 0xf800);
  37 }
  38
  39 static int arch_copy_trampoline_arm_uprobe(struct kprobe *p, struct task_struct *task, int atomic)
  40 {
  41         kprobe_opcode_t insns[UPROBES_TRAMP_LEN];
  42         int uregs, pc_dep;
  43         kprobe_opcode_t insn[MAX_INSN_SIZE];
  44         struct arch_specific_insn ainsn;
  45
  46         p->safe_arm = -1;
  47         if ((unsigned long)p->addr & 0x01) {
  48                 printk("Error in %s at %d: attempt to register kprobe at an unaligned address\n", __FILE__, __LINE__);
  49                 return -EINVAL;
  50         }
  51
  52         insn[0] = p->opcode;
  53         ainsn.insn_arm = insn;
  54         if (!arch_check_insn_arm(&ainsn)) {
  55                 p->safe_arm = 0;
  56         }
  57
  58         uregs = pc_dep = 0;
  59         // Rn, Rm ,Rd
  60         if (ARM_INSN_MATCH(DPIS, insn[0]) || ARM_INSN_MATCH(LRO, insn[0]) ||
  61             ARM_INSN_MATCH(SRO, insn[0])) {
  62                 uregs = 0xb;
  63                 if ((ARM_INSN_REG_RN(insn[0]) == 15) || (ARM_INSN_REG_RM(insn[0]) == 15) ||
  64                     (ARM_INSN_MATCH(SRO, insn[0]) && (ARM_INSN_REG_RD(insn[0]) == 15))) {
  65                         DBPRINTF("Unboostable insn %lx, DPIS/LRO/SRO\n", insn[0]);
  66                         pc_dep = 1;
  67                 }
  68
  69         // Rn ,Rd
  70         } else if (ARM_INSN_MATCH(DPI, insn[0]) || ARM_INSN_MATCH(LIO, insn[0]) ||
  71                    ARM_INSN_MATCH (SIO, insn[0])) {
  72                 uregs = 0x3;
  73                 if ((ARM_INSN_REG_RN(insn[0]) == 15) || (ARM_INSN_MATCH(SIO, insn[0]) &&
  74                     (ARM_INSN_REG_RD(insn[0]) == 15))) {
  75                         pc_dep = 1;
  76                         DBPRINTF("Unboostable insn %lx/%p, DPI/LIO/SIO\n", insn[0], p);
  77                 }
  78
  79         // Rn, Rm, Rs
  80         } else if (ARM_INSN_MATCH(DPRS, insn[0])) {
  81                 uregs = 0xd;
  82                 if ((ARM_INSN_REG_RN(insn[0]) == 15) || (ARM_INSN_REG_RM(insn[0]) == 15) ||
  83                     (ARM_INSN_REG_RS(insn[0]) == 15)) {
  84                         pc_dep = 1;
  85                         DBPRINTF("Unboostable insn %lx, DPRS\n", insn[0]);
  86                 }
  87
  88         // register list
  89         } else if (ARM_INSN_MATCH(SM, insn[0])) {
  90                 uregs = 0x10;
  91                 if (ARM_INSN_REG_MR (insn[0], 15))
  92                 {
  93                         DBPRINTF ("Unboostable insn %lx, SM\n", insn[0]);
  94                         pc_dep = 1;
  95                 }
  96         }
  97
  98         // check instructions that can write result to SP andu uses PC
  99         if (pc_dep  && (ARM_INSN_REG_RD (ainsn.insn_arm[0]) == 13)) {
 100                 printk("Error in %s at %d: instruction check failed (arm)\n", __FILE__, __LINE__);
 101                 p->safe_arm = -1;
 102                 // TODO: move free to later phase
 103                 //free_insn_slot (&uprobe_insn_pages, task, p->ainsn.insn_arm, 0);
 104                 //ret = -EFAULT;
 105         }
 106
 107         if (unlikely(uregs && pc_dep)) {
 108                 memcpy(insns, pc_dep_insn_execbuf, sizeof(insns));
 109                 if (prep_pc_dep_insn_execbuf(insns, insn[0], uregs) != 0) {
 110                         printk("Error in %s at %d: failed to prepare exec buffer for insn %lx!",
 111                                __FILE__, __LINE__, insn[0]);
 112                         p->safe_arm = -1;
 113                         // TODO: move free to later phase
 114                         //free_insn_slot (&uprobe_insn_pages, task, p->ainsn.insn_arm, 0);
 115                         //return -EINVAL;
 116                 }
 117
 118                 insns[6] = (kprobe_opcode_t) (p->addr + 2);
 119         } else {
 120                 memcpy(insns, gen_insn_execbuf, sizeof(insns));
 121                 insns[UPROBES_TRAMP_INSN_IDX] = insn[0];
 122         }
 123
 124         insns[UPROBES_TRAMP_RET_BREAK_IDX] = BREAKPOINT_INSTRUCTION;
 125         insns[7] = (kprobe_opcode_t) (p->addr + 1);
 126
 127         // B
 128         if(ARM_INSN_MATCH(B, ainsn.insn_arm[0])) {
 129                 memcpy(insns, pc_dep_insn_execbuf, sizeof(insns));
 130                 insns[UPROBES_TRAMP_RET_BREAK_IDX] = BREAKPOINT_INSTRUCTION;
 131                 insns[6] = (kprobe_opcode_t)(p->addr + 2);
 132                 insns[7] = get_addr_b(p->opcode, p->addr);
 133         }
 134
 135         DBPRINTF("arch_prepare_uprobe: to %p - %lx %lx %lx %lx %lx %lx %lx %lx %lx",
 136                  p->ainsn.insn_arm, insns[0], insns[1], insns[2], insns[3], insns[4],
 137                  insns[5], insns[6], insns[7], insns[8]);
 138         if (!write_proc_vm_atomic(task, (unsigned long)p->ainsn.insn_arm, insns, sizeof(insns))) {
 139                 panic("failed to write memory %p!\n", p->ainsn.insn_arm);
 140                 // Mr_Nobody: we have to panic, really??...
 141                 //free_insn_slot (&uprobe_insn_pages, task, p->ainsn.insn_arm, 0);
 142                 //return -EINVAL;
 143         }
 144
 145         return 0;
 146 }
 147
 148 static int arch_check_insn_thumb(struct arch_specific_insn *ainsn)
 149 {
 150         int ret = 0;
 151
 152         // check instructions that can change PC
 153         if (THUMB_INSN_MATCH(UNDEF, ainsn->insn_thumb[0]) ||
 154             THUMB_INSN_MATCH(SWI, ainsn->insn_thumb[0]) ||
 155             THUMB_INSN_MATCH(BREAK, ainsn->insn_thumb[0]) ||
 156             THUMB2_INSN_MATCH(BL, ainsn->insn_thumb[0]) ||
 157             THUMB_INSN_MATCH(B1, ainsn->insn_thumb[0]) ||
 158             THUMB_INSN_MATCH(B2, ainsn->insn_thumb[0]) ||
 159             THUMB_INSN_MATCH(CBZ, ainsn->insn_thumb[0]) ||
 160             THUMB2_INSN_MATCH(B1, ainsn->insn_thumb[0]) ||
 161             THUMB2_INSN_MATCH(B2, ainsn->insn_thumb[0]) ||
 162             THUMB2_INSN_MATCH(BLX1, ainsn->insn_thumb[0]) ||
 163             THUMB_INSN_MATCH(BLX2, ainsn->insn_thumb[0]) ||
 164             THUMB_INSN_MATCH(BX, ainsn->insn_thumb[0]) ||
 165             THUMB2_INSN_MATCH(BXJ, ainsn->insn_thumb[0]) ||
 166             (THUMB2_INSN_MATCH(ADR, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RD(ainsn->insn_thumb[0]) == 15) ||
 167             (THUMB2_INSN_MATCH(LDRW, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RT(ainsn->insn_thumb[0]) == 15) ||
 168             (THUMB2_INSN_MATCH(LDRW1, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RT(ainsn->insn_thumb[0]) == 15) ||
 169             (THUMB2_INSN_MATCH(LDRHW, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RT(ainsn->insn_thumb[0]) == 15) ||
 170             (THUMB2_INSN_MATCH(LDRHW1, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RT(ainsn->insn_thumb[0]) == 15) ||
 171             (THUMB2_INSN_MATCH(LDRWL, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RT(ainsn->insn_thumb[0]) == 15) ||
 172             THUMB2_INSN_MATCH(LDMIA, ainsn->insn_thumb[0]) ||
 173             THUMB2_INSN_MATCH(LDMDB, ainsn->insn_thumb[0]) ||
 174             (THUMB2_INSN_MATCH(DP, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RD(ainsn->insn_thumb[0]) == 15) ||
 175             (THUMB2_INSN_MATCH(RSBW, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RD(ainsn->insn_thumb[0]) == 15) ||
 176             (THUMB2_INSN_MATCH(RORW, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RD(ainsn->insn_thumb[0]) == 15) ||
 177             (THUMB2_INSN_MATCH(ROR, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RD(ainsn->insn_thumb[0]) == 15) ||
 178             (THUMB2_INSN_MATCH(LSLW1, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RD(ainsn->insn_thumb[0]) == 15) ||
 179             (THUMB2_INSN_MATCH(LSLW2, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RD(ainsn->insn_thumb[0]) == 15) ||
 180             (THUMB2_INSN_MATCH(LSRW1, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RD(ainsn->insn_thumb[0]) == 15) ||
 181             (THUMB2_INSN_MATCH(LSRW2, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RD(ainsn->insn_thumb[0]) == 15) ||
 182             /* skip PC, #-imm12 -> SP, #-imm8 and Tegra-hanging instructions */
 183             (THUMB2_INSN_MATCH(STRW1, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RN(ainsn->insn_thumb[0]) == 15) ||
 184             (THUMB2_INSN_MATCH(STRBW1, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RN(ainsn->insn_thumb[0]) == 15) ||
 185             (THUMB2_INSN_MATCH(STRHW1, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RN(ainsn->insn_thumb[0]) == 15) ||
 186             (THUMB2_INSN_MATCH(STRW, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RN(ainsn->insn_thumb[0]) == 15) ||
 187             (THUMB2_INSN_MATCH(STRHW, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RN(ainsn->insn_thumb[0]) == 15) ||
 188             (THUMB2_INSN_MATCH(LDRW, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RN(ainsn->insn_thumb[0]) == 15) ||
 189             (THUMB2_INSN_MATCH(LDRBW, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RN(ainsn->insn_thumb[0]) == 15) ||
 190             (THUMB2_INSN_MATCH(LDRHW, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RN(ainsn->insn_thumb[0]) == 15) ||
 191             /* skip STRDx/LDRDx Rt, Rt2, [Rd, ...] */
 192             (THUMB2_INSN_MATCH(LDRD, ainsn->insn_thumb[0]) || THUMB2_INSN_MATCH(LDRD1, ainsn->insn_thumb[0]) || THUMB2_INSN_MATCH(STRD, ainsn->insn_thumb[0])) ) {
 193                 DBPRINTF("Bad insn arch_check_insn_thumb: %lx\n", ainsn->insn_thumb[0]);
 194                 ret = -EFAULT;
 195         }
 196
 197         return ret;
 198 }
 199
 200 static int prep_pc_dep_insn_execbuf_thumb(kprobe_opcode_t * insns, kprobe_opcode_t insn, int uregs)
 201 {
 202         unsigned char mreg = 0;
 203         unsigned char reg = 0;
 204
 205         if (THUMB_INSN_MATCH(APC, insn) || THUMB_INSN_MATCH(LRO3, insn)) {
 206                 reg = ((insn & 0xffff) & uregs) >> 8;
 207         } else {
 208                 if (THUMB_INSN_MATCH(MOV3, insn)) {
 209                         if (((((unsigned char) insn) & 0xff) >> 3) == 15) {
 210                                 reg = (insn & 0xffff) & uregs;
 211                         } else {
 212                                 return 0;
 213                         }
 214                 } else {
 215                         if (THUMB2_INSN_MATCH(ADR, insn)) {
 216                                 reg = ((insn >> 16) & uregs) >> 8;
 217                                 if (reg == 15) {
 218                                         return 0;
 219                                 }
 220                         } else {
 221                                 if (THUMB2_INSN_MATCH(LDRW, insn) || THUMB2_INSN_MATCH(LDRW1, insn) ||
 222                                     THUMB2_INSN_MATCH(LDRHW, insn) || THUMB2_INSN_MATCH(LDRHW1, insn) ||
 223                                     THUMB2_INSN_MATCH(LDRWL, insn)) {
 224                                         reg = ((insn >> 16) & uregs) >> 12;
 225                                         if (reg == 15) {
 226                                                 return 0;
 227                                         }
 228                                 } else {
 229                                         // LDRB.W PC, [PC, #immed] => PLD [PC, #immed], so Rt == PC is skipped
 230                                         if (THUMB2_INSN_MATCH(LDRBW, insn) || THUMB2_INSN_MATCH(LDRBW1, insn) ||
 231                                             THUMB2_INSN_MATCH(LDREX, insn)) {
 232                                                 reg = ((insn >> 16) & uregs) >> 12;
 233                                         } else {
 234                                                 if (THUMB2_INSN_MATCH(DP, insn)) {
 235                                                         reg = ((insn >> 16) & uregs) >> 12;
 236                                                         if (reg == 15) {
 237                                                                 return 0;
 238                                                         }
 239                                                 } else {
 240                                                         if (THUMB2_INSN_MATCH(RSBW, insn)) {
 241                                                                 reg = ((insn >> 12) & uregs) >> 8;
 242                                                                 if (reg == 15){
 243                                                                         return 0;
 244                                                                 }
 245                                                         } else {
 246                                                                 if (THUMB2_INSN_MATCH(RORW, insn)) {
 247                                                                         reg = ((insn >> 12) & uregs) >> 8;
 248                                                                         if (reg == 15) {
 249                                                                                 return 0;
 250                                                                         }
 251                                                                 } else {
 252                                                                         if (THUMB2_INSN_MATCH(ROR, insn) || THUMB2_INSN_MATCH(LSLW1, insn) ||
 253                                                                             THUMB2_INSN_MATCH(LSLW2, insn) || THUMB2_INSN_MATCH(LSRW1, insn) ||
 254                                                                             THUMB2_INSN_MATCH(LSRW2, insn)) {
 255                                                                                 reg = ((insn >> 12) & uregs) >> 8;
 256                                                                                 if (reg == 15) {
 257                                                                                         return 0;
 258                                                                                 }
 259                                                                         } else {
 260                                                                                 if (THUMB2_INSN_MATCH(TEQ1, insn) || THUMB2_INSN_MATCH(TST1, insn)) {
 261                                                                                         reg = 15;
 262                                                                                 } else {
 263                                                                                         if (THUMB2_INSN_MATCH(TEQ2, insn) || THUMB2_INSN_MATCH(TST2, insn)) {
 264                                                                                                 reg = THUMB2_INSN_REG_RM(insn);
 265                                                                                         }
 266                                                                                 }
 267                                                                         }
 268                                                                 }
 269                                                         }
 270                                                 }
 271                                         }
 272                                 }
 273                         }
 274                 }
 275         }
 276
 277         if ((THUMB2_INSN_MATCH(STRW, insn) || THUMB2_INSN_MATCH(STRBW, insn) ||
 278              THUMB2_INSN_MATCH(STRD, insn) || THUMB2_INSN_MATCH(STRHT, insn) ||
 279              THUMB2_INSN_MATCH(STRT, insn) || THUMB2_INSN_MATCH(STRHW1, insn) ||
 280              THUMB2_INSN_MATCH(STRHW, insn)) && THUMB2_INSN_REG_RT(insn) == 15) {
 281                 reg = THUMB2_INSN_REG_RT(insn);
 282         }
 283
 284         if (reg == 6 || reg == 7) {
 285                 *((unsigned short*)insns + 0) = (*((unsigned short*)insns + 0) & 0x00ff) | ((1 << mreg) | (1 << (mreg + 1)));
 286                 *((unsigned short*)insns + 1) = (*((unsigned short*)insns + 1) & 0xf8ff) | (mreg << 8);
 287                 *((unsigned short*)insns + 2) = (*((unsigned short*)insns + 2) & 0xfff8) | (mreg + 1);
 288                 *((unsigned short*)insns + 3) = (*((unsigned short*)insns + 3) & 0xffc7) | (mreg << 3);
 289                 *((unsigned short*)insns + 7) = (*((unsigned short*)insns + 7) & 0xf8ff) | (mreg << 8);
 290                 *((unsigned short*)insns + 8) = (*((unsigned short*)insns + 8) & 0xffc7) | (mreg << 3);
 291                 *((unsigned short*)insns + 9) = (*((unsigned short*)insns + 9) & 0xffc7) | ((mreg + 1) << 3);
 292                 *((unsigned short*)insns + 10) = (*((unsigned short*)insns + 10) & 0x00ff) | (( 1 << mreg) | (1 << (mreg + 1)));
 293         }
 294
 295         if (THUMB_INSN_MATCH(APC, insn)) {
 296                 // ADD Rd, PC, #immed_8*4 -> ADD Rd, SP, #immed_8*4
 297                 *((unsigned short*)insns + 4) = ((insn & 0xffff) | 0x800);                              // ADD Rd, SP, #immed_8*4
 298         } else {
 299                 if (THUMB_INSN_MATCH(LRO3, insn)) {
 300                         // LDR Rd, [PC, #immed_8*4] -> LDR Rd, [SP, #immed_8*4]
 301                         *((unsigned short*)insns + 4) = ((insn & 0xffff) + 0x5000);                     // LDR Rd, [SP, #immed_8*4]
 302                 } else {
 303                         if (THUMB_INSN_MATCH(MOV3, insn)) {
 304                                 // MOV Rd, PC -> MOV Rd, SP
 305                                 *((unsigned short*)insns + 4) = ((insn & 0xffff) ^ 0x10);               // MOV Rd, SP
 306                         } else {
 307                                 if (THUMB2_INSN_MATCH(ADR, insn)) {
 308                                         // ADDW Rd, PC, #imm -> ADDW Rd, SP, #imm
 309                                         insns[2] = (insn & 0xfffffff0) | 0x0d;                          // ADDW Rd, SP, #imm
 310                                 } else {
 311                                         if (THUMB2_INSN_MATCH(LDRW, insn) || THUMB2_INSN_MATCH(LDRBW, insn) ||
 312                                             THUMB2_INSN_MATCH(LDRHW, insn)) {
 313                                                 // LDR.W Rt, [PC, #-<imm_12>] -> LDR.W Rt, [SP, #-<imm_8>]
 314                                                 // !!!!!!!!!!!!!!!!!!!!!!!!
 315                                                 // !!! imm_12 vs. imm_8 !!!
 316                                                 // !!!!!!!!!!!!!!!!!!!!!!!!
 317                                                 insns[2] = (insn & 0xf0fffff0) | 0x0c00000d;            // LDR.W Rt, [SP, #-<imm_8>]
 318                                         } else {
 319                                                 if (THUMB2_INSN_MATCH(LDRW1, insn) || THUMB2_INSN_MATCH(LDRBW1, insn) ||
 320                                                     THUMB2_INSN_MATCH(LDRHW1, insn) || THUMB2_INSN_MATCH(LDRD, insn) ||
 321                                                     THUMB2_INSN_MATCH(LDRD1, insn) || THUMB2_INSN_MATCH(LDREX, insn)) {
 322                                                         // LDRx.W Rt, [PC, #+<imm_12>] -> LDRx.W Rt, [SP, #+<imm_12>] (+/-imm_8 for LDRD Rt, Rt2, [PC, #<imm_8>]
 323                                                         insns[2] = (insn & 0xfffffff0) | 0xd;                                                                                                   // LDRx.W Rt, [SP, #+<imm_12>]
 324                                                 } else {
 325                                                         if (THUMB2_INSN_MATCH(MUL, insn)) {
 326                                                                 insns[2] = (insn & 0xfff0ffff) | 0x000d0000;                                                                                    // MUL Rd, Rn, SP
 327                                                         } else {
 328                                                                 if (THUMB2_INSN_MATCH(DP, insn)) {
 329                                                                         if (THUMB2_INSN_REG_RM(insn) == 15) {
 330                                                                                 insns[2] = (insn & 0xfff0ffff) | 0x000d0000;                                                                    // DP Rd, Rn, PC
 331                                                                         } else if (THUMB2_INSN_REG_RN(insn) == 15) {
 332                                                                                 insns[2] = (insn & 0xfffffff0) | 0xd;                                                                           // DP Rd, PC, Rm
 333                                                                         }
 334                                                                 } else {
 335                                                                         if (THUMB2_INSN_MATCH(LDRWL, insn)) {
 336                                                                                 // LDRx.W Rt, [PC, #<imm_12>] -> LDRx.W Rt, [SP, #+<imm_12>] (+/-imm_8 for LDRD Rt, Rt2, [PC, #<imm_8>]
 337                                                                                 insns[2] = (insn & 0xfffffff0) | 0xd;                                                                           // LDRx.W Rt, [SP, #+<imm_12>]
 338                                                                         } else {
 339                                                                                 if (THUMB2_INSN_MATCH(RSBW, insn)) {
 340                                                                                         insns[2] = (insn & 0xfffffff0) | 0xd;                                                                   // RSB{S}.W Rd, PC, #<const> -> RSB{S}.W Rd, SP, #<const>
 341                                                                                 } else {
 342                                                                                         if (THUMB2_INSN_MATCH(RORW, insn) || THUMB2_INSN_MATCH(LSLW1, insn) || THUMB2_INSN_MATCH(LSRW1, insn)) {
 343                                                                                                 if ((THUMB2_INSN_REG_RM(insn) == 15) && (THUMB2_INSN_REG_RN(insn) == 15)) {
 344                                                                                                         insns[2] = (insn & 0xfffdfffd);                                                         // ROR.W Rd, PC, PC
 345                                                                                                 } else if (THUMB2_INSN_REG_RM(insn) == 15) {
 346                                                                                                         insns[2] = (insn & 0xfff0ffff) | 0xd0000;                                               // ROR.W Rd, Rn, PC
 347                                                                                                 } else if (THUMB2_INSN_REG_RN(insn) == 15) {
 348                                                                                                         insns[2] = (insn & 0xfffffff0) | 0xd;                                                   // ROR.W Rd, PC, Rm
 349                                                                                                 }
 350                                                                                         } else {
 351                                                                                                 if (THUMB2_INSN_MATCH(ROR, insn) || THUMB2_INSN_MATCH(LSLW2, insn) || THUMB2_INSN_MATCH(LSRW2, insn)) {
 352                                                                                                         insns[2] = (insn & 0xfff0ffff) | 0xd0000;                                               // ROR{S} Rd, PC, #<const> -> ROR{S} Rd, SP, #<const>
 353                                                                                                 }
 354                                                                                         }
 355                                                                                 }
 356                                                                         }
 357                                                                 }
 358                                                         }
 359                                                 }
 360                                         }
 361                                 }
 362                         }
 363                 }
 364         }
 365
 366         if (THUMB2_INSN_MATCH(STRW, insn) || THUMB2_INSN_MATCH(STRBW, insn)) {
 367                 insns[2] = (insn & 0xfff0ffff) | 0x000d0000;                                                            // STRx.W Rt, [Rn, SP]
 368         } else {
 369                 if (THUMB2_INSN_MATCH(STRD, insn) || THUMB2_INSN_MATCH(STRHT, insn) ||
 370                     THUMB2_INSN_MATCH(STRT, insn) || THUMB2_INSN_MATCH(STRHW1, insn)) {
 371                         if (THUMB2_INSN_REG_RN(insn) == 15) {
 372                                 insns[2] = (insn & 0xfffffff0) | 0xd;                                                   // STRD/T/HT{.W} Rt, [SP, ...]
 373                         } else {
 374                                 insns[2] = insn;
 375                         }
 376                 } else {
 377                         if (THUMB2_INSN_MATCH(STRHW, insn) && (THUMB2_INSN_REG_RN(insn) == 15)) {
 378                                 if (THUMB2_INSN_REG_RN(insn) == 15) {
 379                                         insns[2] = (insn & 0xf0fffff0) | 0x0c00000d;                                    // STRH.W Rt, [SP, #-<imm_8>]
 380                                 } else {
 381                                         insns[2] = insn;
 382                                 }
 383                         }
 384                 }
 385         }
 386
 387         // STRx PC, xxx
 388         if ((reg == 15) && (THUMB2_INSN_MATCH(STRW, insn)   ||
 389                             THUMB2_INSN_MATCH(STRBW, insn)  ||
 390                             THUMB2_INSN_MATCH(STRD, insn)   ||
 391                             THUMB2_INSN_MATCH(STRHT, insn)  ||
 392                             THUMB2_INSN_MATCH(STRT, insn)   ||
 393                             THUMB2_INSN_MATCH(STRHW1, insn) ||
 394                             THUMB2_INSN_MATCH(STRHW, insn) )) {
 395                 insns[2] = (insns[2] & 0x0fffffff) | 0xd0000000;
 396         }
 397
 398         if (THUMB2_INSN_MATCH(TEQ1, insn) || THUMB2_INSN_MATCH(TST1, insn)) {
 399                 insns[2] = (insn & 0xfffffff0) | 0xd;                                                                   // TEQ SP, #<const>
 400         } else {
 401                 if (THUMB2_INSN_MATCH(TEQ2, insn) || THUMB2_INSN_MATCH(TST2, insn)) {
 402                         if ((THUMB2_INSN_REG_RN(insn) == 15) && (THUMB2_INSN_REG_RM(insn) == 15)) {
 403                                 insns[2] = (insn & 0xfffdfffd);                                                         // TEQ/TST PC, PC
 404                         } else if (THUMB2_INSN_REG_RM(insn) == 15) {
 405                                 insns[2] = (insn & 0xfff0ffff) | 0xd0000;                                               // TEQ/TST Rn, PC
 406                         } else if (THUMB2_INSN_REG_RN(insn) == 15) {
 407                                 insns[2] = (insn & 0xfffffff0) | 0xd;                                                   // TEQ/TST PC, Rm
 408                         }
 409                 }
 410         }
 411
 412         return 0;
 413 }
 414
 415 static int arch_copy_trampoline_thumb_uprobe(struct kprobe *p, struct task_struct *task, int atomic)
 416 {
 417         int uregs, pc_dep;
 418         unsigned int addr;
 419         kprobe_opcode_t insn[MAX_INSN_SIZE];
 420         struct arch_specific_insn ainsn;
 421         kprobe_opcode_t insns[UPROBES_TRAMP_LEN * 2];
 422
 423         p->safe_thumb = -1;
 424         if ((unsigned long)p->addr & 0x01) {
 425                 printk("Error in %s at %d: attempt to register kprobe at an unaligned address\n", __FILE__, __LINE__);
 426                 return -EINVAL;
 427         }
 428
 429         insn[0] = p->opcode;
 430         ainsn.insn_thumb = insn;
 431         if (!arch_check_insn_thumb(&ainsn)) {
 432                 p->safe_thumb = 0;
 433         }
 434
 435         uregs = 0;
 436         pc_dep = 0;
 437
 438         if (THUMB_INSN_MATCH(APC, insn[0]) || THUMB_INSN_MATCH(LRO3, insn[0])) {
 439                 uregs = 0x0700;         // 8-10
 440                 pc_dep = 1;
 441         } else if (THUMB_INSN_MATCH(MOV3, insn[0]) && (((((unsigned char)insn[0]) & 0xff) >> 3) == 15)) {
 442                 // MOV Rd, PC
 443                 uregs = 0x07;
 444                 pc_dep = 1;
 445         } else if THUMB2_INSN_MATCH(ADR, insn[0]) {
 446                 uregs = 0x0f00;         // Rd 8-11
 447                 pc_dep = 1;
 448         } else if (((THUMB2_INSN_MATCH(LDRW, insn[0]) || THUMB2_INSN_MATCH(LDRW1, insn[0]) ||
 449                      THUMB2_INSN_MATCH(LDRBW, insn[0]) || THUMB2_INSN_MATCH(LDRBW1, insn[0]) ||
 450                      THUMB2_INSN_MATCH(LDRHW, insn[0]) || THUMB2_INSN_MATCH(LDRHW1, insn[0]) ||
 451                      THUMB2_INSN_MATCH(LDRWL, insn[0])) && THUMB2_INSN_REG_RN(insn[0]) == 15) ||
 452                      THUMB2_INSN_MATCH(LDREX, insn[0]) ||
 453                      ((THUMB2_INSN_MATCH(STRW, insn[0]) || THUMB2_INSN_MATCH(STRBW, insn[0]) ||
 454                        THUMB2_INSN_MATCH(STRHW, insn[0]) || THUMB2_INSN_MATCH(STRHW1, insn[0])) &&
 455                       (THUMB2_INSN_REG_RN(insn[0]) == 15 || THUMB2_INSN_REG_RT(insn[0]) == 15)) ||
 456                      ((THUMB2_INSN_MATCH(STRT, insn[0]) || THUMB2_INSN_MATCH(STRHT, insn[0])) &&
 457                        (THUMB2_INSN_REG_RN(insn[0]) == 15 || THUMB2_INSN_REG_RT(insn[0]) == 15))) {
 458                 uregs = 0xf000;         // Rt 12-15
 459                 pc_dep = 1;
 460         } else if ((THUMB2_INSN_MATCH(LDRD, insn[0]) || THUMB2_INSN_MATCH(LDRD1, insn[0])) && (THUMB2_INSN_REG_RN(insn[0]) == 15)) {
 461                 uregs = 0xff00;         // Rt 12-15, Rt2 8-11
 462                 pc_dep = 1;
 463         } else if (THUMB2_INSN_MATCH(MUL, insn[0]) && THUMB2_INSN_REG_RM(insn[0]) == 15) {
 464                 uregs = 0xf;
 465                 pc_dep = 1;
 466         } else if (THUMB2_INSN_MATCH(DP, insn[0]) && (THUMB2_INSN_REG_RN(insn[0]) == 15 || THUMB2_INSN_REG_RM(insn[0]) == 15)) {
 467                 uregs = 0xf000;         // Rd 12-15
 468                 pc_dep = 1;
 469         } else if (THUMB2_INSN_MATCH(STRD, insn[0]) && ((THUMB2_INSN_REG_RN(insn[0]) == 15) || (THUMB2_INSN_REG_RT(insn[0]) == 15) || THUMB2_INSN_REG_RT2(insn[0]) == 15)) {
 470                 uregs = 0xff00;         // Rt 12-15, Rt2 8-11
 471                 pc_dep = 1;
 472         } else if (THUMB2_INSN_MATCH(RSBW, insn[0]) && THUMB2_INSN_REG_RN(insn[0]) == 15) {
 473                 uregs = 0x0f00;         // Rd 8-11
 474                 pc_dep = 1;
 475         } else if (THUMB2_INSN_MATCH (RORW, insn[0]) && (THUMB2_INSN_REG_RN(insn[0]) == 15 || THUMB2_INSN_REG_RM(insn[0]) == 15)) {
 476                 uregs = 0x0f00;
 477                 pc_dep = 1;
 478         } else if ((THUMB2_INSN_MATCH(ROR, insn[0]) || THUMB2_INSN_MATCH(LSLW2, insn[0]) || THUMB2_INSN_MATCH(LSRW2, insn[0])) && THUMB2_INSN_REG_RM(insn[0]) == 15) {
 479                 uregs = 0x0f00;         // Rd 8-11
 480                 pc_dep = 1;
 481         } else if ((THUMB2_INSN_MATCH(LSLW1, insn[0]) || THUMB2_INSN_MATCH(LSRW1, insn[0])) && (THUMB2_INSN_REG_RN(insn[0]) == 15 || THUMB2_INSN_REG_RM(insn[0]) == 15)) {
 482                 uregs = 0x0f00;         // Rd 8-11
 483                 pc_dep = 1;
 484         } else if ((THUMB2_INSN_MATCH(TEQ1, insn[0]) || THUMB2_INSN_MATCH(TST1, insn[0])) && THUMB2_INSN_REG_RN(insn[0]) == 15) {
 485                 uregs = 0xf0000;        //Rn 0-3 (16-19)
 486                 pc_dep = 1;
 487         } else if ((THUMB2_INSN_MATCH(TEQ2, insn[0]) || THUMB2_INSN_MATCH(TST2, insn[0])) &&
 488                    (THUMB2_INSN_REG_RN(insn[0]) == 15 || THUMB2_INSN_REG_RM(insn[0]) == 15)) {
 489                 uregs = 0xf0000;        //Rn 0-3 (16-19)
 490                 pc_dep = 1;
 491         }
 492
 493         if (unlikely(uregs && pc_dep)) {
 494                 memcpy(insns, pc_dep_insn_execbuf_thumb, 18 * 2);
 495                 if (prep_pc_dep_insn_execbuf_thumb(insns, insn[0], uregs) != 0) {
 496                         printk("Error in %s at %d: failed to prepare exec buffer for insn %lx!",
 497                                __FILE__, __LINE__, insn[0]);
 498                         p->safe_thumb = -1;
 499                         //free_insn_slot (&uprobe_insn_pages, task, p->ainsn.insn_thumb, 0);
 500                         //return -EINVAL;
 501                 }
 502
 503                 addr = ((unsigned int)p->addr) + 4;
 504                 *((unsigned short*)insns + 13) = 0xdeff;
 505                 *((unsigned short*)insns + 14) = addr & 0x0000ffff;
 506                 *((unsigned short*)insns + 15) = addr >> 16;
 507                 if (!is_thumb2(insn[0])) {
 508                         addr = ((unsigned int)p->addr) + 2;
 509                         *((unsigned short*)insns + 16) = (addr & 0x0000ffff) | 0x1;
 510                         *((unsigned short*)insns + 17) = addr >> 16;
 511                 } else {
 512                         addr = ((unsigned int)p->addr) + 4;
 513                         *((unsigned short*)insns + 16) = (addr & 0x0000ffff) | 0x1;
 514                         *((unsigned short*)insns + 17) = addr >> 16;
 515                 }
 516         } else {
 517                 memcpy(insns, gen_insn_execbuf_thumb, 18 * 2);
 518                 *((unsigned short*)insns + 13) = 0xdeff;
 519                 if (!is_thumb2(insn[0])) {
 520                         addr = ((unsigned int)p->addr) + 2;
 521                         *((unsigned short*)insns + 2) = insn[0];
 522                         *((unsigned short*)insns + 16) = (addr & 0x0000ffff) | 0x1;
 523                         *((unsigned short*)insns + 17) = addr >> 16;
 524                 } else {
 525                         addr = ((unsigned int)p->addr) + 4;
 526                         insns[1] = insn[0];
 527                         *((unsigned short*)insns + 16) = (addr & 0x0000ffff) | 0x1;
 528                         *((unsigned short*)insns + 17) = addr >> 16;
 529                 }
 530         }
 531
 532         if (!write_proc_vm_atomic (task, (unsigned long)p->ainsn.insn_thumb, insns, 18 * 2)) {
 533                 panic("failed to write memory %p!\n", p->ainsn.insn_thumb);
 534                 // Mr_Nobody: we have to panic, really??...
 535                 //free_insn_slot (&uprobe_insn_pages, task, p->ainsn.insn_thumb, 0);
 536                 //return -EINVAL;
 537         }
 538
 539         return 0;
 540 }
 541
 542 int arch_prepare_uprobe(struct kprobe *p, struct task_struct *task, int atomic)
 543 {
 544         int ret = 0;
 545         kprobe_opcode_t insn[MAX_INSN_SIZE];
 546
 547         if ((unsigned long)p->addr & 0x01) {
 548                 printk("Error in %s at %d: attempt to register kprobe at an unaligned address\n", __FILE__, __LINE__);
 549                 return -EINVAL;
 550         }
 551
 552         if (!read_proc_vm_atomic(task, (unsigned long)p->addr, &insn, MAX_INSN_SIZE * sizeof(kprobe_opcode_t))) {
 553                 panic("Failed to read memory task[tgid=%u, comm=%s] %p!\n", task->tgid, task->comm, p->addr);
 554         }
 555
 556         p->opcode = insn[0];
 557         p->ainsn.insn_arm = get_insn_slot(task, &uprobe_insn_pages, atomic);
 558         if (!p->ainsn.insn_arm) {
 559                 printk("Error in %s at %d: kprobe slot allocation error (arm)\n", __FILE__, __LINE__);
 560                 return -ENOMEM;
 561         }
 562
 563         ret = arch_copy_trampoline_arm_uprobe(p, task, 1);
 564         if (ret) {
 565                 free_insn_slot(&uprobe_insn_pages, task, p->ainsn.insn_arm);
 566                 return -EFAULT;
 567         }
 568
 569         p->ainsn.insn_thumb = get_insn_slot(task, &uprobe_insn_pages, atomic);
 570         if (!p->ainsn.insn_thumb) {
 571                 printk("Error in %s at %d: kprobe slot allocation error (thumb)\n", __FILE__, __LINE__);
 572                 return -ENOMEM;
 573         }
 574
 575         ret = arch_copy_trampoline_thumb_uprobe(p, task, 1);
 576         if (ret) {
 577                 free_insn_slot(&uprobe_insn_pages, task, p->ainsn.insn_arm);
 578                 free_insn_slot(&uprobe_insn_pages, task, p->ainsn.insn_thumb);
 579                 return -EFAULT;
 580         }
 581
 582         if ((p->safe_arm == -1) && (p->safe_thumb == -1)) {
 583                 printk("Error in %s at %d: failed arch_copy_trampoline_*_uprobe() (both) [tgid=%u, addr=%lx, data=%lx]\n",
 584                        __FILE__, __LINE__, task->tgid, (unsigned long)p->addr, (unsigned long)p->opcode);
 585                 if (!write_proc_vm_atomic(task, (unsigned long)p->addr, &p->opcode, sizeof(p->opcode))) {
 586                         panic("Failed to write memory %p!\n", p->addr);
 587                 }
 588
 589                 free_insn_slot(&uprobe_insn_pages, task, p->ainsn.insn_arm);
 590                 free_insn_slot(&uprobe_insn_pages, task, p->ainsn.insn_thumb);
 591
 592                 return -EFAULT;
 593         }
 594
 595         return ret;
 596 }
 597
 598 void arch_prepare_uretprobe_hl(struct kretprobe_instance *ri,
 599                                struct pt_regs *regs)
 600 {
 601         /* Set flag of current mode */
 602         ri->sp = (kprobe_opcode_t *)((long)ri->sp | !!thumb_mode(regs));
 603
 604         if (thumb_mode(regs)) {
 605                 regs->ARM_lr = (unsigned long)(ri->rp->kp.ainsn.insn) + 0x1b;
 606         } else {
 607                 regs->ARM_lr = (unsigned long)(ri->rp->kp.ainsn.insn + UPROBES_TRAMP_RET_BREAK_IDX);
 608         }
 609 }
 610
 611 int setjmp_upre_handler(struct kprobe *p, struct pt_regs *regs)
 612 {
 613         struct jprobe *jp = container_of(p, struct jprobe, kp);
 614         kprobe_pre_entry_handler_t pre_entry = (kprobe_pre_entry_handler_t)jp->pre_entry;
 615         entry_point_t entry = (entry_point_t)jp->entry;
 616
 617         if (pre_entry) {
 618                 p->ss_addr = (kprobe_opcode_t *)pre_entry(jp->priv_arg, regs);
 619         }
 620
 621         if (entry) {
 622                 entry(regs->ARM_r0, regs->ARM_r1, regs->ARM_r2,
 623                       regs->ARM_r3, regs->ARM_r4, regs->ARM_r5);
 624         } else {
 625                 dbi_arch_uprobe_return();
 626         }
 627
 628         prepare_singlestep(p, regs);
 629
 630         return 1;
 631 }
 632
 633 int trampoline_uprobe_handler(struct kprobe *p, struct pt_regs *regs)
 634 {
 635         struct kretprobe_instance *ri = NULL;
 636         struct hlist_head *head;
 637         struct hlist_node *node, *tmp;
 638         unsigned long flags, orig_ret_address = 0;
 639         unsigned long trampoline_address = 0;
 640
 641         if (thumb_mode(regs)) {
 642                 trampoline_address = (unsigned long)(p->ainsn.insn) + 0x1b;
 643         } else {
 644                 trampoline_address = (unsigned long)(p->ainsn.insn + UPROBES_TRAMP_RET_BREAK_IDX);
 645         }
 646
 647         spin_lock_irqsave(&kretprobe_lock, flags);
 648
 649         head = kretprobe_inst_table_head(current->mm);
 650
 651         /*
 652          * It is possible to have multiple instances associated with a given
 653          * task either because an multiple functions in the call path
 654          * have a return probe installed on them, and/or more then one
 655          * return probe was registered for a target function.
 656          *
 657          * We can handle this because:
 658          *     - instances are always inserted at the head of the list
 659          *     - when multiple return probes are registered for the same
 660          *       function, the first instance's ret_addr will point to the
 661          *       real return address, and all the rest will point to
 662          *       kretprobe_trampoline
 663          */
 664         hlist_for_each_entry_safe(ri, node, tmp, head, hlist) {
 665                 if (ri->task != current) {
 666                         /* another task is sharing our hash bucket */
 667                         continue;
 668                 }
 669
 670                 if (ri->rp && ri->rp->handler) {
 671                         ri->rp->handler(ri, regs, ri->rp->priv_arg);
 672                 }
 673
 674                 orig_ret_address = (unsigned long)ri->ret_addr;
 675                 recycle_rp_inst(ri);
 676
 677                 if (orig_ret_address != trampoline_address) {
 678                         /*
 679                          * This is the real return address. Any other
 680                          * instances associated with this task are for
 681                          * other calls deeper on the call stack
 682                          */
 683                         break;
 684                 }
 685         }
 686
 687         regs->ARM_pc = orig_ret_address;
 688         if (thumb_mode(regs) && !(regs->ARM_lr & 0x01)) {
 689                 regs->ARM_cpsr &= 0xFFFFFFDF;
 690         } else if (user_mode(regs) && (regs->ARM_lr & 0x01)) {
 691                 regs->ARM_cpsr |= 0x20;
 692         }
 693
 694         spin_unlock_irqrestore(&kretprobe_lock, flags);
 695
 696         /*
 697          * By returning a non-zero value, we are telling
 698          * kprobe_handler() that we don't want the post_handler
 699          * to run (and have re-enabled preemption)
 700          */
 701
 702         return 1;
 703 }
 704
 705 static int check_validity_insn(struct kprobe *p, struct pt_regs *regs, struct task_struct *task)
 706 {
 707         struct kprobe *kp;
 708
 709         if (unlikely(thumb_mode(regs))) {
 710                 if (p->safe_thumb != -1) {
 711                         p->ainsn.insn = p->ainsn.insn_thumb;
 712                         list_for_each_entry_rcu(kp, &p->list, list) {
 713                                 kp->ainsn.insn = p->ainsn.insn_thumb;
 714                         }
 715                 } else {
 716                         printk("Error in %s at %d: we are in thumb mode (!) and check instruction was fail \
 717                                 (%0lX instruction at %p address)!\n", __FILE__, __LINE__, p->opcode, p->addr);
 718                         // Test case when we do our actions on already running application
 719                         arch_disarm_uprobe(p, task);
 720                         return -1;
 721                 }
 722         } else {
 723                 if (p->safe_arm != -1) {
 724                         p->ainsn.insn = p->ainsn.insn_arm;
 725                         list_for_each_entry_rcu(kp, &p->list, list) {
 726                                 kp->ainsn.insn = p->ainsn.insn_arm;
 727                         }
 728                 } else {
 729                         printk("Error in %s at %d: we are in arm mode (!) and check instruction was fail \
 730                                 (%0lX instruction at %p address)!\n", __FILE__, __LINE__, p->opcode, p->addr);
 731                         // Test case when we do our actions on already running application
 732                         arch_disarm_uprobe(p, task);
 733                         return -1;
 734                 }
 735         }
 736
 737         return 0;
 738 }
 739
 740 static int uprobe_handler(struct pt_regs *regs)
 741 {
 742         int err_out = 0;
 743         char *msg_out = NULL;
 744         struct task_struct *task = current;
 745         pid_t tgid = task->tgid;
 746         kprobe_opcode_t *addr = (kprobe_opcode_t *)(regs->ARM_pc);
 747         struct kprobe *p = NULL;
 748         int ret = 0, retprobe = 0;
 749
 750 #ifdef SUPRESS_BUG_MESSAGES
 751         int swap_oops_in_progress;
 752         // oops_in_progress used to avoid BUG() messages that slow down kprobe_handler() execution
 753         swap_oops_in_progress = oops_in_progress;
 754         oops_in_progress = 1;
 755 #endif
 756
 757         p = get_uprobe(addr, tgid);
 758
 759         if (p && (check_validity_insn(p, regs, task) != 0)) {
 760                 goto no_uprobe_live;
 761         }
 762
 763         if (p == NULL) {
 764                 p = get_kprobe_by_insn_slot(addr, tgid, regs);
 765                 if (p == NULL) {
 766                         /* Not one of ours: let kernel handle it */
 767                         goto no_uprobe;
 768                 }
 769
 770                 retprobe = 1;
 771         }
 772
 773         /* restore opcode for thumb app */
 774         if (thumb_mode(regs)) {
 775                 if (!is_thumb2(p->opcode)) {
 776                         unsigned long tmp = p->opcode >> 16;
 777                         write_proc_vm_atomic(task, (unsigned long)((unsigned short*)p->addr + 1), &tmp, 2);
 778
 779                         // "2*sizeof(kprobe_opcode_t)" - strange. Should be "sizeof(kprobe_opcode_t)", need to test
 780                         flush_icache_range((unsigned int)p->addr, ((unsigned int)p->addr) + (2 * sizeof(kprobe_opcode_t)));
 781                 }
 782         }
 783
 784         if (retprobe) {
 785                 ret = trampoline_uprobe_handler(p, regs);
 786         } else if (p->pre_handler) {
 787                 ret = p->pre_handler(p, regs);
 788         }
 789
 790         if (ret) {
 791                 /* handler has already set things up, so skip ss setup */
 792                 err_out = 0;
 793                 goto out;
 794         }
 795
 796 no_uprobe:
 797         msg_out = "no_uprobe\n";
 798         err_out = 1;            // return with death
 799         goto out;
 800
 801 no_uprobe_live:
 802         msg_out = "no_uprobe live\n";
 803         err_out = 0;            // ok - life is life
 804         goto out;
 805
 806 out:
 807 #ifdef SUPRESS_BUG_MESSAGES
 808         oops_in_progress = swap_oops_in_progress;
 809 #endif
 810
 811         if(msg_out) {
 812                 printk(msg_out);
 813         }
 814
 815         return err_out;
 816 }
 817
 818 int uprobe_trap_handler(struct pt_regs *regs, unsigned int instr)
 819 {
 820         int ret;
 821         unsigned long flags;
 822         local_irq_save(flags);
 823
 824         preempt_disable();
 825         ret = uprobe_handler(regs);
 826         preempt_enable_no_resched();
 827
 828         local_irq_restore(flags);
 829         return ret;
 830 }
 831
 832 /* userspace probes hook (arm) */
 833 static struct undef_hook undef_hook_for_us_arm = {
 834         .instr_mask     = 0xffffffff,
 835         .instr_val      = BREAKPOINT_INSTRUCTION,
 836         .cpsr_mask      = MODE_MASK,
 837         .cpsr_val       = USR_MODE,
 838         .fn             = uprobe_trap_handler
 839 };
 840
 841 /* userspace probes hook (thumb) */
 842 static struct undef_hook undef_hook_for_us_thumb = {
 843         .instr_mask     = 0xffffffff,
 844         .instr_val      = BREAKPOINT_INSTRUCTION & 0x0000ffff,
 845         .cpsr_mask      = MODE_MASK,
 846         .cpsr_val       = USR_MODE,
 847         .fn             = uprobe_trap_handler
 848 };
 849
 850 int swap_arch_init_uprobes(void)
 851 {
 852         swap_register_undef_hook(&undef_hook_for_us_arm);
 853         swap_register_undef_hook(&undef_hook_for_us_thumb);
 854
 855         return 0;
 856 }
 857
 858 void swap_arch_exit_uprobes(void)
 859 {
 860         swap_unregister_undef_hook(&undef_hook_for_us_thumb);
 861         swap_unregister_undef_hook(&undef_hook_for_us_arm);
 862 }