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