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         .quad __id; \
  24         .quad __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         cbz     x10, 3f                 /* If reach the end, bail out */
 138         cmp     x10, x0
 139         b.eq    2f                      /* PSCI function found */
 140         add x9, x9, #16                 /* If not match, try next entry */
 141         b       1b
 142
 143 2:      ldr     x11, [x9, #8]           /* Load PSCI function */
 144         blr     x11                     /* Call PSCI function */
 145         psci_return
 146
 147 3:      mov     x0, #ARM_PSCI_RET_NI
 148         psci_return
 149
 150 /*
 151  * Handle SiP service functions defined in SiP service function table.
 152  * Use DECLARE_SECURE_SVC(_name, _id, _fn) to add platform specific SiP
 153  * service function into the SiP service function table.
 154  * SiP service function table is located in '._secure_svc_tbl_entries' section,
 155  * which is next to '._secure.text' section.
 156  */
 157 handle_svc:
 158         adr     x9, __secure_svc_tbl_start
 159         adr     x10, __secure_svc_tbl_end
 160         subs    x12, x10, x9    /* Get number of entries in table */
 161         b.eq    2f              /* Make sure SiP function table is not empty */
 162         psci_enter
 163 1:      ldr x10, [x9]           /* Load SiP function table */
 164         ldr x11, [x9, #8]
 165         cmp     w10, w0
 166         b.eq    2b              /* SiP service function found */
 167         add x9, x9, #SECURE_SVC_TBL_OFFSET      /* Move to next entry */
 168         subs    x12, x12, #SECURE_SVC_TBL_OFFSET
 169         b.eq    3b              /* If reach the end, bail out */
 170         b       1b
 171 2:      ldr     x0, =0xFFFFFFFF
 172         eret
 173
 174 handle_smc32:
 175         /* SMC function ID  0x84000000-0x8400001F: 32 bits PSCI */
 176         ldr     w9, =0x8400001F
 177         cmp     w0, w9
 178         b.gt    handle_svc
 179         ldr     w9, =0x84000000
 180         cmp     w0, w9
 181         b.lt    handle_svc
 182
 183         adr     x9, _psci_32_table
 184         b       handle_psci
 185
 186 handle_smc64:
 187         /* check SMC32 or SMC64 calls */
 188         ubfx    x9, x0, #30, #1
 189         cbz     x9, handle_smc32
 190
 191         /* SMC function ID 0xC4000000-0xC400001F: 64 bits PSCI */
 192         ldr     x9, =0xC400001F
 193         cmp     x0, x9
 194         b.gt    handle_svc
 195         ldr     x9, =0xC4000000
 196         cmp     x0, x9
 197         b.lt    handle_svc
 198
 199         adr     x9, _psci_64_table
 200         b       handle_psci
 201
 202 /*
 203  * Get CPU ID from MPIDR, suppose every cluster has same number of CPU cores,
 204  * Platform with asymmetric clusters should implement their own interface.
 205  * In case this function being called by other platform's C code, the ARM
 206  * Architecture Procedure Call Standard is considered, e.g. register X0 is
 207  * used for the return value, while in this PSCI environment, X0 usually holds
 208  * the SMC function identifier, so X0 should be saved by caller function.
 209  */
 210 ENTRY(psci_get_cpu_id)
 211 #ifdef CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
 212         mrs     x9, MPIDR_EL1
 213         ubfx    x9, x9, #8, #8
 214         ldr     x10, =CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
 215         mul     x9, x10, x9
 216 #else
 217         mov     x9, xzr
 218 #endif
 219         mrs     x10, MPIDR_EL1
 220         ubfx    x10, x10, #0, #8
 221         add     x0, x10, x9
 222         ret
 223 ENDPROC(psci_get_cpu_id)
 224 .weak 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 ENTRY(plat_error_handler)
 265         ret
 266 ENDPROC(plat_error_handler)
 267 .weak plat_error_handler
 268
 269 handle_error:
 270         bl      psci_get_cpu_id
 271         bl      psci_get_cpu_stack_top
 272         mov     x9, #1
 273         msr     spsel, x9
 274         mov     sp, x0
 275
 276         bl      plat_error_handler      /* Platform specific error handling */
 277 deadloop:
 278         b       deadloop                /* Never return */
 279 #endif
 280
 281         .align  11
 282         .globl  el3_exception_vectors
 283 el3_exception_vectors:
 284         b       unhandled_exception     /* Sync, Current EL using SP0 */
 285         .align  7
 286         b       unhandled_exception     /* IRQ, Current EL using SP0 */
 287         .align  7
 288         b       unhandled_exception     /* FIQ, Current EL using SP0 */
 289         .align  7
 290         b       unhandled_exception     /* SError, Current EL using SP0 */
 291         .align  7
 292         b       unhandled_exception     /* Sync, Current EL using SPx */
 293         .align  7
 294         b       unhandled_exception     /* IRQ, Current EL using SPx */
 295         .align  7
 296         b       unhandled_exception     /* FIQ, Current EL using SPx */
 297         .align  7
 298         b       unhandled_exception     /* SError, Current EL using SPx */
 299         .align  7
 300         b       handle_sync             /* Sync, Lower EL using AArch64 */
 301         .align  7
 302         b       unhandled_exception     /* IRQ, Lower EL using AArch64 */
 303         .align  7
 304         b       unhandled_exception     /* FIQ, Lower EL using AArch64 */
 305         .align  7
 306 #ifdef CONFIG_ARMV8_EA_EL3_FIRST
 307         b       handle_error            /* SError, Lower EL using AArch64 */
 308 #else
 309         b       unhandled_exception     /* SError, Lower EL using AArch64 */
 310 #endif
 311         .align  7
 312         b       unhandled_exception     /* Sync, Lower EL using AArch32 */
 313         .align  7
 314         b       unhandled_exception     /* IRQ, Lower EL using AArch32 */
 315         .align  7
 316         b       unhandled_exception     /* FIQ, Lower EL using AArch32 */
 317         .align  7
 318         b       unhandled_exception     /* SError, Lower EL using AArch32 */
 319
 320 ENTRY(psci_setup_vectors)
 321         adr     x0, el3_exception_vectors
 322         msr     vbar_el3, x0
 323         ret
 324 ENDPROC(psci_setup_vectors)
 325
 326 ENTRY(psci_arch_init)
 327         ret
 328 ENDPROC(psci_arch_init)
 329 .weak psci_arch_init
 330
 331 .popsection