uprobe/arch/asm-arm/swap_uprobes.c

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