LXR uboot/arch/arm/mach-tegra/psci.S

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