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