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

   1 // SPDX-License-Identifier: GPL-2.0+
   2
   3 #include <linux/bitops.h>
   4 #include <linux/kernel.h>
   5 #include <linux/kprobes.h>
   6
   7 #include "decode-insn.h"
   8 #include "simulate-insn.h"
   9
  10 static inline bool csky_insn_reg_get_val(struct pt_regs *regs,
  11                                          unsigned long index,
  12                                          unsigned long *ptr)
  13 {
  14         if (index < 14)
  15                 *ptr = *(&regs->a0 + index);
  16
  17         if (index > 15 && index < 31)
  18                 *ptr = *(&regs->exregs[0] + index - 16);
  19
  20         switch (index) {
  21         case 14:
  22                 *ptr = regs->usp;
  23                 break;
  24         case 15:
  25                 *ptr = regs->lr;
  26                 break;
  27         case 31:
  28                 *ptr = regs->tls;
  29                 break;
  30         default:
  31                 goto fail;
  32         }
  33
  34         return true;
  35 fail:
  36         return false;
  37 }
  38
  39 static inline bool csky_insn_reg_set_val(struct pt_regs *regs,
  40                                          unsigned long index,
  41                                          unsigned long val)
  42 {
  43         if (index < 14)
  44                 *(&regs->a0 + index) = val;
  45
  46         if (index > 15 && index < 31)
  47                 *(&regs->exregs[0] + index - 16) = val;
  48
  49         switch (index) {
  50         case 14:
  51                 regs->usp = val;
  52                 break;
  53         case 15:
  54                 regs->lr = val;
  55                 break;
  56         case 31:
  57                 regs->tls = val;
  58                 break;
  59         default:
  60                 goto fail;
  61         }
  62
  63         return true;
  64 fail:
  65         return false;
  66 }
  67
  68 void __kprobes
  69 simulate_br16(u32 opcode, long addr, struct pt_regs *regs)
  70 {
  71         instruction_pointer_set(regs,
  72                 addr + sign_extend32((opcode & 0x3ff) << 1, 9));
  73 }
  74
  75 void __kprobes
  76 simulate_br32(u32 opcode, long addr, struct pt_regs *regs)
  77 {
  78         instruction_pointer_set(regs,
  79                 addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
  80 }
  81
  82 void __kprobes
  83 simulate_bt16(u32 opcode, long addr, struct pt_regs *regs)
  84 {
  85         if (regs->sr & 1)
  86                 instruction_pointer_set(regs,
  87                         addr + sign_extend32((opcode & 0x3ff) << 1, 9));
  88         else
  89                 instruction_pointer_set(regs, addr + 2);
  90 }
  91
  92 void __kprobes
  93 simulate_bt32(u32 opcode, long addr, struct pt_regs *regs)
  94 {
  95         if (regs->sr & 1)
  96                 instruction_pointer_set(regs,
  97                         addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
  98         else
  99                 instruction_pointer_set(regs, addr + 4);
 100 }
 101
 102 void __kprobes
 103 simulate_bf16(u32 opcode, long addr, struct pt_regs *regs)
 104 {
 105         if (!(regs->sr & 1))
 106                 instruction_pointer_set(regs,
 107                         addr + sign_extend32((opcode & 0x3ff) << 1, 9));
 108         else
 109                 instruction_pointer_set(regs, addr + 2);
 110 }
 111
 112 void __kprobes
 113 simulate_bf32(u32 opcode, long addr, struct pt_regs *regs)
 114 {
 115         if (!(regs->sr & 1))
 116                 instruction_pointer_set(regs,
 117                         addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
 118         else
 119                 instruction_pointer_set(regs, addr + 4);
 120 }
 121
 122 void __kprobes
 123 simulate_jmp16(u32 opcode, long addr, struct pt_regs *regs)
 124 {
 125         unsigned long tmp = (opcode >> 2) & 0xf;
 126
 127         csky_insn_reg_get_val(regs, tmp, &tmp);
 128
 129         instruction_pointer_set(regs, tmp & 0xfffffffe);
 130 }
 131
 132 void __kprobes
 133 simulate_jmp32(u32 opcode, long addr, struct pt_regs *regs)
 134 {
 135         unsigned long tmp = opcode & 0x1f;
 136
 137         csky_insn_reg_get_val(regs, tmp, &tmp);
 138
 139         instruction_pointer_set(regs, tmp & 0xfffffffe);
 140 }
 141
 142 void __kprobes
 143 simulate_jsr16(u32 opcode, long addr, struct pt_regs *regs)
 144 {
 145         unsigned long tmp = (opcode >> 2) & 0xf;
 146
 147         csky_insn_reg_get_val(regs, tmp, &tmp);
 148
 149         regs->lr = addr + 2;
 150
 151         instruction_pointer_set(regs, tmp & 0xfffffffe);
 152 }
 153
 154 void __kprobes
 155 simulate_jsr32(u32 opcode, long addr, struct pt_regs *regs)
 156 {
 157         unsigned long tmp = opcode & 0x1f;
 158
 159         csky_insn_reg_get_val(regs, tmp, &tmp);
 160
 161         regs->lr = addr + 4;
 162
 163         instruction_pointer_set(regs, tmp & 0xfffffffe);
 164 }
 165
 166 void __kprobes
 167 simulate_lrw16(u32 opcode, long addr, struct pt_regs *regs)
 168 {
 169         unsigned long val;
 170         unsigned long tmp = (opcode & 0x300) >> 3;
 171         unsigned long offset = ((opcode & 0x1f) | tmp) << 2;
 172
 173         tmp = (opcode & 0xe0) >> 5;
 174
 175         val = *(unsigned int *)(instruction_pointer(regs) + offset);
 176
 177         csky_insn_reg_set_val(regs, tmp, val);
 178 }
 179
 180 void __kprobes
 181 simulate_lrw32(u32 opcode, long addr, struct pt_regs *regs)
 182 {
 183         unsigned long val;
 184         unsigned long offset = (opcode & 0xffff0000) >> 14;
 185         unsigned long tmp = opcode & 0x0000001f;
 186
 187         val = *(unsigned int *)
 188                 ((instruction_pointer(regs) + offset) & 0xfffffffc);
 189
 190         csky_insn_reg_set_val(regs, tmp, val);
 191 }
 192
 193 void __kprobes
 194 simulate_pop16(u32 opcode, long addr, struct pt_regs *regs)
 195 {
 196         unsigned long *tmp = (unsigned long *)regs->usp;
 197         int i;
 198
 199         for (i = 0; i < (opcode & 0xf); i++) {
 200                 csky_insn_reg_set_val(regs, i + 4, *tmp);
 201                 tmp += 1;
 202         }
 203
 204         if (opcode & 0x10) {
 205                 csky_insn_reg_set_val(regs, 15, *tmp);
 206                 tmp += 1;
 207         }
 208
 209         regs->usp = (unsigned long)tmp;
 210
 211         instruction_pointer_set(regs, regs->lr);
 212 }
 213
 214 void __kprobes
 215 simulate_pop32(u32 opcode, long addr, struct pt_regs *regs)
 216 {
 217         unsigned long *tmp = (unsigned long *)regs->usp;
 218         int i;
 219
 220         for (i = 0; i < ((opcode & 0xf0000) >> 16); i++) {
 221                 csky_insn_reg_set_val(regs, i + 4, *tmp);
 222                 tmp += 1;
 223         }
 224
 225         if (opcode & 0x100000) {
 226                 csky_insn_reg_set_val(regs, 15, *tmp);
 227                 tmp += 1;
 228         }
 229
 230         for (i = 0; i < ((opcode & 0xe00000) >> 21); i++) {
 231                 csky_insn_reg_set_val(regs, i + 16, *tmp);
 232                 tmp += 1;
 233         }
 234
 235         if (opcode & 0x1000000) {
 236                 csky_insn_reg_set_val(regs, 29, *tmp);
 237                 tmp += 1;
 238         }
 239
 240         regs->usp = (unsigned long)tmp;
 241
 242         instruction_pointer_set(regs, regs->lr);
 243 }
 244
 245 void __kprobes
 246 simulate_bez32(u32 opcode, long addr, struct pt_regs *regs)
 247 {
 248         unsigned long tmp = opcode & 0x1f;
 249
 250         csky_insn_reg_get_val(regs, tmp, &tmp);
 251
 252         if (tmp == 0) {
 253                 instruction_pointer_set(regs,
 254                         addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
 255         } else
 256                 instruction_pointer_set(regs, addr + 4);
 257 }
 258
 259 void __kprobes
 260 simulate_bnez32(u32 opcode, long addr, struct pt_regs *regs)
 261 {
 262         unsigned long tmp = opcode & 0x1f;
 263
 264         csky_insn_reg_get_val(regs, tmp, &tmp);
 265
 266         if (tmp != 0) {
 267                 instruction_pointer_set(regs,
 268                         addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
 269         } else
 270                 instruction_pointer_set(regs, addr + 4);
 271 }
 272
 273 void __kprobes
 274 simulate_bnezad32(u32 opcode, long addr, struct pt_regs *regs)
 275 {
 276         unsigned long tmp = opcode & 0x1f;
 277         long val;
 278
 279         csky_insn_reg_get_val(regs, tmp, (unsigned long *)&val);
 280
 281         val -= 1;
 282
 283         if (val > 0) {
 284                 instruction_pointer_set(regs,
 285                         addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
 286         } else
 287                 instruction_pointer_set(regs, addr + 4);
 288
 289         csky_insn_reg_set_val(regs, tmp, (unsigned long)val);
 290 }
 291
 292 void __kprobes
 293 simulate_bhsz32(u32 opcode, long addr, struct pt_regs *regs)
 294 {
 295         unsigned long tmp = opcode & 0x1f;
 296         unsigned long val;
 297
 298         csky_insn_reg_get_val(regs, tmp, &val);
 299
 300         if ((long) val >= 0) {
 301                 instruction_pointer_set(regs,
 302                         addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
 303         } else
 304                 instruction_pointer_set(regs, addr + 4);
 305 }
 306
 307 void __kprobes
 308 simulate_bhz32(u32 opcode, long addr, struct pt_regs *regs)
 309 {
 310         unsigned long tmp = opcode & 0x1f;
 311         unsigned long val;
 312
 313         csky_insn_reg_get_val(regs, tmp, &val);
 314
 315         if ((long) val > 0) {
 316                 instruction_pointer_set(regs,
 317                         addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
 318         } else
 319                 instruction_pointer_set(regs, addr + 4);
 320 }
 321
 322 void __kprobes
 323 simulate_blsz32(u32 opcode, long addr, struct pt_regs *regs)
 324 {
 325         unsigned long tmp = opcode & 0x1f;
 326         unsigned long val;
 327
 328         csky_insn_reg_get_val(regs, tmp, &val);
 329
 330         if ((long) val <= 0) {
 331                 instruction_pointer_set(regs,
 332                         addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
 333         } else
 334                 instruction_pointer_set(regs, addr + 4);
 335 }
 336
 337 void __kprobes
 338 simulate_blz32(u32 opcode, long addr, struct pt_regs *regs)
 339 {
 340         unsigned long tmp = opcode & 0x1f;
 341         unsigned long val;
 342
 343         csky_insn_reg_get_val(regs, tmp, &val);
 344
 345         if ((long) val < 0) {
 346                 instruction_pointer_set(regs,
 347                         addr + sign_extend32((opcode & 0xffff0000) >> 15, 15));
 348         } else
 349                 instruction_pointer_set(regs, addr + 4);
 350 }
 351
 352 void __kprobes
 353 simulate_bsr32(u32 opcode, long addr, struct pt_regs *regs)
 354 {
 355         unsigned long tmp;
 356
 357         tmp = (opcode & 0xffff) << 16;
 358         tmp |= (opcode & 0xffff0000) >> 16;
 359
 360         instruction_pointer_set(regs,
 361                 addr + sign_extend32((tmp & 0x3ffffff) << 1, 15));
 362
 363         regs->lr = addr + 4;
 364 }
 365
 366 void __kprobes
 367 simulate_jmpi32(u32 opcode, long addr, struct pt_regs *regs)
 368 {
 369         unsigned long val;
 370         unsigned long offset = ((opcode & 0xffff0000) >> 14);
 371
 372         val = *(unsigned int *)
 373                 ((instruction_pointer(regs) + offset) & 0xfffffffc);
 374
 375         instruction_pointer_set(regs, val);
 376 }
 377
 378 void __kprobes
 379 simulate_jsri32(u32 opcode, long addr, struct pt_regs *regs)
 380 {
 381         unsigned long val;
 382         unsigned long offset = ((opcode & 0xffff0000) >> 14);
 383
 384         val = *(unsigned int *)
 385                 ((instruction_pointer(regs) + offset) & 0xfffffffc);
 386
 387         regs->lr = addr + 4;
 388
 389         instruction_pointer_set(regs, val);
 390 }