diff options
| -rw-r--r-- | lib/sbi/sbi_hart.c | 15 |
1 files changed, 11 insertions, 4 deletions
diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c index 0e7b5d1..cd974cc 100644 --- a/lib/sbi/sbi_hart.c +++ b/lib/sbi/sbi_hart.c @@ -1046,10 +1046,17 @@ sbi_hart_switch_mode(unsigned long arg0, unsigned long arg1, csr_write(CSR_MEPC, next_addr); if (next_mode == PRV_S) { - csr_write(CSR_STVEC, next_addr); - csr_write(CSR_SSCRATCH, 0); - csr_write(CSR_SIE, 0); - csr_write(CSR_SATP, 0); + if (next_virt) { + csr_write(CSR_VSTVEC, next_addr); + csr_write(CSR_VSSCRATCH, 0); + csr_write(CSR_VSIE, 0); + csr_write(CSR_VSATP, 0); + } else { + csr_write(CSR_STVEC, next_addr); + csr_write(CSR_SSCRATCH, 0); + csr_write(CSR_SIE, 0); + csr_write(CSR_SATP, 0); + } } else if (next_mode == PRV_U) { if (misa_extension('N')) { csr_write(CSR_UTVEC, next_addr); |