[PATCH v5 12/12] RISC-V: fix auipc-jalr addresses in patched alternatives

[Heiko Stuebner]

From: Heiko Stuebner <heiko.stuebner at vrull.eu>

Alternatives live in a different section, so addresses used by call
functions will point to wrong locations after the patch got applied.

Similar to arm64, adjust the location to consider that offset.

Reviewed-by: Conor Dooley <conor.dooley at microchip.com>
Reviewed-by: Andrew Jones <ajones at ventanamicro.com>
Reviewed-by: Lad Prabhakar <prabhakar.mahadev-lad.rj at bp.renesas.com>
Tested-by: Lad Prabhakar <prabhakar.mahadev-lad.rj at bp.renesas.com>
Signed-off-by: Heiko Stuebner <heiko.stuebner at vrull.eu>
---
 arch/riscv/include/asm/alternative.h |  3 ++
 arch/riscv/kernel/alternative.c      | 56 ++++++++++++++++++++++++++++
 arch/riscv/kernel/cpufeature.c       |  5 ++-
 3 files changed, 63 insertions(+), 1 deletion(-)

diff --git a/arch/riscv/include/asm/alternative.h b/arch/riscv/include/asm/alternative.h
index 6511dd73e812..1bd4027d34ca 100644
--- a/arch/riscv/include/asm/alternative.h
+++ b/arch/riscv/include/asm/alternative.h
@@ -27,6 +27,9 @@ void __init apply_boot_alternatives(void);
 void __init apply_early_boot_alternatives(void);
 void apply_module_alternatives(void *start, size_t length);
 
+void riscv_alternative_fix_offsets(void *alt_ptr, unsigned int len,
+				   int patch_offset);
+
 struct alt_entry {
 	void *old_ptr;		 /* address of original instruciton or data  */
 	void *alt_ptr;		 /* address of replacement instruction or data */
diff --git a/arch/riscv/kernel/alternative.c b/arch/riscv/kernel/alternative.c
index a7d26a00beea..6212ea0eed72 100644
--- a/arch/riscv/kernel/alternative.c
+++ b/arch/riscv/kernel/alternative.c
@@ -15,6 +15,8 @@
 #include <asm/vendorid_list.h>
 #include <asm/sbi.h>
 #include <asm/csr.h>
+#include <asm/insn.h>
+#include <asm/patch.h>
 
 struct cpu_manufacturer_info_t {
 	unsigned long vendor_id;
@@ -53,6 +55,60 @@ static void __init_or_module riscv_fill_cpu_mfr_info(struct cpu_manufacturer_inf
 	}
 }
 
+static u32 riscv_instruction_at(void *p)
+{
+	u16 *parcel = p;
+
+	return (u32)parcel[0] | (u32)parcel[1] << 16;
+}
+
+static void riscv_alternative_fix_auipc_jalr(void *ptr, u32 auipc_insn,
+					     u32 jalr_insn, int patch_offset)
+{
+	u32 call[2] = { auipc_insn, jalr_insn };
+	s32 imm;
+
+	/* get and adjust new target address */
+	imm = riscv_insn_extract_utype_itype_imm(auipc_insn, jalr_insn);
+	imm -= patch_offset;
+
+	/* update instructions */
+	riscv_insn_insert_utype_itype_imm(&call[0], &call[1], imm);
+
+	/* patch the call place again */
+	patch_text_nosync(ptr, call, sizeof(u32) * 2);
+}
+
+void riscv_alternative_fix_offsets(void *alt_ptr, unsigned int len,
+				      int patch_offset)
+{
+	int num_insn = len / sizeof(u32);
+	int i;
+
+	for (i = 0; i < num_insn; i++) {
+		u32 insn = riscv_instruction_at(alt_ptr + i * sizeof(u32));
+
+		/*
+		 * May be the start of an auipc + jalr pair
+		 * Needs to check that at least one more instruction
+		 * is in the list.
+		 */
+		if (riscv_insn_is_auipc(insn) && i < num_insn - 1) {
+			u32 insn2 = riscv_instruction_at(alt_ptr + (i + 1) * sizeof(u32));
+
+			if (!riscv_insn_is_jalr(insn2))
+				continue;
+
+			/* if instruction pair is a call, it will use the ra register */
+			if (RV_EXTRACT_RD_REG(insn) != 1)
+				continue;
+
+			riscv_alternative_fix_auipc_jalr(alt_ptr + i * sizeof(u32),
+							 insn, insn2, patch_offset);
+		}
+	}
+}
+
 /*
  * This is called very early in the boot process (directly after we run
  * a feature detect on the boot CPU). No need to worry about other CPUs
diff --git a/arch/riscv/kernel/cpufeature.c b/arch/riscv/kernel/cpufeature.c
index 93e45560af30..205bbd6b1fce 100644
--- a/arch/riscv/kernel/cpufeature.c
+++ b/arch/riscv/kernel/cpufeature.c
@@ -339,8 +339,11 @@ void __init_or_module riscv_cpufeature_patch_func(struct alt_entry *begin,
 		}
 
 		tmp = (1U << alt->errata_id);
-		if (cpu_req_feature & tmp)
+		if (cpu_req_feature & tmp) {
 			patch_text_nosync(alt->old_ptr, alt->alt_ptr, alt->alt_len);
+			riscv_alternative_fix_offsets(alt->old_ptr, alt->alt_len,
+						      alt->old_ptr - alt->alt_ptr);
+		}
 	}
 }
 #endif
-- 
2.35.1