psci.S 8.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311
  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. /* Default PSCI function, return -1, Not Implemented */
  11. #define PSCI_DEFAULT(__fn) \
  12. ENTRY(__fn); \
  13. mov w0, #ARM_PSCI_RET_NI; \
  14. ret; \
  15. ENDPROC(__fn); \
  16. .weak __fn
  17. /* PSCI function and ID table definition*/
  18. #define PSCI_TABLE(__id, __fn) \
  19. .word __id; \
  20. .word __fn
  21. .pushsection ._secure.text, "ax"
  22. /* 32 bits PSCI default functions */
  23. PSCI_DEFAULT(psci_version)
  24. PSCI_DEFAULT(psci_cpu_suspend)
  25. PSCI_DEFAULT(psci_cpu_off)
  26. PSCI_DEFAULT(psci_cpu_on)
  27. PSCI_DEFAULT(psci_affinity_info)
  28. PSCI_DEFAULT(psci_migrate)
  29. PSCI_DEFAULT(psci_migrate_info_type)
  30. PSCI_DEFAULT(psci_migrate_info_up_cpu)
  31. PSCI_DEFAULT(psci_system_off)
  32. PSCI_DEFAULT(psci_system_reset)
  33. PSCI_DEFAULT(psci_features)
  34. PSCI_DEFAULT(psci_cpu_freeze)
  35. PSCI_DEFAULT(psci_cpu_default_suspend)
  36. PSCI_DEFAULT(psci_node_hw_state)
  37. PSCI_DEFAULT(psci_system_suspend)
  38. PSCI_DEFAULT(psci_set_suspend_mode)
  39. PSCI_DEFAULT(psi_stat_residency)
  40. PSCI_DEFAULT(psci_stat_count)
  41. .align 3
  42. _psci_32_table:
  43. PSCI_TABLE(ARM_PSCI_FN_CPU_SUSPEND, psci_cpu_suspend)
  44. PSCI_TABLE(ARM_PSCI_FN_CPU_OFF, psci_cpu_off)
  45. PSCI_TABLE(ARM_PSCI_FN_CPU_ON, psci_cpu_on)
  46. PSCI_TABLE(ARM_PSCI_FN_MIGRATE, psci_migrate)
  47. PSCI_TABLE(ARM_PSCI_0_2_FN_PSCI_VERSION, psci_version)
  48. PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_SUSPEND, psci_cpu_suspend)
  49. PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_OFF, psci_cpu_off)
  50. PSCI_TABLE(ARM_PSCI_0_2_FN_CPU_ON, psci_cpu_on)
  51. PSCI_TABLE(ARM_PSCI_0_2_FN_AFFINITY_INFO, psci_affinity_info)
  52. PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE, psci_migrate)
  53. PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE_INFO_TYPE, psci_migrate_info_type)
  54. PSCI_TABLE(ARM_PSCI_0_2_FN_MIGRATE_INFO_UP_CPU, psci_migrate_info_up_cpu)
  55. PSCI_TABLE(ARM_PSCI_0_2_FN_SYSTEM_OFF, psci_system_off)
  56. PSCI_TABLE(ARM_PSCI_0_2_FN_SYSTEM_RESET, psci_system_reset)
  57. PSCI_TABLE(ARM_PSCI_1_0_FN_PSCI_FEATURES, psci_features)
  58. PSCI_TABLE(ARM_PSCI_1_0_FN_CPU_FREEZE, psci_cpu_freeze)
  59. PSCI_TABLE(ARM_PSCI_1_0_FN_CPU_DEFAULT_SUSPEND, psci_cpu_default_suspend)
  60. PSCI_TABLE(ARM_PSCI_1_0_FN_NODE_HW_STATE, psci_node_hw_state)
  61. PSCI_TABLE(ARM_PSCI_1_0_FN_SYSTEM_SUSPEND, psci_system_suspend)
  62. PSCI_TABLE(ARM_PSCI_1_0_FN_SET_SUSPEND_MODE, psci_set_suspend_mode)
  63. PSCI_TABLE(ARM_PSCI_1_0_FN_STAT_RESIDENCY, psi_stat_residency)
  64. PSCI_TABLE(ARM_PSCI_1_0_FN_STAT_COUNT, psci_stat_count)
  65. PSCI_TABLE(0, 0)
  66. /* 64 bits PSCI default functions */
  67. PSCI_DEFAULT(psci_cpu_suspend_64)
  68. PSCI_DEFAULT(psci_cpu_on_64)
  69. PSCI_DEFAULT(psci_affinity_info_64)
  70. PSCI_DEFAULT(psci_migrate_64)
  71. PSCI_DEFAULT(psci_migrate_info_up_cpu_64)
  72. PSCI_DEFAULT(psci_cpu_default_suspend_64)
  73. PSCI_DEFAULT(psci_node_hw_state_64)
  74. PSCI_DEFAULT(psci_system_suspend_64)
  75. PSCI_DEFAULT(psci_stat_residency_64)
  76. PSCI_DEFAULT(psci_stat_count_64)
  77. .align 3
  78. _psci_64_table:
  79. PSCI_TABLE(ARM_PSCI_0_2_FN64_CPU_SUSPEND, psci_cpu_suspend_64)
  80. PSCI_TABLE(ARM_PSCI_0_2_FN64_CPU_ON, psci_cpu_on_64)
  81. PSCI_TABLE(ARM_PSCI_0_2_FN64_AFFINITY_INFO, psci_affinity_info_64)
  82. PSCI_TABLE(ARM_PSCI_0_2_FN64_MIGRATE, psci_migrate_64)
  83. PSCI_TABLE(ARM_PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU, psci_migrate_info_up_cpu_64)
  84. PSCI_TABLE(ARM_PSCI_1_0_FN64_CPU_DEFAULT_SUSPEND, psci_cpu_default_suspend_64)
  85. PSCI_TABLE(ARM_PSCI_1_0_FN64_NODE_HW_STATE, psci_node_hw_state_64)
  86. PSCI_TABLE(ARM_PSCI_1_0_FN64_SYSTEM_SUSPEND, psci_system_suspend_64)
  87. PSCI_TABLE(ARM_PSCI_1_0_FN64_STAT_RESIDENCY, psci_stat_residency_64)
  88. PSCI_TABLE(ARM_PSCI_1_0_FN64_STAT_COUNT, psci_stat_count_64)
  89. PSCI_TABLE(0, 0)
  90. .macro psci_enter
  91. /* PSCI call is Fast Call(atomic), so mask DAIF */
  92. mrs x15, DAIF
  93. stp x15, xzr, [sp, #-16]!
  94. ldr x15, =0x3C0
  95. msr DAIF, x15
  96. /* SMC convention, x18 ~ x30 should be saved by callee */
  97. stp x29, x30, [sp, #-16]!
  98. stp x27, x28, [sp, #-16]!
  99. stp x25, x26, [sp, #-16]!
  100. stp x23, x24, [sp, #-16]!
  101. stp x21, x22, [sp, #-16]!
  102. stp x19, x20, [sp, #-16]!
  103. mrs x15, elr_el3
  104. stp x18, x15, [sp, #-16]!
  105. .endm
  106. .macro psci_return
  107. /* restore registers */
  108. ldp x18, x15, [sp], #16
  109. msr elr_el3, x15
  110. ldp x19, x20, [sp], #16
  111. ldp x21, x22, [sp], #16
  112. ldp x23, x24, [sp], #16
  113. ldp x25, x26, [sp], #16
  114. ldp x27, x28, [sp], #16
  115. ldp x29, x30, [sp], #16
  116. /* restore DAIF */
  117. ldp x15, xzr, [sp], #16
  118. msr DAIF, x15
  119. eret
  120. .endm
  121. /* Caller must put PSCI function-ID table base in x9 */
  122. handle_psci:
  123. psci_enter
  124. 1: ldr x10, [x9] /* Load PSCI function table */
  125. ubfx x11, x10, #32, #32
  126. ubfx x10, x10, #0, #32
  127. cbz x10, 3f /* If reach the end, bail out */
  128. cmp x10, x0
  129. b.eq 2f /* PSCI function found */
  130. add x9, x9, #8 /* If not match, try next entry */
  131. b 1b
  132. 2: blr x11 /* Call PSCI function */
  133. psci_return
  134. 3: mov x0, #ARM_PSCI_RET_NI
  135. psci_return
  136. unknown_smc_id:
  137. ldr x0, =0xFFFFFFFF
  138. eret
  139. handle_smc32:
  140. /* SMC function ID 0x84000000-0x8400001F: 32 bits PSCI */
  141. ldr w9, =0x8400001F
  142. cmp w0, w9
  143. b.gt unknown_smc_id
  144. ldr w9, =0x84000000
  145. cmp w0, w9
  146. b.lt unknown_smc_id
  147. adr x9, _psci_32_table
  148. b handle_psci
  149. handle_smc64:
  150. /* check SMC32 or SMC64 calls */
  151. ubfx x9, x0, #30, #1
  152. cbz x9, handle_smc32
  153. /* SMC function ID 0xC4000000-0xC400001F: 64 bits PSCI */
  154. ldr x9, =0xC400001F
  155. cmp x0, x9
  156. b.gt unknown_smc_id
  157. ldr x9, =0xC4000000
  158. cmp x0, x9
  159. b.lt unknown_smc_id
  160. adr x9, _psci_64_table
  161. b handle_psci
  162. /*
  163. * Get CPU ID from MPIDR, suppose every cluster has same number of CPU cores,
  164. * Platform with asymmetric clusters should implement their own interface.
  165. * In case this function being called by other platform's C code, the ARM
  166. * Architecture Procedure Call Standard is considered, e.g. register X0 is
  167. * used for the return value, while in this PSCI environment, X0 usually holds
  168. * the SMC function identifier, so X0 should be saved by caller function.
  169. */
  170. ENTRY(psci_get_cpu_id)
  171. #ifdef CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
  172. mrs x9, MPIDR_EL1
  173. ubfx x9, x9, #8, #8
  174. ldr x10, =CONFIG_ARMV8_PSCI_CPUS_PER_CLUSTER
  175. mul x9, x10, x9
  176. #else
  177. mov x9, xzr
  178. #endif
  179. mrs x10, MPIDR_EL1
  180. ubfx x10, x10, #0, #8
  181. add x0, x10, x9
  182. ret
  183. ENDPROC(psci_get_cpu_id)
  184. .weak psci_get_cpu_id
  185. /* CPU ID input in x0, stack top output in x0*/
  186. LENTRY(psci_get_cpu_stack_top)
  187. adr x9, __secure_stack_end
  188. lsl x0, x0, #ARM_PSCI_STACK_SHIFT
  189. sub x0, x9, x0
  190. ret
  191. ENDPROC(psci_get_cpu_stack_top)
  192. unhandled_exception:
  193. b unhandled_exception /* simply dead loop */
  194. handle_sync:
  195. mov x15, x30
  196. mov x14, x0
  197. bl psci_get_cpu_id
  198. bl psci_get_cpu_stack_top
  199. mov x9, #1
  200. msr spsel, x9
  201. mov sp, x0
  202. mov x0, x14
  203. mov x30, x15
  204. mrs x9, esr_el3
  205. ubfx x9, x9, #26, #6
  206. cmp x9, #0x13
  207. b.eq handle_smc32
  208. cmp x9, #0x17
  209. b.eq handle_smc64
  210. b unhandled_exception
  211. #ifdef CONFIG_ARMV8_EA_EL3_FIRST
  212. /*
  213. * Override this function if custom error handling is
  214. * needed for asynchronous aborts
  215. */
  216. ENTRY(plat_error_handler)
  217. ret
  218. ENDPROC(plat_error_handler)
  219. .weak plat_error_handler
  220. handle_error:
  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. bl plat_error_handler /* Platform specific error handling */
  227. deadloop:
  228. b deadloop /* Never return */
  229. #endif
  230. .align 11
  231. .globl el3_exception_vectors
  232. el3_exception_vectors:
  233. b unhandled_exception /* Sync, Current EL using SP0 */
  234. .align 7
  235. b unhandled_exception /* IRQ, Current EL using SP0 */
  236. .align 7
  237. b unhandled_exception /* FIQ, Current EL using SP0 */
  238. .align 7
  239. b unhandled_exception /* SError, Current EL using SP0 */
  240. .align 7
  241. b unhandled_exception /* Sync, Current EL using SPx */
  242. .align 7
  243. b unhandled_exception /* IRQ, Current EL using SPx */
  244. .align 7
  245. b unhandled_exception /* FIQ, Current EL using SPx */
  246. .align 7
  247. b unhandled_exception /* SError, Current EL using SPx */
  248. .align 7
  249. b handle_sync /* Sync, Lower EL using AArch64 */
  250. .align 7
  251. b unhandled_exception /* IRQ, Lower EL using AArch64 */
  252. .align 7
  253. b unhandled_exception /* FIQ, Lower EL using AArch64 */
  254. .align 7
  255. #ifdef CONFIG_ARMV8_EA_EL3_FIRST
  256. b handle_error /* SError, Lower EL using AArch64 */
  257. #else
  258. b unhandled_exception /* SError, Lower EL using AArch64 */
  259. #endif
  260. .align 7
  261. b unhandled_exception /* Sync, Lower EL using AArch32 */
  262. .align 7
  263. b unhandled_exception /* IRQ, Lower EL using AArch32 */
  264. .align 7
  265. b unhandled_exception /* FIQ, Lower EL using AArch32 */
  266. .align 7
  267. b unhandled_exception /* SError, Lower EL using AArch32 */
  268. ENTRY(psci_setup_vectors)
  269. adr x0, el3_exception_vectors
  270. msr vbar_el3, x0
  271. ret
  272. ENDPROC(psci_setup_vectors)
  273. ENTRY(psci_arch_init)
  274. ret
  275. ENDPROC(psci_arch_init)
  276. .weak psci_arch_init
  277. .popsection