From 0c0b2837a7ceef00acdd253f524db0d08baa4075 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Thu, 22 Dec 2022 11:11:30 +0100
Subject: bsp/qoriq: Add qoriq_mmu_adjust_and_write_to_tlb1()

---
 bsps/powerpc/qoriq/include/bsp/mmu.h | 10 ++++++++++
 bsps/powerpc/qoriq/start/bspsmp.c    |  9 ++-------
 bsps/powerpc/qoriq/start/mmu.c       | 26 ++++++++++++++++++++++++++
 3 files changed, 38 insertions(+), 7 deletions(-)

(limited to 'bsps')

diff --git a/bsps/powerpc/qoriq/include/bsp/mmu.h b/bsps/powerpc/qoriq/include/bsp/mmu.h
index 2a69f683bc..52c4c36627 100644
--- a/bsps/powerpc/qoriq/include/bsp/mmu.h
+++ b/bsps/powerpc/qoriq/include/bsp/mmu.h
@@ -105,6 +105,16 @@ void qoriq_tlb1_write(
 	int tsize
 );
 
+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
+);
+
 void qoriq_tlb1_invalidate(int esel);
 
 /** @} */
diff --git a/bsps/powerpc/qoriq/start/bspsmp.c b/bsps/powerpc/qoriq/start/bspsmp.c
index 376854c5f3..ec261cb981 100644
--- a/bsps/powerpc/qoriq/start/bspsmp.c
+++ b/bsps/powerpc/qoriq/start/bspsmp.c
@@ -118,11 +118,8 @@ static void bsp_inter_processor_interrupt(void *arg)
 static void setup_boot_page(void)
 {
 #ifdef QORIQ_IS_HYPERVISOR_GUEST
-  qoriq_mmu_context mmu_context;
-
-  qoriq_mmu_context_init(&mmu_context);
-  qoriq_mmu_add(
-    &mmu_context,
+  qoriq_mmu_adjust_and_write_to_tlb1(
+    QORIQ_TLB1_ENTRY_COUNT - 1,
     0xfffff000,
     0xffffffff,
     0,
@@ -130,8 +127,6 @@ static void setup_boot_page(void)
     FSL_EIS_MAS3_SR | FSL_EIS_MAS3_SW,
     0
   );
-  qoriq_mmu_partition(&mmu_context, 1);
-  qoriq_mmu_write_to_tlb1(&mmu_context, QORIQ_TLB1_ENTRY_COUNT - 1);
 #endif
 }
 
diff --git a/bsps/powerpc/qoriq/start/mmu.c b/bsps/powerpc/qoriq/start/mmu.c
index e2b4009ac6..acd481fe12 100644
--- a/bsps/powerpc/qoriq/start/mmu.c
+++ b/bsps/powerpc/qoriq/start/mmu.c
@@ -406,3 +406,29 @@ int qoriq_mmu_find_free_tlb1_entry(void)
 
 	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);
+}
-- 
cgit v1.2.3