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