LXR uboot/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
  39ENTRY(psci_fiq_enter)
  40        movs    pc, lr
  41ENDPROC(psci_fiq_enter)
  42.weak psci_fiq_enter
  43
  44ENTRY(default_psci_vector)
  45        movs    pc, lr
  46ENDPROC(default_psci_vector)
  47.weak default_psci_vector
  48
  49ENTRY(psci_version)
  50ENTRY(psci_cpu_suspend)
  51ENTRY(psci_cpu_off)
  52ENTRY(psci_cpu_on)
  53ENTRY(psci_affinity_info)
  54ENTRY(psci_migrate)
  55ENTRY(psci_migrate_info_type)
  56ENTRY(psci_migrate_info_up_cpu)
  57ENTRY(psci_system_off)
  58ENTRY(psci_system_reset)
  59ENTRY(psci_features)
  60ENTRY(psci_cpu_freeze)
  61ENTRY(psci_cpu_default_suspend)
  62ENTRY(psci_node_hw_state)
  63ENTRY(psci_system_suspend)
  64ENTRY(psci_set_suspend_mode)
  65ENTRY(psi_stat_residency)
  66ENTRY(psci_stat_count)
  67        mov     r0, #ARM_PSCI_RET_NI    @ Return -1 (Not Implemented)
  68        mov     pc, lr
  69ENDPROC(psci_stat_count)
  70ENDPROC(psi_stat_residency)
  71ENDPROC(psci_set_suspend_mode)
  72ENDPROC(psci_system_suspend)
  73ENDPROC(psci_node_hw_state)
  74ENDPROC(psci_cpu_default_suspend)
  75ENDPROC(psci_cpu_freeze)
  76ENDPROC(psci_features)
  77ENDPROC(psci_system_reset)
  78ENDPROC(psci_system_off)
  79ENDPROC(psci_migrate_info_up_cpu)
  80ENDPROC(psci_migrate_info_type)
  81ENDPROC(psci_migrate)
  82ENDPROC(psci_affinity_info)
  83ENDPROC(psci_cpu_on)
  84ENDPROC(psci_cpu_off)
  85ENDPROC(psci_cpu_suspend)
  86ENDPROC(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
 1641:      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
 1762:      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
 182ENTRY(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
 186ENDPROC(psci_get_cpu_id)
 187.weak psci_get_cpu_id
 188
 189/* Imported from Linux kernel */
 190ENTRY(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
 198flush_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
 216loop1:
 217        mov     r9, r7                          @ create working copy of max index
 218loop2:
 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
 226skip:
 227        add     r10, r10, #2                    @ increment cache number
 228        cmp     r3, r10
 229        bgt     flush_levels
 230finished:
 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
 237ENDPROC(psci_v7_flush_dcache_all)
 238
 239ENTRY(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
 246ENDPROC(psci_disable_smp)
 247.weak psci_disable_smp
 248
 249ENTRY(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
 255ENDPROC(psci_enable_smp)
 256.weak psci_enable_smp
 257
 258ENTRY(psci_cpu_off_common)
 259        push    {lr}
 260
 261        mrc     p15, 0, r0, c1, c0, 0           @ SCTLR
 262        bic     r0, r0, #(1 << 2)               @ Clear C bit
 263        mcr     p15, 0, r0, c1, c0, 0           @ SCTLR
 264        isb
 265        dsb
 266
 267        bl      psci_v7_flush_dcache_all
 268
 269        clrex                                   @ Why???
 270
 271        bl      psci_disable_smp
 272
 273        pop     {lr}
 274        bx      lr
 275ENDPROC(psci_cpu_off_common)
 276
 277@ The stacks are allocated in reverse order, i.e.
 278@ the stack for CPU0 has the highest memory address.
 279@
 280@ --------------------  __secure_stack_end
 281@ |  CPU0 target PC  |
 282@ |------------------|
 283@ |                  |
 284@ |    CPU0 stack    |
 285@ |                  |
 286@ |------------------|  __secure_stack_end - 1KB
 287@ |        .         |
 288@ |        .         |
 289@ |        .         |
 290@ |        .         |
 291@ --------------------  __secure_stack_start
 292@
 293@ This expects CPU ID in r0 and returns stack top in r0
 294LENTRY(psci_get_cpu_stack_top)
 295        @ stack top = __secure_stack_end - (cpuid << ARM_PSCI_STACK_SHIFT)
 296        ldr     r3, =__secure_stack_end
 297        sub     r0, r3, r0, LSL #ARM_PSCI_STACK_SHIFT
 298        sub     r0, r0, #4              @ Save space for target PC
 299        bx      lr
 300ENDPROC(psci_get_cpu_stack_top)
 301
 302@ {r0, r1, r2, ip} from _do_nonsec_entry(kernel_entry, 0, machid, r2) in
 303@ arch/arm/lib/bootm.c:boot_jump_linux() must remain unchanged across
 304@ this function.
 305ENTRY(psci_stack_setup)
 306        mov     r6, lr
 307        mov     r7, r0
 308        bl      psci_get_cpu_id         @ CPU ID => r0
 309        bl      psci_get_cpu_stack_top  @ stack top => r0
 310        mov     sp, r0
 311        mov     r0, r7
 312        bx      r6
 313ENDPROC(psci_stack_setup)
 314
 315ENTRY(psci_arch_init)
 316        mov     pc, lr
 317ENDPROC(psci_arch_init)
 318.weak psci_arch_init
 319
 320ENTRY(psci_cpu_entry)
 321        bl      psci_enable_smp
 322
 323        bl      _nonsec_init
 324
 325        bl      psci_get_cpu_id                 @ CPU ID => r0
 326        bl      psci_get_target_pc              @ target PC => r0
 327        b       _do_nonsec_entry
 328ENDPROC(psci_cpu_entry)
 329
 330        .popsection
 331