1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2024 Mark Johnston <markj@FreeBSD.org>
5 */
6
7 #include <sys/systm.h>
8 #include <sys/sdt.h>
9
10 #include <vm/vm.h>
11 #include <vm/pmap.h>
12
13 #include <machine/cpufunc.h>
14 #include <machine/md_var.h>
15 #include <machine/vmparam.h>
16
17 /*
18 * Return true if we can overwrite a nop at "patchpoint" with a jump to the
19 * target address.
20 */
21 bool
sdt_tracepoint_valid(uintptr_t patchpoint,uintptr_t target)22 sdt_tracepoint_valid(uintptr_t patchpoint, uintptr_t target)
23 {
24 void *addr;
25 int64_t offset;
26
27 if (!arm64_get_writable_addr((void *)patchpoint, &addr))
28 return (false);
29
30 if (patchpoint == target ||
31 (patchpoint & (INSN_SIZE - 1)) != 0 ||
32 (target & (INSN_SIZE - 1)) != 0)
33 return (false);
34 offset = target - patchpoint;
35 if (offset < -(1 << 26) || offset > (1 << 26))
36 return (false);
37 return (true);
38 }
39
40 /*
41 * Overwrite the copy of _SDT_ASM_PATCH_INSTR at the tracepoint with a jump to the
42 * target address.
43 */
44 void
sdt_tracepoint_patch(uintptr_t patchpoint,uintptr_t target)45 sdt_tracepoint_patch(uintptr_t patchpoint, uintptr_t target)
46 {
47 void *addr;
48 uint32_t instr;
49
50 KASSERT(sdt_tracepoint_valid(patchpoint, target),
51 ("%s: invalid tracepoint %#lx -> %#lx",
52 __func__, patchpoint, target));
53
54 if (!arm64_get_writable_addr((void *)patchpoint, &addr))
55 panic("%s: Unable to write new instruction", __func__);
56
57 instr = (((target - patchpoint) >> 2) & 0x3fffffful) | 0x14000000;
58 memcpy(addr, &instr, sizeof(instr));
59 cpu_icache_sync_range((void *)patchpoint, INSN_SIZE);
60 }
61
62 /*
63 * Overwrite the patchpoint with a nop instruction.
64 */
65 void
sdt_tracepoint_restore(uintptr_t patchpoint)66 sdt_tracepoint_restore(uintptr_t patchpoint)
67 {
68 void *addr;
69 uint32_t instr;
70
71 if (!arm64_get_writable_addr((void *)patchpoint, &addr))
72 panic("%s: Unable to write new instruction", __func__);
73
74 instr = 0xd503201f;
75 memcpy(addr, &instr, sizeof(instr));
76 cpu_icache_sync_range((void *)patchpoint, INSN_SIZE);
77 }
78