arch/arm64/kernel/insn.c

   1 /*
   2  * Copyright (C) 2013 Huawei Ltd.
   3  * Author: Jiang Liu <[email protected]>
   4  *
   5  * Copyright (C) 2014-2016 Zi Shen Lim <[email protected]>
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License version 2 as
   9  * published by the Free Software Foundation.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  18  */
  19 #include <linux/bitops.h>
  20 #include <linux/bug.h>
  21 #include <linux/compiler.h>
  22 #include <linux/kernel.h>
  23 #include <linux/mm.h>
  24 #include <linux/smp.h>
  25 #include <linux/spinlock.h>
  26 #include <linux/stop_machine.h>
  27 #include <linux/types.h>
  28 #include <linux/uaccess.h>
  29
  30 #include <asm/cacheflush.h>
  31 #include <asm/debug-monitors.h>
  32 #include <asm/fixmap.h>
  33 #include <asm/insn.h>
  34 #include <asm/kprobes.h>
  35
  36 #define AARCH64_INSN_SF_BIT     BIT(31)
  37 #define AARCH64_INSN_N_BIT      BIT(22)
  38 #define AARCH64_INSN_LSL_12     BIT(22)
  39
  40 static int aarch64_insn_encoding_class[] = {
  41         AARCH64_INSN_CLS_UNKNOWN,
  42         AARCH64_INSN_CLS_UNKNOWN,
  43         AARCH64_INSN_CLS_UNKNOWN,
  44         AARCH64_INSN_CLS_UNKNOWN,
  45         AARCH64_INSN_CLS_LDST,
  46         AARCH64_INSN_CLS_DP_REG,
  47         AARCH64_INSN_CLS_LDST,
  48         AARCH64_INSN_CLS_DP_FPSIMD,
  49         AARCH64_INSN_CLS_DP_IMM,
  50         AARCH64_INSN_CLS_DP_IMM,
  51         AARCH64_INSN_CLS_BR_SYS,
  52         AARCH64_INSN_CLS_BR_SYS,
  53         AARCH64_INSN_CLS_LDST,
  54         AARCH64_INSN_CLS_DP_REG,
  55         AARCH64_INSN_CLS_LDST,
  56         AARCH64_INSN_CLS_DP_FPSIMD,
  57 };
  58
  59 enum aarch64_insn_encoding_class __kprobes aarch64_get_insn_class(u32 insn)
  60 {
  61         return aarch64_insn_encoding_class[(insn >> 25) & 0xf];
  62 }
  63
  64 /* NOP is an alias of HINT */
  65 bool __kprobes aarch64_insn_is_nop(u32 insn)
  66 {
  67         if (!aarch64_insn_is_hint(insn))
  68                 return false;
  69
  70         switch (insn & 0xFE0) {
  71         case AARCH64_INSN_HINT_YIELD:
  72         case AARCH64_INSN_HINT_WFE:
  73         case AARCH64_INSN_HINT_WFI:
  74         case AARCH64_INSN_HINT_SEV:
  75         case AARCH64_INSN_HINT_SEVL:
  76                 return false;
  77         default:
  78                 return true;
  79         }
  80 }
  81
  82 bool aarch64_insn_is_branch_imm(u32 insn)
  83 {
  84         return (aarch64_insn_is_b(insn) || aarch64_insn_is_bl(insn) ||
  85                 aarch64_insn_is_tbz(insn) || aarch64_insn_is_tbnz(insn) ||
  86                 aarch64_insn_is_cbz(insn) || aarch64_insn_is_cbnz(insn) ||
  87                 aarch64_insn_is_bcond(insn));
  88 }
  89
  90 static DEFINE_RAW_SPINLOCK(patch_lock);
  91
  92 static void __kprobes *patch_map(void *addr, int fixmap)
  93 {
  94         unsigned long uintaddr = (uintptr_t) addr;
  95         bool module = !core_kernel_text(uintaddr);
  96         struct page *page;
  97
  98         if (module && IS_ENABLED(CONFIG_STRICT_MODULE_RWX))
  99                 page = vmalloc_to_page(addr);
 100         else if (!module)
 101                 page = phys_to_page(__pa_symbol(addr));
 102         else
 103                 return addr;
 104
 105         BUG_ON(!page);
 106         return (void *)set_fixmap_offset(fixmap, page_to_phys(page) +
 107                         (uintaddr & ~PAGE_MASK));
 108 }
 109
 110 static void __kprobes patch_unmap(int fixmap)
 111 {
 112         clear_fixmap(fixmap);
 113 }
 114 /*
 115  * In ARMv8-A, A64 instructions have a fixed length of 32 bits and are always
 116  * little-endian.
 117  */
 118 int __kprobes aarch64_insn_read(void *addr, u32 *insnp)
 119 {
 120         int ret;
 121         __le32 val;
 122
 123         ret = probe_kernel_read(&val, addr, AARCH64_INSN_SIZE);
 124         if (!ret)
 125                 *insnp = le32_to_cpu(val);
 126
 127         return ret;
 128 }
 129
 130 static int __kprobes __aarch64_insn_write(void *addr, __le32 insn)
 131 {
 132         void *waddr = addr;
 133         unsigned long flags = 0;
 134         int ret;
 135
 136         raw_spin_lock_irqsave(&patch_lock, flags);
 137         waddr = patch_map(addr, FIX_TEXT_POKE0);
 138
 139         ret = probe_kernel_write(waddr, &insn, AARCH64_INSN_SIZE);
 140
 141         patch_unmap(FIX_TEXT_POKE0);
 142         raw_spin_unlock_irqrestore(&patch_lock, flags);
 143
 144         return ret;
 145 }
 146
 147 int __kprobes aarch64_insn_write(void *addr, u32 insn)
 148 {
 149         return __aarch64_insn_write(addr, cpu_to_le32(insn));
 150 }
 151
 152 bool __kprobes aarch64_insn_uses_literal(u32 insn)
 153 {
 154         /* ldr/ldrsw (literal), prfm */
 155
 156         return aarch64_insn_is_ldr_lit(insn) ||
 157                 aarch64_insn_is_ldrsw_lit(insn) ||
 158                 aarch64_insn_is_adr_adrp(insn) ||
 159                 aarch64_insn_is_prfm_lit(insn);
 160 }
 161
 162 bool __kprobes aarch64_insn_is_branch(u32 insn)
 163 {
 164         /* b, bl, cb*, tb*, b.cond, br, blr */
 165
 166         return aarch64_insn_is_b(insn) ||
 167                 aarch64_insn_is_bl(insn) ||
 168                 aarch64_insn_is_cbz(insn) ||
 169                 aarch64_insn_is_cbnz(insn) ||
 170                 aarch64_insn_is_tbz(insn) ||
 171                 aarch64_insn_is_tbnz(insn) ||
 172                 aarch64_insn_is_ret(insn) ||
 173                 aarch64_insn_is_br(insn) ||
 174                 aarch64_insn_is_blr(insn) ||
 175                 aarch64_insn_is_bcond(insn);
 176 }
 177
 178 int __kprobes aarch64_insn_patch_text_nosync(void *addr, u32 insn)
 179 {
 180         u32 *tp = addr;
 181         int ret;
 182
 183         /* A64 instructions must be word aligned */
 184         if ((uintptr_t)tp & 0x3)
 185                 return -EINVAL;
 186
 187         ret = aarch64_insn_write(tp, insn);
 188         if (ret == 0)
 189                 __flush_icache_range((uintptr_t)tp,
 190                                      (uintptr_t)tp + AARCH64_INSN_SIZE);
 191
 192         return ret;
 193 }
 194
 195 struct aarch64_insn_patch {
 196         void            **text_addrs;
 197         u32             *new_insns;
 198         int             insn_cnt;
 199         atomic_t        cpu_count;
 200 };
 201
 202 static int __kprobes aarch64_insn_patch_text_cb(void *arg)
 203 {
 204         int i, ret = 0;
 205         struct aarch64_insn_patch *pp = arg;
 206
 207         /* The first CPU becomes master */
 208         if (atomic_inc_return(&pp->cpu_count) == 1) {
 209                 for (i = 0; ret == 0 && i < pp->insn_cnt; i++)
 210                         ret = aarch64_insn_patch_text_nosync(pp->text_addrs[i],
 211                                                              pp->new_insns[i]);
 212                 /* Notify other processors with an additional increment. */
 213                 atomic_inc(&pp->cpu_count);
 214         } else {
 215                 while (atomic_read(&pp->cpu_count) <= num_online_cpus())
 216                         cpu_relax();
 217                 isb();
 218         }
 219
 220         return ret;
 221 }
 222
 223 int __kprobes aarch64_insn_patch_text(void *addrs[], u32 insns[], int cnt)
 224 {
 225         struct aarch64_insn_patch patch = {
 226                 .text_addrs = addrs,
 227                 .new_insns = insns,
 228                 .insn_cnt = cnt,
 229                 .cpu_count = ATOMIC_INIT(0),
 230         };
 231
 232         if (cnt <= 0)
 233                 return -EINVAL;
 234
 235         return stop_machine_cpuslocked(aarch64_insn_patch_text_cb, &patch,
 236                                        cpu_online_mask);
 237 }
 238
 239 static int __kprobes aarch64_get_imm_shift_mask(enum aarch64_insn_imm_type type,
 240                                                 u32 *maskp, int *shiftp)
 241 {
 242         u32 mask;
 243         int shift;
 244
 245         switch (type) {
 246         case AARCH64_INSN_IMM_26:
 247                 mask = BIT(26) - 1;
 248                 shift = 0;
 249                 break;
 250         case AARCH64_INSN_IMM_19:
 251                 mask = BIT(19) - 1;
 252                 shift = 5;
 253                 break;
 254         case AARCH64_INSN_IMM_16:
 255                 mask = BIT(16) - 1;
 256                 shift = 5;
 257                 break;
 258         case AARCH64_INSN_IMM_14:
 259                 mask = BIT(14) - 1;
 260                 shift = 5;
 261                 break;
 262         case AARCH64_INSN_IMM_12:
 263                 mask = BIT(12) - 1;
 264                 shift = 10;
 265                 break;
 266         case AARCH64_INSN_IMM_9:
 267                 mask = BIT(9) - 1;
 268                 shift = 12;
 269                 break;
 270         case AARCH64_INSN_IMM_7:
 271                 mask = BIT(7) - 1;
 272                 shift = 15;
 273                 break;
 274         case AARCH64_INSN_IMM_6:
 275         case AARCH64_INSN_IMM_S:
 276                 mask = BIT(6) - 1;
 277                 shift = 10;
 278                 break;
 279         case AARCH64_INSN_IMM_R:
 280                 mask = BIT(6) - 1;
 281                 shift = 16;
 282                 break;
 283         case AARCH64_INSN_IMM_N:
 284                 mask = 1;
 285                 shift = 22;
 286                 break;
 287         default:
 288                 return -EINVAL;
 289         }
 290
 291         *maskp = mask;
 292         *shiftp = shift;
 293
 294         return 0;
 295 }
 296
 297 #define ADR_IMM_HILOSPLIT       2
 298 #define ADR_IMM_SIZE            SZ_2M
 299 #define ADR_IMM_LOMASK          ((1 << ADR_IMM_HILOSPLIT) - 1)
 300 #define ADR_IMM_HIMASK          ((ADR_IMM_SIZE >> ADR_IMM_HILOSPLIT) - 1)
 301 #define ADR_IMM_LOSHIFT         29
 302 #define ADR_IMM_HISHIFT         5
 303
 304 u64 aarch64_insn_decode_immediate(enum aarch64_insn_imm_type type, u32 insn)
 305 {
 306         u32 immlo, immhi, mask;
 307         int shift;
 308
 309         switch (type) {
 310         case AARCH64_INSN_IMM_ADR:
 311                 shift = 0;
 312                 immlo = (insn >> ADR_IMM_LOSHIFT) & ADR_IMM_LOMASK;
 313                 immhi = (insn >> ADR_IMM_HISHIFT) & ADR_IMM_HIMASK;
 314                 insn = (immhi << ADR_IMM_HILOSPLIT) | immlo;
 315                 mask = ADR_IMM_SIZE - 1;
 316                 break;
 317         default:
 318                 if (aarch64_get_imm_shift_mask(type, &mask, &shift) < 0) {
 319                         pr_err("aarch64_insn_decode_immediate: unknown immediate encoding %d\n",
 320                                type);
 321                         return 0;
 322                 }
 323         }
 324
 325         return (insn >> shift) & mask;
 326 }
 327
 328 u32 __kprobes aarch64_insn_encode_immediate(enum aarch64_insn_imm_type type,
 329                                   u32 insn, u64 imm)
 330 {
 331         u32 immlo, immhi, mask;
 332         int shift;
 333
 334         if (insn == AARCH64_BREAK_FAULT)
 335                 return AARCH64_BREAK_FAULT;
 336
 337         switch (type) {
 338         case AARCH64_INSN_IMM_ADR:
 339                 shift = 0;
 340                 immlo = (imm & ADR_IMM_LOMASK) << ADR_IMM_LOSHIFT;
 341                 imm >>= ADR_IMM_HILOSPLIT;
 342                 immhi = (imm & ADR_IMM_HIMASK) << ADR_IMM_HISHIFT;
 343                 imm = immlo | immhi;
 344                 mask = ((ADR_IMM_LOMASK << ADR_IMM_LOSHIFT) |
 345                         (ADR_IMM_HIMASK << ADR_IMM_HISHIFT));
 346                 break;
 347         default:
 348                 if (aarch64_get_imm_shift_mask(type, &mask, &shift) < 0) {
 349                         pr_err("aarch64_insn_encode_immediate: unknown immediate encoding %d\n",
 350                                type);
 351                         return AARCH64_BREAK_FAULT;
 352                 }
 353         }
 354
 355         /* Update the immediate field. */
 356         insn &= ~(mask << shift);
 357         insn |= (imm & mask) << shift;
 358
 359         return insn;
 360 }
 361
 362 u32 aarch64_insn_decode_register(enum aarch64_insn_register_type type,
 363                                         u32 insn)
 364 {
 365         int shift;
 366
 367         switch (type) {
 368         case AARCH64_INSN_REGTYPE_RT:
 369         case AARCH64_INSN_REGTYPE_RD:
 370                 shift = 0;
 371                 break;
 372         case AARCH64_INSN_REGTYPE_RN:
 373                 shift = 5;
 374                 break;
 375         case AARCH64_INSN_REGTYPE_RT2:
 376         case AARCH64_INSN_REGTYPE_RA:
 377                 shift = 10;
 378                 break;
 379         case AARCH64_INSN_REGTYPE_RM:
 380                 shift = 16;
 381                 break;
 382         default:
 383                 pr_err("%s: unknown register type encoding %d\n", __func__,
 384                        type);
 385                 return 0;
 386         }
 387
 388         return (insn >> shift) & GENMASK(4, 0);
 389 }
 390
 391 static u32 aarch64_insn_encode_register(enum aarch64_insn_register_type type,
 392                                         u32 insn,
 393                                         enum aarch64_insn_register reg)
 394 {
 395         int shift;
 396
 397         if (insn == AARCH64_BREAK_FAULT)
 398                 return AARCH64_BREAK_FAULT;
 399
 400         if (reg < AARCH64_INSN_REG_0 || reg > AARCH64_INSN_REG_SP) {
 401                 pr_err("%s: unknown register encoding %d\n", __func__, reg);
 402                 return AARCH64_BREAK_FAULT;
 403         }
 404
 405         switch (type) {
 406         case AARCH64_INSN_REGTYPE_RT:
 407         case AARCH64_INSN_REGTYPE_RD:
 408                 shift = 0;
 409                 break;
 410         case AARCH64_INSN_REGTYPE_RN:
 411                 shift = 5;
 412                 break;
 413         case AARCH64_INSN_REGTYPE_RT2:
 414         case AARCH64_INSN_REGTYPE_RA:
 415                 shift = 10;
 416                 break;
 417         case AARCH64_INSN_REGTYPE_RM:
 418         case AARCH64_INSN_REGTYPE_RS:
 419                 shift = 16;
 420                 break;
 421         default:
 422                 pr_err("%s: unknown register type encoding %d\n", __func__,
 423                        type);
 424                 return AARCH64_BREAK_FAULT;
 425         }
 426
 427         insn &= ~(GENMASK(4, 0) << shift);
 428         insn |= reg << shift;
 429
 430         return insn;
 431 }
 432
 433 static u32 aarch64_insn_encode_ldst_size(enum aarch64_insn_size_type type,
 434                                          u32 insn)
 435 {
 436         u32 size;
 437
 438         switch (type) {
 439         case AARCH64_INSN_SIZE_8:
 440                 size = 0;
 441                 break;
 442         case AARCH64_INSN_SIZE_16:
 443                 size = 1;
 444                 break;
 445         case AARCH64_INSN_SIZE_32:
 446                 size = 2;
 447                 break;
 448         case AARCH64_INSN_SIZE_64:
 449                 size = 3;
 450                 break;
 451         default:
 452                 pr_err("%s: unknown size encoding %d\n", __func__, type);
 453                 return AARCH64_BREAK_FAULT;
 454         }
 455
 456         insn &= ~GENMASK(31, 30);
 457         insn |= size << 30;
 458
 459         return insn;
 460 }
 461
 462 static inline long branch_imm_common(unsigned long pc, unsigned long addr,
 463                                      long range)
 464 {
 465         long offset;
 466
 467         if ((pc & 0x3) || (addr & 0x3)) {
 468                 pr_err("%s: A64 instructions must be word aligned\n", __func__);
 469                 return range;
 470         }
 471
 472         offset = ((long)addr - (long)pc);
 473
 474         if (offset < -range || offset >= range) {
 475                 pr_err("%s: offset out of range\n", __func__);
 476                 return range;
 477         }
 478
 479         return offset;
 480 }
 481
 482 u32 __kprobes aarch64_insn_gen_branch_imm(unsigned long pc, unsigned long addr,
 483                                           enum aarch64_insn_branch_type type)
 484 {
 485         u32 insn;
 486         long offset;
 487
 488         /*
 489          * B/BL support [-128M, 128M) offset
 490          * ARM64 virtual address arrangement guarantees all kernel and module
 491          * texts are within +/-128M.
 492          */
 493         offset = branch_imm_common(pc, addr, SZ_128M);
 494         if (offset >= SZ_128M)
 495                 return AARCH64_BREAK_FAULT;
 496
 497         switch (type) {
 498         case AARCH64_INSN_BRANCH_LINK:
 499                 insn = aarch64_insn_get_bl_value();
 500                 break;
 501         case AARCH64_INSN_BRANCH_NOLINK:
 502                 insn = aarch64_insn_get_b_value();
 503                 break;
 504         default:
 505                 pr_err("%s: unknown branch encoding %d\n", __func__, type);
 506                 return AARCH64_BREAK_FAULT;
 507         }
 508
 509         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_26, insn,
 510                                              offset >> 2);
 511 }
 512
 513 u32 aarch64_insn_gen_comp_branch_imm(unsigned long pc, unsigned long addr,
 514                                      enum aarch64_insn_register reg,
 515                                      enum aarch64_insn_variant variant,
 516                                      enum aarch64_insn_branch_type type)
 517 {
 518         u32 insn;
 519         long offset;
 520
 521         offset = branch_imm_common(pc, addr, SZ_1M);
 522         if (offset >= SZ_1M)
 523                 return AARCH64_BREAK_FAULT;
 524
 525         switch (type) {
 526         case AARCH64_INSN_BRANCH_COMP_ZERO:
 527                 insn = aarch64_insn_get_cbz_value();
 528                 break;
 529         case AARCH64_INSN_BRANCH_COMP_NONZERO:
 530                 insn = aarch64_insn_get_cbnz_value();
 531                 break;
 532         default:
 533                 pr_err("%s: unknown branch encoding %d\n", __func__, type);
 534                 return AARCH64_BREAK_FAULT;
 535         }
 536
 537         switch (variant) {
 538         case AARCH64_INSN_VARIANT_32BIT:
 539                 break;
 540         case AARCH64_INSN_VARIANT_64BIT:
 541                 insn |= AARCH64_INSN_SF_BIT;
 542                 break;
 543         default:
 544                 pr_err("%s: unknown variant encoding %d\n", __func__, variant);
 545                 return AARCH64_BREAK_FAULT;
 546         }
 547
 548         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RT, insn, reg);
 549
 550         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_19, insn,
 551                                              offset >> 2);
 552 }
 553
 554 u32 aarch64_insn_gen_cond_branch_imm(unsigned long pc, unsigned long addr,
 555                                      enum aarch64_insn_condition cond)
 556 {
 557         u32 insn;
 558         long offset;
 559
 560         offset = branch_imm_common(pc, addr, SZ_1M);
 561
 562         insn = aarch64_insn_get_bcond_value();
 563
 564         if (cond < AARCH64_INSN_COND_EQ || cond > AARCH64_INSN_COND_AL) {
 565                 pr_err("%s: unknown condition encoding %d\n", __func__, cond);
 566                 return AARCH64_BREAK_FAULT;
 567         }
 568         insn |= cond;
 569
 570         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_19, insn,
 571                                              offset >> 2);
 572 }
 573
 574 u32 __kprobes aarch64_insn_gen_hint(enum aarch64_insn_hint_op op)
 575 {
 576         return aarch64_insn_get_hint_value() | op;
 577 }
 578
 579 u32 __kprobes aarch64_insn_gen_nop(void)
 580 {
 581         return aarch64_insn_gen_hint(AARCH64_INSN_HINT_NOP);
 582 }
 583
 584 u32 aarch64_insn_gen_branch_reg(enum aarch64_insn_register reg,
 585                                 enum aarch64_insn_branch_type type)
 586 {
 587         u32 insn;
 588
 589         switch (type) {
 590         case AARCH64_INSN_BRANCH_NOLINK:
 591                 insn = aarch64_insn_get_br_value();
 592                 break;
 593         case AARCH64_INSN_BRANCH_LINK:
 594                 insn = aarch64_insn_get_blr_value();
 595                 break;
 596         case AARCH64_INSN_BRANCH_RETURN:
 597                 insn = aarch64_insn_get_ret_value();
 598                 break;
 599         default:
 600                 pr_err("%s: unknown branch encoding %d\n", __func__, type);
 601                 return AARCH64_BREAK_FAULT;
 602         }
 603
 604         return aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, reg);
 605 }
 606
 607 u32 aarch64_insn_gen_load_store_reg(enum aarch64_insn_register reg,
 608                                     enum aarch64_insn_register base,
 609                                     enum aarch64_insn_register offset,
 610                                     enum aarch64_insn_size_type size,
 611                                     enum aarch64_insn_ldst_type type)
 612 {
 613         u32 insn;
 614
 615         switch (type) {
 616         case AARCH64_INSN_LDST_LOAD_REG_OFFSET:
 617                 insn = aarch64_insn_get_ldr_reg_value();
 618                 break;
 619         case AARCH64_INSN_LDST_STORE_REG_OFFSET:
 620                 insn = aarch64_insn_get_str_reg_value();
 621                 break;
 622         default:
 623                 pr_err("%s: unknown load/store encoding %d\n", __func__, type);
 624                 return AARCH64_BREAK_FAULT;
 625         }
 626
 627         insn = aarch64_insn_encode_ldst_size(size, insn);
 628
 629         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RT, insn, reg);
 630
 631         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn,
 632                                             base);
 633
 634         return aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RM, insn,
 635                                             offset);
 636 }
 637
 638 u32 aarch64_insn_gen_load_store_pair(enum aarch64_insn_register reg1,
 639                                      enum aarch64_insn_register reg2,
 640                                      enum aarch64_insn_register base,
 641                                      int offset,
 642                                      enum aarch64_insn_variant variant,
 643                                      enum aarch64_insn_ldst_type type)
 644 {
 645         u32 insn;
 646         int shift;
 647
 648         switch (type) {
 649         case AARCH64_INSN_LDST_LOAD_PAIR_PRE_INDEX:
 650                 insn = aarch64_insn_get_ldp_pre_value();
 651                 break;
 652         case AARCH64_INSN_LDST_STORE_PAIR_PRE_INDEX:
 653                 insn = aarch64_insn_get_stp_pre_value();
 654                 break;
 655         case AARCH64_INSN_LDST_LOAD_PAIR_POST_INDEX:
 656                 insn = aarch64_insn_get_ldp_post_value();
 657                 break;
 658         case AARCH64_INSN_LDST_STORE_PAIR_POST_INDEX:
 659                 insn = aarch64_insn_get_stp_post_value();
 660                 break;
 661         default:
 662                 pr_err("%s: unknown load/store encoding %d\n", __func__, type);
 663                 return AARCH64_BREAK_FAULT;
 664         }
 665
 666         switch (variant) {
 667         case AARCH64_INSN_VARIANT_32BIT:
 668                 if ((offset & 0x3) || (offset < -256) || (offset > 252)) {
 669                         pr_err("%s: offset must be multiples of 4 in the range of [-256, 252] %d\n",
 670                                __func__, offset);
 671                         return AARCH64_BREAK_FAULT;
 672                 }
 673                 shift = 2;
 674                 break;
 675         case AARCH64_INSN_VARIANT_64BIT:
 676                 if ((offset & 0x7) || (offset < -512) || (offset > 504)) {
 677                         pr_err("%s: offset must be multiples of 8 in the range of [-512, 504] %d\n",
 678                                __func__, offset);
 679                         return AARCH64_BREAK_FAULT;
 680                 }
 681                 shift = 3;
 682                 insn |= AARCH64_INSN_SF_BIT;
 683                 break;
 684         default:
 685                 pr_err("%s: unknown variant encoding %d\n", __func__, variant);
 686                 return AARCH64_BREAK_FAULT;
 687         }
 688
 689         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RT, insn,
 690                                             reg1);
 691
 692         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RT2, insn,
 693                                             reg2);
 694
 695         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn,
 696                                             base);
 697
 698         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_7, insn,
 699                                              offset >> shift);
 700 }
 701
 702 u32 aarch64_insn_gen_load_store_ex(enum aarch64_insn_register reg,
 703                                    enum aarch64_insn_register base,
 704                                    enum aarch64_insn_register state,
 705                                    enum aarch64_insn_size_type size,
 706                                    enum aarch64_insn_ldst_type type)
 707 {
 708         u32 insn;
 709
 710         switch (type) {
 711         case AARCH64_INSN_LDST_LOAD_EX:
 712                 insn = aarch64_insn_get_load_ex_value();
 713                 break;
 714         case AARCH64_INSN_LDST_STORE_EX:
 715                 insn = aarch64_insn_get_store_ex_value();
 716                 break;
 717         default:
 718                 pr_err("%s: unknown load/store exclusive encoding %d\n", __func__, type);
 719                 return AARCH64_BREAK_FAULT;
 720         }
 721
 722         insn = aarch64_insn_encode_ldst_size(size, insn);
 723
 724         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RT, insn,
 725                                             reg);
 726
 727         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn,
 728                                             base);
 729
 730         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RT2, insn,
 731                                             AARCH64_INSN_REG_ZR);
 732
 733         return aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RS, insn,
 734                                             state);
 735 }
 736
 737 static u32 aarch64_insn_encode_prfm_imm(enum aarch64_insn_prfm_type type,
 738                                         enum aarch64_insn_prfm_target target,
 739                                         enum aarch64_insn_prfm_policy policy,
 740                                         u32 insn)
 741 {
 742         u32 imm_type = 0, imm_target = 0, imm_policy = 0;
 743
 744         switch (type) {
 745         case AARCH64_INSN_PRFM_TYPE_PLD:
 746                 break;
 747         case AARCH64_INSN_PRFM_TYPE_PLI:
 748                 imm_type = BIT(0);
 749                 break;
 750         case AARCH64_INSN_PRFM_TYPE_PST:
 751                 imm_type = BIT(1);
 752                 break;
 753         default:
 754                 pr_err("%s: unknown prfm type encoding %d\n", __func__, type);
 755                 return AARCH64_BREAK_FAULT;
 756         }
 757
 758         switch (target) {
 759         case AARCH64_INSN_PRFM_TARGET_L1:
 760                 break;
 761         case AARCH64_INSN_PRFM_TARGET_L2:
 762                 imm_target = BIT(0);
 763                 break;
 764         case AARCH64_INSN_PRFM_TARGET_L3:
 765                 imm_target = BIT(1);
 766                 break;
 767         default:
 768                 pr_err("%s: unknown prfm target encoding %d\n", __func__, target);
 769                 return AARCH64_BREAK_FAULT;
 770         }
 771
 772         switch (policy) {
 773         case AARCH64_INSN_PRFM_POLICY_KEEP:
 774                 break;
 775         case AARCH64_INSN_PRFM_POLICY_STRM:
 776                 imm_policy = BIT(0);
 777                 break;
 778         default:
 779                 pr_err("%s: unknown prfm policy encoding %d\n", __func__, policy);
 780                 return AARCH64_BREAK_FAULT;
 781         }
 782
 783         /* In this case, imm5 is encoded into Rt field. */
 784         insn &= ~GENMASK(4, 0);
 785         insn |= imm_policy | (imm_target << 1) | (imm_type << 3);
 786
 787         return insn;
 788 }
 789
 790 u32 aarch64_insn_gen_prefetch(enum aarch64_insn_register base,
 791                               enum aarch64_insn_prfm_type type,
 792                               enum aarch64_insn_prfm_target target,
 793                               enum aarch64_insn_prfm_policy policy)
 794 {
 795         u32 insn = aarch64_insn_get_prfm_value();
 796
 797         insn = aarch64_insn_encode_ldst_size(AARCH64_INSN_SIZE_64, insn);
 798
 799         insn = aarch64_insn_encode_prfm_imm(type, target, policy, insn);
 800
 801         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn,
 802                                             base);
 803
 804         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_12, insn, 0);
 805 }
 806
 807 u32 aarch64_insn_gen_add_sub_imm(enum aarch64_insn_register dst,
 808                                  enum aarch64_insn_register src,
 809                                  int imm, enum aarch64_insn_variant variant,
 810                                  enum aarch64_insn_adsb_type type)
 811 {
 812         u32 insn;
 813
 814         switch (type) {
 815         case AARCH64_INSN_ADSB_ADD:
 816                 insn = aarch64_insn_get_add_imm_value();
 817                 break;
 818         case AARCH64_INSN_ADSB_SUB:
 819                 insn = aarch64_insn_get_sub_imm_value();
 820                 break;
 821         case AARCH64_INSN_ADSB_ADD_SETFLAGS:
 822                 insn = aarch64_insn_get_adds_imm_value();
 823                 break;
 824         case AARCH64_INSN_ADSB_SUB_SETFLAGS:
 825                 insn = aarch64_insn_get_subs_imm_value();
 826                 break;
 827         default:
 828                 pr_err("%s: unknown add/sub encoding %d\n", __func__, type);
 829                 return AARCH64_BREAK_FAULT;
 830         }
 831
 832         switch (variant) {
 833         case AARCH64_INSN_VARIANT_32BIT:
 834                 break;
 835         case AARCH64_INSN_VARIANT_64BIT:
 836                 insn |= AARCH64_INSN_SF_BIT;
 837                 break;
 838         default:
 839                 pr_err("%s: unknown variant encoding %d\n", __func__, variant);
 840                 return AARCH64_BREAK_FAULT;
 841         }
 842
 843         /* We can't encode more than a 24bit value (12bit + 12bit shift) */
 844         if (imm & ~(BIT(24) - 1))
 845                 goto out;
 846
 847         /* If we have something in the top 12 bits... */
 848         if (imm & ~(SZ_4K - 1)) {
 849                 /* ... and in the low 12 bits -> error */
 850                 if (imm & (SZ_4K - 1))
 851                         goto out;
 852
 853                 imm >>= 12;
 854                 insn |= AARCH64_INSN_LSL_12;
 855         }
 856
 857         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
 858
 859         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, src);
 860
 861         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_12, insn, imm);
 862
 863 out:
 864         pr_err("%s: invalid immediate encoding %d\n", __func__, imm);
 865         return AARCH64_BREAK_FAULT;
 866 }
 867
 868 u32 aarch64_insn_gen_bitfield(enum aarch64_insn_register dst,
 869                               enum aarch64_insn_register src,
 870                               int immr, int imms,
 871                               enum aarch64_insn_variant variant,
 872                               enum aarch64_insn_bitfield_type type)
 873 {
 874         u32 insn;
 875         u32 mask;
 876
 877         switch (type) {
 878         case AARCH64_INSN_BITFIELD_MOVE:
 879                 insn = aarch64_insn_get_bfm_value();
 880                 break;
 881         case AARCH64_INSN_BITFIELD_MOVE_UNSIGNED:
 882                 insn = aarch64_insn_get_ubfm_value();
 883                 break;
 884         case AARCH64_INSN_BITFIELD_MOVE_SIGNED:
 885                 insn = aarch64_insn_get_sbfm_value();
 886                 break;
 887         default:
 888                 pr_err("%s: unknown bitfield encoding %d\n", __func__, type);
 889                 return AARCH64_BREAK_FAULT;
 890         }
 891
 892         switch (variant) {
 893         case AARCH64_INSN_VARIANT_32BIT:
 894                 mask = GENMASK(4, 0);
 895                 break;
 896         case AARCH64_INSN_VARIANT_64BIT:
 897                 insn |= AARCH64_INSN_SF_BIT | AARCH64_INSN_N_BIT;
 898                 mask = GENMASK(5, 0);
 899                 break;
 900         default:
 901                 pr_err("%s: unknown variant encoding %d\n", __func__, variant);
 902                 return AARCH64_BREAK_FAULT;
 903         }
 904
 905         if (immr & ~mask) {
 906                 pr_err("%s: invalid immr encoding %d\n", __func__, immr);
 907                 return AARCH64_BREAK_FAULT;
 908         }
 909         if (imms & ~mask) {
 910                 pr_err("%s: invalid imms encoding %d\n", __func__, imms);
 911                 return AARCH64_BREAK_FAULT;
 912         }
 913
 914         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
 915
 916         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, src);
 917
 918         insn = aarch64_insn_encode_immediate(AARCH64_INSN_IMM_R, insn, immr);
 919
 920         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_S, insn, imms);
 921 }
 922
 923 u32 aarch64_insn_gen_movewide(enum aarch64_insn_register dst,
 924                               int imm, int shift,
 925                               enum aarch64_insn_variant variant,
 926                               enum aarch64_insn_movewide_type type)
 927 {
 928         u32 insn;
 929
 930         switch (type) {
 931         case AARCH64_INSN_MOVEWIDE_ZERO:
 932                 insn = aarch64_insn_get_movz_value();
 933                 break;
 934         case AARCH64_INSN_MOVEWIDE_KEEP:
 935                 insn = aarch64_insn_get_movk_value();
 936                 break;
 937         case AARCH64_INSN_MOVEWIDE_INVERSE:
 938                 insn = aarch64_insn_get_movn_value();
 939                 break;
 940         default:
 941                 pr_err("%s: unknown movewide encoding %d\n", __func__, type);
 942                 return AARCH64_BREAK_FAULT;
 943         }
 944
 945         if (imm & ~(SZ_64K - 1)) {
 946                 pr_err("%s: invalid immediate encoding %d\n", __func__, imm);
 947                 return AARCH64_BREAK_FAULT;
 948         }
 949
 950         switch (variant) {
 951         case AARCH64_INSN_VARIANT_32BIT:
 952                 if (shift != 0 && shift != 16) {
 953                         pr_err("%s: invalid shift encoding %d\n", __func__,
 954                                shift);
 955                         return AARCH64_BREAK_FAULT;
 956                 }
 957                 break;
 958         case AARCH64_INSN_VARIANT_64BIT:
 959                 insn |= AARCH64_INSN_SF_BIT;
 960                 if (shift != 0 && shift != 16 && shift != 32 && shift != 48) {
 961                         pr_err("%s: invalid shift encoding %d\n", __func__,
 962                                shift);
 963                         return AARCH64_BREAK_FAULT;
 964                 }
 965                 break;
 966         default:
 967                 pr_err("%s: unknown variant encoding %d\n", __func__, variant);
 968                 return AARCH64_BREAK_FAULT;
 969         }
 970
 971         insn |= (shift >> 4) << 21;
 972
 973         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
 974
 975         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_16, insn, imm);
 976 }
 977
 978 u32 aarch64_insn_gen_add_sub_shifted_reg(enum aarch64_insn_register dst,
 979                                          enum aarch64_insn_register src,
 980                                          enum aarch64_insn_register reg,
 981                                          int shift,
 982                                          enum aarch64_insn_variant variant,
 983                                          enum aarch64_insn_adsb_type type)
 984 {
 985         u32 insn;
 986
 987         switch (type) {
 988         case AARCH64_INSN_ADSB_ADD:
 989                 insn = aarch64_insn_get_add_value();
 990                 break;
 991         case AARCH64_INSN_ADSB_SUB:
 992                 insn = aarch64_insn_get_sub_value();
 993                 break;
 994         case AARCH64_INSN_ADSB_ADD_SETFLAGS:
 995                 insn = aarch64_insn_get_adds_value();
 996                 break;
 997         case AARCH64_INSN_ADSB_SUB_SETFLAGS:
 998                 insn = aarch64_insn_get_subs_value();
 999                 break;
1000         default:
1001                 pr_err("%s: unknown add/sub encoding %d\n", __func__, type);
1002                 return AARCH64_BREAK_FAULT;
1003         }
1004
1005         switch (variant) {
1006         case AARCH64_INSN_VARIANT_32BIT:
1007                 if (shift & ~(SZ_32 - 1)) {
1008                         pr_err("%s: invalid shift encoding %d\n", __func__,
1009                                shift);
1010                         return AARCH64_BREAK_FAULT;
1011                 }
1012                 break;
1013         case AARCH64_INSN_VARIANT_64BIT:
1014                 insn |= AARCH64_INSN_SF_BIT;
1015                 if (shift & ~(SZ_64 - 1)) {
1016                         pr_err("%s: invalid shift encoding %d\n", __func__,
1017                                shift);
1018                         return AARCH64_BREAK_FAULT;
1019                 }
1020                 break;
1021         default:
1022                 pr_err("%s: unknown variant encoding %d\n", __func__, variant);
1023                 return AARCH64_BREAK_FAULT;
1024         }
1025
1026
1027         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
1028
1029         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, src);
1030
1031         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RM, insn, reg);
1032
1033         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_6, insn, shift);
1034 }
1035
1036 u32 aarch64_insn_gen_data1(enum aarch64_insn_register dst,
1037                            enum aarch64_insn_register src,
1038                            enum aarch64_insn_variant variant,
1039                            enum aarch64_insn_data1_type type)
1040 {
1041         u32 insn;
1042
1043         switch (type) {
1044         case AARCH64_INSN_DATA1_REVERSE_16:
1045                 insn = aarch64_insn_get_rev16_value();
1046                 break;
1047         case AARCH64_INSN_DATA1_REVERSE_32:
1048                 insn = aarch64_insn_get_rev32_value();
1049                 break;
1050         case AARCH64_INSN_DATA1_REVERSE_64:
1051                 if (variant != AARCH64_INSN_VARIANT_64BIT) {
1052                         pr_err("%s: invalid variant for reverse64 %d\n",
1053                                __func__, variant);
1054                         return AARCH64_BREAK_FAULT;
1055                 }
1056                 insn = aarch64_insn_get_rev64_value();
1057                 break;
1058         default:
1059                 pr_err("%s: unknown data1 encoding %d\n", __func__, type);
1060                 return AARCH64_BREAK_FAULT;
1061         }
1062
1063         switch (variant) {
1064         case AARCH64_INSN_VARIANT_32BIT:
1065                 break;
1066         case AARCH64_INSN_VARIANT_64BIT:
1067                 insn |= AARCH64_INSN_SF_BIT;
1068                 break;
1069         default:
1070                 pr_err("%s: unknown variant encoding %d\n", __func__, variant);
1071                 return AARCH64_BREAK_FAULT;
1072         }
1073
1074         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
1075
1076         return aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, src);
1077 }
1078
1079 u32 aarch64_insn_gen_data2(enum aarch64_insn_register dst,
1080                            enum aarch64_insn_register src,
1081                            enum aarch64_insn_register reg,
1082                            enum aarch64_insn_variant variant,
1083                            enum aarch64_insn_data2_type type)
1084 {
1085         u32 insn;
1086
1087         switch (type) {
1088         case AARCH64_INSN_DATA2_UDIV:
1089                 insn = aarch64_insn_get_udiv_value();
1090                 break;
1091         case AARCH64_INSN_DATA2_SDIV:
1092                 insn = aarch64_insn_get_sdiv_value();
1093                 break;
1094         case AARCH64_INSN_DATA2_LSLV:
1095                 insn = aarch64_insn_get_lslv_value();
1096                 break;
1097         case AARCH64_INSN_DATA2_LSRV:
1098                 insn = aarch64_insn_get_lsrv_value();
1099                 break;
1100         case AARCH64_INSN_DATA2_ASRV:
1101                 insn = aarch64_insn_get_asrv_value();
1102                 break;
1103         case AARCH64_INSN_DATA2_RORV:
1104                 insn = aarch64_insn_get_rorv_value();
1105                 break;
1106         default:
1107                 pr_err("%s: unknown data2 encoding %d\n", __func__, type);
1108                 return AARCH64_BREAK_FAULT;
1109         }
1110
1111         switch (variant) {
1112         case AARCH64_INSN_VARIANT_32BIT:
1113                 break;
1114         case AARCH64_INSN_VARIANT_64BIT:
1115                 insn |= AARCH64_INSN_SF_BIT;
1116                 break;
1117         default:
1118                 pr_err("%s: unknown variant encoding %d\n", __func__, variant);
1119                 return AARCH64_BREAK_FAULT;
1120         }
1121
1122         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
1123
1124         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, src);
1125
1126         return aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RM, insn, reg);
1127 }
1128
1129 u32 aarch64_insn_gen_data3(enum aarch64_insn_register dst,
1130                            enum aarch64_insn_register src,
1131                            enum aarch64_insn_register reg1,
1132                            enum aarch64_insn_register reg2,
1133                            enum aarch64_insn_variant variant,
1134                            enum aarch64_insn_data3_type type)
1135 {
1136         u32 insn;
1137
1138         switch (type) {
1139         case AARCH64_INSN_DATA3_MADD:
1140                 insn = aarch64_insn_get_madd_value();
1141                 break;
1142         case AARCH64_INSN_DATA3_MSUB:
1143                 insn = aarch64_insn_get_msub_value();
1144                 break;
1145         default:
1146                 pr_err("%s: unknown data3 encoding %d\n", __func__, type);
1147                 return AARCH64_BREAK_FAULT;
1148         }
1149
1150         switch (variant) {
1151         case AARCH64_INSN_VARIANT_32BIT:
1152                 break;
1153         case AARCH64_INSN_VARIANT_64BIT:
1154                 insn |= AARCH64_INSN_SF_BIT;
1155                 break;
1156         default:
1157                 pr_err("%s: unknown variant encoding %d\n", __func__, variant);
1158                 return AARCH64_BREAK_FAULT;
1159         }
1160
1161         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
1162
1163         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RA, insn, src);
1164
1165         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn,
1166                                             reg1);
1167
1168         return aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RM, insn,
1169                                             reg2);
1170 }
1171
1172 u32 aarch64_insn_gen_logical_shifted_reg(enum aarch64_insn_register dst,
1173                                          enum aarch64_insn_register src,
1174                                          enum aarch64_insn_register reg,
1175                                          int shift,
1176                                          enum aarch64_insn_variant variant,
1177                                          enum aarch64_insn_logic_type type)
1178 {
1179         u32 insn;
1180
1181         switch (type) {
1182         case AARCH64_INSN_LOGIC_AND:
1183                 insn = aarch64_insn_get_and_value();
1184                 break;
1185         case AARCH64_INSN_LOGIC_BIC:
1186                 insn = aarch64_insn_get_bic_value();
1187                 break;
1188         case AARCH64_INSN_LOGIC_ORR:
1189                 insn = aarch64_insn_get_orr_value();
1190                 break;
1191         case AARCH64_INSN_LOGIC_ORN:
1192                 insn = aarch64_insn_get_orn_value();
1193                 break;
1194         case AARCH64_INSN_LOGIC_EOR:
1195                 insn = aarch64_insn_get_eor_value();
1196                 break;
1197         case AARCH64_INSN_LOGIC_EON:
1198                 insn = aarch64_insn_get_eon_value();
1199                 break;
1200         case AARCH64_INSN_LOGIC_AND_SETFLAGS:
1201                 insn = aarch64_insn_get_ands_value();
1202                 break;
1203         case AARCH64_INSN_LOGIC_BIC_SETFLAGS:
1204                 insn = aarch64_insn_get_bics_value();
1205                 break;
1206         default:
1207                 pr_err("%s: unknown logical encoding %d\n", __func__, type);
1208                 return AARCH64_BREAK_FAULT;
1209         }
1210
1211         switch (variant) {
1212         case AARCH64_INSN_VARIANT_32BIT:
1213                 if (shift & ~(SZ_32 - 1)) {
1214                         pr_err("%s: invalid shift encoding %d\n", __func__,
1215                                shift);
1216                         return AARCH64_BREAK_FAULT;
1217                 }
1218                 break;
1219         case AARCH64_INSN_VARIANT_64BIT:
1220                 insn |= AARCH64_INSN_SF_BIT;
1221                 if (shift & ~(SZ_64 - 1)) {
1222                         pr_err("%s: invalid shift encoding %d\n", __func__,
1223                                shift);
1224                         return AARCH64_BREAK_FAULT;
1225                 }
1226                 break;
1227         default:
1228                 pr_err("%s: unknown variant encoding %d\n", __func__, variant);
1229                 return AARCH64_BREAK_FAULT;
1230         }
1231
1232
1233         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
1234
1235         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, src);
1236
1237         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RM, insn, reg);
1238
1239         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_6, insn, shift);
1240 }
1241
1242 /*
1243  * Decode the imm field of a branch, and return the byte offset as a
1244  * signed value (so it can be used when computing a new branch
1245  * target).
1246  */
1247 s32 aarch64_get_branch_offset(u32 insn)
1248 {
1249         s32 imm;
1250
1251         if (aarch64_insn_is_b(insn) || aarch64_insn_is_bl(insn)) {
1252                 imm = aarch64_insn_decode_immediate(AARCH64_INSN_IMM_26, insn);
1253                 return (imm << 6) >> 4;
1254         }
1255
1256         if (aarch64_insn_is_cbz(insn) || aarch64_insn_is_cbnz(insn) ||
1257             aarch64_insn_is_bcond(insn)) {
1258                 imm = aarch64_insn_decode_immediate(AARCH64_INSN_IMM_19, insn);
1259                 return (imm << 13) >> 11;
1260         }
1261
1262         if (aarch64_insn_is_tbz(insn) || aarch64_insn_is_tbnz(insn)) {
1263                 imm = aarch64_insn_decode_immediate(AARCH64_INSN_IMM_14, insn);
1264                 return (imm << 18) >> 16;
1265         }
1266
1267         /* Unhandled instruction */
1268         BUG();
1269 }
1270
1271 /*
1272  * Encode the displacement of a branch in the imm field and return the
1273  * updated instruction.
1274  */
1275 u32 aarch64_set_branch_offset(u32 insn, s32 offset)
1276 {
1277         if (aarch64_insn_is_b(insn) || aarch64_insn_is_bl(insn))
1278                 return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_26, insn,
1279                                                      offset >> 2);
1280
1281         if (aarch64_insn_is_cbz(insn) || aarch64_insn_is_cbnz(insn) ||
1282             aarch64_insn_is_bcond(insn))
1283                 return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_19, insn,
1284                                                      offset >> 2);
1285
1286         if (aarch64_insn_is_tbz(insn) || aarch64_insn_is_tbnz(insn))
1287                 return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_14, insn,
1288                                                      offset >> 2);
1289
1290         /* Unhandled instruction */
1291         BUG();
1292 }
1293
1294 s32 aarch64_insn_adrp_get_offset(u32 insn)
1295 {
1296         BUG_ON(!aarch64_insn_is_adrp(insn));
1297         return aarch64_insn_decode_immediate(AARCH64_INSN_IMM_ADR, insn) << 12;
1298 }
1299
1300 u32 aarch64_insn_adrp_set_offset(u32 insn, s32 offset)
1301 {
1302         BUG_ON(!aarch64_insn_is_adrp(insn));
1303         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_ADR, insn,
1304                                                 offset >> 12);
1305 }
1306
1307 /*
1308  * Extract the Op/CR data from a msr/mrs instruction.
1309  */
1310 u32 aarch64_insn_extract_system_reg(u32 insn)
1311 {
1312         return (insn & 0x1FFFE0) >> 5;
1313 }
1314
1315 bool aarch32_insn_is_wide(u32 insn)
1316 {
1317         return insn >= 0xe800;
1318 }
1319
1320 /*
1321  * Macros/defines for extracting register numbers from instruction.
1322  */
1323 u32 aarch32_insn_extract_reg_num(u32 insn, int offset)
1324 {
1325         return (insn & (0xf << offset)) >> offset;
1326 }
1327
1328 #define OPC2_MASK       0x7
1329 #define OPC2_OFFSET     5
1330 u32 aarch32_insn_mcr_extract_opc2(u32 insn)
1331 {
1332         return (insn & (OPC2_MASK << OPC2_OFFSET)) >> OPC2_OFFSET;
1333 }
1334
1335 #define CRM_MASK        0xf
1336 u32 aarch32_insn_mcr_extract_crm(u32 insn)
1337 {
1338         return insn & CRM_MASK;
1339 }
1340
1341 static bool __kprobes __check_eq(unsigned long pstate)
1342 {
1343         return (pstate & PSR_Z_BIT) != 0;
1344 }
1345
1346 static bool __kprobes __check_ne(unsigned long pstate)
1347 {
1348         return (pstate & PSR_Z_BIT) == 0;
1349 }
1350
1351 static bool __kprobes __check_cs(unsigned long pstate)
1352 {
1353         return (pstate & PSR_C_BIT) != 0;
1354 }
1355
1356 static bool __kprobes __check_cc(unsigned long pstate)
1357 {
1358         return (pstate & PSR_C_BIT) == 0;
1359 }
1360
1361 static bool __kprobes __check_mi(unsigned long pstate)
1362 {
1363         return (pstate & PSR_N_BIT) != 0;
1364 }
1365
1366 static bool __kprobes __check_pl(unsigned long pstate)
1367 {
1368         return (pstate & PSR_N_BIT) == 0;
1369 }
1370
1371 static bool __kprobes __check_vs(unsigned long pstate)
1372 {
1373         return (pstate & PSR_V_BIT) != 0;
1374 }
1375
1376 static bool __kprobes __check_vc(unsigned long pstate)
1377 {
1378         return (pstate & PSR_V_BIT) == 0;
1379 }
1380
1381 static bool __kprobes __check_hi(unsigned long pstate)
1382 {
1383         pstate &= ~(pstate >> 1);       /* PSR_C_BIT &= ~PSR_Z_BIT */
1384         return (pstate & PSR_C_BIT) != 0;
1385 }
1386
1387 static bool __kprobes __check_ls(unsigned long pstate)
1388 {
1389         pstate &= ~(pstate >> 1);       /* PSR_C_BIT &= ~PSR_Z_BIT */
1390         return (pstate & PSR_C_BIT) == 0;
1391 }
1392
1393 static bool __kprobes __check_ge(unsigned long pstate)
1394 {
1395         pstate ^= (pstate << 3);        /* PSR_N_BIT ^= PSR_V_BIT */
1396         return (pstate & PSR_N_BIT) == 0;
1397 }
1398
1399 static bool __kprobes __check_lt(unsigned long pstate)
1400 {
1401         pstate ^= (pstate << 3);        /* PSR_N_BIT ^= PSR_V_BIT */
1402         return (pstate & PSR_N_BIT) != 0;
1403 }
1404
1405 static bool __kprobes __check_gt(unsigned long pstate)
1406 {
1407         /*PSR_N_BIT ^= PSR_V_BIT */
1408         unsigned long temp = pstate ^ (pstate << 3);
1409
1410         temp |= (pstate << 1);  /*PSR_N_BIT |= PSR_Z_BIT */
1411         return (temp & PSR_N_BIT) == 0;
1412 }
1413
1414 static bool __kprobes __check_le(unsigned long pstate)
1415 {
1416         /*PSR_N_BIT ^= PSR_V_BIT */
1417         unsigned long temp = pstate ^ (pstate << 3);
1418
1419         temp |= (pstate << 1);  /*PSR_N_BIT |= PSR_Z_BIT */
1420         return (temp & PSR_N_BIT) != 0;
1421 }
1422
1423 static bool __kprobes __check_al(unsigned long pstate)
1424 {
1425         return true;
1426 }
1427
1428 /*
1429  * Note that the ARMv8 ARM calls condition code 0b1111 "nv", but states that
1430  * it behaves identically to 0b1110 ("al").
1431  */
1432 pstate_check_t * const aarch32_opcode_cond_checks[16] = {
1433         __check_eq, __check_ne, __check_cs, __check_cc,
1434         __check_mi, __check_pl, __check_vs, __check_vc,
1435         __check_hi, __check_ls, __check_ge, __check_lt,
1436         __check_gt, __check_le, __check_al, __check_al
1437 };
1438
1439 static bool range_of_ones(u64 val)
1440 {
1441         /* Doesn't handle full ones or full zeroes */
1442         u64 sval = val >> __ffs64(val);
1443
1444         /* One of Sean Eron Anderson's bithack tricks */
1445         return ((sval + 1) & (sval)) == 0;
1446 }
1447
1448 static u32 aarch64_encode_immediate(u64 imm,
1449                                     enum aarch64_insn_variant variant,
1450                                     u32 insn)
1451 {
1452         unsigned int immr, imms, n, ones, ror, esz, tmp;
1453         u64 mask = ~0UL;
1454
1455         /* Can't encode full zeroes or full ones */
1456         if (!imm || !~imm)
1457                 return AARCH64_BREAK_FAULT;
1458
1459         switch (variant) {
1460         case AARCH64_INSN_VARIANT_32BIT:
1461                 if (upper_32_bits(imm))
1462                         return AARCH64_BREAK_FAULT;
1463                 esz = 32;
1464                 break;
1465         case AARCH64_INSN_VARIANT_64BIT:
1466                 insn |= AARCH64_INSN_SF_BIT;
1467                 esz = 64;
1468                 break;
1469         default:
1470                 pr_err("%s: unknown variant encoding %d\n", __func__, variant);
1471                 return AARCH64_BREAK_FAULT;
1472         }
1473
1474         /*
1475          * Inverse of Replicate(). Try to spot a repeating pattern
1476          * with a pow2 stride.
1477          */
1478         for (tmp = esz / 2; tmp >= 2; tmp /= 2) {
1479                 u64 emask = BIT(tmp) - 1;
1480
1481                 if ((imm & emask) != ((imm >> tmp) & emask))
1482                         break;
1483
1484                 esz = tmp;
1485                 mask = emask;
1486         }
1487
1488         /* N is only set if we're encoding a 64bit value */
1489         n = esz == 64;
1490
1491         /* Trim imm to the element size */
1492         imm &= mask;
1493
1494         /* That's how many ones we need to encode */
1495         ones = hweight64(imm);
1496
1497         /*
1498          * imms is set to (ones - 1), prefixed with a string of ones
1499          * and a zero if they fit. Cap it to 6 bits.
1500          */
1501         imms  = ones - 1;
1502         imms |= 0xf << ffs(esz);
1503         imms &= BIT(6) - 1;
1504
1505         /* Compute the rotation */
1506         if (range_of_ones(imm)) {
1507                 /*
1508                  * Pattern: 0..01..10..0
1509                  *
1510                  * Compute how many rotate we need to align it right
1511                  */
1512                 ror = __ffs64(imm);
1513         } else {
1514                 /*
1515                  * Pattern: 0..01..10..01..1
1516                  *
1517                  * Fill the unused top bits with ones, and check if
1518                  * the result is a valid immediate (all ones with a
1519                  * contiguous ranges of zeroes).
1520                  */
1521                 imm |= ~mask;
1522                 if (!range_of_ones(~imm))
1523                         return AARCH64_BREAK_FAULT;
1524
1525                 /*
1526                  * Compute the rotation to get a continuous set of
1527                  * ones, with the first bit set at position 0
1528                  */
1529                 ror = fls(~imm);
1530         }
1531
1532         /*
1533          * immr is the number of bits we need to rotate back to the
1534          * original set of ones. Note that this is relative to the
1535          * element size...
1536          */
1537         immr = (esz - ror) % esz;
1538
1539         insn = aarch64_insn_encode_immediate(AARCH64_INSN_IMM_N, insn, n);
1540         insn = aarch64_insn_encode_immediate(AARCH64_INSN_IMM_R, insn, immr);
1541         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_S, insn, imms);
1542 }
1543
1544 u32 aarch64_insn_gen_logical_immediate(enum aarch64_insn_logic_type type,
1545                                        enum aarch64_insn_variant variant,
1546                                        enum aarch64_insn_register Rn,
1547                                        enum aarch64_insn_register Rd,
1548                                        u64 imm)
1549 {
1550         u32 insn;
1551
1552         switch (type) {
1553         case AARCH64_INSN_LOGIC_AND:
1554                 insn = aarch64_insn_get_and_imm_value();
1555                 break;
1556         case AARCH64_INSN_LOGIC_ORR:
1557                 insn = aarch64_insn_get_orr_imm_value();
1558                 break;
1559         case AARCH64_INSN_LOGIC_EOR:
1560                 insn = aarch64_insn_get_eor_imm_value();
1561                 break;
1562         case AARCH64_INSN_LOGIC_AND_SETFLAGS:
1563                 insn = aarch64_insn_get_ands_imm_value();
1564                 break;
1565         default:
1566                 pr_err("%s: unknown logical encoding %d\n", __func__, type);
1567                 return AARCH64_BREAK_FAULT;
1568         }
1569
1570         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, Rd);
1571         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, Rn);
1572         return aarch64_encode_immediate(imm, variant, insn);
1573 }
1574
1575 u32 aarch64_insn_gen_extr(enum aarch64_insn_variant variant,
1576                           enum aarch64_insn_register Rm,
1577                           enum aarch64_insn_register Rn,
1578                           enum aarch64_insn_register Rd,
1579                           u8 lsb)
1580 {
1581         u32 insn;
1582
1583         insn = aarch64_insn_get_extr_value();
1584
1585         switch (variant) {
1586         case AARCH64_INSN_VARIANT_32BIT:
1587                 if (lsb > 31)
1588                         return AARCH64_BREAK_FAULT;
1589                 break;
1590         case AARCH64_INSN_VARIANT_64BIT:
1591                 if (lsb > 63)
1592                         return AARCH64_BREAK_FAULT;
1593                 insn |= AARCH64_INSN_SF_BIT;
1594                 insn = aarch64_insn_encode_immediate(AARCH64_INSN_IMM_N, insn, 1);
1595                 break;
1596         default:
1597                 pr_err("%s: unknown variant encoding %d\n", __func__, variant);
1598                 return AARCH64_BREAK_FAULT;
1599         }
1600
1601         insn = aarch64_insn_encode_immediate(AARCH64_INSN_IMM_S, insn, lsb);
1602         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, Rd);
1603         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, Rn);
1604         return aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RM, insn, Rm);
1605 }