arch/arm/mach-mediatek/mt7629/lowlevel_init.S

   1 /* SPDX-License-Identifier: GPL-2.0 */
   2 /*
   3  * Copyright (C) 2018 MediaTek Inc.
   4  */
   5
   6 #include <linux/linkage.h>
   7 #include <asm/proc-armv/ptrace.h>
   8
   9 #define WAIT_CODE_SRAM_BASE     0x0010ff00
  10
  11 #define SLAVE_JUMP_REG          0x10202034
  12 #define SLAVE1_MAGIC_REG        0x10202038
  13 #define SLAVE1_MAGIC_NUM        0x534c4131
  14
  15 #define GIC_CPU_BASE            0x10320000
  16
  17 ENTRY(lowlevel_init)
  18
  19 #ifndef CONFIG_SPL_BUILD
  20         /* Return to U-Boot via saved link register */
  21         mov     pc, lr
  22 #else
  23         /*
  24          * Arch timer :
  25          * set CNTFRQ = 20Mhz, set CNTVOFF = 0
  26          */
  27         movw    r0, #0x2d00
  28         movt    r0, #0x131
  29         mcr     p15, 0, r0, c14, c0, 0
  30
  31         cps     #MON_MODE
  32         mrc     p15, 0, r1, c1, c1, 0   @ Get Secure Config
  33         orr     r0, r1, #1
  34         mcr     p15, 0, r0, c1, c1, 0   @ Set Non Secure bit
  35         isb
  36         mov     r0, #0
  37         mcrr    p15, 4, r0, r0, c14     @ CNTVOFF = 0
  38         isb
  39         mcr     p15, 0, r1, c1, c1, 0   @ Set Secure bit
  40         isb
  41         cps     #SVC_MODE
  42
  43         /* enable SMP bit */
  44         mrc     p15, 0, r0, c1, c0, 1
  45         orr     r0, r0, #0x40
  46         mcr     p15, 0, r0, c1, c0, 1
  47
  48         /* if MP core, handle secondary cores */
  49         mrc     p15, 0, r0, c0, c0, 5
  50         ands    r1, r0, #0x40000000
  51         bne     go                      @ Go if UP
  52         /* read slave CPU number */
  53         ands    r0, r0, #0x0f
  54         beq     go                      @ Go if core0 on primary core tile
  55         b       secondary
  56
  57 go:
  58         /* master CPU */
  59         mov     pc, lr
  60
  61 secondary:
  62         /* enable GIC as cores will be waken up by IPI */
  63         ldr     r2, =GIC_CPU_BASE
  64         mov     r1, #0xf0
  65         str     r1, [r2, #4]
  66         mov     r1, #1
  67         str     r1, [r2, #0]
  68
  69         ldr     r1, [r2]
  70         orr     r1, #1
  71         str     r1, [r2]
  72
  73         /* copy wait code into SRAM */
  74         ldr     r0, =slave_cpu_wait
  75         ldm     r0, {r1 - r8}           @ slave_cpu_wait has eight insns
  76         ldr     r0, =WAIT_CODE_SRAM_BASE
  77         stm     r0, {r1 - r8}
  78
  79         /* pass args to slave_cpu_wait */
  80         ldr     r0, =SLAVE1_MAGIC_REG
  81         ldr     r1, =SLAVE1_MAGIC_NUM
  82
  83         /* jump to wait code in SRAM */
  84         ldr     pc, =WAIT_CODE_SRAM_BASE
  85
  86 #endif
  87 ENDPROC(lowlevel_init)
  88
  89 /* This function will be copied into SRAM */
  90 ENTRY(slave_cpu_wait)
  91         wfi
  92         ldr     r2, [r0]
  93         cmp     r2, r1
  94         bne     slave_cpu_wait
  95         movw    r0, #:lower16:SLAVE_JUMP_REG
  96         movt    r0, #:upper16:SLAVE_JUMP_REG
  97         ldr     r1, [r0]
  98         mov     pc, r1
  99 ENDPROC(slave_cpu_wait)