Implement single stepping on arm64. We need to set the single step bits in

the processor and debug state registers. A flag has been added to the pcb
to tell us when to enable single stepping for a given thread.

Reviewed by:	kib
Sponsored by:	ABT Systems Ltd
Differential Revision:	https://reviews.freebsd.org/D4730

7 files changed, 65 insertions, 5 deletions

diff --git a/sys/arm64/arm64/genassym.c b/sys/arm64/arm64/genassym.c
index 214b99d..67c295f 100644
--- a/sys/arm64/arm64/genassym.c
+++ b/sys/arm64/arm64/genassym.c
@@ -49,10 +49,12 @@ ASSYM(PC_CURTHREAD, offsetof(struct pcpu, pc_curthread));
 
 /* Size of pcb, rounded to keep stack alignment */
 ASSYM(PCB_SIZE, roundup2(sizeof(struct pcb), STACKALIGNBYTES + 1));
+ASSYM(PCB_SINGLE_STEP_SHIFT, PCB_SINGLE_STEP_SHIFT);
 ASSYM(PCB_REGS, offsetof(struct pcb, pcb_x));
 ASSYM(PCB_SP, offsetof(struct pcb, pcb_sp));
 ASSYM(PCB_L1ADDR, offsetof(struct pcb, pcb_l1addr));
 ASSYM(PCB_ONFAULT, offsetof(struct pcb, pcb_onfault));
+ASSYM(PCB_FLAGS, offsetof(struct pcb, pcb_flags));
 
 ASSYM(SF_UC, offsetof(struct sigframe, sf_uc));
 
diff --git a/sys/arm64/arm64/machdep.c b/sys/arm64/arm64/machdep.c
index 409ecd3..f7552c4 100644
--- a/sys/arm64/arm64/machdep.c
+++ b/sys/arm64/arm64/machdep.c
@@ -233,7 +233,8 @@ int
 ptrace_single_step(struct thread *td)
 {
 
-	/* TODO; */
+	td->td_frame->tf_spsr |= PSR_SS;
+	td->td_pcb->pcb_flags |= PCB_SINGLE_STEP;
 	return (0);
 }
 
@@ -241,7 +242,8 @@ int
 ptrace_clear_single_step(struct thread *td)
 {
 
-	/* TODO; */
+	td->td_frame->tf_spsr &= ~PSR_SS;
+	td->td_pcb->pcb_flags &= ~PCB_SINGLE_STEP;
 	return (0);
 }
 
diff --git a/sys/arm64/arm64/mp_machdep.c b/sys/arm64/arm64/mp_machdep.c
index 19cee77..b89982d 100644
--- a/sys/arm64/arm64/mp_machdep.c
+++ b/sys/arm64/arm64/mp_machdep.c
@@ -51,6 +51,7 @@ __FBSDID("$FreeBSD$");
 #include <vm/vm_extern.h>
 #include <vm/vm_kern.h>
 
+#include <machine/debug_monitor.h>
 #include <machine/intr.h>
 #include <machine/smp.h>
 #ifdef VFP
@@ -247,6 +248,8 @@ init_secondary(uint64_t cpu)
 	vfp_init();
 #endif
 
+	dbg_monitor_init();
+
 	/* Enable interrupts */
 	intr_enable();
 
diff --git a/sys/arm64/arm64/swtch.S b/sys/arm64/arm64/swtch.S
index 1f20b3d..3175e87 100644
--- a/sys/arm64/arm64/swtch.S
+++ b/sys/arm64/arm64/swtch.S
@@ -37,10 +37,37 @@
 
 __FBSDID("$FreeBSD$");
 
+.macro clear_step_flag pcbflags, tmp
+	tbz	\pcbflags, #PCB_SINGLE_STEP_SHIFT, 999f
+	mrs	\tmp, mdscr_el1
+	bic	\tmp, \tmp, #1
+	msr	mdscr_el1, \tmp
+	isb
+999:
+.endm
+
+.macro set_step_flag pcbflags, tmp
+	tbz	\pcbflags, #PCB_SINGLE_STEP_SHIFT, 999f
+	mrs	\tmp, mdscr_el1
+	orr	\tmp, \tmp, #1
+	msr	mdscr_el1, \tmp
+	isb
+999:
+.endm
+
 /*
  * void cpu_throw(struct thread *old, struct thread *new)
  */
 ENTRY(cpu_throw)
+	/* Of old == NULL skip disabling stepping */
+	cbz	x0, 1f
+
+	/* If we were single stepping, disable it */
+	ldr	x4, [x0, #TD_PCB]
+	ldr	w5, [x4, #PCB_FLAGS]
+	clear_step_flag w5, x6
+1:
+
 #ifdef VFP
 	/* Backup the new thread pointer around a call to C code */
 	mov	x19, x1
@@ -69,6 +96,10 @@ ENTRY(cpu_throw)
 	dsb	sy
 	isb
 
+	/* If we are single stepping, enable it */
+	ldr	w5, [x4, #PCB_FLAGS]
+	set_step_flag w5, x6
+
 	/* Restore the registers */
 	ldp	x5, x6, [x4, #PCB_SP]
 	mov	sp, x5
@@ -127,6 +158,10 @@ ENTRY(cpu_switch)
 	mrs	x6, tpidr_el0
 	stp	x5, x6, [x4, #PCB_SP]
 
+	/* If we were single stepping, disable it */
+	ldr	w5, [x4, #PCB_FLAGS]
+	clear_step_flag w5, x6
+
 #ifdef VFP
 	mov	x19, x0
 	mov	x20, x1
@@ -174,6 +209,10 @@ ENTRY(cpu_switch)
 	b.eq	1b
 #endif
 
+	/* If we are single stepping, enable it */
+	ldr	w5, [x4, #PCB_FLAGS]
+	set_step_flag w5, x6
+
 	/* Restore the registers */
 	ldp	x5, x6, [x4, #PCB_SP]
 	mov	sp, x5
diff --git a/sys/arm64/arm64/trap.c b/sys/arm64/arm64/trap.c
index ac810ad..89d6d0c 100644
--- a/sys/arm64/arm64/trap.c
+++ b/sys/arm64/arm64/trap.c
@@ -138,7 +138,6 @@ svc_handler(struct trapframe *frame)
 	int error;
 
 	td = curthread;
-	td->td_frame = frame;
 
 	error = syscallenter(td, &sa);
 	syscallret(td, error, &sa);
@@ -338,6 +337,9 @@ do_el0_sync(struct trapframe *frame)
 	    ("Invalid pcpu address from userland: %p (tpidr %lx)",
 	     get_pcpu(), READ_SPECIALREG(tpidr_el1)));
 
+	td = curthread;
+	td->td_frame = frame;
+
 	esr = READ_SPECIALREG(esr_el1);
 	exception = ESR_ELx_EXCEPTION(esr);
 	switch (exception) {
@@ -373,15 +375,22 @@ do_el0_sync(struct trapframe *frame)
 		el0_excp_unknown(frame);
 		break;
 	case EXCP_PC_ALIGN:
-		td = curthread;
 		call_trapsignal(td, SIGBUS, BUS_ADRALN, (void *)frame->tf_elr);
 		userret(td, frame);
 		break;
 	case EXCP_BRK:
-		td = curthread;
 		call_trapsignal(td, SIGTRAP, TRAP_BRKPT, (void *)frame->tf_elr);
 		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) & ~DBG_MDSCR_SS);
+		call_trapsignal(td, SIGTRAP, TRAP_TRACE,
+		    (void *)frame->tf_elr);
+		userret(td, frame);
+		break;
 	default:
 		print_registers(frame);
 		panic("Unknown userland exception %x esr_el1 %lx\n", exception,
diff --git a/sys/arm64/include/armreg.h b/sys/arm64/include/armreg.h
index ffcf989..0c86462 100644
--- a/sys/arm64/include/armreg.h
+++ b/sys/arm64/include/armreg.h
@@ -101,6 +101,7 @@
 #define	 EXCP_SP_ALIGN		0x26	/* SP slignment fault */
 #define	 EXCP_TRAP_FP		0x2c	/* Trapped FP exception */
 #define	 EXCP_SERROR		0x2f	/* SError interrupt */
+#define	 EXCP_SOFTSTP_EL0	0x32	/* Software Step, from lower EL */
 #define	 EXCP_SOFTSTP_EL1	0x33	/* Software Step, from same EL */
 #define	 EXCP_WATCHPT_EL1	0x35	/* Watchpoint, from same EL */
 #define	 EXCP_BRK		0x3c	/* Breakpoint */
diff --git a/sys/arm64/include/pcb.h b/sys/arm64/include/pcb.h
index 027b605..55dd6e9 100644
--- a/sys/arm64/include/pcb.h
+++ b/sys/arm64/include/pcb.h
@@ -45,6 +45,10 @@ struct pcb {
 	/* Fault handler, the error value is passed in x0 */
 	vm_offset_t	pcb_onfault;
 
+	u_int		pcb_flags;
+#define	PCB_SINGLE_STEP_SHIFT	0
+#define	PCB_SINGLE_STEP		(1 << PCB_SINGLE_STEP_SHIFT)
+
 	/* Place last to simplify the asm to access the rest if the struct */
 	__uint128_t	pcb_vfp[32];
 	uint32_t	pcb_fpcr;