You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
296 lines
6.1 KiB
296 lines
6.1 KiB
7 years ago
|
// See LICENSE for license details.
|
||
|
|
|
||
|
|
#include "mtrap.h"
|
||
|
|
#include "bits.h"
|
||
|
|
|
||
|
|
.data
|
||
|
|
.align 6
|
||
|
|
trap_table:
|
||
|
|
#define BAD_TRAP_VECTOR 0
|
||
|
|
.word bad_trap
|
||
|
|
.word pmp_trap
|
||
|
|
.word illegal_insn_trap
|
||
|
|
.word bad_trap
|
||
|
|
.word misaligned_load_trap
|
||
|
|
.word pmp_trap
|
||
|
|
.word misaligned_store_trap
|
||
|
|
.word pmp_trap
|
||
|
|
.word bad_trap
|
||
|
|
.word mcall_trap
|
||
|
|
.word bad_trap
|
||
|
|
.word bad_trap
|
||
|
|
.word bad_trap
|
||
|
|
#define TRAP_FROM_MACHINE_MODE_VECTOR 13
|
||
|
|
.word __trap_from_machine_mode
|
||
|
|
.word bad_trap
|
||
|
|
.word bad_trap
|
||
|
|
|
||
|
|
.option norvc
|
||
|
|
.section .text.init,"ax",@progbits
|
||
|
|
.globl reset_vector
|
||
|
|
reset_vector:
|
||
|
|
j do_reset
|
||
|
|
|
||
|
|
trap_vector:
|
||
|
|
csrrw sp, mscratch, sp
|
||
|
|
beqz sp, .Ltrap_from_machine_mode
|
||
|
|
|
||
|
|
STORE a0, 10*REGBYTES(sp)
|
||
|
|
STORE a1, 11*REGBYTES(sp)
|
||
|
|
|
||
|
|
csrr a1, mcause
|
||
|
|
bgez a1, .Lhandle_trap_in_machine_mode
|
||
|
|
|
||
|
|
# This is an interrupt. Discard the mcause MSB and decode the rest.
|
||
|
|
sll a1, a1, 1
|
||
|
|
|
||
|
|
# Is it a machine timer interrupt?
|
||
|
|
li a0, IRQ_M_TIMER * 2
|
||
|
|
bne a0, a1, 1f
|
||
|
|
|
||
|
|
# Yes. Simply clear MSIE and raise SSIP.
|
||
|
|
li a0, MIP_MTIP
|
||
|
|
csrc mie, a0
|
||
|
|
li a0, MIP_STIP
|
||
|
|
csrs mip, a0
|
||
|
|
|
||
|
|
.Lmret:
|
||
|
|
# Go back whence we came.
|
||
|
|
LOAD a0, 10*REGBYTES(sp)
|
||
|
|
LOAD a1, 11*REGBYTES(sp)
|
||
|
|
csrrw sp, mscratch, sp
|
||
|
|
mret
|
||
|
|
|
||
|
|
1:
|
||
|
|
# Is it an IPI?
|
||
|
|
li a0, IRQ_M_SOFT * 2
|
||
|
|
bne a0, a1, .Lbad_trap
|
||
|
|
|
||
|
|
# Yes. First, clear the MIPI bit.
|
||
|
|
LOAD a0, MENTRY_IPI_OFFSET(sp)
|
||
|
|
sw x0, (a0)
|
||
|
|
fence
|
||
|
|
|
||
|
|
# Now, decode the cause(s).
|
||
|
|
#ifdef __riscv_atomic
|
||
|
|
addi a0, sp, MENTRY_IPI_PENDING_OFFSET
|
||
|
|
amoswap.w a0, x0, (a0)
|
||
|
|
#else
|
||
|
|
lw a0, MENTRY_IPI_PENDING_OFFSET(a0)
|
||
|
|
sw x0, MENTRY_IPI_PENDING_OFFSET(a0)
|
||
|
|
#endif
|
||
|
|
and a1, a0, IPI_SOFT
|
||
|
|
beqz a1, 1f
|
||
|
|
csrs mip, MIP_SSIP
|
||
|
|
1:
|
||
|
|
andi a1, a0, IPI_FENCE_I
|
||
|
|
beqz a1, 1f
|
||
|
|
fence.i
|
||
|
|
1:
|
||
|
|
andi a1, a0, IPI_SFENCE_VMA
|
||
|
|
beqz a1, 1f
|
||
|
|
sfence.vma
|
||
|
|
1:
|
||
|
|
j .Lmret
|
||
|
|
|
||
|
|
|
||
|
|
.Lhandle_trap_in_machine_mode:
|
||
|
|
# Preserve the registers. Compute the address of the trap handler.
|
||
|
|
STORE ra, 1*REGBYTES(sp)
|
||
|
|
STORE gp, 3*REGBYTES(sp)
|
||
|
|
STORE tp, 4*REGBYTES(sp)
|
||
|
|
STORE t0, 5*REGBYTES(sp)
|
||
|
|
1:auipc t0, %pcrel_hi(trap_table) # t0 <- %hi(trap_table)
|
||
|
|
STORE t1, 6*REGBYTES(sp)
|
||
|
|
sll t1, a1, 2 # t1 <- mcause << 2
|
||
|
|
STORE t2, 7*REGBYTES(sp)
|
||
|
|
add t1, t0, t1 # t1 <- %hi(trap_table)[mcause]
|
||
|
|
STORE s0, 8*REGBYTES(sp)
|
||
|
|
LWU t1, %pcrel_lo(1b)(t1) # t1 <- trap_table[mcause]
|
||
|
|
STORE s1, 9*REGBYTES(sp)
|
||
|
|
mv a0, sp # a0 <- regs
|
||
|
|
STORE a2,12*REGBYTES(sp)
|
||
|
|
csrr a2, mepc # a2 <- mepc
|
||
|
|
STORE a3,13*REGBYTES(sp)
|
||
|
|
csrrw t0, mscratch, x0 # t0 <- user sp
|
||
|
|
STORE a4,14*REGBYTES(sp)
|
||
|
|
STORE a5,15*REGBYTES(sp)
|
||
|
|
STORE a6,16*REGBYTES(sp)
|
||
|
|
STORE a7,17*REGBYTES(sp)
|
||
|
|
STORE s2,18*REGBYTES(sp)
|
||
|
|
STORE s3,19*REGBYTES(sp)
|
||
|
|
STORE s4,20*REGBYTES(sp)
|
||
|
|
STORE s5,21*REGBYTES(sp)
|
||
|
|
STORE s6,22*REGBYTES(sp)
|
||
|
|
STORE s7,23*REGBYTES(sp)
|
||
|
|
STORE s8,24*REGBYTES(sp)
|
||
|
|
STORE s9,25*REGBYTES(sp)
|
||
|
|
STORE s10,26*REGBYTES(sp)
|
||
|
|
STORE s11,27*REGBYTES(sp)
|
||
|
|
STORE t3,28*REGBYTES(sp)
|
||
|
|
STORE t4,29*REGBYTES(sp)
|
||
|
|
STORE t5,30*REGBYTES(sp)
|
||
|
|
STORE t6,31*REGBYTES(sp)
|
||
|
|
STORE t0, 2*REGBYTES(sp) # sp
|
||
|
|
|
||
|
|
#ifndef __riscv_flen
|
||
|
|
lw tp, (sp) # Move the emulated FCSR from x0's save slot into tp.
|
||
|
|
#endif
|
||
|
|
STORE x0, (sp) # Zero x0's save slot.
|
||
|
|
|
||
|
|
# Invoke the handler.
|
||
|
|
jalr t1
|
||
|
|
|
||
|
|
#ifndef __riscv_flen
|
||
|
|
sw tp, (sp) # Move the emulated FCSR from tp into x0's save slot.
|
||
|
|
#endif
|
||
|
|
|
||
|
|
restore_mscratch:
|
||
|
|
# Restore mscratch, so future traps will know they didn't come from M-mode.
|
||
|
|
csrw mscratch, sp
|
||
|
|
|
||
|
|
restore_regs:
|
||
|
|
# Restore all of the registers.
|
||
|
|
LOAD ra, 1*REGBYTES(sp)
|
||
|
|
LOAD gp, 3*REGBYTES(sp)
|
||
|
|
LOAD tp, 4*REGBYTES(sp)
|
||
|
|
LOAD t0, 5*REGBYTES(sp)
|
||
|
|
LOAD t1, 6*REGBYTES(sp)
|
||
|
|
LOAD t2, 7*REGBYTES(sp)
|
||
|
|
LOAD s0, 8*REGBYTES(sp)
|
||
|
|
LOAD s1, 9*REGBYTES(sp)
|
||
|
|
LOAD a0,10*REGBYTES(sp)
|
||
|
|
LOAD a1,11*REGBYTES(sp)
|
||
|
|
LOAD a2,12*REGBYTES(sp)
|
||
|
|
LOAD a3,13*REGBYTES(sp)
|
||
|
|
LOAD a4,14*REGBYTES(sp)
|
||
|
|
LOAD a5,15*REGBYTES(sp)
|
||
|
|
LOAD a6,16*REGBYTES(sp)
|
||
|
|
LOAD a7,17*REGBYTES(sp)
|
||
|
|
LOAD s2,18*REGBYTES(sp)
|
||
|
|
LOAD s3,19*REGBYTES(sp)
|
||
|
|
LOAD s4,20*REGBYTES(sp)
|
||
|
|
LOAD s5,21*REGBYTES(sp)
|
||
|
|
LOAD s6,22*REGBYTES(sp)
|
||
|
|
LOAD s7,23*REGBYTES(sp)
|
||
|
|
LOAD s8,24*REGBYTES(sp)
|
||
|
|
LOAD s9,25*REGBYTES(sp)
|
||
|
|
LOAD s10,26*REGBYTES(sp)
|
||
|
|
LOAD s11,27*REGBYTES(sp)
|
||
|
|
LOAD t3,28*REGBYTES(sp)
|
||
|
|
LOAD t4,29*REGBYTES(sp)
|
||
|
|
LOAD t5,30*REGBYTES(sp)
|
||
|
|
LOAD t6,31*REGBYTES(sp)
|
||
|
|
LOAD sp, 2*REGBYTES(sp)
|
||
|
|
mret
|
||
|
|
|
||
|
|
.Ltrap_from_machine_mode:
|
||
|
|
csrr sp, mscratch
|
||
|
|
addi sp, sp, -INTEGER_CONTEXT_SIZE
|
||
|
|
STORE a0,10*REGBYTES(sp)
|
||
|
|
STORE a1,11*REGBYTES(sp)
|
||
|
|
li a1, TRAP_FROM_MACHINE_MODE_VECTOR
|
||
|
|
j .Lhandle_trap_in_machine_mode
|
||
|
|
|
||
|
|
.Lbad_trap:
|
||
|
|
li a1, BAD_TRAP_VECTOR
|
||
|
|
j .Lhandle_trap_in_machine_mode
|
||
|
|
|
||
|
|
.globl __redirect_trap
|
||
|
|
__redirect_trap:
|
||
|
|
# reset sp to top of M-mode stack
|
||
|
|
li t0, MACHINE_STACK_SIZE
|
||
|
|
add sp, sp, t0
|
||
|
|
neg t0, t0
|
||
|
|
and sp, sp, t0
|
||
|
|
addi sp, sp, -MENTRY_FRAME_SIZE
|
||
|
|
j restore_mscratch
|
||
|
|
|
||
|
|
__trap_from_machine_mode:
|
||
|
|
jal trap_from_machine_mode
|
||
|
|
j restore_regs
|
||
|
|
|
||
|
|
do_reset:
|
||
|
|
li x1, 0
|
||
|
|
li x2, 0
|
||
|
|
li x3, 0
|
||
|
|
li x4, 0
|
||
|
|
li x5, 0
|
||
|
|
li x6, 0
|
||
|
|
li x7, 0
|
||
|
|
li x8, 0
|
||
|
|
li x9, 0
|
||
|
|
// save a0 and a1; arguments from previous boot loader stage:
|
||
|
|
// li x10, 0
|
||
|
|
// li x11, 0
|
||
|
|
li x12, 0
|
||
|
|
li x13, 0
|
||
|
|
li x14, 0
|
||
|
|
li x15, 0
|
||
|
|
li x16, 0
|
||
|
|
li x17, 0
|
||
|
|
li x18, 0
|
||
|
|
li x19, 0
|
||
|
|
li x20, 0
|
||
|
|
li x21, 0
|
||
|
|
li x22, 0
|
||
|
|
li x23, 0
|
||
|
|
li x24, 0
|
||
|
|
li x25, 0
|
||
|
|
li x26, 0
|
||
|
|
li x27, 0
|
||
|
|
li x28, 0
|
||
|
|
li x29, 0
|
||
|
|
li x30, 0
|
||
|
|
li x31, 0
|
||
|
|
csrw mscratch, x0
|
||
|
|
|
||
|
|
# write mtvec and make sure it sticks
|
||
|
|
la t0, trap_vector
|
||
|
|
csrw mtvec, t0
|
||
|
|
csrr t1, mtvec
|
||
|
|
1:bne t0, t1, 1b
|
||
|
|
|
||
|
|
la sp, stacks + RISCV_PGSIZE - MENTRY_FRAME_SIZE
|
||
|
|
|
||
|
|
csrr a3, mhartid
|
||
|
|
slli a2, a3, RISCV_PGSHIFT
|
||
|
|
add sp, sp, a2
|
||
|
|
|
||
|
|
# Boot on the first hart
|
||
|
|
beqz a3, init_first_hart
|
||
|
|
|
||
|
|
# set MSIE bit to receive IPI
|
||
|
|
li a2, MIP_MSIP
|
||
|
|
csrw mie, a2
|
||
|
|
|
||
|
|
.LmultiHart:
|
||
|
|
#if MAX_HARTS > 1
|
||
|
|
# wait for an IPI to signal that it's safe to boot
|
||
|
|
wfi
|
||
|
|
|
||
|
|
# masked harts never start
|
||
|
|
la a4, disabled_hart_mask
|
||
|
|
LOAD a4, 0(a4)
|
||
|
|
srl a4, a4, a3
|
||
|
|
andi a4, a4, 1
|
||
|
|
bnez a4, .LmultiHart
|
||
|
|
|
||
|
|
# only start if mip is set
|
||
|
|
csrr a2, mip
|
||
|
|
andi a2, a2, MIP_MSIP
|
||
|
|
beqz a2, .LmultiHart
|
||
|
|
|
||
|
|
# make sure our hart id is within a valid range
|
||
|
|
fence
|
||
|
|
li a2, MAX_HARTS
|
||
|
|
bltu a3, a2, init_other_hart
|
||
|
|
#endif
|
||
|
|
wfi
|
||
|
|
j .LmultiHart
|
||
|
|
|
||
|
|
.bss
|
||
|
|
.align RISCV_PGSHIFT
|
||
|
|
stacks:
|
||
|
|
.skip RISCV_PGSIZE * MAX_HARTS
|