1 files changed, 38 insertions, 4 deletions

diff --git a/arch/sh/kernel/head_32.S b/arch/sh/kernel/head_32.S
index 79ff39517f8e..fe0b743881b0 100644
--- a/arch/sh/kernel/head_32.S
+++ b/arch/sh/kernel/head_32.S
@@ -85,7 +85,7 @@ ENTRY(_stext)
         ldc     r0, r7_bank     ! ... and initial thread_info
 #endif
 
-#if defined(CONFIG_PMB) && !defined(CONFIG_PMB_LEGACY)
+#ifdef CONFIG_PMB
 /*
  * Reconfigure the initial PMB mappings setup by the hardware.
  *
@@ -139,7 +139,6 @@ ENTRY(_stext)
         mov.l   r0, @r1
 
         mov.l   .LMEMORY_SIZE, r5
-        mov     r5, r7
 
         mov     #PMB_E_SHIFT, r0
         mov     #0x1, r4
@@ -150,6 +149,40 @@ ENTRY(_stext)
         mov.l   .LFIRST_ADDR_ENTRY, r2
         mov.l   .LPMB_ADDR, r3
 
+        /*
+         * First we need to walk the PMB and figure out if there are any
+         * existing mappings that match the initial mappings VPN/PPN.
+         * If these have already been established by the bootloader, we
+         * don't bother setting up new entries here, and let the late PMB
+         * initialization take care of things instead.
+         *
+         * Note that we may need to coalesce and merge entries in order
+         * to reclaim more available PMB slots, which is much more than
+         * we want to do at this early stage.
+         */
+        mov     #0, r10
+        mov     #NR_PMB_ENTRIES, r9
+
+        mov     r1, r7          /* temporary PMB_DATA iter */
+
+.Lvalidate_existing_mappings:
+
+        mov.l   @r7, r8
+        and     r0, r8
+        cmp/eq  r0, r8          /* Check for valid __MEMORY_START mappings */
+        bt      .Lpmb_done
+
+        add     #1, r10         /* Increment the loop counter */
+        cmp/eq  r9, r10
+        bf/s    .Lvalidate_existing_mappings
+         add    r4, r7          /* Increment to the next PMB_DATA entry */
+
+        /*
+         * If we've fallen through, continue with setting up the initial
+         * mappings.
+         */
+
+        mov     r5, r7          /* cached_to_uncached */
         mov     #0, r10
 
 #ifdef CONFIG_UNCACHED_MAPPING
@@ -252,7 +285,8 @@ ENTRY(_stext)
         mov.l   6f, r0
         icbi    @r0
 
-#endif /* !CONFIG_PMB_LEGACY */
+.Lpmb_done:
+#endif /* CONFIG_PMB */
 
 #ifndef CONFIG_SH_NO_BSS_INIT
         /*
@@ -304,7 +338,7 @@ ENTRY(stack_start)
 6:      .long   sh_cpu_init
 7:      .long   init_thread_union
 
-#if defined(CONFIG_PMB) && !defined(CONFIG_PMB_LEGACY)
+#ifdef CONFIG_PMB
 .LPMB_ADDR:             .long   PMB_ADDR
 .LPMB_DATA:             .long   PMB_DATA
 .LFIRST_ADDR_ENTRY:     .long   PAGE_OFFSET | PMB_V