Add the MD parts of dtrace needed to use fbt on ARM. For this we need to

emulate the instructions used in function entry and exit.

For function entry ARM will use a push instruction to push up to 16
registers to the stack. While we don't expect all 16 to be used we need to
handle any combination the compiler may generate, even if it doesn't make
sense (e.g. pushing the program counter).

On function return we will either have a pop or branch instruction. The
former is similar to the push instruction, but with care to make sure we
update the stack pointer and program counter correctly in the cases they
are either in the list of registers or not. For branch we need to take the
24-bit offset, sign-extend it, and add that number of 4-byte words to the
program counter. Care needs to be taken as, due to historical reasons, the
address the branch is relative to is not the current instruction, but 8
bytes later.

This allows us to use the following probes on ARM boards:
  dtrace -n 'fbt::malloc:entry { stack() }'
and
  dtrace -n 'fbt::free:return { stack() }'

Differential Revision:	https://reviews.freebsd.org/D2007
Reviewed by:	gnn, rpaulo
Sponsored by:	ABT Systems Ltd

6 files changed, 114 insertions, 17 deletions

diff --git a/sys/arm/arm/db_trace.c b/sys/arm/arm/db_trace.c
index bd2422d..96684f6 100644
--- a/sys/arm/arm/db_trace.c
+++ b/sys/arm/arm/db_trace.c
@@ -66,7 +66,7 @@ db_stack_trace_cmd(struct unwind_state *state)
 
 	finished = false;
 	while (!finished) {
-		finished = unwind_stack_one(state, 0);
+		finished = unwind_stack_one(state, 1);
 
 		/* Print the frame details */
 		sym = db_search_symbol(state->start_pc, DB_STGY_ANY, &offset);
diff --git a/sys/arm/arm/exception.S b/sys/arm/arm/exception.S
index 58ae612..6b856fc 100644
--- a/sys/arm/arm/exception.S
+++ b/sys/arm/arm/exception.S
@@ -57,11 +57,6 @@ __FBSDID("$FreeBSD$");
 #ifdef KDTRACE_HOOKS
 	.bss
 	.align 4
-	.global	_C_LABEL(dtrace_invop_jump_addr)
-_C_LABEL(dtrace_invop_jump_addr):
-	.word 0
-	.word 0
-
 	.global	_C_LABEL(dtrace_invop_calltrap_addr)
 _C_LABEL(dtrace_invop_calltrap_addr):
 	.word 0
@@ -162,7 +157,8 @@ _C_LABEL(dtrace_invop_calltrap_addr):
 	msr	cpsr_c, r2;		/* Punch into SVC mode */	   \
 	mov	r2, sp;			/* Save	SVC sp */		   \
 	bic	sp, sp, #7;		/* Align sp to an 8-byte addrress */  \
-	sub	sp, sp, #4;		/* Pad trapframe to keep alignment */ \
+	sub	sp, sp, #(4 * 17);	/* Pad trapframe to keep alignment */ \
+				    /* and for dtrace to emulate push/pop */  \
 	str	r0, [sp, #-4]!;		/* Push return address */	   \
 	str	lr, [sp, #-4]!;		/* Push SVC lr */		   \
 	str	r2, [sp, #-4]!;		/* Push SVC sp */		   \
@@ -199,7 +195,8 @@ _C_LABEL(dtrace_invop_calltrap_addr):
 	msr	cpsr_c, r2;		/* Punch into SVC mode */	   \
 	mov	r2, sp;			/* Save	SVC sp */		   \
 	bic	sp, sp, #7;		/* Align sp to an 8-byte addrress */  \
-	sub	sp, sp, #4;		/* Pad trapframe to keep alignment */ \
+	sub	sp, sp, #(4 * 17);	/* Pad trapframe to keep alignment */ \
+				    /* and for dtrace to emulate push/pop */  \
 	str	r0, [sp, #-4]!;		/* Push return address */	   \
 	str	lr, [sp, #-4]!;		/* Push SVC lr */		   \
 	str	r2, [sp, #-4]!;		/* Push SVC sp */		   \
diff --git a/sys/arm/arm/undefined.c b/sys/arm/arm/undefined.c
index d82fdd3..0980018 100644
--- a/sys/arm/arm/undefined.c
+++ b/sys/arm/arm/undefined.c
@@ -86,6 +86,10 @@ __FBSDID("$FreeBSD$");
 #include <machine/db_machdep.h>
 #endif
 
+#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];
@@ -286,7 +290,14 @@ undefinedinstruction(struct trapframe *frame)
 			printf("No debugger in kernel.\n");
 #endif
 			return;
-		} else
+		}
+#ifdef KDTRACE_HOOKS
+		else if (dtrace_invop_jump_addr != 0) {
+			dtrace_invop_jump_addr(frame);
+			return;
+		}
+#endif
+		else
 			panic("Undefined instruction in kernel.\n");
 	}
 
diff --git a/sys/cddl/contrib/opensolaris/uts/common/sys/dtrace.h b/sys/cddl/contrib/opensolaris/uts/common/sys/dtrace.h
index 3cb7eb2..4605ee5 100644
--- a/sys/cddl/contrib/opensolaris/uts/common/sys/dtrace.h
+++ b/sys/cddl/contrib/opensolaris/uts/common/sys/dtrace.h
@@ -2436,6 +2436,10 @@ extern void dtrace_helpers_destroy(proc_t *);
 
 #elif defined(__arm__)
 
+#define	DTRACE_INVOP_SHIFT	4
+#define	DTRACE_INVOP_MASK	((1 << DTRACE_INVOP_SHIFT) - 1)
+#define	DTRACE_INVOP_DATA(x)	((x) >> DTRACE_INVOP_SHIFT)
+
 #define DTRACE_INVOP_PUSHM	1
 #define DTRACE_INVOP_POPM	2
 #define DTRACE_INVOP_B		3
diff --git a/sys/cddl/dev/dtrace/arm/dtrace_subr.c b/sys/cddl/dev/dtrace/arm/dtrace_subr.c
index d4c12a6..01c0c39 100644
--- a/sys/cddl/dev/dtrace/arm/dtrace_subr.c
+++ b/sys/cddl/dev/dtrace/arm/dtrace_subr.c
@@ -46,7 +46,11 @@ __FBSDID("$FreeBSD$");
 #include <vm/pmap.h>
 
 #define	DELAYBRANCH(x)	((int)(x) < 0)
-		
+
+#define	BIT_PC		15
+#define	BIT_LR		14
+#define	BIT_SP		13
+
 extern uintptr_t 	dtrace_in_probe_addr;
 extern int		dtrace_in_probe;
 extern dtrace_id_t	dtrace_probeid_error;
@@ -231,16 +235,97 @@ dtrace_probe_error(dtrace_state_t *state, dtrace_epid_t epid, int which,
 static int
 dtrace_invop_start(struct trapframe *frame)
 {
-	printf("IMPLEMENT ME: %s\n", __func__);
-	switch (dtrace_invop(frame->tf_pc, (uintptr_t *)frame, frame->tf_pc)) {
+	register_t *r0, *sp;
+	int data, invop, reg, update_sp;
+
+	invop = dtrace_invop(frame->tf_pc, (uintptr_t *)frame, frame->tf_pc);
+	switch (invop & DTRACE_INVOP_MASK) {
 	case DTRACE_INVOP_PUSHM:
-		// TODO:
+		sp = (register_t *)frame->tf_svc_sp;
+		r0 = &frame->tf_r0;
+		data = DTRACE_INVOP_DATA(invop);
+
+		/*
+		 * Store the pc, lr, and sp. These have their own
+		 * entries in the struct.
+		 */
+		if (data & (1 << BIT_PC)) {
+			sp--;
+			*sp = frame->tf_pc;
+		}
+		if (data & (1 << BIT_LR)) {
+			sp--;
+			*sp = frame->tf_svc_lr;
+		}
+		if (data & (1 << BIT_SP)) {
+			sp--;
+			*sp = frame->tf_svc_sp;
+		}
+
+		/* Store the general registers */
+		for (reg = 12; reg >= 0; reg--) {
+			if (data & (1 << reg)) {
+				sp--;
+				*sp = r0[reg];
+			}
+		}
+
+		/* Update the stack pointer and program counter to continue */
+		frame->tf_svc_sp = (register_t)sp;
+		frame->tf_pc += 4;
 		break;
 	case DTRACE_INVOP_POPM:
-		// TODO:
+		sp = (register_t *)frame->tf_svc_sp;
+		r0 = &frame->tf_r0;
+		data = DTRACE_INVOP_DATA(invop);
+
+		/* Read the general registers */
+		for (reg = 0; reg <= 12; reg++) {
+			if (data & (1 << reg)) {
+				r0[reg] = *sp;
+				sp++;
+			}
+		}
+
+		/*
+		 * Set the stack pointer. If we don't update it here we will
+		 * need to update it at the end as the instruction would do
+		 */
+		update_sp = 1;
+		if (data & (1 << BIT_SP)) {
+			frame->tf_svc_sp = *sp;
+			*sp++;
+			update_sp = 0;
+		}
+
+		/* Update the link register, we need to use the correct copy */
+		if (data & (1 << BIT_LR)) {
+			frame->tf_svc_lr = *sp;
+			*sp++;
+		}
+		/*
+		 * And the program counter. If it's not in the list skip over
+		 * it when we return so to not hit this again.
+		 */
+		if (data & (1 << BIT_PC)) {
+			frame->tf_pc = *sp;
+			*sp++;
+		} else
+			frame->tf_pc += 4;
+
+		/* Update the stack pointer if we haven't already done so */
+		if (update_sp)
+			frame->tf_svc_sp = (register_t)sp;
 		break;
 	case DTRACE_INVOP_B:
-		// TODO
+		data = DTRACE_INVOP_DATA(invop) & 0x00ffffff;
+		/* Sign extend the data */
+		if ((data & (1 << 23)) != 0)
+			data |= 0xff000000;
+		/* The data is the number of 4-byte words to change the pc */
+		data *= 4;
+		data += 8;
+		frame->tf_pc += data;
 		break;
 	default:
 		return (-1);
diff --git a/sys/cddl/dev/fbt/arm/fbt_isa.c b/sys/cddl/dev/fbt/arm/fbt_isa.c
index c6bc0f0..96a8249 100644
--- a/sys/cddl/dev/fbt/arm/fbt_isa.c
+++ b/sys/cddl/dev/fbt/arm/fbt_isa.c
@@ -38,7 +38,7 @@
 
 #include "fbt.h"
 
-#define	FBT_PATCHVAL		0xe06a0cfe /* illegal instruction */
+#define	FBT_PATCHVAL		0xe7f000f0 /* Specified undefined instruction */
 
 #define	FBT_PUSHM		0xe92d0000
 #define	FBT_POPM		0xe8bd0000
@@ -66,7 +66,7 @@ fbt_invop(uintptr_t addr, uintptr_t *stack, uintptr_t rval)
 
 			cpu->cpu_dtrace_caller = 0;
 
-			return (fbt->fbtp_rval);
+			return (fbt->fbtp_rval | (fbt->fbtp_savedval << DTRACE_INVOP_SHIFT));
 		}
 	}