fw_base.S 10 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464
  1. /*
  2. * SPDX-License-Identifier: BSD-2-Clause
  3. *
  4. * Copyright (c) 2019 Western Digital Corporation or its affiliates.
  5. *
  6. * Authors:
  7. * Anup Patel <anup.patel@wdc.com>
  8. */
  9. #include <sbi/riscv_asm.h>
  10. #include <sbi/riscv_encoding.h>
  11. #include <sbi/sbi_platform.h>
  12. #include <sbi/sbi_scratch.h>
  13. #include <sbi/sbi_trap.h>
  14. .align 3
  15. .section .entry, "ax", %progbits
  16. .globl _start
  17. .globl _start_warm
  18. _start:
  19. /*
  20. * Jump to warm-boot if this is not the first core booting,
  21. * that is, for mhartid != 0
  22. */
  23. csrr a6, CSR_MHARTID
  24. blt zero, a6, _wait_for_boot_hart
  25. li ra, 0
  26. call _reset_regs
  27. /* Preload HART details
  28. * s7 -> HART Count
  29. * s8 -> HART Stack Size
  30. */
  31. la a4, platform
  32. #if __riscv_xlen == 64
  33. lwu s7, SBI_PLATFORM_HART_COUNT_OFFSET(a4)
  34. lwu s8, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(a4)
  35. #else
  36. lw s7, SBI_PLATFORM_HART_COUNT_OFFSET(a4)
  37. lw s8, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(a4)
  38. #endif
  39. /* Setup scratch space for all the HARTs*/
  40. la tp, _fw_end
  41. mul a5, s7, s8
  42. add tp, tp, a5
  43. /* Keep a copy of tp */
  44. add t3, tp, zero
  45. /* Counter */
  46. li t2, 1
  47. /* hartid 0 is mandated by ISA */
  48. li t1, 0
  49. _scratch_init:
  50. add tp, t3, zero
  51. mul a5, s8, t1
  52. sub tp, tp, a5
  53. li a5, SBI_SCRATCH_SIZE
  54. sub tp, tp, a5
  55. /* Initialize scratch space */
  56. la a4, _fw_start
  57. la a5, _fw_end
  58. mul t0, s7, s8
  59. add a5, a5, t0
  60. sub a5, a5, a4
  61. REG_S a4, SBI_SCRATCH_FW_START_OFFSET(tp)
  62. REG_S a5, SBI_SCRATCH_FW_SIZE_OFFSET(tp)
  63. /* Note: fw_next_arg1() uses a0, a1, and ra */
  64. call fw_next_arg1
  65. REG_S a0, SBI_SCRATCH_NEXT_ARG1_OFFSET(tp)
  66. /* Note: fw_next_addr() uses a0, a1, and ra */
  67. call fw_next_addr
  68. REG_S a0, SBI_SCRATCH_NEXT_ADDR_OFFSET(tp)
  69. li a4, PRV_S
  70. REG_S a4, SBI_SCRATCH_NEXT_MODE_OFFSET(tp)
  71. la a4, _start_warm
  72. REG_S a4, SBI_SCRATCH_WARMBOOT_ADDR_OFFSET(tp)
  73. la a4, platform
  74. REG_S a4, SBI_SCRATCH_PLATFORM_ADDR_OFFSET(tp)
  75. la a4, _hartid_to_scratch
  76. REG_S a4, SBI_SCRATCH_HARTID_TO_SCRATCH_OFFSET(tp)
  77. REG_S zero, SBI_SCRATCH_TMP0_OFFSET(tp)
  78. #ifdef FW_OPTIONS
  79. li a4, FW_OPTIONS
  80. REG_S a4, SBI_SCRATCH_OPTIONS_OFFSET(tp)
  81. #else
  82. REG_S zero, SBI_SCRATCH_OPTIONS_OFFSET(tp)
  83. #endif
  84. add t1, t1, t2
  85. blt t1, s7, _scratch_init
  86. /* Zero-out BSS */
  87. la a4, _bss_start
  88. la a5, _bss_end
  89. _bss_zero:
  90. REG_S zero, (a4)
  91. add a4, a4, __SIZEOF_POINTER__
  92. blt a4, a5, _bss_zero
  93. /* Override pervious arg1 */
  94. add s0, a0, zero
  95. add s1, a1, zero
  96. call fw_prev_arg1
  97. add t1, a0, zero
  98. add a0, s0, zero
  99. add a1, s1, zero
  100. beqz t1, _prev_arg1_override_done
  101. add a1, t1, zero
  102. _prev_arg1_override_done:
  103. /*
  104. * Relocate Flatened Device Tree (FDT)
  105. * source FDT address = previous arg1
  106. * destination FDT address = next arg1
  107. *
  108. * Note: We will preserve a0 and a1 passed by
  109. * previous booting stage.
  110. */
  111. beqz a1, _fdt_reloc_done
  112. /* Mask values in a3 and a4 */
  113. li a3, ~(__SIZEOF_POINTER__ - 1)
  114. li a4, 0xff
  115. /* t1 = destination FDT start address */
  116. add s0, a0, zero
  117. add s1, a1, zero
  118. call fw_next_arg1
  119. add t1, a0, zero
  120. add a0, s0, zero
  121. add a1, s1, zero
  122. beqz t1, _fdt_reloc_done
  123. and t1, t1, a3
  124. /* t0 = source FDT start address */
  125. add t0, a1, zero
  126. and t0, t0, a3
  127. /* t2 = source FDT size in big-endian */
  128. #if __riscv_xlen == 64
  129. lwu t2, 4(t0)
  130. #else
  131. lw t2, 4(t0)
  132. #endif
  133. /* t3 = bit[15:8] of FDT size */
  134. add t3, t2, zero
  135. srli t3, t3, 16
  136. and t3, t3, a4
  137. slli t3, t3, 8
  138. /* t4 = bit[23:16] of FDT size */
  139. add t4, t2, zero
  140. srli t4, t4, 8
  141. and t4, t4, a4
  142. slli t4, t4, 16
  143. /* t5 = bit[31:24] of FDT size */
  144. add t5, t2, zero
  145. and t5, t5, a4
  146. slli t5, t5, 24
  147. /* t2 = bit[7:0] of FDT size */
  148. srli t2, t2, 24
  149. and t2, t2, a4
  150. /* t2 = FDT size in little-endian */
  151. or t2, t2, t3
  152. or t2, t2, t4
  153. or t2, t2, t5
  154. /* t2 = destination FDT end address */
  155. add t2, t1, t2
  156. /* FDT copy loop */
  157. ble t2, t1, _fdt_reloc_done
  158. _fdt_reloc_again:
  159. REG_L t3, 0(t0)
  160. REG_S t3, 0(t1)
  161. add t0, t0, __SIZEOF_POINTER__
  162. add t1, t1, __SIZEOF_POINTER__
  163. blt t1, t2, _fdt_reloc_again
  164. _fdt_reloc_done:
  165. /* Update boot hart flag */
  166. la a4, _boot_hart_done
  167. li a5, 1
  168. REG_S a5, (a4)
  169. /* Wait for boot hart */
  170. _wait_for_boot_hart:
  171. la a4, _boot_hart_done
  172. REG_L a5, (a4)
  173. /* Reduce the bus traffic so that boot hart may proceed faster */
  174. nop
  175. nop
  176. nop
  177. beqz a5, _wait_for_boot_hart
  178. _start_warm:
  179. li ra, 0
  180. call _reset_regs
  181. /* Disable and clear all interrupts */
  182. csrw CSR_MIE, zero
  183. csrw CSR_MIP, zero
  184. la a4, platform
  185. #if __riscv_xlen == 64
  186. lwu s7, SBI_PLATFORM_HART_COUNT_OFFSET(a4)
  187. lwu s8, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(a4)
  188. #else
  189. lw s7, SBI_PLATFORM_HART_COUNT_OFFSET(a4)
  190. lw s8, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(a4)
  191. #endif
  192. /* HART ID should be within expected limit */
  193. csrr s6, CSR_MHARTID
  194. bge s6, s7, _start_hang
  195. /* find the scratch space for this hart */
  196. la tp, _fw_end
  197. mul a5, s7, s8
  198. add tp, tp, a5
  199. mul a5, s8, s6
  200. sub tp, tp, a5
  201. li a5, SBI_SCRATCH_SIZE
  202. sub tp, tp, a5
  203. /* update the mscratch */
  204. csrw CSR_MSCRATCH, tp
  205. /* Setup stack */
  206. add sp, tp, zero
  207. /* Setup trap handler */
  208. la a4, _trap_handler
  209. csrw CSR_MTVEC, a4
  210. /* Make sure that mtvec is updated */
  211. 1:
  212. csrr a5, CSR_MTVEC
  213. bne a4, a5, 1b
  214. /* Initialize SBI runtime */
  215. csrr a0, CSR_MSCRATCH
  216. call sbi_init
  217. /* We don't expect to reach here hence just hang */
  218. j _start_hang
  219. .align 3
  220. .section .data, "aw"
  221. _boot_hart_done:
  222. RISCV_PTR 0
  223. .align 3
  224. .section .entry, "ax", %progbits
  225. .globl _hartid_to_scratch
  226. _hartid_to_scratch:
  227. add sp, sp, -(3 * __SIZEOF_POINTER__)
  228. REG_S s0, (sp)
  229. REG_S s1, (__SIZEOF_POINTER__)(sp)
  230. REG_S s2, (__SIZEOF_POINTER__ * 2)(sp)
  231. /*
  232. * a0 -> HART ID (passed by caller)
  233. * s0 -> HART Stack Size
  234. * s1 -> HART Stack End
  235. * s2 -> Temporary
  236. */
  237. la s2, platform
  238. #if __riscv_xlen == 64
  239. lwu s0, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(s2)
  240. lwu s2, SBI_PLATFORM_HART_COUNT_OFFSET(s2)
  241. #else
  242. lw s0, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(s2)
  243. lw s2, SBI_PLATFORM_HART_COUNT_OFFSET(s2)
  244. #endif
  245. mul s2, s2, s0
  246. la s1, _fw_end
  247. add s1, s1, s2
  248. mul s2, s0, a0
  249. sub s1, s1, s2
  250. li s2, SBI_SCRATCH_SIZE
  251. sub a0, s1, s2
  252. REG_L s0, (sp)
  253. REG_L s1, (__SIZEOF_POINTER__)(sp)
  254. REG_L s2, (__SIZEOF_POINTER__ * 2)(sp)
  255. add sp, sp, (3 * __SIZEOF_POINTER__)
  256. ret
  257. .align 3
  258. .section .entry, "ax", %progbits
  259. .globl _start_hang
  260. _start_hang:
  261. wfi
  262. j _start_hang
  263. .align 3
  264. .section .entry, "ax", %progbits
  265. .globl _trap_handler
  266. _trap_handler:
  267. /* Swap TP and MSCRATCH */
  268. csrrw tp, CSR_MSCRATCH, tp
  269. /* Save T0 in scratch space */
  270. REG_S t0, SBI_SCRATCH_TMP0_OFFSET(tp)
  271. /* Check which mode we came from */
  272. csrr t0, CSR_MSTATUS
  273. srl t0, t0, MSTATUS_MPP_SHIFT
  274. and t0, t0, PRV_M
  275. xori t0, t0, PRV_M
  276. beq t0, zero, _trap_handler_m_mode
  277. /* We came from S-mode or U-mode */
  278. _trap_handler_s_mode:
  279. /* Set T0 to original SP */
  280. add t0, sp, zero
  281. /* Setup exception stack */
  282. add sp, tp, -(SBI_TRAP_REGS_SIZE)
  283. /* Jump to code common for all modes */
  284. j _trap_handler_all_mode
  285. /* We came from M-mode */
  286. _trap_handler_m_mode:
  287. /* Set T0 to original SP */
  288. add t0, sp, zero
  289. /* Re-use current SP as exception stack */
  290. add sp, sp, -(SBI_TRAP_REGS_SIZE)
  291. _trap_handler_all_mode:
  292. /* Save original SP (from T0) on stack */
  293. REG_S t0, SBI_TRAP_REGS_OFFSET(sp)(sp)
  294. /* Restore T0 from scratch space */
  295. REG_L t0, SBI_SCRATCH_TMP0_OFFSET(tp)
  296. /* Save T0 on stack */
  297. REG_S t0, SBI_TRAP_REGS_OFFSET(t0)(sp)
  298. /* Swap TP and MSCRATCH */
  299. csrrw tp, CSR_MSCRATCH, tp
  300. /* Save MEPC and MSTATUS CSRs */
  301. csrr t0, CSR_MEPC
  302. REG_S t0, SBI_TRAP_REGS_OFFSET(mepc)(sp)
  303. csrr t0, CSR_MSTATUS
  304. REG_S t0, SBI_TRAP_REGS_OFFSET(mstatus)(sp)
  305. /* Save all general regisers except SP and T0 */
  306. REG_S zero, SBI_TRAP_REGS_OFFSET(zero)(sp)
  307. REG_S ra, SBI_TRAP_REGS_OFFSET(ra)(sp)
  308. REG_S gp, SBI_TRAP_REGS_OFFSET(gp)(sp)
  309. REG_S tp, SBI_TRAP_REGS_OFFSET(tp)(sp)
  310. REG_S t1, SBI_TRAP_REGS_OFFSET(t1)(sp)
  311. REG_S t2, SBI_TRAP_REGS_OFFSET(t2)(sp)
  312. REG_S s0, SBI_TRAP_REGS_OFFSET(s0)(sp)
  313. REG_S s1, SBI_TRAP_REGS_OFFSET(s1)(sp)
  314. REG_S a0, SBI_TRAP_REGS_OFFSET(a0)(sp)
  315. REG_S a1, SBI_TRAP_REGS_OFFSET(a1)(sp)
  316. REG_S a2, SBI_TRAP_REGS_OFFSET(a2)(sp)
  317. REG_S a3, SBI_TRAP_REGS_OFFSET(a3)(sp)
  318. REG_S a4, SBI_TRAP_REGS_OFFSET(a4)(sp)
  319. REG_S a5, SBI_TRAP_REGS_OFFSET(a5)(sp)
  320. REG_S a6, SBI_TRAP_REGS_OFFSET(a6)(sp)
  321. REG_S a7, SBI_TRAP_REGS_OFFSET(a7)(sp)
  322. REG_S s2, SBI_TRAP_REGS_OFFSET(s2)(sp)
  323. REG_S s3, SBI_TRAP_REGS_OFFSET(s3)(sp)
  324. REG_S s4, SBI_TRAP_REGS_OFFSET(s4)(sp)
  325. REG_S s5, SBI_TRAP_REGS_OFFSET(s5)(sp)
  326. REG_S s6, SBI_TRAP_REGS_OFFSET(s6)(sp)
  327. REG_S s7, SBI_TRAP_REGS_OFFSET(s7)(sp)
  328. REG_S s8, SBI_TRAP_REGS_OFFSET(s8)(sp)
  329. REG_S s9, SBI_TRAP_REGS_OFFSET(s9)(sp)
  330. REG_S s10, SBI_TRAP_REGS_OFFSET(s10)(sp)
  331. REG_S s11, SBI_TRAP_REGS_OFFSET(s11)(sp)
  332. REG_S t3, SBI_TRAP_REGS_OFFSET(t3)(sp)
  333. REG_S t4, SBI_TRAP_REGS_OFFSET(t4)(sp)
  334. REG_S t5, SBI_TRAP_REGS_OFFSET(t5)(sp)
  335. REG_S t6, SBI_TRAP_REGS_OFFSET(t6)(sp)
  336. /* Call C routine */
  337. add a0, sp, zero
  338. csrr a1, CSR_MSCRATCH
  339. call sbi_trap_handler
  340. /* Restore all general regisers except SP and T0 */
  341. REG_L ra, SBI_TRAP_REGS_OFFSET(ra)(sp)
  342. REG_L gp, SBI_TRAP_REGS_OFFSET(gp)(sp)
  343. REG_L tp, SBI_TRAP_REGS_OFFSET(tp)(sp)
  344. REG_L t1, SBI_TRAP_REGS_OFFSET(t1)(sp)
  345. REG_L t2, SBI_TRAP_REGS_OFFSET(t2)(sp)
  346. REG_L s0, SBI_TRAP_REGS_OFFSET(s0)(sp)
  347. REG_L s1, SBI_TRAP_REGS_OFFSET(s1)(sp)
  348. REG_L a0, SBI_TRAP_REGS_OFFSET(a0)(sp)
  349. REG_L a1, SBI_TRAP_REGS_OFFSET(a1)(sp)
  350. REG_L a2, SBI_TRAP_REGS_OFFSET(a2)(sp)
  351. REG_L a3, SBI_TRAP_REGS_OFFSET(a3)(sp)
  352. REG_L a4, SBI_TRAP_REGS_OFFSET(a4)(sp)
  353. REG_L a5, SBI_TRAP_REGS_OFFSET(a5)(sp)
  354. REG_L a6, SBI_TRAP_REGS_OFFSET(a6)(sp)
  355. REG_L a7, SBI_TRAP_REGS_OFFSET(a7)(sp)
  356. REG_L s2, SBI_TRAP_REGS_OFFSET(s2)(sp)
  357. REG_L s3, SBI_TRAP_REGS_OFFSET(s3)(sp)
  358. REG_L s4, SBI_TRAP_REGS_OFFSET(s4)(sp)
  359. REG_L s5, SBI_TRAP_REGS_OFFSET(s5)(sp)
  360. REG_L s6, SBI_TRAP_REGS_OFFSET(s6)(sp)
  361. REG_L s7, SBI_TRAP_REGS_OFFSET(s7)(sp)
  362. REG_L s8, SBI_TRAP_REGS_OFFSET(s8)(sp)
  363. REG_L s9, SBI_TRAP_REGS_OFFSET(s9)(sp)
  364. REG_L s10, SBI_TRAP_REGS_OFFSET(s10)(sp)
  365. REG_L s11, SBI_TRAP_REGS_OFFSET(s11)(sp)
  366. REG_L t3, SBI_TRAP_REGS_OFFSET(t3)(sp)
  367. REG_L t4, SBI_TRAP_REGS_OFFSET(t4)(sp)
  368. REG_L t5, SBI_TRAP_REGS_OFFSET(t5)(sp)
  369. REG_L t6, SBI_TRAP_REGS_OFFSET(t6)(sp)
  370. /* Restore MEPC and MSTATUS CSRs */
  371. REG_L t0, SBI_TRAP_REGS_OFFSET(mepc)(sp)
  372. csrw CSR_MEPC, t0
  373. REG_L t0, SBI_TRAP_REGS_OFFSET(mstatus)(sp)
  374. csrw CSR_MSTATUS, t0
  375. /* Restore T0 */
  376. REG_L t0, SBI_TRAP_REGS_OFFSET(t0)(sp)
  377. /* Restore SP */
  378. REG_L sp, SBI_TRAP_REGS_OFFSET(sp)(sp)
  379. mret
  380. .align 3
  381. .section .entry, "ax", %progbits
  382. .globl _reset_regs
  383. _reset_regs:
  384. /* flush the instruction cache */
  385. fence.i
  386. /* Reset all registers except ra, a0,a1 */
  387. li sp, 0
  388. li gp, 0
  389. li tp, 0
  390. li t0, 0
  391. li t1, 0
  392. li t2, 0
  393. li s0, 0
  394. li s1, 0
  395. li a2, 0
  396. li a3, 0
  397. li a4, 0
  398. li a5, 0
  399. li a6, 0
  400. li a7, 0
  401. li s2, 0
  402. li s3, 0
  403. li s4, 0
  404. li s5, 0
  405. li s6, 0
  406. li s7, 0
  407. li s8, 0
  408. li s9, 0
  409. li s10, 0
  410. li s11, 0
  411. li t3, 0
  412. li t4, 0
  413. li t5, 0
  414. li t6, 0
  415. csrw CSR_MSCRATCH, 0
  416. ret