[RFC PATCH 2/2] arm64:kgdb: Fix kernel single-stepping

[minyard@acm.org]

From: Corey Minyard <cminyard@mvista.com>

Single stepping on arm64 in kernel mode was just broken.  Here are the
problems:

If single step is set and an interrupt is pending, the interrupt is
taken.  The interrupt should run and then return and the single stepped
instruction run.  However, instead, as soon as the kernel calls
enable_dbg, the single step happens right there.  To fix this,
disable SS in the MDSCR register on entry and save it's value for later
use.

The SS bit in the MDSCR register is set globally for the CPU, not for the
task being single stepped.  If the task migrates, that could cause the
bit to be set on the wrong core.  So instead, store the value of SS in
the thread structure and set it properly on exit back to the kernel.

If a single step occurs, it clears the SS bit in PSTATE.  So subsiquent
single steps will not work.  The bit needs to be reset on each single
step operation.

Signed-off-by: Corey Minyard <cminyard@mvista.com>
---
 arch/arm64/include/asm/ptrace.h    |  4 ++--
 arch/arm64/kernel/asm-offsets.c    |  1 +
 arch/arm64/kernel/debug-monitors.c |  7 ++++---
 arch/arm64/kernel/entry.S          | 21 +++++++++++++++++++++
 arch/arm64/kernel/kgdb.c           |  3 +++
 5 files changed, 31 insertions(+), 5 deletions(-)

diff --git a/arch/arm64/include/asm/ptrace.h b/arch/arm64/include/asm/ptrace.h
index fbebb411ae20..a07847deff8f 100644
--- a/arch/arm64/include/asm/ptrace.h
+++ b/arch/arm64/include/asm/ptrace.h
@@ -168,11 +168,11 @@ struct pt_regs {
 	};
 	u64 orig_x0;
 #ifdef __AARCH64EB__
-	u32 unused2;
+	u32 ss_enable; /* Kernel single-step for a task */
 	s32 syscallno;
 #else
 	s32 syscallno;
-	u32 unused2;
+	u32 ss_enable;
 #endif
 
 	u64 orig_addr_limit;
diff --git a/arch/arm64/kernel/asm-offsets.c b/arch/arm64/kernel/asm-offsets.c
index a5bdce8af65b..038e76d2f0c4 100644
--- a/arch/arm64/kernel/asm-offsets.c
+++ b/arch/arm64/kernel/asm-offsets.c
@@ -62,6 +62,7 @@ int main(void)
   DEFINE(S_PSTATE,		offsetof(struct pt_regs, pstate));
   DEFINE(S_PC,			offsetof(struct pt_regs, pc));
   DEFINE(S_SYSCALLNO,		offsetof(struct pt_regs, syscallno));
+  DEFINE(S_SS_ENABLE,		offsetof(struct pt_regs, ss_enable));
   DEFINE(S_ORIG_ADDR_LIMIT,	offsetof(struct pt_regs, orig_addr_limit));
   DEFINE(S_PMR_SAVE,		offsetof(struct pt_regs, pmr_save));
   DEFINE(S_STACKFRAME,		offsetof(struct pt_regs, stackframe));
diff --git a/arch/arm64/kernel/debug-monitors.c b/arch/arm64/kernel/debug-monitors.c
index 2a0dfd8b1265..d4085cfef5a7 100644
--- a/arch/arm64/kernel/debug-monitors.c
+++ b/arch/arm64/kernel/debug-monitors.c
@@ -409,7 +409,7 @@ void kernel_enable_single_step(struct pt_regs *regs)
 {
 	WARN_ON(!irqs_disabled());
 	set_regs_spsr_ss(regs);
-	mdscr_write(mdscr_read() | DBG_MDSCR_SS);
+	regs->ss_enable = DBG_MDSCR_SS;
 	enable_debug_monitors(DBG_ACTIVE_EL1);
 }
 NOKPROBE_SYMBOL(kernel_enable_single_step);
@@ -417,7 +417,8 @@ NOKPROBE_SYMBOL(kernel_enable_single_step);
 void kernel_disable_single_step(struct pt_regs *regs)
 {
 	WARN_ON(!irqs_disabled());
-	mdscr_write(mdscr_read() & ~DBG_MDSCR_SS);
+	regs->ss_enable = 0;
+	clear_regs_spsr_ss(regs);
 	disable_debug_monitors(DBG_ACTIVE_EL1);
 }
 NOKPROBE_SYMBOL(kernel_disable_single_step);
@@ -425,7 +426,7 @@ NOKPROBE_SYMBOL(kernel_disable_single_step);
 int kernel_active_single_step(struct pt_regs *regs)
 {
 	WARN_ON(!irqs_disabled());
-	return mdscr_read() & DBG_MDSCR_SS;
+	return regs->ss_enable;
 }
 NOKPROBE_SYMBOL(kernel_active_single_step);
 
diff --git a/arch/arm64/kernel/entry.S b/arch/arm64/kernel/entry.S
index 7c6a0a41676f..fcf979b17d94 100644
--- a/arch/arm64/kernel/entry.S
+++ b/arch/arm64/kernel/entry.S
@@ -174,6 +174,17 @@ alternative_cb_end
 	apply_ssbd 1, x22, x23
 
 	.else
+	/*
+	 * If single-step was enabled, save it off and disable it,
+	 * or it will trap on clearing the D bit in PSTATE.
+	 * The restore code will re-enable it if necessary.
+	 */
+	mrs	x20, mdscr_el1
+	bic	x21, x20, #1
+	msr	mdscr_el1, x21
+	and	x20, x20, #1
+	str	w20, [sp, #S_SS_ENABLE]
+
 	add	x21, sp, #S_FRAME_SIZE
 	get_current_task tsk
 	/* Save the task's original addr_limit and set USER_DS */
@@ -349,6 +360,16 @@ alternative_else_nop_endif
 
 	msr	elr_el1, x21			// set up the return data
 	msr	spsr_el1, x22
+
+	.if	\el != 0
+	/* Restore the single-step bit. */
+	ldr	w21, [sp, #S_SS_ENABLE]
+	mrs	x20, mdscr_el1
+	orr	x20, x20, x21
+	msr	mdscr_el1, x20
+	/* PSTATE.D and PSTATE.SS will be restored from SPSR_EL1. */
+	.endif
+
 	ldp	x0, x1, [sp, #16 * 0]
 	ldp	x2, x3, [sp, #16 * 1]
 	ldp	x4, x5, [sp, #16 * 2]
diff --git a/arch/arm64/kernel/kgdb.c b/arch/arm64/kernel/kgdb.c
index 220fe8fd6ace..260f12c76b6e 100644
--- a/arch/arm64/kernel/kgdb.c
+++ b/arch/arm64/kernel/kgdb.c
@@ -223,6 +223,9 @@ int kgdb_arch_handle_exception(int exception_vector, int signo,
 		 */
 		if (!kernel_active_single_step(linux_regs))
 			kernel_enable_single_step(linux_regs);
+		else
+			/* Doing a single-step clears ss, reset it. */
+			linux_regs->pstate |= DBG_SPSR_SS;
 		err = 0;
 		break;
 	default:
-- 
2.17.1