uprobe/arch/asm-arm/swap_uprobes.c

   1 /*
   2  *  Dynamic Binary Instrumentation Module based on KProbes
   3  *  modules/uprobe/arch/asm-arm/swap_uprobes.h
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation; either version 2 of the License, or
   8  * (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program; if not, write to the Free Software
  17  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  18  *
  19  * Copyright (C) Samsung Electronics, 2006-2010
  20  *
  21  * 2008-2009    Alexey Gerenkov <a.gerenkov@samsung.com> User-Space
  22  *              Probes initial implementation; Support x86/ARM/MIPS for both user and kernel spaces.
  23  * 2010         Ekaterina Gorelkina <e.gorelkina@samsung.com>: redesign module for separating core and arch parts
  24  *
  25  */
  26
  27 #include <kprobe/dbi_kprobes.h>
  28 #include <kprobe/arch/asm/dbi_kprobes.h>
  29 #include <kprobe/arch/asm/trampoline_arm.h>
  30 #include <asm/traps.h>
  31 #include <uprobe/swap_uprobes.h>
  32 #include <uprobe/arch/asm/swap_uprobes.h>
  33 #include <kprobe/dbi_insn_slots.h>
  34 #include <kprobe/dbi_kprobes_deps.h>
  35 #include "trampoline_thumb.h"
  36
  37 // FIXME:
  38 #include <kprobe/dbi_kdebug.h>
  39
  40
  41 #define flush_insns(addr, size)                                 \
  42         flush_icache_range((unsigned long)(addr),               \
  43                            (unsigned long)(addr) + (size))
  44
  45 static inline long branch_t16_dest(kprobe_opcode_t insn, unsigned int insn_addr)
  46 {
  47         long offset = insn & 0x3ff;
  48         offset -= insn & 0x400;
  49         return (insn_addr + 4 + offset * 2);
  50 }
  51
  52 static inline long branch_cond_t16_dest(kprobe_opcode_t insn, unsigned int insn_addr)
  53 {
  54         long offset = insn & 0x7f;
  55         offset -= insn & 0x80;
  56         return (insn_addr + 4 + offset * 2);
  57 }
  58
  59 static inline long branch_t32_dest(kprobe_opcode_t insn, unsigned int insn_addr)
  60 {
  61         unsigned int poff = insn & 0x3ff;
  62         unsigned int offset = (insn & 0x07fe0000) >> 17;
  63
  64         poff -= (insn & 0x400);
  65
  66         if (insn & (1 << 12))
  67                 return ((insn_addr + 4 + (poff << 12) + offset * 4));
  68         else
  69         return ((insn_addr + 4 + (poff << 12) + offset * 4) & ~3);
  70 }
  71
  72 static inline long cbz_t16_dest(kprobe_opcode_t insn, unsigned int insn_addr)
  73 {
  74         unsigned int i = (insn & 0x200) >> 3;
  75         unsigned int offset = (insn & 0xf8) >> 2;
  76         return insn_addr + 4 + i + offset;
  77 }
  78
  79 /* is instruction Thumb2 and NOT a branch, etc... */
  80 static int is_thumb2(kprobe_opcode_t insn)
  81 {
  82         return ((insn & 0xf800) == 0xe800 ||
  83                 (insn & 0xf800) == 0xf000 ||
  84                 (insn & 0xf800) == 0xf800);
  85 }
  86
  87 static int arch_copy_trampoline_arm_uprobe(struct uprobe *up)
  88 {
  89         int ret;
  90         struct kprobe *p = up2kp(up);
  91         unsigned long insn = p->opcode;
  92         unsigned long vaddr = (unsigned long)p->addr;
  93         unsigned long *tramp = up->atramp.tramp_arm;
  94
  95         ret = arch_make_trampoline_arm(vaddr, insn, tramp);
  96         p->safe_arm = !!ret;
  97
  98         return ret;
  99 }
 100
 101 static int arch_check_insn_thumb(unsigned long insn)
 102 {
 103         int ret = 0;
 104
 105         /* check instructions that can change PC */
 106         if (THUMB_INSN_MATCH(UNDEF, insn) ||
 107             THUMB_INSN_MATCH(SWI, insn) ||
 108             THUMB_INSN_MATCH(BREAK, insn) ||
 109             THUMB2_INSN_MATCH(B1, insn) ||
 110             THUMB2_INSN_MATCH(B2, insn) ||
 111             THUMB2_INSN_MATCH(BXJ, insn) ||
 112             (THUMB2_INSN_MATCH(ADR, insn) && THUMB2_INSN_REG_RD(insn) == 15) ||
 113             (THUMB2_INSN_MATCH(LDRW, insn) && THUMB2_INSN_REG_RT(insn) == 15) ||
 114             (THUMB2_INSN_MATCH(LDRW1, insn) && THUMB2_INSN_REG_RT(insn) == 15) ||
 115             (THUMB2_INSN_MATCH(LDRHW, insn) && THUMB2_INSN_REG_RT(insn) == 15) ||
 116             (THUMB2_INSN_MATCH(LDRHW1, insn) && THUMB2_INSN_REG_RT(insn) == 15) ||
 117             (THUMB2_INSN_MATCH(LDRWL, insn) && THUMB2_INSN_REG_RT(insn) == 15) ||
 118             THUMB2_INSN_MATCH(LDMIA, insn) ||
 119             THUMB2_INSN_MATCH(LDMDB, insn) ||
 120             (THUMB2_INSN_MATCH(DP, insn) && THUMB2_INSN_REG_RD(insn) == 15) ||
 121             (THUMB2_INSN_MATCH(RSBW, insn) && THUMB2_INSN_REG_RD(insn) == 15) ||
 122             (THUMB2_INSN_MATCH(RORW, insn) && THUMB2_INSN_REG_RD(insn) == 15) ||
 123             (THUMB2_INSN_MATCH(ROR, insn) && THUMB2_INSN_REG_RD(insn) == 15) ||
 124             (THUMB2_INSN_MATCH(LSLW1, insn) && THUMB2_INSN_REG_RD(insn) == 15) ||
 125             (THUMB2_INSN_MATCH(LSLW2, insn) && THUMB2_INSN_REG_RD(insn) == 15) ||
 126             (THUMB2_INSN_MATCH(LSRW1, insn) && THUMB2_INSN_REG_RD(insn) == 15) ||
 127             (THUMB2_INSN_MATCH(LSRW2, insn) && THUMB2_INSN_REG_RD(insn) == 15) ||
 128             /* skip PC, #-imm12 -> SP, #-imm8 and Tegra-hanging instructions */
 129             (THUMB2_INSN_MATCH(STRW1, insn) && THUMB2_INSN_REG_RN(insn) == 15) ||
 130             (THUMB2_INSN_MATCH(STRBW1, insn) && THUMB2_INSN_REG_RN(insn) == 15) ||
 131             (THUMB2_INSN_MATCH(STRHW1, insn) && THUMB2_INSN_REG_RN(insn) == 15) ||
 132             (THUMB2_INSN_MATCH(STRW, insn) && THUMB2_INSN_REG_RN(insn) == 15) ||
 133             (THUMB2_INSN_MATCH(STRHW, insn) && THUMB2_INSN_REG_RN(insn) == 15) ||
 134             (THUMB2_INSN_MATCH(LDRW, insn) && THUMB2_INSN_REG_RN(insn) == 15) ||
 135             (THUMB2_INSN_MATCH(LDRBW, insn) && THUMB2_INSN_REG_RN(insn) == 15) ||
 136             (THUMB2_INSN_MATCH(LDRHW, insn) && THUMB2_INSN_REG_RN(insn) == 15) ||
 137             /* skip STRDx/LDRDx Rt, Rt2, [Rd, ...] */
 138             (THUMB2_INSN_MATCH(LDRD, insn) || THUMB2_INSN_MATCH(LDRD1, insn) || THUMB2_INSN_MATCH(STRD, insn))) {
 139                 printk("Bad insn arch_check_insn_thumb: %lx\n", insn);
 140                 ret = -EFAULT;
 141         }
 142
 143         return ret;
 144 }
 145
 146 static int prep_pc_dep_insn_execbuf_thumb(kprobe_opcode_t * insns, kprobe_opcode_t insn, int uregs)
 147 {
 148         unsigned char mreg = 0;
 149         unsigned char reg = 0;
 150
 151         if (THUMB_INSN_MATCH(APC, insn) || THUMB_INSN_MATCH(LRO3, insn)) {
 152                 reg = ((insn & 0xffff) & uregs) >> 8;
 153         } else {
 154                 if (THUMB_INSN_MATCH(MOV3, insn)) {
 155                         if (((((unsigned char) insn) & 0xff) >> 3) == 15) {
 156                                 reg = (insn & 0xffff) & uregs;
 157                         } else {
 158                                 return 0;
 159                         }
 160                 } else {
 161                         if (THUMB2_INSN_MATCH(ADR, insn)) {
 162                                 reg = ((insn >> 16) & uregs) >> 8;
 163                                 if (reg == 15) {
 164                                         return 0;
 165                                 }
 166                         } else {
 167                                 if (THUMB2_INSN_MATCH(LDRW, insn) || THUMB2_INSN_MATCH(LDRW1, insn) ||
 168                                     THUMB2_INSN_MATCH(LDRHW, insn) || THUMB2_INSN_MATCH(LDRHW1, insn) ||
 169                                     THUMB2_INSN_MATCH(LDRWL, insn)) {
 170                                         reg = ((insn >> 16) & uregs) >> 12;
 171                                         if (reg == 15) {
 172                                                 return 0;
 173                                         }
 174                                 } else {
 175                                         // LDRB.W PC, [PC, #immed] => PLD [PC, #immed], so Rt == PC is skipped
 176                                         if (THUMB2_INSN_MATCH(LDRBW, insn) || THUMB2_INSN_MATCH(LDRBW1, insn) ||
 177                                             THUMB2_INSN_MATCH(LDREX, insn)) {
 178                                                 reg = ((insn >> 16) & uregs) >> 12;
 179                                         } else {
 180                                                 if (THUMB2_INSN_MATCH(DP, insn)) {
 181                                                         reg = ((insn >> 16) & uregs) >> 12;
 182                                                         if (reg == 15) {
 183                                                                 return 0;
 184                                                         }
 185                                                 } else {
 186                                                         if (THUMB2_INSN_MATCH(RSBW, insn)) {
 187                                                                 reg = ((insn >> 12) & uregs) >> 8;
 188                                                                 if (reg == 15){
 189                                                                         return 0;
 190                                                                 }
 191                                                         } else {
 192                                                                 if (THUMB2_INSN_MATCH(RORW, insn)) {
 193                                                                         reg = ((insn >> 12) & uregs) >> 8;
 194                                                                         if (reg == 15) {
 195                                                                                 return 0;
 196                                                                         }
 197                                                                 } else {
 198                                                                         if (THUMB2_INSN_MATCH(ROR, insn) || THUMB2_INSN_MATCH(LSLW1, insn) ||
 199                                                                             THUMB2_INSN_MATCH(LSLW2, insn) || THUMB2_INSN_MATCH(LSRW1, insn) ||
 200                                                                             THUMB2_INSN_MATCH(LSRW2, insn)) {
 201                                                                                 reg = ((insn >> 12) & uregs) >> 8;
 202                                                                                 if (reg == 15) {
 203                                                                                         return 0;
 204                                                                                 }
 205                                                                         } else {
 206                                                                                 if (THUMB2_INSN_MATCH(TEQ1, insn) || THUMB2_INSN_MATCH(TST1, insn)) {
 207                                                                                         reg = 15;
 208                                                                                 } else {
 209                                                                                         if (THUMB2_INSN_MATCH(TEQ2, insn) || THUMB2_INSN_MATCH(TST2, insn)) {
 210                                                                                                 reg = THUMB2_INSN_REG_RM(insn);
 211                                                                                         }
 212                                                                                 }
 213                                                                         }
 214                                                                 }
 215                                                         }
 216                                                 }
 217                                         }
 218                                 }
 219                         }
 220                 }
 221         }
 222
 223         if ((THUMB2_INSN_MATCH(STRW, insn) || THUMB2_INSN_MATCH(STRBW, insn) ||
 224              THUMB2_INSN_MATCH(STRD, insn) || THUMB2_INSN_MATCH(STRHT, insn) ||
 225              THUMB2_INSN_MATCH(STRT, insn) || THUMB2_INSN_MATCH(STRHW1, insn) ||
 226              THUMB2_INSN_MATCH(STRHW, insn)) && THUMB2_INSN_REG_RT(insn) == 15) {
 227                 reg = THUMB2_INSN_REG_RT(insn);
 228         }
 229
 230         if (reg == 6 || reg == 7) {
 231                 *((unsigned short*)insns + 0) = (*((unsigned short*)insns + 0) & 0x00ff) | ((1 << mreg) | (1 << (mreg + 1)));
 232                 *((unsigned short*)insns + 1) = (*((unsigned short*)insns + 1) & 0xf8ff) | (mreg << 8);
 233                 *((unsigned short*)insns + 2) = (*((unsigned short*)insns + 2) & 0xfff8) | (mreg + 1);
 234                 *((unsigned short*)insns + 3) = (*((unsigned short*)insns + 3) & 0xffc7) | (mreg << 3);
 235                 *((unsigned short*)insns + 7) = (*((unsigned short*)insns + 7) & 0xf8ff) | (mreg << 8);
 236                 *((unsigned short*)insns + 8) = (*((unsigned short*)insns + 8) & 0xffc7) | (mreg << 3);
 237                 *((unsigned short*)insns + 9) = (*((unsigned short*)insns + 9) & 0xffc7) | ((mreg + 1) << 3);
 238                 *((unsigned short*)insns + 10) = (*((unsigned short*)insns + 10) & 0x00ff) | (( 1 << mreg) | (1 << (mreg + 1)));
 239         }
 240
 241         if (THUMB_INSN_MATCH(APC, insn)) {
 242                 // ADD Rd, PC, #immed_8*4 -> ADD Rd, SP, #immed_8*4
 243                 *((unsigned short*)insns + 4) = ((insn & 0xffff) | 0x800);                              // ADD Rd, SP, #immed_8*4
 244         } else {
 245                 if (THUMB_INSN_MATCH(LRO3, insn)) {
 246                         // LDR Rd, [PC, #immed_8*4] -> LDR Rd, [SP, #immed_8*4]
 247                         *((unsigned short*)insns + 4) = ((insn & 0xffff) + 0x5000);                     // LDR Rd, [SP, #immed_8*4]
 248                 } else {
 249                         if (THUMB_INSN_MATCH(MOV3, insn)) {
 250                                 // MOV Rd, PC -> MOV Rd, SP
 251                                 *((unsigned short*)insns + 4) = ((insn & 0xffff) ^ 0x10);               // MOV Rd, SP
 252                         } else {
 253                                 if (THUMB2_INSN_MATCH(ADR, insn)) {
 254                                         // ADDW Rd, PC, #imm -> ADDW Rd, SP, #imm
 255                                         insns[2] = (insn & 0xfffffff0) | 0x0d;                          // ADDW Rd, SP, #imm
 256                                 } else {
 257                                         if (THUMB2_INSN_MATCH(LDRW, insn) || THUMB2_INSN_MATCH(LDRBW, insn) ||
 258                                             THUMB2_INSN_MATCH(LDRHW, insn)) {
 259                                                 // LDR.W Rt, [PC, #-<imm_12>] -> LDR.W Rt, [SP, #-<imm_8>]
 260                                                 // !!!!!!!!!!!!!!!!!!!!!!!!
 261                                                 // !!! imm_12 vs. imm_8 !!!
 262                                                 // !!!!!!!!!!!!!!!!!!!!!!!!
 263                                                 insns[2] = (insn & 0xf0fffff0) | 0x0c00000d;            // LDR.W Rt, [SP, #-<imm_8>]
 264                                         } else {
 265                                                 if (THUMB2_INSN_MATCH(LDRW1, insn) || THUMB2_INSN_MATCH(LDRBW1, insn) ||
 266                                                     THUMB2_INSN_MATCH(LDRHW1, insn) || THUMB2_INSN_MATCH(LDRD, insn) ||
 267                                                     THUMB2_INSN_MATCH(LDRD1, insn) || THUMB2_INSN_MATCH(LDREX, insn)) {
 268                                                         // LDRx.W Rt, [PC, #+<imm_12>] -> LDRx.W Rt, [SP, #+<imm_12>] (+/-imm_8 for LDRD Rt, Rt2, [PC, #<imm_8>]
 269                                                         insns[2] = (insn & 0xfffffff0) | 0xd;                                                                                                   // LDRx.W Rt, [SP, #+<imm_12>]
 270                                                 } else {
 271                                                         if (THUMB2_INSN_MATCH(MUL, insn)) {
 272                                                                 insns[2] = (insn & 0xfff0ffff) | 0x000d0000;                                                                                    // MUL Rd, Rn, SP
 273                                                         } else {
 274                                                                 if (THUMB2_INSN_MATCH(DP, insn)) {
 275                                                                         if (THUMB2_INSN_REG_RM(insn) == 15) {
 276                                                                                 insns[2] = (insn & 0xfff0ffff) | 0x000d0000;                                                                    // DP Rd, Rn, PC
 277                                                                         } else if (THUMB2_INSN_REG_RN(insn) == 15) {
 278                                                                                 insns[2] = (insn & 0xfffffff0) | 0xd;                                                                           // DP Rd, PC, Rm
 279                                                                         }
 280                                                                 } else {
 281                                                                         if (THUMB2_INSN_MATCH(LDRWL, insn)) {
 282                                                                                 // LDRx.W Rt, [PC, #<imm_12>] -> LDRx.W Rt, [SP, #+<imm_12>] (+/-imm_8 for LDRD Rt, Rt2, [PC, #<imm_8>]
 283                                                                                 insns[2] = (insn & 0xfffffff0) | 0xd;                                                                           // LDRx.W Rt, [SP, #+<imm_12>]
 284                                                                         } else {
 285                                                                                 if (THUMB2_INSN_MATCH(RSBW, insn)) {
 286                                                                                         insns[2] = (insn & 0xfffffff0) | 0xd;                                                                   // RSB{S}.W Rd, PC, #<const> -> RSB{S}.W Rd, SP, #<const>
 287                                                                                 } else {
 288                                                                                         if (THUMB2_INSN_MATCH(RORW, insn) || THUMB2_INSN_MATCH(LSLW1, insn) || THUMB2_INSN_MATCH(LSRW1, insn)) {
 289                                                                                                 if ((THUMB2_INSN_REG_RM(insn) == 15) && (THUMB2_INSN_REG_RN(insn) == 15)) {
 290                                                                                                         insns[2] = (insn & 0xfffdfffd);                                                         // ROR.W Rd, PC, PC
 291                                                                                                 } else if (THUMB2_INSN_REG_RM(insn) == 15) {
 292                                                                                                         insns[2] = (insn & 0xfff0ffff) | 0xd0000;                                               // ROR.W Rd, Rn, PC
 293                                                                                                 } else if (THUMB2_INSN_REG_RN(insn) == 15) {
 294                                                                                                         insns[2] = (insn & 0xfffffff0) | 0xd;                                                   // ROR.W Rd, PC, Rm
 295                                                                                                 }
 296                                                                                         } else {
 297                                                                                                 if (THUMB2_INSN_MATCH(ROR, insn) || THUMB2_INSN_MATCH(LSLW2, insn) || THUMB2_INSN_MATCH(LSRW2, insn)) {
 298                                                                                                         insns[2] = (insn & 0xfff0ffff) | 0xd0000;                                               // ROR{S} Rd, PC, #<const> -> ROR{S} Rd, SP, #<const>
 299                                                                                                 }
 300                                                                                         }
 301                                                                                 }
 302                                                                         }
 303                                                                 }
 304                                                         }
 305                                                 }
 306                                         }
 307                                 }
 308                         }
 309                 }
 310         }
 311
 312         if (THUMB2_INSN_MATCH(STRW, insn) || THUMB2_INSN_MATCH(STRBW, insn)) {
 313                 insns[2] = (insn & 0xfff0ffff) | 0x000d0000;                                                            // STRx.W Rt, [Rn, SP]
 314         } else {
 315                 if (THUMB2_INSN_MATCH(STRD, insn) || THUMB2_INSN_MATCH(STRHT, insn) ||
 316                     THUMB2_INSN_MATCH(STRT, insn) || THUMB2_INSN_MATCH(STRHW1, insn)) {
 317                         if (THUMB2_INSN_REG_RN(insn) == 15) {
 318                                 insns[2] = (insn & 0xfffffff0) | 0xd;                                                   // STRD/T/HT{.W} Rt, [SP, ...]
 319                         } else {
 320                                 insns[2] = insn;
 321                         }
 322                 } else {
 323                         if (THUMB2_INSN_MATCH(STRHW, insn) && (THUMB2_INSN_REG_RN(insn) == 15)) {
 324                                 if (THUMB2_INSN_REG_RN(insn) == 15) {
 325                                         insns[2] = (insn & 0xf0fffff0) | 0x0c00000d;                                    // STRH.W Rt, [SP, #-<imm_8>]
 326                                 } else {
 327                                         insns[2] = insn;
 328                                 }
 329                         }
 330                 }
 331         }
 332
 333         // STRx PC, xxx
 334         if ((reg == 15) && (THUMB2_INSN_MATCH(STRW, insn)   ||
 335                             THUMB2_INSN_MATCH(STRBW, insn)  ||
 336                             THUMB2_INSN_MATCH(STRD, insn)   ||
 337                             THUMB2_INSN_MATCH(STRHT, insn)  ||
 338                             THUMB2_INSN_MATCH(STRT, insn)   ||
 339                             THUMB2_INSN_MATCH(STRHW1, insn) ||
 340                             THUMB2_INSN_MATCH(STRHW, insn) )) {
 341                 insns[2] = (insns[2] & 0x0fffffff) | 0xd0000000;
 342         }
 343
 344         if (THUMB2_INSN_MATCH(TEQ1, insn) || THUMB2_INSN_MATCH(TST1, insn)) {
 345                 insns[2] = (insn & 0xfffffff0) | 0xd;                                                                   // TEQ SP, #<const>
 346         } else {
 347                 if (THUMB2_INSN_MATCH(TEQ2, insn) || THUMB2_INSN_MATCH(TST2, insn)) {
 348                         if ((THUMB2_INSN_REG_RN(insn) == 15) && (THUMB2_INSN_REG_RM(insn) == 15)) {
 349                                 insns[2] = (insn & 0xfffdfffd);                                                         // TEQ/TST PC, PC
 350                         } else if (THUMB2_INSN_REG_RM(insn) == 15) {
 351                                 insns[2] = (insn & 0xfff0ffff) | 0xd0000;                                               // TEQ/TST Rn, PC
 352                         } else if (THUMB2_INSN_REG_RN(insn) == 15) {
 353                                 insns[2] = (insn & 0xfffffff0) | 0xd;                                                   // TEQ/TST PC, Rm
 354                         }
 355                 }
 356         }
 357
 358         return 0;
 359 }
 360
 361 static int arch_copy_trampoline_thumb_uprobe(struct uprobe *up)
 362 {
 363         int uregs, pc_dep;
 364         struct kprobe *p = up2kp(up);
 365         unsigned int addr;
 366         unsigned long vaddr = (unsigned long)p->addr;
 367         unsigned long insn = p->opcode;
 368         unsigned long *tramp = up->atramp.tramp_thumb;
 369         enum { tramp_len = sizeof(up->atramp.tramp_thumb) };
 370
 371         p->safe_thumb = 1;
 372         if (vaddr & 0x01) {
 373                 printk("Error in %s at %d: attempt to register kprobe at an unaligned address\n", __FILE__, __LINE__);
 374                 return -EINVAL;
 375         }
 376
 377         if (!arch_check_insn_thumb(insn)) {
 378                 p->safe_thumb = 0;
 379         }
 380
 381         uregs = 0;
 382         pc_dep = 0;
 383
 384         if (THUMB_INSN_MATCH(APC, insn) || THUMB_INSN_MATCH(LRO3, insn)) {
 385                 uregs = 0x0700;         /* 8-10 */
 386                 pc_dep = 1;
 387         } else if (THUMB_INSN_MATCH(MOV3, insn) && (((((unsigned char)insn) & 0xff) >> 3) == 15)) {
 388                 /* MOV Rd, PC */
 389                 uregs = 0x07;
 390                 pc_dep = 1;
 391         } else if THUMB2_INSN_MATCH(ADR, insn) {
 392                 uregs = 0x0f00;         /* Rd 8-11 */
 393                 pc_dep = 1;
 394         } else if (((THUMB2_INSN_MATCH(LDRW, insn) || THUMB2_INSN_MATCH(LDRW1, insn) ||
 395                      THUMB2_INSN_MATCH(LDRBW, insn) || THUMB2_INSN_MATCH(LDRBW1, insn) ||
 396                      THUMB2_INSN_MATCH(LDRHW, insn) || THUMB2_INSN_MATCH(LDRHW1, insn) ||
 397                      THUMB2_INSN_MATCH(LDRWL, insn)) && THUMB2_INSN_REG_RN(insn) == 15) ||
 398                      THUMB2_INSN_MATCH(LDREX, insn) ||
 399                      ((THUMB2_INSN_MATCH(STRW, insn) || THUMB2_INSN_MATCH(STRBW, insn) ||
 400                        THUMB2_INSN_MATCH(STRHW, insn) || THUMB2_INSN_MATCH(STRHW1, insn)) &&
 401                       (THUMB2_INSN_REG_RN(insn) == 15 || THUMB2_INSN_REG_RT(insn) == 15)) ||
 402                      ((THUMB2_INSN_MATCH(STRT, insn) || THUMB2_INSN_MATCH(STRHT, insn)) &&
 403                        (THUMB2_INSN_REG_RN(insn) == 15 || THUMB2_INSN_REG_RT(insn) == 15))) {
 404                 uregs = 0xf000;         /* Rt 12-15 */
 405                 pc_dep = 1;
 406         } else if ((THUMB2_INSN_MATCH(LDRD, insn) || THUMB2_INSN_MATCH(LDRD1, insn)) && (THUMB2_INSN_REG_RN(insn) == 15)) {
 407                 uregs = 0xff00;         /* Rt 12-15, Rt2 8-11 */
 408                 pc_dep = 1;
 409         } else if (THUMB2_INSN_MATCH(MUL, insn) && THUMB2_INSN_REG_RM(insn) == 15) {
 410                 uregs = 0xf;
 411                 pc_dep = 1;
 412         } else if (THUMB2_INSN_MATCH(DP, insn) && (THUMB2_INSN_REG_RN(insn) == 15 || THUMB2_INSN_REG_RM(insn) == 15)) {
 413                 uregs = 0xf000;         /* Rd 12-15 */
 414                 pc_dep = 1;
 415         } else if (THUMB2_INSN_MATCH(STRD, insn) && ((THUMB2_INSN_REG_RN(insn) == 15) || (THUMB2_INSN_REG_RT(insn) == 15) || THUMB2_INSN_REG_RT2(insn) == 15)) {
 416                 uregs = 0xff00;         /* Rt 12-15, Rt2 8-11 */
 417                 pc_dep = 1;
 418         } else if (THUMB2_INSN_MATCH(RSBW, insn) && THUMB2_INSN_REG_RN(insn) == 15) {
 419                 uregs = 0x0f00;         /* Rd 8-11 */
 420                 pc_dep = 1;
 421         } else if (THUMB2_INSN_MATCH (RORW, insn) && (THUMB2_INSN_REG_RN(insn) == 15 || THUMB2_INSN_REG_RM(insn) == 15)) {
 422                 uregs = 0x0f00;
 423                 pc_dep = 1;
 424         } else if ((THUMB2_INSN_MATCH(ROR, insn) || THUMB2_INSN_MATCH(LSLW2, insn) || THUMB2_INSN_MATCH(LSRW2, insn)) && THUMB2_INSN_REG_RM(insn) == 15) {
 425                 uregs = 0x0f00;         /* Rd 8-11 */
 426                 pc_dep = 1;
 427         } else if ((THUMB2_INSN_MATCH(LSLW1, insn) || THUMB2_INSN_MATCH(LSRW1, insn)) && (THUMB2_INSN_REG_RN(insn) == 15 || THUMB2_INSN_REG_RM(insn) == 15)) {
 428                 uregs = 0x0f00;         /* Rd 8-11 */
 429                 pc_dep = 1;
 430         } else if ((THUMB2_INSN_MATCH(TEQ1, insn) || THUMB2_INSN_MATCH(TST1, insn)) && THUMB2_INSN_REG_RN(insn) == 15) {
 431                 uregs = 0xf0000;        /* Rn 0-3 (16-19) */
 432                 pc_dep = 1;
 433         } else if ((THUMB2_INSN_MATCH(TEQ2, insn) || THUMB2_INSN_MATCH(TST2, insn)) &&
 434                    (THUMB2_INSN_REG_RN(insn) == 15 || THUMB2_INSN_REG_RM(insn) == 15)) {
 435                 uregs = 0xf0000;        /* Rn 0-3 (16-19) */
 436                 pc_dep = 1;
 437         }
 438
 439         if (unlikely(uregs && pc_dep)) {
 440                 memcpy(tramp, pc_dep_insn_execbuf_thumb, tramp_len);
 441                 if (prep_pc_dep_insn_execbuf_thumb(tramp, insn, uregs) != 0) {
 442                         printk("Error in %s at %d: failed to prepare exec buffer for insn %lx!",
 443                                __FILE__, __LINE__, insn);
 444                         p->safe_thumb = 1;
 445                 }
 446
 447                 addr = vaddr + 4;
 448                 *((unsigned short*)tramp + 13) = 0xdeff;
 449                 *((unsigned short*)tramp + 14) = addr & 0x0000ffff;
 450                 *((unsigned short*)tramp + 15) = addr >> 16;
 451                 if (!is_thumb2(insn)) {
 452                         addr = vaddr + 2;
 453                         *((unsigned short*)tramp + 16) = (addr & 0x0000ffff) | 0x1;
 454                         *((unsigned short*)tramp + 17) = addr >> 16;
 455                 } else {
 456                         addr = vaddr + 4;
 457                         *((unsigned short*)tramp + 16) = (addr & 0x0000ffff) | 0x1;
 458                         *((unsigned short*)tramp + 17) = addr >> 16;
 459                 }
 460         } else {
 461                 memcpy(tramp, gen_insn_execbuf_thumb, tramp_len);
 462                 *((unsigned short*)tramp + 13) = 0xdeff;
 463                 if (!is_thumb2(insn)) {
 464                         addr = vaddr + 2;
 465                         *((unsigned short*)tramp + 2) = insn;
 466                         *((unsigned short*)tramp + 16) = (addr & 0x0000ffff) | 0x1;
 467                         *((unsigned short*)tramp + 17) = addr >> 16;
 468                 } else {
 469                         addr = vaddr + 4;
 470                         tramp[1] = insn;
 471                         *((unsigned short*)tramp + 16) = (addr & 0x0000ffff) | 0x1;
 472                         *((unsigned short*)tramp + 17) = addr >> 16;
 473                 }
 474         }
 475
 476         if (THUMB_INSN_MATCH(B2, insn)) {
 477                 memcpy(tramp, b_off_insn_execbuf_thumb, tramp_len);
 478                 *((unsigned short*)tramp + 13) = 0xdeff;
 479                 addr = branch_t16_dest(insn, vaddr);
 480                 *((unsigned short*)tramp + 14) = (addr & 0x0000ffff) | 0x1;
 481                 *((unsigned short*)tramp + 15) = addr >> 16;
 482                 *((unsigned short*)tramp + 16) = 0;
 483                 *((unsigned short*)tramp + 17) = 0;
 484
 485         } else if (THUMB_INSN_MATCH(B1, insn)) {
 486                 memcpy(tramp, b_cond_insn_execbuf_thumb, tramp_len);
 487                 *((unsigned short*)tramp + 13) = 0xdeff;
 488                 *((unsigned short*)tramp + 0) |= (insn & 0xf00);
 489                 addr = branch_cond_t16_dest(insn, vaddr);
 490                 *((unsigned short*)tramp + 14) = (addr & 0x0000ffff) | 0x1;
 491                 *((unsigned short*)tramp + 15) = addr >> 16;
 492                 addr = vaddr + 2;
 493                 *((unsigned short*)tramp + 16) = (addr & 0x0000ffff) | 0x1;
 494                 *((unsigned short*)tramp + 17) = addr >> 16;
 495
 496         } else if (THUMB_INSN_MATCH(BLX2, insn) ||
 497                    THUMB_INSN_MATCH(BX, insn)) {
 498                 memcpy(tramp, b_r_insn_execbuf_thumb, tramp_len);
 499                 *((unsigned short*)tramp + 13) = 0xdeff;
 500                 *((unsigned short*)tramp + 4) = insn;
 501                 addr = vaddr + 2;
 502                 *((unsigned short*)tramp + 16) = (addr & 0x0000ffff) | 0x1;
 503                 *((unsigned short*)tramp + 17) = addr >> 16;
 504
 505         } else if (THUMB2_INSN_MATCH(BLX1, insn) ||
 506                    THUMB2_INSN_MATCH(BL, insn)) {
 507                 memcpy(tramp, blx_off_insn_execbuf_thumb, tramp_len);
 508                 *((unsigned short*)tramp + 13) = 0xdeff;
 509                 addr = branch_t32_dest(insn, vaddr);
 510                 *((unsigned short*)tramp + 14) = (addr & 0x0000ffff);
 511                 *((unsigned short*)tramp + 15) = addr >> 16;
 512                 addr = vaddr + 4;
 513                 *((unsigned short*)tramp + 16) = (addr & 0x0000ffff) | 0x1;
 514                 *((unsigned short*)tramp + 17) = addr >> 16;
 515
 516         } else if (THUMB_INSN_MATCH(CBZ, insn)) {
 517                 memcpy(tramp, cbz_insn_execbuf_thumb, tramp_len);
 518                 *((unsigned short*)tramp + 13) = 0xdeff;
 519                 /* zero out original branch displacement (imm5 = 0; i = 0) */
 520                 *((unsigned short*)tramp + 0) = insn & (~0x2f8);
 521                 /* replace it with 8 bytes offset in execbuf (imm5 = 0b00010) */
 522                 *((unsigned short*)tramp + 0) |= 0x20;
 523                 addr = cbz_t16_dest(insn, vaddr);
 524                 *((unsigned short*)tramp + 14) = (addr & 0x0000ffff) | 0x1;
 525                 *((unsigned short*)tramp + 15) = addr >> 16;
 526                 addr = vaddr + 2;
 527                 *((unsigned short*)tramp + 16) = (addr & 0x0000ffff) | 0x1;
 528                 *((unsigned short*)tramp + 17) = addr >> 16;
 529         }
 530
 531         return 0;
 532 }
 533
 534 int arch_prepare_uprobe(struct uprobe *up)
 535 {
 536         struct kprobe *p = up2kp(up);
 537         struct task_struct *task = up->task;
 538         unsigned long vaddr = (unsigned long)p->addr;
 539         unsigned long insn;
 540
 541         if (vaddr & 0x01) {
 542                 printk("Error in %s at %d: attempt to register uprobe "
 543                        "at an unaligned address\n", __FILE__, __LINE__);
 544                 return -EINVAL;
 545         }
 546
 547         if (!read_proc_vm_atomic(task, vaddr, &insn, sizeof(insn)))
 548                 panic("failed to read memory %lx!\n", vaddr);
 549
 550         p->opcode = insn;
 551
 552         arch_copy_trampoline_arm_uprobe(up);
 553         arch_copy_trampoline_thumb_uprobe(up);
 554
 555         if ((p->safe_arm) && (p->safe_thumb)) {
 556                 printk("Error in %s at %d: failed "
 557                        "arch_copy_trampoline_*_uprobe() (both) "
 558                        "[tgid=%u, addr=%lx, data=%lx]\n",
 559                        __FILE__, __LINE__, task->tgid, vaddr, insn);
 560
 561                 if (!write_proc_vm_atomic(task, vaddr, &insn, sizeof(insn)))
 562                         panic("Failed to write memory %p!\n", p->addr);
 563
 564                 return -EFAULT;
 565         }
 566
 567         up->atramp.utramp = alloc_insn_slot(up->sm);
 568         if (up->atramp.utramp == NULL) {
 569                 printk("Error: alloc_insn_slot failed (%08lx)\n", vaddr);
 570                 return -ENOMEM;
 571         }
 572
 573         return 0;
 574 }
 575
 576 void arch_opcode_analysis_uretprobe(struct uretprobe *rp)
 577 {
 578         /* Remove retprobe if first insn overwrites lr */
 579         rp->thumb_noret = !!(THUMB2_INSN_MATCH(BL, rp->up.kp.opcode) ||
 580                              THUMB2_INSN_MATCH(BLX1, rp->up.kp.opcode) ||
 581                              THUMB_INSN_MATCH(BLX2, rp->up.kp.opcode));
 582
 583         rp->arm_noret = !!(ARM_INSN_MATCH(BL, rp->up.kp.opcode) ||
 584                            ARM_INSN_MATCH(BLX1, rp->up.kp.opcode) ||
 585                            ARM_INSN_MATCH(BLX2, rp->up.kp.opcode));
 586 }
 587
 588 void arch_prepare_uretprobe(struct uretprobe_instance *ri,
 589                             struct pt_regs *regs)
 590 {
 591         ri->ret_addr = (kprobe_opcode_t *)regs->ARM_lr;
 592         ri->sp = (kprobe_opcode_t *)regs->ARM_sp;
 593
 594         /* Set flag of current mode */
 595         ri->sp = (kprobe_opcode_t *)((long)ri->sp | !!thumb_mode(regs));
 596
 597         if (thumb_mode(regs)) {
 598                 regs->ARM_lr = (unsigned long)(ri->rp->up.kp.ainsn.insn) + 0x1b;
 599         } else {
 600                 regs->ARM_lr = (unsigned long)(ri->rp->up.kp.ainsn.insn + UPROBES_TRAMP_RET_BREAK_IDX);
 601         }
 602 }
 603
 604 int arch_disarm_urp_inst(struct uretprobe_instance *ri,
 605                          struct task_struct *task)
 606 {
 607         unsigned long *tramp;
 608         unsigned long *sp = (unsigned long *)((long)ri->sp & ~1);
 609         unsigned long *stack = sp - RETPROBE_STACK_DEPTH + 1;
 610         unsigned long *found = NULL;
 611         unsigned long *buf[RETPROBE_STACK_DEPTH];
 612         int i, retval;
 613
 614         /* Understand function mode */
 615         if ((long)ri->sp & 1) {
 616                 tramp = (unsigned long *)
 617                         ((unsigned long)ri->rp->up.kp.ainsn.insn + 0x1b);
 618         } else {
 619                 tramp = (unsigned long *)(ri->rp->up.kp.ainsn.insn +
 620                                           UPROBES_TRAMP_RET_BREAK_IDX);
 621         }
 622
 623         retval = read_proc_vm_atomic(task, (unsigned long)stack,
 624                                      buf, sizeof(buf));
 625         if (retval != sizeof(buf)) {
 626                 printk("---> %s (%d/%d): failed to read stack from %08lx\n",
 627                        task->comm, task->tgid, task->pid,
 628                        (unsigned long)stack);
 629                 retval = -EFAULT;
 630                 goto out;
 631         }
 632
 633         /* search the stack from the bottom */
 634         for (i = RETPROBE_STACK_DEPTH - 1; i >= 0; i--) {
 635                 if (buf[i] == tramp) {
 636                         found = stack + i;
 637                         break;
 638                 }
 639         }
 640
 641         if (found) {
 642                 printk("---> %s (%d/%d): trampoline found at "
 643                        "%08lx (%08lx /%+d) - %p\n",
 644                        task->comm, task->tgid, task->pid,
 645                        (unsigned long)found, (unsigned long)sp,
 646                        found - sp, ri->rp->up.kp.addr);
 647                 retval = write_proc_vm_atomic(task, (unsigned long)found,
 648                                               &ri->ret_addr,
 649                                               sizeof(ri->ret_addr));
 650                 if (retval != sizeof(ri->ret_addr)) {
 651                         printk("---> %s (%d/%d): failed to write value "
 652                                "to %08lx",
 653                                task->comm, task->tgid, task->pid, (unsigned long)found);
 654                         retval = -EFAULT;
 655                 } else {
 656                         retval = 0;
 657                 }
 658         } else {
 659                 struct pt_regs *uregs = task_pt_regs(ri->task);
 660                 unsigned long ra = dbi_get_ret_addr(uregs);
 661                 if (ra == (unsigned long)tramp) {
 662                         printk("---> %s (%d/%d): trampoline found at "
 663                                "lr = %08lx - %p\n",
 664                                task->comm, task->tgid, task->pid,
 665                                ra, ri->rp->up.kp.addr);
 666                         dbi_set_ret_addr(uregs, (unsigned long)ri->ret_addr);
 667                         retval = 0;
 668                 } else {
 669                         printk("---> %s (%d/%d): trampoline NOT found at "
 670                                "sp = %08lx, lr = %08lx - %p\n",
 671                                task->comm, task->tgid, task->pid,
 672                                (unsigned long)sp, ra, ri->rp->up.kp.addr);
 673                         retval = -ENOENT;
 674                 }
 675         }
 676
 677 out:
 678         return retval;
 679 }
 680
 681 int setjmp_upre_handler(struct kprobe *p, struct pt_regs *regs)
 682 {
 683         struct uprobe *up = container_of(p, struct uprobe, kp);
 684         struct ujprobe *jp = container_of(up, struct ujprobe, up);
 685
 686         kprobe_pre_entry_handler_t pre_entry = (kprobe_pre_entry_handler_t)jp->pre_entry;
 687         entry_point_t entry = (entry_point_t)jp->entry;
 688
 689         if (pre_entry) {
 690                 p->ss_addr = (kprobe_opcode_t *)pre_entry(jp->priv_arg, regs);
 691         }
 692
 693         if (entry) {
 694                 entry(regs->ARM_r0, regs->ARM_r1, regs->ARM_r2,
 695                       regs->ARM_r3, regs->ARM_r4, regs->ARM_r5);
 696         } else {
 697                 arch_ujprobe_return();
 698         }
 699
 700         return 0;
 701 }
 702
 703 unsigned long arch_get_trampoline_addr(struct kprobe *p, struct pt_regs *regs)
 704 {
 705         return thumb_mode(regs) ?
 706                         (unsigned long)(p->ainsn.insn) + 0x1b :
 707                         (unsigned long)(p->ainsn.insn + UPROBES_TRAMP_RET_BREAK_IDX);
 708 }
 709
 710 void arch_set_orig_ret_addr(unsigned long orig_ret_addr, struct pt_regs *regs)
 711 {
 712         regs->ARM_lr = orig_ret_addr;
 713         regs->ARM_pc = orig_ret_addr & ~0x1;
 714
 715         if (regs->ARM_lr & 0x1)
 716                 regs->ARM_cpsr |= PSR_T_BIT;
 717         else
 718                 regs->ARM_cpsr &= ~PSR_T_BIT;
 719 }
 720
 721 static void restore_opcode_for_thumb(struct kprobe *p, struct pt_regs *regs)
 722 {
 723         if (thumb_mode(regs) && !is_thumb2(p->opcode)) {
 724                 u16 tmp = p->opcode >> 16;
 725                 write_proc_vm_atomic(current,
 726                                 (unsigned long)((u16*)p->addr + 1), &tmp, 2);
 727                 flush_insns(p->addr, 4);
 728         }
 729 }
 730
 731 static int make_trampoline(struct uprobe *up, struct pt_regs *regs)
 732 {
 733         unsigned long *tramp, *utramp;
 734         struct kprobe *p = up2kp(up);
 735         int sw;
 736
 737         /*
 738          * 0 bit - thumb mode           (0 - arm, 1 - thumb)
 739          * 1 bit - arm mode support     (0 - off, 1  on)
 740          * 2 bit - thumb mode support   (0 - off, 1  on)`
 741          */
 742         sw = (!!thumb_mode(regs)) |
 743              (int)!p->safe_arm << 1 |
 744              (int)!p->safe_thumb << 2;
 745
 746         switch (sw) {
 747         /* ARM */
 748         case 0b110:
 749         case 0b010:
 750                 tramp = up->atramp.tramp_arm;
 751                 break;
 752         /* THUMB */
 753         case 0b111:
 754         case 0b101:
 755                 restore_opcode_for_thumb(p, regs);
 756                 tramp = up->atramp.tramp_thumb;
 757                 break;
 758         default:
 759                 printk("Error in %s at %d: we are in arm mode "
 760                        "(!) and check instruction was fail "
 761                        "(%0lX instruction at %p address)!\n",
 762                        __FILE__, __LINE__, p->opcode, p->addr);
 763
 764                 disarm_uprobe(p, up->task);
 765
 766                 return 1;
 767         }
 768
 769         utramp = up->atramp.utramp;
 770
 771         if (!write_proc_vm_atomic(up->task, (unsigned long)utramp, tramp,
 772                                   UPROBES_TRAMP_LEN * sizeof(*tramp)))
 773                 panic("failed to write memory %p!\n", utramp);
 774
 775         p->ainsn.insn = utramp;
 776
 777         return 0;
 778 }
 779
 780 static int uprobe_handler(struct pt_regs *regs)
 781 {
 782         kprobe_opcode_t *addr = (kprobe_opcode_t *)(regs->ARM_pc);
 783         struct task_struct *task = current;
 784         pid_t tgid = task->tgid;
 785         struct kprobe *p;
 786
 787         p = get_ukprobe(addr, tgid);
 788         if (p == NULL) {
 789                 unsigned long offset_bp = thumb_mode(regs) ?
 790                                           0x1a :
 791                                           4 * UPROBES_TRAMP_RET_BREAK_IDX;
 792                 void *tramp_addr = (void *)addr - offset_bp;
 793
 794                 p = get_ukprobe_by_insn_slot(tramp_addr, tgid, regs);
 795                 if (p == NULL) {
 796                         printk("no_uprobe: Not one of ours: let "
 797                                "kernel handle it %p\n", addr);
 798                         return 1;
 799                 }
 800
 801                 trampoline_uprobe_handler(p, regs);
 802         } else {
 803                 if (p->ainsn.insn == NULL) {
 804                         struct uprobe *up = kp2up(p);
 805
 806                         if (make_trampoline(up, regs)) {
 807                                 printk("no_uprobe live\n");
 808                                 return 0;
 809                         }
 810
 811                         /* for uretprobe */
 812                         add_uprobe_table(p);
 813                 }
 814
 815                 if (!p->pre_handler || !p->pre_handler(p, regs)) {
 816                         prepare_singlestep(p, regs);
 817                 }
 818         }
 819
 820         return 0;
 821 }
 822
 823 int uprobe_trap_handler(struct pt_regs *regs, unsigned int instr)
 824 {
 825         int ret;
 826         unsigned long flags;
 827         local_irq_save(flags);
 828
 829         preempt_disable();
 830         ret = uprobe_handler(regs);
 831         preempt_enable_no_resched();
 832
 833         local_irq_restore(flags);
 834         return ret;
 835 }
 836
 837 /* userspace probes hook (arm) */
 838 static struct undef_hook undef_hook_for_us_arm = {
 839         .instr_mask     = 0xffffffff,
 840         .instr_val      = BREAKPOINT_INSTRUCTION,
 841         .cpsr_mask      = MODE_MASK,
 842         .cpsr_val       = USR_MODE,
 843         .fn             = uprobe_trap_handler
 844 };
 845
 846 /* userspace probes hook (thumb) */
 847 static struct undef_hook undef_hook_for_us_thumb = {
 848         .instr_mask     = 0xffffffff,
 849         .instr_val      = BREAKPOINT_INSTRUCTION & 0x0000ffff,
 850         .cpsr_mask      = MODE_MASK,
 851         .cpsr_val       = USR_MODE,
 852         .fn             = uprobe_trap_handler
 853 };
 854
 855 int swap_arch_init_uprobes(void)
 856 {
 857         swap_register_undef_hook(&undef_hook_for_us_arm);
 858         swap_register_undef_hook(&undef_hook_for_us_thumb);
 859
 860         return 0;
 861 }
 862
 863 void swap_arch_exit_uprobes(void)
 864 {
 865         swap_unregister_undef_hook(&undef_hook_for_us_thumb);
 866         swap_unregister_undef_hook(&undef_hook_for_us_arm);
 867 }