Index: sys/arch/arm/arm/undefined.c
===================================================================
RCS file: /cvsroot/src/sys/arch/arm/arm/undefined.c,v
retrieving revision 1.52
diff -u -r1.52 undefined.c
--- sys/arch/arm/arm/undefined.c	5 Mar 2014 02:18:30 -0000	1.52
+++ sys/arch/arm/arm/undefined.c	7 Mar 2014 16:17:31 -0000
@@ -48,6 +48,7 @@
 
 #include "opt_ddb.h"
 #include "opt_kgdb.h"
+#include "opt_dtrace.h"
 
 #include <sys/param.h>
 #ifdef KGDB
@@ -220,6 +221,41 @@
 static struct undefined_handler gdb_uh_thumb;
 #endif
 
+#ifdef KDTRACE_HOOKS
+#include <sys/dtrace_bsd.h>
+
+int (* dtrace_invop_jump_addr)(uintptr_t, uintptr_t *, uintptr_t);
+void (* dtrace_emulation_jump_addr)(int, struct trapframe *);
+
+static int
+dtrace_trapper(u_int addr, struct trapframe *frame)
+{
+	int op;
+	struct trapframe back;
+	u_int insn = read_insn(addr, false);
+
+	if (dtrace_invop_jump_addr == NULL || dtrace_emulation_jump_addr == NULL)
+		return 1;
+
+	if (!DTRACE_IS_BREAKPOINT(insn))
+		return 1;
+
+	/* cond value is encoded in the first byte */
+	if (!arm_cond_ok_p(__SHIFTIN(insn, INSN_COND_MASK), frame->tf_spsr)) {
+		frame->tf_pc += INSN_SIZE;
+		return 0;
+	}
+
+	back = *frame;
+	op = dtrace_invop_jump_addr(addr, (uintptr_t *) frame->tf_svc_sp, frame->tf_r0);
+	*frame = back;
+
+	dtrace_emulation_jump_addr(op, frame);
+
+	return 0;
+}
+#endif
+
 void
 undefined_init(void)
 {
@@ -262,6 +298,15 @@
 
 	und_ev.ev_count++;
 
+#ifdef KDTRACE_HOOKS
+	if ((tf->tf_spsr & PSR_MODE) != PSR_USR32_MODE) {
+		tf->tf_pc -= INSN_SIZE;
+		if (dtrace_trapper(tf->tf_pc, tf) == 0)
+			return;
+		tf->tf_pc += INSN_SIZE; /* Reset for the rest code */
+	}
+#endif
+
 	/* Enable interrupts if they were enabled before the exception. */
 #ifdef acorn26
 	if ((tf->tf_r15 & R15_IRQ_DISABLE) == 0)
Index: sys/arch/arm/include/trap.h
===================================================================
RCS file: /cvsroot/src/sys/arch/arm/include/trap.h,v
retrieving revision 1.8
diff -u -r1.8 trap.h
--- sys/arch/arm/include/trap.h	19 Jan 2008 13:11:09 -0000	1.8
+++ sys/arch/arm/include/trap.h	7 Mar 2014 16:17:31 -0000
@@ -62,6 +62,14 @@
 #define GDB_THUMB_BREAKPOINT	0xdefe		/* Thumb in GDB */
 #define KERNEL_BREAKPOINT	0xe7ffffff	/* Used by DDB */
 
+/*
+ * DTrace uses 0xe7fffef0 to 0xe7fffeff as breakpoints.
+ * The first byte is used to encode a cond value.
+ */
+#define DTRACE_BREAKPOINT	0xe7fffef0
+#define DTRACE_BREAKPOINT_MASK	0xfffffff0
+#define DTRACE_IS_BREAKPOINT(insn)	((insn & DTRACE_BREAKPOINT_MASK) == DTRACE_BREAKPOINT)
+
 #define KBPT_ASM		".word 0xe7ffdefe"
 
 #define USER_BREAKPOINT		GDB_BREAKPOINT