1 files changed, 87 insertions, 1 deletions

diff --git a/arch/loongarch/kernel/inst.c b/arch/loongarch/kernel/inst.c
index 3050329556d1..bf037f0c6b26 100644
--- a/arch/loongarch/kernel/inst.c
+++ b/arch/loongarch/kernel/inst.c
@@ -4,6 +4,8 @@
  */
 #include <linux/sizes.h>
 #include <linux/uaccess.h>
+#include <linux/set_memory.h>
+#include <linux/stop_machine.h>
 
 #include <asm/cacheflush.h>
 #include <asm/inst.h>
@@ -139,6 +141,9 @@ bool insns_not_supported(union loongarch_instruction insn)
 	case amswapw_op ... ammindbdu_op:
 		pr_notice("atomic memory access instructions are not supported\n");
 		return true;
+	case scq_op:
+		pr_notice("sc.q instruction is not supported\n");
+		return true;
 	}
 
 	switch (insn.reg2i14_format.opcode) {
@@ -150,6 +155,15 @@ bool insns_not_supported(union loongarch_instruction insn)
 		return true;
 	}
 
+	switch (insn.reg2_format.opcode) {
+	case llacqw_op:
+	case llacqd_op:
+	case screlw_op:
+	case screld_op:
+		pr_notice("llacq and screl instructions are not supported\n");
+		return true;
+	}
+
 	switch (insn.reg1i21_format.opcode) {
 	case bceqz_op:
 		pr_notice("bceqz and bcnez instructions are not supported\n");
@@ -218,6 +232,50 @@ int larch_insn_patch_text(void *addr, u32 insn)
 	return ret;
 }
 
+struct insn_copy {
+	void *dst;
+	void *src;
+	size_t len;
+	unsigned int cpu;
+};
+
+static int text_copy_cb(void *data)
+{
+	int ret = 0;
+	struct insn_copy *copy = data;
+
+	if (smp_processor_id() == copy->cpu) {
+		ret = copy_to_kernel_nofault(copy->dst, copy->src, copy->len);
+		if (ret)
+			pr_err("%s: operation failed\n", __func__);
+	}
+
+	flush_icache_range((unsigned long)copy->dst, (unsigned long)copy->dst + copy->len);
+
+	return ret;
+}
+
+int larch_insn_text_copy(void *dst, void *src, size_t len)
+{
+	int ret = 0;
+	size_t start, end;
+	struct insn_copy copy = {
+		.dst = dst,
+		.src = src,
+		.len = len,
+		.cpu = smp_processor_id(),
+	};
+
+	start = round_down((size_t)dst, PAGE_SIZE);
+	end   = round_up((size_t)dst + len, PAGE_SIZE);
+
+	set_memory_rw(start, (end - start) / PAGE_SIZE);
+	ret = stop_machine(text_copy_cb, &copy, cpu_online_mask);
+	set_memory_rox(start, (end - start) / PAGE_SIZE);
+
+	return ret;
+}
+
 u32 larch_insn_gen_nop(void)
 {
 	return INSN_NOP;
@@ -323,6 +381,34 @@ u32 larch_insn_gen_lu52id(enum loongarch_gpr rd, enum loongarch_gpr rj, int imm)
 	return insn.word;
 }
 
+u32 larch_insn_gen_beq(enum loongarch_gpr rd, enum loongarch_gpr rj, int imm)
+{
+	union loongarch_instruction insn;
+
+	if ((imm & 3) || imm < -SZ_128K || imm >= SZ_128K) {
+		pr_warn("The generated beq instruction is out of range.\n");
+		return INSN_BREAK;
+	}
+
+	emit_beq(&insn, rj, rd, imm >> 2);
+
+	return insn.word;
+}
+
+u32 larch_insn_gen_bne(enum loongarch_gpr rd, enum loongarch_gpr rj, int imm)
+{
+	union loongarch_instruction insn;
+
+	if ((imm & 3) || imm < -SZ_128K || imm >= SZ_128K) {
+		pr_warn("The generated bne instruction is out of range.\n");
+		return INSN_BREAK;
+	}
+
+	emit_bne(&insn, rj, rd, imm >> 2);
+
+	return insn.word;
+}
+
 u32 larch_insn_gen_jirl(enum loongarch_gpr rd, enum loongarch_gpr rj, int imm)
 {
 	union loongarch_instruction insn;
@@ -332,7 +418,7 @@ u32 larch_insn_gen_jirl(enum loongarch_gpr rd, enum loongarch_gpr rj, int imm)
 		return INSN_BREAK;
 	}
 
-	emit_jirl(&insn, rj, rd, imm >> 2);
+	emit_jirl(&insn, rd, rj, imm >> 2);
 
 	return insn.word;
 }