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         WEAK(__fn); \
  16         mov     w0, #ARM_PSCI_RET_NI; \
  17         ret; \
  18         ENDPROC(__fn); \
  19
  20 /* PSCI function and ID table definition*/
  21 #define PSCI_TABLE(__id, __fn) \
  22         .quad __id; \
  23         .quad __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 PSCI_DEFAULT(psci_system_reset2_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(ARM_PSCI_1_1_FN64_SYSTEM_RESET2, psci_system_reset2_64)
  99 PSCI_TABLE(0, 0)
 100
 101 .macro  psci_enter
 102         /* PSCI call is Fast Call(atomic), so mask DAIF */
 103         mrs     x15, DAIF
 104         stp     x15, xzr, [sp, #-16]!
 105         ldr     x15, =0x3C0
 106         msr     DAIF, x15
 107         /* SMC convention, x18 ~ x30 should be saved by callee */
 108         stp     x29, x30, [sp, #-16]!
 109         stp     x27, x28, [sp, #-16]!
 110         stp     x25, x26, [sp, #-16]!
 111         stp     x23, x24, [sp, #-16]!
 112         stp     x21, x22, [sp, #-16]!
 113         stp     x19, x20, [sp, #-16]!
 114         mrs     x15, elr_el3
 115         stp     x18, x15, [sp, #-16]!
 116 .endm
 117
 118 .macro  psci_return
 119         /* restore registers */
 120         ldp     x18, x15, [sp], #16
 121         msr     elr_el3, x15
 122         ldp     x19, x20, [sp], #16
 123         ldp     x21, x22, [sp], #16
 124         ldp     x23, x24, [sp], #16
 125         ldp     x25, x26, [sp], #16
 126         ldp     x27, x28, [sp], #16
 127         ldp     x29, x30, [sp], #16
 128         /* restore DAIF */
 129         ldp     x15, xzr, [sp], #16
 130         msr     DAIF, x15
 131         eret
 132 .endm
 133
 134 /* Caller must put PSCI function-ID table base in x9 */
 135 handle_psci:
 136         psci_enter
 137 1:      ldr     x10, [x9]               /* Load PSCI function table */
 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, #16                 /* If not match, try next entry */
 142         b       1b
 143
 144 2:      ldr     x11, [x9, #8]           /* Load PSCI function */
 145         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 WEAK(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
 226 /* CPU ID input in x0, stack top output in x0*/
 227 LENTRY(psci_get_cpu_stack_top)
 228         adr     x9, __secure_stack_end
 229         lsl     x0, x0, #ARM_PSCI_STACK_SHIFT
 230         sub     x0, x9, x0
 231         ret
 232 ENDPROC(psci_get_cpu_stack_top)
 233
 234 unhandled_exception:
 235         b       unhandled_exception     /* simply dead loop */
 236
 237 handle_sync:
 238         mov     x15, x30
 239         mov     x14, x0
 240
 241         bl      psci_get_cpu_id
 242         bl      psci_get_cpu_stack_top
 243         mov     x9, #1
 244         msr     spsel, x9
 245         mov     sp, x0
 246
 247         mov     x0, x14
 248         mov     x30, x15
 249
 250         mrs     x9, esr_el3
 251         ubfx    x9, x9, #26, #6
 252         cmp     x9, #0x13
 253         b.eq    handle_smc32
 254         cmp     x9, #0x17
 255         b.eq    handle_smc64
 256
 257         b       unhandled_exception
 258
 259 #ifdef CONFIG_ARMV8_EA_EL3_FIRST
 260 /*
 261  * Override this function if custom error handling is
 262  * needed for asynchronous aborts
 263  */
 264 WEAK(plat_error_handler)
 265         ret
 266 ENDPROC(plat_error_handler)
 267
 268 handle_error:
 269         bl      psci_get_cpu_id
 270         bl      psci_get_cpu_stack_top
 271         mov     x9, #1
 272         msr     spsel, x9
 273         mov     sp, x0
 274
 275         bl      plat_error_handler      /* Platform specific error handling */
 276 deadloop:
 277         b       deadloop                /* Never return */
 278 #endif
 279
 280         .align  11
 281         .globl  el3_exception_vectors
 282 el3_exception_vectors:
 283         b       unhandled_exception     /* Sync, Current EL using SP0 */
 284         .align  7
 285         b       unhandled_exception     /* IRQ, Current EL using SP0 */
 286         .align  7
 287         b       unhandled_exception     /* FIQ, Current EL using SP0 */
 288         .align  7
 289         b       unhandled_exception     /* SError, Current EL using SP0 */
 290         .align  7
 291         b       unhandled_exception     /* Sync, Current EL using SPx */
 292         .align  7
 293         b       unhandled_exception     /* IRQ, Current EL using SPx */
 294         .align  7
 295         b       unhandled_exception     /* FIQ, Current EL using SPx */
 296         .align  7
 297         b       unhandled_exception     /* SError, Current EL using SPx */
 298         .align  7
 299         b       handle_sync             /* Sync, Lower EL using AArch64 */
 300         .align  7
 301         b       unhandled_exception     /* IRQ, Lower EL using AArch64 */
 302         .align  7
 303         b       unhandled_exception     /* FIQ, Lower EL using AArch64 */
 304         .align  7
 305 #ifdef CONFIG_ARMV8_EA_EL3_FIRST
 306         b       handle_error            /* SError, Lower EL using AArch64 */
 307 #else
 308         b       unhandled_exception     /* SError, Lower EL using AArch64 */
 309 #endif
 310         .align  7
 311         b       unhandled_exception     /* Sync, Lower EL using AArch32 */
 312         .align  7
 313         b       unhandled_exception     /* IRQ, Lower EL using AArch32 */
 314         .align  7
 315         b       unhandled_exception     /* FIQ, Lower EL using AArch32 */
 316         .align  7
 317         b       unhandled_exception     /* SError, Lower EL using AArch32 */
 318
 319 ENTRY(psci_setup_vectors)
 320         adr     x0, el3_exception_vectors
 321         msr     vbar_el3, x0
 322         ret
 323 ENDPROC(psci_setup_vectors)
 324
 325 WEAK(psci_arch_init)
 326         ret
 327 ENDPROC(psci_arch_init)
 328
 329 .popsection