9 files changed, 97 insertions, 43 deletions

diff --git a/arch/blackfin/kernel/setup.c b/arch/blackfin/kernel/setup.c
index 07c1cfdc958e..a86bf6545781 100644
--- a/arch/blackfin/kernel/setup.c
+++ b/arch/blackfin/kernel/setup.c
@@ -194,6 +194,17 @@ void __init setup_arch(char **cmdline_p)
         /* this give a chance to get printk() working before crash. */
 #endif
 
+        printk(KERN_INFO "Hardware Trace ");
+        if (bfin_read_TBUFCTL() & 0x1 )
+                printk("Active ");
+        else
+                printk("Off ");
+        if (bfin_read_TBUFCTL() & 0x2)
+                printk("and Enabled\n");
+        else
+        printk("and Disabled\n");
+
+
 #if defined(CONFIG_CHR_DEV_FLASH) || defined(CONFIG_BLK_DEV_FLASH)
         /* we need to initialize the Flashrom device here since we might
          * do things with flash early on in the boot
@@ -397,9 +408,6 @@ void __init setup_arch(char **cmdline_p)
         _bfin_swrst = bfin_read_SWRST();
 #endif
 
-        printk(KERN_INFO "Hardware Trace Enabled\n");
-        bfin_write_TBUFCTL(0x03);
-
         /* Copy atomic sequences to their fixed location, and sanity check that
            these locations are the ones that we advertise to userspace.  */
         memcpy((void *)FIXED_CODE_START, &fixed_code_start,
diff --git a/arch/blackfin/kernel/traps.c b/arch/blackfin/kernel/traps.c
index 56058b0b6d4a..a58d0f50be20 100644
--- a/arch/blackfin/kernel/traps.c
+++ b/arch/blackfin/kernel/traps.c
@@ -33,6 +33,7 @@
 #include <asm/blackfin.h>
 #include <asm/uaccess.h>
 #include <asm/irq_handler.h>
+#include <asm/trace.h>
 #include <linux/interrupt.h>
 #include <linux/module.h>
 #include <linux/kallsyms.h>
@@ -131,16 +132,6 @@ static int printk_address(unsigned long address)
         return printk("[<0x%p>]", (void*)address);
 }
 
-#define trace_buffer_save(x) \
-        do { \
-                (x) = bfin_read_TBUFCTL(); \
-                bfin_write_TBUFCTL((x) & ~TBUFEN); \
-        } while (0)
-#define trace_buffer_restore(x) \
-        do { \
-                bfin_write_TBUFCTL((x));        \
-        } while (0)
-
 asmlinkage void trap_c(struct pt_regs *fp)
 {
         int j, sig = 0;
diff --git a/arch/blackfin/mach-bf533/head.S b/arch/blackfin/mach-bf533/head.S
index 7e2aa8d0f44f..7dd0e9c3a936 100644
--- a/arch/blackfin/mach-bf533/head.S
+++ b/arch/blackfin/mach-bf533/head.S
@@ -30,6 +30,7 @@
 #include <linux/linkage.h>
 #include <linux/init.h>
 #include <asm/blackfin.h>
+#include <asm/trace.h>
 #if CONFIG_BFIN_KERNEL_CLOCK
 #include <asm/mach/mem_init.h>
 #endif
@@ -96,6 +97,10 @@ ENTRY(__start)
         M2 = r0;
         M3 = r0;
 
+        trace_buffer_start(p0,r0);
+        P0 = R1;
+        R0 = R1;
+
 #if CONFIG_DEBUG_KERNEL_START
 
 /*
diff --git a/arch/blackfin/mach-bf537/head.S b/arch/blackfin/mach-bf537/head.S
index 7d902bbd860f..429c8a1019da 100644
--- a/arch/blackfin/mach-bf537/head.S
+++ b/arch/blackfin/mach-bf537/head.S
@@ -30,6 +30,8 @@
 #include <linux/linkage.h>
 #include <linux/init.h>
 #include <asm/blackfin.h>
+#include <asm/trace.h>
+
 #if CONFIG_BFIN_KERNEL_CLOCK
 #include <asm/mach/mem_init.h>
 #endif
@@ -93,6 +95,10 @@ ENTRY(__start)
         M2 = r0;
         M3 = r0;
 
+        trace_buffer_start(p0,r0);
+        P0 = R1;
+        R0 = R1;
+
         /* Turn off the icache */
         p0.l = (IMEM_CONTROL & 0xFFFF);
         p0.h = (IMEM_CONTROL >> 16);
diff --git a/arch/blackfin/mach-bf548/head.S b/arch/blackfin/mach-bf548/head.S
index 0e1a25447391..06751ae8b857 100644
--- a/arch/blackfin/mach-bf548/head.S
+++ b/arch/blackfin/mach-bf548/head.S
@@ -29,6 +29,7 @@
 
 #include <linux/linkage.h>
 #include <asm/blackfin.h>
+#include <asm/trace.h>
 #if CONFIG_BFIN_KERNEL_CLOCK
 #include <asm/mach/mem_init.h>
 #endif
@@ -91,6 +92,10 @@ ENTRY(__stext)
         M2 = r0;
         M3 = r0;
 
+        trace_buffer_start(p0,r0);
+        P0 = R1;
+        R0 = R1;
+
         /* Turn off the icache */
         p0.l = (IMEM_CONTROL & 0xFFFF);
         p0.h = (IMEM_CONTROL >> 16);
diff --git a/arch/blackfin/mach-bf561/head.S b/arch/blackfin/mach-bf561/head.S
index 31cbc75c85cf..2f08bcb2dded 100644
--- a/arch/blackfin/mach-bf561/head.S
+++ b/arch/blackfin/mach-bf561/head.S
@@ -30,6 +30,8 @@
 #include <linux/linkage.h>
 #include <linux/init.h>
 #include <asm/blackfin.h>
+#include <asm/trace.h>
+
 #if CONFIG_BFIN_KERNEL_CLOCK
 #include <asm/mach/mem_init.h>
 #endif
@@ -93,6 +95,10 @@ ENTRY(__start)
         M2 = r0;
         M3 = r0;
 
+        trace_buffer_start(p0,r0);
+        P0 = R1;
+        R0 = R1;
+
         /* Turn off the icache */
         p0.l = (IMEM_CONTROL & 0xFFFF);
         p0.h = (IMEM_CONTROL >> 16);
diff --git a/arch/blackfin/mach-common/entry.S b/arch/blackfin/mach-common/entry.S
index c4a32ea06c4b..15603275f749 100644
--- a/arch/blackfin/mach-common/entry.S
+++ b/arch/blackfin/mach-common/entry.S
@@ -54,29 +54,10 @@
 #include <asm/errno.h>
 #include <asm/thread_info.h>  /* TIF_NEED_RESCHED */
 #include <asm/asm-offsets.h>
+#include <asm/trace.h>
 
 #include <asm/mach-common/context.S>
 
-#ifdef CONFIG_DEBUG_BFIN_NO_KERN_HWTRACE
-        /*
-         * TODO: this should be proper save/restore, but for now
-         * we'll just cheat and use 0x1/0x13
-         */
-# define DEBUG_START_HWTRACE \
-        P5.l = LO(TBUFCTL); \
-        P5.h = HI(TBUFCTL); \
-        R7 = 0x13; \
-        [P5] = R7;
-# define DEBUG_STOP_HWTRACE \
-        P5.l = LO(TBUFCTL); \
-        P5.h = HI(TBUFCTL); \
-        R7 = 0x01; \
-        [P5] = R7;
-#else
-# define DEBUG_START_HWTRACE
-# define DEBUG_STOP_HWTRACE
-#endif
-
 #ifdef CONFIG_EXCPT_IRQ_SYSC_L1
 .section .l1.text
 #else
@@ -110,7 +91,7 @@ ENTRY(_ex_icplb)
         ASTAT = [sp++];
         SAVE_ALL_SYS
         call __cplb_hdr;
-        DEBUG_START_HWTRACE
+        DEBUG_START_HWTRACE(p5, r7)
         RESTORE_ALL_SYS
         SP = RETN;
         rtx;
@@ -128,7 +109,7 @@ ENTRY(_ex_spinlock)
 ENDPROC(_ex_spinlock)
 
 ENTRY(_ex_syscall)
-        DEBUG_START_HWTRACE
+        DEBUG_START_HWTRACE(p5, r7)
         (R7:6,P5:4) = [sp++];
         ASTAT = [sp++];
         raise 15;               /* invoked by TRAP #0, for sys call */
@@ -186,7 +167,7 @@ ENTRY(_ex_single_step)
         if !cc jump _ex_trap_c;
 
 _return_from_exception:
-        DEBUG_START_HWTRACE
+        DEBUG_START_HWTRACE(p5, r7)
 #ifdef ANOMALY_05000257
         R7=LC0;
         LC0=R7;
@@ -208,7 +189,7 @@ ENTRY(_handle_bad_cplb)
          * need to make a CPLB exception look like a normal exception
          */
 
-        DEBUG_START_HWTRACE
+        DEBUG_START_HWTRACE(p5, r7)
         RESTORE_ALL_SYS
         [--sp] = ASTAT;
         [--sp] = (R7:6, P5:4);
@@ -251,7 +232,7 @@ ENTRY(_ex_trap_c)
         R6 = SEQSTAT;
         [P5] = R6;
 
-        DEBUG_START_HWTRACE
+        DEBUG_START_HWTRACE(p5, r7)
         (R7:6,P5:4) = [sp++];
         ASTAT = [sp++];
         SP = RETN;
@@ -335,7 +316,7 @@ ENTRY(_trap) /* Exception: 4th entry into system event table(supervisor mode)*/
         /* Try to deal with syscalls quickly.  */
         [--sp] = ASTAT;
         [--sp] = (R7:6, P5:4);
-        DEBUG_STOP_HWTRACE
+        DEBUG_STOP_HWTRACE(p5, r7)
         r7 = SEQSTAT;           /* reason code is in bit 5:0 */
         r6.l = lo(SEQSTAT_EXCAUSE);
         r6.h = hi(SEQSTAT_EXCAUSE);
diff --git a/arch/blackfin/mach-common/interrupt.S b/arch/blackfin/mach-common/interrupt.S
index 8be548e061bf..203e20709163 100644
--- a/arch/blackfin/mach-common/interrupt.S
+++ b/arch/blackfin/mach-common/interrupt.S
@@ -34,6 +34,7 @@
 #include <linux/linkage.h>
 #include <asm/entry.h>
 #include <asm/asm-offsets.h>
+#include <asm/trace.h>
 
 #include <asm/mach-common/context.S>
 
@@ -170,10 +171,9 @@ ENTRY(_evt_ivhw)
         r7.l = W[p5];
 1:
 #endif
-        p0.l = lo(TBUFCTL);
-        p0.h = hi(TBUFCTL);
-        r0 = 1;
-        [p0] = r0;
+
+        trace_buffer_stop(p0, r0);
+
         r0 = IRQ_HWERR;
         r1 = sp;
 
diff --git a/include/asm-blackfin/trace.h b/include/asm-blackfin/trace.h
new file mode 100644
index 000000000000..9c2474c9a589
--- /dev/null
+++ b/include/asm-blackfin/trace.h
@@ -0,0 +1,52 @@
+/*
+ * Common header file for blackfin family of processors.
+ *
+ */
+
+#ifndef _BLACKFIN_TRACE_
+#define _BLACKFIN_TRACE_
+
+#ifndef __ASSEMBLY__
+/* Trace Macros for C files */
+
+#define trace_buffer_save(x) \
+        do { \
+                (x) = bfin_read_TBUFCTL(); \
+                bfin_write_TBUFCTL((x) & ~TBUFEN); \
+        } while (0)
+
+#define trace_buffer_restore(x) \
+        do { \
+                bfin_write_TBUFCTL((x));        \
+        } while (0)
+
+#else
+/* Trace Macros for Assembly files */
+
+#define TRACE_BUFFER_START(preg, dreg) trace_buffer_start(preg, dreg)
+#define TRACE_BUFFER_STOP(preg, dreg)  trace_buffer_stop(preg, dreg)
+
+#define trace_buffer_stop(preg, dreg)   \
+        preg.L = LO(TBUFCTL);           \
+        preg.H = HI(TBUFCTL);           \
+        dreg = 0x1;                     \
+        [preg] = dreg;
+
+#define trace_buffer_start(preg, dreg) \
+        preg.L = LO(TBUFCTL);           \
+        preg.H = HI(TBUFCTL);           \
+        dreg = 0x13;                    \
+        [preg] = dreg;
+
+#ifdef CONFIG_DEBUG_BFIN_NO_KERN_HWTRACE
+# define DEBUG_START_HWTRACE(preg, dreg) trace_buffer_start(preg, dreg)
+# define DEBUG_STOP_HWTRACE(preg, dreg) trace_buffer_stop(preg, dreg)
+
+#else
+# define DEBUG_START_HWTRACE(preg, dreg)
+# define DEBUG_STOP_HWTRACE(preg, dreg)
+#endif
+
+#endif /* __ASSEMBLY__ */
+
+#endif                          /* _BLACKFIN_TRACE_ */