Index: head/sys/arm/arm/exception.S =================================================================== --- head/sys/arm/arm/exception.S +++ head/sys/arm/arm/exception.S @@ -52,13 +52,15 @@ #include #include #include +#include + __FBSDID("$FreeBSD$"); #ifdef KDTRACE_HOOKS .bss .align 4 - .global _C_LABEL(dtrace_invop_calltrap_addr) -_C_LABEL(dtrace_invop_calltrap_addr): + .global _C_LABEL(dtrace_invop_jump_addr) +_C_LABEL(dtrace_invop_jump_addr): .word 0 .word 0 #endif @@ -361,9 +363,39 @@ */ ASENTRY_NP(undefined_entry) PUSHFRAMEINSVC /* mode stack, build trapframe there. */ + mov r4, r0 /* R0 contains SPSR */ adr lr, exception_exit /* Return from handler via standard */ - mov r0, sp /* exception exit routine. Pass the */ - b undefinedinstruction /* trapframe to the handler. */ + mov r0, sp /* exception exit routine. pass frame */ + + ldr r2, [sp, #(TF_PC)] /* load pc */ +#if __ARM_ARCH >= 7 + tst r4, #(PSR_T) /* test if PSR_T */ + subne r2, r2, #(THUMB_INSN_SIZE) + subeq r2, r2, #(INSN_SIZE) +#else + sub r2, r2, #(INSN_SIZE) /* fix pc */ +#endif + str r2, [sp, #TF_PC] /* store pc */ + +#ifdef KDTRACE_HOOKS + /* Check if dtrace is enabled */ + ldr r1, =_C_LABEL(dtrace_invop_jump_addr) + ldr r3, [r1] + cmp r3, #0 + beq undefinedinstruction + + and r4, r4, #(PSR_MODE) /* Mask out unneeded bits */ + cmp r4, #(PSR_USR32_MODE) /* Check if we came from usermode */ + beq undefinedinstruction + + ldr r4, [r2] /* load instrution */ + ldr r1, =FBT_BREAKPOINT /* load fbt inv op */ + cmp r1, r4 + bne undefinedinstruction + + bx r3 /* call invop_jump_addr */ +#endif + b undefinedinstruction /* call stadnard handler */ END(undefined_entry) /* Index: head/sys/arm/arm/genassym.c =================================================================== --- head/sys/arm/arm/genassym.c +++ head/sys/arm/arm/genassym.c @@ -118,6 +118,7 @@ ASSYM(MD_RAS_START, offsetof(struct mdthread, md_ras_start)); ASSYM(MD_RAS_END, offsetof(struct mdthread, md_ras_end)); +ASSYM(TF_SPSR, offsetof(struct trapframe, tf_spsr)); ASSYM(TF_R0, offsetof(struct trapframe, tf_r0)); ASSYM(TF_R1, offsetof(struct trapframe, tf_r1)); ASSYM(TF_PC, offsetof(struct trapframe, tf_pc)); Index: head/sys/arm/arm/trap-v6.c =================================================================== --- head/sys/arm/arm/trap-v6.c +++ head/sys/arm/arm/trap-v6.c @@ -66,6 +66,10 @@ #include #endif +#ifdef KDTRACE_HOOKS +#include +#endif + extern char fusubailout[]; extern char cachebailout[]; @@ -561,6 +565,13 @@ const char *rw_mode; usermode = TRAPF_USERMODE(tf); +#ifdef KDTRACE_HOOKS + if (!usermode) { + if (dtrace_trap_func != NULL && (*dtrace_trap_func)(tf, far)) + return (0); + } +#endif + mode = usermode ? "user" : "kernel"; rw_mode = fsr & FSR_WNR ? "write" : "read"; disable_interrupts(PSR_I|PSR_F); Index: head/sys/arm/arm/undefined.c =================================================================== --- head/sys/arm/arm/undefined.c +++ head/sys/arm/arm/undefined.c @@ -99,10 +99,6 @@ #define COPROC_VFP 10 -#ifdef KDTRACE_HOOKS -int (*dtrace_invop_jump_addr)(struct trapframe *); -#endif - static int gdb_trapper(u_int, u_int, struct trapframe *, int); LIST_HEAD(, undefined_handler) undefined_handlers[MAX_COPROCS]; @@ -206,12 +202,6 @@ PCPU_INC(cnt.v_trap); -#if __ARM_ARCH >= 7 - if ((frame->tf_spsr & PSR_T) != 0) - frame->tf_pc -= THUMB_INSN_SIZE; - else -#endif - frame->tf_pc -= INSN_SIZE; fault_pc = frame->tf_pc; /* @@ -350,12 +340,6 @@ #endif return; } -#ifdef KDTRACE_HOOKS - else if (dtrace_invop_jump_addr != 0) { - dtrace_invop_jump_addr(frame); - return; - } -#endif else panic("Undefined instruction in kernel.\n"); } Index: head/sys/arm/include/trap.h =================================================================== --- head/sys/arm/include/trap.h +++ head/sys/arm/include/trap.h @@ -7,4 +7,5 @@ #define GDB5_BREAKPOINT 0xe7ffdefe #define PTRACE_BREAKPOINT 0xe7fffff0 #define KERNEL_BREAKPOINT 0xe7ffffff +#define FBT_BREAKPOINT 0xe7f000f0 #endif /* _MACHINE_TRAP_H_ */ Index: head/sys/cddl/dev/fbt/arm/fbt_isa.c =================================================================== --- head/sys/cddl/dev/fbt/arm/fbt_isa.c +++ head/sys/cddl/dev/fbt/arm/fbt_isa.c @@ -35,11 +35,10 @@ #include #include +#include #include "fbt.h" -#define FBT_PATCHVAL 0xe7f000f0 /* Specified undefined instruction */ - #define FBT_PUSHM 0xe92d0000 #define FBT_POPM 0xe8bd0000 #define FBT_JUMP 0xea000000 @@ -53,15 +52,20 @@ struct trapframe *frame = (struct trapframe *)stack; solaris_cpu_t *cpu = &solaris_cpu[curcpu]; fbt_probe_t *fbt = fbt_probetab[FBT_ADDR2NDX(addr)]; + register_t fifthparam; for (; fbt != NULL; fbt = fbt->fbtp_hashnext) { if ((uintptr_t)fbt->fbtp_patchpoint == addr) { cpu->cpu_dtrace_caller = addr; - /* TODO: Need 5th parameter from stack */ + /* Get 5th parameter from stack */ + DTRACE_CPUFLAG_SET(CPU_DTRACE_NOFAULT); + fifthparam = *(register_t *)frame->tf_usr_sp; + DTRACE_CPUFLAG_CLEAR(CPU_DTRACE_NOFAULT | CPU_DTRACE_BADADDR); + dtrace_probe(fbt->fbtp_id, frame->tf_r0, frame->tf_r1, frame->tf_r2, - frame->tf_r3, 0); + frame->tf_r3, fifthparam); cpu->cpu_dtrace_caller = 0; @@ -77,7 +81,7 @@ { *fbt->fbtp_patchpoint = val; - cpu_icache_sync_range((vm_offset_t)fbt->fbtp_patchpoint, 4); + cpu_icache_sync_range((vm_offset_t)fbt->fbtp_patchpoint, sizeof(val)); } int @@ -104,13 +108,6 @@ if (name[0] == '_' && name[1] == '_') return (0); - /* - * Architecture-specific exclusion list, largely to do with FBT trap - * processing, to prevent reentrance. - */ - if (strcmp(name, "undefinedinstruction") == 0) - return (0); - instr = (uint32_t *)symval->value; limit = (uint32_t *)(symval->value + symval->size); @@ -125,12 +122,12 @@ fbt = malloc(sizeof (fbt_probe_t), M_FBT, M_WAITOK | M_ZERO); fbt->fbtp_name = name; fbt->fbtp_id = dtrace_probe_create(fbt_id, modname, - name, FBT_ENTRY, 3, fbt); + name, FBT_ENTRY, 2, fbt); fbt->fbtp_patchpoint = instr; fbt->fbtp_ctl = lf; fbt->fbtp_loadcnt = lf->loadcnt; fbt->fbtp_savedval = *instr; - fbt->fbtp_patchval = FBT_PATCHVAL; + fbt->fbtp_patchval = FBT_BREAKPOINT; fbt->fbtp_rval = DTRACE_INVOP_PUSHM; fbt->fbtp_symindx = symindx; @@ -157,7 +154,6 @@ start = (uint32_t *)symval->value; if (target >= limit || target < start) break; - instr++; /* skip delay slot */ } } @@ -171,7 +167,7 @@ fbt->fbtp_name = name; if (retfbt == NULL) { fbt->fbtp_id = dtrace_probe_create(fbt_id, modname, - name, FBT_RETURN, 3, fbt); + name, FBT_RETURN, 2, fbt); } else { retfbt->fbtp_next = fbt; fbt->fbtp_id = retfbt->fbtp_id; @@ -187,7 +183,7 @@ else fbt->fbtp_rval = DTRACE_INVOP_POPM; fbt->fbtp_savedval = *instr; - fbt->fbtp_patchval = FBT_PATCHVAL; + fbt->fbtp_patchval = FBT_BREAKPOINT; fbt->fbtp_hashnext = fbt_probetab[FBT_ADDR2NDX(instr)]; fbt_probetab[FBT_ADDR2NDX(instr)] = fbt;