[U-Boot] [PATCH 02/11] mt7629: use linux kernel compatible SMP initialization

[Weijie Gao]

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

Signed-off-by: Weijie Gao <weijie.gao at 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)
-- 
2.18.0