arch/arm/mach-tegra/psci.S

   1 /*
   2  * Copyright (C) 2014, NVIDIA
   3  * Copyright (C) 2015, Siemens AG
   4  *
   5  * Authors:
   6  *  Thierry Reding <treding@nvidia.com>
   7  *  Jan Kiszka <jan.kiszka@siemens.com>
   8  *
   9  * SPDX-License-Identifier:     GPL-2.0+
  10  */
  11
  12 #include <linux/linkage.h>
  13 #include <asm/macro.h>
  14 #include <asm/psci.h>
  15
  16         .pushsection ._secure.text, "ax"
  17         .arch_extension sec
  18
  19 #define TEGRA_SB_CSR_0                  0x6000c200
  20 #define NS_RST_VEC_WR_DIS               (1 << 1)
  21
  22 #define TEGRA_RESET_EXCEPTION_VECTOR    0x6000f100
  23
  24 #define TEGRA_FLOW_CTRL_BASE            0x60007000
  25 #define FLOW_CTRL_CPU_CSR               0x08
  26 #define CSR_ENABLE                      (1 << 0)
  27 #define CSR_IMMEDIATE_WAKE              (1 << 3)
  28 #define CSR_WAIT_WFI_SHIFT              8
  29 #define FLOW_CTRL_CPU1_CSR              0x18
  30
  31 @ converts CPU ID into FLOW_CTRL_CPUn_CSR offset
  32 .macro get_csr_reg cpu, ofs, tmp
  33         cmp     \cpu, #0                @ CPU0?
  34         lsl     \tmp, \cpu, #3  @ multiple by 8 (register offset CPU1-3)
  35         moveq   \ofs, #FLOW_CTRL_CPU_CSR
  36         addne   \ofs, \tmp, #FLOW_CTRL_CPU1_CSR - 8
  37 .endm
  38
  39 ENTRY(psci_arch_init)
  40         mov     r6, lr
  41
  42         mrc     p15, 0, r5, c1, c1, 0   @ Read SCR
  43         bic     r5, r5, #1              @ Secure mode
  44         mcr     p15, 0, r5, c1, c1, 0   @ Write SCR
  45         isb
  46
  47         @ lock reset vector for non-secure
  48         ldr     r4, =TEGRA_SB_CSR_0
  49         ldr     r5, [r4]
  50         orr     r5, r5, #NS_RST_VEC_WR_DIS
  51         str     r5, [r4]
  52
  53         bl      psci_get_cpu_id         @ CPU ID => r0
  54
  55         adr     r5, _sys_clock_freq
  56         cmp     r0, #0
  57
  58         mrceq   p15, 0, r7, c14, c0, 0  @ read CNTFRQ from CPU0
  59         streq   r7, [r5]
  60
  61         ldrne   r7, [r5]
  62         mcrne   p15, 0, r7, c14, c0, 0  @ write CNTFRQ to CPU1..3
  63
  64         bl      psci_get_cpu_stack_top  @ stack top => r0
  65         mov     sp, r0
  66
  67         bx      r6
  68 ENDPROC(psci_arch_init)
  69
  70 _sys_clock_freq:
  71         .word   0
  72
  73 ENTRY(psci_cpu_off)
  74         bl      psci_cpu_off_common
  75
  76         bl      psci_get_cpu_id         @ CPU ID => r0
  77
  78         get_csr_reg r0, r2, r3
  79
  80         ldr     r6, =TEGRA_FLOW_CTRL_BASE
  81         mov     r5, #(CSR_ENABLE)
  82         mov     r4, #(1 << CSR_WAIT_WFI_SHIFT)
  83         add     r5, r4, lsl r0
  84         str     r5, [r6, r2]
  85
  86 _loop:  wfi
  87         b       _loop
  88 ENDPROC(psci_cpu_off)
  89
  90 ENTRY(psci_cpu_on)
  91         push    {lr}
  92
  93         mov     r0, r1
  94         bl      psci_get_cpu_stack_top  @ get stack top of target CPU
  95         str     r2, [r0]                @ store target PC at stack top
  96         dsb
  97
  98         ldr     r6, =TEGRA_RESET_EXCEPTION_VECTOR
  99         ldr     r5, =psci_cpu_entry
 100         str     r5, [r6]
 101
 102         get_csr_reg r1, r2, r3
 103
 104         ldr     r6, =TEGRA_FLOW_CTRL_BASE
 105         mov     r5, #(CSR_IMMEDIATE_WAKE | CSR_ENABLE)
 106         str     r5, [r6, r2]
 107
 108         mov     r0, #ARM_PSCI_RET_SUCCESS       @ Return PSCI_RET_SUCCESS
 109         pop     {pc}
 110 ENDPROC(psci_cpu_on)
 111
 112         .globl psci_text_end
 113 psci_text_end:
 114         .popsection