2 files changed, 47 insertions, 13 deletions

diff --git a/arch/arm/kernel/head.S b/arch/arm/kernel/head.S
index c0225da3fb21..f06ff9feb0db 100644
--- a/arch/arm/kernel/head.S
+++ b/arch/arm/kernel/head.S
@@ -391,6 +391,7 @@ ENDPROC(__turn_mmu_on)
 
 
 #ifdef CONFIG_SMP_ON_UP
+        __INIT
 __fixup_smp:
         and     r3, r9, #0x000f0000     @ architecture version
         teq     r3, #0x000f0000         @ CPU ID supported?
@@ -415,18 +416,7 @@ __fixup_smp_on_up:
         sub     r3, r0, r3
         add     r4, r4, r3
         add     r5, r5, r3
-2:      cmp     r4, r5
-        movhs   pc, lr
-        ldmia   r4!, {r0, r6}
- ARM(   str     r6, [r0, r3]    )
- THUMB( add     r0, r0, r3      )
-#ifdef __ARMEB__
- THUMB( mov     r6, r6, ror #16 )       @ Convert word order for big-endian.
-#endif
- THUMB( strh    r6, [r0], #2    )       @ For Thumb-2, store as two halfwords
- THUMB( mov     r6, r6, lsr #16 )       @ to be robust against misaligned r3.
- THUMB( strh    r6, [r0]        )
-        b       2b
+        b       __do_fixup_smp_on_up
 ENDPROC(__fixup_smp)
 
         .align
@@ -440,7 +430,31 @@ smp_on_up:
         ALT_SMP(.long   1)
         ALT_UP(.long    0)
         .popsection
+#endif
 
+        .text
+__do_fixup_smp_on_up:
+        cmp     r4, r5
+        movhs   pc, lr
+        ldmia   r4!, {r0, r6}
+ ARM(   str     r6, [r0, r3]    )
+ THUMB( add     r0, r0, r3      )
+#ifdef __ARMEB__
+ THUMB( mov     r6, r6, ror #16 )       @ Convert word order for big-endian.
 #endif
+ THUMB( strh    r6, [r0], #2    )       @ For Thumb-2, store as two halfwords
+ THUMB( mov     r6, r6, lsr #16 )       @ to be robust against misaligned r3.
+ THUMB( strh    r6, [r0]        )
+        b       __do_fixup_smp_on_up
+ENDPROC(__do_fixup_smp_on_up)
+
+ENTRY(fixup_smp)
+        stmfd   sp!, {r4 - r6, lr}
+        mov     r4, r0
+        add     r5, r0, r1
+        mov     r3, #0
+        bl      __do_fixup_smp_on_up
+        ldmfd   sp!, {r4 - r6, pc}
+ENDPROC(fixup_smp)
 
 #include "head-common.S"
diff --git a/arch/arm/kernel/module.c b/arch/arm/kernel/module.c
index 2cfe8161b478..6d4105e6872f 100644
--- a/arch/arm/kernel/module.c
+++ b/arch/arm/kernel/module.c
@@ -22,6 +22,7 @@
 
 #include <asm/pgtable.h>
 #include <asm/sections.h>
+#include <asm/smp_plat.h>
 #include <asm/unwind.h>
 
 #ifdef CONFIG_XIP_KERNEL
@@ -268,12 +269,28 @@ struct mod_unwind_map {
         const Elf_Shdr *txt_sec;
 };
 
+static const Elf_Shdr *find_mod_section(const Elf32_Ehdr *hdr,
+        const Elf_Shdr *sechdrs, const char *name)
+{
+        const Elf_Shdr *s, *se;
+        const char *secstrs = (void *)hdr + sechdrs[hdr->e_shstrndx].sh_offset;
+
+        for (s = sechdrs, se = sechdrs + hdr->e_shnum; s < se; s++)
+                if (strcmp(name, secstrs + s->sh_name) == 0)
+                        return s;
+
+        return NULL;
+}
+
+extern void fixup_smp(const void *, unsigned long);
+
 int module_finalize(const Elf32_Ehdr *hdr, const Elf_Shdr *sechdrs,
                     struct module *mod)
 {
+        const Elf_Shdr * __maybe_unused s = NULL;
 #ifdef CONFIG_ARM_UNWIND
         const char *secstrs = (void *)hdr + sechdrs[hdr->e_shstrndx].sh_offset;
-        const Elf_Shdr *s, *sechdrs_end = sechdrs + hdr->e_shnum;
+        const Elf_Shdr *sechdrs_end = sechdrs + hdr->e_shnum;
         struct mod_unwind_map maps[ARM_SEC_MAX];
         int i;
 
@@ -315,6 +332,9 @@ int module_finalize(const Elf32_Ehdr *hdr, const Elf_Shdr *sechdrs,
                                                  maps[i].txt_sec->sh_addr,
                                                  maps[i].txt_sec->sh_size);
 #endif
+        s = find_mod_section(hdr, sechdrs, ".alt.smp.init");
+        if (s && !is_smp())
+                fixup_smp((void *)s->sh_addr, s->sh_size);
         return 0;
 }