1 files changed, 42 insertions, 0 deletions

diff --git a/arch/arm64/mm/mmu.c b/arch/arm64/mm/mmu.c
index a6885d896ab6..f4dd585898c5 100644
--- a/arch/arm64/mm/mmu.c
+++ b/arch/arm64/mm/mmu.c
@@ -25,6 +25,7 @@
 #include <linux/nodemask.h>
 #include <linux/memblock.h>
 #include <linux/fs.h>
+#include <linux/io.h>
 
 #include <asm/cputype.h>
 #include <asm/sections.h>
@@ -251,6 +252,47 @@ static void __init create_mapping(phys_addr_t phys, unsigned long virt,
         } while (pgd++, addr = next, addr != end);
 }
 
+#ifdef CONFIG_EARLY_PRINTK
+/*
+ * Create an early I/O mapping using the pgd/pmd entries already populated
+ * in head.S as this function is called too early to allocated any memory. The
+ * mapping size is 2MB with 4KB pages or 64KB or 64KB pages.
+ */
+void __iomem * __init early_io_map(phys_addr_t phys, unsigned long virt)
+{
+        unsigned long size, mask;
+        bool page64k = IS_ENABLED(ARM64_64K_PAGES);
+        pgd_t *pgd;
+        pud_t *pud;
+        pmd_t *pmd;
+        pte_t *pte;
+
+        /*
+         * No early pte entries with !ARM64_64K_PAGES configuration, so using
+         * sections (pmd).
+         */
+        size = page64k ? PAGE_SIZE : SECTION_SIZE;
+        mask = ~(size - 1);
+
+        pgd = pgd_offset_k(virt);
+        pud = pud_offset(pgd, virt);
+        if (pud_none(*pud))
+                return NULL;
+        pmd = pmd_offset(pud, virt);
+
+        if (page64k) {
+                if (pmd_none(*pmd))
+                        return NULL;
+                pte = pte_offset_kernel(pmd, virt);
+                set_pte(pte, __pte((phys & mask) | PROT_DEVICE_nGnRE));
+        } else {
+                set_pmd(pmd, __pmd((phys & mask) | PROT_SECT_DEVICE_nGnRE));
+        }
+
+        return (void __iomem *)((virt & mask) + (phys & ~mask));
+}
+#endif
+
 static void __init map_mem(void)
 {
         struct memblock_region *reg;