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

@@ -167,6 +167,37 @@ static void set_pte_table(u64 *pte, u64 *table)
 	*pte = PTE_TYPE_TABLE | (ulong)table;
 }
 
+/* Splits a block PTE into table with subpages spanning the old block */
+static void split_block(u64 *pte, int level)
+{
+	u64 old_pte = *pte;
+	u64 *new_table;
+	u64 i = 0;
+	/* level describes the parent level, we need the child ones */
+	int levelshift = level2shift(level + 1);
+
+	if (pte_type(pte) != PTE_TYPE_BLOCK)
+		panic("PTE %p (%llx) is not a block. Some driver code wants to "
+		      "modify dcache settings for an range not covered in "
+		      "mem_map.", pte, old_pte);
+
+	new_table = create_table();
+	debug("Splitting pte %p (%llx) into %p\n", pte, old_pte, new_table);
+
+	for (i = 0; i < MAX_PTE_ENTRIES; i++) {
+		new_table[i] = old_pte | (i << levelshift);
+
+		/* Level 3 block PTEs have the table type */
+		if ((level + 1) == 3)
+			new_table[i] |= PTE_TYPE_TABLE;
+
+		debug("Setting new_table[%lld] = %llx\n", i, new_table[i]);
+	}
+
+	/* Set the new table into effect */
+	set_pte_table(pte, new_table);
+}
+
 /* Add one mm_region map entry to the page tables */
 static void add_map(struct mm_region *map)
 {
@@ -188,6 +219,8 @@ static void add_map(struct mm_region *map)
 
 		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);
@@ -199,48 +232,21 @@ static void add_map(struct mm_region *map)
 				addr += blocksize;
 				size -= blocksize;
 				break;
-			} else if ((pte_type(pte) == PTE_TYPE_FAULT)) {
+			} 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);
 			}
 		}
 	}
 }
 
-/* Splits a block PTE into table with subpages spanning the old block */
-static void split_block(u64 *pte, int level)
-{
-	u64 old_pte = *pte;
-	u64 *new_table;
-	u64 i = 0;
-	/* level describes the parent level, we need the child ones */
-	int levelshift = level2shift(level + 1);
-
-	if (pte_type(pte) != PTE_TYPE_BLOCK)
-		panic("PTE %p (%llx) is not a block. Some driver code wants to "
-		      "modify dcache settings for an range not covered in "
-		      "mem_map.", pte, old_pte);
-
-	new_table = create_table();
-	debug("Splitting pte %p (%llx) into %p\n", pte, old_pte, new_table);
-
-	for (i = 0; i < MAX_PTE_ENTRIES; i++) {
-		new_table[i] = old_pte | (i << levelshift);
-
-		/* Level 3 block PTEs have the table type */
-		if ((level + 1) == 3)
-			new_table[i] |= PTE_TYPE_TABLE;
-
-		debug("Setting new_table[%lld] = %llx\n", i, new_table[i]);
-	}
-
-	/* Set the new table into effect */
-	set_pte_table(pte, new_table);
-}
-
 enum pte_type {
 	PTE_INVAL,
 	PTE_BLOCK,