diff --git a/arch/arm/mm/idmap.c b/arch/arm/mm/idmap.c
index 034124d1272d26f6da8f423b0b7282b492c0345c..57299446f7871d8b77adacdc27f7644b5e4c2168 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