arch/arm/cpu/armv8/psci.S

   1 /*
   2  * Copyright 2016 Freescale Semiconductor, Inc.
   3  * Author: Hongbo Zhang <hongbo.zhang@nxp.com>
   4  *
   5  * SPDX-License-Identifier:     GPL-2.0+
   6  * This file implements LS102X platform PSCI SYSTEM-SUSPEND function
   7  */
   8
   9 #include <config.h>
  10 #include <linux/linkage.h>
  11 #include <asm/psci.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 unknown_smc_id:
 152         ldr     x0, =0xFFFFFFFF
 153         eret
 154
 155 handle_smc32:
 156         /* SMC function ID  0x84000000-0x8400001F: 32 bits PSCI */
 157         ldr     w9, =0x8400001F
 158         cmp     w0, w9
 159         b.gt    unknown_smc_id
 160         ldr     w9, =0x84000000
 161         cmp     w0, w9
 162         b.lt    unknown_smc_id
 163
 164         adr     x9, _psci_32_table
 165         b       handle_psci
 166
 167 handle_smc64:
 168         /* check SMC32 or SMC64 calls */
 169         ubfx    x9, x0, #30, #1
 170         cbz     x9, handle_smc32
 171
 172         /* SMC function ID 0xC4000000-0xC400001F: 64 bits PSCI */
 173         ldr     x9, =0xC400001F
 174         cmp     x0, x9
 175         b.gt    unknown_smc_id
 176         ldr     x9, =0xC4000000
 177         cmp     x0, x9
 178         b.lt    unknown_smc_id
 179
 180         adr     x9, _psci_64_table
 181         b       handle_psci
 182
 183 /*
 184  * Get CPU ID from MPIDR, suppose every cluster has same number of CPU cores,
 185  * Platform with asymmetric clusters should implement their own interface.
 186  * In case this function being called by other platform's C code, the ARM
 187  * Architecture Procedure Call Standard is considered, e.g. register X0 is
 188  * used for the return value, while in this PSCI environment, X0 usually holds
 189  * the SMC function identifier, so X0 should be saved by caller function.
 190  */
 191 ENTRY(psci_get_cpu_id)
 192 #ifdef CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
 193         mrs     x9, MPIDR_EL1
 194         ubfx    x9, x9, #8, #8
 195         ldr     x10, =CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
 196         mul     x9, x10, x9
 197 #else
 198         mov     x9, xzr
 199 #endif
 200         mrs     x10, MPIDR_EL1
 201         ubfx    x10, x10, #0, #8
 202         add     x0, x10, x9
 203         ret
 204 ENDPROC(psci_get_cpu_id)
 205 .weak psci_get_cpu_id
 206
 207 /* CPU ID input in x0, stack top output in x0*/
 208 LENTRY(psci_get_cpu_stack_top)
 209         adr     x9, __secure_stack_end
 210         lsl     x0, x0, #ARM_PSCI_STACK_SHIFT
 211         sub     x0, x9, x0
 212         ret
 213 ENDPROC(psci_get_cpu_stack_top)
 214
 215 unhandled_exception:
 216         b       unhandled_exception     /* simply dead loop */
 217
 218 handle_sync:
 219         mov     x15, x30
 220         mov     x14, x0
 221
 222         bl      psci_get_cpu_id
 223         bl      psci_get_cpu_stack_top
 224         mov     x9, #1
 225         msr     spsel, x9
 226         mov     sp, x0
 227
 228         mov     x0, x14
 229         mov     x30, x15
 230
 231         mrs     x9, esr_el3
 232         ubfx    x9, x9, #26, #6
 233         cmp     x9, #0x13
 234         b.eq    handle_smc32
 235         cmp     x9, #0x17
 236         b.eq    handle_smc64
 237
 238         b       unhandled_exception
 239
 240         .align  11
 241         .globl  el3_exception_vectors
 242 el3_exception_vectors:
 243         b       unhandled_exception     /* Sync, Current EL using SP0 */
 244         .align  7
 245         b       unhandled_exception     /* IRQ, Current EL using SP0 */
 246         .align  7
 247         b       unhandled_exception     /* FIQ, Current EL using SP0 */
 248         .align  7
 249         b       unhandled_exception     /* SError, Current EL using SP0 */
 250         .align  7
 251         b       unhandled_exception     /* Sync, Current EL using SPx */
 252         .align  7
 253         b       unhandled_exception     /* IRQ, Current EL using SPx */
 254         .align  7
 255         b       unhandled_exception     /* FIQ, Current EL using SPx */
 256         .align  7
 257         b       unhandled_exception     /* SError, Current EL using SPx */
 258         .align  7
 259         b       handle_sync             /* Sync, Lower EL using AArch64 */
 260         .align  7
 261         b       unhandled_exception     /* IRQ, Lower EL using AArch64 */
 262         .align  7
 263         b       unhandled_exception     /* FIQ, Lower EL using AArch64 */
 264         .align  7
 265         b       unhandled_exception     /* SError, Lower EL using AArch64 */
 266         .align  7
 267         b       unhandled_exception     /* Sync, Lower EL using AArch32 */
 268         .align  7
 269         b       unhandled_exception     /* IRQ, Lower EL using AArch32 */
 270         .align  7
 271         b       unhandled_exception     /* FIQ, Lower EL using AArch32 */
 272         .align  7
 273         b       unhandled_exception     /* SError, Lower EL using AArch32 */
 274
 275 ENTRY(psci_setup_vectors)
 276         adr     x0, el3_exception_vectors
 277         msr     vbar_el3, x0
 278         ret
 279 ENDPROC(psci_setup_vectors)
 280
 281 ENTRY(psci_arch_init)
 282         ret
 283 ENDPROC(psci_arch_init)
 284 .weak psci_arch_init
 285
 286 .popsection