Merge branch 'master' of https://gitlab.denx.de/u-boot/custodians/u-boot-spi

[platform/kernel/u-boot.git] / arch / arm / mach-mediatek / mt7629 / lowlevel_init.S

diff --git a/arch/arm/mach-mediatek/mt7629/lowlevel_init.S b/arch/arm/mach-mediatek/mt7629/lowlevel_init.S
index 90dd4ea..0a0672c 100644 (file)
--- a/arch/arm/mach-mediatek/mt7629/lowlevel_init.S
+++ b/arch/arm/mach-mediatek/mt7629/lowlevel_init.S
@@ -4,6 +4,15 @@
  */
 
 #include <linux/linkage.h>
+#include <asm/proc-armv/ptrace.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)
 
@@ -19,6 +28,18 @@ ENTRY(lowlevel_init)
        movt    r0, #0x131
        mcr     p15, 0, r0, c14, c0, 0
 
+       cps     #MON_MODE
+       mrc     p15, 0, r1, c1, c1, 0   @ Get Secure Config
+       orr     r0, r1, #1
+       mcr     p15, 0, r0, c1, c1, 0   @ Set Non Secure bit
+       isb
+       mov     r0, #0
+       mcrr    p15, 4, r0, r0, c14     @ CNTVOFF = 0
+       isb
+       mcr     p15, 0, r1, c1, c1, 0   @ Set Secure bit
+       isb
+       cps     #SVC_MODE
+
        /* enable SMP bit */
        mrc     p15, 0, r0, c1, c0, 1
        orr     r0, r0, #0x40
@@ -28,6 +49,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 +59,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)