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