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