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