1ecf07a79SMarc Zyngier/* 2ecf07a79SMarc Zyngier * Copyright (C) 2013,2014 - ARM Ltd 3ecf07a79SMarc Zyngier * Author: Marc Zyngier <marc.zyngier@arm.com> 4ecf07a79SMarc Zyngier * 5ecf07a79SMarc Zyngier * This program is free software; you can redistribute it and/or modify 6ecf07a79SMarc Zyngier * it under the terms of the GNU General Public License version 2 as 7ecf07a79SMarc Zyngier * published by the Free Software Foundation. 8ecf07a79SMarc Zyngier * 9ecf07a79SMarc Zyngier * This program is distributed in the hope that it will be useful, 10ecf07a79SMarc Zyngier * but WITHOUT ANY WARRANTY; without even the implied warranty of 11ecf07a79SMarc Zyngier * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12ecf07a79SMarc Zyngier * GNU General Public License for more details. 13ecf07a79SMarc Zyngier * 14ecf07a79SMarc Zyngier * You should have received a copy of the GNU General Public License 15ecf07a79SMarc Zyngier * along with this program. If not, see <http://www.gnu.org/licenses/>. 16ecf07a79SMarc Zyngier */ 17ecf07a79SMarc Zyngier 18ecf07a79SMarc Zyngier#include <config.h> 19ecf07a79SMarc Zyngier#include <linux/linkage.h> 20ecf07a79SMarc Zyngier#include <asm/psci.h> 21ecf07a79SMarc Zyngier 22ecf07a79SMarc Zyngier .pushsection ._secure.text, "ax" 23ecf07a79SMarc Zyngier 24ecf07a79SMarc Zyngier .arch_extension sec 25ecf07a79SMarc Zyngier 26ecf07a79SMarc Zyngier .align 5 27ecf07a79SMarc Zyngier .globl _psci_vectors 28ecf07a79SMarc Zyngier_psci_vectors: 29ecf07a79SMarc Zyngier b default_psci_vector @ reset 30ecf07a79SMarc Zyngier b default_psci_vector @ undef 31ecf07a79SMarc Zyngier b _smc_psci @ smc 32ecf07a79SMarc Zyngier b default_psci_vector @ pabort 33ecf07a79SMarc Zyngier b default_psci_vector @ dabort 34ecf07a79SMarc Zyngier b default_psci_vector @ hyp 35ecf07a79SMarc Zyngier b default_psci_vector @ irq 36ecf07a79SMarc Zyngier b psci_fiq_enter @ fiq 37ecf07a79SMarc Zyngier 38ecf07a79SMarc ZyngierENTRY(psci_fiq_enter) 39ecf07a79SMarc Zyngier movs pc, lr 40ecf07a79SMarc ZyngierENDPROC(psci_fiq_enter) 41ecf07a79SMarc Zyngier.weak psci_fiq_enter 42ecf07a79SMarc Zyngier 43ecf07a79SMarc ZyngierENTRY(default_psci_vector) 44ecf07a79SMarc Zyngier movs pc, lr 45ecf07a79SMarc ZyngierENDPROC(default_psci_vector) 46ecf07a79SMarc Zyngier.weak default_psci_vector 47ecf07a79SMarc Zyngier 48ecf07a79SMarc ZyngierENTRY(psci_cpu_suspend) 49ecf07a79SMarc ZyngierENTRY(psci_cpu_off) 50ecf07a79SMarc ZyngierENTRY(psci_cpu_on) 51ecf07a79SMarc ZyngierENTRY(psci_migrate) 52ecf07a79SMarc Zyngier mov r0, #ARM_PSCI_RET_NI @ Return -1 (Not Implemented) 53ecf07a79SMarc Zyngier mov pc, lr 54ecf07a79SMarc ZyngierENDPROC(psci_migrate) 55ecf07a79SMarc ZyngierENDPROC(psci_cpu_on) 56ecf07a79SMarc ZyngierENDPROC(psci_cpu_off) 57ecf07a79SMarc ZyngierENDPROC(psci_cpu_suspend) 58ecf07a79SMarc Zyngier.weak psci_cpu_suspend 59ecf07a79SMarc Zyngier.weak psci_cpu_off 60ecf07a79SMarc Zyngier.weak psci_cpu_on 61ecf07a79SMarc Zyngier.weak psci_migrate 62ecf07a79SMarc Zyngier 63ecf07a79SMarc Zyngier_psci_table: 64ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_SUSPEND 65ecf07a79SMarc Zyngier .word psci_cpu_suspend 66ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_OFF 67ecf07a79SMarc Zyngier .word psci_cpu_off 68ecf07a79SMarc Zyngier .word ARM_PSCI_FN_CPU_ON 69ecf07a79SMarc Zyngier .word psci_cpu_on 70ecf07a79SMarc Zyngier .word ARM_PSCI_FN_MIGRATE 71ecf07a79SMarc Zyngier .word psci_migrate 72ecf07a79SMarc Zyngier .word 0 73ecf07a79SMarc Zyngier .word 0 74ecf07a79SMarc Zyngier 75ecf07a79SMarc Zyngier_smc_psci: 76ecf07a79SMarc Zyngier push {r4-r7,lr} 77ecf07a79SMarc Zyngier 78ecf07a79SMarc Zyngier @ Switch to secure 79ecf07a79SMarc Zyngier mrc p15, 0, r7, c1, c1, 0 80ecf07a79SMarc Zyngier bic r4, r7, #1 81ecf07a79SMarc Zyngier mcr p15, 0, r4, c1, c1, 0 82ecf07a79SMarc Zyngier isb 83ecf07a79SMarc Zyngier 84ecf07a79SMarc Zyngier adr r4, _psci_table 85ecf07a79SMarc Zyngier1: ldr r5, [r4] @ Load PSCI function ID 86ecf07a79SMarc Zyngier ldr r6, [r4, #4] @ Load target PC 87ecf07a79SMarc Zyngier cmp r5, #0 @ If reach the end, bail out 88ecf07a79SMarc Zyngier moveq r0, #ARM_PSCI_RET_INVAL @ Return -2 (Invalid) 89ecf07a79SMarc Zyngier beq 2f 90ecf07a79SMarc Zyngier cmp r0, r5 @ If not matching, try next entry 91ecf07a79SMarc Zyngier addne r4, r4, #8 92ecf07a79SMarc Zyngier bne 1b 93ecf07a79SMarc Zyngier 94ecf07a79SMarc Zyngier blx r6 @ Execute PSCI function 95ecf07a79SMarc Zyngier 96ecf07a79SMarc Zyngier @ Switch back to non-secure 97ecf07a79SMarc Zyngier2: mcr p15, 0, r7, c1, c1, 0 98ecf07a79SMarc Zyngier 99ecf07a79SMarc Zyngier pop {r4-r7, lr} 100ecf07a79SMarc Zyngier movs pc, lr @ Return to the kernel 101ecf07a79SMarc Zyngier 102680f3968SJan Kiszka@ Requires dense and single-cluster CPU ID space 103680f3968SJan KiszkaENTRY(psci_get_cpu_id) 104680f3968SJan Kiszka mrc p15, 0, r0, c0, c0, 5 /* read MPIDR */ 105680f3968SJan Kiszka and r0, r0, #0xff /* return CPU ID in cluster */ 106680f3968SJan Kiszka bx lr 107680f3968SJan KiszkaENDPROC(psci_get_cpu_id) 108680f3968SJan Kiszka.weak psci_get_cpu_id 109680f3968SJan Kiszka 110b0206e7dSJan Kiszka/* Imported from Linux kernel */ 111b0206e7dSJan KiszkaLENTRY(v7_flush_dcache_all) 112b0206e7dSJan Kiszka dmb @ ensure ordering with previous memory accesses 113b0206e7dSJan Kiszka mrc p15, 1, r0, c0, c0, 1 @ read clidr 114b0206e7dSJan Kiszka ands r3, r0, #0x7000000 @ extract loc from clidr 115b0206e7dSJan Kiszka mov r3, r3, lsr #23 @ left align loc bit field 116b0206e7dSJan Kiszka beq finished @ if loc is 0, then no need to clean 117b0206e7dSJan Kiszka mov r10, #0 @ start clean at cache level 0 118b0206e7dSJan Kiszkaflush_levels: 119b0206e7dSJan Kiszka add r2, r10, r10, lsr #1 @ work out 3x current cache level 120b0206e7dSJan Kiszka mov r1, r0, lsr r2 @ extract cache type bits from clidr 121b0206e7dSJan Kiszka and r1, r1, #7 @ mask of the bits for current cache only 122b0206e7dSJan Kiszka cmp r1, #2 @ see what cache we have at this level 123b0206e7dSJan Kiszka blt skip @ skip if no cache, or just i-cache 124b0206e7dSJan Kiszka mrs r9, cpsr @ make cssr&csidr read atomic 125b0206e7dSJan Kiszka mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr 126b0206e7dSJan Kiszka isb @ isb to sych the new cssr&csidr 127b0206e7dSJan Kiszka mrc p15, 1, r1, c0, c0, 0 @ read the new csidr 128b0206e7dSJan Kiszka msr cpsr_c, r9 129b0206e7dSJan Kiszka and r2, r1, #7 @ extract the length of the cache lines 130b0206e7dSJan Kiszka add r2, r2, #4 @ add 4 (line length offset) 131b0206e7dSJan Kiszka ldr r4, =0x3ff 132b0206e7dSJan Kiszka ands r4, r4, r1, lsr #3 @ find maximum number on the way size 133b0206e7dSJan Kiszka clz r5, r4 @ find bit position of way size increment 134b0206e7dSJan Kiszka ldr r7, =0x7fff 135b0206e7dSJan Kiszka ands r7, r7, r1, lsr #13 @ extract max number of the index size 136b0206e7dSJan Kiszkaloop1: 137b0206e7dSJan Kiszka mov r9, r7 @ create working copy of max index 138b0206e7dSJan Kiszkaloop2: 139b0206e7dSJan Kiszka orr r11, r10, r4, lsl r5 @ factor way and cache number into r11 140b0206e7dSJan Kiszka orr r11, r11, r9, lsl r2 @ factor index number into r11 141b0206e7dSJan Kiszka mcr p15, 0, r11, c7, c14, 2 @ clean & invalidate by set/way 142b0206e7dSJan Kiszka subs r9, r9, #1 @ decrement the index 143b0206e7dSJan Kiszka bge loop2 144b0206e7dSJan Kiszka subs r4, r4, #1 @ decrement the way 145b0206e7dSJan Kiszka bge loop1 146b0206e7dSJan Kiszkaskip: 147b0206e7dSJan Kiszka add r10, r10, #2 @ increment cache number 148b0206e7dSJan Kiszka cmp r3, r10 149b0206e7dSJan Kiszka bgt flush_levels 150b0206e7dSJan Kiszkafinished: 151b0206e7dSJan Kiszka mov r10, #0 @ swith back to cache level 0 152b0206e7dSJan Kiszka mcr p15, 2, r10, c0, c0, 0 @ select current cache level in cssr 153b0206e7dSJan Kiszka dsb st 154b0206e7dSJan Kiszka isb 155b0206e7dSJan Kiszka bx lr 156b0206e7dSJan KiszkaENDPROC(v7_flush_dcache_all) 157b0206e7dSJan Kiszka 158b0206e7dSJan KiszkaENTRY(psci_disable_smp) 159b0206e7dSJan Kiszka mrc p15, 0, r0, c1, c0, 1 @ ACTLR 160b0206e7dSJan Kiszka bic r0, r0, #(1 << 6) @ Clear SMP bit 161b0206e7dSJan Kiszka mcr p15, 0, r0, c1, c0, 1 @ ACTLR 162b0206e7dSJan Kiszka isb 163b0206e7dSJan Kiszka dsb 164b0206e7dSJan Kiszka bx lr 165b0206e7dSJan KiszkaENDPROC(psci_disable_smp) 166b0206e7dSJan Kiszka.weak psci_disable_smp 167b0206e7dSJan Kiszka 1684ce4de1eSJan KiszkaENTRY(psci_enable_smp) 1694ce4de1eSJan Kiszka mrc p15, 0, r0, c1, c0, 1 @ ACTLR 1704ce4de1eSJan Kiszka orr r0, r0, #(1 << 6) @ Set SMP bit 1714ce4de1eSJan Kiszka mcr p15, 0, r0, c1, c0, 1 @ ACTLR 1724ce4de1eSJan Kiszka isb 1734ce4de1eSJan Kiszka bx lr 1744ce4de1eSJan KiszkaENDPROC(psci_enable_smp) 1754ce4de1eSJan Kiszka.weak psci_enable_smp 1764ce4de1eSJan Kiszka 177b0206e7dSJan KiszkaENTRY(psci_cpu_off_common) 178b0206e7dSJan Kiszka push {lr} 179b0206e7dSJan Kiszka 180b0206e7dSJan Kiszka mrc p15, 0, r0, c1, c0, 0 @ SCTLR 181b0206e7dSJan Kiszka bic r0, r0, #(1 << 2) @ Clear C bit 182b0206e7dSJan Kiszka mcr p15, 0, r0, c1, c0, 0 @ SCTLR 183b0206e7dSJan Kiszka isb 184b0206e7dSJan Kiszka dsb 185b0206e7dSJan Kiszka 186b0206e7dSJan Kiszka bl v7_flush_dcache_all 187b0206e7dSJan Kiszka 188b0206e7dSJan Kiszka clrex @ Why??? 189b0206e7dSJan Kiszka 190b0206e7dSJan Kiszka bl psci_disable_smp 191b0206e7dSJan Kiszka 192b0206e7dSJan Kiszka pop {lr} 193b0206e7dSJan Kiszka bx lr 194b0206e7dSJan KiszkaENDPROC(psci_cpu_off_common) 195b0206e7dSJan Kiszka 196*4c681a3dSJan Kiszka@ expects CPU ID in r0 and returns stack top in r0 197*4c681a3dSJan KiszkaENTRY(psci_get_cpu_stack_top) 198*4c681a3dSJan Kiszka mov r5, #0x400 @ 1kB of stack per CPU 199*4c681a3dSJan Kiszka mul r0, r0, r5 200*4c681a3dSJan Kiszka 201*4c681a3dSJan Kiszka ldr r5, =psci_text_end @ end of monitor text 202*4c681a3dSJan Kiszka add r5, r5, #0x2000 @ Skip two pages 203*4c681a3dSJan Kiszka lsr r5, r5, #12 @ Align to start of page 204*4c681a3dSJan Kiszka lsl r5, r5, #12 205*4c681a3dSJan Kiszka sub r0, r5, r0 @ here's our stack! 206*4c681a3dSJan Kiszka 207*4c681a3dSJan Kiszka bx lr 208*4c681a3dSJan KiszkaENDPROC(psci_get_cpu_stack_top) 209*4c681a3dSJan Kiszka 2104ce4de1eSJan KiszkaENTRY(psci_cpu_entry) 2114ce4de1eSJan Kiszka bl psci_enable_smp 2124ce4de1eSJan Kiszka 2134ce4de1eSJan Kiszka bl _nonsec_init 2144ce4de1eSJan Kiszka 2154ce4de1eSJan Kiszka adr r0, _psci_target_pc 2164ce4de1eSJan Kiszka ldr r0, [r0] 2174ce4de1eSJan Kiszka b _do_nonsec_entry 2184ce4de1eSJan KiszkaENDPROC(psci_cpu_entry) 2194ce4de1eSJan Kiszka 2204ce4de1eSJan Kiszka.globl _psci_target_pc 2214ce4de1eSJan Kiszka_psci_target_pc: 2224ce4de1eSJan Kiszka .word 0 2234ce4de1eSJan Kiszka 224ecf07a79SMarc Zyngier .popsection 225