|
@@ -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)
|