1 /* $NetBSD: dtrace_subr.c,v 1.4 2018/05/28 21:05:03 chs 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 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 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 (dtrace_invop_hdlr_t), KM_SLEEP); 98 hdlr->dtih_func = func; 99 hdlr->dtih_next = dtrace_invop_hdlr; 100 dtrace_invop_hdlr = hdlr; 101 } 102 103 void 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 (dtrace_invop_hdlr_t)); 128 } 129 130 /*ARGSUSED*/ 131 void 132 dtrace_toxic_ranges(void (*func)(uintptr_t base, uintptr_t limit)) 133 { 134 (*func)(0, kernelbase); 135 } 136 137 static void 138 xcall_func(void *arg0, void *arg1) 139 { 140 dtrace_xcall_t func = arg0; 141 142 (*func)(arg1); 143 } 144 145 void 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 166 dtrace_sync_func(void) 167 { 168 } 169 170 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 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 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 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 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 259 dtrace_gethrtime_init(void *arg) 260 { 261 /* FIXME */ 262 } 263 264 static uint32_t 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 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 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 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 600 void dtrace_invop_init(void) 601 { 602 dtrace_invop_jump_addr = dtrace_invop_start; 603 } 604 605 void dtrace_invop_uninit(void) 606 { 607 dtrace_invop_jump_addr = 0; 608 } 609