1 /* $NetBSD: dtrace_subr.c,v 1.5 2021/04/06 12:48:59 simonb Exp $ */
2
3 /*
4 * CDDL HEADER START
5 *
6 * The contents of this file are subject to the terms of the
7 * Common Development and Distribution License, Version 1.0 only
8 * (the "License"). You may not use this file except in compliance
9 * with the License.
10 *
11 * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
12 * or http://www.opensolaris.org/os/licensing.
13 * See the License for the specific language governing permissions
14 * and limitations under the License.
15 *
16 * When distributing Covered Code, include this CDDL HEADER in each
17 * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
18 * If applicable, add the following below this CDDL HEADER, with the
19 * fields enclosed by brackets "[]" replaced with your own identifying
20 * information: Portions Copyright [yyyy] [name of copyright owner]
21 *
22 * CDDL HEADER END
23 *
24 * $FreeBSD: head/sys/cddl/dev/dtrace/arm/dtrace_subr.c 308457 2016-11-08 23:59:41Z bdrewery $
25 *
26 */
27 /*
28 * Copyright 2005 Sun Microsystems, Inc. All rights reserved.
29 * Use is subject to license terms.
30 */
31
32 #include <sys/param.h>
33 #include <sys/systm.h>
34 #include <sys/types.h>
35 #include <sys/kernel.h>
36 #include <sys/malloc.h>
37 #include <sys/kmem.h>
38 #include <sys/xcall.h>
39 #include <sys/cpu.h>
40 #include <sys/cpuvar.h>
41 #include <sys/dtrace_impl.h>
42 #include <sys/dtrace_bsd.h>
43 #include <machine/cpu.h>
44 #include <machine/frame.h>
45 #include <machine/vmparam.h>
46 #include <uvm/uvm_pglist.h>
47 #include <uvm/uvm_prot.h>
48 #include <uvm/uvm_pmap.h>
49
50 #define FAULT_ALIGN FAULT_ALIGN_0
51 extern uintptr_t kernelbase;
52 extern uintptr_t dtrace_in_probe_addr;
53 extern int dtrace_in_probe;
54
55 void dtrace_gethrtime_init(void *arg);
56
57 #define DELAYBRANCH(x) ((int)(x) < 0)
58
59 #define BIT_PC 15
60 #define BIT_LR 14
61 #define BIT_SP 13
62
63 extern dtrace_id_t dtrace_probeid_error;
64 extern int (*dtrace_invop_jump_addr)(struct trapframe *);
65 extern void dtrace_getnanotime(struct timespec *tsp);
66
67 int dtrace_invop(uintptr_t, struct trapframe *, uintptr_t);
68 void dtrace_invop_init(void);
69 void dtrace_invop_uninit(void);
70
71 typedef struct dtrace_invop_hdlr {
72 int (*dtih_func)(uintptr_t, struct trapframe *, uintptr_t);
73 struct dtrace_invop_hdlr *dtih_next;
74 } dtrace_invop_hdlr_t;
75
76 dtrace_invop_hdlr_t *dtrace_invop_hdlr;
77
78 int
dtrace_invop(uintptr_t addr,struct trapframe * frame,uintptr_t eax)79 dtrace_invop(uintptr_t addr, struct trapframe *frame, uintptr_t eax)
80 {
81 dtrace_invop_hdlr_t *hdlr;
82 int rval;
83
84 for (hdlr = dtrace_invop_hdlr; hdlr != NULL; hdlr = hdlr->dtih_next)
85 if ((rval = hdlr->dtih_func(addr, frame, eax)) != 0)
86 return (rval);
87
88 return (0);
89 }
90
91
92 void
dtrace_invop_add(int (* func)(uintptr_t,struct trapframe *,uintptr_t))93 dtrace_invop_add(int (*func)(uintptr_t, struct trapframe *, uintptr_t))
94 {
95 dtrace_invop_hdlr_t *hdlr;
96
97 hdlr = kmem_alloc(sizeof(*hdlr), KM_SLEEP);
98 hdlr->dtih_func = func;
99 hdlr->dtih_next = dtrace_invop_hdlr;
100 dtrace_invop_hdlr = hdlr;
101 }
102
103 void
dtrace_invop_remove(int (* func)(uintptr_t,struct trapframe *,uintptr_t))104 dtrace_invop_remove(int (*func)(uintptr_t, struct trapframe *, uintptr_t))
105 {
106 dtrace_invop_hdlr_t *hdlr = dtrace_invop_hdlr, *prev = NULL;
107
108 for (;;) {
109 if (hdlr == NULL)
110 panic("attempt to remove non-existent invop handler");
111
112 if (hdlr->dtih_func == func)
113 break;
114
115 prev = hdlr;
116 hdlr = hdlr->dtih_next;
117 }
118
119 if (prev == NULL) {
120 ASSERT(dtrace_invop_hdlr == hdlr);
121 dtrace_invop_hdlr = hdlr->dtih_next;
122 } else {
123 ASSERT(dtrace_invop_hdlr != hdlr);
124 prev->dtih_next = hdlr->dtih_next;
125 }
126
127 kmem_free(hdlr, sizeof(*hdlr));
128 }
129
130 /*ARGSUSED*/
131 void
dtrace_toxic_ranges(void (* func)(uintptr_t base,uintptr_t limit))132 dtrace_toxic_ranges(void (*func)(uintptr_t base, uintptr_t limit))
133 {
134 (*func)(0, kernelbase);
135 }
136
137 static void
xcall_func(void * arg0,void * arg1)138 xcall_func(void *arg0, void *arg1)
139 {
140 dtrace_xcall_t func = arg0;
141
142 (*func)(arg1);
143 }
144
145 void
dtrace_xcall(processorid_t cpu,dtrace_xcall_t func,void * arg)146 dtrace_xcall(processorid_t cpu, dtrace_xcall_t func, void *arg)
147 {
148 uint64_t where;
149
150 if (cpu == DTRACE_CPUALL) {
151 where = xc_broadcast(0, xcall_func, func, arg);
152 } else {
153 struct cpu_info *cinfo = cpu_lookup(cpu);
154
155 KASSERT(cinfo != NULL);
156 where = xc_unicast(0, xcall_func, func, arg, cinfo);
157 }
158 xc_wait(where);
159
160 /* XXX Q. Do we really need the other cpus to wait also?
161 * (see solaris:xc_sync())
162 */
163 }
164
165 static void
dtrace_sync_func(void)166 dtrace_sync_func(void)
167 {
168 }
169
170 void
dtrace_sync(void)171 dtrace_sync(void)
172 {
173 dtrace_xcall(DTRACE_CPUALL, (dtrace_xcall_t)dtrace_sync_func, NULL);
174 }
175
176 /*
177 * DTrace needs a high resolution time function which can
178 * be called from a probe context and guaranteed not to have
179 * instrumented with probes itself.
180 *
181 * Returns nanoseconds since boot.
182 */
183 uint64_t
dtrace_gethrtime(void)184 dtrace_gethrtime(void)
185 {
186 struct timespec curtime;
187
188 nanouptime(&curtime);
189
190 return (curtime.tv_sec * 1000000000UL + curtime.tv_nsec);
191 }
192
193 uint64_t
dtrace_gethrestime(void)194 dtrace_gethrestime(void)
195 {
196 struct timespec current_time;
197
198 dtrace_getnanotime(¤t_time);
199
200 return (current_time.tv_sec * 1000000000UL + current_time.tv_nsec);
201 }
202
203 /* Function to handle DTrace traps during probes. Not used on ARM yet */
204 int
dtrace_trap(struct trapframe * frame,u_int type)205 dtrace_trap(struct trapframe *frame, u_int type)
206 {
207 cpuid_t curcpu_id = cpu_number(); /* current cpu id */
208
209 /*
210 * A trap can occur while DTrace executes a probe. Before
211 * executing the probe, DTrace blocks re-scheduling and sets
212 * a flag in its per-cpu flags to indicate that it doesn't
213 * want to fault. On returning from the probe, the no-fault
214 * flag is cleared and finally re-scheduling is enabled.
215 *
216 * Check if DTrace has enabled 'no-fault' mode:
217 *
218 */
219
220 if ((cpu_core[curcpu_id].cpuc_dtrace_flags & CPU_DTRACE_NOFAULT) != 0) {
221 /*
222 * There are only a couple of trap types that are expected.
223 * All the rest will be handled in the usual way.
224 */
225 switch (type) {
226 /* Page fault. */
227 case FAULT_ALIGN:
228 /* Flag a bad address. */
229 cpu_core[curcpu_id].cpuc_dtrace_flags |= CPU_DTRACE_BADADDR;
230 cpu_core[curcpu_id].cpuc_dtrace_illval = 0;
231
232 /*
233 * Offset the instruction pointer to the instruction
234 * following the one causing the fault.
235 */
236 frame->tf_pc += sizeof(int);
237 return (1);
238 default:
239 /* Handle all other traps in the usual way. */
240 break;
241 }
242 }
243
244 /* Handle the trap in the usual way. */
245 return (0);
246 }
247
248 void
dtrace_probe_error(dtrace_state_t * state,dtrace_epid_t epid,int which,int fault,int fltoffs,uintptr_t illval)249 dtrace_probe_error(dtrace_state_t *state, dtrace_epid_t epid, int which,
250 int fault, int fltoffs, uintptr_t illval)
251 {
252
253 dtrace_probe(dtrace_probeid_error, (uint64_t)(uintptr_t)state,
254 (uintptr_t)epid,
255 (uintptr_t)which, (uintptr_t)fault, (uintptr_t)fltoffs);
256 }
257
258 void
dtrace_gethrtime_init(void * arg)259 dtrace_gethrtime_init(void *arg)
260 {
261 /* FIXME */
262 }
263
264 static uint32_t
dtrace_expand_imm(uint32_t imm12)265 dtrace_expand_imm(uint32_t imm12)
266 {
267 uint32_t unrot = imm12 & 0xff;
268 int amount = 2 * (imm12 >> 8);
269
270 if (amount)
271 return (unrot >> amount) | (unrot << (32 - amount));
272 else
273 return unrot;
274 }
275
276 static uint32_t
dtrace_add_with_carry(uint32_t x,uint32_t y,int carry_in,int * carry_out,int * overflow)277 dtrace_add_with_carry(uint32_t x, uint32_t y, int carry_in,
278 int *carry_out, int *overflow)
279 {
280 uint32_t result;
281 uint64_t unsigned_sum = x + y + (uint32_t)carry_in;
282 int64_t signed_sum = (int32_t)x + (int32_t)y + (int32_t)carry_in;
283 KASSERT(carry_in == 1);
284
285 result = (uint32_t)(unsigned_sum & 0xffffffff);
286 *carry_out = ((uint64_t)result == unsigned_sum) ? 1 : 0;
287 *overflow = ((int64_t)result == signed_sum) ? 0 : 1;
288
289 return result;
290 }
291
292 static void
dtrace_invop_emulate(int invop,struct trapframe * frame)293 dtrace_invop_emulate(int invop, struct trapframe *frame)
294 {
295 uint32_t op = invop;
296 #if 1
297 /* nbsd encoding */
298 uint32_t code = op >> 28;
299 uint32_t data = op;
300 #else
301 /* fbsd encoding */
302 uint32_t code = op & DTRACE_INVOP_MASK;
303 uint32_t data = DTRACE_INVOP_DATA(invop);
304 #endif
305
306 switch (code) {
307 case DTRACE_INVOP_MOV_IP_SP:
308 /* mov ip, sp */
309 frame->tf_ip = frame->tf_svc_sp;
310 frame->tf_pc += 4;
311 break;
312 case DTRACE_INVOP_BX_LR:
313 /* bx lr */
314 frame->tf_pc = frame->tf_svc_lr;
315 break;
316 case DTRACE_INVOP_MOV_PC_LR:
317 /* mov pc, lr */
318 frame->tf_pc = frame->tf_svc_lr;
319 break;
320 case DTRACE_INVOP_LDM:
321 /* ldm sp, {..., pc} */
322 /* FALLTHRU */
323 case DTRACE_INVOP_POPM: {
324 /* ldmib sp, {..., pc} */
325 uint32_t register_list = (op & 0xffff);
326 uint32_t *sp = (uint32_t *)(intptr_t)frame->tf_svc_sp;
327 uint32_t *regs = &frame->tf_r0;
328 int i;
329
330 /* POPM */
331 if (code == DTRACE_INVOP_POPM)
332 sp++;
333
334 for (i = 0; i <= 12; i++) {
335 if (register_list & (1 << i))
336 regs[i] = *sp++;
337 }
338 if (register_list & (1 << 13))
339 frame->tf_svc_sp = *sp++;
340 if (register_list & (1 << 14))
341 frame->tf_svc_lr = *sp++;
342 frame->tf_pc = *sp;
343 break;
344 }
345 case DTRACE_INVOP_LDR_IMM: {
346 /* ldr r?, [{pc,r?}, #?] */
347 uint32_t rt = (op >> 12) & 0xf;
348 uint32_t rn = (op >> 16) & 0xf;
349 uint32_t imm = op & 0xfff;
350 uint32_t *regs = &frame->tf_r0;
351 KDASSERT(rt <= 12);
352 KDASSERT(rn == 15 || rn <= 12);
353 if (rn == 15)
354 regs[rt] = *((uint32_t *)(intptr_t)(frame->tf_pc + 8 + imm));
355 else
356 regs[rt] = *((uint32_t *)(intptr_t)(regs[rn] + imm));
357 frame->tf_pc += 4;
358 break;
359 }
360 case DTRACE_INVOP_MOVW: {
361 /* movw r?, #? */
362 uint32_t rd = (op >> 12) & 0xf;
363 uint32_t imm = (op & 0xfff) | ((op & 0xf0000) >> 4);
364 uint32_t *regs = &frame->tf_r0;
365 KDASSERT(rd <= 12);
366 regs[rd] = imm;
367 frame->tf_pc += 4;
368 break;
369 }
370 case DTRACE_INVOP_MOV_IMM: {
371 /* mov r?, #? */
372 uint32_t rd = (op >> 12) & 0xf;
373 uint32_t imm = dtrace_expand_imm(op & 0xfff);
374 uint32_t *regs = &frame->tf_r0;
375 KDASSERT(rd <= 12);
376 regs[rd] = imm;
377 frame->tf_pc += 4;
378 break;
379 }
380 case DTRACE_INVOP_CMP_IMM: {
381 /* cmp r?, #? */
382 uint32_t rn = (op >> 16) & 0xf;
383 uint32_t *regs = &frame->tf_r0;
384 uint32_t imm = dtrace_expand_imm(op & 0xfff);
385 uint32_t spsr = frame->tf_spsr;
386 uint32_t result;
387 int carry;
388 int overflow;
389 /*
390 * (result, carry, overflow) = AddWithCarry(R[n], NOT(imm32), ’1’);
391 * APSR.N = result<31>;
392 * APSR.Z = IsZeroBit(result);
393 * APSR.C = carry;
394 * APSR.V = overflow;
395 */
396 KDASSERT(rn <= 12);
397 result = dtrace_add_with_carry(regs[rn], ~imm, 1, &carry, &overflow);
398 if (result & 0x80000000)
399 spsr |= PSR_N_bit;
400 else
401 spsr &= ~PSR_N_bit;
402 if (result == 0)
403 spsr |= PSR_Z_bit;
404 else
405 spsr &= ~PSR_Z_bit;
406 if (carry)
407 spsr |= PSR_C_bit;
408 else
409 spsr &= ~PSR_C_bit;
410 if (overflow)
411 spsr |= PSR_V_bit;
412 else
413 spsr &= ~PSR_V_bit;
414
415 #if 0
416 aprint_normal("pc=%x Rn=%x imm=%x %c%c%c%c\n", frame->tf_pc, regs[rn], imm,
417 (spsr & PSR_N_bit) ? 'N' : 'n',
418 (spsr & PSR_Z_bit) ? 'Z' : 'z',
419 (spsr & PSR_C_bit) ? 'C' : 'c',
420 (spsr & PSR_V_bit) ? 'V' : 'v');
421 #endif
422 frame->tf_spsr = spsr;
423 frame->tf_pc += 4;
424 break;
425 }
426 case DTRACE_INVOP_B: {
427 /* b ??? */
428 uint32_t imm = (op & 0x00ffffff) << 2;
429 int32_t diff;
430 /* SignExtend(imm26, 32) */
431 if (imm & 0x02000000)
432 imm |= 0xfc000000;
433 diff = (int32_t)imm;
434 frame->tf_pc += 8 + diff;
435 break;
436 }
437 case DTRACE_INVOP_PUSHM: {
438 /* push {...} */
439 uint32_t register_list = (op & 0xffff);
440 uint32_t *sp = (uint32_t *)(intptr_t)frame->tf_svc_sp;
441 uint32_t *regs = &frame->tf_r0;
442 int i;
443 int count = 0;
444
445 #if 0
446 if ((op & 0x0fff0fff) == 0x052d0004) {
447 /* A2: str r4, [sp, #-4]! */
448 *(sp - 1) = regs[4];
449 frame->tf_pc += 4;
450 break;
451 }
452 #endif
453
454 for (i = 0; i < 16; i++) {
455 if (register_list & (1 << i))
456 count++;
457 }
458 sp -= count;
459
460 for (i = 0; i <= 12; i++) {
461 if (register_list & (1 << i))
462 *sp++ = regs[i];
463 }
464 if (register_list & (1 << 13))
465 *sp++ = frame->tf_svc_sp;
466 if (register_list & (1 << 14))
467 *sp++ = frame->tf_svc_lr;
468 if (register_list & (1 << 15))
469 *sp = frame->tf_pc + 8;
470
471 /* make sure the caches and memory are in sync */
472 cpu_dcache_wbinv_range(frame->tf_svc_sp, count * 4);
473
474 /* In case the current page tables have been modified ... */
475 cpu_tlb_flushID();
476 cpu_cpwait();
477
478 frame->tf_svc_sp -= count * 4;
479 frame->tf_pc += 4;
480
481 break;
482 }
483 default:
484 KDASSERTMSG(0, "invop 0x%08x code %u tf %p", invop, code, frame);
485 }
486 }
487
488 static int
dtrace_invop_start(struct trapframe * frame)489 dtrace_invop_start(struct trapframe *frame)
490 {
491 #if 0
492 register_t *r0, *sp;
493 int data, invop, reg, update_sp;
494 #endif
495 int invop;
496
497 invop = dtrace_invop(frame->tf_pc, frame, frame->tf_r0);
498
499 dtrace_invop_emulate(invop, frame);
500
501 #if 0
502 switch (invop & DTRACE_INVOP_MASK) {
503 case DTRACE_INVOP_PUSHM:
504 sp = (register_t *)frame->tf_svc_sp;
505 r0 = &frame->tf_r0;
506 data = DTRACE_INVOP_DATA(invop);
507
508 /*
509 * Store the pc, lr, and sp. These have their own
510 * entries in the struct.
511 */
512 if (data & (1 << BIT_PC)) {
513 sp--;
514 *sp = frame->tf_pc;
515 }
516 if (data & (1 << BIT_LR)) {
517 sp--;
518 *sp = frame->tf_svc_lr;
519 }
520 if (data & (1 << BIT_SP)) {
521 sp--;
522 *sp = frame->tf_svc_sp;
523 }
524
525 /* Store the general registers */
526 for (reg = 12; reg >= 0; reg--) {
527 if (data & (1 << reg)) {
528 sp--;
529 *sp = r0[reg];
530 }
531 }
532
533 /* Update the stack pointer and program counter to continue */
534 frame->tf_svc_sp = (register_t)sp;
535 frame->tf_pc += 4;
536 break;
537 case DTRACE_INVOP_POPM:
538 sp = (register_t *)frame->tf_svc_sp;
539 r0 = &frame->tf_r0;
540 data = DTRACE_INVOP_DATA(invop);
541
542 /* Read the general registers */
543 for (reg = 0; reg <= 12; reg++) {
544 if (data & (1 << reg)) {
545 r0[reg] = *sp;
546 sp++;
547 }
548 }
549
550 /*
551 * Set the stack pointer. If we don't update it here we will
552 * need to update it at the end as the instruction would do
553 */
554 update_sp = 1;
555 if (data & (1 << BIT_SP)) {
556 frame->tf_svc_sp = *sp;
557 *sp++;
558 update_sp = 0;
559 }
560
561 /* Update the link register, we need to use the correct copy */
562 if (data & (1 << BIT_LR)) {
563 frame->tf_svc_lr = *sp;
564 *sp++;
565 }
566 /*
567 * And the program counter. If it's not in the list skip over
568 * it when we return so to not hit this again.
569 */
570 if (data & (1 << BIT_PC)) {
571 frame->tf_pc = *sp;
572 *sp++;
573 } else
574 frame->tf_pc += 4;
575
576 /* Update the stack pointer if we haven't already done so */
577 if (update_sp)
578 frame->tf_svc_sp = (register_t)sp;
579 break;
580 case DTRACE_INVOP_B:
581 data = DTRACE_INVOP_DATA(invop) & 0x00ffffff;
582 /* Sign extend the data */
583 if ((data & (1 << 23)) != 0)
584 data |= 0xff000000;
585 /* The data is the number of 4-byte words to change the pc */
586 data *= 4;
587 data += 8;
588 frame->tf_pc += data;
589 break;
590
591 default:
592 return (-1);
593 break;
594 }
595 #endif
596
597 return (0);
598 }
599
dtrace_invop_init(void)600 void dtrace_invop_init(void)
601 {
602 dtrace_invop_jump_addr = dtrace_invop_start;
603 }
604
dtrace_invop_uninit(void)605 void dtrace_invop_uninit(void)
606 {
607 dtrace_invop_jump_addr = 0;
608 }
609