Index: sys/arm64/arm64/exec_machdep.c
===================================================================
--- sys/arm64/arm64/exec_machdep.c
+++ sys/arm64/arm64/exec_machdep.c
@@ -115,6 +115,13 @@
 	{
 		frame->tf_elr = regs->elr;
 		frame->tf_spsr |= regs->spsr & PSR_FLAGS;
+		if ((frame->tf_spsr & PSR_SS) != 0) {
+			td->td_pcb->pcb_flags |= PCB_SINGLE_STEP;
+
+			WRITE_SPECIALREG(mdscr_el1,
+			    READ_SPECIALREG(mdscr_el1) | MDSCR_SS);
+			isb();
+		}
 	}
 	return (0);
 }
@@ -450,6 +457,13 @@
 	tf->tf_lr = mcp->mc_gpregs.gp_lr;
 	tf->tf_elr = mcp->mc_gpregs.gp_elr;
 	tf->tf_spsr = mcp->mc_gpregs.gp_spsr;
+	if ((tf->tf_spsr & PSR_SS) != 0) {
+		td->td_pcb->pcb_flags |= PCB_SINGLE_STEP;
+
+		WRITE_SPECIALREG(mdscr_el1,
+		    READ_SPECIALREG(mdscr_el1) | MDSCR_SS);
+		isb();
+	}
 	set_fpcontext(td, mcp);
 
 	return (0);
@@ -603,6 +617,10 @@
 	tf->tf_elr = (register_t)catcher;
 	tf->tf_sp = (register_t)fp;
 	tf->tf_lr = (register_t)p->p_sysent->sv_sigcode_base;
+	td->td_pcb->pcb_flags &= ~PCB_SINGLE_STEP;
+	WRITE_SPECIALREG(mdscr_el1,
+	    READ_SPECIALREG(mdscr_el1) & ~MDSCR_SS);
+	isb();
 
 	CTR3(KTR_SIG, "sendsig: return td=%p pc=%#x sp=%#x", td, tf->tf_elr,
 	    tf->tf_sp);
Index: sys/arm64/arm64/ptrace_machdep.c
===================================================================
--- sys/arm64/arm64/ptrace_machdep.c
+++ sys/arm64/arm64/ptrace_machdep.c
@@ -59,18 +59,22 @@
 int
 ptrace_single_step(struct thread *td)
 {
-
-	td->td_frame->tf_spsr |= PSR_SS;
-	td->td_pcb->pcb_flags |= PCB_SINGLE_STEP;
+	PROC_LOCK_ASSERT(td->td_proc, MA_OWNED);
+	if ((td->td_frame->tf_spsr & PSR_SS) == 0) {
+		td->td_frame->tf_spsr |= PSR_SS;
+		td->td_pcb->pcb_flags |= PCB_SINGLE_STEP;
+		td->td_dbgflags |= TDB_STEP;
+	}
 	return (0);
 }
 
 int
 ptrace_clear_single_step(struct thread *td)
 {
-
+	PROC_LOCK_ASSERT(td->td_proc, MA_OWNED);
 	td->td_frame->tf_spsr &= ~PSR_SS;
 	td->td_pcb->pcb_flags &= ~PCB_SINGLE_STEP;
+	td->td_dbgflags &= ~TDB_STEP;
 	return (0);
 }
 
Index: sys/arm64/arm64/trap.c
===================================================================
--- sys/arm64/arm64/trap.c
+++ sys/arm64/arm64/trap.c
@@ -620,10 +620,14 @@
 		userret(td, frame);
 		break;
 	case EXCP_SOFTSTP_EL0:
-		td->td_frame->tf_spsr &= ~PSR_SS;
-		td->td_pcb->pcb_flags &= ~PCB_SINGLE_STEP;
-		WRITE_SPECIALREG(mdscr_el1,
-		    READ_SPECIALREG(mdscr_el1) & ~MDSCR_SS);
+		PROC_LOCK(td->td_proc);
+		if ((td->td_dbgflags & TDB_STEP) != 0) {
+			td->td_frame->tf_spsr &= ~PSR_SS;
+			td->td_pcb->pcb_flags &= ~PCB_SINGLE_STEP;
+			WRITE_SPECIALREG(mdscr_el1,
+			    READ_SPECIALREG(mdscr_el1) & ~MDSCR_SS);
+		}
+		PROC_UNLOCK(td->td_proc);
 		call_trapsignal(td, SIGTRAP, TRAP_TRACE,
 		    (void *)frame->tf_elr, exception);
 		userret(td, frame);
Index: sys/arm64/include/armreg.h
===================================================================
--- sys/arm64/include/armreg.h
+++ sys/arm64/include/armreg.h
@@ -1314,7 +1314,7 @@
 #define	PSR_C		0x20000000
 #define	PSR_Z		0x40000000
 #define	PSR_N		0x80000000
-#define	PSR_FLAGS	0xf0000000
+#define	PSR_FLAGS	0xf0200000
 
 /* TCR_EL1 - Translation Control Register */
 /* Bits 63:59 are reserved */
Index: tests/sys/kern/basic_signal.c
===================================================================
--- tests/sys/kern/basic_signal.c
+++ tests/sys/kern/basic_signal.c
@@ -10,6 +10,10 @@
 #include <stdbool.h>
 #include <stdlib.h>
 
+#ifdef __aarch64__
+#include <machine/armreg.h>
+#endif
+
 static volatile sig_atomic_t signal_fired = 0;
 
 static void
@@ -62,6 +66,56 @@
 	ATF_CHECK(signal_fired == 3);
 }
 
+/*
+ * Check setting the machine dependent single step flag works when supported.
+ */
+/* TODO: Add x86 support */
+#ifdef __aarch64__
+static volatile sig_atomic_t trap_signal_fired = 0;
+
+#include <stdio.h>
+static void
+trap_sig_handler(int signo, siginfo_t *info __unused, void *_ucp)
+{
+	ucontext_t *ucp = _ucp;
+
+	if (trap_signal_fired < 9)
+		ucp->uc_mcontext.mc_gpregs.gp_spsr |= PSR_SS;
+	else
+		ucp->uc_mcontext.mc_gpregs.gp_spsr &= ~PSR_SS;
+	trap_signal_fired++;
+}
+
+ATF_TC(trap_signal_test);
+
+ATF_TC_HEAD(trap_signal_test, tc)
+{
+
+	atf_tc_set_md_var(tc, "descr",
+	    "Testing signal handler setting the MD single step flag");
+}
+
+ATF_TC_BODY(trap_signal_test, tc)
+{
+	/*
+	 * Setup the signal handlers
+	 */
+	struct sigaction sa = {
+		.sa_sigaction = trap_sig_handler,
+		.sa_flags = SA_SIGINFO,
+	};
+	ATF_REQUIRE(sigemptyset(&sa.sa_mask) == 0);
+	ATF_REQUIRE(sigaction(SIGTRAP, &sa, NULL) == 0);
+
+	/*
+	 * Fire SIGTRAP
+	 */
+	ATF_CHECK(trap_signal_fired == 0);
+	ATF_REQUIRE(raise(SIGTRAP) == 0);
+	ATF_CHECK(trap_signal_fired == 10);
+}
+#endif
+
 /*
  * Special tests for 32-bit arm. We can call thumb code (really just t32) from
  * normal (a32) mode and vice versa. Likewise, signals can interrupt a T32
@@ -150,6 +204,9 @@
 {
 
 	ATF_TP_ADD_TC(tp, signal_test);
+#ifdef __aarch64__
+	ATF_TP_ADD_TC(tp, trap_signal_test);
+#endif
 #ifdef __arm__
 	ATF_TP_ADD_TC(tp, signal_test_T32_to_A32);
 	ATF_TP_ADD_TC(tp, signal_test_A32_to_T32);