1 files changed, 55 insertions, 4 deletions

diff --git a/bsps/powerpc/qoriq/start/mmu.c b/bsps/powerpc/qoriq/start/mmu.c
index fa24357852..706c9fd293 100644
--- a/bsps/powerpc/qoriq/start/mmu.c
+++ b/bsps/powerpc/qoriq/start/mmu.c
@@ -9,7 +9,7 @@
  */
 
 /*
- * Copyright (c) 2011, 2018 embedded brains GmbH.  All rights reserved.
+ * Copyright (C) 2011, 2018 embedded brains GmbH & Co. KG
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -358,17 +358,20 @@ 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);
+		PPC_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS1, mas1);
 		if ((mas1 & FSL_EIS_MAS1_V) != 0) {
 			uint32_t mask = 0x3ff;
-			uint32_t mas3 = PPC_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS3);
+			uint32_t mas3;
+			PPC_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS3, mas3);
 
 			if ((mas3 & mask) == test) {
 				mas3 &= ~(clear & mask);
@@ -382,3 +385,51 @@ 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();
+
+		PPC_SPECIAL_PURPOSE_REGISTER(FSL_EIS_MAS1, mas1);
+		if ((mas1 & FSL_EIS_MAS1_V) == 0) {
+			return i;
+		}
+	}
+
+	return -1;
+}
+
+void qoriq_mmu_adjust_and_write_to_tlb1(
+	int tlb,
+	uintptr_t begin,
+	uintptr_t last,
+	uint32_t mas1,
+	uint32_t mas2,
+	uint32_t mas3,
+	uint32_t mas7
+)
+{
+	qoriq_mmu_context context;
+
+	qoriq_mmu_context_init(&context);
+	qoriq_mmu_add(
+		&context,
+		begin,
+		last,
+		mas1,
+		mas2,
+		mas3,
+		mas7
+	);
+	qoriq_mmu_partition(&context, 1);
+	qoriq_mmu_write_to_tlb1(&context, tlb);
+}