arch/arm64/kernel/probes/simulate-insn.c

   1 /*
   2  * arch/arm64/kernel/probes/simulate-insn.c
   3  *
   4  * Copyright (C) 2013 Linaro Limited.
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License version 2 as
   8  * published by the Free Software Foundation.
   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 GNU
  13  * General Public License for more details.
  14  */
  15
  16 #include <linux/kernel.h>
  17 #include <linux/kprobes.h>
  18
  19 #include "simulate-insn.h"
  20
  21 #define sign_extend(x, signbit)         \
  22         ((x) | (0 - ((x) & (1 << (signbit)))))
  23
  24 #define bbl_displacement(insn)          \
  25         sign_extend(((insn) & 0x3ffffff) << 2, 27)
  26
  27 #define bcond_displacement(insn)        \
  28         sign_extend(((insn >> 5) & 0x7ffff) << 2, 20)
  29
  30 #define cbz_displacement(insn)  \
  31         sign_extend(((insn >> 5) & 0x7ffff) << 2, 20)
  32
  33 #define tbz_displacement(insn)  \
  34         sign_extend(((insn >> 5) & 0x3fff) << 2, 15)
  35
  36 #define ldr_displacement(insn)  \
  37         sign_extend(((insn >> 5) & 0x7ffff) << 2, 20)
  38
  39 static inline void set_x_reg(struct pt_regs *regs, int reg, u64 val)
  40 {
  41         if (reg < 31)
  42                 regs->regs[reg] = val;
  43 }
  44
  45 static inline void set_w_reg(struct pt_regs *regs, int reg, u64 val)
  46 {
  47         if (reg < 31)
  48                 regs->regs[reg] = lower_32_bits(val);
  49 }
  50
  51 static inline u64 get_x_reg(struct pt_regs *regs, int reg)
  52 {
  53         if (reg < 31)
  54                 return regs->regs[reg];
  55         else
  56                 return 0;
  57 }
  58
  59 static inline u32 get_w_reg(struct pt_regs *regs, int reg)
  60 {
  61         if (reg < 31)
  62                 return lower_32_bits(regs->regs[reg]);
  63         else
  64                 return 0;
  65 }
  66
  67 static bool __kprobes check_cbz(u32 opcode, struct pt_regs *regs)
  68 {
  69         int xn = opcode & 0x1f;
  70
  71         return (opcode & (1 << 31)) ?
  72             (get_x_reg(regs, xn) == 0) : (get_w_reg(regs, xn) == 0);
  73 }
  74
  75 static bool __kprobes check_cbnz(u32 opcode, struct pt_regs *regs)
  76 {
  77         int xn = opcode & 0x1f;
  78
  79         return (opcode & (1 << 31)) ?
  80             (get_x_reg(regs, xn) != 0) : (get_w_reg(regs, xn) != 0);
  81 }
  82
  83 static bool __kprobes check_tbz(u32 opcode, struct pt_regs *regs)
  84 {
  85         int xn = opcode & 0x1f;
  86         int bit_pos = ((opcode & (1 << 31)) >> 26) | ((opcode >> 19) & 0x1f);
  87
  88         return ((get_x_reg(regs, xn) >> bit_pos) & 0x1) == 0;
  89 }
  90
  91 static bool __kprobes check_tbnz(u32 opcode, struct pt_regs *regs)
  92 {
  93         int xn = opcode & 0x1f;
  94         int bit_pos = ((opcode & (1 << 31)) >> 26) | ((opcode >> 19) & 0x1f);
  95
  96         return ((get_x_reg(regs, xn) >> bit_pos) & 0x1) != 0;
  97 }
  98
  99 /*
 100  * instruction simulation functions
 101  */
 102 void __kprobes
 103 simulate_adr_adrp(u32 opcode, long addr, struct pt_regs *regs)
 104 {
 105         long imm, xn, val;
 106
 107         xn = opcode & 0x1f;
 108         imm = ((opcode >> 3) & 0x1ffffc) | ((opcode >> 29) & 0x3);
 109         imm = sign_extend(imm, 20);
 110         if (opcode & 0x80000000)
 111                 val = (imm<<12) + (addr & 0xfffffffffffff000);
 112         else
 113                 val = imm + addr;
 114
 115         set_x_reg(regs, xn, val);
 116
 117         instruction_pointer_set(regs, instruction_pointer(regs) + 4);
 118 }
 119
 120 void __kprobes
 121 simulate_b_bl(u32 opcode, long addr, struct pt_regs *regs)
 122 {
 123         int disp = bbl_displacement(opcode);
 124
 125         /* Link register is x30 */
 126         if (opcode & (1 << 31))
 127                 set_x_reg(regs, 30, addr + 4);
 128
 129         instruction_pointer_set(regs, addr + disp);
 130 }
 131
 132 void __kprobes
 133 simulate_b_cond(u32 opcode, long addr, struct pt_regs *regs)
 134 {
 135         int disp = 4;
 136
 137         if (aarch32_opcode_cond_checks[opcode & 0xf](regs->pstate & 0xffffffff))
 138                 disp = bcond_displacement(opcode);
 139
 140         instruction_pointer_set(regs, addr + disp);
 141 }
 142
 143 void __kprobes
 144 simulate_br_blr_ret(u32 opcode, long addr, struct pt_regs *regs)
 145 {
 146         int xn = (opcode >> 5) & 0x1f;
 147
 148         /* update pc first in case we're doing a "blr lr" */
 149         instruction_pointer_set(regs, get_x_reg(regs, xn));
 150
 151         /* Link register is x30 */
 152         if (((opcode >> 21) & 0x3) == 1)
 153                 set_x_reg(regs, 30, addr + 4);
 154 }
 155
 156 void __kprobes
 157 simulate_cbz_cbnz(u32 opcode, long addr, struct pt_regs *regs)
 158 {
 159         int disp = 4;
 160
 161         if (opcode & (1 << 24)) {
 162                 if (check_cbnz(opcode, regs))
 163                         disp = cbz_displacement(opcode);
 164         } else {
 165                 if (check_cbz(opcode, regs))
 166                         disp = cbz_displacement(opcode);
 167         }
 168         instruction_pointer_set(regs, addr + disp);
 169 }
 170
 171 void __kprobes
 172 simulate_tbz_tbnz(u32 opcode, long addr, struct pt_regs *regs)
 173 {
 174         int disp = 4;
 175
 176         if (opcode & (1 << 24)) {
 177                 if (check_tbnz(opcode, regs))
 178                         disp = tbz_displacement(opcode);
 179         } else {
 180                 if (check_tbz(opcode, regs))
 181                         disp = tbz_displacement(opcode);
 182         }
 183         instruction_pointer_set(regs, addr + disp);
 184 }
 185
 186 void __kprobes
 187 simulate_ldr_literal(u32 opcode, long addr, struct pt_regs *regs)
 188 {
 189         u64 *load_addr;
 190         int xn = opcode & 0x1f;
 191         int disp;
 192
 193         disp = ldr_displacement(opcode);
 194         load_addr = (u64 *) (addr + disp);
 195
 196         if (opcode & (1 << 30)) /* x0-x30 */
 197                 set_x_reg(regs, xn, *load_addr);
 198         else                    /* w0-w30 */
 199                 set_w_reg(regs, xn, *load_addr);
 200
 201         instruction_pointer_set(regs, instruction_pointer(regs) + 4);
 202 }
 203
 204 void __kprobes
 205 simulate_ldrsw_literal(u32 opcode, long addr, struct pt_regs *regs)
 206 {
 207         s32 *load_addr;
 208         int xn = opcode & 0x1f;
 209         int disp;
 210
 211         disp = ldr_displacement(opcode);
 212         load_addr = (s32 *) (addr + disp);
 213
 214         set_x_reg(regs, xn, *load_addr);
 215
 216         instruction_pointer_set(regs, instruction_pointer(regs) + 4);
 217 }