Introduce ddb(4) support for ARM64

Obtained from: Semihalf
Reviewed by:   emaste
Sponsored by:  The FreeBSD Foundation

4 files changed, 848 insertions, 0 deletions

diff --git a/sys/arm64/arm64/db_disasm.c b/sys/arm64/arm64/db_disasm.c
new file mode 100644
index 0000000..ec943a4
--- /dev/null
+++ b/sys/arm64/arm64/db_disasm.c
@@ -0,0 +1,41 @@
+/*-
+ * Copyright (c) 2015 The FreeBSD Foundation
+ * All rights reserved.
+ *
+ * This software was developed by Semihalf under
+ * the sponsorship of the FreeBSD Foundation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+#include <sys/param.h>
+#include <ddb/ddb.h>
+
+vm_offset_t
+db_disasm(vm_offset_t loc, boolean_t altfmt)
+{
+	return 0;
+}
+
+/* End of db_disasm.c */
diff --git a/sys/arm64/arm64/db_interface.c b/sys/arm64/arm64/db_interface.c
new file mode 100644
index 0000000..ef29c77
--- /dev/null
+++ b/sys/arm64/arm64/db_interface.c
@@ -0,0 +1,168 @@
+/*-
+ * Copyright (c) 2015 The FreeBSD Foundation
+ * All rights reserved.
+ *
+ * This software was developed by Semihalf under
+ * the sponsorship of the FreeBSD Foundation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+#include <sys/param.h>
+#include <sys/proc.h>
+#include <vm/vm.h>
+#include <vm/pmap.h>
+#include <vm/vm_map.h>
+
+#ifdef KDB
+#include <sys/kdb.h>
+#endif
+
+#include <ddb/ddb.h>
+#include <ddb/db_variables.h>
+
+#include <machine/cpu.h>
+#include <machine/pcb.h>
+#include <machine/vmparam.h>
+
+static int
+db_frame(struct db_variable *vp, db_expr_t *valuep, int op)
+{
+	long *reg;
+
+	if (kdb_frame == NULL)
+		return (0);
+
+	reg = (long *)((uintptr_t)kdb_frame + (db_expr_t)vp->valuep);
+	if (op == DB_VAR_GET)
+		*valuep = *reg;
+	else
+		*reg = *valuep;
+	return (1);
+}
+
+#define DB_OFFSET(x)	(db_expr_t *)offsetof(struct trapframe, x)
+struct db_variable db_regs[] = {
+	{ "spsr", DB_OFFSET(tf_spsr),	db_frame },
+	{ "x0", DB_OFFSET(tf_x[0]),	db_frame },
+	{ "x1", DB_OFFSET(tf_x[1]),	db_frame },
+	{ "x2", DB_OFFSET(tf_x[2]),	db_frame },
+	{ "x3", DB_OFFSET(tf_x[3]),	db_frame },
+	{ "x4", DB_OFFSET(tf_x[4]),	db_frame },
+	{ "x5", DB_OFFSET(tf_x[5]),	db_frame },
+	{ "x6", DB_OFFSET(tf_x[6]),	db_frame },
+	{ "x7", DB_OFFSET(tf_x[7]),	db_frame },
+	{ "x8", DB_OFFSET(tf_x[8]),	db_frame },
+	{ "x9", DB_OFFSET(tf_x[9]),	db_frame },
+	{ "x10", DB_OFFSET(tf_x[10]),	db_frame },
+	{ "x11", DB_OFFSET(tf_x[11]),	db_frame },
+	{ "x12", DB_OFFSET(tf_x[12]),	db_frame },
+	{ "x13", DB_OFFSET(tf_x[13]),	db_frame },
+	{ "x14", DB_OFFSET(tf_x[14]),	db_frame },
+	{ "x15", DB_OFFSET(tf_x[15]),	db_frame },
+	{ "x16", DB_OFFSET(tf_x[16]),	db_frame },
+	{ "x17", DB_OFFSET(tf_x[17]),	db_frame },
+	{ "x18", DB_OFFSET(tf_x[18]),	db_frame },
+	{ "x19", DB_OFFSET(tf_x[19]),	db_frame },
+	{ "x20", DB_OFFSET(tf_x[20]),	db_frame },
+	{ "x21", DB_OFFSET(tf_x[21]),	db_frame },
+	{ "x22", DB_OFFSET(tf_x[22]),	db_frame },
+	{ "x23", DB_OFFSET(tf_x[23]),	db_frame },
+	{ "x24", DB_OFFSET(tf_x[24]),	db_frame },
+	{ "x25", DB_OFFSET(tf_x[25]),	db_frame },
+	{ "x26", DB_OFFSET(tf_x[26]),	db_frame },
+	{ "x27", DB_OFFSET(tf_x[27]),	db_frame },
+	{ "x28", DB_OFFSET(tf_x[28]),	db_frame },
+	{ "x29", DB_OFFSET(tf_x[29]),	db_frame },
+	{ "lr", DB_OFFSET(tf_lr),	db_frame },
+	{ "elr", DB_OFFSET(tf_elr),	db_frame },
+	{ "sp", DB_OFFSET(tf_sp), db_frame },
+};
+
+struct db_variable *db_eregs = db_regs + sizeof(db_regs)/sizeof(db_regs[0]);
+
+void
+db_show_mdpcpu(struct pcpu *pc)
+{
+}
+
+static int
+db_validate_address(vm_offset_t addr)
+{
+	struct proc *p = curproc;
+	struct pmap *pmap;
+
+	if (!p || !p->p_vmspace || !p->p_vmspace->vm_map.pmap ||
+	    addr >= VM_MAXUSER_ADDRESS)
+		pmap = pmap_kernel();
+	else
+		pmap = p->p_vmspace->vm_map.pmap;
+
+	return (pmap_extract(pmap, addr) == FALSE);
+}
+
+/*
+ * Read bytes from kernel address space for debugger.
+ */
+int
+db_read_bytes(vm_offset_t addr, size_t size, char *data)
+{
+	const char *src = (const char *)addr;
+
+	while (size-- > 0) {
+		if (db_validate_address((u_int)src)) {
+			db_printf("address %p is invalid\n", src);
+			return (-1);
+		}
+		*data++ = *src++;
+	}
+	return (0);
+}
+
+/*
+ * Write bytes to kernel address space for debugger.
+ */
+int
+db_write_bytes(vm_offset_t addr, size_t size, char *data)
+{
+	char *dst;
+
+	dst = (char *)addr;
+	while (size-- > 0) {
+		if (db_validate_address((u_int)dst)) {
+			db_printf("address %p is invalid\n", dst);
+			return (-1);
+		}
+		*dst++ = *data++;
+	}
+
+	dsb();
+	/* Clean D-cache and invalidate I-cache */
+	cpu_dcache_wb_range(addr, (vm_size_t)size);
+	cpu_icache_sync_range(addr, (vm_size_t)size);
+	dsb();
+	isb();
+
+	return (0);
+}
diff --git a/sys/arm64/arm64/db_trace.c b/sys/arm64/arm64/db_trace.c
new file mode 100644
index 0000000..1e89bac
--- /dev/null
+++ b/sys/arm64/arm64/db_trace.c
@@ -0,0 +1,152 @@
+/*-
+ * Copyright (c) 2015 The FreeBSD Foundation
+ * All rights reserved.
+ *
+ * This software was developed by Semihalf under
+ * the sponsorship of the FreeBSD Foundation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+#include <sys/param.h>
+#include <sys/proc.h>
+#include <sys/kdb.h>
+#include <machine/pcb.h>
+#include <ddb/ddb.h>
+#include <ddb/db_sym.h>
+
+#include <machine/armreg.h>
+#include <machine/debug_monitor.h>
+
+struct unwind_state {
+	uint64_t fp;
+	uint64_t sp;
+	uint64_t pc;
+};
+
+void
+db_md_list_watchpoints()
+{
+
+	dbg_show_watchpoint();
+}
+
+int
+db_md_clr_watchpoint(db_expr_t addr, db_expr_t size)
+{
+
+	return (dbg_remove_watchpoint(addr, size, DBG_FROM_EL1));
+}
+
+int
+db_md_set_watchpoint(db_expr_t addr, db_expr_t size)
+{
+
+	return (dbg_setup_watchpoint(addr, size, DBG_FROM_EL1,
+	    HW_BREAKPOINT_RW));
+}
+
+static int
+db_unwind_frame(struct unwind_state *frame)
+{
+	uint64_t fp = frame->fp;
+
+	if (fp == 0)
+		return -1;
+
+	frame->sp = fp + 0x10;
+	/* FP to previous frame (X29) */
+	frame->fp = *(uint64_t *)(fp);
+	/* LR (X30) */
+	frame->pc = *(uint64_t *)(fp + 8) - 4;
+	return (0);
+}
+
+static void
+db_stack_trace_cmd(struct unwind_state *frame)
+{
+	c_db_sym_t sym;
+	const char *name;
+	db_expr_t value;
+	db_expr_t offset;
+
+	while (1) {
+		uint64_t pc = frame->pc;
+		int ret;
+
+		ret = db_unwind_frame(frame);
+		if (ret < 0)
+			break;
+
+		sym = db_search_symbol(pc, DB_STGY_ANY, &offset);
+		if (sym == C_DB_SYM_NULL) {
+			value = 0;
+			name = "(null)";
+		} else
+			db_symbol_values(sym, &name, &value);
+
+		db_printf("%s() at ", name);
+		db_printsym(frame->pc, DB_STGY_PROC);
+		db_printf("\n");
+
+		db_printf("\t pc = 0x%016lx  lr = 0x%016lx\n", pc,
+		    frame->pc);
+		db_printf("\t sp = 0x%016lx  fp = 0x%016lx\n", frame->sp,
+		    frame->fp);
+		/* TODO: Show some more registers */
+		db_printf("\n");
+	}
+}
+
+int
+db_trace_thread(struct thread *thr, int count)
+{
+	struct unwind_state frame;
+	struct pcb *ctx;
+
+	if (thr != curthread) {
+		ctx = kdb_thr_ctx(thr);
+
+		frame.sp = (uint64_t)ctx->pcb_sp;
+		frame.fp = (uint64_t)ctx->pcb_x[29];
+		frame.pc = (uint64_t)ctx->pcb_x[30];
+		db_stack_trace_cmd(&frame);
+	} else
+		db_trace_self();
+	return (0);
+}
+
+void
+db_trace_self(void)
+{
+	struct unwind_state frame;
+	uint64_t sp;
+
+	__asm __volatile("mov %0, sp" : "=&r" (sp));
+
+	frame.sp = sp;
+	frame.fp = (uint64_t)__builtin_frame_address(0);
+	frame.pc = (uint64_t)db_trace_self;
+	db_stack_trace_cmd(&frame);
+}
diff --git a/sys/arm64/arm64/debug_monitor.c b/sys/arm64/arm64/debug_monitor.c
new file mode 100644
index 0000000..50d663d
--- /dev/null
+++ b/sys/arm64/arm64/debug_monitor.c
@@ -0,0 +1,487 @@
+/*-
+ * Copyright (c) 2014 The FreeBSD Foundation
+ * All rights reserved.
+ *
+ * This software was developed by Semihalf under
+ * the sponsorship of the FreeBSD Foundation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/types.h>
+#include <sys/kdb.h>
+#include <sys/pcpu.h>
+#include <sys/systm.h>
+
+#include <machine/armreg.h>
+#include <machine/cpu.h>
+#include <machine/debug_monitor.h>
+#include <machine/kdb.h>
+#include <machine/param.h>
+
+#include <ddb/ddb.h>
+#include <ddb/db_sym.h>
+
+enum dbg_t {
+	DBG_TYPE_BREAKPOINT = 0,
+	DBG_TYPE_WATCHPOINT = 1,
+};
+
+static int dbg_watchpoint_num;
+static int dbg_breakpoint_num;
+static int dbg_ref_count_mde[MAXCPU];
+static int dbg_ref_count_kde[MAXCPU];
+
+/* Watchpoints/breakpoints control register bitfields */
+#define DBG_WATCH_CTRL_LEN_1		(0x1 << 5)
+#define DBG_WATCH_CTRL_LEN_2		(0x3 << 5)
+#define DBG_WATCH_CTRL_LEN_4		(0xf << 5)
+#define DBG_WATCH_CTRL_LEN_8		(0xff << 5)
+#define DBG_WATCH_CTRL_LEN_MASK(x)	((x) & (0xff << 5))
+#define DBG_WATCH_CTRL_EXEC		(0x0 << 3)
+#define DBG_WATCH_CTRL_LOAD		(0x1 << 3)
+#define DBG_WATCH_CTRL_STORE		(0x2 << 3)
+#define DBG_WATCH_CTRL_ACCESS_MASK(x)	((x) & (0x3 << 3))
+
+/* Common for breakpoint and watchpoint */
+#define DBG_WB_CTRL_EL1		(0x1 << 1)
+#define DBG_WB_CTRL_EL0		(0x2 << 1)
+#define DBG_WB_CTRL_ELX_MASK(x)	((x) & (0x3 << 1))
+#define DBG_WB_CTRL_E		(0x1 << 0)
+
+#define DBG_REG_BASE_BVR	0
+#define DBG_REG_BASE_BCR	(DBG_REG_BASE_BVR + 16)
+#define DBG_REG_BASE_WVR	(DBG_REG_BASE_BCR + 16)
+#define DBG_REG_BASE_WCR	(DBG_REG_BASE_WVR + 16)
+
+/* Watchpoint/breakpoint helpers */
+#define DBG_WB_WVR	"wvr"
+#define DBG_WB_WCR	"wcr"
+#define DBG_WB_BVR	"bvr"
+#define DBG_WB_BCR	"bcr"
+
+#define DBG_WB_READ(reg, num, val) do {					\
+	__asm __volatile("mrs %0, dbg" reg #num "_el1" : "=r" (val));	\
+} while (0)
+
+#define DBG_WB_WRITE(reg, num, val) do {				\
+	__asm __volatile("msr dbg" reg #num "_el1, %0" :: "r" (val));	\
+} while (0)
+
+#define READ_WB_REG_CASE(reg, num, offset, val)		\
+	case (num + offset):				\
+		DBG_WB_READ(reg, num, val);		\
+		break
+
+#define WRITE_WB_REG_CASE(reg, num, offset, val)	\
+	case (num + offset):				\
+		DBG_WB_WRITE(reg, num, val);		\
+		break
+
+#define SWITCH_CASES_READ_WB_REG(reg, offset, val)	\
+	READ_WB_REG_CASE(reg,  0, offset, val);		\
+	READ_WB_REG_CASE(reg,  1, offset, val);		\
+	READ_WB_REG_CASE(reg,  2, offset, val);		\
+	READ_WB_REG_CASE(reg,  3, offset, val);		\
+	READ_WB_REG_CASE(reg,  4, offset, val);		\
+	READ_WB_REG_CASE(reg,  5, offset, val);		\
+	READ_WB_REG_CASE(reg,  6, offset, val);		\
+	READ_WB_REG_CASE(reg,  7, offset, val);		\
+	READ_WB_REG_CASE(reg,  8, offset, val);		\
+	READ_WB_REG_CASE(reg,  9, offset, val);		\
+	READ_WB_REG_CASE(reg, 10, offset, val);		\
+	READ_WB_REG_CASE(reg, 11, offset, val);		\
+	READ_WB_REG_CASE(reg, 12, offset, val);		\
+	READ_WB_REG_CASE(reg, 13, offset, val);		\
+	READ_WB_REG_CASE(reg, 14, offset, val);		\
+	READ_WB_REG_CASE(reg, 15, offset, val)
+
+#define SWITCH_CASES_WRITE_WB_REG(reg, offset, val)	\
+	WRITE_WB_REG_CASE(reg,  0, offset, val);	\
+	WRITE_WB_REG_CASE(reg,  1, offset, val);	\
+	WRITE_WB_REG_CASE(reg,  2, offset, val);	\
+	WRITE_WB_REG_CASE(reg,  3, offset, val);	\
+	WRITE_WB_REG_CASE(reg,  4, offset, val);	\
+	WRITE_WB_REG_CASE(reg,  5, offset, val);	\
+	WRITE_WB_REG_CASE(reg,  6, offset, val);	\
+	WRITE_WB_REG_CASE(reg,  7, offset, val);	\
+	WRITE_WB_REG_CASE(reg,  8, offset, val);	\
+	WRITE_WB_REG_CASE(reg,  9, offset, val);	\
+	WRITE_WB_REG_CASE(reg, 10, offset, val);	\
+	WRITE_WB_REG_CASE(reg, 11, offset, val);	\
+	WRITE_WB_REG_CASE(reg, 12, offset, val);	\
+	WRITE_WB_REG_CASE(reg, 13, offset, val);	\
+	WRITE_WB_REG_CASE(reg, 14, offset, val);	\
+	WRITE_WB_REG_CASE(reg, 15, offset, val)
+
+static uint64_t
+dbg_wb_read_reg(int reg, int n)
+{
+	uint64_t val = 0;
+
+	switch (reg + n) {
+	SWITCH_CASES_READ_WB_REG(DBG_WB_WVR, DBG_REG_BASE_WVR, val);
+	SWITCH_CASES_READ_WB_REG(DBG_WB_WCR, DBG_REG_BASE_WCR, val);
+	SWITCH_CASES_READ_WB_REG(DBG_WB_BVR, DBG_REG_BASE_BVR, val);
+	SWITCH_CASES_READ_WB_REG(DBG_WB_BCR, DBG_REG_BASE_BCR, val);
+	default:
+		db_printf("trying to read from wrong debug register %d\n", n);
+	}
+
+	return val;
+}
+
+static void
+dbg_wb_write_reg(int reg, int n, uint64_t val)
+{
+	switch (reg + n) {
+	SWITCH_CASES_WRITE_WB_REG(DBG_WB_WVR, DBG_REG_BASE_WVR, val);
+	SWITCH_CASES_WRITE_WB_REG(DBG_WB_WCR, DBG_REG_BASE_WCR, val);
+	SWITCH_CASES_WRITE_WB_REG(DBG_WB_BVR, DBG_REG_BASE_BVR, val);
+	SWITCH_CASES_WRITE_WB_REG(DBG_WB_BCR, DBG_REG_BASE_BCR, val);
+	default:
+		db_printf("trying to write to wrong debug register %d\n", n);
+	}
+	isb();
+}
+
+void
+kdb_cpu_set_singlestep(void)
+{
+
+	kdb_frame->tf_spsr |= DBG_SPSR_SS;
+	WRITE_SPECIALREG(MDSCR_EL1, READ_SPECIALREG(MDSCR_EL1) |
+	    DBG_MDSCR_SS | DBG_MDSCR_KDE);
+
+	/*
+	 * Disable breakpoints and watchpoints, e.g. stepping
+	 * over watched instruction will trigger break exception instead of
+	 * single-step exception and locks CPU on that instruction for ever.
+	 */
+	if (dbg_ref_count_mde[PCPU_GET(cpuid)] > 0) {
+		WRITE_SPECIALREG(MDSCR_EL1,
+		    READ_SPECIALREG(MDSCR_EL1) & ~DBG_MDSCR_MDE);
+	}
+}
+
+void
+kdb_cpu_clear_singlestep(void)
+{
+
+	WRITE_SPECIALREG(MDSCR_EL1, READ_SPECIALREG(MDSCR_EL1) &
+	    ~(DBG_MDSCR_SS | DBG_MDSCR_KDE));
+
+	/* Restore breakpoints and watchpoints */
+	if (dbg_ref_count_mde[PCPU_GET(cpuid)] > 0) {
+		WRITE_SPECIALREG(MDSCR_EL1,
+		    READ_SPECIALREG(MDSCR_EL1) | DBG_MDSCR_MDE);
+	}
+
+	if (dbg_ref_count_kde[PCPU_GET(cpuid)] > 0) {
+		WRITE_SPECIALREG(MDSCR_EL1,
+		    READ_SPECIALREG(MDSCR_EL1) | DBG_MDSCR_KDE);
+	}
+}
+
+static const char *
+dbg_watchtype_str(uint32_t type)
+{
+	switch (type) {
+		case DBG_WATCH_CTRL_EXEC:
+			return ("execute");
+		case DBG_WATCH_CTRL_STORE:
+			return ("write");
+		case DBG_WATCH_CTRL_LOAD:
+			return ("read");
+		case DBG_WATCH_CTRL_LOAD | DBG_WATCH_CTRL_STORE:
+			return ("read/write");
+		default:
+			return ("invalid");
+	}
+}
+
+static int
+dbg_watchtype_len(uint32_t len)
+{
+	switch (len) {
+	case DBG_WATCH_CTRL_LEN_1:
+		return (1);
+	case DBG_WATCH_CTRL_LEN_2:
+		return (2);
+	case DBG_WATCH_CTRL_LEN_4:
+		return (4);
+	case DBG_WATCH_CTRL_LEN_8:
+		return (8);
+	default:
+		return (0);
+	}
+}
+
+void
+dbg_show_watchpoint(void)
+{
+	uint32_t wcr, len, type;
+	uint64_t addr;
+	int i;
+
+	db_printf("\nhardware watchpoints:\n");
+	db_printf("  watch    status        type  len             address              symbol\n");
+	db_printf("  -----  --------  ----------  ---  ------------------  ------------------\n");
+	for (i = 0; i < dbg_watchpoint_num; i++) {
+		wcr = dbg_wb_read_reg(DBG_REG_BASE_WCR, i);
+		if ((wcr & DBG_WB_CTRL_E) != 0) {
+			type = DBG_WATCH_CTRL_ACCESS_MASK(wcr);
+			len = DBG_WATCH_CTRL_LEN_MASK(wcr);
+			addr = dbg_wb_read_reg(DBG_REG_BASE_WVR, i);
+			db_printf("  %-5d  %-8s  %10s  %3d  0x%16lx  ",
+			    i, "enabled", dbg_watchtype_str(type),
+			    dbg_watchtype_len(len), addr);
+			db_printsym((db_addr_t)addr, DB_STGY_ANY);
+			db_printf("\n");
+		} else {
+			db_printf("  %-5d  disabled\n", i);
+		}
+	}
+}
+
+
+static int
+dbg_find_free_slot(enum dbg_t type)
+{
+	u_int max, reg, i;
+
+	switch(type) {
+	case DBG_TYPE_BREAKPOINT:
+		max = dbg_breakpoint_num;
+		reg = DBG_REG_BASE_BCR;
+
+		break;
+	case DBG_TYPE_WATCHPOINT:
+		max = dbg_watchpoint_num;
+		reg = DBG_REG_BASE_WCR;
+		break;
+	default:
+		db_printf("Unsupported debug type\n");
+		return (i);
+	}
+
+	for (i = 0; i < max; i++) {
+		if ((dbg_wb_read_reg(reg, i) & DBG_WB_CTRL_E) == 0)
+			return (i);
+	}
+
+	return (-1);
+}
+
+static int
+dbg_find_slot(enum dbg_t type, db_expr_t addr)
+{
+	u_int max, reg_addr, reg_ctrl, i;
+
+	switch(type) {
+	case DBG_TYPE_BREAKPOINT:
+		max = dbg_breakpoint_num;
+		reg_addr = DBG_REG_BASE_BVR;
+		reg_ctrl = DBG_REG_BASE_BCR;
+		break;
+	case DBG_TYPE_WATCHPOINT:
+		max = dbg_watchpoint_num;
+		reg_addr = DBG_REG_BASE_WVR;
+		reg_ctrl = DBG_REG_BASE_WCR;
+		break;
+	default:
+		db_printf("Unsupported debug type\n");
+		return (i);
+	}
+
+	for (i = 0; i < max; i++) {
+		if ((dbg_wb_read_reg(reg_addr, i) == addr) &&
+		    ((dbg_wb_read_reg(reg_ctrl, i) & DBG_WB_CTRL_E) != 0))
+			return (i);
+	}
+
+	return (-1);
+}
+
+static void
+dbg_enable_monitor(enum dbg_el_t el)
+{
+	uint64_t reg_mdcr = 0;
+
+	/*
+	 * There is no need to have debug monitor on permanently, thus we are
+	 * refcounting and turn it on only if any of CPU is going to use that.
+	 */
+	if (atomic_fetchadd_int(&dbg_ref_count_mde[PCPU_GET(cpuid)], 1) == 0)
+		reg_mdcr = DBG_MDSCR_MDE;
+
+	if ((el == DBG_FROM_EL1) &&
+	    atomic_fetchadd_int(&dbg_ref_count_kde[PCPU_GET(cpuid)], 1) == 0)
+		reg_mdcr |= DBG_MDSCR_KDE;
+
+	if (reg_mdcr)
+		WRITE_SPECIALREG(MDSCR_EL1, READ_SPECIALREG(MDSCR_EL1) | reg_mdcr);
+}
+
+static void
+dbg_disable_monitor(enum dbg_el_t el)
+{
+	uint64_t reg_mdcr = 0;
+
+	if (atomic_fetchadd_int(&dbg_ref_count_mde[PCPU_GET(cpuid)], -1) == 1)
+		reg_mdcr = DBG_MDSCR_MDE;
+
+	if ((el == DBG_FROM_EL1) &&
+	    atomic_fetchadd_int(&dbg_ref_count_kde[PCPU_GET(cpuid)], -1) == 1)
+		reg_mdcr |= DBG_MDSCR_KDE;
+
+	if (reg_mdcr)
+		WRITE_SPECIALREG(MDSCR_EL1, READ_SPECIALREG(MDSCR_EL1) & ~reg_mdcr);
+}
+
+int
+dbg_setup_watchpoint(db_expr_t addr, db_expr_t size, enum dbg_el_t el,
+    enum dbg_access_t access)
+{
+	uint64_t wcr_size, wcr_priv, wcr_access;
+	u_int i;
+
+	i = dbg_find_free_slot(DBG_TYPE_WATCHPOINT);
+	if (i == -1) {
+		db_printf("Can not find slot for watchpoint, max %d"
+		    " watchpoints supported\n", dbg_watchpoint_num);
+		return (i);
+	}
+
+	switch(size) {
+	case 1:
+		wcr_size = DBG_WATCH_CTRL_LEN_1;
+		break;
+	case 2:
+		wcr_size = DBG_WATCH_CTRL_LEN_2;
+		break;
+	case 4:
+		wcr_size = DBG_WATCH_CTRL_LEN_4;
+		break;
+	case 8:
+		wcr_size = DBG_WATCH_CTRL_LEN_8;
+		break;
+	default:
+		db_printf("Unsupported address size for watchpoint\n");
+		return (-1);
+	}
+
+	switch(el) {
+	case DBG_FROM_EL0:
+		wcr_priv = DBG_WB_CTRL_EL0;
+		break;
+	case DBG_FROM_EL1:
+		wcr_priv = DBG_WB_CTRL_EL1;
+		break;
+	default:
+		db_printf("Unsupported exception level for watchpoint\n");
+		return (-1);
+	}
+
+	switch(access) {
+	case HW_BREAKPOINT_X:
+		wcr_access = DBG_WATCH_CTRL_EXEC;
+		break;
+	case HW_BREAKPOINT_R:
+		wcr_access = DBG_WATCH_CTRL_LOAD;
+		break;
+	case HW_BREAKPOINT_W:
+		wcr_access = DBG_WATCH_CTRL_STORE;
+		break;
+	case HW_BREAKPOINT_RW:
+		wcr_access = DBG_WATCH_CTRL_LOAD | DBG_WATCH_CTRL_STORE;
+		break;
+	default:
+		db_printf("Unsupported exception level for watchpoint\n");
+		return (-1);
+	}
+
+	dbg_wb_write_reg(DBG_REG_BASE_WVR, i, addr);
+	dbg_wb_write_reg(DBG_REG_BASE_WCR, i, wcr_size | wcr_access | wcr_priv |
+	    DBG_WB_CTRL_E);
+	dbg_enable_monitor(el);
+	return (0);
+}
+
+int
+dbg_remove_watchpoint(db_expr_t addr, db_expr_t size, enum dbg_el_t el)
+{
+	u_int i;
+
+	i = dbg_find_slot(DBG_TYPE_WATCHPOINT, addr);
+	if (i == -1) {
+		db_printf("Can not find watchpoint for address 0%lx\n", addr);
+		return (i);
+	}
+
+	dbg_wb_write_reg(DBG_REG_BASE_WCR, i, 0);
+	dbg_disable_monitor(el);
+	return (0);
+}
+
+void
+dbg_monitor_init(void)
+{
+	u_int i;
+
+	/* Clear OS lock */
+	WRITE_SPECIALREG(OSLAR_EL1, 0);
+
+	/* Find out many breakpoints and watchpoints we can use */
+	dbg_watchpoint_num = ((READ_SPECIALREG(ID_AA64DFR0_EL1) >> 20) & 0xf) + 1;
+	dbg_breakpoint_num = ((READ_SPECIALREG(ID_AA64DFR0_EL1) >> 12) & 0xf) + 1;
+
+	if (bootverbose && PCPU_GET(cpuid) == 0) {
+		db_printf("%d watchpoints and %d breakpoints supported\n",
+		    dbg_watchpoint_num, dbg_breakpoint_num);
+	}
+
+	/*
+	 * We have limited number of {watch,break}points, each consists of
+	 * two registers:
+	 * - wcr/bcr regsiter configurates corresponding {watch,break}point
+	 *   behaviour
+	 * - wvr/bvr register keeps address we are hunting for
+	 *
+	 * Reset all breakpoints and watchpoints.
+	 */
+	for (i = 0; i < dbg_watchpoint_num; ++i) {
+		dbg_wb_write_reg(DBG_REG_BASE_WCR, i, 0);
+		dbg_wb_write_reg(DBG_REG_BASE_WVR, i, 0);
+	}
+
+	for (i = 0; i < dbg_breakpoint_num; ++i) {
+		dbg_wb_write_reg(DBG_REG_BASE_BCR, i, 0);
+		dbg_wb_write_reg(DBG_REG_BASE_BVR, i, 0);
+	}
+
+	dbg_enable();
+}