-/*
- * Remain in M-mode after mret
- */
- li t0, MSTATUS_MPP
- csrs mstatus, t0
- LREG x1, 1*REGBYTES(sp)
- LREG x2, 2*REGBYTES(sp)
- LREG x3, 3*REGBYTES(sp)
- LREG x4, 4*REGBYTES(sp)
- LREG x5, 5*REGBYTES(sp)
- LREG x6, 6*REGBYTES(sp)
- LREG x7, 7*REGBYTES(sp)
- LREG x8, 8*REGBYTES(sp)
- LREG x9, 9*REGBYTES(sp)
- LREG x10, 10*REGBYTES(sp)
- LREG x11, 11*REGBYTES(sp)
- LREG x12, 12*REGBYTES(sp)
- LREG x13, 13*REGBYTES(sp)
- LREG x14, 14*REGBYTES(sp)
- LREG x15, 15*REGBYTES(sp)
- LREG x16, 16*REGBYTES(sp)
- LREG x17, 17*REGBYTES(sp)
- LREG x18, 18*REGBYTES(sp)
- LREG x19, 19*REGBYTES(sp)
- LREG x20, 20*REGBYTES(sp)
- LREG x21, 21*REGBYTES(sp)
- LREG x22, 22*REGBYTES(sp)
- LREG x23, 23*REGBYTES(sp)
- LREG x24, 24*REGBYTES(sp)
- LREG x25, 25*REGBYTES(sp)
- LREG x26, 26*REGBYTES(sp)
- LREG x27, 27*REGBYTES(sp)
- LREG x28, 28*REGBYTES(sp)
- LREG x29, 29*REGBYTES(sp)
- LREG x30, 30*REGBYTES(sp)
- LREG x31, 31*REGBYTES(sp)
- addi sp, sp, 32*REGBYTES
- mret
-
-#ifdef CONFIG_INIT_CRITICAL
-cpu_init_crit:
- ret
+/* SMP relocation entry */
+secondary_hart_relocate:
+ /* a1: new sp */
+ /* a2: new gd */
+ /* tp: hart id */
+
+ /* setup stack */
+ slli t0, tp, CONFIG_STACK_SIZE_SHIFT
+ sub sp, a1, t0
+
+ /* update global data pointer */
+ mv gp, a2
+#endif
+
+secondary_hart_loop:
+ wfi
+
+#ifdef CONFIG_SMP
+ csrr t0, MODE_PREFIX(ip)
+#if CONFIG_IS_ENABLED(RISCV_MMODE)
+ andi t0, t0, MIE_MSIE
+#else
+ andi t0, t0, SIE_SSIE
+#endif
+ beqz t0, secondary_hart_loop
+
+ mv a0, tp
+ jal handle_ipi