From 1306acc017d6d84688ad00ce2a3717ac38ce685b Mon Sep 17 00:00:00 2001
From: liankong <xhsw.new@qq.com>
Date: Sun, 30 Mar 2025 02:02:53 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 kernel/kpm/kpm.c | 39 +++++++++++++++++++++++++++++----------
 1 file changed, 29 insertions(+), 10 deletions(-)

diff --git a/kernel/kpm/kpm.c b/kernel/kpm/kpm.c
index 997a290d..73545905 100644
--- a/kernel/kpm/kpm.c
+++ b/kernel/kpm/kpm.c
@@ -406,33 +406,52 @@ typedef enum {
     RELOC_OP_PAGE
 } reloc_op_t;
 
-// 移植自内核 arch/arm64/kernel/insn.c
+typedef enum {
+    AARCH64_INSN_IMM_16,     // 16-bit 立即数（MOVZ/MOVK）
+    AARCH64_INSN_IMM_26,     // 26-bit 跳转偏移（B/BL）
+    AARCH64_INSN_IMM_ADR,    // 21-bit 页偏移（ADR/ADRP）
+    AARCH64_INSN_IMM_19      // 19-bit 条件跳转
+} aarch64_insn_imm_type;
+
+/* 编码立即数到指令 */
+static u32 aarch64_insn_encode_immediate(u32 insn, s64 imm, int shift, int bits)
+{
+    u32 mask = (BIT(bits) - 1) << shift;
+    return (insn & ~mask) | ((imm & (BIT(bits) - 1)) << shift);
+}
+
+/* 修补指令中的立即数字段 */
 int aarch64_insn_patch_imm(void *addr, aarch64_insn_imm_type type, s64 imm)
 {
     u32 insn = le32_to_cpu(*(u32 *)addr);
     u32 new_insn;
 
     switch (type) {
-    case AARCH64_INSN_IMM_16:       // MOVZ/MOVK 的 16-bit 立即数
-        new_insn = aarch64_insn_encode_immediate(type, insn, imm);
+    case AARCH64_INSN_IMM_16:
+        /* MOVZ/MOVK: imm[15:0] → shift=5, bits=16 */
+        new_insn = aarch64_insn_encode_immediate(insn, imm, 5, 16);
         break;
-    case AARCH64_INSN_IMM_26:        // B/BL 的 26-bit 偏移
-        new_insn = aarch64_insn_encode_offset(insn, imm, 26);
+    case AARCH64_INSN_IMM_26:
+        /* B/BL: offset[25:0] → shift=0, bits=26 */
+        new_insn = aarch64_insn_encode_immediate(insn, imm, 0, 26);
         break;
-    case AARCH64_INSN_IMM_ADR:       // ADR 的 21-bit 页偏移
-        new_insn = aarch64_insn_encode_offset(insn, imm, 21);
+    case AARCH64_INSN_IMM_ADR:
+        /* ADR/ADRP: imm[20:0] → shift=5, bits=21 */
+        new_insn = aarch64_insn_encode_immediate(insn, imm, 5, 21);
         break;
-    case AARCH64_INSN_IMM_19:        // 条件跳转的 19-bit 偏移
-        new_insn = aarch64_insn_encode_offset(insn, imm, 19);
+    case AARCH64_INSN_IMM_19:
+        /* 条件跳转: offset[18:0] → shift=5, bits=19 */
+        new_insn = aarch64_insn_encode_immediate(insn, imm, 5, 19);
         break;
     default:
         return -EINVAL;
     }
 
+    /* 写入新指令并刷新缓存 */
     *(u32 *)addr = cpu_to_le32(new_insn);
+    flush_icache_range((unsigned long)addr, (unsigned long)addr + 4);
     return 0;
 }
-
 /* 指令编码辅助函数 */
 static int reloc_data(reloc_op_t op, void *loc, u64 val, int len)
 {