xnu-4903.270.47.tar.gz

[apple/xnu.git] / bsd / dev / arm / fasttrap_isa.c

diff --git a/bsd/dev/arm/fasttrap_isa.c b/bsd/dev/arm/fasttrap_isa.c
index d48b48a71bdbc91e6c5cdb2738912bf04d1eb3cb..c45a952882baa38a2fa6d0db1785f27a3a9c63f6 100644 (file)
--- a/bsd/dev/arm/fasttrap_isa.c
+++ b/bsd/dev/arm/fasttrap_isa.c
@@ -33,7 +33,7 @@
 
 #ifdef KERNEL
 #ifndef _KERNEL
 
 #ifdef KERNEL
 #ifndef _KERNEL

-#define _KERNEL                        /* Solaris vs. Darwin */
+#define _KERNEL                 /* Solaris vs. Darwin */

 #endif
 #endif
 
 #endif
 #endif
 


@@ -88,13 +88,13 @@ extern int dtrace_decode_thumb(uint32_t instr);
 
 #define THUMB_INSTR(x) (*(uint16_t*) &(x))
 
 
 #define THUMB_INSTR(x) (*(uint16_t*) &(x))
 

-#define SIGNEXTEND(x,v) ((((int) (x)) << (32-(v))) >> (32-(v)))
-#define ALIGNADDR(x,v) (((x) >> (v)) << (v))
+#define SIGNEXTEND(x, v) ((((int) (x)) << (32-(v))) >> (32-(v)))
+#define ALIGNADDR(x, v) (((x) >> (v)) << (v))

 #define GETITSTATE(x) ((((x) >> 8) & 0xFC) | (((x) >> 25) & 0x3))
 #define ISLASTINIT(x) (((x) & 0xF) == 8)
 
 #define GETITSTATE(x) ((((x) >> 8) & 0xFC) | (((x) >> 25) & 0x3))
 #define ISLASTINIT(x) (((x) & 0xF) == 8)
 

-#define SET16(x,w) *((uint16_t*) (x)) = (w)
-#define SET32(x,w) *((uint32_t*) (x)) = (w)
+#define SET16(x, w) *((uint16_t*) (x)) = (w)
+#define SET32(x, w) *((uint32_t*) (x)) = (w)

 
 #define IS_ARM_NOP(x) ((x) == 0xE1A00000)
 /* Marker for is-enabled probes */
 
 #define IS_ARM_NOP(x) ((x) == 0xE1A00000)
 /* Marker for is-enabled probes */


@@ -113,19 +113,9 @@ extern int dtrace_decode_thumb(uint32_t instr);
 
 extern int dtrace_arm_condition_true(int cond, int cpsr);
 
 
 extern int dtrace_arm_condition_true(int cond, int cpsr);
 

-static
-void flush_caches(void)
-{
-       /* TODO There were some problems with flushing just the cache line that had been modified.
-        * For now, we'll flush the entire cache, until we figure out how to flush just the patched block.
-        */
-       FlushPoU_Dcache();
-       InvalidatePoU_Icache();
-}
-

 int
 fasttrap_tracepoint_init(proc_t *p, fasttrap_tracepoint_t *tp,
 int
 fasttrap_tracepoint_init(proc_t *p, fasttrap_tracepoint_t *tp,

-                        user_addr_t pc, fasttrap_probe_type_t type)
+    user_addr_t pc, fasttrap_probe_type_t type)

 {
 #pragma unused(type)
        uint32_t instr;
 {
 #pragma unused(type)
        uint32_t instr;


@@ -138,12 +128,13 @@ fasttrap_tracepoint_init(proc_t *p, fasttrap_tracepoint_t *tp,
         * pages, we potentially read the instruction in two parts. If the
         * second part fails, we just zero out that part of the instruction.
         */
         * pages, we potentially read the instruction in two parts. If the
         * second part fails, we just zero out that part of the instruction.
         */

-       /*      
+       /*

         * APPLE NOTE: Of course, we do not have a P_PR_LOCK, so this is racey...
         * APPLE NOTE: Of course, we do not have a P_PR_LOCK, so this is racey...

-        */             
+        */

 
 

-       if (uread(p, &instr, 4, pc) != 0)
-               return (-1);
+       if (uread(p, &instr, 4, pc) != 0) {
+               return -1;
+       }

 
        /* We want &instr to always point to the saved instruction, so just copy the
         * whole thing When cast to a pointer to a uint16_t, that will give us a
 
        /* We want &instr to always point to the saved instruction, so just copy the
         * whole thing When cast to a pointer to a uint16_t, that will give us a


@@ -152,37 +143,37 @@ fasttrap_tracepoint_init(proc_t *p, fasttrap_tracepoint_t *tp,
        tp->ftt_instr = instr;
 
        if (tp->ftt_fntype != FASTTRAP_FN_DONE_INIT) {
        tp->ftt_instr = instr;
 
        if (tp->ftt_fntype != FASTTRAP_FN_DONE_INIT) {

-               switch(tp->ftt_fntype) {
-                       case FASTTRAP_FN_UNKNOWN:
-                               /* Can't instrument without any information. We can add some heuristics later if necessary. */
-                               return (-1);
-
-                       case FASTTRAP_FN_USDT:
-                               if (IS_ARM_NOP(instr) || IS_ARM_IS_ENABLED(instr)) {
-                                       tp->ftt_thumb = 0;
-                               } else if (IS_THUMB_NOP(THUMB_INSTR(instr)) || IS_THUMB_IS_ENABLED(THUMB_INSTR(instr))) {
-                                       tp->ftt_thumb = 1;
-                               } else {
-                                       /* Shouldn't reach here - this means we don't recognize
-                                        * the instruction at one of the USDT probe locations
-                                        */
-                                       return (-1);
-                               }
-                               tp->ftt_fntype = FASTTRAP_FN_DONE_INIT;
-                               break;
+               switch (tp->ftt_fntype) {
+               case FASTTRAP_FN_UNKNOWN:
+                       /* Can't instrument without any information. We can add some heuristics later if necessary. */
+                       return -1;

 
 

-                       case FASTTRAP_FN_ARM:
+               case FASTTRAP_FN_USDT:
+                       if (IS_ARM_NOP(instr) || IS_ARM_IS_ENABLED(instr)) {

                                tp->ftt_thumb = 0;
                                tp->ftt_thumb = 0;

-                               tp->ftt_fntype = FASTTRAP_FN_DONE_INIT;
-                               break;
-
-                       case FASTTRAP_FN_THUMB:
+                       } else if (IS_THUMB_NOP(THUMB_INSTR(instr)) || IS_THUMB_IS_ENABLED(THUMB_INSTR(instr))) {

                                tp->ftt_thumb = 1;
                                tp->ftt_thumb = 1;

-                               tp->ftt_fntype = FASTTRAP_FN_DONE_INIT;
-                               break;
+                       } else {
+                               /* Shouldn't reach here - this means we don't recognize
+                                * the instruction at one of the USDT probe locations
+                                */
+                               return -1;
+                       }
+                       tp->ftt_fntype = FASTTRAP_FN_DONE_INIT;
+                       break;

 
 

-                       default:
-                               return (-1);
+               case FASTTRAP_FN_ARM:
+                       tp->ftt_thumb = 0;
+                       tp->ftt_fntype = FASTTRAP_FN_DONE_INIT;
+                       break;
+
+               case FASTTRAP_FN_THUMB:
+                       tp->ftt_thumb = 1;
+                       tp->ftt_fntype = FASTTRAP_FN_DONE_INIT;
+                       break;
+
+               default:
+                       return -1;

                }
        }
 
                }
        }
 


@@ -195,95 +186,11 @@ fasttrap_tracepoint_init(proc_t *p, fasttrap_tracepoint_t *tp,
        if (tp->ftt_type == FASTTRAP_T_INV) {
                /* This is an instruction we either don't recognize or can't instrument */
                printf("dtrace: fasttrap: Unrecognized instruction: %08x at %08x\n",
        if (tp->ftt_type == FASTTRAP_T_INV) {
                /* This is an instruction we either don't recognize or can't instrument */
                printf("dtrace: fasttrap: Unrecognized instruction: %08x at %08x\n",

-                       (tp->ftt_thumb && dtrace_instr_size(tp->ftt_instr,tp->ftt_thumb) == 2) ? tp->ftt_instr1 : instr, pc);
-               return (-1);
+                   (tp->ftt_thumb && dtrace_instr_size(tp->ftt_instr, tp->ftt_thumb) == 2) ? tp->ftt_instr1 : instr, pc);
+               return -1;

        }
 
        }
 

-       return (0);
-}
-
-// These are not exported from vm_map.h.
-extern kern_return_t vm_map_write_user(vm_map_t map, void *src_p, vm_map_address_t dst_addr, vm_size_t size);
-
-/* Patches the instructions. Almost like uwrite, but need special instructions on ARM to flush the caches. */
-static
-int patchInst(proc_t *p, void *buf, user_size_t len, user_addr_t a)
-{
-       kern_return_t ret;
-
-       ASSERT(p != NULL);
-       ASSERT(p->task != NULL);
-
-       task_t task = p->task;
-
-       /*
-        * Grab a reference to the task vm_map_t to make sure
-        * the map isn't pulled out from under us.
-        *
-        * Because the proc_lock is not held at all times on all code
-        * paths leading here, it is possible for the proc to have
-        * exited. If the map is null, fail.
-        */
-       vm_map_t map = get_task_map_reference(task);
-       if (map) {
-               /* Find the memory permissions. */
-               uint32_t nestingDepth=999999;
-               vm_region_submap_short_info_data_64_t info;
-               mach_msg_type_number_t count = VM_REGION_SUBMAP_SHORT_INFO_COUNT_64;
-               mach_vm_address_t address = (mach_vm_address_t)a;
-               mach_vm_size_t sizeOfRegion = (mach_vm_size_t)len;
-
-               ret = mach_vm_region_recurse(map, &address, &sizeOfRegion, &nestingDepth, (vm_region_recurse_info_t)&info, &count);
-               if (ret != KERN_SUCCESS)
-                       goto done;
-
-               vm_prot_t reprotect;
-
-               if (!(info.protection & VM_PROT_WRITE)) {
-                       /* Save the original protection values for restoration later */
-                       reprotect = info.protection;
-                       if (info.max_protection & VM_PROT_WRITE) {
-                               /* The memory is not currently writable, but can be made writable. */
-                               /* Making it both writable and executable at the same time causes warning on embedded */
-                               ret = mach_vm_protect (map, (mach_vm_offset_t)a, (mach_vm_size_t)len, 0, (reprotect & ~VM_PROT_EXECUTE) | VM_PROT_WRITE);
-                       } else {
-                               /*
-                                * The memory is not currently writable, and cannot be made writable. We need to COW this memory.
-                                *
-                                * Strange, we can't just say "reprotect | VM_PROT_COPY", that fails.
-                                */
-                               ret = mach_vm_protect (map, (mach_vm_offset_t)a, (mach_vm_size_t)len, 0, VM_PROT_COPY | VM_PROT_READ | VM_PROT_WRITE);
-                       }
-
-                       if (ret != KERN_SUCCESS)
-                               goto done;
-
-               } else {
-                       /* The memory was already writable. */
-                       reprotect = VM_PROT_NONE;
-               }
-
-               ret = vm_map_write_user( map,
-                                        buf,
-                                        (vm_map_address_t)a,
-                                        (vm_size_t)len);
-
-               flush_caches();
-
-               if (ret != KERN_SUCCESS)
-                       goto done;
-
-               if (reprotect != VM_PROT_NONE) {
-                       ASSERT(reprotect & VM_PROT_EXECUTE);
-                       ret = mach_vm_protect (map, (mach_vm_offset_t)a, (mach_vm_size_t)len, 0, reprotect);
-               }
-
-done:
-               vm_map_deallocate(map);
-       } else
-               ret = KERN_TERMINATED;
-
-       return (int)ret;
+       return 0;

 }
 
 int
 }
 
 int


@@ -299,12 +206,13 @@ fasttrap_tracepoint_install(proc_t *p, fasttrap_tracepoint_t *tp)
                instr = FASTTRAP_ARM_INSTR;
        }
 
                instr = FASTTRAP_ARM_INSTR;
        }
 

-       if (patchInst(p, &instr, size, tp->ftt_pc) != 0)
-               return (-1);
+       if (uwrite(p, &instr, size, tp->ftt_pc) != 0) {
+               return -1;
+       }

 
        tp->ftt_installed = 1;
 
 
        tp->ftt_installed = 1;
 

-       return (0);
+       return 0;

 }
 
 int
 }
 
 int


@@ -318,22 +226,26 @@ fasttrap_tracepoint_remove(proc_t *p, fasttrap_tracepoint_t *tp)
         * Distinguish between read or write failures and a changed
         * instruction.
         */
         * Distinguish between read or write failures and a changed
         * instruction.
         */

-       if (uread(p, &instr, size, tp->ftt_pc) != 0)
+       if (uread(p, &instr, size, tp->ftt_pc) != 0) {

                goto end;
                goto end;

+       }

        if (tp->ftt_thumb) {
        if (tp->ftt_thumb) {

-               if (*((uint16_t*) &instr) != FASTTRAP_THUMB_INSTR)
+               if (*((uint16_t*) &instr) != FASTTRAP_THUMB_INSTR) {

                        goto end;
                        goto end;

+               }

        } else {
        } else {

-               if (instr != FASTTRAP_ARM_INSTR)
+               if (instr != FASTTRAP_ARM_INSTR) {

                        goto end;
                        goto end;

+               }
+       }
+       if (uwrite(p, &tp->ftt_instr, size, tp->ftt_pc) != 0) {
+               return -1;

        }
        }

-       if (patchInst(p, &tp->ftt_instr, size, tp->ftt_pc) != 0)
-               return (-1);

 
 end:
        tp->ftt_installed = 0;
 
 
 end:
        tp->ftt_installed = 0;
 

-       return (0);
+       return 0;

 }
 
 static void
 }
 
 static void


@@ -352,15 +264,16 @@ fasttrap_return_common(proc_t *p, arm_saved_state_t *regs, user_addr_t pc, user_
 
        for (tp = bucket->ftb_data; tp != NULL; tp = tp->ftt_next) {
                if (pid == tp->ftt_pid && pc == tp->ftt_pc &&
 
        for (tp = bucket->ftb_data; tp != NULL; tp = tp->ftt_next) {
                if (pid == tp->ftt_pid && pc == tp->ftt_pc &&

-               tp->ftt_proc->ftpc_acount != 0)
+                   tp->ftt_proc->ftpc_acount != 0) {

                        break;
                        break;

+               }

        }
 
        /*
         * Don't sweat it if we can't find the tracepoint again; unlike
         * when we're in fasttrap_pid_probe(), finding the tracepoint here
         * is not essential to the correct execution of the process.
        }
 
        /*
         * Don't sweat it if we can't find the tracepoint again; unlike
         * when we're in fasttrap_pid_probe(), finding the tracepoint here
         * is not essential to the correct execution of the process.

-        */
+        */

        if (tp == NULL) {
                lck_mtx_unlock(pid_mtx);
                return;
        if (tp == NULL) {
                lck_mtx_unlock(pid_mtx);
                return;


@@ -375,8 +288,9 @@ fasttrap_return_common(proc_t *p, arm_saved_state_t *regs, user_addr_t pc, user_
                 */
                if (tp->ftt_type != FASTTRAP_T_LDM_PC &&
                    tp->ftt_type != FASTTRAP_T_POP_PC &&
                 */
                if (tp->ftt_type != FASTTRAP_T_LDM_PC &&
                    tp->ftt_type != FASTTRAP_T_POP_PC &&

-                   new_pc - probe->ftp_faddr < probe->ftp_fsize)
+                   new_pc - probe->ftp_faddr < probe->ftp_fsize) {

                        continue;
                        continue;

+               }

 
                if (probe->ftp_prov->ftp_provider_type == DTFTP_PROVIDER_ONESHOT) {
                        uint8_t already_triggered = atomic_or_8(&probe->ftp_triggered, 1);
 
                if (probe->ftp_prov->ftp_provider_type == DTFTP_PROVIDER_ONESHOT) {
                        uint8_t already_triggered = atomic_or_8(&probe->ftp_triggered, 1);


@@ -395,14 +309,14 @@ fasttrap_return_common(proc_t *p, arm_saved_state_t *regs, user_addr_t pc, user_
 #ifndef CONFIG_EMBEDDED
                if (ISSET(current_proc()->p_lflag, P_LNOATTACH)) {
                        dtrace_probe(dtrace_probeid_error, 0 /* state */, id->fti_probe->ftp_id,
 #ifndef CONFIG_EMBEDDED
                if (ISSET(current_proc()->p_lflag, P_LNOATTACH)) {
                        dtrace_probe(dtrace_probeid_error, 0 /* state */, id->fti_probe->ftp_id,

-                                    1 /* ndx */, -1 /* offset */, DTRACEFLT_UPRIV);
+                           1 /* ndx */, -1 /* offset */, DTRACEFLT_UPRIV);

 #else
                if (FALSE) {
 #endif
                } else {
                        dtrace_probe(id->fti_probe->ftp_id,
 #else
                if (FALSE) {
 #endif
                } else {
                        dtrace_probe(id->fti_probe->ftp_id,

-                                    pc - id->fti_probe->ftp_faddr,
-                                    regs->r[0], 0, 0, 0);
+                           pc - id->fti_probe->ftp_faddr,
+                           regs->r[0], 0, 0, 0);

                }
        }
        if (retire_tp) {
                }
        }
        if (retire_tp) {


@@ -434,16 +348,16 @@ fasttrap_sigsegv(proc_t *p, uthread_t t, user_addr_t addr, arm_saved_state_t *re
        t->uu_code = addr;
        t->uu_siglist |= sigmask(SIGSEGV);
 
        t->uu_code = addr;
        t->uu_siglist |= sigmask(SIGSEGV);
 

-       /* 
+       /*

         * XXX These two line may be redundant; if not, then we need
         * XXX to potentially set the data address in the machine
         * XXX specific thread state structure to indicate the address.
         * XXX These two line may be redundant; if not, then we need
         * XXX to potentially set the data address in the machine
         * XXX specific thread state structure to indicate the address.

-        */         
+        */

        t->uu_exception = KERN_INVALID_ADDRESS;         /* SIGSEGV */
        t->uu_subcode = 0;      /* XXX pad */
        t->uu_exception = KERN_INVALID_ADDRESS;         /* SIGSEGV */
        t->uu_subcode = 0;      /* XXX pad */

-                
-       proc_unlock(p); 
-                                     
+
+       proc_unlock(p);
+

        /* raise signal */
        signal_setast(t->uu_context.vc_thread);
 #endif
        /* raise signal */
        signal_setast(t->uu_context.vc_thread);
 #endif


@@ -470,7 +384,8 @@ fasttrap_usdt_args(fasttrap_probe_t *probe, arm_saved_state_t *regs, int argc,
        }
 }
 
        }
 }
 

-static void set_thumb_flag(arm_saved_state_t *regs, user_addr_t pc)
+static void
+set_thumb_flag(arm_saved_state_t *regs, user_addr_t pc)

 {
        if (pc & 1) {
                regs->cpsr |= PSR_TF;
 {
        if (pc & 1) {
                regs->cpsr |= PSR_TF;


@@ -506,7 +421,7 @@ fasttrap_pid_probe(arm_saved_state_t *regs)
        if (uthread->t_dtrace_step) {
                ASSERT(uthread->t_dtrace_on);
                fasttrap_sigtrap(p, uthread, pc);
        if (uthread->t_dtrace_step) {
                ASSERT(uthread->t_dtrace_on);
                fasttrap_sigtrap(p, uthread, pc);

-               return (0);
+               return 0;

        }
 
        /*
        }
 
        /*


@@ -525,23 +440,25 @@ fasttrap_pid_probe(arm_saved_state_t *regs)
         */
        if (p->p_lflag & P_LINVFORK) {
                proc_list_lock();
         */
        if (p->p_lflag & P_LINVFORK) {
                proc_list_lock();

-               while (p->p_lflag & P_LINVFORK)
+               while (p->p_lflag & P_LINVFORK) {

                        p = p->p_pptr;
                        p = p->p_pptr;

+               }

                proc_list_unlock();
        }
 
        pid = p->p_pid;
        pid_mtx = &cpu_core[CPU->cpu_id].cpuc_pid_lock;
        lck_mtx_lock(pid_mtx);
                proc_list_unlock();
        }
 
        pid = p->p_pid;
        pid_mtx = &cpu_core[CPU->cpu_id].cpuc_pid_lock;
        lck_mtx_lock(pid_mtx);

-       bucket = &fasttrap_tpoints.fth_table[FASTTRAP_TPOINTS_INDEX(pid,pc)];
+       bucket = &fasttrap_tpoints.fth_table[FASTTRAP_TPOINTS_INDEX(pid, pc)];

 
        /*
         * Lookup the tracepoint that the process just hit.
         */
        for (tp = bucket->ftb_data; tp != NULL; tp = tp->ftt_next) {
                if (pid == tp->ftt_pid && pc == tp->ftt_pc &&
 
        /*
         * Lookup the tracepoint that the process just hit.
         */
        for (tp = bucket->ftb_data; tp != NULL; tp = tp->ftt_next) {
                if (pid == tp->ftt_pid && pc == tp->ftt_pc &&

-                   tp->ftt_proc->ftpc_acount != 0)
+                   tp->ftt_proc->ftpc_acount != 0) {

                        break;
                        break;

+               }

        }
 
        /*
        }
 
        /*


@@ -551,7 +468,7 @@ fasttrap_pid_probe(arm_saved_state_t *regs)
         */
        if (tp == NULL) {
                lck_mtx_unlock(pid_mtx);
         */
        if (tp == NULL) {
                lck_mtx_unlock(pid_mtx);

-               return (-1);
+               return -1;

        }
 
        /* Default to always execute */
        }
 
        /* Default to always execute */


@@ -564,11 +481,11 @@ fasttrap_pid_probe(arm_saved_state_t *regs)
                                condition_code = itstate >> 4;
                        } else {
                                printf("dtrace: fasttrap: Tried to trace instruction %08x at %08x but not at end of IT block\n",
                                condition_code = itstate >> 4;
                        } else {
                                printf("dtrace: fasttrap: Tried to trace instruction %08x at %08x but not at end of IT block\n",

-                                   (tp->ftt_thumb && dtrace_instr_size(tp->ftt_instr,tp->ftt_thumb) == 2) ? tp->ftt_instr1 : tp->ftt_instr, pc);
+                                   (tp->ftt_thumb && dtrace_instr_size(tp->ftt_instr, tp->ftt_thumb) == 2) ? tp->ftt_instr1 : tp->ftt_instr, pc);

 
                                fasttrap_tracepoint_remove(p, tp);
                                lck_mtx_unlock(pid_mtx);
 
                                fasttrap_tracepoint_remove(p, tp);
                                lck_mtx_unlock(pid_mtx);

-                               return (-1);
+                               return -1;

                        }
                }
        } else {
                        }
                }
        } else {


@@ -581,7 +498,7 @@ fasttrap_pid_probe(arm_saved_state_t *regs)
                 */
                fasttrap_tracepoint_remove(p, tp);
                lck_mtx_unlock(pid_mtx);
                 */
                fasttrap_tracepoint_remove(p, tp);
                lck_mtx_unlock(pid_mtx);

-               return (-1);
+               return -1;

        }
 
        if (tp->ftt_ids != NULL) {
        }
 
        if (tp->ftt_ids != NULL) {


@@ -599,7 +516,7 @@ fasttrap_pid_probe(arm_saved_state_t *regs)
 #ifndef CONFIG_EMBEDDED
                        if (ISSET(current_proc()->p_lflag, P_LNOATTACH)) {
                                dtrace_probe(dtrace_probeid_error, 0 /* state */, probe->ftp_id,
 #ifndef CONFIG_EMBEDDED
                        if (ISSET(current_proc()->p_lflag, P_LNOATTACH)) {
                                dtrace_probe(dtrace_probeid_error, 0 /* state */, probe->ftp_id,

-                                            1 /* ndx */, -1 /* offset */, DTRACEFLT_UPRIV);
+                                   1 /* ndx */, -1 /* offset */, DTRACEFLT_UPRIV);

 #else
                        if (FALSE) {
 #endif
 #else
                        if (FALSE) {
 #endif


@@ -686,498 +603,498 @@ fasttrap_pid_probe(arm_saved_state_t *regs)
                goto done;
        }
 
                goto done;
        }
 

-       instr_size = dtrace_instr_size(tp->ftt_instr,tp->ftt_thumb);
+       instr_size = dtrace_instr_size(tp->ftt_instr, tp->ftt_thumb);

 
        switch (tp->ftt_type) {
 
        switch (tp->ftt_type) {

-               case FASTTRAP_T_MOV_PC_REG:
-               case FASTTRAP_T_CPY_PC:
-               {
-                       if (!dtrace_arm_condition_true(condition_code, regs->cpsr)) {
-                               new_pc = pc + instr_size;
-                               break;
-                       }
+       case FASTTRAP_T_MOV_PC_REG:
+       case FASTTRAP_T_CPY_PC:
+       {
+               if (!dtrace_arm_condition_true(condition_code, regs->cpsr)) {
+                       new_pc = pc + instr_size;
+                       break;
+               }

 
 

-                       int rm;
-                       if (tp->ftt_thumb) {
-                               rm = THUMB16_HRM(tp->ftt_instr1);
-                       } else {
-                               rm = tp->ftt_instr & 0xF;
-                       }
-                       new_pc = regs->r[rm];
+               int rm;
+               if (tp->ftt_thumb) {
+                       rm = THUMB16_HRM(tp->ftt_instr1);
+               } else {
+                       rm = tp->ftt_instr & 0xF;
+               }
+               new_pc = regs->r[rm];

 
 

-                       /* This instruction does not change the Thumb state */
+               /* This instruction does not change the Thumb state */

 
 

-                       break;
-               }
+               break;
+       }

 
 

-               case FASTTRAP_T_STM_LR:
-               case FASTTRAP_T_PUSH_LR:
-               {
-                       /*
-                        * This is a very common case, so we want to emulate this instruction if
-                        * possible. However, on a push, it is possible that we might reach the end
-                        * of a page and have to allocate a new page. Most of the time this will not
-                        * happen, and we know that the push instruction can store at most 16 words,
-                        * so check to see if we are far from the boundary, and if so, emulate. This
-                        * can be made more aggressive by checking the actual number of words being
-                        * pushed, but we won't do that for now.
-                        *
-                        * Some of the same issues that apply to POP_PC probably apply here also.
-                        */
+       case FASTTRAP_T_STM_LR:
+       case FASTTRAP_T_PUSH_LR:
+       {
+               /*
+                * This is a very common case, so we want to emulate this instruction if
+                * possible. However, on a push, it is possible that we might reach the end
+                * of a page and have to allocate a new page. Most of the time this will not
+                * happen, and we know that the push instruction can store at most 16 words,
+                * so check to see if we are far from the boundary, and if so, emulate. This
+                * can be made more aggressive by checking the actual number of words being
+                * pushed, but we won't do that for now.
+                *
+                * Some of the same issues that apply to POP_PC probably apply here also.
+                */

 
 

-                       int reglist;
-                       int ret;
-                       uintptr_t* base;
+               int reglist;
+               int ret;
+               uintptr_t* base;

 
 

-                       if (!dtrace_arm_condition_true(condition_code, regs->cpsr)) {
-                               new_pc = pc + instr_size;
-                               break;
-                       }
+               if (!dtrace_arm_condition_true(condition_code, regs->cpsr)) {
+                       new_pc = pc + instr_size;
+                       break;
+               }

 
 

-                       base = (uintptr_t*) regs->sp;
-                       if (((((uintptr_t) base)-16*4) >> PAGE_SHIFT) != (((uintptr_t) base) >> PAGE_SHIFT)) {
-                               /* Crosses the page boundary, go to emulation */
-                               goto instr_emulate;
-                       }
+               base = (uintptr_t*) regs->sp;
+               if (((((uintptr_t) base) - 16 * 4) >> PAGE_SHIFT) != (((uintptr_t) base) >> PAGE_SHIFT)) {
+                       /* Crosses the page boundary, go to emulation */
+                       goto instr_emulate;
+               }

 
 

-                       if (tp->ftt_thumb) {
-                               if (instr_size == 4) {
-                                       /* We know we have to push lr, never push sp or pc */
-                                       reglist = tp->ftt_instr2 & 0x1FFF;
-                               } else {
-                                       reglist = tp->ftt_instr1 & 0xFF;
-                               }
-                       } else {
+               if (tp->ftt_thumb) {
+                       if (instr_size == 4) {

                                /* We know we have to push lr, never push sp or pc */
                                /* We know we have to push lr, never push sp or pc */

-                               reglist = tp->ftt_instr & 0x1FFF;
+                               reglist = tp->ftt_instr2 & 0x1FFF;
+                       } else {
+                               reglist = tp->ftt_instr1 & 0xFF;

                        }
                        }

+               } else {
+                       /* We know we have to push lr, never push sp or pc */
+                       reglist = tp->ftt_instr & 0x1FFF;
+               }

 
 

-                       /* Push the link register */
-                       base--;
-                       ret = fasttrap_suword32((uint32_t) base, regs->lr);
-                       if (ret == -1) {
-                               fasttrap_sigsegv(p, uthread, (user_addr_t) base, regs);
-                               new_pc = regs->pc;
-                               break;
-                       }
+               /* Push the link register */
+               base--;
+               ret = fasttrap_suword32((uint32_t) base, regs->lr);
+               if (ret == -1) {
+                       fasttrap_sigsegv(p, uthread, (user_addr_t) base, regs);
+                       new_pc = regs->pc;
+                       break;
+               }

 
 

-                       /* Start pushing from $r12 */
-                       int regmask = 1 << 12;
-                       int regnum = 12;
-
-                       while (regmask) {
-                               if (reglist & regmask) {
-                                       base--;
-                                       ret = fasttrap_suword32((uint32_t) base, regs->r[regnum]);
-                                       if (ret == -1) {
-                                               fasttrap_sigsegv(p, uthread, (user_addr_t) base, regs);
-                                               new_pc = regs->pc;
-                                               break;
-                                       }
+               /* Start pushing from $r12 */
+               int regmask = 1 << 12;
+               int regnum = 12;
+
+               while (regmask) {
+                       if (reglist & regmask) {
+                               base--;
+                               ret = fasttrap_suword32((uint32_t) base, regs->r[regnum]);
+                               if (ret == -1) {
+                                       fasttrap_sigsegv(p, uthread, (user_addr_t) base, regs);
+                                       new_pc = regs->pc;
+                                       break;

                                }
                                }

-                               regmask >>= 1;
-                               regnum--;

                        }
                        }

-
-                       regs->sp = (uintptr_t) base;
-
-                       new_pc = pc + instr_size;
-
-                       break;
+                       regmask >>= 1;
+                       regnum--;

                }
 
                }
 

+               regs->sp = (uintptr_t) base;

 
 

-               case FASTTRAP_T_LDM_PC:
-               case FASTTRAP_T_POP_PC:
-               {
-                       /* TODO Two issues that will eventually need to be resolved:
-                        *
-                        * 1. Understand what the hardware does if we have to segfault (data abort) in
-                        * the middle of a load multiple. We currently don't have a working segfault
-                        * handler anyway, and with no swapfile we should never segfault on this load.
-                        * If we do, we'll just kill the process by setting the pc to 0.
-                        *
-                        * 2. The emulation is no longer atomic. We currently only emulate pop for
-                        * function epilogues, and so we should never have a race here because one
-                        * thread should never be trying to manipulate another thread's stack frames.
-                        * That is almost certainly a bug in the program.
-                        * 
-                        * This will need to be fixed if we ever:
-                        *   a. Ship dtrace externally, as this could be a potential attack vector
-                        *   b. Support instruction level tracing, as we might then pop/ldm non epilogues.
-                        *
-                        */
-
-                       /* Assume ldmia! sp/pop ... pc */
-
-                       int regnum = 0, reglist;
-                       int ret;
-                       uintptr_t* base;
-
-                       if (!dtrace_arm_condition_true(condition_code, regs->cpsr)) {
-                               new_pc = pc + instr_size;
-                               break;
-                       }
+               new_pc = pc + instr_size;

 
 

-                       if (tp->ftt_thumb) {
-                               if (instr_size == 4) {
-                                       /* We know we have to load the pc, don't do it twice */
-                                       reglist = tp->ftt_instr2 & 0x7FFF;
-                               } else {
-                                       reglist = tp->ftt_instr1 & 0xFF;
-                               }
-                       } else {
-                               /* We know we have to load the pc, don't do it twice */
-                               reglist = tp->ftt_instr & 0x7FFF;
-                       }
+               break;
+       }

 
 

-                       base = (uintptr_t*) regs->sp;
-                       while (reglist) {
-                               if (reglist & 1) {
-                                       ret = fasttrap_fuword32((uint32_t) base, &regs->r[regnum]);
-                                       if (ret == -1) {
-                                               fasttrap_sigsegv(p, uthread, (user_addr_t) base, regs);
-                                               new_pc = regs->pc;
-                                               break;
-                                       }
-                                       base++;
-                               }
-                               reglist >>= 1;
-                               regnum++;
-                       }

 
 

-                       ret = fasttrap_fuword32((uint32_t) base, &new_pc);
-                       if (ret == -1) {
-                               fasttrap_sigsegv(p, uthread, (user_addr_t) base, regs);
-                               new_pc = regs->pc;
-                               break;
-                       }
-                       base++;
+       case FASTTRAP_T_LDM_PC:
+       case FASTTRAP_T_POP_PC:
+       {
+               /* TODO Two issues that will eventually need to be resolved:
+                *
+                * 1. Understand what the hardware does if we have to segfault (data abort) in
+                * the middle of a load multiple. We currently don't have a working segfault
+                * handler anyway, and with no swapfile we should never segfault on this load.
+                * If we do, we'll just kill the process by setting the pc to 0.
+                *
+                * 2. The emulation is no longer atomic. We currently only emulate pop for
+                * function epilogues, and so we should never have a race here because one
+                * thread should never be trying to manipulate another thread's stack frames.
+                * That is almost certainly a bug in the program.
+                *
+                * This will need to be fixed if we ever:
+                *   a. Ship dtrace externally, as this could be a potential attack vector
+                *   b. Support instruction level tracing, as we might then pop/ldm non epilogues.
+                *
+                */

 
 

-                       regs->sp = (uintptr_t) base;
+               /* Assume ldmia! sp/pop ... pc */

 
 

-                       set_thumb_flag(regs, new_pc);
+               int regnum = 0, reglist;
+               int ret;
+               uintptr_t* base;

 
 

+               if (!dtrace_arm_condition_true(condition_code, regs->cpsr)) {
+                       new_pc = pc + instr_size;

                        break;
                }
 
                        break;
                }
 

-               case FASTTRAP_T_CB_N_Z:
-               {
-                       /* Thumb mode instruction, and not permitted in IT block, so skip the condition code check */
-                       int rn = tp->ftt_instr1 & 0x7;
-                       int offset = (((tp->ftt_instr1 & 0x00F8) >> 2) | ((tp->ftt_instr1 & 0x0200) >> 3)) + 4;
-                       int nonzero = tp->ftt_instr1 & 0x0800;
-                       if (!nonzero != !(regs->r[rn] == 0)) {
-                               new_pc = pc + offset;
+               if (tp->ftt_thumb) {
+                       if (instr_size == 4) {
+                               /* We know we have to load the pc, don't do it twice */
+                               reglist = tp->ftt_instr2 & 0x7FFF;

                        } else {
                        } else {

-                               new_pc = pc + instr_size;
+                               reglist = tp->ftt_instr1 & 0xFF;

                        }
                        }

-                       break;
+               } else {
+                       /* We know we have to load the pc, don't do it twice */
+                       reglist = tp->ftt_instr & 0x7FFF;

                }
 
                }
 

-               case FASTTRAP_T_B_COND:
-               {
-                       /* Use the condition code in the instruction and ignore the ITSTATE */
-
-                       int code, offset;
-                       if (tp->ftt_thumb) {
-                               if (instr_size == 4) {
-                                       code = (tp->ftt_instr1 >> 6) & 0xF;
-                                       if (code == 14 || code == 15) {
-                                               panic("fasttrap: Emulation of invalid branch");
-                                       }
-                                       int S = (tp->ftt_instr1 >> 10) & 1,
-                                           J1 = (tp->ftt_instr2 >> 13) & 1,
-                                           J2 = (tp->ftt_instr2 >> 11) & 1;
-                                       offset = 4 + SIGNEXTEND(
-                                           (S << 20) | (J2 << 19) | (J1 << 18) |
-                                           ((tp->ftt_instr1 & 0x003F) << 12) |
-                                           ((tp->ftt_instr2 & 0x07FF) << 1),
-                                           21);
-                               } else {
-                                       code = (tp->ftt_instr1 >> 8) & 0xF;
-                                       if (code == 14 || code == 15) {
-                                               panic("fasttrap: Emulation of invalid branch");
-                                       }
-                                       offset = 4 + (SIGNEXTEND(tp->ftt_instr1 & 0xFF, 8) << 1);
-                               }
-                       } else {
-                               code = ARM_CONDCODE(tp->ftt_instr);
-                               if (code == 15) {
-                                       panic("fasttrap: Emulation of invalid branch");
+               base = (uintptr_t*) regs->sp;
+               while (reglist) {
+                       if (reglist & 1) {
+                               ret = fasttrap_fuword32((uint32_t) base, &regs->r[regnum]);
+                               if (ret == -1) {
+                                       fasttrap_sigsegv(p, uthread, (user_addr_t) base, regs);
+                                       new_pc = regs->pc;
+                                       break;

                                }
                                }

-                               offset = 8 + (SIGNEXTEND(tp->ftt_instr & 0x00FFFFFF, 24) << 2);
-                       }
-
-                       if (dtrace_arm_condition_true(code, regs->cpsr)) {
-                               new_pc = pc + offset;
-                       } else {
-                               new_pc = pc + instr_size;
+                               base++;

                        }
                        }

+                       reglist >>= 1;
+                       regnum++;
+               }

 
 

+               ret = fasttrap_fuword32((uint32_t) base, &new_pc);
+               if (ret == -1) {
+                       fasttrap_sigsegv(p, uthread, (user_addr_t) base, regs);
+                       new_pc = regs->pc;

                        break;
                }
                        break;
                }

+               base++;

 
 

-               case FASTTRAP_T_B_UNCOND:
-               {
-                       int offset;
+               regs->sp = (uintptr_t) base;

 
 

-                       /* Unconditional branches can only be taken from Thumb mode */
-                       /* (This is different from an ARM branch with condition code "always") */
-                       ASSERT(tp->ftt_thumb == 1);
+               set_thumb_flag(regs, new_pc);

 
 

-                       if (!dtrace_arm_condition_true(condition_code, regs->cpsr)) {
-                               new_pc = pc + instr_size;
-                               break;
-                       }
+               break;
+       }

 
 

+       case FASTTRAP_T_CB_N_Z:
+       {
+               /* Thumb mode instruction, and not permitted in IT block, so skip the condition code check */
+               int rn = tp->ftt_instr1 & 0x7;
+               int offset = (((tp->ftt_instr1 & 0x00F8) >> 2) | ((tp->ftt_instr1 & 0x0200) >> 3)) + 4;
+               int nonzero = tp->ftt_instr1 & 0x0800;
+               if (!nonzero != !(regs->r[rn] == 0)) {
+                       new_pc = pc + offset;
+               } else {
+                       new_pc = pc + instr_size;
+               }
+               break;
+       }
+
+       case FASTTRAP_T_B_COND:
+       {
+               /* Use the condition code in the instruction and ignore the ITSTATE */
+
+               int code, offset;
+               if (tp->ftt_thumb) {

                        if (instr_size == 4) {
                        if (instr_size == 4) {

+                               code = (tp->ftt_instr1 >> 6) & 0xF;
+                               if (code == 14 || code == 15) {
+                                       panic("fasttrap: Emulation of invalid branch");
+                               }

                                int S = (tp->ftt_instr1 >> 10) & 1,
                                    J1 = (tp->ftt_instr2 >> 13) & 1,
                                    J2 = (tp->ftt_instr2 >> 11) & 1;
                                int S = (tp->ftt_instr1 >> 10) & 1,
                                    J1 = (tp->ftt_instr2 >> 13) & 1,
                                    J2 = (tp->ftt_instr2 >> 11) & 1;

-                               int I1 = (J1 != S) ? 0 : 1, I2 = (J2 != S) ? 0 : 1;

                                offset = 4 + SIGNEXTEND(
                                offset = 4 + SIGNEXTEND(

-                                   (S << 24) | (I1 << 23) | (I2 << 22) |
-                                   ((tp->ftt_instr1 & 0x03FF) << 12) |
-                                   ((tp->ftt_instr2 & 0x07FF) << 1),
-                                   25);
+                                       (S << 20) | (J2 << 19) | (J1 << 18) |
+                                       ((tp->ftt_instr1 & 0x003F) << 12) |
+                                       ((tp->ftt_instr2 & 0x07FF) << 1),
+                                       21);

                        } else {
                        } else {

-                               uint32_t instr1 = tp->ftt_instr1;
-                               offset = 4 + (SIGNEXTEND(instr1 & 0x7FF, 11) << 1);
+                               code = (tp->ftt_instr1 >> 8) & 0xF;
+                               if (code == 14 || code == 15) {
+                                       panic("fasttrap: Emulation of invalid branch");
+                               }
+                               offset = 4 + (SIGNEXTEND(tp->ftt_instr1 & 0xFF, 8) << 1);
+                       }
+               } else {
+                       code = ARM_CONDCODE(tp->ftt_instr);
+                       if (code == 15) {
+                               panic("fasttrap: Emulation of invalid branch");

                        }
                        }

+                       offset = 8 + (SIGNEXTEND(tp->ftt_instr & 0x00FFFFFF, 24) << 2);
+               }

 
 

+               if (dtrace_arm_condition_true(code, regs->cpsr)) {

                        new_pc = pc + offset;
                        new_pc = pc + offset;

+               } else {
+                       new_pc = pc + instr_size;
+               }

 
 

+               break;
+       }
+
+       case FASTTRAP_T_B_UNCOND:
+       {
+               int offset;
+
+               /* Unconditional branches can only be taken from Thumb mode */
+               /* (This is different from an ARM branch with condition code "always") */
+               ASSERT(tp->ftt_thumb == 1);
+
+               if (!dtrace_arm_condition_true(condition_code, regs->cpsr)) {
+                       new_pc = pc + instr_size;

                        break;
                }
 
                        break;
                }
 

-               case FASTTRAP_T_BX_REG:
-               {
-                       int reg;
+               if (instr_size == 4) {
+                       int S = (tp->ftt_instr1 >> 10) & 1,
+                           J1 = (tp->ftt_instr2 >> 13) & 1,
+                           J2 = (tp->ftt_instr2 >> 11) & 1;
+                       int I1 = (J1 != S) ? 0 : 1, I2 = (J2 != S) ? 0 : 1;
+                       offset = 4 + SIGNEXTEND(
+                               (S << 24) | (I1 << 23) | (I2 << 22) |
+                               ((tp->ftt_instr1 & 0x03FF) << 12) |
+                               ((tp->ftt_instr2 & 0x07FF) << 1),
+                               25);
+               } else {
+                       uint32_t instr1 = tp->ftt_instr1;
+                       offset = 4 + (SIGNEXTEND(instr1 & 0x7FF, 11) << 1);
+               }

 
 

-                       if (!dtrace_arm_condition_true(condition_code, regs->cpsr)) {
-                               new_pc = pc + instr_size;
-                               break;
-                       }
+               new_pc = pc + offset;

 
 

-                       if (tp->ftt_thumb) {
-                               reg = THUMB16_HRM(tp->ftt_instr1);
-                       } else {
-                               reg = ARM_RM(tp->ftt_instr);
-                       }
-                       new_pc = regs->r[reg];
-                       set_thumb_flag(regs, new_pc);
+               break;
+       }

 
 

+       case FASTTRAP_T_BX_REG:
+       {
+               int reg;
+
+               if (!dtrace_arm_condition_true(condition_code, regs->cpsr)) {
+                       new_pc = pc + instr_size;

                        break;
                }
 
                        break;
                }
 

-               case FASTTRAP_T_LDR_PC_IMMED:
-               case FASTTRAP_T_VLDR_PC_IMMED:
-                       /* Handle these instructions by replacing the PC in the instruction with another
-                        * register. They are common, so we'd like to support them, and this way we do so
-                        * without any risk of having to simulate a segfault.
-                        */
+               if (tp->ftt_thumb) {
+                       reg = THUMB16_HRM(tp->ftt_instr1);
+               } else {
+                       reg = ARM_RM(tp->ftt_instr);
+               }
+               new_pc = regs->r[reg];
+               set_thumb_flag(regs, new_pc);

 
 

-                       /* Fall through */
+               break;
+       }

 
 

-               instr_emulate:
-               case FASTTRAP_T_COMMON:
-               {
-                       user_addr_t addr;
-                       uint8_t scratch[32];
-                       uint_t i = 0;
-                       fasttrap_instr_t emul_instr;
-                       emul_instr.instr32 = tp->ftt_instr;
-                       int emul_instr_size;
+       case FASTTRAP_T_LDR_PC_IMMED:
+       case FASTTRAP_T_VLDR_PC_IMMED:
+               /* Handle these instructions by replacing the PC in the instruction with another
+                * register. They are common, so we'd like to support them, and this way we do so
+                * without any risk of having to simulate a segfault.
+                */

 
 

-                       /*
-                        * Unfortunately sometimes when we emulate the instruction and have to replace the
-                        * PC, there is no longer a thumb mode equivalent. We end up having to run the
-                        * modified instruction in ARM mode. We use this variable to keep track of which
-                        * mode we should emulate in. We still use the original variable to determine
-                        * what mode to return to.
-                        */
-                       uint8_t emul_thumb = tp->ftt_thumb;
-                       int save_reg = -1;
-                       uint32_t save_val = 0;
+               /* Fall through */

 
 

-                       /*
-                        * Dealing with condition codes and emulation:
-                        * We can't just uniformly do a condition code check here because not all instructions
-                        * have condition codes. We currently do not support an instruction by instruction trace,
-                        * so we can assume that either: 1. We are executing a Thumb instruction, in which case
-                        * we either are not in an IT block and should execute always, or we are last in an IT
-                        * block. Either way, the traced instruction will run correctly, and we won't have any
-                        * problems when we return to the original code, because we will no longer be in the IT
-                        * block. 2. We are executing an ARM instruction, in which case we are ok as long as
-                        * we don't attempt to change the condition code.
+instr_emulate:
+       case FASTTRAP_T_COMMON:
+       {
+               user_addr_t addr;
+               uint8_t scratch[32];
+               uint_t i = 0;
+               fasttrap_instr_t emul_instr;
+               emul_instr.instr32 = tp->ftt_instr;
+               int emul_instr_size;
+
+               /*
+                * Unfortunately sometimes when we emulate the instruction and have to replace the
+                * PC, there is no longer a thumb mode equivalent. We end up having to run the
+                * modified instruction in ARM mode. We use this variable to keep track of which
+                * mode we should emulate in. We still use the original variable to determine
+                * what mode to return to.
+                */
+               uint8_t emul_thumb = tp->ftt_thumb;
+               int save_reg = -1;
+               uint32_t save_val = 0;
+
+               /*
+                * Dealing with condition codes and emulation:
+                * We can't just uniformly do a condition code check here because not all instructions
+                * have condition codes. We currently do not support an instruction by instruction trace,
+                * so we can assume that either: 1. We are executing a Thumb instruction, in which case
+                * we either are not in an IT block and should execute always, or we are last in an IT
+                * block. Either way, the traced instruction will run correctly, and we won't have any
+                * problems when we return to the original code, because we will no longer be in the IT
+                * block. 2. We are executing an ARM instruction, in which case we are ok as long as
+                * we don't attempt to change the condition code.
+                */
+               if (tp->ftt_type == FASTTRAP_T_LDR_PC_IMMED) {
+                       /* We know we always have a free register (the one we plan to write the
+                        * result value to!). So we'll replace the pc with that one.

                         */
                         */

-                       if (tp->ftt_type == FASTTRAP_T_LDR_PC_IMMED) {
-                               /* We know we always have a free register (the one we plan to write the
-                                * result value to!). So we'll replace the pc with that one.
-                                */
-                               int new_reg;
-                               if (tp->ftt_thumb) {
-                                       /* Check to see if thumb or thumb2 */
-                                       if (instr_size == 2) {
-                                               /*
-                                                * Sadness. We need to emulate this instruction in ARM mode
-                                                * because it has an 8 bit immediate offset. Instead of having
-                                                * to deal with condition codes in the ARM instruction, we'll
-                                                * just check the condition and abort if the condition is false.
-                                                */
-                                               if (!dtrace_arm_condition_true(condition_code, regs->cpsr)) {
-                                                       new_pc = pc + instr_size;
-                                                       break;
-                                               }
-
-                                               new_reg = (tp->ftt_instr1 >> 8) & 0x7;
-                                               regs->r[new_reg] = ALIGNADDR(regs->pc + 4, 2);
-                                               emul_thumb = 0;
-                                               emul_instr.instr32 = 0xE5900000 | (new_reg << 16) | (new_reg << 12) | ((tp->ftt_instr1 & 0xFF) << 2);
-                                       } else {
-                                               /* Thumb2. Just replace the register. */
-                                               new_reg = (tp->ftt_instr2 >> 12) & 0xF;
-                                               regs->r[new_reg] = ALIGNADDR(regs->pc + 4, 2);
-                                               emul_instr.instr16.instr1 &= ~0x000F;
-                                               emul_instr.instr16.instr1 |= new_reg;
+                       int new_reg;
+                       if (tp->ftt_thumb) {
+                               /* Check to see if thumb or thumb2 */
+                               if (instr_size == 2) {
+                                       /*
+                                        * Sadness. We need to emulate this instruction in ARM mode
+                                        * because it has an 8 bit immediate offset. Instead of having
+                                        * to deal with condition codes in the ARM instruction, we'll
+                                        * just check the condition and abort if the condition is false.
+                                        */
+                                       if (!dtrace_arm_condition_true(condition_code, regs->cpsr)) {
+                                               new_pc = pc + instr_size;
+                                               break;

                                        }
                                        }

+
+                                       new_reg = (tp->ftt_instr1 >> 8) & 0x7;
+                                       regs->r[new_reg] = ALIGNADDR(regs->pc + 4, 2);
+                                       emul_thumb = 0;
+                                       emul_instr.instr32 = 0xE5900000 | (new_reg << 16) | (new_reg << 12) | ((tp->ftt_instr1 & 0xFF) << 2);

                                } else {
                                } else {

-                                       /* ARM. Just replace the register. */
-                                       new_reg = (tp->ftt_instr >> 12) & 0xF;
-                                       regs->r[new_reg] = ALIGNADDR(regs->pc + 8,2);
-                                       emul_instr.instr32 &= ~0x000F0000;
-                                       emul_instr.instr32 |= new_reg << 16;
-                               }
-                       } else if (tp->ftt_type == FASTTRAP_T_VLDR_PC_IMMED) {
-                               /* This instruction only uses one register, and if we're here, we know
-                                * it must be the pc. So we'll just replace it with R0.
-                                */
-                               save_reg = 0;
-                               save_val = regs->r[0];
-                               regs->r[save_reg] = ALIGNADDR(regs->pc + (tp->ftt_thumb ? 4 : 8), 2);
-                               if (tp->ftt_thumb) {
+                                       /* Thumb2. Just replace the register. */
+                                       new_reg = (tp->ftt_instr2 >> 12) & 0xF;
+                                       regs->r[new_reg] = ALIGNADDR(regs->pc + 4, 2);

                                        emul_instr.instr16.instr1 &= ~0x000F;
                                        emul_instr.instr16.instr1 &= ~0x000F;

-                               } else {
-                                       emul_instr.instr32 &= ~0x000F0000;
+                                       emul_instr.instr16.instr1 |= new_reg;

                                }
                                }

+                       } else {
+                               /* ARM. Just replace the register. */
+                               new_reg = (tp->ftt_instr >> 12) & 0xF;
+                               regs->r[new_reg] = ALIGNADDR(regs->pc + 8, 2);
+                               emul_instr.instr32 &= ~0x000F0000;
+                               emul_instr.instr32 |= new_reg << 16;

                        }
                        }

-
-                       emul_instr_size = dtrace_instr_size(emul_instr.instr32, emul_thumb);
-
-                       /*
-                        * At this point:
-                        *   tp->ftt_thumb = thumb mode of original instruction
-                        *   emul_thumb = thumb mode for emulation
-                        *   emul_instr = instruction we are using to emulate original instruction
-                        *   emul_instr_size = size of emulating instruction
+               } else if (tp->ftt_type == FASTTRAP_T_VLDR_PC_IMMED) {
+                       /* This instruction only uses one register, and if we're here, we know
+                        * it must be the pc. So we'll just replace it with R0.

                         */
                         */

+                       save_reg = 0;
+                       save_val = regs->r[0];
+                       regs->r[save_reg] = ALIGNADDR(regs->pc + (tp->ftt_thumb ? 4 : 8), 2);
+                       if (tp->ftt_thumb) {
+                               emul_instr.instr16.instr1 &= ~0x000F;
+                       } else {
+                               emul_instr.instr32 &= ~0x000F0000;
+                       }
+               }

 
 

-                       addr = uthread->t_dtrace_scratch->addr;
+               emul_instr_size = dtrace_instr_size(emul_instr.instr32, emul_thumb);

 
 

-                       if (addr == 0LL) {
-                               fasttrap_sigtrap(p, uthread, pc); // Should be killing target proc
-                               new_pc = pc;
-                               break;
-                       }
+               /*
+                * At this point:
+                *   tp->ftt_thumb = thumb mode of original instruction
+                *   emul_thumb = thumb mode for emulation
+                *   emul_instr = instruction we are using to emulate original instruction
+                *   emul_instr_size = size of emulating instruction
+                */

 
 

-                       uthread->t_dtrace_scrpc = addr;
-                       if (emul_thumb) {
-                               /*
-                                * No way to do an unconditional branch in Thumb mode, shove the address
-                                * onto the user stack and go to the next location with a pop. This can
-                                * segfault if this push happens to cross a stack page, but that's ok, since
-                                * we are running in userland, and the kernel knows how to handle userland
-                                * stack expansions correctly.
-                                *
-                                * Layout of scratch space for Thumb mode:
-                                *   Emulated instruction
-                                *   ldr save_reg, [pc, #16] (if necessary, restore any register we clobbered)
-                                *   push { r0, r1 }
-                                *   ldr r0, [pc, #4]
-                                *   str r0, [sp, #4]
-                                *   pop { r0, pc }
-                                *   Location we should return to in original program
-                                *   Saved value of clobbered register (if necessary)
-                                */
+               addr = uthread->t_dtrace_scratch->addr;

 
 

-                               bcopy(&emul_instr, &scratch[i], emul_instr_size); i += emul_instr_size;
+               if (addr == 0LL) {
+                       fasttrap_sigtrap(p, uthread, pc);         // Should be killing target proc
+                       new_pc = pc;
+                       break;
+               }

 
 

-                               if (save_reg != -1) {
-                                       uint16_t restore_inst = 0x4803;
-                                       restore_inst |= (save_reg & 0x7) << 8;
-                                       SET16(scratch+i, restore_inst); i += 2;         // ldr reg, [pc , #16]
-                               }
+               uthread->t_dtrace_scrpc = addr;
+               if (emul_thumb) {
+                       /*
+                        * No way to do an unconditional branch in Thumb mode, shove the address
+                        * onto the user stack and go to the next location with a pop. This can
+                        * segfault if this push happens to cross a stack page, but that's ok, since
+                        * we are running in userland, and the kernel knows how to handle userland
+                        * stack expansions correctly.
+                        *
+                        * Layout of scratch space for Thumb mode:
+                        *   Emulated instruction
+                        *   ldr save_reg, [pc, #16] (if necessary, restore any register we clobbered)
+                        *   push { r0, r1 }
+                        *   ldr r0, [pc, #4]
+                        *   str r0, [sp, #4]
+                        *   pop { r0, pc }
+                        *   Location we should return to in original program
+                        *   Saved value of clobbered register (if necessary)
+                        */

 
 

-                               SET16(scratch+i, 0xB403); i += 2;                       // push { r0, r1 }
-                               SET16(scratch+i, 0x4801); i += 2;                       // ldr r0, [pc, #4]
-                               SET16(scratch+i, 0x9001); i += 2;                       // str r0, [sp, #4]
-                               SET16(scratch+i, 0xBD01); i += 2;                       // pop { r0, pc }
+                       bcopy(&emul_instr, &scratch[i], emul_instr_size); i += emul_instr_size;

 
 

-                               if (i % 4) {
-                                       SET16(scratch+i, 0); i += 2;                    // padding - saved 32 bit words must be aligned
-                               }
-                               SET32(scratch+i, pc + instr_size + (tp->ftt_thumb ? 1 : 0)); i += 4;    // Return address
-                               if (save_reg != -1) {
-                                       SET32(scratch+i, save_val); i += 4;             // saved value of clobbered register
-                               }
+                       if (save_reg != -1) {
+                               uint16_t restore_inst = 0x4803;
+                               restore_inst |= (save_reg & 0x7) << 8;
+                               SET16(scratch + i, restore_inst); i += 2;               // ldr reg, [pc , #16]
+                       }

 
 

-                               uthread->t_dtrace_astpc = addr + i;
-                               bcopy(&emul_instr, &scratch[i], emul_instr_size); i += emul_instr_size;
-                               SET16(scratch+i, FASTTRAP_THUMB_RET_INSTR); i += 2;
-                       } else {
-                               /*
-                                * Layout of scratch space for ARM mode:
-                                *   Emulated instruction
-                                *   ldr save_reg, [pc, #12] (if necessary, restore any register we clobbered)
-                                *   ldr pc, [pc, #4]
-                                *   Location we should return to in original program
-                                *   Saved value of clobbered register (if necessary)
-                                */
+                       SET16(scratch + i, 0xB403); i += 2;                             // push { r0, r1 }
+                       SET16(scratch + i, 0x4801); i += 2;                             // ldr r0, [pc, #4]
+                       SET16(scratch + i, 0x9001); i += 2;                             // str r0, [sp, #4]
+                       SET16(scratch + i, 0xBD01); i += 2;                             // pop { r0, pc }

 
 

-                               bcopy(&emul_instr, &scratch[i], emul_instr_size); i += emul_instr_size;
+                       if (i % 4) {
+                               SET16(scratch + i, 0); i += 2;                          // padding - saved 32 bit words must be aligned
+                       }
+                       SET32(scratch + i, pc + instr_size + (tp->ftt_thumb ? 1 : 0)); i += 4;          // Return address
+                       if (save_reg != -1) {
+                               SET32(scratch + i, save_val); i += 4;                   // saved value of clobbered register
+                       }

 
 

-                               if (save_reg != -1) {
-                                       uint32_t restore_inst = 0xE59F0004;
-                                       restore_inst |= save_reg << 12;
-                                       SET32(scratch+i, restore_inst); i += 4;         // ldr reg, [pc, #12]
-                               }
-                               SET32(scratch+i, 0xE51FF004); i += 4;                   // ldr pc, [pc, #4]
+                       uthread->t_dtrace_astpc = addr + i;
+                       bcopy(&emul_instr, &scratch[i], emul_instr_size); i += emul_instr_size;
+                       SET16(scratch + i, FASTTRAP_THUMB_RET_INSTR); i += 2;
+               } else {
+                       /*
+                        * Layout of scratch space for ARM mode:
+                        *   Emulated instruction
+                        *   ldr save_reg, [pc, #12] (if necessary, restore any register we clobbered)
+                        *   ldr pc, [pc, #4]
+                        *   Location we should return to in original program
+                        *   Saved value of clobbered register (if necessary)
+                        */

 
 

-                               SET32(scratch+i, pc + instr_size + (tp->ftt_thumb ? 1 : 0)); i += 4;    // Return address
-                               if (save_reg != -1) {
-                                       SET32(scratch+i, save_val); i += 4;             // Saved value of clobbered register
-                               }
+                       bcopy(&emul_instr, &scratch[i], emul_instr_size); i += emul_instr_size;

 
 

-                               uthread->t_dtrace_astpc = addr + i;
-                               bcopy(&emul_instr, &scratch[i], emul_instr_size); i += emul_instr_size;
-                               SET32(scratch+i, FASTTRAP_ARM_RET_INSTR); i += 4;
+                       if (save_reg != -1) {
+                               uint32_t restore_inst = 0xE59F0004;
+                               restore_inst |= save_reg << 12;
+                               SET32(scratch + i, restore_inst); i += 4;               // ldr reg, [pc, #12]

                        }
                        }

+                       SET32(scratch + i, 0xE51FF004); i += 4;                         // ldr pc, [pc, #4]

 
 

-                       if (patchInst(p, scratch, i, uthread->t_dtrace_scratch->write_addr) != KERN_SUCCESS) {
-                               fasttrap_sigtrap(p, uthread, pc);
-                               new_pc = pc;
-                               break;
+                       SET32(scratch + i, pc + instr_size + (tp->ftt_thumb ? 1 : 0)); i += 4;          // Return address
+                       if (save_reg != -1) {
+                               SET32(scratch + i, save_val); i += 4;                   // Saved value of clobbered register

                        }
 
                        }
 

-                       if (tp->ftt_retids != NULL) {
-                               uthread->t_dtrace_step = 1;
-                               uthread->t_dtrace_ret = 1;
-                               new_pc = uthread->t_dtrace_astpc + (emul_thumb ? 1 : 0);
-                       } else {
-                               new_pc = uthread->t_dtrace_scrpc + (emul_thumb ? 1 : 0);
-                       }
+                       uthread->t_dtrace_astpc = addr + i;
+                       bcopy(&emul_instr, &scratch[i], emul_instr_size); i += emul_instr_size;
+                       SET32(scratch + i, FASTTRAP_ARM_RET_INSTR); i += 4;
+               }

 
 

-                       uthread->t_dtrace_pc = pc;
-                       uthread->t_dtrace_npc = pc + instr_size;
-                       uthread->t_dtrace_on = 1;
-                       was_simulated = 0;
-                       set_thumb_flag(regs, new_pc);
+               if (uwrite(p, scratch, i, uthread->t_dtrace_scratch->write_addr) != KERN_SUCCESS) {
+                       fasttrap_sigtrap(p, uthread, pc);
+                       new_pc = pc;

                        break;
                }
 
                        break;
                }
 

-               default:
-                       panic("fasttrap: mishandled an instruction");
+               if (tp->ftt_retids != NULL) {
+                       uthread->t_dtrace_step = 1;
+                       uthread->t_dtrace_ret = 1;
+                       new_pc = uthread->t_dtrace_astpc + (emul_thumb ? 1 : 0);
+               } else {
+                       new_pc = uthread->t_dtrace_scrpc + (emul_thumb ? 1 : 0);
+               }
+
+               uthread->t_dtrace_pc = pc;
+               uthread->t_dtrace_npc = pc + instr_size;
+               uthread->t_dtrace_on = 1;
+               was_simulated = 0;
+               set_thumb_flag(regs, new_pc);
+               break;
+       }
+
+       default:
+               panic("fasttrap: mishandled an instruction");

        }
 
 done:
        }
 
 done:


@@ -1191,12 +1108,12 @@ done:
         */
        regs->pc = new_pc;
 
         */
        regs->pc = new_pc;
 

-       /*                      
+       /*

         * If there were no return probes when we first found the tracepoint,
         * we should feel no obligation to honor any return probes that were
         * subsequently enabled -- they'll just have to wait until the next
         * If there were no return probes when we first found the tracepoint,
         * we should feel no obligation to honor any return probes that were
         * subsequently enabled -- they'll just have to wait until the next

-        * time around. 
-        */                     
+        * time around.
+        */

        if (tp->ftt_retids != NULL) {
                /*
                 * We need to wait until the results of the instruction are
        if (tp->ftt_retids != NULL) {
                /*
                 * We need to wait until the results of the instruction are


@@ -1221,7 +1138,7 @@ done:
                }
        }
 
                }
        }
 

-       return (0);
+       return 0;

 }
 
 int
 }
 
 int


@@ -1244,8 +1161,9 @@ fasttrap_return_probe(arm_saved_state_t *regs)
         */
        if (p->p_lflag & P_LINVFORK) {
                proc_list_lock();
         */
        if (p->p_lflag & P_LINVFORK) {
                proc_list_lock();

-               while (p->p_lflag & P_LINVFORK)
+               while (p->p_lflag & P_LINVFORK) {

                        p = p->p_pptr;
                        p = p->p_pptr;

+               }

                proc_list_unlock();
        }
 
                proc_list_unlock();
        }
 


@@ -1256,29 +1174,30 @@ fasttrap_return_probe(arm_saved_state_t *regs)
         * complex web of lies. dtrace_return_probe() (our caller)
         * will correctly set %pc after we return.
         */
         * complex web of lies. dtrace_return_probe() (our caller)
         * will correctly set %pc after we return.
         */

-        regs->pc = pc;
+       regs->pc = pc;

 
        fasttrap_return_common(p, regs, pc, npc);
 
 
        fasttrap_return_common(p, regs, pc, npc);
 

-       return (0);
+       return 0;

 }
 
 uint64_t
 fasttrap_pid_getarg(void *arg, dtrace_id_t id, void *parg, int argno,
 }
 
 uint64_t
 fasttrap_pid_getarg(void *arg, dtrace_id_t id, void *parg, int argno,

-               int aframes)
+    int aframes)

 {
 #pragma unused(arg, id, parg, aframes)
        arm_saved_state_t* regs = find_user_regs(current_thread());
 
        /* First four arguments are in registers */
 {
 #pragma unused(arg, id, parg, aframes)
        arm_saved_state_t* regs = find_user_regs(current_thread());
 
        /* First four arguments are in registers */

-       if (argno < 4)
+       if (argno < 4) {

                return regs->r[argno];
                return regs->r[argno];

+       }

 
        /* Look on the stack for the rest */
        uint32_t value;
        uint32_t* sp = (uint32_t*) regs->sp;
        DTRACE_CPUFLAG_SET(CPU_DTRACE_NOFAULT);
 
        /* Look on the stack for the rest */
        uint32_t value;
        uint32_t* sp = (uint32_t*) regs->sp;
        DTRACE_CPUFLAG_SET(CPU_DTRACE_NOFAULT);

-       value = dtrace_fuword32((user_addr_t) (sp+argno-4));
+       value = dtrace_fuword32((user_addr_t) (sp + argno - 4));

        DTRACE_CPUFLAG_CLEAR(CPU_DTRACE_NOFAULT | CPU_DTRACE_BADADDR);
 
        return value;
        DTRACE_CPUFLAG_CLEAR(CPU_DTRACE_NOFAULT | CPU_DTRACE_BADADDR);
 
        return value;


@@ -1289,9 +1208,8 @@ fasttrap_usdt_getarg(void *arg, dtrace_id_t id, void *parg, int argno, int afram
 {
 #pragma unused(arg, id, parg, argno, aframes)
 #if 0
 {
 #pragma unused(arg, id, parg, argno, aframes)
 #if 0

-       return (fasttrap_anarg(ttolwp(curthread)->lwp_regs, 0, argno));
+       return fasttrap_anarg(ttolwp(curthread)->lwp_regs, 0, argno);

 #endif
 
        return 0;
 }
 #endif
 
        return 0;
 }

-