2 files changed, 86 insertions, 4 deletions

diff --git a/arch/arm/mm/idmap.c b/arch/arm/mm/idmap.c
index 83cb3ac27095..8e0e52eb76b5 100644
--- a/arch/arm/mm/idmap.c
+++ b/arch/arm/mm/idmap.c
@@ -10,6 +10,7 @@
 #include <asm/system_info.h>
 
 pgd_t *idmap_pgd;
+phys_addr_t (*arch_virt_to_idmap) (unsigned long x);
 
 #ifdef CONFIG_ARM_LPAE
 static void idmap_add_pmd(pud_t *pud, unsigned long addr, unsigned long end,
@@ -67,8 +68,9 @@ static void identity_mapping_add(pgd_t *pgd, const char *text_start,
         unsigned long addr, end;
         unsigned long next;
 
-        addr = virt_to_phys(text_start);
-        end = virt_to_phys(text_end);
+        addr = virt_to_idmap(text_start);
+        end = virt_to_idmap(text_end);
+        pr_info("Setting up static identity map for 0x%lx - 0x%lx\n", addr, end);
 
         prot |= PMD_TYPE_SECT | PMD_SECT_AP_WRITE | PMD_SECT_AF;
 
@@ -90,8 +92,6 @@ static int __init init_static_idmap(void)
         if (!idmap_pgd)
                 return -ENOMEM;
 
-        pr_info("Setting up static identity map for 0x%p - 0x%p\n",
-                __idmap_text_start, __idmap_text_end);
         identity_mapping_add(idmap_pgd, __idmap_text_start,
                              __idmap_text_end, 0);
 
diff --git a/arch/arm/mm/mmu.c b/arch/arm/mm/mmu.c
index b1d17eeb59b8..78eeeca78f5a 100644
--- a/arch/arm/mm/mmu.c
+++ b/arch/arm/mm/mmu.c
@@ -28,6 +28,8 @@
 #include <asm/highmem.h>
 #include <asm/system_info.h>
 #include <asm/traps.h>
+#include <asm/procinfo.h>
+#include <asm/memory.h>
 
 #include <asm/mach/arch.h>
 #include <asm/mach/map.h>
@@ -1315,6 +1317,86 @@ static void __init map_lowmem(void)
         }
 }
 
+#ifdef CONFIG_ARM_LPAE
+/*
+ * early_paging_init() recreates boot time page table setup, allowing machines
+ * to switch over to a high (>4G) address space on LPAE systems
+ */
+void __init early_paging_init(const struct machine_desc *mdesc,
+                              struct proc_info_list *procinfo)
+{
+        pmdval_t pmdprot = procinfo->__cpu_mm_mmu_flags;
+        unsigned long map_start, map_end;
+        pgd_t *pgd0, *pgdk;
+        pud_t *pud0, *pudk, *pud_start;
+        pmd_t *pmd0, *pmdk;
+        phys_addr_t phys;
+        int i;
+
+        if (!(mdesc->init_meminfo))
+                return;
+
+        /* remap kernel code and data */
+        map_start = init_mm.start_code;
+        map_end   = init_mm.brk;
+
+        /* get a handle on things... */
+        pgd0 = pgd_offset_k(0);
+        pud_start = pud0 = pud_offset(pgd0, 0);
+        pmd0 = pmd_offset(pud0, 0);
+
+        pgdk = pgd_offset_k(map_start);
+        pudk = pud_offset(pgdk, map_start);
+        pmdk = pmd_offset(pudk, map_start);
+
+        mdesc->init_meminfo();
+
+        /* Run the patch stub to update the constants */
+        fixup_pv_table(&__pv_table_begin,
+                (&__pv_table_end - &__pv_table_begin) << 2);
+
+        /*
+         * Cache cleaning operations for self-modifying code
+         * We should clean the entries by MVA but running a
+         * for loop over every pv_table entry pointer would
+         * just complicate the code.
+         */
+        flush_cache_louis();
+        dsb();
+        isb();
+
+        /* remap level 1 table */
+        for (i = 0; i < PTRS_PER_PGD; pud0++, i++) {
+                set_pud(pud0,
+                        __pud(__pa(pmd0) | PMD_TYPE_TABLE | L_PGD_SWAPPER));
+                pmd0 += PTRS_PER_PMD;
+        }
+
+        /* remap pmds for kernel mapping */
+        phys = __pa(map_start) & PMD_MASK;
+        do {
+                *pmdk++ = __pmd(phys | pmdprot);
+                phys += PMD_SIZE;
+        } while (phys < map_end);
+
+        flush_cache_all();
+        cpu_switch_mm(pgd0, &init_mm);
+        cpu_set_ttbr(1, __pa(pgd0) + TTBR1_OFFSET);
+        local_flush_bp_all();
+        local_flush_tlb_all();
+}
+
+#else
+
+void __init early_paging_init(const struct machine_desc *mdesc,
+                              struct proc_info_list *procinfo)
+{
+        if (mdesc->init_meminfo)
+                mdesc->init_meminfo();
+}
+
+#endif
+
 /*
  * paging_init() sets up the page tables, initialises the zone memory
  * maps, and sets up the zero page, bad page and bad page tables.