MIPS: Join the coherent domain when a CM is present

MIPS Linux expects the bootloader to leave the boot CPU a member of the
coherent domain when running on a system with a CM, and we will need to
do so if we wish to make use of IOCUs to have cache-coherent DMA in
U-Boot (and on some systems there is no choice in that matter). When a
CM is present, join the coherent domain after completing cache
initialisation.

Signed-off-by: Paul Burton <paul.burton@imgtec.com>
diff --git a/arch/mips/include/asm/cm.h b/arch/mips/include/asm/cm.h
index 62ecef2..b9ab0c6 100644
--- a/arch/mips/include/asm/cm.h
+++ b/arch/mips/include/asm/cm.h
@@ -19,6 +19,7 @@
 #define GCR_L2_TAG_STATE_UPPER		0x060c
 #define GCR_L2_DATA			0x0610
 #define GCR_L2_DATA_UPPER		0x0614
+#define GCR_Cx_COHERENCE		0x2008
 
 /* GCR_REV CM versions */
 #define GCR_REV_CM3			0x0800
@@ -32,6 +33,10 @@
 #define GCR_L2_CONFIG_SETSZ_BITS	4
 #define GCR_L2_CONFIG_BYPASS		(1 << 20)
 
+/* GCR_Cx_COHERENCE */
+#define GCR_Cx_COHERENCE_DOM_EN		(0xff << 0)
+#define GCR_Cx_COHERENCE_EN		(0x1 << 0)
+
 #ifndef __ASSEMBLY__
 
 #include <asm/io.h>
diff --git a/arch/mips/lib/cache_init.S b/arch/mips/lib/cache_init.S
index ecb88e5..e61432e 100644
--- a/arch/mips/lib/cache_init.S
+++ b/arch/mips/lib/cache_init.S
@@ -378,6 +378,44 @@
 	ehb
 
 2:
+# ifdef CONFIG_MIPS_CM
+	/* Config3 must exist for a CM to be present */
+	mfc0		t0, CP0_CONFIG, 1
+	bgez		t0, 2f
+	mfc0		t0, CP0_CONFIG, 2
+	bgez		t0, 2f
+
+	/* Check Config3.CMGCR to determine CM presence */
+	mfc0		t0, CP0_CONFIG, 3
+	and		t0, t0, MIPS_CONF3_CMGCR
+	beqz		t0, 2f
+
+	/* Change Config.K0 to a coherent CCA */
+	mfc0		t0, CP0_CONFIG
+	li		t1, CONF_CM_CACHABLE_COW
+#if __mips_isa_rev >= 2
+	ins		t0, t1, 0, 3
+#else
+	ori		t0, t0, CONF_CM_CMASK
+	xori		t0, t0, CONF_CM_CMASK
+	or		t0, t0, t1
+#endif
+	mtc0		t0, CP0_CONFIG
+
+	/*
+	 * Join the coherent domain such that the caches of this core are kept
+	 * coherent with those of other cores.
+	 */
+	PTR_LI		t0, CKSEG1ADDR(CONFIG_MIPS_CM_BASE)
+	lw		t1, GCR_REV(t0)
+	li		t2, GCR_REV_CM3
+	li		t3, GCR_Cx_COHERENCE_EN
+	bge		t1, t2, 1f
+	li		t3, GCR_Cx_COHERENCE_DOM_EN
+1:	sw		t3, GCR_Cx_COHERENCE(t0)
+	ehb
+2:
+# endif
 #endif
 
 return: