mt7629: use linux kernel compatible SMP initialization

This patch changes mt7629 to use the compatible platform SMP initialization
method of linux kernel.

Signed-off-by: Weijie Gao <weijie.gao@mediatek.com>

diff --git a/arch/arm/mach-mediatek/mt7629/lowlevel_init.S b/arch/arm/mach-mediatek/mt7629/lowlevel_init.S
index 90dd4ea..3375796 100644 (file)
--- a/arch/arm/mach-mediatek/mt7629/lowlevel_init.S
+++ b/arch/arm/mach-mediatek/mt7629/lowlevel_init.S
@@ -5,6 +5,14 @@
 
 #include <linux/linkage.h>
 
+#define WAIT_CODE_SRAM_BASE    0x0010ff00
+
+#define SLAVE_JUMP_REG         0x10202034
+#define SLAVE1_MAGIC_REG       0x10202038
+#define SLAVE1_MAGIC_NUM       0x534c4131
+
+#define GIC_CPU_BASE           0x10320000
+
 ENTRY(lowlevel_init)
 
 #ifndef CONFIG_SPL_BUILD
@@ -28,6 +36,7 @@ ENTRY(lowlevel_init)
        mrc     p15, 0, r0, c0, c0, 5
        ands    r1, r0, #0x40000000
        bne     go                      @ Go if UP
+       /* read slave CPU number */
        ands    r0, r0, #0x0f
        beq     go                      @ Go if core0 on primary core tile
        b       secondary
@@ -37,14 +46,41 @@ go:
        mov     pc, lr
 
 secondary:
-       /* read slave CPU number into r0 firstly */
-       mrc     p15, 0, r0, c0, c0, 5
-       and     r0, r0, #0x0f
+       /* enable GIC as cores will be waken up by IPI */
+       ldr     r2, =GIC_CPU_BASE
+       mov     r1, #0xf0
+       str     r1, [r2, #4]
+       mov     r1, #1
+       str     r1, [r2, #0]
+
+       ldr     r1, [r2]
+       orr     r1, #1
+       str     r1, [r2]
+
+       /* copy wait code into SRAM */
+       ldr     r0, =slave_cpu_wait
+       ldm     r0, {r1 - r8}           @ slave_cpu_wait has eight insns
+       ldr     r0, =WAIT_CODE_SRAM_BASE
+       stm     r0, {r1 - r8}
+
+       /* pass args to slave_cpu_wait */
+       ldr     r0, =SLAVE1_MAGIC_REG
+       ldr     r1, =SLAVE1_MAGIC_NUM
+
+       /* jump to wait code in SRAM */
+       ldr     pc, =WAIT_CODE_SRAM_BASE
 
-loop:
-       dsb
-       isb
-       wfi                             @Zzz...
-       b       loop
 #endif
 ENDPROC(lowlevel_init)
+
+/* This function will be copied into SRAM */
+ENTRY(slave_cpu_wait)
+       wfi
+       ldr     r2, [r0]
+       cmp     r2, r1
+       bne     slave_cpu_wait
+       movw    r0, #:lower16:SLAVE_JUMP_REG
+       movt    r0, #:upper16:SLAVE_JUMP_REG
+       ldr     r1, [r0]
+       mov     pc, r1
+ENDPROC(slave_cpu_wait)