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