[PATCH 4/5] bsp/qoriq: Add qoriq_mmu_find_free_tlb1_entry()

[Sebastian Huber]

---
 bsps/powerpc/qoriq/include/bsp/mmu.h |  2 ++
 bsps/powerpc/qoriq/start/mmu.c       | 26 +++++++++++++++++++++++++-
 2 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/bsps/powerpc/qoriq/include/bsp/mmu.h b/bsps/powerpc/qoriq/include/bsp/mmu.h
index e9aad505b5..2a69f683bc 100644
--- a/bsps/powerpc/qoriq/include/bsp/mmu.h
+++ b/bsps/powerpc/qoriq/include/bsp/mmu.h
@@ -91,6 +91,8 @@ void qoriq_mmu_write_to_tlb1(qoriq_mmu_context *self, int first_tlb);
 
 void qoriq_mmu_change_perm(uint32_t test, uint32_t set, uint32_t clear);
 
+int qoriq_mmu_find_free_tlb1_entry(void);
+
 void qoriq_mmu_config(bool boot_processor, int first_tlb, int scratch_tlb);
 
 void qoriq_tlb1_write(
diff --git a/bsps/powerpc/qoriq/start/mmu.c b/bsps/powerpc/qoriq/start/mmu.c
index fa24357852..e2b4009ac6 100644
--- a/bsps/powerpc/qoriq/start/mmu.c
+++ b/bsps/powerpc/qoriq/start/mmu.c
@@ -358,12 +358,14 @@ void qoriq_mmu_change_perm(uint32_t test, uint32_t set, uint32_t clear)
 {
 	int i = 0;
 
-	for (i = 0; i < 16; ++i) {
+	for (i = 0; i < QORIQ_TLB1_ENTRY_COUNT; ++i) {
 		uint32_t mas0 = FSL_EIS_MAS0_TLBSEL | FSL_EIS_MAS0_ESEL(i);
 		uint32_t mas1 = 0;
 
 		PPC_SET_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS0, mas0);
+		ppc_synchronize_instructions();
 		ppc_tlbre();
+		ppc_synchronize_instructions();
 
 		mas1 = PPC_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS1);
 		if ((mas1 & FSL_EIS_MAS1_V) != 0) {
@@ -382,3 +384,25 @@ void qoriq_mmu_change_perm(uint32_t test, uint32_t set, uint32_t clear)
 		}
 	}
 }
+
+int qoriq_mmu_find_free_tlb1_entry(void)
+{
+	int i = 0;
+
+	for (i = 0; i < QORIQ_TLB1_ENTRY_COUNT; ++i) {
+		uint32_t mas0 = FSL_EIS_MAS0_TLBSEL | FSL_EIS_MAS0_ESEL(i);
+		uint32_t mas1;
+
+		PPC_SET_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS0, mas0);
+		ppc_synchronize_instructions();
+		ppc_tlbre();
+		ppc_synchronize_instructions();
+
+		mas1 = PPC_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS1);
+		if ((mas1 & FSL_EIS_MAS1_V) == 0) {
+			return i;
+		}
+	}
+
+	return -1;
+}
-- 
2.35.3