arch/arm64/kernel/insn.c

   1 /*
   2  * Copyright (C) 2013 Huawei Ltd.
   3  * Author: Jiang Liu <liuj97@gmail.com>
   4  *
   5  * Copyright (C) 2014 Zi Shen Lim <zlim.lnx@gmail.com>
   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/compiler.h>
  21 #include <linux/kernel.h>
  22 #include <linux/smp.h>
  23 #include <linux/stop_machine.h>
  24 #include <linux/uaccess.h>
  25
  26 #include <asm/cacheflush.h>
  27 #include <asm/debug-monitors.h>
  28 #include <asm/insn.h>
  29
  30 #define AARCH64_INSN_SF_BIT     BIT(31)
  31 #define AARCH64_INSN_N_BIT      BIT(22)
  32
  33 static int aarch64_insn_encoding_class[] = {
  34         AARCH64_INSN_CLS_UNKNOWN,
  35         AARCH64_INSN_CLS_UNKNOWN,
  36         AARCH64_INSN_CLS_UNKNOWN,
  37         AARCH64_INSN_CLS_UNKNOWN,
  38         AARCH64_INSN_CLS_LDST,
  39         AARCH64_INSN_CLS_DP_REG,
  40         AARCH64_INSN_CLS_LDST,
  41         AARCH64_INSN_CLS_DP_FPSIMD,
  42         AARCH64_INSN_CLS_DP_IMM,
  43         AARCH64_INSN_CLS_DP_IMM,
  44         AARCH64_INSN_CLS_BR_SYS,
  45         AARCH64_INSN_CLS_BR_SYS,
  46         AARCH64_INSN_CLS_LDST,
  47         AARCH64_INSN_CLS_DP_REG,
  48         AARCH64_INSN_CLS_LDST,
  49         AARCH64_INSN_CLS_DP_FPSIMD,
  50 };
  51
  52 enum aarch64_insn_encoding_class __kprobes aarch64_get_insn_class(u32 insn)
  53 {
  54         return aarch64_insn_encoding_class[(insn >> 25) & 0xf];
  55 }
  56
  57 /* NOP is an alias of HINT */
  58 bool __kprobes aarch64_insn_is_nop(u32 insn)
  59 {
  60         if (!aarch64_insn_is_hint(insn))
  61                 return false;
  62
  63         switch (insn & 0xFE0) {
  64         case AARCH64_INSN_HINT_YIELD:
  65         case AARCH64_INSN_HINT_WFE:
  66         case AARCH64_INSN_HINT_WFI:
  67         case AARCH64_INSN_HINT_SEV:
  68         case AARCH64_INSN_HINT_SEVL:
  69                 return false;
  70         default:
  71                 return true;
  72         }
  73 }
  74
  75 /*
  76  * In ARMv8-A, A64 instructions have a fixed length of 32 bits and are always
  77  * little-endian.
  78  */
  79 int __kprobes aarch64_insn_read(void *addr, u32 *insnp)
  80 {
  81         int ret;
  82         u32 val;
  83
  84         ret = probe_kernel_read(&val, addr, AARCH64_INSN_SIZE);
  85         if (!ret)
  86                 *insnp = le32_to_cpu(val);
  87
  88         return ret;
  89 }
  90
  91 int __kprobes aarch64_insn_write(void *addr, u32 insn)
  92 {
  93         insn = cpu_to_le32(insn);
  94         return probe_kernel_write(addr, &insn, AARCH64_INSN_SIZE);
  95 }
  96
  97 static bool __kprobes __aarch64_insn_hotpatch_safe(u32 insn)
  98 {
  99         if (aarch64_get_insn_class(insn) != AARCH64_INSN_CLS_BR_SYS)
 100                 return false;
 101
 102         return  aarch64_insn_is_b(insn) ||
 103                 aarch64_insn_is_bl(insn) ||
 104                 aarch64_insn_is_svc(insn) ||
 105                 aarch64_insn_is_hvc(insn) ||
 106                 aarch64_insn_is_smc(insn) ||
 107                 aarch64_insn_is_brk(insn) ||
 108                 aarch64_insn_is_nop(insn);
 109 }
 110
 111 /*
 112  * ARM Architecture Reference Manual for ARMv8 Profile-A, Issue A.a
 113  * Section B2.6.5 "Concurrent modification and execution of instructions":
 114  * Concurrent modification and execution of instructions can lead to the
 115  * resulting instruction performing any behavior that can be achieved by
 116  * executing any sequence of instructions that can be executed from the
 117  * same Exception level, except where the instruction before modification
 118  * and the instruction after modification is a B, BL, NOP, BKPT, SVC, HVC,
 119  * or SMC instruction.
 120  */
 121 bool __kprobes aarch64_insn_hotpatch_safe(u32 old_insn, u32 new_insn)
 122 {
 123         return __aarch64_insn_hotpatch_safe(old_insn) &&
 124                __aarch64_insn_hotpatch_safe(new_insn);
 125 }
 126
 127 int __kprobes aarch64_insn_patch_text_nosync(void *addr, u32 insn)
 128 {
 129         u32 *tp = addr;
 130         int ret;
 131
 132         /* A64 instructions must be word aligned */
 133         if ((uintptr_t)tp & 0x3)
 134                 return -EINVAL;
 135
 136         ret = aarch64_insn_write(tp, insn);
 137         if (ret == 0)
 138                 flush_icache_range((uintptr_t)tp,
 139                                    (uintptr_t)tp + AARCH64_INSN_SIZE);
 140
 141         return ret;
 142 }
 143
 144 struct aarch64_insn_patch {
 145         void            **text_addrs;
 146         u32             *new_insns;
 147         int             insn_cnt;
 148         atomic_t        cpu_count;
 149 };
 150
 151 static int __kprobes aarch64_insn_patch_text_cb(void *arg)
 152 {
 153         int i, ret = 0;
 154         struct aarch64_insn_patch *pp = arg;
 155
 156         /* The first CPU becomes master */
 157         if (atomic_inc_return(&pp->cpu_count) == 1) {
 158                 for (i = 0; ret == 0 && i < pp->insn_cnt; i++)
 159                         ret = aarch64_insn_patch_text_nosync(pp->text_addrs[i],
 160                                                              pp->new_insns[i]);
 161                 /*
 162                  * aarch64_insn_patch_text_nosync() calls flush_icache_range(),
 163                  * which ends with "dsb; isb" pair guaranteeing global
 164                  * visibility.
 165                  */
 166                 atomic_set(&pp->cpu_count, -1);
 167         } else {
 168                 while (atomic_read(&pp->cpu_count) != -1)
 169                         cpu_relax();
 170                 isb();
 171         }
 172
 173         return ret;
 174 }
 175
 176 int __kprobes aarch64_insn_patch_text_sync(void *addrs[], u32 insns[], int cnt)
 177 {
 178         struct aarch64_insn_patch patch = {
 179                 .text_addrs = addrs,
 180                 .new_insns = insns,
 181                 .insn_cnt = cnt,
 182                 .cpu_count = ATOMIC_INIT(0),
 183         };
 184
 185         if (cnt <= 0)
 186                 return -EINVAL;
 187
 188         return stop_machine(aarch64_insn_patch_text_cb, &patch,
 189                             cpu_online_mask);
 190 }
 191
 192 int __kprobes aarch64_insn_patch_text(void *addrs[], u32 insns[], int cnt)
 193 {
 194         int ret;
 195         u32 insn;
 196
 197         /* Unsafe to patch multiple instructions without synchronizaiton */
 198         if (cnt == 1) {
 199                 ret = aarch64_insn_read(addrs[0], &insn);
 200                 if (ret)
 201                         return ret;
 202
 203                 if (aarch64_insn_hotpatch_safe(insn, insns[0])) {
 204                         /*
 205                          * ARMv8 architecture doesn't guarantee all CPUs see
 206                          * the new instruction after returning from function
 207                          * aarch64_insn_patch_text_nosync(). So send IPIs to
 208                          * all other CPUs to achieve instruction
 209                          * synchronization.
 210                          */
 211                         ret = aarch64_insn_patch_text_nosync(addrs[0], insns[0]);
 212                         kick_all_cpus_sync();
 213                         return ret;
 214                 }
 215         }
 216
 217         return aarch64_insn_patch_text_sync(addrs, insns, cnt);
 218 }
 219
 220 u32 __kprobes aarch64_insn_encode_immediate(enum aarch64_insn_imm_type type,
 221                                   u32 insn, u64 imm)
 222 {
 223         u32 immlo, immhi, lomask, himask, mask;
 224         int shift;
 225
 226         switch (type) {
 227         case AARCH64_INSN_IMM_ADR:
 228                 lomask = 0x3;
 229                 himask = 0x7ffff;
 230                 immlo = imm & lomask;
 231                 imm >>= 2;
 232                 immhi = imm & himask;
 233                 imm = (immlo << 24) | (immhi);
 234                 mask = (lomask << 24) | (himask);
 235                 shift = 5;
 236                 break;
 237         case AARCH64_INSN_IMM_26:
 238                 mask = BIT(26) - 1;
 239                 shift = 0;
 240                 break;
 241         case AARCH64_INSN_IMM_19:
 242                 mask = BIT(19) - 1;
 243                 shift = 5;
 244                 break;
 245         case AARCH64_INSN_IMM_16:
 246                 mask = BIT(16) - 1;
 247                 shift = 5;
 248                 break;
 249         case AARCH64_INSN_IMM_14:
 250                 mask = BIT(14) - 1;
 251                 shift = 5;
 252                 break;
 253         case AARCH64_INSN_IMM_12:
 254                 mask = BIT(12) - 1;
 255                 shift = 10;
 256                 break;
 257         case AARCH64_INSN_IMM_9:
 258                 mask = BIT(9) - 1;
 259                 shift = 12;
 260                 break;
 261         case AARCH64_INSN_IMM_7:
 262                 mask = BIT(7) - 1;
 263                 shift = 15;
 264                 break;
 265         case AARCH64_INSN_IMM_6:
 266         case AARCH64_INSN_IMM_S:
 267                 mask = BIT(6) - 1;
 268                 shift = 10;
 269                 break;
 270         case AARCH64_INSN_IMM_R:
 271                 mask = BIT(6) - 1;
 272                 shift = 16;
 273                 break;
 274         default:
 275                 pr_err("aarch64_insn_encode_immediate: unknown immediate encoding %d\n",
 276                         type);
 277                 return 0;
 278         }
 279
 280         /* Update the immediate field. */
 281         insn &= ~(mask << shift);
 282         insn |= (imm & mask) << shift;
 283
 284         return insn;
 285 }
 286
 287 static u32 aarch64_insn_encode_register(enum aarch64_insn_register_type type,
 288                                         u32 insn,
 289                                         enum aarch64_insn_register reg)
 290 {
 291         int shift;
 292
 293         if (reg < AARCH64_INSN_REG_0 || reg > AARCH64_INSN_REG_SP) {
 294                 pr_err("%s: unknown register encoding %d\n", __func__, reg);
 295                 return 0;
 296         }
 297
 298         switch (type) {
 299         case AARCH64_INSN_REGTYPE_RT:
 300         case AARCH64_INSN_REGTYPE_RD:
 301                 shift = 0;
 302                 break;
 303         case AARCH64_INSN_REGTYPE_RN:
 304                 shift = 5;
 305                 break;
 306         case AARCH64_INSN_REGTYPE_RT2:
 307         case AARCH64_INSN_REGTYPE_RA:
 308                 shift = 10;
 309                 break;
 310         case AARCH64_INSN_REGTYPE_RM:
 311                 shift = 16;
 312                 break;
 313         default:
 314                 pr_err("%s: unknown register type encoding %d\n", __func__,
 315                        type);
 316                 return 0;
 317         }
 318
 319         insn &= ~(GENMASK(4, 0) << shift);
 320         insn |= reg << shift;
 321
 322         return insn;
 323 }
 324
 325 static u32 aarch64_insn_encode_ldst_size(enum aarch64_insn_size_type type,
 326                                          u32 insn)
 327 {
 328         u32 size;
 329
 330         switch (type) {
 331         case AARCH64_INSN_SIZE_8:
 332                 size = 0;
 333                 break;
 334         case AARCH64_INSN_SIZE_16:
 335                 size = 1;
 336                 break;
 337         case AARCH64_INSN_SIZE_32:
 338                 size = 2;
 339                 break;
 340         case AARCH64_INSN_SIZE_64:
 341                 size = 3;
 342                 break;
 343         default:
 344                 pr_err("%s: unknown size encoding %d\n", __func__, type);
 345                 return 0;
 346         }
 347
 348         insn &= ~GENMASK(31, 30);
 349         insn |= size << 30;
 350
 351         return insn;
 352 }
 353
 354 static inline long branch_imm_common(unsigned long pc, unsigned long addr,
 355                                      long range)
 356 {
 357         long offset;
 358
 359         /*
 360          * PC: A 64-bit Program Counter holding the address of the current
 361          * instruction. A64 instructions must be word-aligned.
 362          */
 363         BUG_ON((pc & 0x3) || (addr & 0x3));
 364
 365         offset = ((long)addr - (long)pc);
 366         BUG_ON(offset < -range || offset >= range);
 367
 368         return offset;
 369 }
 370
 371 u32 __kprobes aarch64_insn_gen_branch_imm(unsigned long pc, unsigned long addr,
 372                                           enum aarch64_insn_branch_type type)
 373 {
 374         u32 insn;
 375         long offset;
 376
 377         /*
 378          * B/BL support [-128M, 128M) offset
 379          * ARM64 virtual address arrangement guarantees all kernel and module
 380          * texts are within +/-128M.
 381          */
 382         offset = branch_imm_common(pc, addr, SZ_128M);
 383
 384         switch (type) {
 385         case AARCH64_INSN_BRANCH_LINK:
 386                 insn = aarch64_insn_get_bl_value();
 387                 break;
 388         case AARCH64_INSN_BRANCH_NOLINK:
 389                 insn = aarch64_insn_get_b_value();
 390                 break;
 391         default:
 392                 BUG_ON(1);
 393                 return AARCH64_BREAK_FAULT;
 394         }
 395
 396         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_26, insn,
 397                                              offset >> 2);
 398 }
 399
 400 u32 aarch64_insn_gen_comp_branch_imm(unsigned long pc, unsigned long addr,
 401                                      enum aarch64_insn_register reg,
 402                                      enum aarch64_insn_variant variant,
 403                                      enum aarch64_insn_branch_type type)
 404 {
 405         u32 insn;
 406         long offset;
 407
 408         offset = branch_imm_common(pc, addr, SZ_1M);
 409
 410         switch (type) {
 411         case AARCH64_INSN_BRANCH_COMP_ZERO:
 412                 insn = aarch64_insn_get_cbz_value();
 413                 break;
 414         case AARCH64_INSN_BRANCH_COMP_NONZERO:
 415                 insn = aarch64_insn_get_cbnz_value();
 416                 break;
 417         default:
 418                 BUG_ON(1);
 419                 return AARCH64_BREAK_FAULT;
 420         }
 421
 422         switch (variant) {
 423         case AARCH64_INSN_VARIANT_32BIT:
 424                 break;
 425         case AARCH64_INSN_VARIANT_64BIT:
 426                 insn |= AARCH64_INSN_SF_BIT;
 427                 break;
 428         default:
 429                 BUG_ON(1);
 430                 return AARCH64_BREAK_FAULT;
 431         }
 432
 433         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RT, insn, reg);
 434
 435         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_19, insn,
 436                                              offset >> 2);
 437 }
 438
 439 u32 aarch64_insn_gen_cond_branch_imm(unsigned long pc, unsigned long addr,
 440                                      enum aarch64_insn_condition cond)
 441 {
 442         u32 insn;
 443         long offset;
 444
 445         offset = branch_imm_common(pc, addr, SZ_1M);
 446
 447         insn = aarch64_insn_get_bcond_value();
 448
 449         BUG_ON(cond < AARCH64_INSN_COND_EQ || cond > AARCH64_INSN_COND_AL);
 450         insn |= cond;
 451
 452         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_19, insn,
 453                                              offset >> 2);
 454 }
 455
 456 u32 __kprobes aarch64_insn_gen_hint(enum aarch64_insn_hint_op op)
 457 {
 458         return aarch64_insn_get_hint_value() | op;
 459 }
 460
 461 u32 __kprobes aarch64_insn_gen_nop(void)
 462 {
 463         return aarch64_insn_gen_hint(AARCH64_INSN_HINT_NOP);
 464 }
 465
 466 u32 aarch64_insn_gen_branch_reg(enum aarch64_insn_register reg,
 467                                 enum aarch64_insn_branch_type type)
 468 {
 469         u32 insn;
 470
 471         switch (type) {
 472         case AARCH64_INSN_BRANCH_NOLINK:
 473                 insn = aarch64_insn_get_br_value();
 474                 break;
 475         case AARCH64_INSN_BRANCH_LINK:
 476                 insn = aarch64_insn_get_blr_value();
 477                 break;
 478         case AARCH64_INSN_BRANCH_RETURN:
 479                 insn = aarch64_insn_get_ret_value();
 480                 break;
 481         default:
 482                 BUG_ON(1);
 483                 return AARCH64_BREAK_FAULT;
 484         }
 485
 486         return aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, reg);
 487 }
 488
 489 u32 aarch64_insn_gen_load_store_reg(enum aarch64_insn_register reg,
 490                                     enum aarch64_insn_register base,
 491                                     enum aarch64_insn_register offset,
 492                                     enum aarch64_insn_size_type size,
 493                                     enum aarch64_insn_ldst_type type)
 494 {
 495         u32 insn;
 496
 497         switch (type) {
 498         case AARCH64_INSN_LDST_LOAD_REG_OFFSET:
 499                 insn = aarch64_insn_get_ldr_reg_value();
 500                 break;
 501         case AARCH64_INSN_LDST_STORE_REG_OFFSET:
 502                 insn = aarch64_insn_get_str_reg_value();
 503                 break;
 504         default:
 505                 BUG_ON(1);
 506                 return AARCH64_BREAK_FAULT;
 507         }
 508
 509         insn = aarch64_insn_encode_ldst_size(size, insn);
 510
 511         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RT, insn, reg);
 512
 513         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn,
 514                                             base);
 515
 516         return aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RM, insn,
 517                                             offset);
 518 }
 519
 520 u32 aarch64_insn_gen_load_store_pair(enum aarch64_insn_register reg1,
 521                                      enum aarch64_insn_register reg2,
 522                                      enum aarch64_insn_register base,
 523                                      int offset,
 524                                      enum aarch64_insn_variant variant,
 525                                      enum aarch64_insn_ldst_type type)
 526 {
 527         u32 insn;
 528         int shift;
 529
 530         switch (type) {
 531         case AARCH64_INSN_LDST_LOAD_PAIR_PRE_INDEX:
 532                 insn = aarch64_insn_get_ldp_pre_value();
 533                 break;
 534         case AARCH64_INSN_LDST_STORE_PAIR_PRE_INDEX:
 535                 insn = aarch64_insn_get_stp_pre_value();
 536                 break;
 537         case AARCH64_INSN_LDST_LOAD_PAIR_POST_INDEX:
 538                 insn = aarch64_insn_get_ldp_post_value();
 539                 break;
 540         case AARCH64_INSN_LDST_STORE_PAIR_POST_INDEX:
 541                 insn = aarch64_insn_get_stp_post_value();
 542                 break;
 543         default:
 544                 BUG_ON(1);
 545                 return AARCH64_BREAK_FAULT;
 546         }
 547
 548         switch (variant) {
 549         case AARCH64_INSN_VARIANT_32BIT:
 550                 /* offset must be multiples of 4 in the range [-256, 252] */
 551                 BUG_ON(offset & 0x3);
 552                 BUG_ON(offset < -256 || offset > 252);
 553                 shift = 2;
 554                 break;
 555         case AARCH64_INSN_VARIANT_64BIT:
 556                 /* offset must be multiples of 8 in the range [-512, 504] */
 557                 BUG_ON(offset & 0x7);
 558                 BUG_ON(offset < -512 || offset > 504);
 559                 shift = 3;
 560                 insn |= AARCH64_INSN_SF_BIT;
 561                 break;
 562         default:
 563                 BUG_ON(1);
 564                 return AARCH64_BREAK_FAULT;
 565         }
 566
 567         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RT, insn,
 568                                             reg1);
 569
 570         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RT2, insn,
 571                                             reg2);
 572
 573         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn,
 574                                             base);
 575
 576         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_7, insn,
 577                                              offset >> shift);
 578 }
 579
 580 u32 aarch64_insn_gen_add_sub_imm(enum aarch64_insn_register dst,
 581                                  enum aarch64_insn_register src,
 582                                  int imm, enum aarch64_insn_variant variant,
 583                                  enum aarch64_insn_adsb_type type)
 584 {
 585         u32 insn;
 586
 587         switch (type) {
 588         case AARCH64_INSN_ADSB_ADD:
 589                 insn = aarch64_insn_get_add_imm_value();
 590                 break;
 591         case AARCH64_INSN_ADSB_SUB:
 592                 insn = aarch64_insn_get_sub_imm_value();
 593                 break;
 594         case AARCH64_INSN_ADSB_ADD_SETFLAGS:
 595                 insn = aarch64_insn_get_adds_imm_value();
 596                 break;
 597         case AARCH64_INSN_ADSB_SUB_SETFLAGS:
 598                 insn = aarch64_insn_get_subs_imm_value();
 599                 break;
 600         default:
 601                 BUG_ON(1);
 602                 return AARCH64_BREAK_FAULT;
 603         }
 604
 605         switch (variant) {
 606         case AARCH64_INSN_VARIANT_32BIT:
 607                 break;
 608         case AARCH64_INSN_VARIANT_64BIT:
 609                 insn |= AARCH64_INSN_SF_BIT;
 610                 break;
 611         default:
 612                 BUG_ON(1);
 613                 return AARCH64_BREAK_FAULT;
 614         }
 615
 616         BUG_ON(imm & ~(SZ_4K - 1));
 617
 618         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
 619
 620         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, src);
 621
 622         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_12, insn, imm);
 623 }
 624
 625 u32 aarch64_insn_gen_bitfield(enum aarch64_insn_register dst,
 626                               enum aarch64_insn_register src,
 627                               int immr, int imms,
 628                               enum aarch64_insn_variant variant,
 629                               enum aarch64_insn_bitfield_type type)
 630 {
 631         u32 insn;
 632         u32 mask;
 633
 634         switch (type) {
 635         case AARCH64_INSN_BITFIELD_MOVE:
 636                 insn = aarch64_insn_get_bfm_value();
 637                 break;
 638         case AARCH64_INSN_BITFIELD_MOVE_UNSIGNED:
 639                 insn = aarch64_insn_get_ubfm_value();
 640                 break;
 641         case AARCH64_INSN_BITFIELD_MOVE_SIGNED:
 642                 insn = aarch64_insn_get_sbfm_value();
 643                 break;
 644         default:
 645                 BUG_ON(1);
 646                 return AARCH64_BREAK_FAULT;
 647         }
 648
 649         switch (variant) {
 650         case AARCH64_INSN_VARIANT_32BIT:
 651                 mask = GENMASK(4, 0);
 652                 break;
 653         case AARCH64_INSN_VARIANT_64BIT:
 654                 insn |= AARCH64_INSN_SF_BIT | AARCH64_INSN_N_BIT;
 655                 mask = GENMASK(5, 0);
 656                 break;
 657         default:
 658                 BUG_ON(1);
 659                 return AARCH64_BREAK_FAULT;
 660         }
 661
 662         BUG_ON(immr & ~mask);
 663         BUG_ON(imms & ~mask);
 664
 665         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
 666
 667         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, src);
 668
 669         insn = aarch64_insn_encode_immediate(AARCH64_INSN_IMM_R, insn, immr);
 670
 671         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_S, insn, imms);
 672 }
 673
 674 u32 aarch64_insn_gen_movewide(enum aarch64_insn_register dst,
 675                               int imm, int shift,
 676                               enum aarch64_insn_variant variant,
 677                               enum aarch64_insn_movewide_type type)
 678 {
 679         u32 insn;
 680
 681         switch (type) {
 682         case AARCH64_INSN_MOVEWIDE_ZERO:
 683                 insn = aarch64_insn_get_movz_value();
 684                 break;
 685         case AARCH64_INSN_MOVEWIDE_KEEP:
 686                 insn = aarch64_insn_get_movk_value();
 687                 break;
 688         case AARCH64_INSN_MOVEWIDE_INVERSE:
 689                 insn = aarch64_insn_get_movn_value();
 690                 break;
 691         default:
 692                 BUG_ON(1);
 693                 return AARCH64_BREAK_FAULT;
 694         }
 695
 696         BUG_ON(imm & ~(SZ_64K - 1));
 697
 698         switch (variant) {
 699         case AARCH64_INSN_VARIANT_32BIT:
 700                 BUG_ON(shift != 0 && shift != 16);
 701                 break;
 702         case AARCH64_INSN_VARIANT_64BIT:
 703                 insn |= AARCH64_INSN_SF_BIT;
 704                 BUG_ON(shift != 0 && shift != 16 && shift != 32 &&
 705                        shift != 48);
 706                 break;
 707         default:
 708                 BUG_ON(1);
 709                 return AARCH64_BREAK_FAULT;
 710         }
 711
 712         insn |= (shift >> 4) << 21;
 713
 714         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
 715
 716         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_16, insn, imm);
 717 }
 718
 719 u32 aarch64_insn_gen_add_sub_shifted_reg(enum aarch64_insn_register dst,
 720                                          enum aarch64_insn_register src,
 721                                          enum aarch64_insn_register reg,
 722                                          int shift,
 723                                          enum aarch64_insn_variant variant,
 724                                          enum aarch64_insn_adsb_type type)
 725 {
 726         u32 insn;
 727
 728         switch (type) {
 729         case AARCH64_INSN_ADSB_ADD:
 730                 insn = aarch64_insn_get_add_value();
 731                 break;
 732         case AARCH64_INSN_ADSB_SUB:
 733                 insn = aarch64_insn_get_sub_value();
 734                 break;
 735         case AARCH64_INSN_ADSB_ADD_SETFLAGS:
 736                 insn = aarch64_insn_get_adds_value();
 737                 break;
 738         case AARCH64_INSN_ADSB_SUB_SETFLAGS:
 739                 insn = aarch64_insn_get_subs_value();
 740                 break;
 741         default:
 742                 BUG_ON(1);
 743                 return AARCH64_BREAK_FAULT;
 744         }
 745
 746         switch (variant) {
 747         case AARCH64_INSN_VARIANT_32BIT:
 748                 BUG_ON(shift & ~(SZ_32 - 1));
 749                 break;
 750         case AARCH64_INSN_VARIANT_64BIT:
 751                 insn |= AARCH64_INSN_SF_BIT;
 752                 BUG_ON(shift & ~(SZ_64 - 1));
 753                 break;
 754         default:
 755                 BUG_ON(1);
 756                 return AARCH64_BREAK_FAULT;
 757         }
 758
 759
 760         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
 761
 762         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, src);
 763
 764         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RM, insn, reg);
 765
 766         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_6, insn, shift);
 767 }
 768
 769 u32 aarch64_insn_gen_data1(enum aarch64_insn_register dst,
 770                            enum aarch64_insn_register src,
 771                            enum aarch64_insn_variant variant,
 772                            enum aarch64_insn_data1_type type)
 773 {
 774         u32 insn;
 775
 776         switch (type) {
 777         case AARCH64_INSN_DATA1_REVERSE_16:
 778                 insn = aarch64_insn_get_rev16_value();
 779                 break;
 780         case AARCH64_INSN_DATA1_REVERSE_32:
 781                 insn = aarch64_insn_get_rev32_value();
 782                 break;
 783         case AARCH64_INSN_DATA1_REVERSE_64:
 784                 BUG_ON(variant != AARCH64_INSN_VARIANT_64BIT);
 785                 insn = aarch64_insn_get_rev64_value();
 786                 break;
 787         default:
 788                 BUG_ON(1);
 789                 return AARCH64_BREAK_FAULT;
 790         }
 791
 792         switch (variant) {
 793         case AARCH64_INSN_VARIANT_32BIT:
 794                 break;
 795         case AARCH64_INSN_VARIANT_64BIT:
 796                 insn |= AARCH64_INSN_SF_BIT;
 797                 break;
 798         default:
 799                 BUG_ON(1);
 800                 return AARCH64_BREAK_FAULT;
 801         }
 802
 803         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
 804
 805         return aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, src);
 806 }
 807
 808 u32 aarch64_insn_gen_data2(enum aarch64_insn_register dst,
 809                            enum aarch64_insn_register src,
 810                            enum aarch64_insn_register reg,
 811                            enum aarch64_insn_variant variant,
 812                            enum aarch64_insn_data2_type type)
 813 {
 814         u32 insn;
 815
 816         switch (type) {
 817         case AARCH64_INSN_DATA2_UDIV:
 818                 insn = aarch64_insn_get_udiv_value();
 819                 break;
 820         case AARCH64_INSN_DATA2_SDIV:
 821                 insn = aarch64_insn_get_sdiv_value();
 822                 break;
 823         case AARCH64_INSN_DATA2_LSLV:
 824                 insn = aarch64_insn_get_lslv_value();
 825                 break;
 826         case AARCH64_INSN_DATA2_LSRV:
 827                 insn = aarch64_insn_get_lsrv_value();
 828                 break;
 829         case AARCH64_INSN_DATA2_ASRV:
 830                 insn = aarch64_insn_get_asrv_value();
 831                 break;
 832         case AARCH64_INSN_DATA2_RORV:
 833                 insn = aarch64_insn_get_rorv_value();
 834                 break;
 835         default:
 836                 BUG_ON(1);
 837                 return AARCH64_BREAK_FAULT;
 838         }
 839
 840         switch (variant) {
 841         case AARCH64_INSN_VARIANT_32BIT:
 842                 break;
 843         case AARCH64_INSN_VARIANT_64BIT:
 844                 insn |= AARCH64_INSN_SF_BIT;
 845                 break;
 846         default:
 847                 BUG_ON(1);
 848                 return AARCH64_BREAK_FAULT;
 849         }
 850
 851         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
 852
 853         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, src);
 854
 855         return aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RM, insn, reg);
 856 }
 857
 858 u32 aarch64_insn_gen_data3(enum aarch64_insn_register dst,
 859                            enum aarch64_insn_register src,
 860                            enum aarch64_insn_register reg1,
 861                            enum aarch64_insn_register reg2,
 862                            enum aarch64_insn_variant variant,
 863                            enum aarch64_insn_data3_type type)
 864 {
 865         u32 insn;
 866
 867         switch (type) {
 868         case AARCH64_INSN_DATA3_MADD:
 869                 insn = aarch64_insn_get_madd_value();
 870                 break;
 871         case AARCH64_INSN_DATA3_MSUB:
 872                 insn = aarch64_insn_get_msub_value();
 873                 break;
 874         default:
 875                 BUG_ON(1);
 876                 return AARCH64_BREAK_FAULT;
 877         }
 878
 879         switch (variant) {
 880         case AARCH64_INSN_VARIANT_32BIT:
 881                 break;
 882         case AARCH64_INSN_VARIANT_64BIT:
 883                 insn |= AARCH64_INSN_SF_BIT;
 884                 break;
 885         default:
 886                 BUG_ON(1);
 887                 return AARCH64_BREAK_FAULT;
 888         }
 889
 890         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
 891
 892         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RA, insn, src);
 893
 894         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn,
 895                                             reg1);
 896
 897         return aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RM, insn,
 898                                             reg2);
 899 }
 900
 901 u32 aarch64_insn_gen_logical_shifted_reg(enum aarch64_insn_register dst,
 902                                          enum aarch64_insn_register src,
 903                                          enum aarch64_insn_register reg,
 904                                          int shift,
 905                                          enum aarch64_insn_variant variant,
 906                                          enum aarch64_insn_logic_type type)
 907 {
 908         u32 insn;
 909
 910         switch (type) {
 911         case AARCH64_INSN_LOGIC_AND:
 912                 insn = aarch64_insn_get_and_value();
 913                 break;
 914         case AARCH64_INSN_LOGIC_BIC:
 915                 insn = aarch64_insn_get_bic_value();
 916                 break;
 917         case AARCH64_INSN_LOGIC_ORR:
 918                 insn = aarch64_insn_get_orr_value();
 919                 break;
 920         case AARCH64_INSN_LOGIC_ORN:
 921                 insn = aarch64_insn_get_orn_value();
 922                 break;
 923         case AARCH64_INSN_LOGIC_EOR:
 924                 insn = aarch64_insn_get_eor_value();
 925                 break;
 926         case AARCH64_INSN_LOGIC_EON:
 927                 insn = aarch64_insn_get_eon_value();
 928                 break;
 929         case AARCH64_INSN_LOGIC_AND_SETFLAGS:
 930                 insn = aarch64_insn_get_ands_value();
 931                 break;
 932         case AARCH64_INSN_LOGIC_BIC_SETFLAGS:
 933                 insn = aarch64_insn_get_bics_value();
 934                 break;
 935         default:
 936                 BUG_ON(1);
 937                 return AARCH64_BREAK_FAULT;
 938         }
 939
 940         switch (variant) {
 941         case AARCH64_INSN_VARIANT_32BIT:
 942                 BUG_ON(shift & ~(SZ_32 - 1));
 943                 break;
 944         case AARCH64_INSN_VARIANT_64BIT:
 945                 insn |= AARCH64_INSN_SF_BIT;
 946                 BUG_ON(shift & ~(SZ_64 - 1));
 947                 break;
 948         default:
 949                 BUG_ON(1);
 950                 return AARCH64_BREAK_FAULT;
 951         }
 952
 953
 954         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RD, insn, dst);
 955
 956         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn, src);
 957
 958         insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RM, insn, reg);
 959
 960         return aarch64_insn_encode_immediate(AARCH64_INSN_IMM_6, insn, shift);
 961 }
 962
 963 bool aarch32_insn_is_wide(u32 insn)
 964 {
 965         return insn >= 0xe800;
 966 }
 967
 968 /*
 969  * Macros/defines for extracting register numbers from instruction.
 970  */
 971 u32 aarch32_insn_extract_reg_num(u32 insn, int offset)
 972 {
 973         return (insn & (0xf << offset)) >> offset;
 974 }
 975
 976 #define OPC2_MASK       0x7
 977 #define OPC2_OFFSET     5
 978 u32 aarch32_insn_mcr_extract_opc2(u32 insn)
 979 {
 980         return (insn & (OPC2_MASK << OPC2_OFFSET)) >> OPC2_OFFSET;
 981 }
 982
 983 #define CRM_MASK        0xf
 984 u32 aarch32_insn_mcr_extract_crm(u32 insn)
 985 {
 986         return insn & CRM_MASK;
 987 }