From 45187e2f9c73ea96e69d1f46f0c24ff31d7f9298 Mon Sep 17 00:00:00 2001
From: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
Date: Tue, 28 Sep 2021 11:25:25 +0100
Subject: [PATCH 2/2] Extend Stage 1 mapping limit

As Stage 1 mappings are limited to 512GB, registering FF-A RxTx buffers
fails when the physical address of these buffers exceeds 512GB. This
fix removes the limitation and allows Stage 1 mapping up to the
supported PA range.

Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
Change-Id: I4cb8d68fc18e0edf4a7ee06ae636849d552d72a9
Upstream-Status: Pending [Not submitted to upstream yet]
---
 inc/hf/arch/mm.h      |  7 ++++++-
 src/arch/aarch64/mm.c | 38 ++++++++++++++++++++++++++------------
 src/arch/fake/mm.c    |  9 +++++++--
 src/mm.c              |  8 +++++++-
 4 files changed, 46 insertions(+), 16 deletions(-)

diff --git a/inc/hf/arch/mm.h b/inc/hf/arch/mm.h
index ef64dd3..d277369 100644
--- a/inc/hf/arch/mm.h
+++ b/inc/hf/arch/mm.h
@@ -162,7 +162,12 @@ uint32_t arch_mm_stage1_attrs_to_mode(uint64_t attrs);
 /**
  * Initializes the arch specific memory management.
  */
-bool arch_mm_init(paddr_t table);
+bool arch_mm_init(void);
+
+/**
+ * Set ptable base address.
+ */
+void arch_mm_set_ptable(paddr_t table);
 
 /**
  * Return the arch specific mm mode for send/recv pages of given VM ID.
diff --git a/src/arch/aarch64/mm.c b/src/arch/aarch64/mm.c
index 7256c28..0ee0bb6 100644
--- a/src/arch/aarch64/mm.c
+++ b/src/arch/aarch64/mm.c
@@ -141,6 +141,7 @@ struct arch_mm_config {
 	uintreg_t hcr_el2;
 } arch_mm_config;
 
+static uint8_t mm_s1_max_level;
 static uint8_t mm_s2_max_level;
 static uint8_t mm_s2_root_table_count;
 
@@ -676,12 +677,7 @@ uint32_t arch_mm_stage2_attrs_to_mode(uint64_t attrs)
 
 uint8_t arch_mm_stage1_max_level(void)
 {
-	/*
-	 * For stage 1 we hard-code this to 2 for now so that we can
-	 * save one page table level at the expense of limiting the
-	 * physical memory to 512GB.
-	 */
-	return 2;
+	return mm_s1_max_level;
 }
 
 uint8_t arch_mm_stage2_max_level(void)
@@ -735,10 +731,16 @@ uint64_t arch_mm_combine_table_entry_attrs(uint64_t table_attrs,
 	return block_attrs;
 }
 
+void
+arch_mm_set_ptable(paddr_t table)
+{
+	arch_mm_config.ttbr0_el2 = pa_addr(table);
+}
+
 /**
  * This is called early in initialization without MMU or caches enabled.
  */
-bool arch_mm_init(paddr_t table)
+bool arch_mm_init(void)
 {
 	static const int pa_bits_table[16] = {32, 36, 40, 42, 44, 48};
 	uint64_t features = read_msr(id_aa64mmfr0_el1);
@@ -784,6 +786,13 @@ bool arch_mm_init(paddr_t table)
 		mm_s2_max_level = 1;
 	}
 
+	if (pa_bits >= 40) {
+		mm_s1_max_level = 3;
+	} else {
+		/* Setting to 2 covers physical memory upto 512GB */
+		mm_s1_max_level = 2;
+	}
+
 	/*
 	 * Since the shallowest possible tree is used, the maximum number of
 	 * concatenated tables must be used. This means if no more than 4 bits
@@ -800,6 +809,10 @@ bool arch_mm_init(paddr_t table)
 		"Stage 2 has %d page table levels with %d pages at the root.\n",
 		mm_s2_max_level + 1, mm_s2_root_table_count);
 
+	dlog_info("Stage 1 has %d page table levels with %d pages at the "
+		  "root.\n", mm_s1_max_level + 1,
+		  arch_mm_stage1_root_table_count());
+
 	/*
 	 * If the PE implements S-EL2 then VTCR_EL2.NSA/NSW bits are significant
 	 * in secure state. In non-secure state, NSA/NSW behave as if set to
@@ -818,8 +831,6 @@ bool arch_mm_init(paddr_t table)
 	}
 
 	arch_mm_config = (struct arch_mm_config){
-		.ttbr0_el2 = pa_addr(table),
-
 		.vtcr_el2 =
 			(1U << 31) |		   /* RES1. */
 			(nsa_nsw << 29) |	   /* NSA/NSW. */
@@ -879,14 +890,16 @@ bool arch_mm_init(paddr_t table)
 			 << 24) | /* IRGN1, normal mem, WB RA WA Cacheable. */
 			(1UL << 23) | /* EPD1 - Disable TTBR1_EL2 translation */
 			(0UL << 22) | /* TTBR0_EL2.ASID defines ASID */
-			(25UL << 16) | /* T1SZ, input address is 2^39 bytes. */
+			((64 - pa_bits) << 16) | /* T1SZ, input address is *
+						    2^pa_bits bytes. */
 			(0UL << 14) |  /* TG0, granule size, 4KB. */
 			(3UL << 12) |  /* SH0, inner shareable. */
 			(1UL
 			 << 10) | /* ORGN0, normal mem, WB RA WA Cacheable. */
 			(1UL
 			 << 8) | /* IRGN0, normal mem, WB RA WA Cacheable. */
-			(25UL << 0) | /* T0SZ, input address is 2^39 bytes. */
+			((64 - pa_bits) << 0) | /* T0SZ, input address is *
+						   2^pa_bits bytes. */
 			0;
 	} else {
 		arch_mm_config.tcr_el2 =
@@ -896,7 +909,8 @@ bool arch_mm_init(paddr_t table)
 			(3 << 12) |		   /* SH0, inner shareable. */
 			(1 << 10) | /* ORGN0, normal mem, WB RA WA Cacheable. */
 			(1 << 8) |  /* IRGN0, normal mem, WB RA WA Cacheable. */
-			(25 << 0) | /* T0SZ, input address is 2^39 bytes. */
+			((64 - pa_bits) << 0) | /* T0SZ, input address is *
+						   2^pa_bits bytes. */
 			0;
 	}
 	return true;
diff --git a/src/arch/fake/mm.c b/src/arch/fake/mm.c
index e06a8b7..8ac63fa 100644
--- a/src/arch/fake/mm.c
+++ b/src/arch/fake/mm.c
@@ -161,13 +161,18 @@ uint32_t arch_mm_stage1_attrs_to_mode(uint64_t attrs)
 	return attrs >> PTE_ATTR_MODE_SHIFT;
 }
 
-bool arch_mm_init(paddr_t table)
+bool arch_mm_init(void)
 {
 	/* No initialization required. */
-	(void)table;
 	return true;
 }
 
+void arch_mm_set_ptable(paddr_t table)
+{
+	/* No initialization required. */
+	(void)table;
+}
+
 uint32_t arch_mm_extra_attributes_from_vm(ffa_vm_id_t id)
 {
 	(void)id;
diff --git a/src/mm.c b/src/mm.c
index c57e24e..aa0c512 100644
--- a/src/mm.c
+++ b/src/mm.c
@@ -1114,6 +1114,10 @@ bool mm_init(struct mpool *ppool)
 	dlog_info("data: %#x - %#x\n", pa_addr(layout_data_begin()),
 		  pa_addr(layout_data_end()));
 
+	if (!arch_mm_init()) {
+		return false;
+	}
+
 	/* ASID 0 is reserved for use by the hypervisor. */
 	if (!mm_ptable_init(&ptable, 0, MM_FLAG_STAGE1, ppool)) {
 		dlog_error("Unable to allocate memory for page table.\n");
@@ -1133,5 +1137,7 @@ bool mm_init(struct mpool *ppool)
 	mm_identity_map(stage1_locked, layout_data_begin(), layout_data_end(),
 			MM_MODE_R | MM_MODE_W, ppool);
 
-	return arch_mm_init(ptable.root);
+	arch_mm_set_ptable(ptable.root);
+
+	return true;
 }
-- 
2.30.2

