[apple/xnu.git] / bsd / dev / arm64 / fbt_arm.c

/*
 * Copyright (c) 2007 Apple Inc. All rights reserved.
 */
/*
 * CDDL HEADER START
 *
 * The contents of this file are subject to the terms of the
 * Common Development and Distribution License, Version 1.0 only
 * (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
 */
/*
 * Copyright 2005 Sun Microsystems, Inc.  All rights reserved.
 * Use is subject to license terms.
 */

/* #pragma ident	"@(#)fbt.c	1.15	05/09/19 SMI" */

#ifdef KERNEL
#ifndef _KERNEL
#define _KERNEL			/* Solaris vs. Darwin */
#endif
#endif

#define MACH__POSIX_C_SOURCE_PRIVATE 1	/* pulls in suitable savearea from
					 * mach/ppc/thread_status.h */
#include <kern/thread.h>
#include <mach/thread_status.h>
#include <arm/proc_reg.h>
#include <arm/caches_internal.h>

#include <mach-o/loader.h>
#include <mach-o/nlist.h>
#include <libkern/kernel_mach_header.h>

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/errno.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <sys/conf.h>
#include <sys/fcntl.h>
#include <miscfs/devfs/devfs.h>

#include <sys/dtrace.h>
#include <sys/dtrace_impl.h>
#include <sys/fbt.h>

#include <sys/dtrace_glue.h>

#if __has_include(<ptrauth.h>)
#include <ptrauth.h>
#endif

#define DTRACE_INVOP_PUSH_FRAME 11

#define DTRACE_INVOP_NOP_SKIP		4
#define DTRACE_INVOP_ADD_FP_SP_SKIP	4

#define DTRACE_INVOP_POP_PC_SKIP 2

/*
 * stp	fp, lr, [sp, #val]
 * stp	fp, lr, [sp, #val]!
 */
#define FBT_IS_ARM64_FRAME_PUSH(x)	\
	(((x) & 0xffc07fff) == 0xa9007bfd || ((x) & 0xffc07fff) == 0xa9807bfd)

/*
 * stp	Xt1, Xt2, [sp, #val]
 * stp	Xt1, Xt2, [sp, #val]!
 */
#define FBT_IS_ARM64_PUSH(x)		\
	(((x) & 0xffc003e0) == 0xa90003e0 || ((x) & 0xffc003e0) == 0xa98003e0)

/*
 * ldp	fp, lr, [sp,  #val]
 * ldp	fp, lr, [sp], #val
 */
#define FBT_IS_ARM64_FRAME_POP(x)	\
	(((x) & 0xffc07fff) == 0xa9407bfd || ((x) & 0xffc07fff) == 0xa8c07bfd)

#define FBT_IS_ARM64_ADD_FP_SP(x)	(((x) & 0xffc003ff) == 0x910003fd)	/* add fp, sp, #val  (add fp, sp, #0 == mov fp, sp) */
#define FBT_IS_ARM64_RET(x)		(((x) == 0xd65f03c0) || ((x) == 0xd65f0fff)) 			/* ret, retab */


#define FBT_B_MASK 			0xff000000
#define FBT_B_IMM_MASK			0x00ffffff
#define FBT_B_INSTR			0x14000000

#define FBT_IS_ARM64_B_INSTR(x)		((x & FBT_B_MASK) == FBT_B_INSTR)
#define FBT_GET_ARM64_B_IMM(x)		((x & FBT_B_IMM_MASK) << 2)

#define	FBT_PATCHVAL			0xe7eeee7e
#define FBT_AFRAMES_ENTRY		7
#define FBT_AFRAMES_RETURN		7

#define	FBT_ENTRY	"entry"
#define	FBT_RETURN	"return"
#define	FBT_ADDR2NDX(addr)	((((uintptr_t)(addr)) >> 4) & fbt_probetab_mask)

extern dtrace_provider_id_t	fbt_id;
extern fbt_probe_t		 **fbt_probetab;
extern int      		fbt_probetab_mask;

kern_return_t fbt_perfCallback(int, struct arm_saved_state *, __unused int, __unused int);

int
fbt_invop(uintptr_t addr, uintptr_t * stack, uintptr_t rval)
{
	fbt_probe_t    *fbt = fbt_probetab[FBT_ADDR2NDX(addr)];

	for (; fbt != NULL; fbt = fbt->fbtp_hashnext) {
		if ((uintptr_t) fbt->fbtp_patchpoint == addr) {
			if (0 == CPU->cpu_dtrace_invop_underway) {
				CPU->cpu_dtrace_invop_underway = 1;	/* Race not possible on
									 * this per-cpu state */

				if (fbt->fbtp_roffset == 0) {
					/*
					 * Stack looks like this:
					 *
					 *	[Higher addresses]
					 *
					 *	Frame of caller
					 *	Extra args for callee
					 *	------------------------
					 *	Frame from traced function: <previous sp (e.g. 0x1000), return address>
					 *	------------------------
					 *	arm_context_t
					 *	------------------------
					 *	Frame from trap handler:  <previous sp (e.g. 0x1000) , traced PC >
					 *				The traced function never got to mov fp, sp,
					 *				so there is no frame in the backtrace pointing
					 *				to the frame on the stack containing the LR in the
					 *				caller.
					 *	------------------------
					 *	     |
					 *	     |
					 *	     |  stack grows this way
					 *	     |
					 *	     |
					 *	     v
					 *	[Lower addresses]
					 */

					arm_saved_state_t *regs = (arm_saved_state_t *)(&((arm_context_t *)stack)->ss);

					/*
					 * cpu_dtrace_caller compensates for fact that the traced function never got to update its fp.
					 * When walking the stack, when we reach the frame where we extract a PC in the patched
					 * function, we put the cpu_dtrace_caller in the backtrace instead.  The next frame we extract
					 * will be in the caller's caller, so we output a backtrace starting at the caller and going
					 * sequentially up the stack.
					 */
					CPU->cpu_dtrace_caller = get_saved_state_lr(regs);
					dtrace_probe(fbt->fbtp_id, get_saved_state_reg(regs, 0), get_saved_state_reg(regs, 1),
					    get_saved_state_reg(regs, 2), get_saved_state_reg(regs, 3),get_saved_state_reg(regs, 4));
					CPU->cpu_dtrace_caller = 0;
				} else {
					/*
					 * When fbtp_roffset is non-zero, we know we are handling a return probe point.
					 *
					 *
					 * Stack looks like this, as we've already popped the frame in the traced callee, and
					 * we trap with lr set to the return address in the caller.
					 *	[Higher addresses]
					 *
					 *	Frame of caller
					 *	Extra args for callee
					 *	------------------------
					 *	arm_context_t
					 *	------------------------
					 *	Frame from trap handler:  <sp at time of trap, traced PC >
					 *	------------------------
					 *	     |
					 *	     |
					 *	     |  stack grows this way
					 *	     |
					 *	     |
					 *	     v
					 *	[Lower addresses]
					 */
					arm_saved_state_t *regs = (arm_saved_state_t *)(&((arm_context_t *)stack)->ss);

					CPU->cpu_dtrace_caller = get_saved_state_lr(regs);
					dtrace_probe(fbt->fbtp_id, fbt->fbtp_roffset, rval, 0, 0, 0);
					CPU->cpu_dtrace_caller = 0;
				}
				CPU->cpu_dtrace_invop_underway = 0;
			}

			/*
				On other architectures, we return a DTRACE constant to let the callback function
				know what was replaced. On the ARM, since the function prologue/epilogue machine code
				can vary, we need the actual bytes of the instruction, so return the savedval instead.
			*/
			return (fbt->fbtp_savedval);
		}
	}

	return (0);
}

#define IS_USER_TRAP(regs)   (PSR64_IS_USER(get_saved_state_cpsr(regs)))
#define T_INVALID_OPCODE EXC_BAD_INSTRUCTION
#define FBT_EXCEPTION_CODE T_INVALID_OPCODE

kern_return_t
fbt_perfCallback(
		 int trapno,
		 struct arm_saved_state * regs,
		 __unused int unused1,
		 __unused int unused2)
{
	kern_return_t   retval = KERN_FAILURE;

	if (FBT_EXCEPTION_CODE == trapno && !IS_USER_TRAP(regs)) {
		boolean_t oldlevel = 0;
		machine_inst_t emul = 0;
		uint64_t sp, pc, lr, imm;

		oldlevel = ml_set_interrupts_enabled(FALSE);

		__asm__ volatile(
			"Ldtrace_invop_callsite_pre_label:\n"
			".data\n"
			".private_extern _dtrace_invop_callsite_pre\n"
			"_dtrace_invop_callsite_pre:\n"
			"  .quad Ldtrace_invop_callsite_pre_label\n"
			".text\n"
				 );

		emul = dtrace_invop(get_saved_state_pc(regs), (uintptr_t*) regs, get_saved_state_reg(regs,0));

		__asm__ volatile(
			"Ldtrace_invop_callsite_post_label:\n"
			".data\n"
			".private_extern _dtrace_invop_callsite_post\n"
			"_dtrace_invop_callsite_post:\n"
			"  .quad Ldtrace_invop_callsite_post_label\n"
			".text\n"
				 );

		if (emul == DTRACE_INVOP_NOP) {
			/*
			 * Skip over the patched NOP planted by sdt
			 */
			pc = get_saved_state_pc(regs);
			set_saved_state_pc(regs, pc + DTRACE_INVOP_NOP_SKIP);
			retval = KERN_SUCCESS;
		} else if (FBT_IS_ARM64_ADD_FP_SP(emul)) {
			/* retrieve the value to add */
			uint64_t val = (emul >> 10) & 0xfff;
			assert(val < 4096);

			/* retrieve sp */
			sp = get_saved_state_sp(regs);

			/*
			 * emulate the instruction:
			 * 	add 	fp, sp, #val
			 */
			assert(sp < (UINT64_MAX - val));
			set_saved_state_fp(regs, sp + val);

			/* skip over the bytes of the patched instruction */
			pc = get_saved_state_pc(regs);
			set_saved_state_pc(regs, pc + DTRACE_INVOP_ADD_FP_SP_SKIP);

			retval = KERN_SUCCESS;
		} else if (FBT_IS_ARM64_RET(emul)) {
			lr = get_saved_state_lr(regs);
#if __has_feature(ptrauth_calls)
			lr = (user_addr_t) ptrauth_strip((void *)lr, ptrauth_key_return_address);
#endif
			set_saved_state_pc(regs, lr);
			retval = KERN_SUCCESS;
		} else if (FBT_IS_ARM64_B_INSTR(emul)) {
			pc = get_saved_state_pc(regs);
			imm = FBT_GET_ARM64_B_IMM(emul);
			set_saved_state_pc(regs, pc + imm);
			retval = KERN_SUCCESS;
		} else if (emul == FBT_PATCHVAL) {
			/* Means we encountered an error but handled it, try same inst again */
			retval = KERN_SUCCESS;
		} else {
			retval = KERN_FAILURE;
		}

		ml_set_interrupts_enabled(oldlevel);
	}

	return retval;
}

void
fbt_provide_probe(struct modctl *ctl, const char *modname, const char* symbolName, machine_inst_t* symbolStart, machine_inst_t *instrHigh)
{
        int		doenable = 0;
	dtrace_id_t	thisid;

	fbt_probe_t	*newfbt, *retfbt, *entryfbt;
	machine_inst_t *instr, *pushinstr = NULL, *limit, theInstr;
	int             foundPushLR, savedRegs;

	/*
	 * Guard against null and invalid symbols
	 */
	if (!symbolStart || !instrHigh || instrHigh < symbolStart) {
		kprintf("dtrace: %s has an invalid address\n", symbolName);
		return;
	}

	/*
	 * Assume the compiler doesn't schedule instructions in the prologue.
	 */
	foundPushLR = 0;
	savedRegs = -1;
	limit = (machine_inst_t *)instrHigh;

	assert(sizeof(*instr) == 4);

	for (instr = symbolStart, theInstr = 0; instr < instrHigh; instr++)
	{
		/*
		 * Count the number of time we pushed something onto the stack
		 * before hitting a frame push. That will give us an estimation
		 * of how many stack pops we should expect when looking for the
		 * RET instruction.
		 */
		theInstr = *instr;
		if (FBT_IS_ARM64_FRAME_PUSH(theInstr)) {
			foundPushLR = 1;
			pushinstr = instr;
		}

		if (foundPushLR && (FBT_IS_ARM64_ADD_FP_SP(theInstr)))
			/* Guard against a random setting of fp from sp, we make sure we found the push first */
			break;
		if (FBT_IS_ARM64_RET(theInstr)) /* We've gone too far, bail. */
			break;
		if (FBT_IS_ARM64_FRAME_POP(theInstr)) /* We've gone too far, bail. */
			break;
	}

	if (!(foundPushLR && (FBT_IS_ARM64_ADD_FP_SP(theInstr)))) {
		return;
	}

	thisid = dtrace_probe_lookup(fbt_id, modname, symbolName, FBT_ENTRY);
	newfbt = kmem_zalloc(sizeof(fbt_probe_t), KM_SLEEP);
	newfbt->fbtp_next = NULL;
	strlcpy( (char *)&(newfbt->fbtp_name), symbolName, MAX_FBTP_NAME_CHARS );

	if (thisid != 0) {
		/*
		 * The dtrace_probe previously existed, so we have to hook
		 * the newfbt entry onto the end of the existing fbt's
		 * chain.
		 * If we find an fbt entry that was previously patched to
		 * fire, (as indicated by the current patched value), then
		 * we want to enable this newfbt on the spot.
		 */
		entryfbt = dtrace_probe_arg (fbt_id, thisid);
		ASSERT (entryfbt != NULL);
		for(; entryfbt != NULL; entryfbt = entryfbt->fbtp_next) {
			if (entryfbt->fbtp_currentval == entryfbt->fbtp_patchval)
				doenable++;

			if (entryfbt->fbtp_next == NULL) {
				entryfbt->fbtp_next = newfbt;
				newfbt->fbtp_id = entryfbt->fbtp_id;
				break;
			}
		}
	}
	else {
		/*
		 * The dtrace_probe did not previously exist, so we
		 * create it and hook in the newfbt.  Since the probe is
		 * new, we obviously do not need to enable it on the spot.
		 */
		newfbt->fbtp_id = dtrace_probe_create(fbt_id, modname, symbolName, FBT_ENTRY, FBT_AFRAMES_ENTRY, newfbt);
		doenable = 0;
	}

	newfbt->fbtp_patchpoint = instr;
	newfbt->fbtp_ctl = ctl;
	newfbt->fbtp_loadcnt = ctl->mod_loadcnt;
	newfbt->fbtp_rval = DTRACE_INVOP_PUSH_FRAME;
	newfbt->fbtp_savedval = theInstr;
	newfbt->fbtp_patchval = FBT_PATCHVAL;
	newfbt->fbtp_currentval = 0;
	newfbt->fbtp_hashnext = fbt_probetab[FBT_ADDR2NDX(instr)];
	fbt_probetab[FBT_ADDR2NDX(instr)] = newfbt;

	if (doenable)
		fbt_enable(NULL, newfbt->fbtp_id, newfbt);

	/*
	 * The fbt entry chain is in place, one entry point per symbol.
	 * The fbt return chain can have multiple return points per
	 * symbol.
	 * Here we find the end of the fbt return chain.
	 */

	doenable=0;

	thisid = dtrace_probe_lookup(fbt_id, modname, symbolName, FBT_RETURN);

	if (thisid != 0) {
		/* The dtrace_probe previously existed, so we have to
		 * find the end of the existing fbt chain.  If we find
		 * an fbt return that was previously patched to fire,
		 * (as indicated by the currrent patched value), then
		 * we want to enable any new fbts on the spot.
		 */
		retfbt = dtrace_probe_arg (fbt_id, thisid);
		ASSERT(retfbt != NULL);
		for (;  retfbt != NULL; retfbt =  retfbt->fbtp_next) {
			if (retfbt->fbtp_currentval == retfbt->fbtp_patchval)
				doenable++;
			if(retfbt->fbtp_next == NULL)
				break;
		}
	}
	else {
		doenable = 0;
		retfbt = NULL;
	}

	/*
	 * Go back to the start of the function, in case
	 * the compiler emitted pcrel data loads
	 * before FP was adjusted.
	 */
	instr = pushinstr + 1;
again:
	if (instr >= limit)
		return;

	/* XXX FIXME ... extra jump table detection? */

	/*
	 * OK, it's an instruction.
	 */
	theInstr = *instr;

	/* Walked onto the start of the next routine? If so, bail out from this function */
	if (FBT_IS_ARM64_FRAME_PUSH(theInstr)) {
		if (!retfbt)
			kprintf("dtrace: fbt: No return probe for %s, walked to next routine at 0x%016llx\n",symbolName,(uint64_t)instr);
		return;
	}

	/* XXX fancy detection of end of function using PC-relative loads */

	/*
	 * Look for:
	 * 	ldp fp, lr, [sp], #val
	 * 	ldp fp, lr, [sp,  #val]
	 */
	if (!FBT_IS_ARM64_FRAME_POP(theInstr)) {
		instr++;
		goto again;
	}

	/* go to the next instruction */
	instr++;

	/* Scan ahead for a ret or a branch outside the function */
	for (; instr < limit; instr++) {
		theInstr = *instr;
		if (FBT_IS_ARM64_RET(theInstr))
			break;
		if (FBT_IS_ARM64_B_INSTR(theInstr)) {
			machine_inst_t *dest = instr + FBT_GET_ARM64_B_IMM(theInstr);
			/*
			 * Check whether the destination of the branch
			 * is outside of the function
			 */
			if (dest >= limit || dest < symbolStart)
				break;
		}
	}

	if (!FBT_IS_ARM64_RET(theInstr) && !FBT_IS_ARM64_B_INSTR(theInstr))
		return;

	newfbt = kmem_zalloc(sizeof(fbt_probe_t), KM_SLEEP);
	newfbt->fbtp_next = NULL;
	strlcpy( (char *)&(newfbt->fbtp_name), symbolName, MAX_FBTP_NAME_CHARS );

	if (retfbt == NULL) {
		newfbt->fbtp_id = dtrace_probe_create(fbt_id, modname,
		    symbolName, FBT_RETURN, FBT_AFRAMES_RETURN, newfbt);
	} else {
		retfbt->fbtp_next = newfbt;
		newfbt->fbtp_id = retfbt->fbtp_id;
	}

	retfbt = newfbt;
	newfbt->fbtp_patchpoint = instr;
	newfbt->fbtp_ctl = ctl;
	newfbt->fbtp_loadcnt = ctl->mod_loadcnt;

	ASSERT(FBT_IS_ARM64_RET(theInstr));
	newfbt->fbtp_rval = DTRACE_INVOP_RET;
	newfbt->fbtp_roffset = (uintptr_t) ((uint8_t*) instr - (uint8_t *)symbolStart);
	newfbt->fbtp_savedval = theInstr;
	newfbt->fbtp_patchval = FBT_PATCHVAL;
	newfbt->fbtp_currentval = 0;
	newfbt->fbtp_hashnext = fbt_probetab[FBT_ADDR2NDX(instr)];
	fbt_probetab[FBT_ADDR2NDX(instr)] = newfbt;

	if (doenable)
		fbt_enable(NULL, newfbt->fbtp_id, newfbt);

	instr++;
	goto again;
}
Commit	Line	Data
5ba3f43e A	1	/*
	2	* Copyright (c) 2007 Apple Inc. All rights reserved.
	3	*/
	4	/*
	5	* CDDL HEADER START
	6	*
	7	* The contents of this file are subject to the terms of the
	8	* Common Development and Distribution License, Version 1.0 only
	9	* (the "License"). You may not use this file except in compliance
	10	* with the License.
	11	*
	12	* You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
	13	* or http://www.opensolaris.org/os/licensing.
	14	* See the License for the specific language governing permissions
	15	* and limitations under the License.
	16	*
	17	* When distributing Covered Code, include this CDDL HEADER in each
	18	* file and include the License file at usr/src/OPENSOLARIS.LICENSE.
	19	* If applicable, add the following below this CDDL HEADER, with the
	20	* fields enclosed by brackets "[]" replaced with your own identifying
	21	* information: Portions Copyright [yyyy] [name of copyright owner]
	22	*
	23	* CDDL HEADER END
	24	*/
	25	/*
	26	* Copyright 2005 Sun Microsystems, Inc. All rights reserved.
	27	* Use is subject to license terms.
	28	*/
	29
	30	/* #pragma ident "@(#)fbt.c 1.15 05/09/19 SMI" */
	31
	32	#ifdef KERNEL
	33	#ifndef _KERNEL
	34	#define _KERNEL /* Solaris vs. Darwin */
	35	#endif
	36	#endif
	37
	38	#define MACH__POSIX_C_SOURCE_PRIVATE 1 /* pulls in suitable savearea from
	39	* mach/ppc/thread_status.h */
	40	#include <kern/thread.h>
	41	#include <mach/thread_status.h>
	42	#include <arm/proc_reg.h>
	43	#include <arm/caches_internal.h>
	44
	45	#include <mach-o/loader.h>
	46	#include <mach-o/nlist.h>
	47	#include <libkern/kernel_mach_header.h>
	48
	49	#include <sys/param.h>
	50	#include <sys/systm.h>
	51	#include <sys/errno.h>
	52	#include <sys/stat.h>
	53	#include <sys/ioctl.h>
	54	#include <sys/conf.h>
	55	#include <sys/fcntl.h>
	56	#include <miscfs/devfs/devfs.h>
	57
	58	#include <sys/dtrace.h>
	59	#include <sys/dtrace_impl.h>
	60	#include <sys/fbt.h>
	61
	62	#include <sys/dtrace_glue.h>
	63
d9a64523 A	64	#if __has_include(<ptrauth.h>)
	65	#include <ptrauth.h>
	66	#endif
	67
5ba3f43e A	68	#define DTRACE_INVOP_PUSH_FRAME 11
	69
	70	#define DTRACE_INVOP_NOP_SKIP 4
	71	#define DTRACE_INVOP_ADD_FP_SP_SKIP 4
	72
	73	#define DTRACE_INVOP_POP_PC_SKIP 2
	74
	75	/*
	76	* stp fp, lr, [sp, #val]
	77	* stp fp, lr, [sp, #val]!
	78	*/
	79	#define FBT_IS_ARM64_FRAME_PUSH(x) \
	80	(((x) & 0xffc07fff) == 0xa9007bfd \|\| ((x) & 0xffc07fff) == 0xa9807bfd)
	81
	82	/*
	83	* stp Xt1, Xt2, [sp, #val]
	84	* stp Xt1, Xt2, [sp, #val]!
	85	*/
	86	#define FBT_IS_ARM64_PUSH(x) \
	87	(((x) & 0xffc003e0) == 0xa90003e0 \|\| ((x) & 0xffc003e0) == 0xa98003e0)
	88
	89	/*
	90	* ldp fp, lr, [sp, #val]
	91	* ldp fp, lr, [sp], #val
	92	*/
	93	#define FBT_IS_ARM64_FRAME_POP(x) \
	94	(((x) & 0xffc07fff) == 0xa9407bfd \|\| ((x) & 0xffc07fff) == 0xa8c07bfd)
	95
	96	#define FBT_IS_ARM64_ADD_FP_SP(x) (((x) & 0xffc003ff) == 0x910003fd) /* add fp, sp, #val (add fp, sp, #0 == mov fp, sp) */
d9a64523	97	#define FBT_IS_ARM64_RET(x) (((x) == 0xd65f03c0) \|\| ((x) == 0xd65f0fff)) /* ret, retab */
5ba3f43e A	98
	99
	100	#define FBT_B_MASK 0xff000000
	101	#define FBT_B_IMM_MASK 0x00ffffff
	102	#define FBT_B_INSTR 0x14000000
	103
	104	#define FBT_IS_ARM64_B_INSTR(x) ((x & FBT_B_MASK) == FBT_B_INSTR)
	105	#define FBT_GET_ARM64_B_IMM(x) ((x & FBT_B_IMM_MASK) << 2)
	106
	107	#define FBT_PATCHVAL 0xe7eeee7e
	108	#define FBT_AFRAMES_ENTRY 7
	109	#define FBT_AFRAMES_RETURN 7
	110
	111	#define FBT_ENTRY "entry"
	112	#define FBT_RETURN "return"
	113	#define FBT_ADDR2NDX(addr) ((((uintptr_t)(addr)) >> 4) & fbt_probetab_mask)
	114
	115	extern dtrace_provider_id_t fbt_id;
	116	extern fbt_probe_t **fbt_probetab;
	117	extern int fbt_probetab_mask;
	118
	119	kern_return_t fbt_perfCallback(int, struct arm_saved_state *, __unused int, __unused int);
	120
	121	int
	122	fbt_invop(uintptr_t addr, uintptr_t * stack, uintptr_t rval)
	123	{
	124	fbt_probe_t *fbt = fbt_probetab[FBT_ADDR2NDX(addr)];
	125
	126	for (; fbt != NULL; fbt = fbt->fbtp_hashnext) {
	127	if ((uintptr_t) fbt->fbtp_patchpoint == addr) {
	128	if (0 == CPU->cpu_dtrace_invop_underway) {
	129	CPU->cpu_dtrace_invop_underway = 1; /* Race not possible on
	130	* this per-cpu state */
	131
	132	if (fbt->fbtp_roffset == 0) {
	133	/*
	134	* Stack looks like this:
d9a64523	135	*
5ba3f43e	136	* [Higher addresses]
d9a64523	137	*
5ba3f43e A	138	* Frame of caller
5ba3f43e A	139	* Extra args for callee
d9a64523	140	* ------------------------
5ba3f43e A	141	* Frame from traced function: <previous sp (e.g. 0x1000), return address>
	142	* ------------------------
	143	* arm_context_t
	144	* ------------------------
	145	* Frame from trap handler: <previous sp (e.g. 0x1000) , traced PC >
	146	* The traced function never got to mov fp, sp,
d9a64523	147	* so there is no frame in the backtrace pointing
5ba3f43e A	148	* to the frame on the stack containing the LR in the
	149	* caller.
	150	* ------------------------
	151	* \|
	152	* \|
	153	* \| stack grows this way
	154	* \|
	155	* \|
	156	* v
	157	* [Lower addresses]
	158	*/
	159
	160	arm_saved_state_t regs = (arm_saved_state_t )(&((arm_context_t *)stack)->ss);
	161
d9a64523 A	162	/*
	163	* cpu_dtrace_caller compensates for fact that the traced function never got to update its fp.
	164	* When walking the stack, when we reach the frame where we extract a PC in the patched
5ba3f43e	165	* function, we put the cpu_dtrace_caller in the backtrace instead. The next frame we extract
d9a64523	166	* will be in the caller's caller, so we output a backtrace starting at the caller and going
5ba3f43e A	167	* sequentially up the stack.
5ba3f43e A	168	*/
d9a64523	169	CPU->cpu_dtrace_caller = get_saved_state_lr(regs);
5ba3f43e A	170	dtrace_probe(fbt->fbtp_id, get_saved_state_reg(regs, 0), get_saved_state_reg(regs, 1),
	171	get_saved_state_reg(regs, 2), get_saved_state_reg(regs, 3),get_saved_state_reg(regs, 4));
	172	CPU->cpu_dtrace_caller = 0;
	173	} else {
	174	/*
	175	* When fbtp_roffset is non-zero, we know we are handling a return probe point.
d9a64523	176	*
5ba3f43e A	177	*
	178	* Stack looks like this, as we've already popped the frame in the traced callee, and
	179	* we trap with lr set to the return address in the caller.
	180	* [Higher addresses]
d9a64523	181	*
5ba3f43e A	182	* Frame of caller
5ba3f43e A	183	* Extra args for callee
d9a64523	184	* ------------------------
5ba3f43e A	185	* arm_context_t
	186	* ------------------------
	187	* Frame from trap handler: <sp at time of trap, traced PC >
	188	* ------------------------
	189	* \|
	190	* \|
	191	* \| stack grows this way
	192	* \|
	193	* \|
	194	* v
	195	* [Lower addresses]
	196	*/
	197	arm_saved_state_t regs = (arm_saved_state_t )(&((arm_context_t *)stack)->ss);
	198
	199	CPU->cpu_dtrace_caller = get_saved_state_lr(regs);
	200	dtrace_probe(fbt->fbtp_id, fbt->fbtp_roffset, rval, 0, 0, 0);
	201	CPU->cpu_dtrace_caller = 0;
	202	}
	203	CPU->cpu_dtrace_invop_underway = 0;
	204	}
d9a64523	205
5ba3f43e A	206	/*
	207	On other architectures, we return a DTRACE constant to let the callback function
	208	know what was replaced. On the ARM, since the function prologue/epilogue machine code
	209	can vary, we need the actual bytes of the instruction, so return the savedval instead.
	210	*/
	211	return (fbt->fbtp_savedval);
	212	}
	213	}
	214
	215	return (0);
	216	}
	217
	218	#define IS_USER_TRAP(regs) (PSR64_IS_USER(get_saved_state_cpsr(regs)))
	219	#define T_INVALID_OPCODE EXC_BAD_INSTRUCTION
	220	#define FBT_EXCEPTION_CODE T_INVALID_OPCODE
	221
	222	kern_return_t
	223	fbt_perfCallback(
	224	int trapno,
	225	struct arm_saved_state * regs,
	226	__unused int unused1,
	227	__unused int unused2)
	228	{
	229	kern_return_t retval = KERN_FAILURE;
	230
	231	if (FBT_EXCEPTION_CODE == trapno && !IS_USER_TRAP(regs)) {
	232	boolean_t oldlevel = 0;
	233	machine_inst_t emul = 0;
	234	uint64_t sp, pc, lr, imm;
	235
	236	oldlevel = ml_set_interrupts_enabled(FALSE);
	237
	238	__asm__ volatile(
	239	"Ldtrace_invop_callsite_pre_label:\n"
	240	".data\n"
	241	".private_extern _dtrace_invop_callsite_pre\n"
	242	"_dtrace_invop_callsite_pre:\n"
	243	" .quad Ldtrace_invop_callsite_pre_label\n"
	244	".text\n"
	245	);
	246
	247	emul = dtrace_invop(get_saved_state_pc(regs), (uintptr_t*) regs, get_saved_state_reg(regs,0));
	248
	249	__asm__ volatile(
	250	"Ldtrace_invop_callsite_post_label:\n"
	251	".data\n"
	252	".private_extern _dtrace_invop_callsite_post\n"
	253	"_dtrace_invop_callsite_post:\n"
	254	" .quad Ldtrace_invop_callsite_post_label\n"
	255	".text\n"
	256	);
	257
	258	if (emul == DTRACE_INVOP_NOP) {
	259	/*
	260	* Skip over the patched NOP planted by sdt
	261	*/
	262	pc = get_saved_state_pc(regs);
	263	set_saved_state_pc(regs, pc + DTRACE_INVOP_NOP_SKIP);
	264	retval = KERN_SUCCESS;
	265	} else if (FBT_IS_ARM64_ADD_FP_SP(emul)) {
	266	/* retrieve the value to add */
	267	uint64_t val = (emul >> 10) & 0xfff;
	268	assert(val < 4096);
	269
270	/* retrieve sp */
271	sp = get_saved_state_sp(regs);
272
273	/*
274	* emulate the instruction:
275	* add fp, sp, #val
276	*/
277	assert(sp < (UINT64_MAX - val));
278	set_saved_state_fp(regs, sp + val);
279
280	/* skip over the bytes of the patched instruction */
281	pc = get_saved_state_pc(regs);
282	set_saved_state_pc(regs, pc + DTRACE_INVOP_ADD_FP_SP_SKIP);
283
284	retval = KERN_SUCCESS;
285	} else if (FBT_IS_ARM64_RET(emul)) {
286	lr = get_saved_state_lr(regs);
d9a64523 A	287	#if __has_feature(ptrauth_calls)
	288	lr = (user_addr_t) ptrauth_strip((void *)lr, ptrauth_key_return_address);
	289	#endif
5ba3f43e	290	set_saved_state_pc(regs, lr);
d9a64523	291	retval = KERN_SUCCESS;
5ba3f43e A	292	} else if (FBT_IS_ARM64_B_INSTR(emul)) {
	293	pc = get_saved_state_pc(regs);
	294	imm = FBT_GET_ARM64_B_IMM(emul);
	295	set_saved_state_pc(regs, pc + imm);
	296	retval = KERN_SUCCESS;
	297	} else if (emul == FBT_PATCHVAL) {
	298	/* Means we encountered an error but handled it, try same inst again */
	299	retval = KERN_SUCCESS;
	300	} else {
	301	retval = KERN_FAILURE;
	302	}
	303
	304	ml_set_interrupts_enabled(oldlevel);
	305	}
	306
	307	return retval;
	308	}
	309
	310	void
d9a64523	311	fbt_provide_probe(struct modctl ctl, const char modname, const char* symbolName, machine_inst_t* symbolStart, machine_inst_t *instrHigh)
5ba3f43e	312	{
5ba3f43e A	313	int doenable = 0;
	314	dtrace_id_t thisid;
	315
	316	fbt_probe_t newfbt, retfbt, *entryfbt;
	317	machine_inst_t instr, pushinstr = NULL, *limit, theInstr;
	318	int foundPushLR, savedRegs;
d9a64523	319
5ba3f43e	320	/*
d9a64523	321	* Guard against null and invalid symbols
5ba3f43e	322	*/
d9a64523	323	if (!symbolStart \|\| !instrHigh \|\| instrHigh < symbolStart) {
5ba3f43e A	324	kprintf("dtrace: %s has an invalid address\n", symbolName);
	325	return;
	326	}
	327
	328	/*
	329	* Assume the compiler doesn't schedule instructions in the prologue.
	330	*/
5ba3f43e A	331	foundPushLR = 0;
	332	savedRegs = -1;
	333	limit = (machine_inst_t *)instrHigh;
	334
	335	assert(sizeof(*instr) == 4);
	336
d9a64523	337	for (instr = symbolStart, theInstr = 0; instr < instrHigh; instr++)
5ba3f43e A	338	{
	339	/*
	340	* Count the number of time we pushed something onto the stack
	341	* before hitting a frame push. That will give us an estimation
	342	* of how many stack pops we should expect when looking for the
	343	* RET instruction.
	344	*/
	345	theInstr = *instr;
	346	if (FBT_IS_ARM64_FRAME_PUSH(theInstr)) {
	347	foundPushLR = 1;
	348	pushinstr = instr;
	349	}
	350
	351	if (foundPushLR && (FBT_IS_ARM64_ADD_FP_SP(theInstr)))
	352	/* Guard against a random setting of fp from sp, we make sure we found the push first */
	353	break;
	354	if (FBT_IS_ARM64_RET(theInstr)) /* We've gone too far, bail. */
	355	break;
	356	if (FBT_IS_ARM64_FRAME_POP(theInstr)) /* We've gone too far, bail. */
	357	break;
	358	}
	359
	360	if (!(foundPushLR && (FBT_IS_ARM64_ADD_FP_SP(theInstr)))) {
	361	return;
	362	}
	363
	364	thisid = dtrace_probe_lookup(fbt_id, modname, symbolName, FBT_ENTRY);
	365	newfbt = kmem_zalloc(sizeof(fbt_probe_t), KM_SLEEP);
	366	newfbt->fbtp_next = NULL;
	367	strlcpy( (char *)&(newfbt->fbtp_name), symbolName, MAX_FBTP_NAME_CHARS );
d9a64523	368
5ba3f43e A	369	if (thisid != 0) {
	370	/*
	371	* The dtrace_probe previously existed, so we have to hook
	372	* the newfbt entry onto the end of the existing fbt's
	373	* chain.
	374	* If we find an fbt entry that was previously patched to
	375	* fire, (as indicated by the current patched value), then
	376	* we want to enable this newfbt on the spot.
	377	*/
	378	entryfbt = dtrace_probe_arg (fbt_id, thisid);
	379	ASSERT (entryfbt != NULL);
	380	for(; entryfbt != NULL; entryfbt = entryfbt->fbtp_next) {
	381	if (entryfbt->fbtp_currentval == entryfbt->fbtp_patchval)
	382	doenable++;
	383
	384	if (entryfbt->fbtp_next == NULL) {
	385	entryfbt->fbtp_next = newfbt;
	386	newfbt->fbtp_id = entryfbt->fbtp_id;
	387	break;
	388	}
	389	}
	390	}
	391	else {
	392	/*
	393	* The dtrace_probe did not previously exist, so we
	394	* create it and hook in the newfbt. Since the probe is
	395	* new, we obviously do not need to enable it on the spot.
	396	*/
	397	newfbt->fbtp_id = dtrace_probe_create(fbt_id, modname, symbolName, FBT_ENTRY, FBT_AFRAMES_ENTRY, newfbt);
	398	doenable = 0;
	399	}
	400
	401	newfbt->fbtp_patchpoint = instr;
	402	newfbt->fbtp_ctl = ctl;
	403	newfbt->fbtp_loadcnt = ctl->mod_loadcnt;
	404	newfbt->fbtp_rval = DTRACE_INVOP_PUSH_FRAME;
	405	newfbt->fbtp_savedval = theInstr;
	406	newfbt->fbtp_patchval = FBT_PATCHVAL;
	407	newfbt->fbtp_currentval = 0;
	408	newfbt->fbtp_hashnext = fbt_probetab[FBT_ADDR2NDX(instr)];
	409	fbt_probetab[FBT_ADDR2NDX(instr)] = newfbt;
	410
	411	if (doenable)
	412	fbt_enable(NULL, newfbt->fbtp_id, newfbt);
	413
	414	/*
	415	* The fbt entry chain is in place, one entry point per symbol.
	416	* The fbt return chain can have multiple return points per
	417	* symbol.
	418	* Here we find the end of the fbt return chain.
	419	*/
	420
	421	doenable=0;
	422
	423	thisid = dtrace_probe_lookup(fbt_id, modname, symbolName, FBT_RETURN);
d9a64523	424
5ba3f43e A	425	if (thisid != 0) {
	426	/* The dtrace_probe previously existed, so we have to
	427	* find the end of the existing fbt chain. If we find
	428	* an fbt return that was previously patched to fire,
	429	* (as indicated by the currrent patched value), then
	430	* we want to enable any new fbts on the spot.
	431	*/
	432	retfbt = dtrace_probe_arg (fbt_id, thisid);
	433	ASSERT(retfbt != NULL);
	434	for (; retfbt != NULL; retfbt = retfbt->fbtp_next) {
	435	if (retfbt->fbtp_currentval == retfbt->fbtp_patchval)
	436	doenable++;
	437	if(retfbt->fbtp_next == NULL)
	438	break;
	439	}
	440	}
	441	else {
	442	doenable = 0;
	443	retfbt = NULL;
	444	}
	445
	446	/*
	447	* Go back to the start of the function, in case
	448	* the compiler emitted pcrel data loads
	449	* before FP was adjusted.
	450	*/
	451	instr = pushinstr + 1;
	452	again:
	453	if (instr >= limit)
	454	return;
	455
	456	/* XXX FIXME ... extra jump table detection? */
	457
	458	/*
	459	* OK, it's an instruction.
	460	*/
	461	theInstr = *instr;
d9a64523	462
5ba3f43e A	463	/* Walked onto the start of the next routine? If so, bail out from this function */
	464	if (FBT_IS_ARM64_FRAME_PUSH(theInstr)) {
	465	if (!retfbt)
	466	kprintf("dtrace: fbt: No return probe for %s, walked to next routine at 0x%016llx\n",symbolName,(uint64_t)instr);
	467	return;
	468	}
	469
	470	/* XXX fancy detection of end of function using PC-relative loads */
	471
	472	/*
	473	* Look for:
	474	* ldp fp, lr, [sp], #val
	475	* ldp fp, lr, [sp, #val]
	476	*/
	477	if (!FBT_IS_ARM64_FRAME_POP(theInstr)) {
	478	instr++;
	479	goto again;
	480	}
	481
	482	/* go to the next instruction */
	483	instr++;
	484
	485	/* Scan ahead for a ret or a branch outside the function */
	486	for (; instr < limit; instr++) {
	487	theInstr = *instr;
	488	if (FBT_IS_ARM64_RET(theInstr))
	489	break;
	490	if (FBT_IS_ARM64_B_INSTR(theInstr)) {
	491	machine_inst_t *dest = instr + FBT_GET_ARM64_B_IMM(theInstr);
	492	/*
	493	* Check whether the destination of the branch
	494	* is outside of the function
	495	*/
	496	if (dest >= limit \|\| dest < symbolStart)
	497	break;
	498	}
	499	}
	500
	501	if (!FBT_IS_ARM64_RET(theInstr) && !FBT_IS_ARM64_B_INSTR(theInstr))
	502	return;
	503
	504	newfbt = kmem_zalloc(sizeof(fbt_probe_t), KM_SLEEP);
d9a64523	505	newfbt->fbtp_next = NULL;
5ba3f43e A	506	strlcpy( (char *)&(newfbt->fbtp_name), symbolName, MAX_FBTP_NAME_CHARS );
	507
	508	if (retfbt == NULL) {
	509	newfbt->fbtp_id = dtrace_probe_create(fbt_id, modname,
	510	symbolName, FBT_RETURN, FBT_AFRAMES_RETURN, newfbt);
	511	} else {
	512	retfbt->fbtp_next = newfbt;
	513	newfbt->fbtp_id = retfbt->fbtp_id;
	514	}
	515
	516	retfbt = newfbt;
	517	newfbt->fbtp_patchpoint = instr;
	518	newfbt->fbtp_ctl = ctl;
	519	newfbt->fbtp_loadcnt = ctl->mod_loadcnt;
	520
	521	ASSERT(FBT_IS_ARM64_RET(theInstr));
	522	newfbt->fbtp_rval = DTRACE_INVOP_RET;
	523	newfbt->fbtp_roffset = (uintptr_t) ((uint8_t) instr - (uint8_t )symbolStart);
	524	newfbt->fbtp_savedval = theInstr;
	525	newfbt->fbtp_patchval = FBT_PATCHVAL;
	526	newfbt->fbtp_currentval = 0;
	527	newfbt->fbtp_hashnext = fbt_probetab[FBT_ADDR2NDX(instr)];
	528	fbt_probetab[FBT_ADDR2NDX(instr)] = newfbt;
	529
	530	if (doenable)
	531	fbt_enable(NULL, newfbt->fbtp_id, newfbt);
	532
	533	instr++;
	534	goto again;
	535	}