git.sur5r.net Git - u-boot/blob - arch/arm/cpu/armv8/psci.S

   1 /* SPDX-License-Identifier: GPL-2.0+ */
   2 /*
   3  * Copyright 2016 Freescale Semiconductor, Inc.
   4  * Author: Hongbo Zhang <hongbo.zhang@nxp.com>
   5  * This file implements LS102X platform PSCI SYSTEM-SUSPEND function
   6  */
   7
   8 #include <config.h>
   9 #include <linux/linkage.h>
  10 #include <asm/psci.h>
  11
  12 /* Default PSCI function, return -1, Not Implemented */
  13 #define PSCI_DEFAULT(__fn) \
  14         ENTRY(__fn); \
  15         mov     w0, #ARM_PSCI_RET_NI; \
  16         ret; \
  17         ENDPROC(__fn); \
  18         .weak __fn
  19
  20 /* PSCI function and ID table definition*/
  21 #define PSCI_TABLE(__id, __fn) \
  22         .word __id; \
  23         .word __fn
  24
  25 .pushsection ._secure.text, "ax"
  26
  27 /* 32 bits PSCI default functions */
  28 PSCI_DEFAULT(psci_version)
  29 PSCI_DEFAULT(psci_cpu_suspend)
  30 PSCI_DEFAULT(psci_cpu_off)
  31 PSCI_DEFAULT(psci_cpu_on)
  32 PSCI_DEFAULT(psci_affinity_info)
  33 PSCI_DEFAULT(psci_migrate)
  34 PSCI_DEFAULT(psci_migrate_info_type)
  35 PSCI_DEFAULT(psci_migrate_info_up_cpu)
  36 PSCI_DEFAULT(psci_system_off)
  37 PSCI_DEFAULT(psci_system_reset)
  38 PSCI_DEFAULT(psci_features)
  39 PSCI_DEFAULT(psci_cpu_freeze)
  40 PSCI_DEFAULT(psci_cpu_default_suspend)
  41 PSCI_DEFAULT(psci_node_hw_state)
  42 PSCI_DEFAULT(psci_system_suspend)
  43 PSCI_DEFAULT(psci_set_suspend_mode)
  44 PSCI_DEFAULT(psi_stat_residency)
  45 PSCI_DEFAULT(psci_stat_count)
  46
  47 .align 3
  48 _psci_32_table:
  49 PSCI_TABLE(ARM_PSCI_FN_CPU_SUSPEND, psci_cpu_suspend)
  50 PSCI_TABLE(ARM_PSCI_FN_CPU_OFF, psci_cpu_off)
  51 PSCI_TABLE(ARM_PSCI_FN_CPU_ON, psci_cpu_on)
  52 PSCI_TABLE(ARM_PSCI_FN_MIGRATE, psci_migrate)
  53 PSCI_TABLE(ARM_PSCI_0_2_FN_PSCI_VERSION, psci_version)
  54 PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_SUSPEND, psci_cpu_suspend)
  55 PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_OFF, psci_cpu_off)
  56 PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_ON, psci_cpu_on)
  57 PSCI_TABLE(ARM_PSCI_0_2_FN_AFFINITY_INFO, psci_affinity_info)
  58 PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE, psci_migrate)
  59 PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE, psci_migrate_info_type)
  60 PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE_INFO_UP_CPU, psci_migrate_info_up_cpu)
  61 PSCI_TABLE(ARM_PSCI_0_2_FN_SYSTEM_OFF, psci_system_off)
  62 PSCI_TABLE(ARM_PSCI_0_2_FN_SYSTEM_RESET, psci_system_reset)
  63 PSCI_TABLE(ARM_PSCI_1_0_FN_PSCI_FEATURES, psci_features)
  64 PSCI_TABLE(ARM_PSCI_1_0_FN_CPU_FREEZE, psci_cpu_freeze)
  65 PSCI_TABLE(ARM_PSCI_1_0_FN_CPU_DEFAULT_SUSPEND, psci_cpu_default_suspend)
  66 PSCI_TABLE(ARM_PSCI_1_0_FN_NODE_HW_STATE, psci_node_hw_state)
  67 PSCI_TABLE(ARM_PSCI_1_0_FN_SYSTEM_SUSPEND, psci_system_suspend)
  68 PSCI_TABLE(ARM_PSCI_1_0_FN_SET_SUSPEND_MODE, psci_set_suspend_mode)
  69 PSCI_TABLE(ARM_PSCI_1_0_FN_STAT_RESIDENCY, psi_stat_residency)
  70 PSCI_TABLE(ARM_PSCI_1_0_FN_STAT_COUNT, psci_stat_count)
  71 PSCI_TABLE(0, 0)
  72
  73 /* 64 bits PSCI default functions */
  74 PSCI_DEFAULT(psci_cpu_suspend_64)
  75 PSCI_DEFAULT(psci_cpu_on_64)
  76 PSCI_DEFAULT(psci_affinity_info_64)
  77 PSCI_DEFAULT(psci_migrate_64)
  78 PSCI_DEFAULT(psci_migrate_info_up_cpu_64)
  79 PSCI_DEFAULT(psci_cpu_default_suspend_64)
  80 PSCI_DEFAULT(psci_node_hw_state_64)
  81 PSCI_DEFAULT(psci_system_suspend_64)
  82 PSCI_DEFAULT(psci_stat_residency_64)
  83 PSCI_DEFAULT(psci_stat_count_64)
  84
  85 .align 3
  86 _psci_64_table:
  87 PSCI_TABLE(ARM_PSCI_0_2_FN64_CPU_SUSPEND, psci_cpu_suspend_64)
  88 PSCI_TABLE(ARM_PSCI_0_2_FN64_CPU_ON, psci_cpu_on_64)
  89 PSCI_TABLE(ARM_PSCI_0_2_FN64_AFFINITY_INFO, psci_affinity_info_64)
  90 PSCI_TABLE(ARM_PSCI_0_2_FN64_MIGRATE, psci_migrate_64)
  91 PSCI_TABLE(ARM_PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU, psci_migrate_info_up_cpu_64)
  92 PSCI_TABLE(ARM_PSCI_1_0_FN64_CPU_DEFAULT_SUSPEND, psci_cpu_default_suspend_64)
  93 PSCI_TABLE(ARM_PSCI_1_0_FN64_NODE_HW_STATE, psci_node_hw_state_64)
  94 PSCI_TABLE(ARM_PSCI_1_0_FN64_SYSTEM_SUSPEND, psci_system_suspend_64)
  95 PSCI_TABLE(ARM_PSCI_1_0_FN64_STAT_RESIDENCY, psci_stat_residency_64)
  96 PSCI_TABLE(ARM_PSCI_1_0_FN64_STAT_COUNT, psci_stat_count_64)
  97 PSCI_TABLE(0, 0)
  98
  99 .macro  psci_enter
 100         /* PSCI call is Fast Call(atomic), so mask DAIF */
 101         mrs     x15, DAIF
 102         stp     x15, xzr, [sp, #-16]!
 103         ldr     x15, =0x3C0
 104         msr     DAIF, x15
 105         /* SMC convention, x18 ~ x30 should be saved by callee */
 106         stp     x29, x30, [sp, #-16]!
 107         stp     x27, x28, [sp, #-16]!
 108         stp     x25, x26, [sp, #-16]!
 109         stp     x23, x24, [sp, #-16]!
 110         stp     x21, x22, [sp, #-16]!
 111         stp     x19, x20, [sp, #-16]!
 112         mrs     x15, elr_el3
 113         stp     x18, x15, [sp, #-16]!
 114 .endm
 115
 116 .macro  psci_return
 117         /* restore registers */
 118         ldp     x18, x15, [sp], #16
 119         msr     elr_el3, x15
 120         ldp     x19, x20, [sp], #16
 121         ldp     x21, x22, [sp], #16
 122         ldp     x23, x24, [sp], #16
 123         ldp     x25, x26, [sp], #16
 124         ldp     x27, x28, [sp], #16
 125         ldp     x29, x30, [sp], #16
 126         /* restore DAIF */
 127         ldp     x15, xzr, [sp], #16
 128         msr     DAIF, x15
 129         eret
 130 .endm
 131
 132 /* Caller must put PSCI function-ID table base in x9 */
 133 handle_psci:
 134         psci_enter
 135 1:      ldr x10, [x9]                   /* Load PSCI function table */
 136         ubfx x11, x10, #32, #32
 137         ubfx x10, x10, #0, #32
 138         cbz     x10, 3f                 /* If reach the end, bail out */
 139         cmp     x10, x0
 140         b.eq    2f                      /* PSCI function found */
 141         add x9, x9, #8                  /* If not match, try next entry */
 142         b       1b
 143
 144 2:      blr     x11                     /* Call PSCI function */
 145         psci_return
 146
 147 3:      mov     x0, #ARM_PSCI_RET_NI
 148         psci_return
 149
 150 unknown_smc_id:
 151         ldr     x0, =0xFFFFFFFF
 152         eret
 153
 154 handle_smc32:
 155         /* SMC function ID  0x84000000-0x8400001F: 32 bits PSCI */
 156         ldr     w9, =0x8400001F
 157         cmp     w0, w9
 158         b.gt    unknown_smc_id
 159         ldr     w9, =0x84000000
 160         cmp     w0, w9
 161         b.lt    unknown_smc_id
 162
 163         adr     x9, _psci_32_table
 164         b       handle_psci
 165
 166 handle_smc64:
 167         /* check SMC32 or SMC64 calls */
 168         ubfx    x9, x0, #30, #1
 169         cbz     x9, handle_smc32
 170
 171         /* SMC function ID 0xC4000000-0xC400001F: 64 bits PSCI */
 172         ldr     x9, =0xC400001F
 173         cmp     x0, x9
 174         b.gt    unknown_smc_id
 175         ldr     x9, =0xC4000000
 176         cmp     x0, x9
 177         b.lt    unknown_smc_id
 178
 179         adr     x9, _psci_64_table
 180         b       handle_psci
 181
 182 /*
 183  * Get CPU ID from MPIDR, suppose every cluster has same number of CPU cores,
 184  * Platform with asymmetric clusters should implement their own interface.
 185  * In case this function being called by other platform's C code, the ARM
 186  * Architecture Procedure Call Standard is considered, e.g. register X0 is
 187  * used for the return value, while in this PSCI environment, X0 usually holds
 188  * the SMC function identifier, so X0 should be saved by caller function.
 189  */
 190 ENTRY(psci_get_cpu_id)
 191 #ifdef CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
 192         mrs     x9, MPIDR_EL1
 193         ubfx    x9, x9, #8, #8
 194         ldr     x10, =CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
 195         mul     x9, x10, x9
 196 #else
 197         mov     x9, xzr
 198 #endif
 199         mrs     x10, MPIDR_EL1
 200         ubfx    x10, x10, #0, #8
 201         add     x0, x10, x9
 202         ret
 203 ENDPROC(psci_get_cpu_id)
 204 .weak psci_get_cpu_id
 205
 206 /* CPU ID input in x0, stack top output in x0*/
 207 LENTRY(psci_get_cpu_stack_top)
 208         adr     x9, __secure_stack_end
 209         lsl     x0, x0, #ARM_PSCI_STACK_SHIFT
 210         sub     x0, x9, x0
 211         ret
 212 ENDPROC(psci_get_cpu_stack_top)
 213
 214 unhandled_exception:
 215         b       unhandled_exception     /* simply dead loop */
 216
 217 handle_sync:
 218         mov     x15, x30
 219         mov     x14, x0
 220
 221         bl      psci_get_cpu_id
 222         bl      psci_get_cpu_stack_top
 223         mov     x9, #1
 224         msr     spsel, x9
 225         mov     sp, x0
 226
 227         mov     x0, x14
 228         mov     x30, x15
 229
 230         mrs     x9, esr_el3
 231         ubfx    x9, x9, #26, #6
 232         cmp     x9, #0x13
 233         b.eq    handle_smc32
 234         cmp     x9, #0x17
 235         b.eq    handle_smc64
 236
 237         b       unhandled_exception
 238
 239         .align  11
 240         .globl  el3_exception_vectors
 241 el3_exception_vectors:
 242         b       unhandled_exception     /* Sync, Current EL using SP0 */
 243         .align  7
 244         b       unhandled_exception     /* IRQ, Current EL using SP0 */
 245         .align  7
 246         b       unhandled_exception     /* FIQ, Current EL using SP0 */
 247         .align  7
 248         b       unhandled_exception     /* SError, Current EL using SP0 */
 249         .align  7
 250         b       unhandled_exception     /* Sync, Current EL using SPx */
 251         .align  7
 252         b       unhandled_exception     /* IRQ, Current EL using SPx */
 253         .align  7
 254         b       unhandled_exception     /* FIQ, Current EL using SPx */
 255         .align  7
 256         b       unhandled_exception     /* SError, Current EL using SPx */
 257         .align  7
 258         b       handle_sync             /* Sync, Lower EL using AArch64 */
 259         .align  7
 260         b       unhandled_exception     /* IRQ, Lower EL using AArch64 */
 261         .align  7
 262         b       unhandled_exception     /* FIQ, Lower EL using AArch64 */
 263         .align  7
 264         b       unhandled_exception     /* SError, Lower EL using AArch64 */
 265         .align  7
 266         b       unhandled_exception     /* Sync, Lower EL using AArch32 */
 267         .align  7
 268         b       unhandled_exception     /* IRQ, Lower EL using AArch32 */
 269         .align  7
 270         b       unhandled_exception     /* FIQ, Lower EL using AArch32 */
 271         .align  7
 272         b       unhandled_exception     /* SError, Lower EL using AArch32 */
 273
 274 ENTRY(psci_setup_vectors)
 275         adr     x0, el3_exception_vectors
 276         msr     vbar_el3, x0
 277         ret
 278 ENDPROC(psci_setup_vectors)
 279
 280 ENTRY(psci_arch_init)
 281         ret
 282 ENDPROC(psci_arch_init)
 283 .weak psci_arch_init
 284
 285 .popsection