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 39ENTRY(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 68ENDPROC(psci_arch_init) 69 70_sys_clock_freq: 71 .word 0 72 73ENTRY(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 88ENDPROC(psci_cpu_off) 89 90ENTRY(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} 110ENDPROC(psci_cpu_on) 111 112 .globl psci_text_end 113psci_text_end: 114 .popsection 115