1 /* SPDX-License-Identifier: GPL-2.0+ */
3 * Copyright 2016 Freescale Semiconductor, Inc.
4 * Author: Hongbo Zhang <hongbo.zhang@nxp.com>
5 * This file implements LS102X platform PSCI SYSTEM-SUSPEND function
9 #include <linux/linkage.h>
11 #include <asm/secure.h>
13 /* Default PSCI function, return -1, Not Implemented */
14 #define PSCI_DEFAULT(__fn) \
16 mov w0, #ARM_PSCI_RET_NI; \
21 /* PSCI function and ID table definition*/
22 #define PSCI_TABLE(__id, __fn) \
26 .pushsection ._secure.text, "ax"
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)
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)
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)
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)
101 /* PSCI call is Fast Call(atomic), so mask DAIF */
103 stp x15, xzr, [sp, #-16]!
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]!
114 stp x18, x15, [sp, #-16]!
118 /* restore registers */
119 ldp x18, x15, [sp], #16
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
128 ldp x15, xzr, [sp], #16
133 /* Caller must put PSCI function-ID table base in x9 */
136 1: ldr x10, [x9] /* Load PSCI function table */
137 cbz x10, 3f /* If reach the end, bail out */
139 b.eq 2f /* PSCI function found */
140 add x9, x9, #16 /* If not match, try next entry */
143 2: ldr x11, [x9, #8] /* Load PSCI function */
144 blr x11 /* Call PSCI function */
147 3: mov x0, #ARM_PSCI_RET_NI
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.
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 */
163 1: ldr x10, [x9] /* Load SiP function table */
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 */
171 2: ldr x0, =0xFFFFFFFF
175 /* SMC function ID 0x84000000-0x8400001F: 32 bits PSCI */
183 adr x9, _psci_32_table
187 /* check SMC32 or SMC64 calls */
191 /* SMC function ID 0xC4000000-0xC400001F: 64 bits PSCI */
199 adr x9, _psci_64_table
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.
210 ENTRY(psci_get_cpu_id)
211 #ifdef CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
214 ldr x10, =CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
220 ubfx x10, x10, #0, #8
223 ENDPROC(psci_get_cpu_id)
224 .weak psci_get_cpu_id
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
232 ENDPROC(psci_get_cpu_stack_top)
235 b unhandled_exception /* simply dead loop */
242 bl psci_get_cpu_stack_top
257 b unhandled_exception
259 #ifdef CONFIG_ARMV8_EA_EL3_FIRST
261 * Override this function if custom error handling is
262 * needed for asynchronous aborts
264 ENTRY(plat_error_handler)
266 ENDPROC(plat_error_handler)
267 .weak plat_error_handler
271 bl psci_get_cpu_stack_top
276 bl plat_error_handler /* Platform specific error handling */
278 b deadloop /* Never return */
282 .globl el3_exception_vectors
283 el3_exception_vectors:
284 b unhandled_exception /* Sync, Current EL using SP0 */
286 b unhandled_exception /* IRQ, Current EL using SP0 */
288 b unhandled_exception /* FIQ, Current EL using SP0 */
290 b unhandled_exception /* SError, Current EL using SP0 */
292 b unhandled_exception /* Sync, Current EL using SPx */
294 b unhandled_exception /* IRQ, Current EL using SPx */
296 b unhandled_exception /* FIQ, Current EL using SPx */
298 b unhandled_exception /* SError, Current EL using SPx */
300 b handle_sync /* Sync, Lower EL using AArch64 */
302 b unhandled_exception /* IRQ, Lower EL using AArch64 */
304 b unhandled_exception /* FIQ, Lower EL using AArch64 */
306 #ifdef CONFIG_ARMV8_EA_EL3_FIRST
307 b handle_error /* SError, Lower EL using AArch64 */
309 b unhandled_exception /* SError, Lower EL using AArch64 */
312 b unhandled_exception /* Sync, Lower EL using AArch32 */
314 b unhandled_exception /* IRQ, Lower EL using AArch32 */
316 b unhandled_exception /* FIQ, Lower EL using AArch32 */
318 b unhandled_exception /* SError, Lower EL using AArch32 */
320 ENTRY(psci_setup_vectors)
321 adr x0, el3_exception_vectors
324 ENDPROC(psci_setup_vectors)
326 ENTRY(psci_arch_init)
328 ENDPROC(psci_arch_init)