diff --git a/arch/blackfin/include/asm/pseudo_instructions.h b/arch/blackfin/include/asm/pseudo_instructions.h index 7173719fb531..b00adfa08169 100644 --- a/arch/blackfin/include/asm/pseudo_instructions.h +++ b/arch/blackfin/include/asm/pseudo_instructions.h @@ -13,5 +13,6 @@ #include extern bool execute_pseudodbg_assert(struct pt_regs *fp, unsigned int opcode); +extern bool execute_pseudodbg(struct pt_regs *fp, unsigned int opcode); #endif diff --git a/arch/blackfin/kernel/pseudodbg.c b/arch/blackfin/kernel/pseudodbg.c index 4474b8db3501..a5a4636124aa 100644 --- a/arch/blackfin/kernel/pseudodbg.c +++ b/arch/blackfin/kernel/pseudodbg.c @@ -9,6 +9,30 @@ #include #include +/* + * Unfortunately, the pt_regs structure is not laid out the same way as the + * hardware register file, so we need to do some fix ups. + */ +static bool fix_up_reg(struct pt_regs *fp, long *value, int grp, int reg) +{ + long *val = &fp->r0; + + /* Only do Dregs and Pregs for now */ + if (grp > 1) + return false; + + if (grp == 0 || (grp == 1 && reg < 6)) + val -= (reg + 8 * grp); + else if (grp == 1 && reg == 6) + val = &fp->usp; + else if (grp == 1 && reg == 7) + val = &fp->fp; + + *value = *val; + return true; + +} + #define PseudoDbg_Assert_opcode 0xf0000000 #define PseudoDbg_Assert_expected_bits 0 #define PseudoDbg_Assert_expected_mask 0xffff @@ -23,47 +47,38 @@ #define PseudoDbg_Assert_code_bits 27 #define PseudoDbg_Assert_code_mask 0x1f +/* + * DBGA - debug assert + */ bool execute_pseudodbg_assert(struct pt_regs *fp, unsigned int opcode) { int expected = ((opcode >> PseudoDbg_Assert_expected_bits) & PseudoDbg_Assert_expected_mask); int dbgop = ((opcode >> (PseudoDbg_Assert_dbgop_bits)) & PseudoDbg_Assert_dbgop_mask); int grp = ((opcode >> (PseudoDbg_Assert_grp_bits)) & PseudoDbg_Assert_grp_mask); int regtest = ((opcode >> (PseudoDbg_Assert_regtest_bits)) & PseudoDbg_Assert_regtest_mask); - long *value = &fp->r0; + long value; if ((opcode & 0xFF000000) != PseudoDbg_Assert_opcode) return false; - /* Only do Dregs and Pregs for now */ - if (grp > 1) + if (!fix_up_reg(fp, &value, grp, regtest)) return false; - /* - * Unfortunately, the pt_regs structure is not laid out the same way as the - * hardware register file, so we need to do some fix ups. - */ - if (grp == 0 || (grp == 1 && regtest < 6)) - value -= (regtest + 8 * grp); - else if (grp == 1 && regtest == 6) - value = &fp->usp; - else if (grp == 1 && regtest == 7) - value = &fp->fp; - if (dbgop == 0 || dbgop == 2) { /* DBGA ( regs_lo , uimm16 ) */ /* DBGAL ( regs , uimm16 ) */ - if (expected != (*value & 0xFFFF)) { + if (expected != (value & 0xFFFF)) { pr_notice("DBGA (%s%i.L,0x%x) failure, got 0x%x\n", grp ? "P" : "R", - regtest, expected, (unsigned int)(*value & 0xFFFF)); + regtest, expected, (unsigned int)(value & 0xFFFF)); return false; } } else if (dbgop == 1 || dbgop == 3) { /* DBGA ( regs_hi , uimm16 ) */ /* DBGAH ( regs , uimm16 ) */ - if (expected != ((*value >> 16) & 0xFFFF)) { + if (expected != ((value >> 16) & 0xFFFF)) { pr_notice("DBGA (%s%i.H,0x%x) failure, got 0x%x\n", grp ? "P" : "R", - regtest, expected, (unsigned int)((*value >> 16) & 0xFFFF)); + regtest, expected, (unsigned int)((value >> 16) & 0xFFFF)); return false; } } @@ -71,3 +86,38 @@ bool execute_pseudodbg_assert(struct pt_regs *fp, unsigned int opcode) fp->pc += 4; return true; } + +#define PseudoDbg_opcode 0xf8000000 +#define PseudoDbg_reg_bits 0 +#define PseudoDbg_reg_mask 0x7 +#define PseudoDbg_grp_bits 3 +#define PseudoDbg_grp_mask 0x7 +#define PseudoDbg_fn_bits 6 +#define PseudoDbg_fn_mask 0x3 +#define PseudoDbg_code_bits 8 +#define PseudoDbg_code_mask 0xff + +/* + * DBG - debug (dump a register value out) + */ +bool execute_pseudodbg(struct pt_regs *fp, unsigned int opcode) +{ + int grp, fn, reg; + long value; + + if ((opcode & 0xFF000000) != PseudoDbg_opcode) + return false; + + opcode >>= 16; + grp = ((opcode >> PseudoDbg_grp_bits) & PseudoDbg_reg_mask); + fn = ((opcode >> PseudoDbg_fn_bits) & PseudoDbg_fn_mask); + reg = ((opcode >> PseudoDbg_reg_bits) & PseudoDbg_reg_mask); + + if (!fix_up_reg(fp, &value, grp, reg)) + return false; + + pr_notice("DBG %s%d = %08lx\n", grp ? "P" : "R", reg, value); + + fp->pc += 2; + return true; +} diff --git a/arch/blackfin/kernel/traps.c b/arch/blackfin/kernel/traps.c index 9369836365bb..59c1df75e4de 100644 --- a/arch/blackfin/kernel/traps.c +++ b/arch/blackfin/kernel/traps.c @@ -213,6 +213,8 @@ asmlinkage notrace void trap_c(struct pt_regs *fp) if (!kernel_mode_regs(fp) && get_instruction(&opcode, (unsigned short *)fp->pc)) { if (execute_pseudodbg_assert(fp, opcode)) goto traps_done; + if (execute_pseudodbg(fp, opcode)) + goto traps_done; } #endif info.si_code = ILL_ILLOPC;