From 2c68a47e08da568d8e94a2ee07936f480c8c75d7 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Fri, 22 Apr 2016 13:11:15 +0200
Subject: bsp/qoriq: Add qoriq_tlb1_invalidate_all_by_ts()

Generalize qoriq_tlb1_ts_0_only() to qoriq_tlb1_invalidate_all_by_ts().
---
 c/src/lib/libbsp/powerpc/qoriq/start/start.S      | 10 ++++++----
 c/src/lib/libbsp/powerpc/qoriq/startup/mmu-tlb1.S | 14 +++++++++-----
 2 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/c/src/lib/libbsp/powerpc/qoriq/start/start.S b/c/src/lib/libbsp/powerpc/qoriq/start/start.S
index 64f7f2ec91..959347c61e 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/start/start.S
+++ b/c/src/lib/libbsp/powerpc/qoriq/start/start.S
@@ -183,8 +183,11 @@ _start:
 .Linitmore:
 	mflr	SAVED_LINK_REGISTER
 
-	/* Initial MMU setup */
-	bl	qoriq_tlb1_ts_0_only
+	/* Invalidate all TS1 MMU entries */
+	li	r3, 1
+	bl	qoriq_tlb1_invalidate_all_by_ts
+
+	/* Add TS1 entry for the first 4GiB of RAM */
 	li	r3, SCRATCH_TLB
 	li	r4, FSL_EIS_MAS1_TS
 	li	r5, FSL_EIS_MAS2_I
@@ -194,7 +197,7 @@ _start:
 	li	r9, 11
 	bl	qoriq_tlb1_write
 
-	/* MSR initialization */
+	/* MSR initialization and use TS1 for address translation */
 	LWI	INITIAL_MSR, QORIQ_INITIAL_MSR
 	ori	r0, INITIAL_MSR, MSR_IS | MSR_DS
 	mtmsr	r0
@@ -228,7 +231,6 @@ _start:
 	mtlr	SAVED_LINK_REGISTER
 	blr
 
-
 #ifdef INITIALIZE_FPU
 	/*
 	 * Write a value to the FPRs to initialize the hidden tag bits.  See
diff --git a/c/src/lib/libbsp/powerpc/qoriq/startup/mmu-tlb1.S b/c/src/lib/libbsp/powerpc/qoriq/startup/mmu-tlb1.S
index 5d04c2f211..25ef05303c 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/startup/mmu-tlb1.S
+++ b/c/src/lib/libbsp/powerpc/qoriq/startup/mmu-tlb1.S
@@ -26,7 +26,7 @@
 
 	.global qoriq_tlb1_write
 	.global qoriq_tlb1_invalidate
-	.global qoriq_tlb1_ts_0_only
+	.global qoriq_tlb1_invalidate_all_by_ts
 
 	.section ".bsp_start_text", "ax"
 
@@ -70,19 +70,23 @@ qoriq_tlb1_invalidate:
 	isync
 	blr
 
-qoriq_tlb1_ts_0_only:
+/* r3 = 0 for TS0, 1 for TS1 */
+qoriq_tlb1_invalidate_all_by_ts:
 	mflr	r12
-	li	r11, 16
+	li	r11, QORIQ_TLB1_ENTRY_COUNT
 	mtctr	r11
 	li	r11, 0
+	mr	r10, r3
+
 2:
 	rlwinm	r0, r11, 16, 10, 15
 	oris	r0, r0, (FSL_EIS_MAS0_TLBSEL >> 16)
 	mtspr	FSL_EIS_MAS0, r0
 	tlbre
 	mfspr	r0, FSL_EIS_MAS1
-	andi.	r0, r0, FSL_EIS_MAS1_TS
-	beq	1f
+	rlwinm	r0, r0, 20, 31, 31
+	cmpw	r0, r10
+	bne	1f
 	mr	r3, r11
 	bl	qoriq_tlb1_invalidate
 1:
-- 
cgit v1.2.3