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         dmb                                     @ ensure ordering with previous memory accesses
 114         mrc     p15, 1, r0, c0, c0, 1           @ read clidr
 115         ands    r3, r0, #0x7000000              @ extract loc from clidr
 116         mov     r3, r3, lsr #23                 @ left align loc bit field
 117         beq     finished                        @ if loc is 0, then no need to clean
 118         mov     r10, #0                         @ start clean at cache level 0
 119 flush_levels:
 120         add     r2, r10, r10, lsr #1            @ work out 3x current cache level
 121         mov     r1, r0, lsr r2                  @ extract cache type bits from clidr
 122         and     r1, r1, #7                      @ mask of the bits for current cache only
 123         cmp     r1, #2                          @ see what cache we have at this level
 124         blt     skip                            @ skip if no cache, or just i-cache
 125         mrs     r9, cpsr                        @ make cssr&csidr read atomic
 126         mcr     p15, 2, r10, c0, c0, 0          @ select current cache level in cssr
 127         isb                                     @ isb to sych the new cssr&csidr
 128         mrc     p15, 1, r1, c0, c0, 0           @ read the new csidr
 129         msr     cpsr_c, r9
 130         and     r2, r1, #7                      @ extract the length of the cache lines
 131         add     r2, r2, #4                      @ add 4 (line length offset)
 132         ldr     r4, =0x3ff
 133         ands    r4, r4, r1, lsr #3              @ find maximum number on the way size
 134         clz     r5, r4                          @ find bit position of way size increment
 135         ldr     r7, =0x7fff
 136         ands    r7, r7, r1, lsr #13             @ extract max number of the index size
 137 loop1:
 138         mov     r9, r7                          @ create working copy of max index
 139 loop2:
 140         orr     r11, r10, r4, lsl r5            @ factor way and cache number into r11
 141         orr     r11, r11, r9, lsl r2            @ factor index number into r11
 142         mcr     p15, 0, r11, c7, c14, 2         @ clean & invalidate by set/way
 143         subs    r9, r9, #1                      @ decrement the index
 144         bge     loop2
 145         subs    r4, r4, #1                      @ decrement the way
 146         bge     loop1
 147 skip:
 148         add     r10, r10, #2                    @ increment cache number
 149         cmp     r3, r10
 150         bgt     flush_levels
 151 finished:
 152         mov     r10, #0                         @ swith back to cache level 0
 153         mcr     p15, 2, r10, c0, c0, 0          @ select current cache level in cssr
 154         dsb     st
 155         isb
 156         bx      lr
 157 ENDPROC(v7_flush_dcache_all)
 158
 159 ENTRY(psci_disable_smp)
 160         mrc     p15, 0, r0, c1, c0, 1           @ ACTLR
 161         bic     r0, r0, #(1 << 6)               @ Clear SMP bit
 162         mcr     p15, 0, r0, c1, c0, 1           @ ACTLR
 163         isb
 164         dsb
 165         bx      lr
 166 ENDPROC(psci_disable_smp)
 167 .weak psci_disable_smp
 168
 169 ENTRY(psci_enable_smp)
 170         mrc     p15, 0, r0, c1, c0, 1           @ ACTLR
 171         orr     r0, r0, #(1 << 6)               @ Set SMP bit
 172         mcr     p15, 0, r0, c1, c0, 1           @ ACTLR
 173         isb
 174         bx      lr
 175 ENDPROC(psci_enable_smp)
 176 .weak psci_enable_smp
 177
 178 ENTRY(psci_cpu_off_common)
 179         push    {lr}
 180
 181         mrc     p15, 0, r0, c1, c0, 0           @ SCTLR
 182         bic     r0, r0, #(1 << 2)               @ Clear C bit
 183         mcr     p15, 0, r0, c1, c0, 0           @ SCTLR
 184         isb
 185         dsb
 186
 187         bl      v7_flush_dcache_all
 188
 189         clrex                                   @ Why???
 190
 191         bl      psci_disable_smp
 192
 193         pop     {lr}
 194         bx      lr
 195 ENDPROC(psci_cpu_off_common)
 196
 197 @ expects CPU ID in r0 and returns stack top in r0
 198 ENTRY(psci_get_cpu_stack_top)
 199         mov     r5, #0x400                      @ 1kB of stack per CPU
 200         mul     r0, r0, r5
 201
 202         ldr     r5, =psci_text_end              @ end of monitor text
 203         add     r5, r5, #0x2000                 @ Skip two pages
 204         lsr     r5, r5, #12                     @ Align to start of page
 205         lsl     r5, r5, #12
 206         sub     r5, r5, #4                      @ reserve 1 word for target PC
 207         sub     r0, r5, r0                      @ here's our stack!
 208
 209         bx      lr
 210 ENDPROC(psci_get_cpu_stack_top)
 211
 212 ENTRY(psci_cpu_entry)
 213         bl      psci_enable_smp
 214
 215         bl      _nonsec_init
 216
 217         bl      psci_get_cpu_id                 @ CPU ID => r0
 218         bl      psci_get_cpu_stack_top          @ stack top => r0
 219         ldr     r0, [r0]                        @ target PC at stack top
 220         b       _do_nonsec_entry
 221 ENDPROC(psci_cpu_entry)
 222
 223         .popsection