#include <asm/cputype.h>
 #include <asm/daifflags.h>
 #include <asm/debug-monitors.h>
+#include <asm/exception.h>
 #include <asm/kgdb.h>
 #include <asm/kprobes.h>
 #include <asm/system_misc.h>
 #define set_regs_spsr_ss(r)    set_user_regs_spsr_ss(&(r)->user_regs)
 #define clear_regs_spsr_ss(r)  clear_user_regs_spsr_ss(&(r)->user_regs)
 
-/*
- * Call single step handlers
- * There is no Syndrome info to check for determining the handler.
- * However, there is only one possible handler for user and kernel modes, so
- * check and call the appropriate one.
- */
-static int call_step_hook(struct pt_regs *regs, unsigned long esr)
-{
-       if (user_mode(regs))
-               return uprobe_single_step_handler(regs, esr);
-
-       return kgdb_single_step_handler(regs, esr);
-}
-NOKPROBE_SYMBOL(call_step_hook);
-
 static void send_user_sigtrap(int si_code)
 {
        struct pt_regs *regs = current_pt_regs();
                              "User debug trap");
 }
 
-static int single_step_handler(unsigned long unused, unsigned long esr,
-                              struct pt_regs *regs)
+/*
+ * We have already unmasked interrupts and enabled preemption
+ * when calling do_el0_softstep() from entry-common.c.
+ */
+void do_el0_softstep(unsigned long esr, struct pt_regs *regs)
 {
+       if (uprobe_single_step_handler(regs, esr) == DBG_HOOK_HANDLED)
+               return;
+
+       send_user_sigtrap(TRAP_TRACE);
        /*
-        * If we are stepping a pending breakpoint, call the hw_breakpoint
-        * handler first.
+        * ptrace will disable single step unless explicitly
+        * asked to re-enable it. For other clients, it makes
+        * sense to leave it enabled (i.e. rewind the controls
+        * to the active-not-pending state).
         */
-       if (try_step_suspended_breakpoints(regs))
-               return 0;
-
-       if (call_step_hook(regs, esr) == DBG_HOOK_HANDLED)
-               return 0;
+       user_rewind_single_step(current);
+}
 
-       if (user_mode(regs)) {
-               send_user_sigtrap(TRAP_TRACE);
-
-               /*
-                * ptrace will disable single step unless explicitly
-                * asked to re-enable it. For other clients, it makes
-                * sense to leave it enabled (i.e. rewind the controls
-                * to the active-not-pending state).
-                */
-               user_rewind_single_step(current);
-       } else {
-               pr_warn("Unexpected kernel single-step exception at EL1\n");
-               /*
-                * Re-enable stepping since we know that we will be
-                * returning to regs.
-                */
-               set_regs_spsr_ss(regs);
-       }
+void do_el1_softstep(unsigned long esr, struct pt_regs *regs)
+{
+       if (kgdb_single_step_handler(regs, esr) == DBG_HOOK_HANDLED)
+               return;
 
-       return 0;
+       pr_warn("Unexpected kernel single-step exception at EL1\n");
+       /*
+        * Re-enable stepping since we know that we will be
+        * returning to regs.
+        */
+       set_regs_spsr_ss(regs);
 }
-NOKPROBE_SYMBOL(single_step_handler);
+NOKPROBE_SYMBOL(do_el1_softstep);
 
 static int call_break_hook(struct pt_regs *regs, unsigned long esr)
 {
 
 void __init debug_traps_init(void)
 {
-       hook_debug_fault_code(DBG_ESR_EVT_HWSS, single_step_handler, SIGTRAP,
-                             TRAP_TRACE, "single-step handler");
        hook_debug_fault_code(DBG_ESR_EVT_BRK, brk_handler, SIGTRAP,
                              TRAP_BRKPT, "BRK handler");
 }
 
        arm64_exit_el1_dbg(regs);
 }
 
+static void noinstr el1_softstp(struct pt_regs *regs, unsigned long esr)
+{
+       arm64_enter_el1_dbg(regs);
+       if (!cortex_a76_erratum_1463225_debug_handler(regs)) {
+               debug_exception_enter(regs);
+               /*
+                * After handling a breakpoint, we suspend the breakpoint
+                * and use single-step to move to the next instruction.
+                * If we are stepping a suspended breakpoint there's nothing more to do:
+                * the single-step is complete.
+                */
+               if (!try_step_suspended_breakpoints(regs))
+                       do_el1_softstep(esr, regs);
+               debug_exception_exit(regs);
+       }
+       arm64_exit_el1_dbg(regs);
+}
+
 static void noinstr el1_dbg(struct pt_regs *regs, unsigned long esr)
 {
        unsigned long far = read_sysreg(far_el1);
                el1_breakpt(regs, esr);
                break;
        case ESR_ELx_EC_SOFTSTP_CUR:
+               el1_softstp(regs, esr);
+               break;
        case ESR_ELx_EC_WATCHPT_CUR:
        case ESR_ELx_EC_BRK64:
                el1_dbg(regs, esr);
        exit_to_user_mode(regs);
 }
 
+static void noinstr el0_softstp(struct pt_regs *regs, unsigned long esr)
+{
+       if (!is_ttbr0_addr(regs->pc))
+               arm64_apply_bp_hardening();
+
+       enter_from_user_mode(regs);
+       /*
+        * After handling a breakpoint, we suspend the breakpoint
+        * and use single-step to move to the next instruction.
+        * If we are stepping a suspended breakpoint there's nothing more to do:
+        * the single-step is complete.
+        */
+       if (!try_step_suspended_breakpoints(regs)) {
+               local_daif_restore(DAIF_PROCCTX);
+               do_el0_softstep(esr, regs);
+       }
+       exit_to_user_mode(regs);
+}
+
 static void noinstr el0_dbg(struct pt_regs *regs, unsigned long esr)
 {
        /* Only watchpoints write FAR_EL1, otherwise its UNKNOWN */
                el0_breakpt(regs, esr);
                break;
        case ESR_ELx_EC_SOFTSTP_LOW:
+               el0_softstp(regs, esr);
+               break;
        case ESR_ELx_EC_WATCHPT_LOW:
        case ESR_ELx_EC_BRK64:
                el0_dbg(regs, esr);
                el0_breakpt(regs, esr);
                break;
        case ESR_ELx_EC_SOFTSTP_LOW:
+               el0_softstp(regs, esr);
+               break;
        case ESR_ELx_EC_WATCHPT_LOW:
        case ESR_ELx_EC_BKPT32:
                el0_dbg(regs, esr);