-/*
- * trap entry
- */
-.align 2
-trap_entry:
- addi sp, sp, -32*REGBYTES
- SREG x1, 1*REGBYTES(sp)
- SREG x2, 2*REGBYTES(sp)
- SREG x3, 3*REGBYTES(sp)
- SREG x4, 4*REGBYTES(sp)
- SREG x5, 5*REGBYTES(sp)
- SREG x6, 6*REGBYTES(sp)
- SREG x7, 7*REGBYTES(sp)
- SREG x8, 8*REGBYTES(sp)
- SREG x9, 9*REGBYTES(sp)
- SREG x10, 10*REGBYTES(sp)
- SREG x11, 11*REGBYTES(sp)
- SREG x12, 12*REGBYTES(sp)
- SREG x13, 13*REGBYTES(sp)
- SREG x14, 14*REGBYTES(sp)
- SREG x15, 15*REGBYTES(sp)
- SREG x16, 16*REGBYTES(sp)
- SREG x17, 17*REGBYTES(sp)
- SREG x18, 18*REGBYTES(sp)
- SREG x19, 19*REGBYTES(sp)
- SREG x20, 20*REGBYTES(sp)
- SREG x21, 21*REGBYTES(sp)
- SREG x22, 22*REGBYTES(sp)
- SREG x23, 23*REGBYTES(sp)
- SREG x24, 24*REGBYTES(sp)
- SREG x25, 25*REGBYTES(sp)
- SREG x26, 26*REGBYTES(sp)
- SREG x27, 27*REGBYTES(sp)
- SREG x28, 28*REGBYTES(sp)
- SREG x29, 29*REGBYTES(sp)
- SREG x30, 30*REGBYTES(sp)
- SREG x31, 31*REGBYTES(sp)
- csrr a0, MODE_PREFIX(cause)
- csrr a1, MODE_PREFIX(epc)
- mv a2, sp
- jal handle_trap
- csrw MODE_PREFIX(epc), a0
-
-#ifdef CONFIG_RISCV_SMODE
-/*
- * Remain in S-mode after sret
- */
- li t0, SSTATUS_SPP
+#ifdef CONFIG_SMP
+hart_out_of_bounds_loop:
+ /* Harts in this loop are out of bounds, increase CONFIG_NR_CPUS. */
+ wfi
+ j hart_out_of_bounds_loop
+
+/* 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