4 files changed, 176 insertions, 29 deletions

diff --git a/arch/blackfin/include/asm/ipipe_base.h b/arch/blackfin/include/asm/ipipe_base.h
index 3e8acbd1a3be..490098f532a7 100644
--- a/arch/blackfin/include/asm/ipipe_base.h
+++ b/arch/blackfin/include/asm/ipipe_base.h
@@ -51,23 +51,23 @@
 
 extern unsigned long __ipipe_root_status; /* Alias to ipipe_root_cpudom_var(status) */
 
-static inline void __ipipe_stall_root(void)
-{
-	volatile unsigned long *p = &__ipipe_root_status;
-	set_bit(0, p);
-}
+#define __ipipe_stall_root()						\
+	do {								\
+		volatile unsigned long *p = &__ipipe_root_status;	\
+		set_bit(0, p);						\
+	} while (0)
 
-static inline unsigned long __ipipe_test_and_stall_root(void)
-{
-	volatile unsigned long *p = &__ipipe_root_status;
-	return test_and_set_bit(0, p);
-}
+#define __ipipe_test_and_stall_root()					\
+	({								\
+		volatile unsigned long *p = &__ipipe_root_status;	\
+		test_and_set_bit(0, p);					\
+	})
 
-static inline unsigned long __ipipe_test_root(void)
-{
-	const unsigned long *p = &__ipipe_root_status;
-	return test_bit(0, p);
-}
+#define __ipipe_test_root()					\
+	({							\
+		const unsigned long *p = &__ipipe_root_status;	\
+		test_bit(0, p);					\
+	})
 
 #endif /* !__ASSEMBLY__ */
 
diff --git a/arch/blackfin/include/asm/irq.h b/arch/blackfin/include/asm/irq.h
index 9a7f63a83c47..42a15f5ce0d0 100644
--- a/arch/blackfin/include/asm/irq.h
+++ b/arch/blackfin/include/asm/irq.h
@@ -22,13 +22,6 @@
 /* SYS_IRQS and NR_IRQS are defined in <mach-bf5xx/irq.h> */
 #include <mach/irq.h>
 
-/* Xenomai IPIPE helpers */
-#define local_irq_restore_hw(x) local_irq_restore(x)
-#define local_irq_save_hw(x)    local_irq_save(x)
-#define local_irq_enable_hw(x)  local_irq_enable(x)
-#define local_irq_disable_hw(x) local_irq_disable(x)
-#define irqs_disabled_hw(x)     irqs_disabled(x)
-
 #if ANOMALY_05000244 && defined(CONFIG_BFIN_ICACHE)
 # define NOP_PAD_ANOMALY_05000244 "nop; nop;"
 #else
diff --git a/arch/blackfin/include/asm/irqflags.h b/arch/blackfin/include/asm/irqflags.h
index 139cba4651b1..9b19a19d9ae9 100644
--- a/arch/blackfin/include/asm/irqflags.h
+++ b/arch/blackfin/include/asm/irqflags.h
@@ -31,6 +31,150 @@ static inline unsigned long bfin_cli(void)
 	return flags;
 }
 
+#ifdef CONFIG_IPIPE
+
+#include <linux/ipipe_base.h>
+#include <linux/ipipe_trace.h>
+
+#ifdef CONFIG_DEBUG_HWERR
+# define bfin_no_irqs 0x3f
+#else
+# define bfin_no_irqs 0x1f
+#endif
+
+#define raw_local_irq_disable()				\
+	do {						\
+		ipipe_check_context(ipipe_root_domain);	\
+		__ipipe_stall_root();			\
+		barrier();				\
+	} while (0)
+
+static inline void raw_local_irq_enable(void)
+{
+	barrier();
+	ipipe_check_context(ipipe_root_domain);
+	__ipipe_unstall_root();
+}
+
+#define raw_local_save_flags_ptr(x)					\
+	do {								\
+		*(x) = __ipipe_test_root() ? bfin_no_irqs : bfin_irq_flags; \
+	} while (0)
+
+#define raw_local_save_flags(x)		raw_local_save_flags_ptr(&(x))
+
+#define raw_irqs_disabled_flags(x)	((x) == bfin_no_irqs)
+
+#define raw_local_irq_save_ptr(x)					\
+	do {								\
+		*(x) = __ipipe_test_and_stall_root() ? bfin_no_irqs : bfin_irq_flags; \
+		barrier();						\
+	} while (0)
+
+#define raw_local_irq_save(x)				\
+	do {						\
+		ipipe_check_context(ipipe_root_domain);	\
+		raw_local_irq_save_ptr(&(x));		\
+	} while (0)
+
+static inline unsigned long raw_mangle_irq_bits(int virt, unsigned long real)
+{
+	/*
+	 * Merge virtual and real interrupt mask bits into a single
+	 * 32bit word.
+	 */
+	return (real & ~(1 << 31)) | ((virt != 0) << 31);
+}
+
+static inline int raw_demangle_irq_bits(unsigned long *x)
+{
+	int virt = (*x & (1 << 31)) != 0;
+	*x &= ~(1L << 31);
+	return virt;
+}
+
+static inline void local_irq_disable_hw_notrace(void)
+{
+	bfin_cli();
+}
+
+static inline void local_irq_enable_hw_notrace(void)
+{
+	bfin_sti(bfin_irq_flags);
+}
+
+#define local_save_flags_hw(flags)			\
+	do {						\
+		(flags) = bfin_read_IMASK();		\
+	} while (0)
+
+#define irqs_disabled_flags_hw(flags) (((flags) & ~0x3f) == 0)
+
+#define irqs_disabled_hw()			\
+	({					\
+	unsigned long flags;			\
+	local_save_flags_hw(flags);		\
+	irqs_disabled_flags_hw(flags);		\
+	})
+
+static inline void local_irq_save_ptr_hw(unsigned long *flags)
+{
+	*flags = bfin_cli();
+#ifdef CONFIG_DEBUG_HWERR
+	bfin_sti(0x3f);
+#endif
+}
+
+#define local_irq_save_hw_notrace(flags)		\
+	do {						\
+		local_irq_save_ptr_hw(&(flags));	\
+	} while (0)
+
+static inline void local_irq_restore_hw_notrace(unsigned long flags)
+{
+	if (!irqs_disabled_flags_hw(flags))
+		local_irq_enable_hw_notrace();
+}
+
+#ifdef CONFIG_IPIPE_TRACE_IRQSOFF
+# define local_irq_disable_hw()				\
+	do {						\
+		if (!irqs_disabled_hw()) {		\
+			local_irq_disable_hw_notrace();	\
+			ipipe_trace_begin(0x80000000);	\
+		}					\
+	} while (0)
+# define local_irq_enable_hw()				\
+	do {						\
+		if (irqs_disabled_hw()) {		\
+			ipipe_trace_end(0x80000000);	\
+			local_irq_enable_hw_notrace();	\
+		}					\
+	} while (0)
+# define local_irq_save_hw(flags)			\
+	do {						\
+		local_save_flags_hw(flags);		\
+		if (!irqs_disabled_flags_hw(flags)) {	\
+			local_irq_disable_hw_notrace();	\
+			ipipe_trace_begin(0x80000001);	\
+		}					\
+	} while (0)
+# define local_irq_restore_hw(flags)			\
+	do {						\
+		if (!irqs_disabled_flags_hw(flags)) {	\
+			ipipe_trace_end(0x80000001);	\
+			local_irq_enable_hw_notrace();	\
+		}					\
+	} while (0)
+#else /* !CONFIG_IPIPE_TRACE_IRQSOFF */
+# define local_irq_disable_hw()		local_irq_disable_hw_notrace()
+# define local_irq_enable_hw()		local_irq_enable_hw_notrace()
+# define local_irq_save_hw(flags)	local_irq_save_hw_notrace(flags)
+# define local_irq_restore_hw(flags)	local_irq_restore_hw_notrace(flags)
+#endif /* !CONFIG_IPIPE_TRACE_IRQSOFF */
+
+#else /* CONFIG_IPIPE */
+
 static inline void raw_local_irq_disable(void)
 {
 	bfin_cli();
@@ -44,12 +188,6 @@ static inline void raw_local_irq_enable(void)
 
 #define raw_irqs_disabled_flags(flags) (((flags) & ~0x3f) == 0)
 
-static inline void raw_local_irq_restore(unsigned long flags)
-{
-	if (!raw_irqs_disabled_flags(flags))
-		raw_local_irq_enable();
-}
-
 static inline unsigned long __raw_local_irq_save(void)
 {
 	unsigned long flags = bfin_cli();
@@ -60,4 +198,18 @@ static inline unsigned long __raw_local_irq_save(void)
 }
 #define raw_local_irq_save(flags) do { (flags) = __raw_local_irq_save(); } while (0)
 
+#define local_irq_save_hw(flags)	raw_local_irq_save(flags)
+#define local_irq_restore_hw(flags)	raw_local_irq_restore(flags)
+#define local_irq_enable_hw()		raw_local_irq_enable()
+#define local_irq_disable_hw()		raw_local_irq_disable()
+#define irqs_disabled_hw()		irqs_disabled()
+
+#endif /* !CONFIG_IPIPE */
+
+static inline void raw_local_irq_restore(unsigned long flags)
+{
+	if (!raw_irqs_disabled_flags(flags))
+		raw_local_irq_enable();
+}
+
 #endif
diff --git a/arch/blackfin/include/asm/system.h b/arch/blackfin/include/asm/system.h
index 294dbda24164..85e8f16cf8c2 100644
--- a/arch/blackfin/include/asm/system.h
+++ b/arch/blackfin/include/asm/system.h
@@ -135,11 +135,13 @@ struct __xchg_dummy {
 };
 #define __xg(x) ((volatile struct __xchg_dummy *)(x))
 
+#include <mach/blackfin.h>
+
 static inline unsigned long __xchg(unsigned long x, volatile void *ptr,
 				   int size)
 {
 	unsigned long tmp = 0;
-	unsigned long flags = 0;
+	unsigned long flags;
 
 	local_irq_save_hw(flags);