arch/arm/cpu/armv7/psci.S

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