[U-Boot] [PATCH] 85xx: Fix enabling of L1 cache parity on secondary cores

[Kumar Gala]

Use the same code between primary and secondary cores to init the
L1 cache.  We were not enabling cache parity on the secondary cores.

Also, reworked the L1 cache init code to match the e500mc L2 init code
that first invalidates the cache and locks.  Than enables the cache and
makes sure its enabled before continuing.

Signed-off-by: Kumar Gala <galak at kernel.crashing.org>
---
 cpu/mpc85xx/release.S       |   38 ++++++++++++++++++++++++++++++--------
 cpu/mpc85xx/start.S         |   38 +++++++++++++++++++++++++++++++++-----
 include/asm-ppc/processor.h |    2 ++
 3 files changed, 65 insertions(+), 13 deletions(-)

diff --git a/cpu/mpc85xx/release.S b/cpu/mpc85xx/release.S
index 00c4c54..dab784e 100644
--- a/cpu/mpc85xx/release.S
+++ b/cpu/mpc85xx/release.S
@@ -70,18 +70,40 @@ __secondary_start_page:
 	mttbu	r3
 
 	/* Enable/invalidate the I-Cache */
-	mfspr	r0,SPRN_L1CSR1
-	ori	r0,r0,(L1CSR1_ICFI|L1CSR1_ICE)
-	mtspr	SPRN_L1CSR1,r0
+	lis	r2,(L1CSR1_ICFI|L1CSR1_ICLFR)@h
+	ori	r2,r2,(L1CSR1_ICFI|L1CSR1_ICLFR)@l
+	mtspr	SPRN_L1CSR1,r2
+1:
+	mfspr	r3,SPRN_L1CSR1
+	and.	r1,r3,r2
+	bne	1b
+
+	lis	r3,(L1CSR1_CPE|L1CSR1_ICE)@h
+	ori	r3,r3,(L1CSR1_CPE|L1CSR1_ICE)@l
+	mtspr	SPRN_L1CSR1,r3
 	isync
+2:
+	mfspr	r3,SPRN_L1CSR1
+	andi.	r1,r3,L1CSR1_ICE at l
+	beq	2b
 
 	/* Enable/invalidate the D-Cache */
-	mfspr	r0,SPRN_L1CSR0
-	ori	r0,r0,(L1CSR0_DCFI|L1CSR0_DCE)
-	msync
-	isync
-	mtspr	SPRN_L1CSR0,r0
+	lis	r2,(L1CSR0_DCFI|L1CSR0_DCLFR)@h
+	ori	r2,r2,(L1CSR0_DCFI|L1CSR0_DCLFR)@l
+	mtspr	SPRN_L1CSR0,r2
+1:
+	mfspr	r3,SPRN_L1CSR0
+	and.	r1,r3,r2
+	bne	1b
+
+	lis	r3,(L1CSR0_CPE|L1CSR0_DCE)@h
+	ori	r3,r3,(L1CSR0_CPE|L1CSR0_DCE)@l
+	mtspr	SPRN_L1CSR0,r3
 	isync
+2:
+	mfspr	r3,SPRN_L1CSR0
+	andi.	r1,r3,L1CSR0_DCE at l
+	beq	2b
 
 #define toreset(x) (x - __secondary_start_page + 0xfffff000)
 
diff --git a/cpu/mpc85xx/start.S b/cpu/mpc85xx/start.S
index 386fa81..af18c1c 100644
--- a/cpu/mpc85xx/start.S
+++ b/cpu/mpc85xx/start.S
@@ -108,13 +108,41 @@ _start_e500:
 	mtspr	L1CSR2,r2
 #endif
 
-	lis	r2,L1CSR0_CPE at H	/* enable parity */
-	ori	r2,r2,L1CSR0_DCE
-	mtspr	L1CSR0,r2	/* enable L1 Dcache */
+	/* Enable/invalidate the I-Cache */
+	lis	r2,(L1CSR1_ICFI|L1CSR1_ICLFR)@h
+	ori	r2,r2,(L1CSR1_ICFI|L1CSR1_ICLFR)@l
+	mtspr	SPRN_L1CSR1,r2
+1:
+	mfspr	r3,SPRN_L1CSR1
+	and.	r1,r3,r2
+	bne	1b
+
+	lis	r3,(L1CSR1_CPE|L1CSR1_ICE)@h
+	ori	r3,r3,(L1CSR1_CPE|L1CSR1_ICE)@l
+	mtspr	SPRN_L1CSR1,r3
 	isync
-	mtspr	L1CSR1,r2	/* enable L1 Icache */
+2:
+	mfspr	r3,SPRN_L1CSR1
+	andi.	r1,r3,L1CSR1_ICE at l
+	beq	2b
+
+	/* Enable/invalidate the D-Cache */
+	lis	r2,(L1CSR0_DCFI|L1CSR0_DCLFR)@h
+	ori	r2,r2,(L1CSR0_DCFI|L1CSR0_DCLFR)@l
+	mtspr	SPRN_L1CSR0,r2
+1:
+	mfspr	r3,SPRN_L1CSR0
+	and.	r1,r3,r2
+	bne	1b
+
+	lis	r3,(L1CSR0_CPE|L1CSR0_DCE)@h
+	ori	r3,r3,(L1CSR0_CPE|L1CSR0_DCE)@l
+	mtspr	SPRN_L1CSR0,r3
 	isync
-	msync
+2:
+	mfspr	r3,SPRN_L1CSR0
+	andi.	r1,r3,L1CSR0_DCE at l
+	beq	2b
 
 	/* Setup interrupt vectors */
 	lis	r1,TEXT_BASE at h
diff --git a/include/asm-ppc/processor.h b/include/asm-ppc/processor.h
index c6da411..9b3d616 100644
--- a/include/asm-ppc/processor.h
+++ b/include/asm-ppc/processor.h
@@ -485,10 +485,12 @@
 #define SPRN_L2CFG0	0x207	/* L2 Cache Configuration Register 0 */
 #define SPRN_L1CSR0	0x3f2	/* L1 Data Cache Control and Status Register 0 */
 #define   L1CSR0_CPE		0x00010000	/* Data Cache Parity Enable */
+#define   L1CSR0_DCLFR		0x00000100	/* D-Cache Lock Flash Reset */
 #define   L1CSR0_DCFI		0x00000002	/* Data Cache Flash Invalidate */
 #define   L1CSR0_DCE		0x00000001	/* Data Cache Enable */
 #define SPRN_L1CSR1	0x3f3	/* L1 Instruction Cache Control and Status Register 1 */
 #define   L1CSR1_CPE		0x00010000	/* Instruction Cache Parity Enable */
+#define   L1CSR1_ICLFR		0x00000100	/* I-Cache Lock Flash Reset */
 #define   L1CSR1_ICFI		0x00000002	/* Instruction Cache Flash Invalidate */
 #define   L1CSR1_ICE		0x00000001	/* Instruction Cache Enable */
 #define SPRN_L1CSR2	0x25e	/* L1 Data Cache Control and Status Register 2 */
-- 
1.6.0.6