Blackfin arch: rewrite blackfin_invalidate_entire_dcache function

rewrite blackfin_invalidate_entire_dcache() in C for easier management,
better optimization, and so we take all SSYNC anomalies into account

Signed-off-by: Mike Frysinger <vapier.adi@gmail.com>
Signed-off-by: Bryan Wu <cooloney@kernel.org>

diff --git a/arch/blackfin/mach-common/Makefile b/arch/blackfin/mach-common/Makefile
index 9388b4a..38911c6 100644
--- a/arch/blackfin/mach-common/Makefile
+++ b/arch/blackfin/mach-common/Makefile
@@ -3,7 +3,7 @@
 #
 
 obj-y := \
-	cache.o entry.o head.o \
+	cache.o cache-c.o entry.o head.o \
 	interrupt.o irqpanic.o arch_checks.o ints-priority.o
 
 obj-$(CONFIG_BFIN_ICACHE_LOCK) += lock.o
diff --git a/arch/blackfin/mach-common/cache-c.c b/arch/blackfin/mach-common/cache-c.c
new file mode 100644
index 0000000..e6ab1f8
--- /dev/null
+++ b/arch/blackfin/mach-common/cache-c.c
@@ -0,0 +1,24 @@
+/*
+ * Blackfin cache control code (simpler control-style functions)
+ *
+ * Copyright 2004-2008 Analog Devices Inc.
+ *
+ * Enter bugs at http://blackfin.uclinux.org/
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <asm/blackfin.h>
+
+/* Invalidate the Entire Data cache by
+ * clearing DMC[1:0] bits
+ */
+void blackfin_invalidate_entire_dcache(void)
+{
+	u32 dmem = bfin_read_DMEM_CONTROL();
+	SSYNC();
+	bfin_write_DMEM_CONTROL(dmem & ~0xc);
+	SSYNC();
+	bfin_write_DMEM_CONTROL(dmem);
+	SSYNC();
+}
diff --git a/arch/blackfin/mach-common/cache.S b/arch/blackfin/mach-common/cache.S
index 1187512..3c98dac 100644
--- a/arch/blackfin/mach-common/cache.S
+++ b/arch/blackfin/mach-common/cache.S
@@ -97,39 +97,3 @@
 	P1 = 1 << (PAGE_SHIFT - L1_CACHE_SHIFT);
 	jump .Ldfr;
 ENDPROC(_blackfin_dflush_page)
-
-/* Invalidate the Entire Data cache by
- * clearing DMC[1:0] bits
- */
-ENTRY(_blackfin_invalidate_entire_dcache)
-	[--SP] = ( R7:5);
-
-	P0.L = LO(DMEM_CONTROL);
-	P0.H = HI(DMEM_CONTROL);
-	R7 = [P0];
-	R5 = R7;	/* Save DMEM_CNTR */
-
-	/* Clear the DMC[1:0] bits, All valid bits in the data
-	 * cache are set to the invalid state
-	 */
-	BITCLR(R7,DMC0_P);
-	BITCLR(R7,DMC1_P);
-	CLI R6;
-	SSYNC;		/* SSYNC required before writing to DMEM_CONTROL. */
-	.align 8;
-	[P0] = R7;
-	SSYNC;
-	STI R6;
-
-	/* Configures the data cache again */
-
-	CLI R6;
-	SSYNC;		/* SSYNC required before writing to DMEM_CONTROL. */
-	.align 8;
-	[P0] = R5;
-	SSYNC;
-	STI R6;
-
-	( R7:5) = [SP++];
-	RTS;
-ENDPROC(_blackfin_invalidate_entire_dcache)