arch/arm/probes/kprobes/actions-common.c

   1 /*
   2  * arch/arm/probes/kprobes/actions-common.c
   3  *
   4  * Copyright (C) 2011 Jon Medhurst <tixy@yxit.co.uk>.
   5  *
   6  * Some contents moved here from arch/arm/include/asm/kprobes-arm.c which is
   7  * Copyright (C) 2006, 2007 Motorola Inc.
   8  *
   9  * This program is free software; you can redistribute it and/or modify
  10  * it under the terms of the GNU General Public License version 2 as
  11  * published by the Free Software Foundation.
  12  */
  13
  14 #include <linux/kernel.h>
  15 #include <linux/kprobes.h>
  16 #include <asm/opcodes.h>
  17
  18 #include "core.h"
  19
  20
  21 static void __kprobes simulate_ldm1stm1(probes_opcode_t insn,
  22                 struct arch_probes_insn *asi,
  23                 struct pt_regs *regs)
  24 {
  25         int rn = (insn >> 16) & 0xf;
  26         int lbit = insn & (1 << 20);
  27         int wbit = insn & (1 << 21);
  28         int ubit = insn & (1 << 23);
  29         int pbit = insn & (1 << 24);
  30         long *addr = (long *)regs->uregs[rn];
  31         int reg_bit_vector;
  32         int reg_count;
  33
  34         reg_count = 0;
  35         reg_bit_vector = insn & 0xffff;
  36         while (reg_bit_vector) {
  37                 reg_bit_vector &= (reg_bit_vector - 1);
  38                 ++reg_count;
  39         }
  40
  41         if (!ubit)
  42                 addr -= reg_count;
  43         addr += (!pbit == !ubit);
  44
  45         reg_bit_vector = insn & 0xffff;
  46         while (reg_bit_vector) {
  47                 int reg = __ffs(reg_bit_vector);
  48                 reg_bit_vector &= (reg_bit_vector - 1);
  49                 if (lbit)
  50                         regs->uregs[reg] = *addr++;
  51                 else
  52                         *addr++ = regs->uregs[reg];
  53         }
  54
  55         if (wbit) {
  56                 if (!ubit)
  57                         addr -= reg_count;
  58                 addr -= (!pbit == !ubit);
  59                 regs->uregs[rn] = (long)addr;
  60         }
  61 }
  62
  63 static void __kprobes simulate_stm1_pc(probes_opcode_t insn,
  64         struct arch_probes_insn *asi,
  65         struct pt_regs *regs)
  66 {
  67         unsigned long addr = regs->ARM_pc - 4;
  68
  69         regs->ARM_pc = (long)addr + str_pc_offset;
  70         simulate_ldm1stm1(insn, asi, regs);
  71         regs->ARM_pc = (long)addr + 4;
  72 }
  73
  74 static void __kprobes simulate_ldm1_pc(probes_opcode_t insn,
  75         struct arch_probes_insn *asi,
  76         struct pt_regs *regs)
  77 {
  78         simulate_ldm1stm1(insn, asi, regs);
  79         load_write_pc(regs->ARM_pc, regs);
  80 }
  81
  82 static void __kprobes
  83 emulate_generic_r0_12_noflags(probes_opcode_t insn,
  84         struct arch_probes_insn *asi, struct pt_regs *regs)
  85 {
  86         register void *rregs asm("r1") = regs;
  87         register void *rfn asm("lr") = asi->insn_fn;
  88
  89         __asm__ __volatile__ (
  90                 "stmdb  sp!, {%[regs], r11}     \n\t"
  91                 "ldmia  %[regs], {r0-r12}       \n\t"
  92 #if __LINUX_ARM_ARCH__ >= 6
  93                 "blx    %[fn]                   \n\t"
  94 #else
  95                 "str    %[fn], [sp, #-4]!       \n\t"
  96                 "adr    lr, 1f                  \n\t"
  97                 "ldr    pc, [sp], #4            \n\t"
  98                 "1:                             \n\t"
  99 #endif
 100                 "ldr    lr, [sp], #4            \n\t" /* lr = regs */
 101                 "stmia  lr, {r0-r12}            \n\t"
 102                 "ldr    r11, [sp], #4           \n\t"
 103                 : [regs] "=r" (rregs), [fn] "=r" (rfn)
 104                 : "0" (rregs), "1" (rfn)
 105                 : "r0", "r2", "r3", "r4", "r5", "r6", "r7",
 106                   "r8", "r9", "r10", "r12", "memory", "cc"
 107                 );
 108 }
 109
 110 static void __kprobes
 111 emulate_generic_r2_14_noflags(probes_opcode_t insn,
 112         struct arch_probes_insn *asi, struct pt_regs *regs)
 113 {
 114         emulate_generic_r0_12_noflags(insn, asi,
 115                 (struct pt_regs *)(regs->uregs+2));
 116 }
 117
 118 static void __kprobes
 119 emulate_ldm_r3_15(probes_opcode_t insn,
 120         struct arch_probes_insn *asi, struct pt_regs *regs)
 121 {
 122         emulate_generic_r0_12_noflags(insn, asi,
 123                 (struct pt_regs *)(regs->uregs+3));
 124         load_write_pc(regs->ARM_pc, regs);
 125 }
 126
 127 enum probes_insn __kprobes
 128 kprobe_decode_ldmstm(probes_opcode_t insn, struct arch_probes_insn *asi,
 129                 const struct decode_header *h)
 130 {
 131         probes_insn_handler_t *handler = 0;
 132         unsigned reglist = insn & 0xffff;
 133         int is_ldm = insn & 0x100000;
 134         int rn = (insn >> 16) & 0xf;
 135
 136         if (rn <= 12 && (reglist & 0xe000) == 0) {
 137                 /* Instruction only uses registers in the range R0..R12 */
 138                 handler = emulate_generic_r0_12_noflags;
 139
 140         } else if (rn >= 2 && (reglist & 0x8003) == 0) {
 141                 /* Instruction only uses registers in the range R2..R14 */
 142                 rn -= 2;
 143                 reglist >>= 2;
 144                 handler = emulate_generic_r2_14_noflags;
 145
 146         } else if (rn >= 3 && (reglist & 0x0007) == 0) {
 147                 /* Instruction only uses registers in the range R3..R15 */
 148                 if (is_ldm && (reglist & 0x8000)) {
 149                         rn -= 3;
 150                         reglist >>= 3;
 151                         handler = emulate_ldm_r3_15;
 152                 }
 153         }
 154
 155         if (handler) {
 156                 /* We can emulate the instruction in (possibly) modified form */
 157                 asi->insn[0] = __opcode_to_mem_arm((insn & 0xfff00000) |
 158                                                    (rn << 16) | reglist);
 159                 asi->insn_handler = handler;
 160                 return INSN_GOOD;
 161         }
 162
 163         /* Fallback to slower simulation... */
 164         if (reglist & 0x8000)
 165                 handler = is_ldm ? simulate_ldm1_pc : simulate_stm1_pc;
 166         else
 167                 handler = simulate_ldm1stm1;
 168         asi->insn_handler = handler;
 169         return INSN_GOOD_NO_SLOT;
 170 }
 171