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

/*
 * Copyright (c) 2007-2018 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.
 */

#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 */

				/*
				 * Stack looks like this:
				 *
				 *      [Higher addresses]
				 *
				 *      Frame of caller
				 *      Extra args for callee
				 *      ------------------------
				 *      fbt entry probe:
				 *          Frame from traced function: <previous sp (e.g. 0x1000), return address>
				 *      fbt return probe:
				 *          Missing as the return probe has already popped the frame in the callee and
				 *          traps with LR set to the return address in caller.
				 *      ------------------------
				 *      arm_context_t
				 *      ------------------------
				 *      Frame from trap handler:  <previous sp (e.g. 0x1000) , traced PC >
				 *                                The traced function has either never pushed the frame
				 *                                or already popped it.  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]
				 *
				 * cpu_dtrace_caller compensates for fact that the LR is not stored on stack as explained
				 * above.  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.
				 */
				arm_saved_state_t *regs = (arm_saved_state_t *)(&((arm_context_t *)stack)->ss);

				CPU->cpu_dtrace_caller = get_saved_state_lr(regs);

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