Implement FBT provider (MD part) for DTrace on MIPS.

Tested on MIPS64.

Sponsored by:	DARPA, AFRL
Sponsored by:	HEIF5

sys/cddl/contrib/opensolaris/uts/common/sys/dtrace.h

@@ -2483,6 +2483,18 @@ extern void dtrace_helpers_destroy(proc_t *);
 #define	DTRACE_INVOP_RET	2
 #define	DTRACE_INVOP_B		3
 
+#elif defined(__mips__)
+
+#define	INSN_SIZE		4
+
+/* Load/Store double RA to/from SP */
+#define	LDSD_RA_SP_MASK		0xffff0000
+#define	LDSD_DATA_MASK		0x0000ffff
+#define	SD_RA_SP		0xffbf0000
+#define	LD_RA_SP		0xdfbf0000
+
+#define	DTRACE_INVOP_SD		1
+#define	DTRACE_INVOP_LD		2
 #endif
 
 #ifdef	__cplusplus

sys/cddl/dev/dtrace/mips/dtrace_asm.S

@@ -28,7 +28,6 @@
 
 #define _ASM
 #define _LOCORE
-#define LOCORE
 
 #include <sys/cpuvar_defs.h>
 #include <sys/dtrace.h>
@@ -226,28 +225,6 @@ LEAF(dtrace_copystr)
 	nop
 END(dtrace_copystr)
 
-/*
-void dtrace_invop_init(void)
-*/
-LEAF(dtrace_invop_init)
-	/* XXX: impement it properly */
-	PTR_LA	t0, dtrace_invop_jump_addr
-	/* dla	t1, dtrace_invop_start */
-	PTR_S	zero, 0(t0)
-	j	ra
-	nop
-END(dtrace_invop_init)
-
-/*
-void dtrace_invop_uninit(void)
-*/
-LEAF(dtrace_invop_uninit)
-	PTR_LA	t0, dtrace_invop_jump_addr
-	PTR_S	zero, 0(t0)
-	j	ra
-	nop
-END(dtrace_invop_uninit)
-
 /*
 uintptr_t
 dtrace_caller(int aframes)

sys/cddl/dev/dtrace/mips/dtrace_subr.c

@@ -46,12 +46,13 @@ __FBSDID("$FreeBSD$");
 
 #define	DELAYBRANCH(x)	((int)(x) < 0)
 
+extern int (*dtrace_invop_jump_addr)(struct trapframe *);
 extern dtrace_id_t	dtrace_probeid_error;
 
 int dtrace_invop(uintptr_t, struct trapframe *, uintptr_t);
 
 typedef struct dtrace_invop_hdlr {
-	int (*dtih_func)(uintptr_t, uintptr_t *, uintptr_t);
+	int (*dtih_func)(uintptr_t, struct trapframe *, uintptr_t);
 	struct dtrace_invop_hdlr *dtih_next;
 } dtrace_invop_hdlr_t;
 
@@ -70,6 +71,46 @@ dtrace_invop(uintptr_t addr, struct trapframe *stack, uintptr_t eax)
 	return (0);
 }
 
+void
+dtrace_invop_add(int (*func)(uintptr_t, struct trapframe *, uintptr_t))
+{
+	dtrace_invop_hdlr_t *hdlr;
+
+	hdlr = kmem_alloc(sizeof (dtrace_invop_hdlr_t), KM_SLEEP);
+	hdlr->dtih_func = func;
+	hdlr->dtih_next = dtrace_invop_hdlr;
+	dtrace_invop_hdlr = hdlr;
+}
+
+void
+dtrace_invop_remove(int (*func)(uintptr_t, struct trapframe *, uintptr_t))
+{
+	dtrace_invop_hdlr_t *hdlr, *prev;
+
+	hdlr = dtrace_invop_hdlr;
+	prev = NULL;
+
+	for (;;) {
+		if (hdlr == NULL)
+			panic("attempt to remove non-existent invop handler");
+
+		if (hdlr->dtih_func == func)
+			break;
+
+		prev = hdlr;
+		hdlr = hdlr->dtih_next;
+	}
+
+	if (prev == NULL) {
+		ASSERT(dtrace_invop_hdlr == hdlr);
+		dtrace_invop_hdlr = hdlr->dtih_next;
+	} else {
+		ASSERT(dtrace_invop_hdlr != hdlr);
+		prev->dtih_next = hdlr->dtih_next;
+	}
+
+	kmem_free(hdlr, 0);
+}
 
 /*ARGSUSED*/
 void
@@ -199,3 +240,45 @@ dtrace_probe_error(dtrace_state_t *state, dtrace_epid_t epid, int which,
 	    (uintptr_t)epid,
 	    (uintptr_t)which, (uintptr_t)fault, (uintptr_t)fltoffs);
 }
+
+static int
+dtrace_invop_start(struct trapframe *frame)
+{
+	register_t *sp;
+	int16_t offs;
+	int invop;
+
+	invop = dtrace_invop(frame->pc, frame, frame->pc);
+	offs = (invop & LDSD_DATA_MASK);
+	sp = (register_t *)((uint8_t *)frame->sp + offs);
+
+	switch (invop & LDSD_RA_SP_MASK) {
+	case LD_RA_SP:
+		frame->ra = *sp;
+		frame->pc += INSN_SIZE;
+		break;
+	case SD_RA_SP:
+		*(sp) = frame->ra;
+		frame->pc += INSN_SIZE;
+		break;
+	default:
+		printf("%s: 0x%x undefined\n", __func__, invop);
+		return (-1);
+	};
+
+	return (0);
+}
+
+void
+dtrace_invop_init(void)
+{
+
+	dtrace_invop_jump_addr = dtrace_invop_start;
+}
+
+void
+dtrace_invop_uninit(void)
+{
+
+	dtrace_invop_jump_addr = 0;
+}

sys/cddl/dev/fbt/mips/fbt_isa.c

@@ -0,0 +1,165 @@
+/*
+ * CDDL HEADER START
+ *
+ * The contents of this file are subject to the terms of the
+ * Common Development and Distribution License (the "License").
+ * You may not use this file except in compliance with the License.
+ *
+ * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
+ * or http://www.opensolaris.org/os/licensing.
+ * See the License for the specific language governing permissions
+ * and limitations under the License.
+ *
+ * When distributing Covered Code, include this CDDL HEADER in each
+ * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
+ * If applicable, add the following below this CDDL HEADER, with the
+ * fields enclosed by brackets "[]" replaced with your own identifying
+ * information: Portions Copyright [yyyy] [name of copyright owner]
+ *
+ * CDDL HEADER END
+ *
+ * Portions Copyright 2006-2008 John Birrell jb@freebsd.org
+ * Portions Copyright 2013 Justin Hibbits jhibbits@freebsd.org
+ * Portions Copyright 2013 Howard Su howardsu@freebsd.org
+ * Portions Copyright 2015-2016 Ruslan Bukin <br@bsdpad.com>
+ *
+ * $FreeBSD$
+ */
+
+/*
+ * Copyright 2006 Sun Microsystems, Inc.  All rights reserved.
+ * Use is subject to license terms.
+ */
+
+#include <sys/cdefs.h>
+#include <sys/param.h>
+#include <sys/dtrace.h>
+
+#include <machine/cpuregs.h>
+#include <machine/cache.h>
+
+#include "fbt.h"
+
+#define	FBT_PATCHVAL		(MIPS_BREAK_INSTR)
+#define	FBT_ENTRY		"entry"
+#define	FBT_RETURN		"return"
+
+int
+fbt_invop(uintptr_t addr, struct trapframe *frame, uintptr_t rval)
+{
+	solaris_cpu_t *cpu;
+	fbt_probe_t *fbt;
+
+	cpu = &solaris_cpu[curcpu];
+	fbt = fbt_probetab[FBT_ADDR2NDX(addr)];
+
+	for (; fbt != NULL; fbt = fbt->fbtp_hashnext) {
+		if ((uintptr_t)fbt->fbtp_patchpoint == addr) {
+			cpu->cpu_dtrace_caller = addr;
+
+			dtrace_probe(fbt->fbtp_id, frame->a0,
+			    frame->a1, frame->a2,
+			    frame->a3, frame->a4);
+
+			cpu->cpu_dtrace_caller = 0;
+			return (fbt->fbtp_savedval);
+		}
+	}
+
+	return (0);
+}
+
+void
+fbt_patch_tracepoint(fbt_probe_t *fbt, fbt_patchval_t val)
+{
+
+	*fbt->fbtp_patchpoint = val;
+	mips_icache_sync_range((vm_offset_t)fbt->fbtp_patchpoint, 4);
+}
+
+int
+fbt_provide_module_function(linker_file_t lf, int symindx,
+    linker_symval_t *symval, void *opaque)
+{
+	fbt_probe_t *fbt, *retfbt;
+	uint32_t *target, *start;
+	uint32_t *instr, *limit;
+	const char *name;
+	char *modname;
+
+	modname = opaque;
+	name = symval->name;
+
+	/* Check if function is excluded from instrumentation */
+	if (fbt_excluded(name))
+		return (0);
+
+	instr = (uint32_t *)(symval->value);
+	limit = (uint32_t *)(symval->value + symval->size);
+
+	/* Look for store double to ra register */
+	for (; instr < limit; instr++) {
+		if ((*instr & LDSD_RA_SP_MASK) == SD_RA_SP)
+			break;
+	}
+
+	if (instr >= limit)
+		return (0);
+
+	fbt = malloc(sizeof (fbt_probe_t), M_FBT, M_WAITOK | M_ZERO);
+	fbt->fbtp_name = name;
+	fbt->fbtp_id = dtrace_probe_create(fbt_id, modname,
+	    name, FBT_ENTRY, 3, fbt);
+	fbt->fbtp_patchpoint = instr;
+	fbt->fbtp_ctl = lf;
+	fbt->fbtp_loadcnt = lf->loadcnt;
+	fbt->fbtp_savedval = *instr;
+	fbt->fbtp_patchval = FBT_PATCHVAL;
+	fbt->fbtp_rval = DTRACE_INVOP_SD;
+	fbt->fbtp_symindx = symindx;
+
+	fbt->fbtp_hashnext = fbt_probetab[FBT_ADDR2NDX(instr)];
+	fbt_probetab[FBT_ADDR2NDX(instr)] = fbt;
+
+	lf->fbt_nentries++;
+
+	retfbt = NULL;
+again:
+	for (; instr < limit; instr++) {
+		if ((*instr & LDSD_RA_SP_MASK) == LD_RA_SP) {
+			break;
+		}
+	}
+
+	if (instr >= limit)
+		return (0);
+
+	/*
+	 * We have a winner!
+	 */
+	fbt = malloc(sizeof (fbt_probe_t), M_FBT, M_WAITOK | M_ZERO);
+	fbt->fbtp_name = name;
+	if (retfbt == NULL) {
+		fbt->fbtp_id = dtrace_probe_create(fbt_id, modname,
+		    name, FBT_RETURN, 3, fbt);
+	} else {
+		retfbt->fbtp_next = fbt;
+		fbt->fbtp_id = retfbt->fbtp_id;
+	}
+	retfbt = fbt;
+
+	fbt->fbtp_patchpoint = instr;
+	fbt->fbtp_ctl = lf;
+	fbt->fbtp_loadcnt = lf->loadcnt;
+	fbt->fbtp_symindx = symindx;
+	fbt->fbtp_rval = DTRACE_INVOP_LD;
+	fbt->fbtp_savedval = *instr;
+	fbt->fbtp_patchval = FBT_PATCHVAL;
+	fbt->fbtp_hashnext = fbt_probetab[FBT_ADDR2NDX(instr)];
+	fbt_probetab[FBT_ADDR2NDX(instr)] = fbt;
+
+	lf->fbt_nentries++;
+
+	instr++;
+	goto again;
+}

sys/cddl/dev/fbt/mips/fbt_isa.h

@@ -0,0 +1,30 @@
+/*
+ * CDDL HEADER START
+ *
+ * The contents of this file are subject to the terms of the
+ * Common Development and Distribution License (the "License").
+ * You may not use this file except in compliance with the License.
+ *
+ * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
+ * or http://www.opensolaris.org/os/licensing.
+ * See the License for the specific language governing permissions
+ * and limitations under the License.
+ *
+ * When distributing Covered Code, include this CDDL HEADER in each
+ * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
+ * If applicable, add the following below this CDDL HEADER, with the
+ * fields enclosed by brackets "[]" replaced with your own identifying
+ * information: Portions Copyright [yyyy] [name of copyright owner]
+ *
+ * CDDL HEADER END
+ *
+ * $FreeBSD$
+ *
+ */
+
+#ifndef _FBT_ISA_H_
+#define _FBT_ISA_H_
+
+typedef uint32_t fbt_patchval_t;
+
+#endif

sys/conf/files.mips

@@ -95,3 +95,9 @@ dev/hwpmc/hwpmc_mips74k.c		optional	hwpmc_mips74k
 
 # ofw support
 dev/ofw/ofwpci.c			optional	fdt pci
+
+# DTrace
+cddl/compat/opensolaris/kern/opensolaris_atomic.c	optional zfs | dtrace compile-with "${CDDL_C}"
+cddl/dev/dtrace/mips/dtrace_asm.S			optional dtrace compile-with "${DTRACE_S}"
+cddl/dev/dtrace/mips/dtrace_subr.c			optional dtrace compile-with "${DTRACE_C}"
+cddl/dev/fbt/mips/fbt_isa.c				optional dtrace_fbt | dtraceall compile-with "${FBT_C}"

sys/mips/mips/exception.S

@@ -69,13 +69,6 @@
 
 #ifdef KDTRACE_HOOKS
 	.data
-	.globl	dtrace_invop_jump_addr
-	.align	4
-	.type	dtrace_invop_jump_addr, @object
-        .size	dtrace_invop_jump_addr, 8
-dtrace_invop_jump_addr:
-	.word	0
-	.word	0
 	.globl	dtrace_invop_calltrap_addr
 	.align	4
 	.type	dtrace_invop_calltrap_addr, @object

sys/mips/mips/trap.c

@@ -160,6 +160,8 @@ static void log_bad_page_fault(char *, struct trapframe *, int);
 static void log_frame_dump(struct trapframe *frame);
 static void get_mapping_info(vm_offset_t, pd_entry_t **, pt_entry_t **);
 
+int (*dtrace_invop_jump_addr)(struct trapframe *);
+
 #ifdef TRAP_DEBUG
 static void trap_frame_dump(struct trapframe *frame);
 #endif
@@ -808,10 +810,18 @@ trap(struct trapframe *trapframe)
 			return (trapframe->pc);
 		}
 
-#ifdef DDB
+#if defined(KDTRACE_HOOKS) || defined(DDB)
 	case T_BREAK:
+#ifdef KDTRACE_HOOKS
+		if (!usermode && dtrace_invop_jump_addr != 0) {
+			dtrace_invop_jump_addr(trapframe);
+			return (trapframe->pc);
+		}
+#endif
+#ifdef DDB
 		kdb_trap(type, 0, trapframe);
 		return (trapframe->pc);
+#endif
 #endif
 
 	case T_BREAK + T_USER: