This patch changes mt7629 to use the compatible platform SMP initialization method of linux kernel.
Signed-off-by: Weijie Gao weijie.gao@mediatek.com --- arch/arm/mach-mediatek/mt7629/lowlevel_init.S | 52 ++++++++++++++++--- 1 file changed, 44 insertions(+), 8 deletions(-)
diff --git a/arch/arm/mach-mediatek/mt7629/lowlevel_init.S b/arch/arm/mach-mediatek/mt7629/lowlevel_init.S index 90dd4ea48e4..3375796b797 100644 --- 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)