Index: head/sys/arm/arm/db_trace.c =================================================================== --- head/sys/arm/arm/db_trace.c +++ head/sys/arm/arm/db_trace.c @@ -66,7 +66,7 @@ finished = false; while (!finished) { - finished = unwind_stack_one(state, 0); + finished = unwind_stack_one(state, 1); /* Print the frame details */ sym = db_search_symbol(state->start_pc, DB_STGY_ANY, &offset); Index: head/sys/arm/arm/exception.S =================================================================== --- head/sys/arm/arm/exception.S +++ head/sys/arm/arm/exception.S @@ -57,11 +57,6 @@ #ifdef KDTRACE_HOOKS .bss .align 4 - .global _C_LABEL(dtrace_invop_jump_addr) -_C_LABEL(dtrace_invop_jump_addr): - .word 0 - .word 0 - .global _C_LABEL(dtrace_invop_calltrap_addr) _C_LABEL(dtrace_invop_calltrap_addr): .word 0 @@ -162,7 +157,8 @@ msr cpsr_c, r2; /* Punch into SVC mode */ \ mov r2, sp; /* Save SVC sp */ \ bic sp, sp, #7; /* Align sp to an 8-byte addrress */ \ - sub sp, sp, #4; /* Pad trapframe to keep alignment */ \ + sub sp, sp, #(4 * 17); /* Pad trapframe to keep alignment */ \ + /* and for dtrace to emulate push/pop */ \ str r0, [sp, #-4]!; /* Push return address */ \ str lr, [sp, #-4]!; /* Push SVC lr */ \ str r2, [sp, #-4]!; /* Push SVC sp */ \ @@ -199,7 +195,8 @@ msr cpsr_c, r2; /* Punch into SVC mode */ \ mov r2, sp; /* Save SVC sp */ \ bic sp, sp, #7; /* Align sp to an 8-byte addrress */ \ - sub sp, sp, #4; /* Pad trapframe to keep alignment */ \ + sub sp, sp, #(4 * 17); /* Pad trapframe to keep alignment */ \ + /* and for dtrace to emulate push/pop */ \ str r0, [sp, #-4]!; /* Push return address */ \ str lr, [sp, #-4]!; /* Push SVC lr */ \ str r2, [sp, #-4]!; /* Push SVC sp */ \ Index: head/sys/arm/arm/undefined.c =================================================================== --- head/sys/arm/arm/undefined.c +++ head/sys/arm/arm/undefined.c @@ -86,6 +86,10 @@ #include #endif +#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]; @@ -286,7 +290,14 @@ printf("No debugger in kernel.\n"); #endif return; - } else + } +#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/cddl/contrib/opensolaris/uts/common/sys/dtrace.h =================================================================== --- head/sys/cddl/contrib/opensolaris/uts/common/sys/dtrace.h +++ head/sys/cddl/contrib/opensolaris/uts/common/sys/dtrace.h @@ -2436,6 +2436,10 @@ #elif defined(__arm__) +#define DTRACE_INVOP_SHIFT 4 +#define DTRACE_INVOP_MASK ((1 << DTRACE_INVOP_SHIFT) - 1) +#define DTRACE_INVOP_DATA(x) ((x) >> DTRACE_INVOP_SHIFT) + #define DTRACE_INVOP_PUSHM 1 #define DTRACE_INVOP_POPM 2 #define DTRACE_INVOP_B 3 Index: head/sys/cddl/dev/dtrace/arm/dtrace_subr.c =================================================================== --- head/sys/cddl/dev/dtrace/arm/dtrace_subr.c +++ head/sys/cddl/dev/dtrace/arm/dtrace_subr.c @@ -46,7 +46,11 @@ #include #define DELAYBRANCH(x) ((int)(x) < 0) - + +#define BIT_PC 15 +#define BIT_LR 14 +#define BIT_SP 13 + extern uintptr_t dtrace_in_probe_addr; extern int dtrace_in_probe; extern dtrace_id_t dtrace_probeid_error; @@ -231,16 +235,97 @@ static int dtrace_invop_start(struct trapframe *frame) { - printf("IMPLEMENT ME: %s\n", __func__); - switch (dtrace_invop(frame->tf_pc, (uintptr_t *)frame, frame->tf_pc)) { + register_t *r0, *sp; + int data, invop, reg, update_sp; + + invop = dtrace_invop(frame->tf_pc, (uintptr_t *)frame, frame->tf_pc); + switch (invop & DTRACE_INVOP_MASK) { case DTRACE_INVOP_PUSHM: - // TODO: + sp = (register_t *)frame->tf_svc_sp; + r0 = &frame->tf_r0; + data = DTRACE_INVOP_DATA(invop); + + /* + * Store the pc, lr, and sp. These have their own + * entries in the struct. + */ + if (data & (1 << BIT_PC)) { + sp--; + *sp = frame->tf_pc; + } + if (data & (1 << BIT_LR)) { + sp--; + *sp = frame->tf_svc_lr; + } + if (data & (1 << BIT_SP)) { + sp--; + *sp = frame->tf_svc_sp; + } + + /* Store the general registers */ + for (reg = 12; reg >= 0; reg--) { + if (data & (1 << reg)) { + sp--; + *sp = r0[reg]; + } + } + + /* Update the stack pointer and program counter to continue */ + frame->tf_svc_sp = (register_t)sp; + frame->tf_pc += 4; break; case DTRACE_INVOP_POPM: - // TODO: + sp = (register_t *)frame->tf_svc_sp; + r0 = &frame->tf_r0; + data = DTRACE_INVOP_DATA(invop); + + /* Read the general registers */ + for (reg = 0; reg <= 12; reg++) { + if (data & (1 << reg)) { + r0[reg] = *sp; + sp++; + } + } + + /* + * Set the stack pointer. If we don't update it here we will + * need to update it at the end as the instruction would do + */ + update_sp = 1; + if (data & (1 << BIT_SP)) { + frame->tf_svc_sp = *sp; + *sp++; + update_sp = 0; + } + + /* Update the link register, we need to use the correct copy */ + if (data & (1 << BIT_LR)) { + frame->tf_svc_lr = *sp; + *sp++; + } + /* + * And the program counter. If it's not in the list skip over + * it when we return so to not hit this again. + */ + if (data & (1 << BIT_PC)) { + frame->tf_pc = *sp; + *sp++; + } else + frame->tf_pc += 4; + + /* Update the stack pointer if we haven't already done so */ + if (update_sp) + frame->tf_svc_sp = (register_t)sp; break; case DTRACE_INVOP_B: - // TODO + data = DTRACE_INVOP_DATA(invop) & 0x00ffffff; + /* Sign extend the data */ + if ((data & (1 << 23)) != 0) + data |= 0xff000000; + /* The data is the number of 4-byte words to change the pc */ + data *= 4; + data += 8; + frame->tf_pc += data; break; default: return (-1); 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 @@ -38,7 +38,7 @@ #include "fbt.h" -#define FBT_PATCHVAL 0xe06a0cfe /* illegal instruction */ +#define FBT_PATCHVAL 0xe7f000f0 /* Specified undefined instruction */ #define FBT_PUSHM 0xe92d0000 #define FBT_POPM 0xe8bd0000 @@ -66,7 +66,7 @@ cpu->cpu_dtrace_caller = 0; - return (fbt->fbtp_rval); + return (fbt->fbtp_rval | (fbt->fbtp_savedval << DTRACE_INVOP_SHIFT)); } }