From f733d46620d0efb93091f147f81a4bf9588fad3f Mon Sep 17 00:00:00 2001
From: York Sun <york.sun@nxp.com>
Date: Fri, 24 Jun 2016 16:46:21 -0700
Subject: [PATCH] armv8: mmu: split block if necessary

When page tables are created, allow later table to be created on
previous block entry. Splitting block feature is already working
with current code. This patch only rearranges the code order and
adds one condition to call split_block().

Signed-off-by: York Sun <york.sun@nxp.com>
---
 arch/arm/cpu/armv8/cache_v8.c | 92 +++++++++++++++++++----------------
 1 file changed, 49 insertions(+), 43 deletions(-)

diff --git a/arch/arm/cpu/armv8/cache_v8.c b/arch/arm/cpu/armv8/cache_v8.c
index b8867a79ce..8604035e14 100644
--- a/arch/arm/cpu/armv8/cache_v8.c
+++ b/arch/arm/cpu/armv8/cache_v8.c
@@ -167,49 +167,6 @@ static void set_pte_table(u64 *pte, u64 *table)
 	*pte = PTE_TYPE_TABLE | (ulong)table;
 }
 
-/* Add one mm_region map entry to the page tables */
-static void add_map(struct mm_region *map)
-{
-	u64 *pte;
-	u64 addr = map->base;
-	u64 size = map->size;
-	u64 attrs = map->attrs | PTE_TYPE_BLOCK | PTE_BLOCK_AF;
-	u64 blocksize;
-	int level;
-	u64 *new_table;
-
-	while (size) {
-		pte = find_pte(addr, 0);
-		if (pte && (pte_type(pte) == PTE_TYPE_FAULT)) {
-			debug("Creating table for addr 0x%llx\n", addr);
-			new_table = create_table();
-			set_pte_table(pte, new_table);
-		}
-
-		for (level = 1; level < 4; level++) {
-			pte = find_pte(addr, level);
-			blocksize = 1ULL << level2shift(level);
-			debug("Checking if pte fits for addr=%llx size=%llx "
-			      "blocksize=%llx\n", addr, size, blocksize);
-			if (size >= blocksize && !(addr & (blocksize - 1))) {
-				/* Page fits, create block PTE */
-				debug("Setting PTE %p to block addr=%llx\n",
-				      pte, addr);
-				*pte = addr | attrs;
-				addr += blocksize;
-				size -= blocksize;
-				break;
-			} else if ((pte_type(pte) == PTE_TYPE_FAULT)) {
-				/* Page doesn't fit, create subpages */
-				debug("Creating subtable for addr 0x%llx "
-				      "blksize=%llx\n", addr, blocksize);
-				new_table = create_table();
-				set_pte_table(pte, new_table);
-			}
-		}
-	}
-}
-
 /* Splits a block PTE into table with subpages spanning the old block */
 static void split_block(u64 *pte, int level)
 {
@@ -241,6 +198,55 @@ static void split_block(u64 *pte, int level)
 	set_pte_table(pte, new_table);
 }
 
+/* Add one mm_region map entry to the page tables */
+static void add_map(struct mm_region *map)
+{
+	u64 *pte;
+	u64 addr = map->base;
+	u64 size = map->size;
+	u64 attrs = map->attrs | PTE_TYPE_BLOCK | PTE_BLOCK_AF;
+	u64 blocksize;
+	int level;
+	u64 *new_table;
+
+	while (size) {
+		pte = find_pte(addr, 0);
+		if (pte && (pte_type(pte) == PTE_TYPE_FAULT)) {
+			debug("Creating table for addr 0x%llx\n", addr);
+			new_table = create_table();
+			set_pte_table(pte, new_table);
+		}
+
+		for (level = 1; level < 4; level++) {
+			pte = find_pte(addr, level);
+			if (!pte)
+				panic("pte not found\n");
+			blocksize = 1ULL << level2shift(level);
+			debug("Checking if pte fits for addr=%llx size=%llx "
+			      "blocksize=%llx\n", addr, size, blocksize);
+			if (size >= blocksize && !(addr & (blocksize - 1))) {
+				/* Page fits, create block PTE */
+				debug("Setting PTE %p to block addr=%llx\n",
+				      pte, addr);
+				*pte = addr | attrs;
+				addr += blocksize;
+				size -= blocksize;
+				break;
+			} else if (pte_type(pte) == PTE_TYPE_FAULT) {
+				/* Page doesn't fit, create subpages */
+				debug("Creating subtable for addr 0x%llx "
+				      "blksize=%llx\n", addr, blocksize);
+				new_table = create_table();
+				set_pte_table(pte, new_table);
+			} else if (pte_type(pte) == PTE_TYPE_BLOCK) {
+				debug("Split block into subtable for addr 0x%llx blksize=0x%llx\n",
+				      addr, blocksize);
+				split_block(pte, level);
+			}
+		}
+	}
+}
+
 enum pte_type {
 	PTE_INVAL,
 	PTE_BLOCK,