3 files changed, 89 insertions, 0 deletions

diff --git a/arch/blackfin/kernel/Makefile b/arch/blackfin/kernel/Makefile
index 2fc7f32ae32f..30d0d1f01dc7 100644
--- a/arch/blackfin/kernel/Makefile
+++ b/arch/blackfin/kernel/Makefile
@@ -31,6 +31,7 @@ obj-$(CONFIG_EARLY_PRINTK)           += early_printk.o
 obj-$(CONFIG_EARLY_PRINTK)           += shadow_console.o
 obj-$(CONFIG_STACKTRACE)             += stacktrace.o
 obj-$(CONFIG_DEBUG_VERBOSE)          += trace.o
+obj-$(CONFIG_BFIN_PSEUDODBG_INSNS)   += pseudodbg.o
 
 # the kgdb test puts code into L2 and without linker
 # relaxation, we need to force long calls to/from it
diff --git a/arch/blackfin/kernel/pseudodbg.c b/arch/blackfin/kernel/pseudodbg.c
new file mode 100644
index 000000000000..4474b8db3501
--- /dev/null
+++ b/arch/blackfin/kernel/pseudodbg.c
@@ -0,0 +1,73 @@
+/* The fake debug assert instructions
+ *
+ * Copyright 2010 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later
+ */
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/ptrace.h>
+
+#define PseudoDbg_Assert_opcode         0xf0000000
+#define PseudoDbg_Assert_expected_bits  0
+#define PseudoDbg_Assert_expected_mask  0xffff
+#define PseudoDbg_Assert_regtest_bits   16
+#define PseudoDbg_Assert_regtest_mask   0x7
+#define PseudoDbg_Assert_grp_bits       19
+#define PseudoDbg_Assert_grp_mask       0x7
+#define PseudoDbg_Assert_dbgop_bits     22
+#define PseudoDbg_Assert_dbgop_mask     0x3
+#define PseudoDbg_Assert_dontcare_bits  24
+#define PseudoDbg_Assert_dontcare_mask  0x7
+#define PseudoDbg_Assert_code_bits      27
+#define PseudoDbg_Assert_code_mask      0x1f
+
+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;
+
+        if ((opcode & 0xFF000000) != PseudoDbg_Assert_opcode)
+                return false;
+
+        /* Only do Dregs and Pregs for now */
+        if (grp > 1)
+                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)) {
+                        pr_notice("DBGA (%s%i.L,0x%x) failure, got 0x%x\n", grp ? "P" : "R",
+                                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)) {
+                        pr_notice("DBGA (%s%i.H,0x%x) failure, got 0x%x\n", grp ? "P" : "R",
+                                regtest, expected, (unsigned int)((*value >> 16) & 0xFFFF));
+                        return false;
+                }
+        }
+
+        fp->pc += 4;
+        return true;
+}
diff --git a/arch/blackfin/kernel/traps.c b/arch/blackfin/kernel/traps.c
index fffcf8a516bf..9369836365bb 100644
--- a/arch/blackfin/kernel/traps.c
+++ b/arch/blackfin/kernel/traps.c
@@ -16,6 +16,7 @@
 #include <linux/irq.h>
 #include <asm/trace.h>
 #include <asm/fixed_code.h>
+#include <asm/pseudo_instructions.h>
 
 #ifdef CONFIG_KGDB
 # include <linux/kgdb.h>
@@ -68,6 +69,9 @@ asmlinkage notrace void trap_c(struct pt_regs *fp)
 #ifdef CONFIG_DEBUG_BFIN_HWTRACE_ON
         int j;
 #endif
+#ifdef CONFIG_BFIN_PSEUDODBG_INSNS
+        int opcode;
+#endif
         unsigned int cpu = raw_smp_processor_id();
         const char *strerror = NULL;
         int sig = 0;
@@ -200,6 +204,17 @@ asmlinkage notrace void trap_c(struct pt_regs *fp)
                         }
                 }
 #endif
+#ifdef CONFIG_BFIN_PSEUDODBG_INSNS
+                /*
+                 * Support for the fake instructions, if the instruction fails,
+                 * then just execute a illegal opcode failure (like normal).
+                 * Don't support these instructions inside the kernel
+                 */
+                if (!kernel_mode_regs(fp) && get_instruction(&opcode, (unsigned short *)fp->pc)) {
+                        if (execute_pseudodbg_assert(fp, opcode))
+                                goto traps_done;
+                }
+#endif
                 info.si_code = ILL_ILLOPC;
                 sig = SIGILL;
                 strerror = KERN_NOTICE EXC_0x21(KERN_NOTICE);