Index: sys/arm/arm/exception.S
===================================================================
--- sys/arm/arm/exception.S
+++ sys/arm/arm/exception.S
@@ -57,8 +57,8 @@
 #ifdef KDTRACE_HOOKS
 	.bss
 	.align 4
-	.global	_C_LABEL(dtrace_invop_calltrap_addr)
-_C_LABEL(dtrace_invop_calltrap_addr):
+	.global _C_LABEL(dtrace_invop_jump_addr)
+_C_LABEL(dtrace_invop_jump_addr):
 	.word 0
 	.word 0
 #endif
@@ -363,7 +363,41 @@
 	PUSHFRAMEINSVC			/* mode stack, build trapframe there. */
 	adr	lr, exception_exit      /* Return from handler via standard */
 	mov	r0, sp			/* exception exit routine.  Pass the */
-	b	undefinedinstruction	/* trapframe to the handler. */
+
+#ifdef KDTRACE_HOOKS
+	ldr     r1, =_C_LABEL(dtrace_invop_jump_addr) /* check if dtrace enabled */
+	ldr	r3, [r1]
+	cmp	r3, #0
+	beq	undefinedinstruction
+
+	ldrb	r4, [sp]
+	tst	r4, #0x10		/* test if FAULT_USER */
+	bne	undefinedinstruction
+
+	ldr	r2, [sp, #0x72]		/* load pc */
+#if  __ARM_ARCH >= 7
+	tst	r4, #0x20		/* test if PSR_T */
+	sub	r3, r2, #2
+	b	.next
+#else
+	sub	r3, r2, #4		/* fix pc */
+#endif
+.next:
+	str	r3, [sp, #0x72]		/* store pc */
+	ldr	r4, [r2]		/* load instrution */
+	ldr	r1, =0xe7ffffff		/* load ikernel inv op */
+	cmp	r1, r4
+	bne	undefinedinstruction
+
+	blx	r3
+
+	cmp	r0, #0			/* check ret code */
+	beq	exception_exit		/* exit if sucess */
+
+	adr     lr, exception_exit	/* prepare call standard */
+	mov	r0, sp			/* prepare parameter */
+#endif
+	b	undefinedinstruction	/* call stadnard handler */
 END(undefined_entry)
 
 /*
Index: sys/arm/arm/undefined.c
===================================================================
--- sys/arm/arm/undefined.c
+++ sys/arm/arm/undefined.c
@@ -99,10 +99,6 @@
 
 #define	COPROC_VFP	10
 
-#ifdef KDTRACE_HOOKS
-int (*dtrace_invop_jump_addr)(struct trapframe *);
-#endif
-
 static int gdb_trapper(u_int, u_int, struct trapframe *, int);
 
 LIST_HEAD(, undefined_handler) undefined_handlers[MAX_COPROCS];
@@ -350,12 +346,6 @@
 #endif
 			return;
 		}
-#ifdef KDTRACE_HOOKS
-		else if (dtrace_invop_jump_addr != 0) {
-			dtrace_invop_jump_addr(frame);
-			return;
-		}
-#endif
 		else
 			panic("Undefined instruction in kernel.\n");
 	}