diff --git a/xen/arch/arm/arm32/head.S b/xen/arch/arm/arm32/head.S
index 3a72195..348ca7d 100644
--- a/xen/arch/arm/arm32/head.S
+++ b/xen/arch/arm/arm32/head.S
@@ -45,7 +45,7 @@
  *   r3  -
  *   r4  -
  *   r5  -
- *   r6  -
+ *   r6  - identity map in place
  *   r7  - CPUID
  *   r8  - DTB address (boot CPU only)
  *   r9  - paddr(start)
@@ -250,6 +250,8 @@ cpu_init_done:
          * mapping. So each CPU must rebuild the page tables here with
          * the 1:1 in place. */
 
+        mov   r6, #0                /* r6 := identity map not yet in place */
+
         /* Write Xen's PT's paddr into the HTTBR */
         ldr   r4, =boot_pgtable
         add   r4, r4, r10            /* r4 := paddr (boot_pagetable) */
@@ -275,6 +277,7 @@ cpu_init_done:
         orr   r2, r2, #PT_LOWER(MEM)
         lsl   r1, r1, #3             /* r1 := Slot offset */
         strd  r2, r3, [r4, r1]       /* Mapping of paddr(start) */
+        mov   r6, #1                 /* r6 := identity map now in place */
 
 1:      /* Setup boot_second: */
         ldr   r4, =boot_second
@@ -290,6 +293,8 @@ cpu_init_done:
         strd  r2, r3, [r4, #8]       /* Map it in slot 1 */
 
         /* ... map of paddr(start) in boot_second */
+        cmp   r6, #1                 /* r6 is set if already created */
+        beq   1f
         lsrs  r1, r9, #SECOND_SHIFT  /* Offset of base paddr in boot_second */
         mov   r2, #0x0ff             /* r2 := LPAE entries mask */
         orr   r2, r2, #0x100
@@ -307,6 +312,7 @@ cpu_init_done:
         orr   r2, r2, #PT_LOWER(MEM)
         lsl   r1, r1, #3             /* r1 := Slot offset */
         strd  r2, r3, [r4, r1]       /* Mapping of paddr(start) */
+        mov   r6, #1                 /* r6 := identity map now in place */
 
         /* Setup boot_third: */
 1:      ldr   r4, =boot_third
@@ -330,6 +336,12 @@ cpu_init_done:
 
         /* boot pagetable setup complete */
 
+        cmp   r6, #1                /* Did we manage to create an identity mapping ? */
+        beq   1f
+        PRINT("Unable to build boot page tables - Failed to identity map Xen.\r\n")
+        b     fail
+
+1:
         PRINT("- Turning on paging -\r\n")
 
         ldr   r1, =paging            /* Explicit vaddr, not RIP-relative */
diff --git a/xen/arch/arm/arm64/head.S b/xen/arch/arm/arm64/head.S
index 023a9b9..7152d9c 100644
--- a/xen/arch/arm/arm64/head.S
+++ b/xen/arch/arm/arm64/head.S
@@ -62,7 +62,7 @@
  *  x22 - is_secondary_cpu
  *  x23 - UART address
  *  x24 - cpuid
- *  x25 -
+ *  x25 - identity map in place
  *  x26 -
  *  x27 -
  *  x28 -
@@ -253,6 +253,8 @@ skip_bss:
          * mapping. So each CPU must rebuild the page tables here with
          * the 1:1 in place. */
 
+        mov   x25, #0                /* x25 := identity map not yet in place */
+
         /* Write Xen's PT's paddr into TTBR0_EL2 */
         ldr   x4, =boot_pgtable
         add   x4, x4, x20            /* x4 := paddr (boot_pagetable) */
@@ -293,6 +295,7 @@ skip_bss:
 
         lsl   x1, x1, #3             /* x1 := Slot offset */
         str   x2, [x4, x1]           /* Mapping of paddr(start)*/
+        mov   x25, #1                /* x25 := identity map now in place */
 
 1:      /* Setup boot_first: */
         ldr   x4, =boot_first        /* Next level into boot_first */
@@ -306,6 +309,7 @@ skip_bss:
         str   x2, [x4, #0]           /* Map it in slot 0 */
 
         /* ... map of paddr(start) in boot_first */
+        cbnz  x25, 1f                /* x25 is set if already created */
         lsr   x2, x19, #FIRST_SHIFT  /* x2 := Offset of base paddr in boot_first */
         and   x1, x2, 0x1ff          /* x1 := Slot to use */
         cbz   x1, 1f                 /* It's in slot 0, map in boot_second */
@@ -315,6 +319,7 @@ skip_bss:
         orr   x2, x2, x3
         lsl   x1, x1, #3             /* x1 := Slot offset */
         str   x2, [x4, x1]           /* Create mapping of paddr(start)*/
+        mov   x25, #1                /* x25 := identity map now in place */
 
 1:      /* Setup boot_second: */
         ldr   x4, =boot_second       /* Next level into boot_second */
@@ -328,6 +333,7 @@ skip_bss:
         str   x2, [x4, #8]           /* Map it in slot 1 */
 
         /* ... map of paddr(start) in boot_second */
+        cbnz  x25, 1f                /* x25 is set if already created */
         lsr   x2, x19, #SECOND_SHIFT /* x2 := Offset of base paddr in boot_second */
         and   x1, x2, 0x1ff          /* x1 := Slot to use */
         cmp   x1, #1
@@ -343,6 +349,7 @@ skip_bss:
         orr   x2, x2, x3
         lsl   x1, x1, #3             /* x1 := Slot offset */
         str   x2, [x4, x1]           /* Create mapping of paddr(start)*/
+        mov   x25, #1                /* x25 := identity map now in place */
 
 1:      /* Setup boot_third: */
         ldr   x4, =boot_third
@@ -366,6 +373,11 @@ skip_bss:
 
         /* boot pagetable setup complete */
 
+        cbnz  x25, 1f                /* Did we manage to create an identity mapping ? */
+        PRINT("Unable to build boot page tables - Failed to identity map Xen.\r\n")
+        b     fail
+
+1:
         PRINT("- Turning on paging -\r\n")
 
         ldr   x1, =paging            /* Explicit vaddr, not RIP-relative */
