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_version)
  50 ENTRY(psci_cpu_suspend)
  51 ENTRY(psci_cpu_off)
  52 ENTRY(psci_cpu_on)
  53 ENTRY(psci_affinity_info)
  54 ENTRY(psci_migrate)
  55 ENTRY(psci_migrate_info_type)
  56 ENTRY(psci_migrate_info_up_cpu)
  57 ENTRY(psci_system_off)
  58 ENTRY(psci_system_reset)
  59 ENTRY(psci_features)
  60 ENTRY(psci_cpu_freeze)
  61 ENTRY(psci_cpu_default_suspend)
  62 ENTRY(psci_node_hw_state)
  63 ENTRY(psci_system_suspend)
  64 ENTRY(psci_set_suspend_mode)
  65 ENTRY(psi_stat_residency)
  66 ENTRY(psci_stat_count)
  67         mov     r0, #ARM_PSCI_RET_NI    @ Return -1 (Not Implemented)
  68         mov     pc, lr
  69 ENDPROC(psci_stat_count)
  70 ENDPROC(psi_stat_residency)
  71 ENDPROC(psci_set_suspend_mode)
  72 ENDPROC(psci_system_suspend)
  73 ENDPROC(psci_node_hw_state)
  74 ENDPROC(psci_cpu_default_suspend)
  75 ENDPROC(psci_cpu_freeze)
  76 ENDPROC(psci_features)
  77 ENDPROC(psci_system_reset)
  78 ENDPROC(psci_system_off)
  79 ENDPROC(psci_migrate_info_up_cpu)
  80 ENDPROC(psci_migrate_info_type)
  81 ENDPROC(psci_migrate)
  82 ENDPROC(psci_affinity_info)
  83 ENDPROC(psci_cpu_on)
  84 ENDPROC(psci_cpu_off)
  85 ENDPROC(psci_cpu_suspend)
  86 ENDPROC(psci_version)
  87 .weak psci_version
  88 .weak psci_cpu_suspend
  89 .weak psci_cpu_off
  90 .weak psci_cpu_on
  91 .weak psci_affinity_info
  92 .weak psci_migrate
  93 .weak psci_migrate_info_type
  94 .weak psci_migrate_info_up_cpu
  95 .weak psci_system_off
  96 .weak psci_system_reset
  97 .weak psci_features
  98 .weak psci_cpu_freeze
  99 .weak psci_cpu_default_suspend
 100 .weak psci_node_hw_state
 101 .weak psci_system_suspend
 102 .weak psci_set_suspend_mode
 103 .weak psi_stat_residency
 104 .weak psci_stat_count
 105
 106 _psci_table:
 107         .word   ARM_PSCI_FN_CPU_SUSPEND
 108         .word   psci_cpu_suspend
 109         .word   ARM_PSCI_FN_CPU_OFF
 110         .word   psci_cpu_off
 111         .word   ARM_PSCI_FN_CPU_ON
 112         .word   psci_cpu_on
 113         .word   ARM_PSCI_FN_MIGRATE
 114         .word   psci_migrate
 115         .word   ARM_PSCI_0_2_FN_PSCI_VERSION
 116         .word   psci_version
 117         .word   ARM_PSCI_0_2_FN_CPU_SUSPEND
 118         .word   psci_cpu_suspend
 119         .word   ARM_PSCI_0_2_FN_CPU_OFF
 120         .word   psci_cpu_off
 121         .word   ARM_PSCI_0_2_FN_CPU_ON
 122         .word   psci_cpu_on
 123         .word   ARM_PSCI_0_2_FN_AFFINITY_INFO
 124         .word   psci_affinity_info
 125         .word   ARM_PSCI_0_2_FN_MIGRATE
 126         .word   psci_migrate
 127         .word   ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE
 128         .word   psci_migrate_info_type
 129         .word   ARM_PSCI_0_2_FN_MIGRATE_INFO_UP_CPU
 130         .word   psci_migrate_info_up_cpu
 131         .word   ARM_PSCI_0_2_FN_SYSTEM_OFF
 132         .word   psci_system_off
 133         .word   ARM_PSCI_0_2_FN_SYSTEM_RESET
 134         .word   psci_system_reset
 135         .word   ARM_PSCI_1_0_FN_PSCI_FEATURES
 136         .word   psci_features
 137         .word   ARM_PSCI_1_0_FN_CPU_FREEZE
 138         .word   psci_cpu_freeze
 139         .word   ARM_PSCI_1_0_FN_CPU_DEFAULT_SUSPEND
 140         .word   psci_cpu_default_suspend
 141         .word   ARM_PSCI_1_0_FN_NODE_HW_STATE
 142         .word   psci_node_hw_state
 143         .word   ARM_PSCI_1_0_FN_SYSTEM_SUSPEND
 144         .word   psci_system_suspend
 145         .word   ARM_PSCI_1_0_FN_SET_SUSPEND_MODE
 146         .word   psci_set_suspend_mode
 147         .word   ARM_PSCI_1_0_FN_STAT_RESIDENCY
 148         .word   psi_stat_residency
 149         .word   ARM_PSCI_1_0_FN_STAT_COUNT
 150         .word   psci_stat_count
 151         .word   0
 152         .word   0
 153
 154 _smc_psci:
 155         push    {r4-r7,lr}
 156
 157         @ Switch to secure
 158         mrc     p15, 0, r7, c1, c1, 0
 159         bic     r4, r7, #1
 160         mcr     p15, 0, r4, c1, c1, 0
 161         isb
 162
 163         adr     r4, _psci_table
 164 1:      ldr     r5, [r4]                @ Load PSCI function ID
 165         ldr     r6, [r4, #4]            @ Load target PC
 166         cmp     r5, #0                  @ If reach the end, bail out
 167         moveq   r0, #ARM_PSCI_RET_INVAL @ Return -2 (Invalid)
 168         beq     2f
 169         cmp     r0, r5                  @ If not matching, try next entry
 170         addne   r4, r4, #8
 171         bne     1b
 172
 173         blx     r6                      @ Execute PSCI function
 174
 175         @ Switch back to non-secure
 176 2:      mcr     p15, 0, r7, c1, c1, 0
 177
 178         pop     {r4-r7, lr}
 179         movs    pc, lr                  @ Return to the kernel
 180
 181 @ Requires dense and single-cluster CPU ID space
 182 ENTRY(psci_get_cpu_id)
 183         mrc     p15, 0, r0, c0, c0, 5   /* read MPIDR */
 184         and     r0, r0, #0xff           /* return CPU ID in cluster */
 185         bx      lr
 186 ENDPROC(psci_get_cpu_id)
 187 .weak psci_get_cpu_id
 188
 189 /* Imported from Linux kernel */
 190 ENTRY(psci_v7_flush_dcache_all)
 191         stmfd   sp!, {r4-r5, r7, r9-r11, lr}
 192         dmb                                     @ ensure ordering with previous memory accesses
 193         mrc     p15, 1, r0, c0, c0, 1           @ read clidr
 194         ands    r3, r0, #0x7000000              @ extract loc from clidr
 195         mov     r3, r3, lsr #23                 @ left align loc bit field
 196         beq     finished                        @ if loc is 0, then no need to clean
 197         mov     r10, #0                         @ start clean at cache level 0
 198 flush_levels:
 199         add     r2, r10, r10, lsr #1            @ work out 3x current cache level
 200         mov     r1, r0, lsr r2                  @ extract cache type bits from clidr
 201         and     r1, r1, #7                      @ mask of the bits for current cache only
 202         cmp     r1, #2                          @ see what cache we have at this level
 203         blt     skip                            @ skip if no cache, or just i-cache
 204         mrs     r9, cpsr                        @ make cssr&csidr read atomic
 205         mcr     p15, 2, r10, c0, c0, 0          @ select current cache level in cssr
 206         isb                                     @ isb to sych the new cssr&csidr
 207         mrc     p15, 1, r1, c0, c0, 0           @ read the new csidr
 208         msr     cpsr_c, r9
 209         and     r2, r1, #7                      @ extract the length of the cache lines
 210         add     r2, r2, #4                      @ add 4 (line length offset)
 211         ldr     r4, =0x3ff
 212         ands    r4, r4, r1, lsr #3              @ find maximum number on the way size
 213         clz     r5, r4                          @ find bit position of way size increment
 214         ldr     r7, =0x7fff
 215         ands    r7, r7, r1, lsr #13             @ extract max number of the index size
 216 loop1:
 217         mov     r9, r7                          @ create working copy of max index
 218 loop2:
 219         orr     r11, r10, r4, lsl r5            @ factor way and cache number into r11
 220         orr     r11, r11, r9, lsl r2            @ factor index number into r11
 221         mcr     p15, 0, r11, c7, c14, 2         @ clean & invalidate by set/way
 222         subs    r9, r9, #1                      @ decrement the index
 223         bge     loop2
 224         subs    r4, r4, #1                      @ decrement the way
 225         bge     loop1
 226 skip:
 227         add     r10, r10, #2                    @ increment cache number
 228         cmp     r3, r10
 229         bgt     flush_levels
 230 finished:
 231         mov     r10, #0                         @ swith back to cache level 0
 232         mcr     p15, 2, r10, c0, c0, 0          @ select current cache level in cssr
 233         dsb     st
 234         isb
 235         ldmfd   sp!, {r4-r5, r7, r9-r11, lr}
 236         bx      lr
 237 ENDPROC(psci_v7_flush_dcache_all)
 238
 239 ENTRY(psci_disable_smp)
 240         mrc     p15, 0, r0, c1, c0, 1           @ ACTLR
 241         bic     r0, r0, #(1 << 6)               @ Clear SMP bit
 242         mcr     p15, 0, r0, c1, c0, 1           @ ACTLR
 243         isb
 244         dsb
 245         bx      lr
 246 ENDPROC(psci_disable_smp)
 247 .weak psci_disable_smp
 248
 249 ENTRY(psci_enable_smp)
 250         mrc     p15, 0, r0, c1, c0, 1           @ ACTLR
 251         orr     r0, r0, #(1 << 6)               @ Set SMP bit
 252         mcr     p15, 0, r0, c1, c0, 1           @ ACTLR
 253         isb
 254         bx      lr
 255 ENDPROC(psci_enable_smp)
 256 .weak psci_enable_smp
 257
 258 ENTRY(psci_cpu_off_common)
 259         push    {lr}
 260
 261         bl      psci_v7_flush_dcache_all
 262
 263         clrex                                   @ Why???
 264
 265         mrc     p15, 0, r0, c1, c0, 0           @ SCTLR
 266         bic     r0, r0, #(1 << 2)               @ Clear C bit
 267         mcr     p15, 0, r0, c1, c0, 0           @ SCTLR
 268         isb
 269         dsb
 270
 271         bl      psci_v7_flush_dcache_all
 272
 273         clrex                                   @ Why???
 274
 275         bl      psci_disable_smp
 276
 277         pop     {lr}
 278         bx      lr
 279 ENDPROC(psci_cpu_off_common)
 280
 281 @ The stacks are allocated in reverse order, i.e.
 282 @ the stack for CPU0 has the highest memory address.
 283 @
 284 @ --------------------  __secure_stack_end
 285 @ |  CPU0 target PC  |
 286 @ |------------------|
 287 @ |                  |
 288 @ |    CPU0 stack    |
 289 @ |                  |
 290 @ |------------------|  __secure_stack_end - 1KB
 291 @ |        .         |
 292 @ |        .         |
 293 @ |        .         |
 294 @ |        .         |
 295 @ --------------------  __secure_stack_start
 296 @
 297 @ This expects CPU ID in r0 and returns stack top in r0
 298 LENTRY(psci_get_cpu_stack_top)
 299         @ stack top = __secure_stack_end - (cpuid << ARM_PSCI_STACK_SHIFT)
 300         ldr     r3, =__secure_stack_end
 301         sub     r0, r3, r0, LSL #ARM_PSCI_STACK_SHIFT
 302         sub     r0, r0, #4              @ Save space for target PC
 303         bx      lr
 304 ENDPROC(psci_get_cpu_stack_top)
 305
 306 @ {r0, r1, r2, ip} from _do_nonsec_entry(kernel_entry, 0, machid, r2) in
 307 @ arch/arm/lib/bootm.c:boot_jump_linux() must remain unchanged across
 308 @ this function.
 309 ENTRY(psci_stack_setup)
 310         mov     r6, lr
 311         mov     r7, r0
 312         bl      psci_get_cpu_id         @ CPU ID => r0
 313         bl      psci_get_cpu_stack_top  @ stack top => r0
 314         mov     sp, r0
 315         mov     r0, r7
 316         bx      r6
 317 ENDPROC(psci_stack_setup)
 318
 319 ENTRY(psci_arch_init)
 320         mov     pc, lr
 321 ENDPROC(psci_arch_init)
 322 .weak psci_arch_init
 323
 324 ENTRY(psci_cpu_entry)
 325         bl      psci_enable_smp
 326
 327         bl      _nonsec_init
 328
 329         bl      psci_get_cpu_id                 @ CPU ID => r0
 330         mov     r2, r0                          @ CPU ID => r2
 331         bl      psci_get_context_id             @ context id => r0
 332         mov     r1, r0                          @ context id => r1
 333         mov     r0, r2                          @ CPU ID => r0
 334         bl      psci_get_target_pc              @ target PC => r0
 335         b       _do_nonsec_entry
 336 ENDPROC(psci_cpu_entry)
 337
 338         .popsection