bsd/dev/arm/fbt_arm.c

   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 #include <arm/thread.h>
  45
  46 #include <mach-o/loader.h>
  47 #include <mach-o/nlist.h>
  48 #include <libkern/kernel_mach_header.h>
  49
  50 #include <sys/param.h>
  51 #include <sys/systm.h>
  52 #include <sys/errno.h>
  53 #include <sys/stat.h>
  54 #include <sys/ioctl.h>
  55 #include <sys/conf.h>
  56 #include <sys/fcntl.h>
  57 #include <miscfs/devfs/devfs.h>
  58
  59 #include <sys/dtrace.h>
  60 #include <sys/dtrace_impl.h>
  61 #include <sys/fbt.h>
  62
  63 #include <sys/dtrace_glue.h>
  64
  65 #define DTRACE_INVOP_PUSH_LR 8
  66 #define DTRACE_INVOP_BL 9
  67 #define DTRACE_INVOP_POP_PC 10
  68
  69 #define DTRACE_INVOP_THUMB_NOP_SKIP 2
  70 #define DTRACE_INVOP_POP_PC_SKIP 2
  71 #define DTRACE_INVOP_THUMB_SET_R7_SKIP 2
  72 #define DTRACE_INVOP_THUMB_MOV_SP_TO_R7_SKIP 2
  73
  74 #define FBT_IS_THUMB_PUSH_LR(x)         (((x) & 0x0000ff00) == 0x0000b500)
  75 #define FBT_IS_THUMB_POP_R7(x)          (((x) & 0x0000ff80) == 0x0000bc80)
  76 #define FBT_IS_THUMB32_POP_R7LR(x,y)    (((x) == 0x0000e8bd) && (((y) & 0x00004080) == 0x00004080))
  77 #define FBT_IS_THUMB_POP_PC(x)          (((x) & 0x0000ff00) == 0x0000bd00)
  78 #define FBT_IS_THUMB_SET_R7(x)          (((x) & 0x0000ff00) == 0x0000af00)
  79 #define FBT_IS_THUMB_MOV_SP_TO_R7(x)            (((x) & 0x0000ffff) == 0x0000466f)
  80 #define FBT_THUMB_SET_R7_OFFSET(x)      (((x) & 0x000000ff) << 2)
  81 #define FBT_IS_THUMB_LDR_PC(x)          (((x) & 0x0000f800) == 0x00004800)
  82 #define FBT_IS_THUMB32_LDR_PC(x,y)      ((x) == 0x0000f8df)                     /* Only for positive offset PC relative loads */
  83 #define FBT_THUMB_STACK_REGS(x)         ((x) & 0x00FF)
  84 #define FBT_IS_THUMB_BX_REG(x)          (((x) & 0x0000ff87) == 0x00004700)
  85
  86 #define FBT_PATCHVAL                    0xdefc
  87 #define FBT_AFRAMES_ENTRY               8
  88 #define FBT_AFRAMES_RETURN              6
  89
  90 #define FBT_ENTRY       "entry"
  91 #define FBT_RETURN      "return"
  92 #define FBT_ADDR2NDX(addr)      ((((uintptr_t)(addr)) >> 4) & fbt_probetab_mask)
  93
  94 #define VFPSAVE_ALIGN_DTRACE    16      /* This value should come from VFPSAVE_ALIGN */
  95
  96 extern dtrace_provider_id_t     fbt_id;
  97 extern fbt_probe_t               **fbt_probetab;
  98 extern int                      fbt_probetab_mask;
  99
 100 kern_return_t fbt_perfCallback(int, struct arm_saved_state *, __unused int, __unused int);
 101
 102 extern int dtrace_arm_condition_true(int cond, int cpsr);
 103
 104
 105 /* Calculate the address of the ldr. (From the ARM Architecture reference) */
 106 /* Does not check to see if it's really a load instruction, caller must do that */
 107
 108 static uint32_t thumb_ldr_pc_address(uint32_t address)
 109 {
 110         return (address & 0xFFFFFFFC) + (*(uint16_t*) address & 0xFF) * 4 + 4;
 111 }
 112
 113 static uint32_t thumb32_ldr_pc_address(uint32_t address)
 114 {
 115         return (address & 0xFFFFFFFC) + (*(uint16_t*) (address+2) & 0xFFF) + 4;
 116 }
 117
 118 /* Extract the current ITSTATE from the CPSR */
 119 static uint32_t get_itstate(uint32_t cpsr)
 120 {
 121         return
 122                 ((cpsr & 0x06000000) >> 25) |
 123                 ((cpsr & 0x0000FC00) >> 8);
 124 }
 125
 126 static void clear_itstate(uint32_t* cpsr)
 127 {
 128         *cpsr &= ~0x0600FC00;
 129 }
 130
 131 int
 132 fbt_invop(uintptr_t addr, uintptr_t * stack, uintptr_t rval)
 133 {
 134         fbt_probe_t    *fbt = fbt_probetab[FBT_ADDR2NDX(addr)];
 135
 136         for (; fbt != NULL; fbt = fbt->fbtp_hashnext) {
 137                 if ((uintptr_t) fbt->fbtp_patchpoint == addr) {
 138                         if (0 == CPU->cpu_dtrace_invop_underway) {
 139                                 CPU->cpu_dtrace_invop_underway = 1;     /* Race not possible on
 140                                                                          * this per-cpu state */
 141
 142                                 struct arm_saved_state* regs = (struct arm_saved_state*) stack;
 143                                 uintptr_t stack4 = *((uintptr_t*) regs->sp);
 144
 145                                 if ((regs->cpsr & PSR_MODE_MASK) == PSR_FIQ_MODE) {
 146                                         /*
 147                                          * We do not handle probes firing from FIQ context. We used to
 148                                          * try to undo the patch and rerun the instruction, but
 149                                          * most of the time we can't do that successfully anyway.
 150                                          * Instead, we just panic now so we fail fast.
 151                                          */
 152                                         panic("dtrace: fbt: The probe at %08x was called from FIQ_MODE",(unsigned) addr);
 153                                 }
 154
 155                                 /*
 156                                  * If we are not outside an IT block, and are not executing the last instruction of an IT block,
 157                                  * then that is an instrumentation error or a code gen error. Either way, we panic.
 158                                  */
 159                                 uint32_t itstate = get_itstate(regs->cpsr);
 160                                 if ((itstate & 0x7) != 0) {
 161                                         panic("dtrace: fbt: Instruction stream error: Middle of IT block at %08x",(unsigned) addr);
 162                                 }
 163
 164                                 if (fbt->fbtp_roffset == 0) {
 165                                         /*
 166                                                 We need the frames to set up the backtrace, but we won't have the frame pointers
 167                                                 until after the instruction is emulated. So here we calculate the address of the
 168                                                 frame pointer from the saved instruction and put it in the stack. Yes, we end up
 169                                                 repeating this work again when we emulate the instruction.
 170
 171                                                 This assumes that the frame area is immediately after the saved reg storage!
 172                                         */
 173                                         uint32_t offset = ((uint32_t) regs) + sizeof(struct arm_saved_state);
 174 #if __ARM_VFP__
 175                                         /* Match the stack alignment required for arm_vfpsaved_state */
 176                                         offset &= ~(VFPSAVE_ALIGN_DTRACE - 1);
 177                                         offset += VFPSAVE_ALIGN_DTRACE + sizeof(struct arm_vfpsaved_state);
 178 #endif /* __ARM_VFP__ */
 179                                         if (FBT_IS_THUMB_SET_R7(fbt->fbtp_savedval))
 180                                                 *((uint32_t*) offset) = regs->sp + FBT_THUMB_SET_R7_OFFSET(fbt->fbtp_savedval);
 181                                         else
 182                                                 *((uint32_t*) offset) = regs->sp;
 183
 184                                         CPU->cpu_dtrace_caller = regs->lr;
 185                                         dtrace_probe(fbt->fbtp_id, regs->r[0], regs->r[1], regs->r[2], regs->r[3], stack4);
 186                                         CPU->cpu_dtrace_caller = 0;
 187                                 } else {
 188                                         /* Check to see if we're in the middle of an IT block. */
 189                                         if (itstate != 0) {
 190                                                 /*
 191                                                  * We've already checked previously to see how far we are in the IT block.
 192                                                  * Here we must be getting ready to execute the last instruction.
 193                                                  */
 194                                                 int condition_it = (itstate & 0xF0) >> 4;
 195
 196                                                 if (dtrace_arm_condition_true(condition_it, regs->cpsr) == 0) {
 197                                                         /* Condition wasn't true, so becomes a nop. */
 198                                                         clear_itstate(&regs->cpsr);
 199                                                         CPU->cpu_dtrace_invop_underway = 0;
 200                                                         return DTRACE_INVOP_NOP;
 201                                                 }
 202                                         }
 203
 204                                         dtrace_probe(fbt->fbtp_id, fbt->fbtp_roffset, rval, 0, 0, 0);
 205                                         CPU->cpu_dtrace_caller = 0;
 206
 207                                         /* The dtrace script may access cpsr, so make sure to clear only after probe fired. */
 208                                         clear_itstate(&regs->cpsr);
 209                                 }
 210                                 CPU->cpu_dtrace_invop_underway = 0;
 211                         }
 212
 213                         /*
 214                                 On other architectures, we return a DTRACE constant to let the callback function
 215                                 know what was replaced. On the ARM, since the function prologue/epilogue machine code
 216                                 can vary, we need the actual bytes of the instruction, so return the savedval instead.
 217                         */
 218                         return (fbt->fbtp_savedval);
 219                 }
 220         }
 221
 222         return (0);
 223 }
 224
 225 #define IS_USER_TRAP(regs)  (((regs)->cpsr & PSR_MODE_MASK) == PSR_USER_MODE)
 226 #define T_INVALID_OPCODE EXC_BAD_INSTRUCTION
 227 #define FBT_EXCEPTION_CODE T_INVALID_OPCODE
 228
 229 kern_return_t
 230 fbt_perfCallback(
 231                  int trapno,
 232                  struct arm_saved_state * regs,
 233                  __unused int unused1,
 234                  __unused int unused2)
 235 {
 236 #pragma unused (unused1)
 237 #pragma unused (unused2)
 238         kern_return_t   retval = KERN_FAILURE;
 239
 240         if (FBT_EXCEPTION_CODE == trapno && !IS_USER_TRAP(regs)) {
 241                 boolean_t oldlevel = 0;
 242                 machine_inst_t emul = 0;
 243
 244                 oldlevel = ml_set_interrupts_enabled(FALSE);
 245
 246                 __asm__ volatile(
 247                         "Ldtrace_invop_callsite_pre_label:\n"
 248                         ".data\n"
 249                         ".private_extern _dtrace_invop_callsite_pre\n"
 250                         "_dtrace_invop_callsite_pre:\n"
 251                         "  .long Ldtrace_invop_callsite_pre_label\n"
 252                         ".text\n"
 253                                  );
 254
 255                 emul = dtrace_invop(regs->pc, (uintptr_t*) regs, regs->r[0]);
 256
 257                 __asm__ volatile(
 258                         "Ldtrace_invop_callsite_post_label:\n"
 259                         ".data\n"
 260                         ".private_extern _dtrace_invop_callsite_post\n"
 261                         "_dtrace_invop_callsite_post:\n"
 262                         "  .long Ldtrace_invop_callsite_post_label\n"
 263                         ".text\n"
 264                                  );
 265
 266                 /*
 267                  * The following emulation code does not execute properly if we are in the middle of
 268                  * an IT block. IT blocks need to be handled in the dtrace_invop function. If we do
 269                  * manage to get here and we are inside an IT block, then we missed a case somewhere
 270                  * prior to this point.
 271                  */
 272                 uint32_t itstate = get_itstate(regs->cpsr);
 273                 if (itstate != 0) {
 274                         panic("dtrace: fbt: Not emulated: Middle of IT block at %08x",(unsigned) regs->pc);
 275                 }
 276
 277                 if (emul == DTRACE_INVOP_NOP) {
 278                         regs->pc += DTRACE_INVOP_THUMB_NOP_SKIP;
 279                         retval = KERN_SUCCESS;
 280                 } else if (FBT_IS_THUMB_SET_R7(emul)) {
 281                         regs->r[7] = regs->sp + FBT_THUMB_SET_R7_OFFSET(emul);
 282                         regs->pc += DTRACE_INVOP_THUMB_SET_R7_SKIP;
 283                         retval = KERN_SUCCESS;
 284                 } else if (FBT_IS_THUMB_MOV_SP_TO_R7(emul)) {
 285                         regs->r[7] = regs->sp;
 286                         regs->pc += DTRACE_INVOP_THUMB_MOV_SP_TO_R7_SKIP;
 287                         retval = KERN_SUCCESS;
 288                 } else if (FBT_IS_THUMB_POP_PC(emul)) {
 289                         uintptr_t* sp = (uintptr_t*) regs->sp;
 290
 291                         machine_inst_t mask = 0x0001;
 292                         int regnum = 0;
 293                         while (mask & 0x00ff) {
 294                                 if (emul & mask) {
 295                                         /* Pop this register */
 296                                         regs->r[regnum] = *sp++;
 297                                 }
 298                                 mask <<= 1;
 299                                 regnum++;
 300                         }
 301
 302                         regs->pc = *sp++;
 303                         regs->sp = (uintptr_t) sp;
 304                         if (regs->pc & 1) {
 305                                 regs->cpsr |= PSR_TF;
 306                         } else {
 307                                 regs->cpsr &= ~PSR_TF;
 308                         }
 309
 310                         retval = KERN_SUCCESS;
 311                 } else if (FBT_IS_THUMB_BX_REG(emul)) {
 312                         regs->pc = regs->r[(emul >> 3) & 0xF];
 313
 314                         if (regs->pc & 1) {
 315                                 regs->cpsr |= PSR_TF;
 316                         } else {
 317                                 regs->cpsr &= ~PSR_TF;
 318                         }
 319
 320                         retval = KERN_SUCCESS;
 321                 } else if (emul == FBT_PATCHVAL) {
 322                         /* Means we encountered an error but handled it, try same inst again */
 323                         retval = KERN_SUCCESS;
 324                 } else {
 325                         retval = KERN_FAILURE;
 326                 }
 327
 328                 ml_set_interrupts_enabled(oldlevel);
 329         }
 330
 331         return retval;
 332 }
 333
 334 void
 335 fbt_provide_probe(struct modctl *ctl, const char *modname, const char* symbolName, machine_inst_t* symbolStart, machine_inst_t *instrHigh)
 336 {
 337         unsigned int    j;
 338         int             doenable = 0;
 339         dtrace_id_t     thisid;
 340
 341         fbt_probe_t     *newfbt, *retfbt, *entryfbt;
 342         machine_inst_t *instr, *pushinstr = NULL, *limit, theInstr;
 343         int             foundPushLR, savedRegs;
 344
 345         /*
 346          * Guard against null symbols
 347          */
 348         if (!symbolStart || !instrHigh || instrHigh < symbolStart) {
 349                 kprintf("dtrace: %s has an invalid address\n", symbolName);
 350                 return;
 351         }
 352
 353         /*
 354          * Assume the compiler doesn't schedule instructions in the prologue.
 355          */
 356         foundPushLR = 0;
 357         savedRegs = -1;
 358         limit = (machine_inst_t *)instrHigh;
 359         for (j = 0, instr = symbolStart, theInstr = 0;
 360              (j < 8) && instr < instrHigh; j++, instr++)
 361         {
 362                 theInstr = *instr;
 363                 if (FBT_IS_THUMB_PUSH_LR(theInstr)) {
 364                         foundPushLR = 1;
 365                         /* Keep track of what registers we pushed. Compare this against the pop later. */
 366                         savedRegs = FBT_THUMB_STACK_REGS(theInstr);
 367                         pushinstr = instr;
 368                 }
 369                 if (foundPushLR && (FBT_IS_THUMB_SET_R7(theInstr) || FBT_IS_THUMB_MOV_SP_TO_R7(theInstr)))
 370                         /* Guard against a random setting of r7 from sp, we make sure we found the push first */
 371                         break;
 372                 if (FBT_IS_THUMB_BX_REG(theInstr)) /* We've gone too far, bail. */
 373                         break;
 374                 if (FBT_IS_THUMB_POP_PC(theInstr)) /* We've gone too far, bail. */
 375                         break;
 376
 377                 /* Check for 4 byte thumb instruction */
 378                 if (dtrace_instr_size(theInstr,1) == 4)
 379                         instr++;
 380         }
 381
 382         if (!(foundPushLR && (FBT_IS_THUMB_SET_R7(theInstr) || FBT_IS_THUMB_MOV_SP_TO_R7(theInstr)))) {
 383                 return;
 384         }
 385
 386         thisid = dtrace_probe_lookup(fbt_id, modname, symbolName, FBT_ENTRY);
 387         newfbt = kmem_zalloc(sizeof(fbt_probe_t), KM_SLEEP);
 388         newfbt->fbtp_next = NULL;
 389         strlcpy( (char *)&(newfbt->fbtp_name), symbolName, MAX_FBTP_NAME_CHARS );
 390
 391         if (thisid != 0) {
 392                 /*
 393                  * The dtrace_probe previously existed, so we have to hook
 394                  * the newfbt entry onto the end of the existing fbt's
 395                  * chain.
 396                  * If we find an fbt entry that was previously patched to
 397                  * fire, (as indicated by the current patched value), then
 398                  * we want to enable this newfbt on the spot.
 399                  */
 400                 entryfbt = dtrace_probe_arg (fbt_id, thisid);
 401                 ASSERT (entryfbt != NULL);
 402                 for(; entryfbt != NULL; entryfbt = entryfbt->fbtp_next) {
 403                         if (entryfbt->fbtp_currentval == entryfbt->fbtp_patchval)
 404                                 doenable++;
 405
 406                         if (entryfbt->fbtp_next == NULL) {
 407                                 entryfbt->fbtp_next = newfbt;
 408                                 newfbt->fbtp_id = entryfbt->fbtp_id;
 409                                 break;
 410                         }
 411                 }
 412         }
 413         else {
 414                 /*
 415                  * The dtrace_probe did not previously exist, so we
 416                  * create it and hook in the newfbt.  Since the probe is
 417                  * new, we obviously do not need to enable it on the spot.
 418                  */
 419                 newfbt->fbtp_id = dtrace_probe_create(fbt_id, modname, symbolName, FBT_ENTRY, FBT_AFRAMES_ENTRY, newfbt);
 420                 doenable = 0;
 421         }
 422
 423         newfbt->fbtp_patchpoint = instr;
 424         newfbt->fbtp_ctl = ctl;
 425         newfbt->fbtp_loadcnt = ctl->mod_loadcnt;
 426         newfbt->fbtp_rval = DTRACE_INVOP_PUSH_LR;
 427         newfbt->fbtp_savedval = theInstr;
 428         newfbt->fbtp_patchval = FBT_PATCHVAL;
 429         newfbt->fbtp_currentval = 0;
 430         newfbt->fbtp_hashnext = fbt_probetab[FBT_ADDR2NDX(instr)];
 431         fbt_probetab[FBT_ADDR2NDX(instr)] = newfbt;
 432
 433         if (doenable)
 434                 fbt_enable(NULL, newfbt->fbtp_id, newfbt);
 435
 436         /*
 437          * The fbt entry chain is in place, one entry point per symbol.
 438          * The fbt return chain can have multiple return points per
 439          * symbol.
 440          * Here we find the end of the fbt return chain.
 441          */
 442
 443         doenable=0;
 444
 445         thisid = dtrace_probe_lookup(fbt_id, modname, symbolName, FBT_RETURN);
 446
 447         if (thisid != 0) {
 448                 /* The dtrace_probe previously existed, so we have to
 449                  * find the end of the existing fbt chain.  If we find
 450                  * an fbt return that was previously patched to fire,
 451                  * (as indicated by the currrent patched value), then
 452                  * we want to enable any new fbts on the spot.
 453                  */
 454                 retfbt = dtrace_probe_arg (fbt_id, thisid);
 455                 ASSERT(retfbt != NULL);
 456                 for (;  retfbt != NULL; retfbt =  retfbt->fbtp_next) {
 457                         if (retfbt->fbtp_currentval == retfbt->fbtp_patchval)
 458                                 doenable++;
 459                         if(retfbt->fbtp_next == NULL)
 460                                 break;
 461                 }
 462         }
 463         else {
 464                 doenable = 0;
 465                 retfbt = NULL;
 466         }
 467
 468         /*
 469          * Go back to the start of the function, in case
 470          * the compiler emitted pcrel data loads
 471          * before R7 was adjusted.
 472          */
 473         instr = pushinstr + 1;
 474 again:
 475         if (instr >= limit)
 476                 return;
 477
 478         /*
 479          * We (desperately) want to avoid erroneously instrumenting a
 480          * jump table. To determine if we're looking at a true instruction
 481          * or an inline jump table that happens to contain the same
 482          * byte sequences, we resort to some heuristic sleeze:  we
 483          * treat this instruction as being contained within a pointer,
 484          * and see if that pointer points to within the body of the
 485          * function.  If it does, we refuse to instrument it.
 486          */
 487         if (((uintptr_t)instr & 0x3) == 0) {
 488                 machine_inst_t *ptr = *(machine_inst_t **)(void *)instr;
 489
 490                 if (ptr >= (machine_inst_t *)symbolStart && ptr < limit) {
 491                         /* kprintf("dtrace: fbt: Found jump table in %s, at %08x\n",symbolName,(unsigned)instr); */
 492                         instr++;
 493                         goto again;
 494                 }
 495         }
 496
 497         /*
 498          * OK, it's an instruction.
 499          */
 500         theInstr = *instr;
 501
 502         /* Walked onto the start of the next routine? If so, bail out from this function */
 503         if (FBT_IS_THUMB_PUSH_LR(theInstr)) {
 504                 if (!retfbt)
 505                         kprintf("dtrace: fbt: No return probe for %s, walked to next routine at %08x\n",symbolName,(unsigned)instr);
 506                 return;
 507         }
 508
 509         /* The PC relative data should be stored after the end of the function. If
 510          * we see a PC relative load, assume the address to load from is the new end
 511          * of the function. */
 512         if (FBT_IS_THUMB_LDR_PC(theInstr)) {
 513                 uint32_t newlimit = thumb_ldr_pc_address((uint32_t) instr);
 514                 if (newlimit < (uint32_t) limit)
 515                         limit = (machine_inst_t*) newlimit;
 516         }
 517         if ((instr+1) < limit && FBT_IS_THUMB32_LDR_PC(*instr,*(instr+1))) {
 518                 uint32_t newlimit = thumb32_ldr_pc_address((uint32_t) instr);
 519                 if (newlimit < (uint32_t) limit)
 520                         limit = (machine_inst_t*) newlimit;
 521         }
 522
 523         /* Look for the 1. pop { ..., pc } or 2. pop { ..., r7 } ... bx reg or 3. ldmia.w sp!, { ..., r7, lr } ... bx reg */
 524         if (!FBT_IS_THUMB_POP_PC(theInstr) &&
 525             !FBT_IS_THUMB_POP_R7(theInstr) &&
 526             !FBT_IS_THUMB32_POP_R7LR(theInstr,*(instr+1))) {
 527                 instr++;
 528                 if (dtrace_instr_size(theInstr,1) == 4)
 529                         instr++;
 530                 goto again;
 531         }
 532
 533         if (FBT_IS_THUMB_POP_PC(theInstr)) {
 534                 if (savedRegs != FBT_THUMB_STACK_REGS(theInstr)) {
 535                         /* What we're popping doesn't match what we're pushing, assume that we've
 536                          * gone too far in the function. Bail.
 537                          */
 538                         kprintf("dtrace: fbt: No return probe for %s, popped regs don't match at %08x\n",symbolName,(unsigned)instr);
 539                         return;
 540                 }
 541         } else {
 542                 /* Scan ahead for the bx */
 543                 for (j = 0; (j < 4) && (instr < limit); j++, instr++) {
 544                         theInstr = *instr;
 545                         if (FBT_IS_THUMB_BX_REG(theInstr))
 546                                 break;
 547                         if (dtrace_instr_size(theInstr,1) == 4)
 548                                 instr++;
 549                 }
 550
 551                 if (!FBT_IS_THUMB_BX_REG(theInstr))
 552                         return;
 553         }
 554
 555         /*
 556          * pop { ..., pc}, bx reg -- We have a winner!
 557          */
 558
 559         newfbt = kmem_zalloc(sizeof(fbt_probe_t), KM_SLEEP);
 560         newfbt->fbtp_next = NULL;
 561         strlcpy( (char *)&(newfbt->fbtp_name), symbolName, MAX_FBTP_NAME_CHARS );
 562
 563         if (retfbt == NULL) {
 564                 newfbt->fbtp_id = dtrace_probe_create(fbt_id, modname,
 565                     symbolName, FBT_RETURN, FBT_AFRAMES_RETURN, newfbt);
 566         } else {
 567                 retfbt->fbtp_next = newfbt;
 568                 newfbt->fbtp_id = retfbt->fbtp_id;
 569         }
 570
 571         retfbt = newfbt;
 572         newfbt->fbtp_patchpoint = instr;
 573         newfbt->fbtp_ctl = ctl;
 574         newfbt->fbtp_loadcnt = ctl->mod_loadcnt;
 575
 576         ASSERT(FBT_IS_THUMB_POP_PC(theInstr) || FBT_IS_THUMB_BX_REG(theInstr));
 577         newfbt->fbtp_rval = DTRACE_INVOP_POP_PC;
 578         newfbt->fbtp_roffset =
 579             (uintptr_t) ((uint8_t*) instr - (uint8_t *)symbolStart);
 580         newfbt->fbtp_savedval = theInstr;
 581         newfbt->fbtp_patchval = FBT_PATCHVAL;
 582         newfbt->fbtp_currentval = 0;
 583         newfbt->fbtp_hashnext = fbt_probetab[FBT_ADDR2NDX(instr)];
 584         fbt_probetab[FBT_ADDR2NDX(instr)] = newfbt;
 585
 586         if (doenable)
 587                 fbt_enable(NULL, newfbt->fbtp_id, newfbt);
 588
 589         instr++;
 590         goto again;
 591 }
 592