xtensa: support hardware breakpoints/watchpoints

Use perf framework to manage hardware instruction and data breakpoints.
Add two new ptrace calls: PTRACE_GETHBPREGS and PTRACE_SETHBPREGS to
query and set instruction and data breakpoints.
Address bit 0 choose instruction (0) or data (1) break register, bits
31..1 are the register number.
Both calls transfer two 32-bit words: address (0) and control (1).
Instruction breakpoint contorl word is 0 to clear breakpoint, 1 to set.
Data breakpoint control word bit 31 is 'trigger on store', bit 30 is
'trigger on load, bits 29..0 are length. Length 0 is used to clear a
breakpoint. To set a breakpoint length must be a power of 2 in the range
1..64 and the address must be length-aligned.

Introduce new thread_info flag: TIF_DB_DISABLED. Set it if debug
exception is raised by the kernel code accessing watched userspace
address and disable corresponding data breakpoint. On exit to userspace
check that flag and, if set, restore all data breakpoints.

Handle debug exceptions raised with PS.EXCM set. This may happen when
window overflow/underflow handler or fast exception handler hits data
breakpoint, in which case save and disable all data breakpoints,
single-step faulting instruction and restore data breakpoints.

Signed-off-by: Max Filippov <jcmvbkbc@gmail.com>

7 files changed, 571 insertions, 8 deletions

diff --git a/arch/xtensa/kernel/Makefile b/arch/xtensa/kernel/Makefile
index 17fa04dce3ba..c31f5d5afc7d 100644
--- a/arch/xtensa/kernel/Makefile
+++ b/arch/xtensa/kernel/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_MODULES) += xtensa_ksyms.o module.o
 obj-$(CONFIG_FUNCTION_TRACER) += mcount.o
 obj-$(CONFIG_SMP) += smp.o mxhead.o
 obj-$(CONFIG_XTENSA_VARIANT_HAVE_PERF_EVENTS) += perf_event.o
+obj-$(CONFIG_HAVE_HW_BREAKPOINT) += hw_breakpoint.o
 
 AFLAGS_head.o += -mtext-section-literals
 AFLAGS_mxhead.o += -mtext-section-literals
diff --git a/arch/xtensa/kernel/asm-offsets.c b/arch/xtensa/kernel/asm-offsets.c
index 8fd46c6404ea..8e10e357ee32 100644
--- a/arch/xtensa/kernel/asm-offsets.c
+++ b/arch/xtensa/kernel/asm-offsets.c
@@ -122,6 +122,12 @@ int main(void)
 	DEFINE(DT_DEBUG_EXCEPTION,
 	       offsetof(struct debug_table, debug_exception));
 	DEFINE(DT_DEBUG_SAVE, offsetof(struct debug_table, debug_save));
+#ifdef CONFIG_HAVE_HW_BREAKPOINT
+	DEFINE(DT_DBREAKC_SAVE, offsetof(struct debug_table, dbreakc_save));
+	DEFINE(DT_ICOUNT_SAVE, offsetof(struct debug_table, icount_save));
+	DEFINE(DT_ICOUNT_LEVEL_SAVE,
+	       offsetof(struct debug_table, icount_level_save));
+#endif
 
 	return 0;
 }
diff --git a/arch/xtensa/kernel/entry.S b/arch/xtensa/kernel/entry.S
index ab7904fd027f..fe8f7e7efb9d 100644
--- a/arch/xtensa/kernel/entry.S
+++ b/arch/xtensa/kernel/entry.S
@@ -543,6 +543,12 @@ common_exception_return:
 #endif
 
 5:
+#ifdef CONFIG_HAVE_HW_BREAKPOINT
+	_bbci.l	a4, TIF_DB_DISABLED, 7f
+	movi	a4, restore_dbreak
+	callx4	a4
+7:
+#endif
 #ifdef CONFIG_DEBUG_TLB_SANITY
 	l32i	a4, a1, PT_DEPC
 	bgeui	a4, VALID_DOUBLE_EXCEPTION_ADDRESS, 4f
@@ -808,6 +814,18 @@ ENTRY(debug_exception)
 	s32i	a0, a2, PT_AREG2
 	mov	a1, a2
 
+	/* Debug exception is handled as an exception, so interrupts will
+	 * likely be enabled in the common exception handler. Disable
+	 * preemption if we have HW breakpoints to preserve DEBUGCAUSE.DBNUM
+	 * meaning.
+	 */
+#if defined(CONFIG_PREEMPT_COUNT) && defined(CONFIG_HAVE_HW_BREAKPOINT)
+	GET_THREAD_INFO(a2, a1)
+	l32i	a3, a2, TI_PRE_COUNT
+	addi	a3, a3, 1
+	s32i	a3, a2, TI_PRE_COUNT
+#endif
+
 	rsr	a2, ps
 	bbsi.l	a2, PS_UM_BIT, _user_exception
 	j	_kernel_exception
@@ -816,8 +834,60 @@ ENTRY(debug_exception)
 	l32i	a2, a2, EXC_TABLE_KSTK	# load kernel stack pointer
 	j	3b
 
-	/* Debug exception while in exception mode. */
+#ifdef CONFIG_HAVE_HW_BREAKPOINT
+	/* Debug exception while in exception mode. This may happen when
+	 * window overflow/underflow handler or fast exception handler hits
+	 * data breakpoint, in which case save and disable all data
+	 * breakpoints, single-step faulting instruction and restore data
+	 * breakpoints.
+	 */
+1:
+	bbci.l	a0, PS_UM_BIT, 1b	# jump if kernel mode
+
+	rsr	a0, debugcause
+	bbsi.l	a0, DEBUGCAUSE_DBREAK_BIT, .Ldebug_save_dbreak
+
+	.set	_index, 0
+	.rept	XCHAL_NUM_DBREAK
+	l32i	a0, a3, DT_DBREAKC_SAVE + _index * 4
+	wsr	a0, SREG_DBREAKC + _index
+	.set	_index, _index + 1
+	.endr
+
+	l32i	a0, a3, DT_ICOUNT_LEVEL_SAVE
+	wsr	a0, icountlevel
+
+	l32i	a0, a3, DT_ICOUNT_SAVE
+	xsr	a0, icount
+
+	l32i	a0, a3, DT_DEBUG_SAVE
+	xsr	a3, SREG_EXCSAVE + XCHAL_DEBUGLEVEL
+	rfi	XCHAL_DEBUGLEVEL
+
+.Ldebug_save_dbreak:
+	.set	_index, 0
+	.rept	XCHAL_NUM_DBREAK
+	movi	a0, 0
+	xsr	a0, SREG_DBREAKC + _index
+	s32i	a0, a3, DT_DBREAKC_SAVE + _index * 4
+	.set	_index, _index + 1
+	.endr
+
+	movi	a0, XCHAL_EXCM_LEVEL + 1
+	xsr	a0, icountlevel
+	s32i	a0, a3, DT_ICOUNT_LEVEL_SAVE
+
+	movi	a0, 0xfffffffe
+	xsr	a0, icount
+	s32i	a0, a3, DT_ICOUNT_SAVE
+
+	l32i	a0, a3, DT_DEBUG_SAVE
+	xsr	a3, SREG_EXCSAVE + XCHAL_DEBUGLEVEL
+	rfi	XCHAL_DEBUGLEVEL
+#else
+	/* Debug exception while in exception mode. Should not happen. */
 1:	j	1b	// FIXME!!
+#endif
 
 ENDPROC(debug_exception)
 
diff --git a/arch/xtensa/kernel/hw_breakpoint.c b/arch/xtensa/kernel/hw_breakpoint.c
new file mode 100644
index 000000000000..b35656ab7dbd
--- /dev/null
+++ b/arch/xtensa/kernel/hw_breakpoint.c
@@ -0,0 +1,317 @@
+/*
+ * Xtensa hardware breakpoints/watchpoints handling functions
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ *
+ * Copyright (C) 2016 Cadence Design Systems Inc.
+ */
+
+#include <linux/hw_breakpoint.h>
+#include <linux/log2.h>
+#include <linux/percpu.h>
+#include <linux/perf_event.h>
+#include <variant/core.h>
+
+/* Breakpoint currently in use for each IBREAKA. */
+static DEFINE_PER_CPU(struct perf_event *, bp_on_reg[XCHAL_NUM_IBREAK]);
+
+/* Watchpoint currently in use for each DBREAKA. */
+static DEFINE_PER_CPU(struct perf_event *, wp_on_reg[XCHAL_NUM_DBREAK]);
+
+int hw_breakpoint_slots(int type)
+{
+	switch (type) {
+	case TYPE_INST:
+		return XCHAL_NUM_IBREAK;
+	case TYPE_DATA:
+		return XCHAL_NUM_DBREAK;
+	default:
+		pr_warn("unknown slot type: %d\n", type);
+		return 0;
+	}
+}
+
+int arch_check_bp_in_kernelspace(struct perf_event *bp)
+{
+	unsigned int len;
+	unsigned long va;
+	struct arch_hw_breakpoint *info = counter_arch_bp(bp);
+
+	va = info->address;
+	len = bp->attr.bp_len;
+
+	return (va >= TASK_SIZE) && ((va + len - 1) >= TASK_SIZE);
+}
+
+/*
+ * Construct an arch_hw_breakpoint from a perf_event.
+ */
+static int arch_build_bp_info(struct perf_event *bp)
+{
+	struct arch_hw_breakpoint *info = counter_arch_bp(bp);
+
+	/* Type */
+	switch (bp->attr.bp_type) {
+	case HW_BREAKPOINT_X:
+		info->type = XTENSA_BREAKPOINT_EXECUTE;
+		break;
+	case HW_BREAKPOINT_R:
+		info->type = XTENSA_BREAKPOINT_LOAD;
+		break;
+	case HW_BREAKPOINT_W:
+		info->type = XTENSA_BREAKPOINT_STORE;
+		break;
+	case HW_BREAKPOINT_RW:
+		info->type = XTENSA_BREAKPOINT_LOAD | XTENSA_BREAKPOINT_STORE;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Len */
+	info->len = bp->attr.bp_len;
+	if (info->len < 1 || info->len > 64 || !is_power_of_2(info->len))
+		return -EINVAL;
+
+	/* Address */
+	info->address = bp->attr.bp_addr;
+	if (info->address & (info->len - 1))
+		return -EINVAL;
+
+	return 0;
+}
+
+int arch_validate_hwbkpt_settings(struct perf_event *bp)
+{
+	int ret;
+
+	/* Build the arch_hw_breakpoint. */
+	ret = arch_build_bp_info(bp);
+	return ret;
+}
+
+int hw_breakpoint_exceptions_notify(struct notifier_block *unused,
+				    unsigned long val, void *data)
+{
+	return NOTIFY_DONE;
+}
+
+static void xtensa_wsr(unsigned long v, u8 sr)
+{
+	/* We don't have indexed wsr and creating instruction dynamically
+	 * doesn't seem worth it given how small XCHAL_NUM_IBREAK and
+	 * XCHAL_NUM_DBREAK are. Thus the switch. In case build breaks here
+	 * the switch below needs to be extended.
+	 */
+	BUILD_BUG_ON(XCHAL_NUM_IBREAK > 2);
+	BUILD_BUG_ON(XCHAL_NUM_DBREAK > 2);
+
+	switch (sr) {
+#if XCHAL_NUM_IBREAK > 0
+	case SREG_IBREAKA + 0:
+		WSR(v, SREG_IBREAKA + 0);
+		break;
+#endif
+#if XCHAL_NUM_IBREAK > 1
+	case SREG_IBREAKA + 1:
+		WSR(v, SREG_IBREAKA + 1);
+		break;
+#endif
+
+#if XCHAL_NUM_DBREAK > 0
+	case SREG_DBREAKA + 0:
+		WSR(v, SREG_DBREAKA + 0);
+		break;
+	case SREG_DBREAKC + 0:
+		WSR(v, SREG_DBREAKC + 0);
+		break;
+#endif
+#if XCHAL_NUM_DBREAK > 1
+	case SREG_DBREAKA + 1:
+		WSR(v, SREG_DBREAKA + 1);
+		break;
+
+	case SREG_DBREAKC + 1:
+		WSR(v, SREG_DBREAKC + 1);
+		break;
+#endif
+	}
+}
+
+static int alloc_slot(struct perf_event **slot, size_t n,
+		      struct perf_event *bp)
+{
+	size_t i;
+
+	for (i = 0; i < n; ++i) {
+		if (!slot[i]) {
+			slot[i] = bp;
+			return i;
+		}
+	}
+	return -EBUSY;
+}
+
+static void set_ibreak_regs(int reg, struct perf_event *bp)
+{
+	struct arch_hw_breakpoint *info = counter_arch_bp(bp);
+	unsigned long ibreakenable;
+
+	xtensa_wsr(info->address, SREG_IBREAKA + reg);
+	RSR(ibreakenable, SREG_IBREAKENABLE);
+	WSR(ibreakenable | (1 << reg), SREG_IBREAKENABLE);
+}
+
+static void set_dbreak_regs(int reg, struct perf_event *bp)
+{
+	struct arch_hw_breakpoint *info = counter_arch_bp(bp);
+	unsigned long dbreakc = DBREAKC_MASK_MASK & -info->len;
+
+	if (info->type & XTENSA_BREAKPOINT_LOAD)
+		dbreakc |= DBREAKC_LOAD_MASK;
+	if (info->type & XTENSA_BREAKPOINT_STORE)
+		dbreakc |= DBREAKC_STOR_MASK;
+
+	xtensa_wsr(info->address, SREG_DBREAKA + reg);
+	xtensa_wsr(dbreakc, SREG_DBREAKC + reg);
+}
+
+int arch_install_hw_breakpoint(struct perf_event *bp)
+{
+	int i;
+
+	if (counter_arch_bp(bp)->type == XTENSA_BREAKPOINT_EXECUTE) {
+		/* Breakpoint */
+		i = alloc_slot(this_cpu_ptr(bp_on_reg), XCHAL_NUM_IBREAK, bp);
+		if (i < 0)
+			return i;
+		set_ibreak_regs(i, bp);
+
+	} else {
+		/* Watchpoint */
+		i = alloc_slot(this_cpu_ptr(wp_on_reg), XCHAL_NUM_DBREAK, bp);
+		if (i < 0)
+			return i;
+		set_dbreak_regs(i, bp);
+	}
+	return 0;
+}
+
+static int free_slot(struct perf_event **slot, size_t n,
+		     struct perf_event *bp)
+{
+	size_t i;
+
+	for (i = 0; i < n; ++i) {
+		if (slot[i] == bp) {
+			slot[i] = NULL;
+			return i;
+		}
+	}
+	return -EBUSY;
+}
+
+void arch_uninstall_hw_breakpoint(struct perf_event *bp)
+{
+	struct arch_hw_breakpoint *info = counter_arch_bp(bp);
+	int i;
+
+	if (info->type == XTENSA_BREAKPOINT_EXECUTE) {
+		unsigned long ibreakenable;
+
+		/* Breakpoint */
+		i = free_slot(this_cpu_ptr(bp_on_reg), XCHAL_NUM_IBREAK, bp);
+		if (i >= 0) {
+			RSR(ibreakenable, SREG_IBREAKENABLE);
+			WSR(ibreakenable & ~(1 << i), SREG_IBREAKENABLE);
+		}
+	} else {
+		/* Watchpoint */
+		i = free_slot(this_cpu_ptr(wp_on_reg), XCHAL_NUM_DBREAK, bp);
+		if (i >= 0)
+			xtensa_wsr(0, SREG_DBREAKC + i);
+	}
+}
+
+void hw_breakpoint_pmu_read(struct perf_event *bp)
+{
+}
+
+void flush_ptrace_hw_breakpoint(struct task_struct *tsk)
+{
+	int i;
+	struct thread_struct *t = &tsk->thread;
+
+	for (i = 0; i < XCHAL_NUM_IBREAK; ++i) {
+		if (t->ptrace_bp[i]) {
+			unregister_hw_breakpoint(t->ptrace_bp[i]);
+			t->ptrace_bp[i] = NULL;
+		}
+	}
+	for (i = 0; i < XCHAL_NUM_DBREAK; ++i) {
+		if (t->ptrace_wp[i]) {
+			unregister_hw_breakpoint(t->ptrace_wp[i]);
+			t->ptrace_wp[i] = NULL;
+		}
+	}
+}
+
+/*
+ * Set ptrace breakpoint pointers to zero for this task.
+ * This is required in order to prevent child processes from unregistering
+ * breakpoints held by their parent.
+ */
+void clear_ptrace_hw_breakpoint(struct task_struct *tsk)
+{
+	memset(tsk->thread.ptrace_bp, 0, sizeof(tsk->thread.ptrace_bp));
+	memset(tsk->thread.ptrace_wp, 0, sizeof(tsk->thread.ptrace_wp));
+}
+
+void restore_dbreak(void)
+{
+	int i;
+
+	for (i = 0; i < XCHAL_NUM_DBREAK; ++i) {
+		struct perf_event *bp = this_cpu_ptr(wp_on_reg)[i];
+
+		if (bp)
+			set_dbreak_regs(i, bp);
+	}
+	clear_thread_flag(TIF_DB_DISABLED);
+}
+
+int check_hw_breakpoint(struct pt_regs *regs)
+{
+	if (regs->debugcause & BIT(DEBUGCAUSE_IBREAK_BIT)) {
+		int i;
+		struct perf_event **bp = this_cpu_ptr(bp_on_reg);
+
+		for (i = 0; i < XCHAL_NUM_IBREAK; ++i) {
+			if (bp[i] && !bp[i]->attr.disabled &&
+			    regs->pc == bp[i]->attr.bp_addr)
+				perf_bp_event(bp[i], regs);
+		}
+		return 0;
+	} else if (regs->debugcause & BIT(DEBUGCAUSE_DBREAK_BIT)) {
+		struct perf_event **bp = this_cpu_ptr(wp_on_reg);
+		int dbnum = (regs->debugcause & DEBUGCAUSE_DBNUM_MASK) >>
+			DEBUGCAUSE_DBNUM_SHIFT;
+
+		if (dbnum < XCHAL_NUM_DBREAK && bp[dbnum]) {
+			if (user_mode(regs)) {
+				perf_bp_event(bp[dbnum], regs);
+			} else {
+				set_thread_flag(TIF_DB_DISABLED);
+				xtensa_wsr(0, SREG_DBREAKC + dbnum);
+			}
+		} else {
+			WARN_ONCE(1,
+				  "Wrong/unconfigured DBNUM reported in DEBUGCAUSE: %d\n",
+				  dbnum);
+		}
+		return 0;
+	}
+	return -ENOENT;
+}
diff --git a/arch/xtensa/kernel/process.c b/arch/xtensa/kernel/process.c
index 1c85323f01d7..5bbfed81c97b 100644
--- a/arch/xtensa/kernel/process.c
+++ b/arch/xtensa/kernel/process.c
@@ -24,6 +24,7 @@
 #include <linux/unistd.h>
 #include <linux/ptrace.h>
 #include <linux/elf.h>
+#include <linux/hw_breakpoint.h>
 #include <linux/init.h>
 #include <linux/prctl.h>
 #include <linux/init_task.h>
@@ -43,6 +44,7 @@
 #include <linux/atomic.h>
 #include <asm/asm-offsets.h>
 #include <asm/regs.h>
+#include <asm/hw_breakpoint.h>
 
 extern void ret_from_fork(void);
 extern void ret_from_kernel_thread(void);
@@ -131,6 +133,7 @@ void flush_thread(void)
 	coprocessor_flush_all(ti);
 	coprocessor_release_all(ti);
 #endif
+	flush_ptrace_hw_breakpoint(current);
 }
 
 /*
@@ -273,6 +276,8 @@ int copy_thread(unsigned long clone_flags, unsigned long usp_thread_fn,
 	ti->cpenable = 0;
 #endif
 
+	clear_ptrace_hw_breakpoint(p);
+
 	return 0;
 }
 
diff --git a/arch/xtensa/kernel/ptrace.c b/arch/xtensa/kernel/ptrace.c
index 4d54b481123b..a651f3a628ee 100644
--- a/arch/xtensa/kernel/ptrace.c
+++ b/arch/xtensa/kernel/ptrace.c
@@ -13,21 +13,23 @@
  * Marc Gauthier<marc@tensilica.com> <marc@alumni.uwaterloo.ca>
  */
 
+#include <linux/errno.h>
+#include <linux/hw_breakpoint.h>
 #include <linux/kernel.h>
-#include <linux/sched.h>
 #include <linux/mm.h>
-#include <linux/errno.h>
+#include <linux/perf_event.h>
 #include <linux/ptrace.h>
-#include <linux/smp.h>
+#include <linux/sched.h>
 #include <linux/security.h>
 #include <linux/signal.h>
+#include <linux/smp.h>
 
-#include <asm/pgtable.h>
+#include <asm/coprocessor.h>
+#include <asm/elf.h>
 #include <asm/page.h>
-#include <asm/uaccess.h>
+#include <asm/pgtable.h>
 #include <asm/ptrace.h>
-#include <asm/elf.h>
-#include <asm/coprocessor.h>
+#include <asm/uaccess.h>
 
 
 void user_enable_single_step(struct task_struct *child)
@@ -267,6 +269,146 @@ int ptrace_pokeusr(struct task_struct *child, long regno, long val)
 	return 0;
 }
 
+#ifdef CONFIG_HAVE_HW_BREAKPOINT
+static void ptrace_hbptriggered(struct perf_event *bp,
+				struct perf_sample_data *data,
+				struct pt_regs *regs)
+{
+	int i;
+	siginfo_t info;
+	struct arch_hw_breakpoint *bkpt = counter_arch_bp(bp);
+
+	if (bp->attr.bp_type & HW_BREAKPOINT_X) {
+		for (i = 0; i < XCHAL_NUM_IBREAK; ++i)
+			if (current->thread.ptrace_bp[i] == bp)
+				break;
+		i <<= 1;
+	} else {
+		for (i = 0; i < XCHAL_NUM_DBREAK; ++i)
+			if (current->thread.ptrace_wp[i] == bp)
+				break;
+		i = (i << 1) | 1;
+	}
+
+	info.si_signo = SIGTRAP;
+	info.si_errno = i;
+	info.si_code = TRAP_HWBKPT;
+	info.si_addr = (void __user *)bkpt->address;
+
+	force_sig_info(SIGTRAP, &info, current);
+}
+
+static struct perf_event *ptrace_hbp_create(struct task_struct *tsk, int type)
+{
+	struct perf_event_attr attr;
+
+	ptrace_breakpoint_init(&attr);
+
+	/* Initialise fields to sane defaults. */
+	attr.bp_addr	= 0;
+	attr.bp_len	= 1;
+	attr.bp_type	= type;
+	attr.disabled	= 1;
+
+	return register_user_hw_breakpoint(&attr, ptrace_hbptriggered, NULL,
+					   tsk);
+}
+
+/*
+ * Address bit 0 choose instruction (0) or data (1) break register, bits
+ * 31..1 are the register number.
+ * Both PTRACE_GETHBPREGS and PTRACE_SETHBPREGS transfer two 32-bit words:
+ * address (0) and control (1).
+ * Instruction breakpoint contorl word is 0 to clear breakpoint, 1 to set.
+ * Data breakpoint control word bit 31 is 'trigger on store', bit 30 is
+ * 'trigger on load, bits 29..0 are length. Length 0 is used to clear a
+ * breakpoint. To set a breakpoint length must be a power of 2 in the range
+ * 1..64 and the address must be length-aligned.
+ */
+
+static long ptrace_gethbpregs(struct task_struct *child, long addr,
+			      long __user *datap)
+{
+	struct perf_event *bp;
+	u32 user_data[2] = {0};
+	bool dbreak = addr & 1;
+	unsigned idx = addr >> 1;
+
+	if ((!dbreak && idx >= XCHAL_NUM_IBREAK) ||
+	    (dbreak && idx >= XCHAL_NUM_DBREAK))
+		return -EINVAL;
+
+	if (dbreak)
+		bp = child->thread.ptrace_wp[idx];
+	else
+		bp = child->thread.ptrace_bp[idx];
+
+	if (bp) {
+		user_data[0] = bp->attr.bp_addr;
+		user_data[1] = bp->attr.disabled ? 0 : bp->attr.bp_len;
+		if (dbreak) {
+			if (bp->attr.bp_type & HW_BREAKPOINT_R)
+				user_data[1] |= DBREAKC_LOAD_MASK;
+			if (bp->attr.bp_type & HW_BREAKPOINT_W)
+				user_data[1] |= DBREAKC_STOR_MASK;
+		}
+	}
+
+	if (copy_to_user(datap, user_data, sizeof(user_data)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static long ptrace_sethbpregs(struct task_struct *child, long addr,
+			      long __user *datap)
+{
+	struct perf_event *bp;
+	struct perf_event_attr attr;
+	u32 user_data[2];
+	bool dbreak = addr & 1;
+	unsigned idx = addr >> 1;
+	int bp_type = 0;
+
+	if ((!dbreak && idx >= XCHAL_NUM_IBREAK) ||
+	    (dbreak && idx >= XCHAL_NUM_DBREAK))
+		return -EINVAL;
+
+	if (copy_from_user(user_data, datap, sizeof(user_data)))
+		return -EFAULT;
+
+	if (dbreak) {
+		bp = child->thread.ptrace_wp[idx];
+		if (user_data[1] & DBREAKC_LOAD_MASK)
+			bp_type |= HW_BREAKPOINT_R;
+		if (user_data[1] & DBREAKC_STOR_MASK)
+			bp_type |= HW_BREAKPOINT_W;
+	} else {
+		bp = child->thread.ptrace_bp[idx];
+		bp_type = HW_BREAKPOINT_X;
+	}
+
+	if (!bp) {
+		bp = ptrace_hbp_create(child,
+				       bp_type ? bp_type : HW_BREAKPOINT_RW);
+		if (IS_ERR(bp))
+			return PTR_ERR(bp);
+		if (dbreak)
+			child->thread.ptrace_wp[idx] = bp;
+		else
+			child->thread.ptrace_bp[idx] = bp;
+	}
+
+	attr = bp->attr;
+	attr.bp_addr = user_data[0];
+	attr.bp_len = user_data[1] & ~(DBREAKC_LOAD_MASK | DBREAKC_STOR_MASK);
+	attr.bp_type = bp_type;
+	attr.disabled = !attr.bp_len;
+
+	return modify_user_hw_breakpoint(bp, &attr);
+}
+#endif
+
 long arch_ptrace(struct task_struct *child, long request,
 		 unsigned long addr, unsigned long data)
 {
@@ -307,7 +449,15 @@ long arch_ptrace(struct task_struct *child, long request,
 	case PTRACE_SETXTREGS:
 		ret = ptrace_setxregs(child, datap);
 		break;
+#ifdef CONFIG_HAVE_HW_BREAKPOINT
+	case PTRACE_GETHBPREGS:
+		ret = ptrace_gethbpregs(child, addr, datap);
+		break;
 
+	case PTRACE_SETHBPREGS:
+		ret = ptrace_sethbpregs(child, addr, datap);
+		break;
+#endif
 	default:
 		ret = ptrace_request(child, request, addr, data);
 		break;
diff --git a/arch/xtensa/kernel/traps.c b/arch/xtensa/kernel/traps.c
index e4764f216a7a..d02fc304b31c 100644
--- a/arch/xtensa/kernel/traps.c
+++ b/arch/xtensa/kernel/traps.c
@@ -39,6 +39,7 @@
 #include <asm/pgtable.h>
 #include <asm/processor.h>
 #include <asm/traps.h>
+#include <asm/hw_breakpoint.h>
 
 /*
  * Machine specific interrupt handlers
@@ -338,9 +339,22 @@ do_unaligned_user (struct pt_regs *regs)
 }
 #endif
 
+/* Handle debug events.
+ * When CONFIG_HAVE_HW_BREAKPOINT is on this handler is called with
+ * preemption disabled to avoid rescheduling and keep mapping of hardware
+ * breakpoint structures to debug registers intact, so that
+ * DEBUGCAUSE.DBNUM could be used in case of data breakpoint hit.
+ */
 void
 do_debug(struct pt_regs *regs)
 {
+#ifdef CONFIG_HAVE_HW_BREAKPOINT
+	int ret = check_hw_breakpoint(regs);
+
+	preempt_enable();
+	if (ret == 0)
+		return;
+#endif
 	__die_if_kernel("Breakpoint in kernel", regs, SIGKILL);
 
 	/* If in user mode, send SIGTRAP signal to current process */