ARM: Allow SMP kernels to boot on UP systems

UP systems do not implement all the instructions that SMP systems have,
so in order to boot a SMP kernel on a UP system, we need to rewrite
parts of the kernel.

Do this using an 'alternatives' scheme, where the kernel code and data
is modified prior to initialization to replace the SMP instructions,
thereby rendering the problematical code ineffectual.  We use the linker
to generate a list of 32-bit word locations and their replacement values,
and run through these replacements when we detect a UP system.

Signed-off-by: Russell King <rmk+kernel@arm.linux.org.uk>

1 files changed, 50 insertions, 0 deletions

diff --git a/arch/arm/kernel/head.S b/arch/arm/kernel/head.S
index eb62bf947212..b44d21e1e344 100644
--- a/arch/arm/kernel/head.S
+++ b/arch/arm/kernel/head.S
@@ -86,6 +86,9 @@ ENTRY(stext)
         movs    r8, r5                          @ invalid machine (r5=0)?
         beq     __error_a                       @ yes, error 'a'
         bl      __vet_atags
+#ifdef CONFIG_SMP_ON_UP
+        bl      __fixup_smp
+#endif
         bl      __create_page_tables
 
         /*
@@ -333,4 +336,51 @@ __create_page_tables:
 ENDPROC(__create_page_tables)
         .ltorg
 
+#ifdef CONFIG_SMP_ON_UP
+__fixup_smp:
+        mov     r7, #0x00070000
+        orr     r6, r7, #0xff000000     @ mask 0xff070000
+        orr     r7, r7, #0x41000000     @ val 0x41070000
+        and     r0, r9, r6
+        teq     r0, r7                  @ ARM CPU and ARMv6/v7?
+        bne     __fixup_smp_on_up       @ no, assume UP
+
+        orr     r6, r6, #0x0000ff00
+        orr     r6, r6, #0x000000f0     @ mask 0xff07fff0
+        orr     r7, r7, #0x0000b000
+        orr     r7, r7, #0x00000020     @ val 0x4107b020
+        and     r0, r9, r6
+        teq     r0, r7                  @ ARM 11MPCore?
+        moveq   pc, lr                  @ yes, assume SMP
+
+        mrc     p15, 0, r0, c0, c0, 5   @ read MPIDR
+        tst     r0, #1 << 31
+        movne   pc, lr                  @ bit 31 => SMP
+
+__fixup_smp_on_up:
+        adr     r0, 1f
+        ldmia   r0, {r3, r6, r7}
+        sub     r3, r0, r3
+        add     r6, r6, r3
+        add     r7, r7, r3
+2:      cmp     r6, r7
+        ldmia   r6!, {r0, r4}
+        strlo   r4, [r0, r3]
+        blo     2b
+        mov     pc, lr
+ENDPROC(__fixup_smp)
+
+1:      .word   .
+        .word   __smpalt_begin
+        .word   __smpalt_end
+
+        .pushsection .data
+        .globl  smp_on_up
+smp_on_up:
+        ALT_SMP(.long   1)
+        ALT_UP(.long    0)
+        .popsection
+
+#endif
+
 #include "head-common.S"