diff --git a/dv/uvm/core_ibex/tests/core_ibex_test_lib.sv b/dv/uvm/core_ibex/tests/core_ibex_test_lib.sv
index 487b711b..f0cb12fd 100644
--- a/dv/uvm/core_ibex/tests/core_ibex_test_lib.sv
+++ b/dv/uvm/core_ibex/tests/core_ibex_test_lib.sv
@@ -901,6 +901,7 @@ class core_ibex_nested_irq_test extends core_ibex_directed_test;
     bit valid_irq;
     bit valid_nested_irq;
     int unsigned initial_irq_delay;
+    vseq.irq_raise_seq_h.max_delay = 5000;
     forever begin
       send_irq_stimulus_start(1'b1, 1'b0, valid_irq);
       if (valid_irq) begin
diff --git a/vendor/google_riscv-dv/src/riscv_instr_pkg.sv b/vendor/google_riscv-dv/src/riscv_instr_pkg.sv
index b21edc9f..ffef4593 100644
--- a/vendor/google_riscv-dv/src/riscv_instr_pkg.sv
+++ b/vendor/google_riscv-dv/src/riscv_instr_pkg.sv
@@ -1379,10 +1379,10 @@ package riscv_instr_pkg;
                                                    ref string instr[$]);
     string store_instr = (XLEN == 32) ? "sw" : "sd";
     if (scratch inside {implemented_csr}) begin
-      // Use kernal stack for handling exceptions
-      // Save the user mode stack pointer to the scratch register
-      instr.push_back($sformatf("csrrw x%0d, 0x%0x, x%0d", sp, scratch, sp));
-      // Move TP to SP
+      // Push USP from gpr.SP onto the kernel-mode stack
+      instr.push_back($sformatf("addi x%0d, x%0d, -4", tp, tp));
+      instr.push_back($sformatf("%0s  x%0d, (x%0d)", store_instr, sp, tp));
+      // Move KSP to gpr.SP
       instr.push_back($sformatf("add x%0d, x%0d, zero", sp, tp));
     end
     // If MPRV is set and MPP is S/U mode, it means the address translation and memory protection
@@ -1404,11 +1404,13 @@ package riscv_instr_pkg;
       end
     end
     // Reserve space from kernel stack to save all 32 GPR except for x0
-    instr.push_back($sformatf("1: addi x%0d, x%0d, -%0d", sp, sp, 31 * (XLEN/8)));
+    instr.push_back($sformatf("1: addi x%0d, x%0d, -%0d", sp, sp, 32 * (XLEN/8)));
     // Push all GPRs to kernel stack
     for(int i = 1; i < 32; i++) begin
       instr.push_back($sformatf("%0s  x%0d, %0d(x%0d)", store_instr, i, i * (XLEN/8), sp));
     end
+    // Move KSP back to gpr.tp (this is needed if we again take an interrupt before restoring our USP)
+    instr.push_back($sformatf("add x%0d, x%0d, zero", tp, sp));
   endfunction
 
   // Pop general purpose register from stack, this is needed before returning to user program
@@ -1419,17 +1421,19 @@ package riscv_instr_pkg;
                                                     riscv_reg_t tp,
                                                     ref string instr[$]);
     string load_instr = (XLEN == 32) ? "lw" : "ld";
-    // Pop user mode GPRs from kernel stack
+    // Move KSP to gpr.SP
+    instr.push_back($sformatf("add x%0d, x%0d, zero", sp, tp));
+    // Pop GPRs from kernel stack
     for(int i = 1; i < 32; i++) begin
       instr.push_back($sformatf("%0s  x%0d, %0d(x%0d)", load_instr, i, i * (XLEN/8), sp));
     end
-    // Restore kernel stack pointer
-    instr.push_back($sformatf("addi x%0d, x%0d, %0d", sp, sp, 31 * (XLEN/8)));
+    instr.push_back($sformatf("addi x%0d, x%0d, %0d", sp, sp, 32 * (XLEN/8)));
     if (scratch inside {implemented_csr}) begin
-      // Move SP to TP
+      // Move KSP back to gpr.TP
       instr.push_back($sformatf("add x%0d, x%0d, zero", tp, sp));
-      // Restore user mode stack pointer
-      instr.push_back($sformatf("csrrw x%0d, 0x%0x, x%0d", sp, scratch, sp));
+      // Pop USP from the kernel stack, move back to gpr.sp
+      instr.push_back($sformatf("%0s  x%0d, (x%0d)", load_instr, sp, tp));
+      instr.push_back($sformatf("addi x%0d, x%0d, 4", tp, tp));
     end
   endfunction