380 lines
11 KiB
ArmAsm
380 lines
11 KiB
ArmAsm
|
// See LICENSE for license details
|
||
|
|
|
||
|
|
#ifndef KL3_ENTRY_S
|
||
|
|
#define KL3_ENTRY_S
|
||
|
|
|
||
|
|
#include "encoding.h"
|
||
|
|
#include "bits.h"
|
||
|
|
|
||
|
|
.section .iram
|
||
|
|
|
||
|
|
#define MSTATUS_PRV1 0x1800
|
||
|
|
|
||
|
|
/* When trap is an interrupt, this function is called */
|
||
|
|
interrupt:
|
||
|
|
.global pxCurrentTCB
|
||
|
|
/* make room in stack */
|
||
|
|
#ifndef __riscv_float_abi_soft
|
||
|
|
addi sp, sp, -REGBYTES * 64
|
||
|
|
#else
|
||
|
|
addi sp, sp, -REGBYTES * 32
|
||
|
|
#endif
|
||
|
|
/* Save Context */
|
||
|
|
STORE x1, 0x0(sp)
|
||
|
|
STORE x2, 1 * REGBYTES(sp)
|
||
|
|
STORE x3, 2 * REGBYTES(sp)
|
||
|
|
STORE x4, 3 * REGBYTES(sp)
|
||
|
|
STORE x6, 5 * REGBYTES(sp)
|
||
|
|
STORE x7, 6 * REGBYTES(sp)
|
||
|
|
STORE x8, 30 * REGBYTES(sp)
|
||
|
|
STORE x9, 8 * REGBYTES(sp)
|
||
|
|
STORE x10, 9 * REGBYTES(sp)
|
||
|
|
STORE x11, 10 * REGBYTES(sp)
|
||
|
|
STORE x12, 11 * REGBYTES(sp)
|
||
|
|
STORE x13, 12 * REGBYTES(sp)
|
||
|
|
STORE x14, 13 * REGBYTES(sp)
|
||
|
|
STORE x15, 14 * REGBYTES(sp)
|
||
|
|
STORE x16, 15 * REGBYTES(sp)
|
||
|
|
STORE x17, 16 * REGBYTES(sp)
|
||
|
|
STORE x18, 17 * REGBYTES(sp)
|
||
|
|
STORE x19, 18 * REGBYTES(sp)
|
||
|
|
STORE x20, 19 * REGBYTES(sp)
|
||
|
|
STORE x21, 20 * REGBYTES(sp)
|
||
|
|
STORE x22, 21 * REGBYTES(sp)
|
||
|
|
STORE x23, 22 * REGBYTES(sp)
|
||
|
|
STORE x24, 23 * REGBYTES(sp)
|
||
|
|
STORE x25, 24 * REGBYTES(sp)
|
||
|
|
STORE x26, 25 * REGBYTES(sp)
|
||
|
|
STORE x27, 26 * REGBYTES(sp)
|
||
|
|
STORE x28, 27 * REGBYTES(sp)
|
||
|
|
STORE x29, 28 * REGBYTES(sp)
|
||
|
|
STORE x30, 29 * REGBYTES(sp)
|
||
|
|
STORE x31, 7 * REGBYTES(sp)
|
||
|
|
#ifndef __riscv_float_abi_soft
|
||
|
|
fsw f0, 31 * REGBYTES(sp)
|
||
|
|
fsw f1, 32 * REGBYTES(sp)
|
||
|
|
fsw f2, 33 * REGBYTES(sp)
|
||
|
|
fsw f3, 34 * REGBYTES(sp)
|
||
|
|
fsw f4, 35 * REGBYTES(sp)
|
||
|
|
fsw f5, 36 * REGBYTES(sp)
|
||
|
|
fsw f6, 37 * REGBYTES(sp)
|
||
|
|
fsw f7, 38 * REGBYTES(sp)
|
||
|
|
fsw f8, 39 * REGBYTES(sp)
|
||
|
|
fsw f9, 40 * REGBYTES(sp)
|
||
|
|
fsw f10, 41 * REGBYTES(sp)
|
||
|
|
fsw f11, 42 * REGBYTES(sp)
|
||
|
|
fsw f12, 43 * REGBYTES(sp)
|
||
|
|
fsw f13, 44 * REGBYTES(sp)
|
||
|
|
fsw f14, 45 * REGBYTES(sp)
|
||
|
|
fsw f15, 46 * REGBYTES(sp)
|
||
|
|
fsw f16, 47 * REGBYTES(sp)
|
||
|
|
fsw f17, 48 * REGBYTES(sp)
|
||
|
|
fsw f18, 49 * REGBYTES(sp)
|
||
|
|
fsw f19, 50 * REGBYTES(sp)
|
||
|
|
fsw f20, 51 * REGBYTES(sp)
|
||
|
|
fsw f21, 52 * REGBYTES(sp)
|
||
|
|
fsw f22, 53 * REGBYTES(sp)
|
||
|
|
fsw f23, 54 * REGBYTES(sp)
|
||
|
|
fsw f24, 55 * REGBYTES(sp)
|
||
|
|
fsw f25, 56 * REGBYTES(sp)
|
||
|
|
fsw f26, 57 * REGBYTES(sp)
|
||
|
|
fsw f27, 58 * REGBYTES(sp)
|
||
|
|
fsw f28, 59 * REGBYTES(sp)
|
||
|
|
fsw f29, 60 * REGBYTES(sp)
|
||
|
|
fsw f30, 61 * REGBYTES(sp)
|
||
|
|
fsw f31, 62 * REGBYTES(sp)
|
||
|
|
#endif
|
||
|
|
csrr x5, mscratch
|
||
|
|
STORE x5, 4 * REGBYTES(sp)
|
||
|
|
/* Store current stackpointer in task control block (TCB) */
|
||
|
|
LOAD t0, pxCurrentTCB //pointer
|
||
|
|
|
||
|
|
#if RISCV_SMP_ENABLE
|
||
|
|
csrr t1, mhartid
|
||
|
|
li t2, 1
|
||
|
|
beq t1, t2, 3f
|
||
|
|
LOAD t0, pxCurrentTCB+4
|
||
|
|
3:
|
||
|
|
#endif
|
||
|
|
|
||
|
|
STORE sp, 0x0(t0)
|
||
|
|
/* Saves current error program counter (EPC) as task program counter */
|
||
|
|
csrr t0, mepc
|
||
|
|
#ifndef __riscv_float_abi_soft
|
||
|
|
STORE t0, 63 * REGBYTES(sp)
|
||
|
|
#else
|
||
|
|
STORE t0, 31 * REGBYTES(sp)
|
||
|
|
#endif
|
||
|
|
/* Interrupt handling*/
|
||
|
|
csrr a0, mcause
|
||
|
|
csrr a1, mepc
|
||
|
|
mv a2, sp
|
||
|
|
call handle_interrupt
|
||
|
|
NOP
|
||
|
|
/* Load stack pointer from the current TCB */
|
||
|
|
LOAD sp, pxCurrentTCB
|
||
|
|
|
||
|
|
#if RISCV_SMP_ENABLE
|
||
|
|
csrr t1, mhartid
|
||
|
|
li t0, 1
|
||
|
|
beq t1, t0, 3f
|
||
|
|
LOAD sp, pxCurrentTCB+4
|
||
|
|
3:
|
||
|
|
#endif
|
||
|
|
LOAD sp, 0x0(sp)
|
||
|
|
/* Load task program counter */
|
||
|
|
#ifndef __riscv_float_abi_soft
|
||
|
|
LOAD t0, 63 * REGBYTES(sp)
|
||
|
|
#else
|
||
|
|
LOAD t0, 31 * REGBYTES(sp)
|
||
|
|
#endif
|
||
|
|
csrw mepc, t0
|
||
|
|
/* Run in machine mode */
|
||
|
|
li t0, MSTATUS_PRV1
|
||
|
|
csrs mstatus, t0
|
||
|
|
/* Restore registers, Skip global pointer because that does not change */
|
||
|
|
LOAD x1, 0x0(sp)
|
||
|
|
LOAD x4, 3 * REGBYTES(sp)
|
||
|
|
LOAD x5, 4 * REGBYTES(sp)
|
||
|
|
LOAD x6, 5 * REGBYTES(sp)
|
||
|
|
LOAD x7, 6 * REGBYTES(sp)
|
||
|
|
LOAD x8, 30 * REGBYTES(sp)
|
||
|
|
LOAD x9, 8 * REGBYTES(sp)
|
||
|
|
LOAD x10, 9 * REGBYTES(sp)
|
||
|
|
LOAD x11, 10 * REGBYTES(sp)
|
||
|
|
LOAD x12, 11 * REGBYTES(sp)
|
||
|
|
LOAD x13, 12 * REGBYTES(sp)
|
||
|
|
LOAD x14, 13 * REGBYTES(sp)
|
||
|
|
LOAD x15, 14 * REGBYTES(sp)
|
||
|
|
LOAD x16, 15 * REGBYTES(sp)
|
||
|
|
LOAD x17, 16 * REGBYTES(sp)
|
||
|
|
LOAD x18, 17 * REGBYTES(sp)
|
||
|
|
LOAD x19, 18 * REGBYTES(sp)
|
||
|
|
LOAD x20, 19 * REGBYTES(sp)
|
||
|
|
LOAD x21, 20 * REGBYTES(sp)
|
||
|
|
LOAD x22, 21 * REGBYTES(sp)
|
||
|
|
LOAD x23, 22 * REGBYTES(sp)
|
||
|
|
LOAD x24, 23 * REGBYTES(sp)
|
||
|
|
LOAD x25, 24 * REGBYTES(sp)
|
||
|
|
LOAD x26, 25 * REGBYTES(sp)
|
||
|
|
LOAD x27, 26 * REGBYTES(sp)
|
||
|
|
LOAD x28, 27 * REGBYTES(sp)
|
||
|
|
LOAD x29, 28 * REGBYTES(sp)
|
||
|
|
LOAD x30, 29 * REGBYTES(sp)
|
||
|
|
LOAD x31, 7 * REGBYTES(sp)
|
||
|
|
#ifndef __riscv_float_abi_soft
|
||
|
|
flw f0, 31 * REGBYTES(sp)
|
||
|
|
flw f1, 32 * REGBYTES(sp)
|
||
|
|
flw f2, 33 * REGBYTES(sp)
|
||
|
|
flw f3, 34 * REGBYTES(sp)
|
||
|
|
flw f4, 35 * REGBYTES(sp)
|
||
|
|
flw f5, 36 * REGBYTES(sp)
|
||
|
|
flw f6, 37 * REGBYTES(sp)
|
||
|
|
flw f7, 38 * REGBYTES(sp)
|
||
|
|
flw f8, 39 * REGBYTES(sp)
|
||
|
|
flw f9, 40 * REGBYTES(sp)
|
||
|
|
flw f10, 41 * REGBYTES(sp)
|
||
|
|
flw f11, 42 * REGBYTES(sp)
|
||
|
|
flw f12, 43 * REGBYTES(sp)
|
||
|
|
flw f13, 44 * REGBYTES(sp)
|
||
|
|
flw f14, 45 * REGBYTES(sp)
|
||
|
|
flw f15, 46 * REGBYTES(sp)
|
||
|
|
flw f16, 47 * REGBYTES(sp)
|
||
|
|
flw f17, 48 * REGBYTES(sp)
|
||
|
|
flw f18, 49 * REGBYTES(sp)
|
||
|
|
flw f19, 50 * REGBYTES(sp)
|
||
|
|
flw f20, 51 * REGBYTES(sp)
|
||
|
|
flw f21, 52 * REGBYTES(sp)
|
||
|
|
flw f22, 53 * REGBYTES(sp)
|
||
|
|
flw f23, 54 * REGBYTES(sp)
|
||
|
|
flw f24, 55 * REGBYTES(sp)
|
||
|
|
flw f25, 56 * REGBYTES(sp)
|
||
|
|
flw f26, 57 * REGBYTES(sp)
|
||
|
|
flw f27, 58 * REGBYTES(sp)
|
||
|
|
flw f28, 59 * REGBYTES(sp)
|
||
|
|
flw f29, 60 * REGBYTES(sp)
|
||
|
|
flw f30, 61 * REGBYTES(sp)
|
||
|
|
flw f31, 62 * REGBYTES(sp)
|
||
|
|
#endif
|
||
|
|
#ifndef __riscv_float_abi_soft
|
||
|
|
addi sp, sp, REGBYTES * 64
|
||
|
|
#else
|
||
|
|
addi sp, sp, REGBYTES * 32
|
||
|
|
#endif
|
||
|
|
mret
|
||
|
|
|
||
|
|
.section .iram.entry
|
||
|
|
.align 4
|
||
|
|
.global kl3_trap_entry
|
||
|
|
|
||
|
|
kl3_trap_entry:
|
||
|
|
/* Interrupt trap */
|
||
|
|
csrw mscratch, t0
|
||
|
|
csrr t0, mcause
|
||
|
|
blt t0, x0, interrupt
|
||
|
|
|
||
|
|
/* System call and other traps */
|
||
|
|
csrrw t0, mscratch, sp
|
||
|
|
la sp, _trap_sp
|
||
|
|
#ifndef __riscv_float_abi_soft
|
||
|
|
addi sp, sp, -REGBYTES*64
|
||
|
|
#else
|
||
|
|
addi sp, sp, -REGBYTES*32
|
||
|
|
#endif
|
||
|
|
STORE x1, 0*REGBYTES(sp)
|
||
|
|
STORE x2, 1*REGBYTES(sp)
|
||
|
|
STORE x3, 2*REGBYTES(sp)
|
||
|
|
STORE x4, 3*REGBYTES(sp)
|
||
|
|
STORE x5, 4*REGBYTES(sp)
|
||
|
|
STORE x6, 5*REGBYTES(sp)
|
||
|
|
STORE x7, 6*REGBYTES(sp)
|
||
|
|
STORE x8, 30*REGBYTES(sp)
|
||
|
|
STORE x9, 8*REGBYTES(sp)
|
||
|
|
STORE x10, 9*REGBYTES(sp)
|
||
|
|
STORE x11, 10*REGBYTES(sp)
|
||
|
|
STORE x12, 11*REGBYTES(sp)
|
||
|
|
STORE x13, 12*REGBYTES(sp)
|
||
|
|
STORE x14, 13*REGBYTES(sp)
|
||
|
|
STORE x15, 14*REGBYTES(sp)
|
||
|
|
STORE x16, 15*REGBYTES(sp)
|
||
|
|
STORE x17, 16*REGBYTES(sp)
|
||
|
|
STORE x18, 17*REGBYTES(sp)
|
||
|
|
STORE x19, 18*REGBYTES(sp)
|
||
|
|
STORE x20, 19*REGBYTES(sp)
|
||
|
|
STORE x21, 20*REGBYTES(sp)
|
||
|
|
STORE x22, 21*REGBYTES(sp)
|
||
|
|
STORE x23, 22*REGBYTES(sp)
|
||
|
|
STORE x24, 23*REGBYTES(sp)
|
||
|
|
STORE x25, 24*REGBYTES(sp)
|
||
|
|
STORE x26, 25*REGBYTES(sp)
|
||
|
|
STORE x27, 26*REGBYTES(sp)
|
||
|
|
STORE x28, 27*REGBYTES(sp)
|
||
|
|
STORE x29, 28*REGBYTES(sp)
|
||
|
|
STORE x30, 29*REGBYTES(sp)
|
||
|
|
STORE x31, 7*REGBYTES(sp)
|
||
|
|
#ifndef __riscv_float_abi_soft
|
||
|
|
fsw f0, 31 * REGBYTES(sp)
|
||
|
|
fsw f1, 32 * REGBYTES(sp)
|
||
|
|
fsw f2, 33 * REGBYTES(sp)
|
||
|
|
fsw f3, 34 * REGBYTES(sp)
|
||
|
|
fsw f4, 35 * REGBYTES(sp)
|
||
|
|
fsw f5, 36 * REGBYTES(sp)
|
||
|
|
fsw f6, 37 * REGBYTES(sp)
|
||
|
|
fsw f7, 38 * REGBYTES(sp)
|
||
|
|
fsw f8, 39 * REGBYTES(sp)
|
||
|
|
fsw f9, 40 * REGBYTES(sp)
|
||
|
|
fsw f10, 41 * REGBYTES(sp)
|
||
|
|
fsw f11, 42 * REGBYTES(sp)
|
||
|
|
fsw f12, 43 * REGBYTES(sp)
|
||
|
|
fsw f13, 44 * REGBYTES(sp)
|
||
|
|
fsw f14, 45 * REGBYTES(sp)
|
||
|
|
fsw f15, 46 * REGBYTES(sp)
|
||
|
|
fsw f16, 47 * REGBYTES(sp)
|
||
|
|
fsw f17, 48 * REGBYTES(sp)
|
||
|
|
fsw f18, 49 * REGBYTES(sp)
|
||
|
|
fsw f19, 50 * REGBYTES(sp)
|
||
|
|
fsw f20, 51 * REGBYTES(sp)
|
||
|
|
fsw f21, 52 * REGBYTES(sp)
|
||
|
|
fsw f22, 53 * REGBYTES(sp)
|
||
|
|
fsw f23, 54 * REGBYTES(sp)
|
||
|
|
fsw f24, 55 * REGBYTES(sp)
|
||
|
|
fsw f25, 56 * REGBYTES(sp)
|
||
|
|
fsw f26, 57 * REGBYTES(sp)
|
||
|
|
fsw f27, 58 * REGBYTES(sp)
|
||
|
|
fsw f28, 59 * REGBYTES(sp)
|
||
|
|
fsw f29, 60 * REGBYTES(sp)
|
||
|
|
fsw f30, 61 * REGBYTES(sp)
|
||
|
|
fsw f31, 62 * REGBYTES(sp)
|
||
|
|
#endif
|
||
|
|
csrr a0, mcause
|
||
|
|
csrr a1, mepc
|
||
|
|
mv a2, sp
|
||
|
|
#ifndef __riscv_float_abi_soft
|
||
|
|
STORE a1, 63*REGBYTES(sp)
|
||
|
|
#else
|
||
|
|
STORE a1, 31*REGBYTES(sp)
|
||
|
|
#endif
|
||
|
|
call kl3_handle_trap
|
||
|
|
csrw mepc, a0
|
||
|
|
# Remain in M-mode after mret
|
||
|
|
li t0, MSTATUS_MPP
|
||
|
|
csrs mstatus, t0
|
||
|
|
LOAD x1, 0*REGBYTES(sp)
|
||
|
|
LOAD x2, 1*REGBYTES(sp)
|
||
|
|
LOAD x3, 2*REGBYTES(sp)
|
||
|
|
LOAD x4, 3*REGBYTES(sp)
|
||
|
|
LOAD x5, 4*REGBYTES(sp)
|
||
|
|
LOAD x6, 5*REGBYTES(sp)
|
||
|
|
LOAD x7, 6*REGBYTES(sp)
|
||
|
|
LOAD x8, 30*REGBYTES(sp)
|
||
|
|
LOAD x9, 8*REGBYTES(sp)
|
||
|
|
LOAD x10, 9*REGBYTES(sp)
|
||
|
|
LOAD x11, 10*REGBYTES(sp)
|
||
|
|
LOAD x12, 11*REGBYTES(sp)
|
||
|
|
LOAD x13, 12*REGBYTES(sp)
|
||
|
|
LOAD x14, 13*REGBYTES(sp)
|
||
|
|
LOAD x15, 14*REGBYTES(sp)
|
||
|
|
LOAD x16, 15*REGBYTES(sp)
|
||
|
|
LOAD x17, 16*REGBYTES(sp)
|
||
|
|
LOAD x18, 17*REGBYTES(sp)
|
||
|
|
LOAD x19, 18*REGBYTES(sp)
|
||
|
|
LOAD x20, 19*REGBYTES(sp)
|
||
|
|
LOAD x21, 20*REGBYTES(sp)
|
||
|
|
LOAD x22, 21*REGBYTES(sp)
|
||
|
|
LOAD x23, 22*REGBYTES(sp)
|
||
|
|
LOAD x24, 23*REGBYTES(sp)
|
||
|
|
LOAD x25, 24*REGBYTES(sp)
|
||
|
|
LOAD x26, 25*REGBYTES(sp)
|
||
|
|
LOAD x27, 26*REGBYTES(sp)
|
||
|
|
LOAD x28, 27*REGBYTES(sp)
|
||
|
|
LOAD x29, 28*REGBYTES(sp)
|
||
|
|
LOAD x30, 29*REGBYTES(sp)
|
||
|
|
LOAD x31, 7*REGBYTES(sp)
|
||
|
|
#ifndef __riscv_float_abi_soft
|
||
|
|
flw f0, 31 * REGBYTES(sp)
|
||
|
|
flw f1, 32 * REGBYTES(sp)
|
||
|
|
flw f2, 33 * REGBYTES(sp)
|
||
|
|
flw f3, 34 * REGBYTES(sp)
|
||
|
|
flw f4, 35 * REGBYTES(sp)
|
||
|
|
flw f5, 36 * REGBYTES(sp)
|
||
|
|
flw f6, 37 * REGBYTES(sp)
|
||
|
|
flw f7, 38 * REGBYTES(sp)
|
||
|
|
flw f8, 39 * REGBYTES(sp)
|
||
|
|
flw f9, 40 * REGBYTES(sp)
|
||
|
|
flw f10, 41 * REGBYTES(sp)
|
||
|
|
flw f11, 42 * REGBYTES(sp)
|
||
|
|
flw f12, 43 * REGBYTES(sp)
|
||
|
|
flw f13, 44 * REGBYTES(sp)
|
||
|
|
flw f14, 45 * REGBYTES(sp)
|
||
|
|
flw f15, 46 * REGBYTES(sp)
|
||
|
|
flw f16, 47 * REGBYTES(sp)
|
||
|
|
flw f17, 48 * REGBYTES(sp)
|
||
|
|
flw f18, 49 * REGBYTES(sp)
|
||
|
|
flw f19, 50 * REGBYTES(sp)
|
||
|
|
flw f20, 51 * REGBYTES(sp)
|
||
|
|
flw f21, 52 * REGBYTES(sp)
|
||
|
|
flw f22, 53 * REGBYTES(sp)
|
||
|
|
flw f23, 54 * REGBYTES(sp)
|
||
|
|
flw f24, 55 * REGBYTES(sp)
|
||
|
|
flw f25, 56 * REGBYTES(sp)
|
||
|
|
flw f26, 57 * REGBYTES(sp)
|
||
|
|
flw f27, 58 * REGBYTES(sp)
|
||
|
|
flw f28, 59 * REGBYTES(sp)
|
||
|
|
flw f29, 60 * REGBYTES(sp)
|
||
|
|
flw f30, 61 * REGBYTES(sp)
|
||
|
|
flw f31, 62 * REGBYTES(sp)
|
||
|
|
#endif
|
||
|
|
#ifndef __riscv_float_abi_soft
|
||
|
|
addi sp, sp, REGBYTES*64
|
||
|
|
#else
|
||
|
|
addi sp, sp, REGBYTES*32
|
||
|
|
#endif
|
||
|
|
csrr sp, mscratch
|
||
|
|
mret
|
||
|
|
|
||
|
|
.weak kl3_handle_trap
|
||
|
|
kl3_handle_trap:
|
||
|
|
1:
|
||
|
|
j 1b
|
||
|
|
#endif
|