diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c
index 0e7b5d1d..cd974cc1 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);