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