1 files changed, 31 insertions, 15 deletions

diff --git a/arch/arm/mm/idmap.c b/arch/arm/mm/idmap.c
index 034124d1272d..57299446f787 100644
--- a/arch/arm/mm/idmap.c
+++ b/arch/arm/mm/idmap.c
@@ -4,33 +4,49 @@
 #include <asm/pgalloc.h>
 #include <asm/pgtable.h>
 
+static void idmap_add_pmd(pgd_t *pgd, unsigned long addr, unsigned long end,
+        unsigned long prot)
+{
+        pmd_t *pmd = pmd_offset(pgd, addr);
+
+        addr = (addr & PMD_MASK) | prot;
+        pmd[0] = __pmd(addr);
+        addr += SECTION_SIZE;
+        pmd[1] = __pmd(addr);
+        flush_pmd_entry(pmd);
+}
+
 void identity_mapping_add(pgd_t *pgd, unsigned long addr, unsigned long end)
 {
-        unsigned long prot;
+        unsigned long prot, next;
 
         prot = PMD_TYPE_SECT | PMD_SECT_AP_WRITE;
         if (cpu_architecture() <= CPU_ARCH_ARMv5TEJ && !cpu_is_xscale())
                 prot |= PMD_BIT4;
 
-        for (addr &= PGDIR_MASK; addr < end;) {
-                pmd_t *pmd = pmd_offset(pgd + pgd_index(addr), addr);
-                pmd[0] = __pmd(addr | prot);
-                addr += SECTION_SIZE;
-                pmd[1] = __pmd(addr | prot);
-                addr += SECTION_SIZE;
-                flush_pmd_entry(pmd);
-        }
+        pgd += pgd_index(addr);
+        do {
+                next = pgd_addr_end(addr, end);
+                idmap_add_pmd(pgd, addr, next, prot);
+        } while (pgd++, addr = next, addr != end);
 }
 
 #ifdef CONFIG_SMP
+static void idmap_del_pmd(pgd_t *pgd, unsigned long addr, unsigned long end)
+{
+        pmd_t *pmd = pmd_offset(pgd, addr);
+        pmd_clear(pmd);
+}
+
 void identity_mapping_del(pgd_t *pgd, unsigned long addr, unsigned long end)
 {
-        for (addr &= PGDIR_MASK; addr < end; addr += PGDIR_SIZE) {
-                pmd_t *pmd = pmd_offset(pgd + pgd_index(addr), addr);
-                pmd[0] = __pmd(0);
-                pmd[1] = __pmd(0);
-                clean_pmd_entry(pmd);
-        }
+        unsigned long next;
+
+        pgd += pgd_index(addr);
+        do {
+                next = pgd_addr_end(addr, end);
+                idmap_del_pmd(pgd, addr, next);
+        } while (pgd++, addr = next, addr != end);
 }
 #endif