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 */ 28PSCI_DEFAULT(psci_version) 29PSCI_DEFAULT(psci_cpu_suspend) 30PSCI_DEFAULT(psci_cpu_off) 31PSCI_DEFAULT(psci_cpu_on) 32PSCI_DEFAULT(psci_affinity_info) 33PSCI_DEFAULT(psci_migrate) 34PSCI_DEFAULT(psci_migrate_info_type) 35PSCI_DEFAULT(psci_migrate_info_up_cpu) 36PSCI_DEFAULT(psci_system_off) 37PSCI_DEFAULT(psci_system_reset) 38PSCI_DEFAULT(psci_features) 39PSCI_DEFAULT(psci_cpu_freeze) 40PSCI_DEFAULT(psci_cpu_default_suspend) 41PSCI_DEFAULT(psci_node_hw_state) 42PSCI_DEFAULT(psci_system_suspend) 43PSCI_DEFAULT(psci_set_suspend_mode) 44PSCI_DEFAULT(psi_stat_residency) 45PSCI_DEFAULT(psci_stat_count) 46 47.align 3 48_psci_32_table: 49PSCI_TABLE(ARM_PSCI_FN_CPU_SUSPEND, psci_cpu_suspend) 50PSCI_TABLE(ARM_PSCI_FN_CPU_OFF, psci_cpu_off) 51PSCI_TABLE(ARM_PSCI_FN_CPU_ON, psci_cpu_on) 52PSCI_TABLE(ARM_PSCI_FN_MIGRATE, psci_migrate) 53PSCI_TABLE(ARM_PSCI_0_2_FN_PSCI_VERSION, psci_version) 54PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_SUSPEND, psci_cpu_suspend) 55PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_OFF, psci_cpu_off) 56PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_ON, psci_cpu_on) 57PSCI_TABLE(ARM_PSCI_0_2_FN_AFFINITY_INFO, psci_affinity_info) 58PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE, psci_migrate) 59PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE, psci_migrate_info_type) 60PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE_INFO_UP_CPU, psci_migrate_info_up_cpu) 61PSCI_TABLE(ARM_PSCI_0_2_FN_SYSTEM_OFF, psci_system_off) 62PSCI_TABLE(ARM_PSCI_0_2_FN_SYSTEM_RESET, psci_system_reset) 63PSCI_TABLE(ARM_PSCI_1_0_FN_PSCI_FEATURES, psci_features) 64PSCI_TABLE(ARM_PSCI_1_0_FN_CPU_FREEZE, psci_cpu_freeze) 65PSCI_TABLE(ARM_PSCI_1_0_FN_CPU_DEFAULT_SUSPEND, psci_cpu_default_suspend) 66PSCI_TABLE(ARM_PSCI_1_0_FN_NODE_HW_STATE, psci_node_hw_state) 67PSCI_TABLE(ARM_PSCI_1_0_FN_SYSTEM_SUSPEND, psci_system_suspend) 68PSCI_TABLE(ARM_PSCI_1_0_FN_SET_SUSPEND_MODE, psci_set_suspend_mode) 69PSCI_TABLE(ARM_PSCI_1_0_FN_STAT_RESIDENCY, psi_stat_residency) 70PSCI_TABLE(ARM_PSCI_1_0_FN_STAT_COUNT, psci_stat_count) 71PSCI_TABLE(0, 0) 72 73/* 64 bits PSCI default functions */ 74PSCI_DEFAULT(psci_cpu_suspend_64) 75PSCI_DEFAULT(psci_cpu_on_64) 76PSCI_DEFAULT(psci_affinity_info_64) 77PSCI_DEFAULT(psci_migrate_64) 78PSCI_DEFAULT(psci_migrate_info_up_cpu_64) 79PSCI_DEFAULT(psci_cpu_default_suspend_64) 80PSCI_DEFAULT(psci_node_hw_state_64) 81PSCI_DEFAULT(psci_system_suspend_64) 82PSCI_DEFAULT(psci_stat_residency_64) 83PSCI_DEFAULT(psci_stat_count_64) 84 85.align 3 86_psci_64_table: 87PSCI_TABLE(ARM_PSCI_0_2_FN64_CPU_SUSPEND, psci_cpu_suspend_64) 88PSCI_TABLE(ARM_PSCI_0_2_FN64_CPU_ON, psci_cpu_on_64) 89PSCI_TABLE(ARM_PSCI_0_2_FN64_AFFINITY_INFO, psci_affinity_info_64) 90PSCI_TABLE(ARM_PSCI_0_2_FN64_MIGRATE, psci_migrate_64) 91PSCI_TABLE(ARM_PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU, psci_migrate_info_up_cpu_64) 92PSCI_TABLE(ARM_PSCI_1_0_FN64_CPU_DEFAULT_SUSPEND, psci_cpu_default_suspend_64) 93PSCI_TABLE(ARM_PSCI_1_0_FN64_NODE_HW_STATE, psci_node_hw_state_64) 94PSCI_TABLE(ARM_PSCI_1_0_FN64_SYSTEM_SUSPEND, psci_system_suspend_64) 95PSCI_TABLE(ARM_PSCI_1_0_FN64_STAT_RESIDENCY, psci_stat_residency_64) 96PSCI_TABLE(ARM_PSCI_1_0_FN64_STAT_COUNT, psci_stat_count_64) 97PSCI_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 */ 133handle_psci: 134 psci_enter 1351: 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 1442: blr x11 /* Call PSCI function */ 145 psci_return 146 1473: mov x0, #ARM_PSCI_RET_NI 148 psci_return 149 150unknown_smc_id: 151 ldr x0, =0xFFFFFFFF 152 eret 153 154handle_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 166handle_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 */ 190ENTRY(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 203ENDPROC(psci_get_cpu_id) 204.weak psci_get_cpu_id 205 206/* CPU ID input in x0, stack top output in x0*/ 207LENTRY(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 212ENDPROC(psci_get_cpu_stack_top) 213 214unhandled_exception: 215 b unhandled_exception /* simply dead loop */ 216 217handle_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 241el3_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 274ENTRY(psci_setup_vectors) 275 adr x0, el3_exception_vectors 276 msr vbar_el3, x0 277 ret 278ENDPROC(psci_setup_vectors) 279 280ENTRY(psci_arch_init) 281 ret 282ENDPROC(psci_arch_init) 283.weak psci_arch_init 284 285.popsection 286