psci.S 9.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331
  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. #include <config.h>
  8. #include <linux/linkage.h>
  9. #include <asm/psci.h>
  10. #include <asm/secure.h>
  11. /* Default PSCI function, return -1, Not Implemented */
  12. #define PSCI_DEFAULT(__fn) \
  13. ENTRY(__fn); \
  14. mov w0, #ARM_PSCI_RET_NI; \
  15. ret; \
  16. ENDPROC(__fn); \
  17. .weak __fn
  18. /* PSCI function and ID table definition*/
  19. #define PSCI_TABLE(__id, __fn) \
  20. .quad __id; \
  21. .quad __fn
  22. .pushsection ._secure.text, "ax"
  23. /* 32 bits PSCI default functions */
  24. PSCI_DEFAULT(psci_version)
  25. PSCI_DEFAULT(psci_cpu_suspend)
  26. PSCI_DEFAULT(psci_cpu_off)
  27. PSCI_DEFAULT(psci_cpu_on)
  28. PSCI_DEFAULT(psci_affinity_info)
  29. PSCI_DEFAULT(psci_migrate)
  30. PSCI_DEFAULT(psci_migrate_info_type)
  31. PSCI_DEFAULT(psci_migrate_info_up_cpu)
  32. PSCI_DEFAULT(psci_system_off)
  33. PSCI_DEFAULT(psci_system_reset)
  34. PSCI_DEFAULT(psci_features)
  35. PSCI_DEFAULT(psci_cpu_freeze)
  36. PSCI_DEFAULT(psci_cpu_default_suspend)
  37. PSCI_DEFAULT(psci_node_hw_state)
  38. PSCI_DEFAULT(psci_system_suspend)
  39. PSCI_DEFAULT(psci_set_suspend_mode)
  40. PSCI_DEFAULT(psi_stat_residency)
  41. PSCI_DEFAULT(psci_stat_count)
  42. .align 3
  43. _psci_32_table:
  44. PSCI_TABLE(ARM_PSCI_FN_CPU_SUSPEND, psci_cpu_suspend)
  45. PSCI_TABLE(ARM_PSCI_FN_CPU_OFF, psci_cpu_off)
  46. PSCI_TABLE(ARM_PSCI_FN_CPU_ON, psci_cpu_on)
  47. PSCI_TABLE(ARM_PSCI_FN_MIGRATE, psci_migrate)
  48. PSCI_TABLE(ARM_PSCI_0_2_FN_PSCI_VERSION, psci_version)
  49. PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_SUSPEND, psci_cpu_suspend)
  50. PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_OFF, psci_cpu_off)
  51. PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_ON, psci_cpu_on)
  52. PSCI_TABLE(ARM_PSCI_0_2_FN_AFFINITY_INFO, psci_affinity_info)
  53. PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE, psci_migrate)
  54. PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE, psci_migrate_info_type)
  55. PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE_INFO_UP_CPU, psci_migrate_info_up_cpu)
  56. PSCI_TABLE(ARM_PSCI_0_2_FN_SYSTEM_OFF, psci_system_off)
  57. PSCI_TABLE(ARM_PSCI_0_2_FN_SYSTEM_RESET, psci_system_reset)
  58. PSCI_TABLE(ARM_PSCI_1_0_FN_PSCI_FEATURES, psci_features)
  59. PSCI_TABLE(ARM_PSCI_1_0_FN_CPU_FREEZE, psci_cpu_freeze)
  60. PSCI_TABLE(ARM_PSCI_1_0_FN_CPU_DEFAULT_SUSPEND, psci_cpu_default_suspend)
  61. PSCI_TABLE(ARM_PSCI_1_0_FN_NODE_HW_STATE, psci_node_hw_state)
  62. PSCI_TABLE(ARM_PSCI_1_0_FN_SYSTEM_SUSPEND, psci_system_suspend)
  63. PSCI_TABLE(ARM_PSCI_1_0_FN_SET_SUSPEND_MODE, psci_set_suspend_mode)
  64. PSCI_TABLE(ARM_PSCI_1_0_FN_STAT_RESIDENCY, psi_stat_residency)
  65. PSCI_TABLE(ARM_PSCI_1_0_FN_STAT_COUNT, psci_stat_count)
  66. PSCI_TABLE(0, 0)
  67. /* 64 bits PSCI default functions */
  68. PSCI_DEFAULT(psci_cpu_suspend_64)
  69. PSCI_DEFAULT(psci_cpu_on_64)
  70. PSCI_DEFAULT(psci_affinity_info_64)
  71. PSCI_DEFAULT(psci_migrate_64)
  72. PSCI_DEFAULT(psci_migrate_info_up_cpu_64)
  73. PSCI_DEFAULT(psci_cpu_default_suspend_64)
  74. PSCI_DEFAULT(psci_node_hw_state_64)
  75. PSCI_DEFAULT(psci_system_suspend_64)
  76. PSCI_DEFAULT(psci_stat_residency_64)
  77. PSCI_DEFAULT(psci_stat_count_64)
  78. .align 3
  79. _psci_64_table:
  80. PSCI_TABLE(ARM_PSCI_0_2_FN64_CPU_SUSPEND, psci_cpu_suspend_64)
  81. PSCI_TABLE(ARM_PSCI_0_2_FN64_CPU_ON, psci_cpu_on_64)
  82. PSCI_TABLE(ARM_PSCI_0_2_FN64_AFFINITY_INFO, psci_affinity_info_64)
  83. PSCI_TABLE(ARM_PSCI_0_2_FN64_MIGRATE, psci_migrate_64)
  84. PSCI_TABLE(ARM_PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU, psci_migrate_info_up_cpu_64)
  85. PSCI_TABLE(ARM_PSCI_1_0_FN64_CPU_DEFAULT_SUSPEND, psci_cpu_default_suspend_64)
  86. PSCI_TABLE(ARM_PSCI_1_0_FN64_NODE_HW_STATE, psci_node_hw_state_64)
  87. PSCI_TABLE(ARM_PSCI_1_0_FN64_SYSTEM_SUSPEND, psci_system_suspend_64)
  88. PSCI_TABLE(ARM_PSCI_1_0_FN64_STAT_RESIDENCY, psci_stat_residency_64)
  89. PSCI_TABLE(ARM_PSCI_1_0_FN64_STAT_COUNT, psci_stat_count_64)
  90. PSCI_TABLE(0, 0)
  91. .macro psci_enter
  92. /* PSCI call is Fast Call(atomic), so mask DAIF */
  93. mrs x15, DAIF
  94. stp x15, xzr, [sp, #-16]!
  95. ldr x15, =0x3C0
  96. msr DAIF, x15
  97. /* SMC convention, x18 ~ x30 should be saved by callee */
  98. stp x29, x30, [sp, #-16]!
  99. stp x27, x28, [sp, #-16]!
  100. stp x25, x26, [sp, #-16]!
  101. stp x23, x24, [sp, #-16]!
  102. stp x21, x22, [sp, #-16]!
  103. stp x19, x20, [sp, #-16]!
  104. mrs x15, elr_el3
  105. stp x18, x15, [sp, #-16]!
  106. .endm
  107. .macro psci_return
  108. /* restore registers */
  109. ldp x18, x15, [sp], #16
  110. msr elr_el3, x15
  111. ldp x19, x20, [sp], #16
  112. ldp x21, x22, [sp], #16
  113. ldp x23, x24, [sp], #16
  114. ldp x25, x26, [sp], #16
  115. ldp x27, x28, [sp], #16
  116. ldp x29, x30, [sp], #16
  117. /* restore DAIF */
  118. ldp x15, xzr, [sp], #16
  119. msr DAIF, x15
  120. eret
  121. .endm
  122. /* Caller must put PSCI function-ID table base in x9 */
  123. handle_psci:
  124. psci_enter
  125. 1: ldr x10, [x9] /* Load PSCI function table */
  126. cbz x10, 3f /* If reach the end, bail out */
  127. cmp x10, x0
  128. b.eq 2f /* PSCI function found */
  129. add x9, x9, #16 /* If not match, try next entry */
  130. b 1b
  131. 2: ldr x11, [x9, #8] /* Load PSCI function */
  132. blr x11 /* Call PSCI function */
  133. psci_return
  134. 3: mov x0, #ARM_PSCI_RET_NI
  135. psci_return
  136. /*
  137. * Handle SiP service functions defined in SiP service function table.
  138. * Use DECLARE_SECURE_SVC(_name, _id, _fn) to add platform specific SiP
  139. * service function into the SiP service function table.
  140. * SiP service function table is located in '._secure_svc_tbl_entries' section,
  141. * which is next to '._secure.text' section.
  142. */
  143. handle_svc:
  144. adr x9, __secure_svc_tbl_start
  145. adr x10, __secure_svc_tbl_end
  146. subs x12, x10, x9 /* Get number of entries in table */
  147. b.eq 2f /* Make sure SiP function table is not empty */
  148. psci_enter
  149. 1: ldr x10, [x9] /* Load SiP function table */
  150. ldr x11, [x9, #8]
  151. cmp w10, w0
  152. b.eq 2b /* SiP service function found */
  153. add x9, x9, #SECURE_SVC_TBL_OFFSET /* Move to next entry */
  154. subs x12, x12, #SECURE_SVC_TBL_OFFSET
  155. b.eq 3b /* If reach the end, bail out */
  156. b 1b
  157. 2: ldr x0, =0xFFFFFFFF
  158. eret
  159. handle_smc32:
  160. /* SMC function ID 0x84000000-0x8400001F: 32 bits PSCI */
  161. ldr w9, =0x8400001F
  162. cmp w0, w9
  163. b.gt handle_svc
  164. ldr w9, =0x84000000
  165. cmp w0, w9
  166. b.lt handle_svc
  167. adr x9, _psci_32_table
  168. b handle_psci
  169. handle_smc64:
  170. /* check SMC32 or SMC64 calls */
  171. ubfx x9, x0, #30, #1
  172. cbz x9, handle_smc32
  173. /* SMC function ID 0xC4000000-0xC400001F: 64 bits PSCI */
  174. ldr x9, =0xC400001F
  175. cmp x0, x9
  176. b.gt handle_svc
  177. ldr x9, =0xC4000000
  178. cmp x0, x9
  179. b.lt handle_svc
  180. adr x9, _psci_64_table
  181. b handle_psci
  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. */
  190. ENTRY(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
  203. ENDPROC(psci_get_cpu_id)
  204. .weak psci_get_cpu_id
  205. /* CPU ID input in x0, stack top output in x0*/
  206. LENTRY(psci_get_cpu_stack_top)
  207. adr x9, __secure_stack_end
  208. lsl x0, x0, #ARM_PSCI_STACK_SHIFT
  209. sub x0, x9, x0
  210. ret
  211. ENDPROC(psci_get_cpu_stack_top)
  212. unhandled_exception:
  213. b unhandled_exception /* simply dead loop */
  214. handle_sync:
  215. mov x15, x30
  216. mov x14, x0
  217. bl psci_get_cpu_id
  218. bl psci_get_cpu_stack_top
  219. mov x9, #1
  220. msr spsel, x9
  221. mov sp, x0
  222. mov x0, x14
  223. mov x30, x15
  224. mrs x9, esr_el3
  225. ubfx x9, x9, #26, #6
  226. cmp x9, #0x13
  227. b.eq handle_smc32
  228. cmp x9, #0x17
  229. b.eq handle_smc64
  230. b unhandled_exception
  231. #ifdef CONFIG_ARMV8_EA_EL3_FIRST
  232. /*
  233. * Override this function if custom error handling is
  234. * needed for asynchronous aborts
  235. */
  236. ENTRY(plat_error_handler)
  237. ret
  238. ENDPROC(plat_error_handler)
  239. .weak plat_error_handler
  240. handle_error:
  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. bl plat_error_handler /* Platform specific error handling */
  247. deadloop:
  248. b deadloop /* Never return */
  249. #endif
  250. .align 11
  251. .globl el3_exception_vectors
  252. el3_exception_vectors:
  253. b unhandled_exception /* Sync, Current EL using SP0 */
  254. .align 7
  255. b unhandled_exception /* IRQ, Current EL using SP0 */
  256. .align 7
  257. b unhandled_exception /* FIQ, Current EL using SP0 */
  258. .align 7
  259. b unhandled_exception /* SError, Current EL using SP0 */
  260. .align 7
  261. b unhandled_exception /* Sync, Current EL using SPx */
  262. .align 7
  263. b unhandled_exception /* IRQ, Current EL using SPx */
  264. .align 7
  265. b unhandled_exception /* FIQ, Current EL using SPx */
  266. .align 7
  267. b unhandled_exception /* SError, Current EL using SPx */
  268. .align 7
  269. b handle_sync /* Sync, Lower EL using AArch64 */
  270. .align 7
  271. b unhandled_exception /* IRQ, Lower EL using AArch64 */
  272. .align 7
  273. b unhandled_exception /* FIQ, Lower EL using AArch64 */
  274. .align 7
  275. #ifdef CONFIG_ARMV8_EA_EL3_FIRST
  276. b handle_error /* SError, Lower EL using AArch64 */
  277. #else
  278. b unhandled_exception /* SError, Lower EL using AArch64 */
  279. #endif
  280. .align 7
  281. b unhandled_exception /* Sync, Lower EL using AArch32 */
  282. .align 7
  283. b unhandled_exception /* IRQ, Lower EL using AArch32 */
  284. .align 7
  285. b unhandled_exception /* FIQ, Lower EL using AArch32 */
  286. .align 7
  287. b unhandled_exception /* SError, Lower EL using AArch32 */
  288. ENTRY(psci_setup_vectors)
  289. adr x0, el3_exception_vectors
  290. msr vbar_el3, x0
  291. ret
  292. ENDPROC(psci_setup_vectors)
  293. ENTRY(psci_arch_init)
  294. ret
  295. ENDPROC(psci_arch_init)
  296. .weak psci_arch_init
  297. .popsection