diff --git a/arch/arm64/Kconfig b/arch/arm64/Kconfig
index be0a9f0052ee..d2cc7daecce3 100644
--- a/arch/arm64/Kconfig
+++ b/arch/arm64/Kconfig
@@ -231,6 +231,7 @@ config ARM64
 	select TRACE_IRQFLAGS_SUPPORT
 	select TRACE_IRQFLAGS_NMI_SUPPORT
 	select HAVE_SOFTIRQ_ON_OWN_STACK
+	select HAVE_IRQ_EXIT_ON_IRQ_STACK
 	help
 	  ARM 64-bit (AArch64) Linux support.
 
diff --git a/arch/arm64/include/asm/exception.h b/arch/arm64/include/asm/exception.h
index d94aecff9690..8bff0aa7ab50 100644
--- a/arch/arm64/include/asm/exception.h
+++ b/arch/arm64/include/asm/exception.h
@@ -54,7 +54,9 @@ asmlinkage void el0t_32_fiq_handler(struct pt_regs *regs);
 asmlinkage void el0t_32_error_handler(struct pt_regs *regs);
 
 asmlinkage void call_on_irq_stack(struct pt_regs *regs,
-				  void (*func)(struct pt_regs *));
+				  void (*func)(struct pt_regs *),
+				  void (*do_func)(struct pt_regs *,
+						  void (*)(struct pt_regs *)));
 asmlinkage void asm_exit_to_user_mode(struct pt_regs *regs);
 
 void do_mem_abort(unsigned long far, unsigned long esr, struct pt_regs *regs);
diff --git a/arch/arm64/kernel/entry-common.c b/arch/arm64/kernel/entry-common.c
index c75ca36b4a49..935d1ab150b5 100644
--- a/arch/arm64/kernel/entry-common.c
+++ b/arch/arm64/kernel/entry-common.c
@@ -266,14 +266,16 @@ static void __sched arm64_preempt_schedule_irq(void)
 }
 
 static void do_interrupt_handler(struct pt_regs *regs,
-				 void (*handler)(struct pt_regs *))
+				 void (*handler)(struct pt_regs *),
+				 void (*do_handler)(struct pt_regs *,
+						    void (*)(struct pt_regs *)))
 {
 	struct pt_regs *old_regs = set_irq_regs(regs);
 
 	if (on_thread_stack())
-		call_on_irq_stack(regs, handler);
+		call_on_irq_stack(regs, handler, do_handler);
 	else
-		handler(regs);
+		do_handler(regs, handler);
 
 	set_irq_regs(old_regs);
 }
@@ -441,22 +443,32 @@ asmlinkage void noinstr el1h_64_sync_handler(struct pt_regs *regs)
 	}
 }
 
+static void nmi_handler(struct pt_regs *regs, void (*handler)(struct pt_regs *))
+{
+	handler(regs);
+}
+
 static __always_inline void __el1_pnmi(struct pt_regs *regs,
 				       void (*handler)(struct pt_regs *))
 {
 	arm64_enter_nmi(regs);
-	do_interrupt_handler(regs, handler);
+	do_interrupt_handler(regs, handler, nmi_handler);
 	arm64_exit_nmi(regs);
 }
 
+static void irq_handler(struct pt_regs *regs, void (*handler)(struct pt_regs *))
+{
+	irq_enter_rcu();
+	handler(regs);
+	irq_exit_rcu();
+}
+
 static __always_inline void __el1_irq(struct pt_regs *regs,
 				      void (*handler)(struct pt_regs *))
 {
 	enter_from_kernel_mode(regs);
 
-	irq_enter_rcu();
-	do_interrupt_handler(regs, handler);
-	irq_exit_rcu();
+	do_interrupt_handler(regs, handler, irq_handler);
 
 	arm64_preempt_schedule_irq();
 
@@ -699,9 +711,7 @@ static void noinstr el0_interrupt(struct pt_regs *regs,
 	if (regs->pc & BIT(55))
 		arm64_apply_bp_hardening();
 
-	irq_enter_rcu();
-	do_interrupt_handler(regs, handler);
-	irq_exit_rcu();
+	do_interrupt_handler(regs, handler, irq_handler);
 
 	exit_to_user_mode(regs);
 }
diff --git a/arch/arm64/kernel/entry.S b/arch/arm64/kernel/entry.S
index 254fe31c03a0..1c351391f6bd 100644
--- a/arch/arm64/kernel/entry.S
+++ b/arch/arm64/kernel/entry.S
@@ -867,7 +867,9 @@ NOKPROBE(ret_from_fork)
 
 /*
  * void call_on_irq_stack(struct pt_regs *regs,
- * 		          void (*func)(struct pt_regs *));
+ * 		          void (*func)(struct pt_regs *)
+ * 			  void (*do_func)(struct pt_regs *,
+ *					  void (*)(struct pt_regs *)));
  *
  * Calls func(regs) using this CPU's irq stack and shadow irq stack.
  */
@@ -886,7 +888,7 @@ SYM_FUNC_START(call_on_irq_stack)
 
 	/* Move to the new stack and call the function there */
 	mov	sp, x16
-	blr	x1
+	blr	x2
 
 	/*
 	 * Restore the SP from the FP, and restore the FP and LR from the frame
diff --git a/arch/arm64/kernel/irq.c b/arch/arm64/kernel/irq.c
index c36ad20a52f3..003db605bc4f 100644
--- a/arch/arm64/kernel/irq.c
+++ b/arch/arm64/kernel/irq.c
@@ -73,14 +73,15 @@ static void init_irq_stacks(void)
 #endif
 
 #ifndef CONFIG_PREEMPT_RT
-static void ____do_softirq(struct pt_regs *regs)
+static void ____do_softirq(struct pt_regs *regs,
+			   void (*handler)(struct pt_regs *))
 {
 	__do_softirq();
 }
 
 void do_softirq_own_stack(void)
 {
-	call_on_irq_stack(NULL, ____do_softirq);
+	call_on_irq_stack(NULL, NULL, ____do_softirq);
 }
 #endif
 
