[PATCHv2 09/10] arm64/mm: make __create_pgd_mapping() coped with pgtable's paddr

[Pingfan Liu]

This patch is the last preparation for calling __create_pgd_mapping()
from head.S.

Under mmu-offset situation, pud_t */pmd_t */pte_t * points to paddr.
During the building of pgtable, they should be carefully handled to
avoid the involvement of __va().

Signed-off-by: Pingfan Liu <kernelfans at gmail.com>
Cc: Catalin Marinas <catalin.marinas at arm.com>
Cc: Will Deacon <will at kernel.org>
Cc: Marc Zyngier <maz at kernel.org>
Cc: Kristina Martsenko <kristina.martsenko at arm.com>
Cc: James Morse <james.morse at arm.com>
Cc: Steven Price <steven.price at arm.com>
Cc: Jonathan Cameron <Jonathan.Cameron at huawei.com>
Cc: Pavel Tatashin <pasha.tatashin at soleen.com>
Cc: Anshuman Khandual <anshuman.khandual at arm.com>
Cc: Atish Patra <atish.patra at wdc.com>
Cc: Mike Rapoport <rppt at kernel.org>
Cc: Logan Gunthorpe <logang at deltatee.com>
Cc: Mark Brown <broonie at kernel.org>
To: linux-arm-kernel at lists.infradead.org
---
 arch/arm64/mm/mmu_include.c | 29 +++++++++++++++++++++++++----
 1 file changed, 25 insertions(+), 4 deletions(-)

diff --git a/arch/arm64/mm/mmu_include.c b/arch/arm64/mm/mmu_include.c
index 746cb2b502a3..c4ea00bae4df 100644
--- a/arch/arm64/mm/mmu_include.c
+++ b/arch/arm64/mm/mmu_include.c
@@ -54,6 +54,9 @@ static void init_pte(pmd_t *pmdp, unsigned long addr, unsigned long end,
 
 	if (likely(!(flags & NO_FIXMAP)))
 		ptep = pte_set_fixmap_offset(pmdp, addr);
+	else if (flags & BOOT_HEAD)
+		/* for head.S, there is no __va() */
+		ptep = (pte_t *)__pmd_to_phys(*pmdp) + pte_index(addr);
 	else
 		ptep = pte_offset_kernel(pmdp, addr);
 	do {
@@ -121,10 +124,19 @@ static void init_pmd(pud_t *pudp, unsigned long addr, unsigned long end,
 	unsigned long next;
 	pmd_t *pmdp;
 
-	if (likely(!(flags & NO_FIXMAP)))
+	if (likely(!(flags & NO_FIXMAP))) {
 		pmdp = pmd_set_fixmap_offset(pudp, addr);
-	else
+	} else if (flags & BOOT_HEAD) {
+#if CONFIG_PGTABLE_LEVELS > 2
+		/* for head.S, there is no __va() */
+		pmdp = (pmd_t *)__pud_to_phys(*pudp) + pmd_index(addr);
+#else
+		pmdp = (pmd_t *)pudp;
+#endif
+	} else {
 		pmdp = pmd_offset(pudp, addr);
+	}
+
 	do {
 		pmd_t old_pmd = READ_ONCE(*pmdp);
 
@@ -228,10 +240,19 @@ static void alloc_init_pud(pgd_t *pgdp, unsigned long addr, unsigned long end,
 	}
 	SAFE_BUG_ON(flags, p4d_bad(p4d));
 
-	if (likely(!(flags & NO_FIXMAP)))
+	if (likely(!(flags & NO_FIXMAP))) {
 		pudp = pud_set_fixmap_offset(p4dp, addr);
-	else
+	} else if (flags & BOOT_HEAD) {
+#if CONFIG_PGTABLE_LEVELS > 3
+		/* for head.S, there is no __va() */
+		pudp = (pud_t *)__p4d_to_phys(*p4dp) + pud_index(addr);
+#else
+		pudp = (pud_t *)p4dp;
+#endif
+	} else {
 		pudp = pud_offset(p4dp, addr);
+	}
+
 	do {
 		pud_t old_pud = READ_ONCE(*pudp);
 
-- 
2.29.2