5 files changed, 271 insertions, 5 deletions

diff --git a/arch/arm64/include/asm/insn.h b/arch/arm64/include/asm/insn.h
index aecc07e09a18..29cb2ca756f6 100644
--- a/arch/arm64/include/asm/insn.h
+++ b/arch/arm64/include/asm/insn.h
@@ -80,6 +80,7 @@ enum aarch64_insn_register_type {
         AARCH64_INSN_REGTYPE_RM,
         AARCH64_INSN_REGTYPE_RD,
         AARCH64_INSN_REGTYPE_RA,
+        AARCH64_INSN_REGTYPE_RS,
 };
 
 enum aarch64_insn_register {
@@ -188,6 +189,8 @@ enum aarch64_insn_ldst_type {
         AARCH64_INSN_LDST_STORE_PAIR_PRE_INDEX,
         AARCH64_INSN_LDST_LOAD_PAIR_POST_INDEX,
         AARCH64_INSN_LDST_STORE_PAIR_POST_INDEX,
+        AARCH64_INSN_LDST_LOAD_EX,
+        AARCH64_INSN_LDST_STORE_EX,
 };
 
 enum aarch64_insn_adsb_type {
@@ -240,6 +243,23 @@ enum aarch64_insn_logic_type {
         AARCH64_INSN_LOGIC_BIC_SETFLAGS
 };
 
+enum aarch64_insn_prfm_type {
+        AARCH64_INSN_PRFM_TYPE_PLD,
+        AARCH64_INSN_PRFM_TYPE_PLI,
+        AARCH64_INSN_PRFM_TYPE_PST,
+};
+
+enum aarch64_insn_prfm_target {
+        AARCH64_INSN_PRFM_TARGET_L1,
+        AARCH64_INSN_PRFM_TARGET_L2,
+        AARCH64_INSN_PRFM_TARGET_L3,
+};
+
+enum aarch64_insn_prfm_policy {
+        AARCH64_INSN_PRFM_POLICY_KEEP,
+        AARCH64_INSN_PRFM_POLICY_STRM,
+};
+
 #define __AARCH64_INSN_FUNCS(abbr, mask, val)   \
 static __always_inline bool aarch64_insn_is_##abbr(u32 code) \
 { return (code & (mask)) == (val); } \
@@ -248,6 +268,7 @@ static __always_inline u32 aarch64_insn_get_##abbr##_value(void) \
 
 __AARCH64_INSN_FUNCS(adr,       0x9F000000, 0x10000000)
 __AARCH64_INSN_FUNCS(adrp,      0x9F000000, 0x90000000)
+__AARCH64_INSN_FUNCS(prfm,      0x3FC00000, 0x39800000)
 __AARCH64_INSN_FUNCS(prfm_lit,  0xFF000000, 0xD8000000)
 __AARCH64_INSN_FUNCS(str_reg,   0x3FE0EC00, 0x38206800)
 __AARCH64_INSN_FUNCS(ldr_reg,   0x3FE0EC00, 0x38606800)
@@ -357,6 +378,11 @@ u32 aarch64_insn_gen_load_store_pair(enum aarch64_insn_register reg1,
                                      int offset,
                                      enum aarch64_insn_variant variant,
                                      enum aarch64_insn_ldst_type type);
+u32 aarch64_insn_gen_load_store_ex(enum aarch64_insn_register reg,
+                                   enum aarch64_insn_register base,
+                                   enum aarch64_insn_register state,
+                                   enum aarch64_insn_size_type size,
+                                   enum aarch64_insn_ldst_type type);
 u32 aarch64_insn_gen_add_sub_imm(enum aarch64_insn_register dst,
                                  enum aarch64_insn_register src,
                                  int imm, enum aarch64_insn_variant variant,
@@ -397,6 +423,10 @@ u32 aarch64_insn_gen_logical_shifted_reg(enum aarch64_insn_register dst,
                                          int shift,
                                          enum aarch64_insn_variant variant,
                                          enum aarch64_insn_logic_type type);
+u32 aarch64_insn_gen_prefetch(enum aarch64_insn_register base,
+                              enum aarch64_insn_prfm_type type,
+                              enum aarch64_insn_prfm_target target,
+                              enum aarch64_insn_prfm_policy policy);
 s32 aarch64_get_branch_offset(u32 insn);
 u32 aarch64_set_branch_offset(u32 insn, s32 offset);
 
diff --git a/arch/arm64/kernel/insn.c b/arch/arm64/kernel/insn.c
index 3a63954a8b14..b884a926a632 100644
--- a/arch/arm64/kernel/insn.c
+++ b/arch/arm64/kernel/insn.c
@@ -474,6 +474,7 @@ static u32 aarch64_insn_encode_register(enum aarch64_insn_register_type type,
                 shift = 10;
                 break;
         case AARCH64_INSN_REGTYPE_RM:
+        case AARCH64_INSN_REGTYPE_RS:
                 shift = 16;
                 break;
         default:
@@ -757,6 +758,111 @@ u32 aarch64_insn_gen_load_store_pair(enum aarch64_insn_register reg1,
                                              offset >> shift);
 }
 
+u32 aarch64_insn_gen_load_store_ex(enum aarch64_insn_register reg,
+                                   enum aarch64_insn_register base,
+                                   enum aarch64_insn_register state,
+                                   enum aarch64_insn_size_type size,
+                                   enum aarch64_insn_ldst_type type)
+{
+        u32 insn;
+
+        switch (type) {
+        case AARCH64_INSN_LDST_LOAD_EX:
+                insn = aarch64_insn_get_load_ex_value();
+                break;
+        case AARCH64_INSN_LDST_STORE_EX:
+                insn = aarch64_insn_get_store_ex_value();
+                break;
+        default:
+                pr_err("%s: unknown load/store exclusive encoding %d\n", __func__, type);
+                return AARCH64_BREAK_FAULT;
+        }
+
+        insn = aarch64_insn_encode_ldst_size(size, insn);
+
+        insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RT, insn,
+                                            reg);
+
+        insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RN, insn,
+                                            base);
+
+        insn = aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RT2, insn,
+                                            AARCH64_INSN_REG_ZR);
+
+        return aarch64_insn_encode_register(AARCH64_INSN_REGTYPE_RS, insn,
+                                            state);
+}
+
+static u32 aarch64_insn_encode_prfm_imm(enum aarch64_insn_prfm_type type,
+                                        enum aarch64_insn_prfm_target target,
+                                        enum aarch64_insn_prfm_policy policy,
+                                        u32 insn)
+{
+        u32 imm_type = 0, imm_target = 0, imm_policy = 0;
+
+        switch (type) {
+        case AARCH64_INSN_PRFM_TYPE_PLD:
+                break;
+        case AARCH64_INSN_PRFM_TYPE_PLI:
+                imm_type = BIT(0);
+                break;
+        case AARCH64_INSN_PRFM_TYPE_PST:
+                imm_type = BIT(1);
+                break;
+        default:
+                pr_err("%s: unknown prfm type encoding %d\n", __func__, type);
+                return AARCH64_BREAK_FAULT;
+        }
+
+        switch (target) {
+        case AARCH64_INSN_PRFM_TARGET_L1:
+                break;
+        case AARCH64_INSN_PRFM_TARGET_L2:
+                imm_target = BIT(0);
+                break;
+        case AARCH64_INSN_PRFM_TARGET_L3:
+                imm_target = BIT(1);
+                break;
+        default:
+                pr_err("%s: unknown prfm target encoding %d\n", __func__, target);
+                return AARCH64_BREAK_FAULT;
+        }
+
+        switch (policy) {
+        case AARCH64_INSN_PRFM_POLICY_KEEP:
+                break;
+        case AARCH64_INSN_PRFM_POLICY_STRM:
+                imm_policy = BIT(0);
+                break;
+        default:
+                pr_err("%s: unknown prfm policy encoding %d\n", __func__, policy);
+                return AARCH64_BREAK_FAULT;
+        }
+
+        /* In this case, imm5 is encoded into Rt field. */
+        insn &= ~GENMASK(4, 0);
+        insn |= imm_policy | (imm_target << 1) | (imm_type << 3);
+
+        return insn;
+}
+
+u32 aarch64_insn_gen_prefetch(enum aarch64_insn_register base,
+                              enum aarch64_insn_prfm_type type,
+                              enum aarch64_insn_prfm_target target,
+                              enum aarch64_insn_prfm_policy policy)
+{
+        u32 insn = aarch64_insn_get_prfm_value();
+
+        insn = aarch64_insn_encode_ldst_size(AARCH64_INSN_SIZE_64, insn);
+
+        insn = aarch64_insn_encode_prfm_imm(type, target, policy, insn);
+