diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index c4207cf9bb17..6b68ec498e67 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -200,6 +200,7 @@ config ARM64
 	select HAVE_STACKPROTECTOR
 	select HAVE_SYSCALL_TRACEPOINTS
 	select HAVE_KPROBES
+	select HAVE_OPTPROBES
 	select HAVE_KRETPROBES
 	select HAVE_GENERIC_VDSO
 	select IOMMU_DMA if IOMMU_SUPPORT
diff --git a/arch/arm64/include/asm/kprobes.h b/arch/arm64/include/asm/kprobes.h
index 05cd82eeca13..3838e4317877 100644
--- a/arch/arm64/include/asm/kprobes.h
+++ b/arch/arm64/include/asm/kprobes.h
@@ -39,6 +39,27 @@ void arch_remove_kprobe(struct kprobe *);
 int kprobe_fault_handler(struct pt_regs *regs, unsigned int fsr);
 int kprobe_exceptions_notify(struct notifier_block *self,
 			     unsigned long val, void *data);
+
+struct arch_optimized_insn {
+	kprobe_opcode_t orig_insn[1];
+	kprobe_opcode_t *trampoline;
+};
+
+#define MAX_OPTIMIZED_LENGTH	sizeof(kprobe_opcode_t)
+#define MAX_OPTINSN_SIZE                                                       \
+	((unsigned long)optprobe_template_restore_end - (unsigned long)optprobe_template_entry)
+
+extern __visible kprobe_opcode_t optprobe_template_entry[];
+extern __visible kprobe_opcode_t optprobe_template_val[];
+extern __visible kprobe_opcode_t optprobe_template_common[];
+extern __visible kprobe_opcode_t optprobe_template_end[];
+extern __visible kprobe_opcode_t optprobe_template_restore_begin[];
+extern __visible kprobe_opcode_t optprobe_template_restore_orig_insn[];
+extern __visible kprobe_opcode_t optprobe_template_restore_end[];
+extern __visible kprobe_opcode_t optinsn_slot[];
+
+void optprobe_common(void);
+
 void __kretprobe_trampoline(void);
 void __kprobes *trampoline_probe_handler(struct pt_regs *regs);
 
diff --git a/arch/arm64/kernel/probes/Makefile b/arch/arm64/kernel/probes/Makefile
index 8e4be92e25b1..07105fd3261d 100644
--- a/arch/arm64/kernel/probes/Makefile
+++ b/arch/arm64/kernel/probes/Makefile
@@ -4,3 +4,5 @@ obj-$(CONFIG_KPROBES)		+= kprobes.o decode-insn.o	\
 				   simulate-insn.o
 obj-$(CONFIG_UPROBES)		+= uprobes.o decode-insn.o	\
 				   simulate-insn.o
+obj-$(CONFIG_OPTPROBES)		+= opt_arm64.o			\
+				   optprobe_trampoline.o
diff --git a/arch/arm64/kernel/probes/opt_arm64.c b/arch/arm64/kernel/probes/opt_arm64.c
new file mode 100644
index 000000000000..1bad5cddcdf2
--- /dev/null
+++ b/arch/arm64/kernel/probes/opt_arm64.c
@@ -0,0 +1,199 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Code for Kernel probes Jump optimization.
+ *
+ * Copyright (C) 2021 Hisilicon Limited
+ */
+
+#include <linux/jump_label.h>
+#include <linux/kprobes.h>
+
+#include <asm/cacheflush.h>
+#include <asm/compiler.h>
+#include <asm/insn.h>
+#include <asm/kprobes.h>
+#include <asm/patching.h>
+
+#define OPTPROBE_BATCH_SIZE 64
+#define GET_LO_VAL(val)		FIELD_GET(GENMASK(31, 0), val)
+#define GET_HI_VAL(val)		FIELD_GET(GENMASK(63, 32), val)
+
+#define TMPL_VAL_IDX \
+	(optprobe_template_val - optprobe_template_entry)
+#define TMPL_CALL_COMMON \
+	(optprobe_template_common - optprobe_template_entry)
+#define TMPL_RESTORE_ORIGN_INSN \
+	(optprobe_template_restore_orig_insn - optprobe_template_entry)
+#define TMPL_RESTORE_END \
+	(optprobe_template_restore_end - optprobe_template_entry)
+#define TMPL_END_IDX \
+	(optprobe_template_end - optprobe_template_entry)
+
+static bool insn_page_in_use;
+
+void *alloc_optinsn_page(void)
+{
+	if (insn_page_in_use)
+		return NULL;
+	insn_page_in_use = true;
+	return &optinsn_slot;
+}
+
+void free_optinsn_page(void *page)
+{
+	insn_page_in_use = false;
+}
+
+int arch_check_optimized_kprobe(struct optimized_kprobe *op)
+{
+	return 0;
+}
+
+int arch_prepared_optinsn(struct arch_optimized_insn *optinsn)
+{
+	return optinsn->trampoline != NULL;
+}
+
+int arch_within_optimized_kprobe(struct optimized_kprobe *op, kprobe_opcode_t *addr)
+{
+	return op->kp.addr == addr;
+}
+
+static void optprobe_set_pc_value(struct optimized_kprobe *op, struct pt_regs *regs)
+{
+	regs->pc = (unsigned long)op->kp.addr;
+}
+
+static int optprobe_check_branch_limit(unsigned long pc, unsigned long addr)
+{
+	long offset;
+
+	if ((pc & 0x3) || (addr & 0x3))
+		return -ERANGE;
+
+	offset = (long)addr - (long)pc;
+	if (offset < -SZ_128M || offset >= SZ_128M)
+		return -ERANGE;
+
+	return 0;
+}
+
+int arch_prepare_optimized_kprobe(struct optimized_kprobe *op, struct kprobe *orig)
+{
+	kprobe_opcode_t *code, *buf;
+	u32 insn;
+	int ret = -ENOMEM;
+	int i;
+
+	buf = kcalloc(MAX_OPTINSN_SIZE, sizeof(kprobe_opcode_t), GFP_KERNEL);
+	if (!buf)
+		return ret;
+
+	code = get_optinsn_slot();
+	if (!code)
+		goto out;
+
+	if (optprobe_check_branch_limit((unsigned long)code, (unsigned long)orig->addr + 8)) {
+		ret = -ERANGE;
+		goto error;
+	}
+
+	op->set_pc = optprobe_set_pc_value;
+	memcpy(buf, optprobe_template_entry, MAX_OPTINSN_SIZE * sizeof(kprobe_opcode_t));
+
+	insn = aarch64_insn_gen_branch_imm((unsigned long)&code[TMPL_CALL_COMMON],
+					   (unsigned long)&optprobe_common,
+					   AARCH64_INSN_BRANCH_LINK);
+	buf[TMPL_CALL_COMMON] = insn;
+
+	insn = aarch64_insn_gen_branch_imm((unsigned long)&code[TMPL_RESTORE_END],
+					   (unsigned long)op->kp.addr + 4,
+					   AARCH64_INSN_BRANCH_NOLINK);
+	buf[TMPL_RESTORE_END] = insn;
+
+	buf[TMPL_VAL_IDX] = cpu_to_le32(GET_LO_VAL((unsigned long)op));
+	buf[TMPL_VAL_IDX + 1] = cpu_to_le32(GET_HI_VAL((unsigned long)op));
+	buf[TMPL_RESTORE_ORIGN_INSN] = orig->opcode;
+
+	/* Setup template */
+	for (i = 0; i < MAX_OPTINSN_SIZE; i++)
+		aarch64_insn_patch_text_nosync(code + i, buf[i]);
+
+	flush_icache_range((unsigned long)code, (unsigned long)(&code[TMPL_VAL_IDX]));
+	/* Set op->optinsn.trampoline means prepared. */
+	op->optinsn.trampoline = code;
+
+out:
+	kfree(buf);
+	return ret;
+
+error:
+	free_optinsn_slot(code, 0);
+	goto out;
+}
+
+void arch_optimize_kprobes(struct list_head *oplist)
+{
+	struct optimized_kprobe *op, *tmp;
+	kprobe_opcode_t insns[OPTPROBE_BATCH_SIZE];
+	void *addrs[OPTPROBE_BATCH_SIZE];
+	int i = 0;
+
+	list_for_each_entry_safe(op, tmp, oplist, list) {
+		WARN_ON(kprobe_disabled(&op->kp));
+
+		/*
+		 * Backup instructions which will be replaced
+		 * by jump address
+		 */
+		memcpy(op->optinsn.orig_insn, op->kp.addr, AARCH64_INSN_SIZE);
+
+		addrs[i] = (void *)op->kp.addr;
+		insns[i] = aarch64_insn_gen_branch_imm((unsigned long)op->kp.addr,
+						       (unsigned long)op->optinsn.trampoline,
+						       AARCH64_INSN_BRANCH_NOLINK);
+
+		list_del_init(&op->list);
+		if (++i == OPTPROBE_BATCH_SIZE)
+			break;
+	}
+
+	aarch64_insn_patch_text(addrs, insns, i);
+}
+
+void arch_unoptimize_kprobe(struct optimized_kprobe *op)
+{
+	arch_arm_kprobe(&op->kp);
+}
+
+/*
+ * Recover original instructions and breakpoints from relative jumps.
+ * Caller must call with locking kprobe_mutex.
+ */
+void arch_unoptimize_kprobes(struct list_head *oplist,
+			    struct list_head *done_list)
+{
+	struct optimized_kprobe *op, *tmp;
+	kprobe_opcode_t insns[OPTPROBE_BATCH_SIZE];
+	void *addrs[OPTPROBE_BATCH_SIZE];
+	int i = 0;
+
+	list_for_each_entry_safe(op, tmp, oplist, list) {
+		addrs[i] = (void *)op->kp.addr;
+		insns[i] = BRK64_OPCODE_KPROBES;
+		list_move(&op->list, done_list);
+
+		if (++i == OPTPROBE_BATCH_SIZE)
+			break;
+	}
+
+	aarch64_insn_patch_text(addrs, insns, i);
+}
+
+void arch_remove_optimized_kprobe(struct optimized_kprobe *op)
+{
+	if (op->optinsn.trampoline) {
+		free_optinsn_slot(op->optinsn.trampoline, 1);
+		op->optinsn.trampoline = NULL;
+	}
+}
diff --git a/arch/arm64/kernel/probes/optprobe_trampoline.S b/arch/arm64/kernel/probes/optprobe_trampoline.S
new file mode 100644
index 000000000000..1376daeae844
--- /dev/null
+++ b/arch/arm64/kernel/probes/optprobe_trampoline.S
@@ -0,0 +1,97 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * trampoline entry and return code for optprobes.
+ */
+
+#include <linux/linkage.h>
+#include <asm/asm-offsets.h>
+#include <asm/assembler.h>
+
+#define        OPT_SLOT_SIZE   65536
+
+	.global optinsn_slot
+optinsn_slot:
+	.space  OPT_SLOT_SIZE
+
+SYM_CODE_START(optprobe_common)
+	stp x2, x3, [sp, #S_X2]
+	stp x4, x5, [sp, #S_X4]
+	stp x6, x7, [sp, #S_X6]
+	stp x8, x9, [sp, #S_X8]
+	stp x10, x11, [sp, #S_X10]
+	stp x12, x13, [sp, #S_X12]
+	stp x14, x15, [sp, #S_X14]
+	stp x16, x17, [sp, #S_X16]
+	stp x18, x19, [sp, #S_X18]
+	stp x20, x21, [sp, #S_X20]
+	stp x22, x23, [sp, #S_X22]
+	stp x24, x25, [sp, #S_X24]
+	stp x26, x27, [sp, #S_X26]
+	stp x28, x29, [sp, #S_X28]
+	add x2, sp, #PT_REGS_SIZE
+	str x2, [sp, #S_SP]
+	/* Construct a useful saved PSTATE */
+	mrs x2, nzcv
+	mrs x3, daif
+	orr x2, x2, x3
+	mrs x3, CurrentEL
+	orr x2, x2, x3
+	mrs x3, SPSel
+	orr x2, x2, x3
+	stp xzr, x2, [sp, #S_PC]
+
+	/* set the pt_regs address to x1 */
+	mov	x1, sp
+	/* store lr of optprobe_common temporary */
+	sub sp, sp, #0x10
+	str lr, [sp, #0x8]
+
+	bl optprobe_optimized_callback
+
+	ldr lr, [sp, #0x8]
+	add sp, sp, #0x10
+	ldr x0, [sp, #S_PSTATE]
+	and x0, x0, #(PSR_N_BIT | PSR_Z_BIT | PSR_C_BIT | PSR_V_BIT)
+	msr nzcv, x0
+	ldp x0, x1, [sp, #S_X0]
+	ldp x2, x3, [sp, #S_X2]
+	ldp x4, x5, [sp, #S_X4]
+	ldp x6, x7, [sp, #S_X6]
+	ldp x8, x9, [sp, #S_X8]
+	ldp x10, x11, [sp, #S_X10]
+	ldp x12, x13, [sp, #S_X12]
+	ldp x14, x15, [sp, #S_X14]
+	ldp x16, x17, [sp, #S_X16]
+	ldp x18, x19, [sp, #S_X18]
+	ldp x20, x21, [sp, #S_X20]
+	ldp x22, x23, [sp, #S_X22]
+	ldp x24, x25, [sp, #S_X24]
+	ldp x26, x27, [sp, #S_X26]
+	ldp x28, x29, [sp, #S_X28]
+	ret
+SYM_CODE_END(optprobe_common)
+
+	.global optprobe_template_entry
+optprobe_template_entry:
+	sub sp, sp, #PT_REGS_SIZE
+	str lr, [sp, #S_LR]
+	stp x0, x1, [sp, #S_X0]
+	/* Get parameters to optimized_callback() */
+	ldr x0, 1f
+	.global optprobe_template_common
+optprobe_template_common:
+	nop
+	ldr lr, [sp, #S_LR]
+       add sp, sp, #PT_REGS_SIZE
+	.global optprobe_template_restore_orig_insn
+optprobe_template_restore_orig_insn:
+	nop
+	.global optprobe_template_restore_end
+optprobe_template_restore_end:
+	nop
+	.global optprobe_template_val
+optprobe_template_val:
+	1:	.long 0
+		.long 0
+	.global optprobe_template_end
+optprobe_template_end:
diff --git a/include/linux/kprobes.h b/include/linux/kprobes.h
index 8c8f7a4d93af..7be680155df7 100644
--- a/include/linux/kprobes.h
+++ b/include/linux/kprobes.h
@@ -329,6 +329,7 @@ struct optimized_kprobe {
 	struct kprobe kp;
 	struct list_head list;	/* list for optimizing queue */
 	struct arch_optimized_insn optinsn;
+	void (*set_pc)(struct optimized_kprobe *op, struct pt_regs *regs);
 };
 
 /* Architecture dependent functions for direct jump optimization */
@@ -345,6 +346,7 @@ extern int arch_within_optimized_kprobe(struct optimized_kprobe *op,
 					kprobe_opcode_t *addr);
 
 extern void opt_pre_handler(struct kprobe *p, struct pt_regs *regs);
+extern void optprobe_optimized_callback(struct optimized_kprobe *op, struct pt_regs *regs);
 
 DEFINE_INSN_CACHE_OPS(optinsn);
 
diff --git a/kernel/kprobes.c b/kernel/kprobes.c
index 21eccc961bba..d799e562a39a 100644
--- a/kernel/kprobes.c
+++ b/kernel/kprobes.c
@@ -430,6 +430,28 @@ void opt_pre_handler(struct kprobe *p, struct pt_regs *regs)
 }
 NOKPROBE_SYMBOL(opt_pre_handler);
 
+void optprobe_optimized_callback(struct optimized_kprobe *op, struct pt_regs *regs)
+{
+	if (kprobe_disabled(&op->kp))
+		return;
+
+	preempt_disable();
+
+	if (kprobe_running()) {
+		kprobes_inc_nmissed_count(&op->kp);
+	} else {
+		op->set_pc(op, regs);
+		__this_cpu_write(current_kprobe, &op->kp);
+		get_kprobe_ctlblk()->kprobe_status = KPROBE_HIT_ACTIVE;
+		opt_pre_handler(&op->kp, regs);
+		__this_cpu_write(current_kprobe, NULL);
+	}
+
+	preempt_enable();
+}
+NOKPROBE_SYMBOL(optprobe_optimized_callback)
+
+
 /* Free optimized instructions and optimized_kprobe */
 static void free_aggr_kprobe(struct kprobe *p)
 {
