diff --git a/rtl/ibex_cs_registers.sv b/rtl/ibex_cs_registers.sv
index 06a46565d1..549a4b1337 100644
--- a/rtl/ibex_cs_registers.sv
+++ b/rtl/ibex_cs_registers.sv
@@ -575,10 +575,10 @@ module ibex_cs_registers #(
     mtval_en     = 1'b0;
     mtval_d      = csr_wdata_int;
     mtvec_en     = csr_mtvec_init_i;
-    // mtvec.MODE set to vectored
+    // mtvec.MODE default set to vectored,but can change to direct
     // mtvec.BASE must be 256-byte aligned
     mtvec_d      = csr_mtvec_init_i ? {boot_addr_i[31:8], 6'b0, 2'b01} :
-                                      {csr_wdata_int[31:8], 6'b0, 2'b01};
+                                      {csr_wdata_int[31:8], 6'b0, csr_wdata_int[1:0]};
     dcsr_en      = 1'b0;
     dcsr_d       = dcsr_q;
     depc_d       = {csr_wdata_int[31:1], 1'b0};
@@ -634,7 +634,13 @@ module ibex_cs_registers #(
         CSR_MTVAL: mtval_en = 1'b1;
 
         // mtvec
-        CSR_MTVEC: mtvec_en = 1'b1;
+        CSR_MTVEC: begin
+          mtvec_en = 1'b1;
+          // Change to direct MODE if software writes an unsupported value
+          if ((mtvec_d[1:0] != 2'b00) && (mtvec_d[1:0] != 2'b01)) begin
+            mtvec_d[1:0] = 2'b00;
+          end
+        end
 
         CSR_DCSR: begin
           dcsr_d = csr_wdata_int;
diff --git a/rtl/ibex_if_stage.sv b/rtl/ibex_if_stage.sv
index b5650cac6b..3785ad898f 100644
--- a/rtl/ibex_if_stage.sv
+++ b/rtl/ibex_if_stage.sv
@@ -172,21 +172,23 @@ module ibex_if_stage import ibex_pkg::*; #(
   ibex_pkg::pc_sel_e pc_mux_internal;
 
   logic        [7:0] unused_boot_addr;
-  logic        [7:0] unused_csr_mtvec;
+  logic        [5:0] unused_csr_mtvec;
   logic              unused_exc_cause;
 
   assign unused_boot_addr = boot_addr_i[7:0];
-  assign unused_csr_mtvec = csr_mtvec_i[7:0];
+  assign unused_csr_mtvec = csr_mtvec_i[7:2];
 
   assign unused_exc_cause = |{exc_cause.irq_ext, exc_cause.irq_int};
 
   // exception PC selection mux
   always_comb begin : exc_pc_mux
-    irq_vec = exc_cause.lower_cause;
-
-    if (exc_cause.irq_int) begin
+    // mtvec is vectored mode
+    if (csr_mtvec_i[1:0] == 2'b01) begin
       // All internal interrupts go to the NMI vector
-      irq_vec = ExcCauseIrqNm.lower_cause;
+      irq_vec = exc_cause.irq_int ? ExcCauseIrqNm.lower_cause : exc_cause.lower_cause;
+    end else begin
+      // mtvec is direct mode, so irq_vec is always 0
+      irq_vec = 5'b00000;
     end
 
     unique case (exc_pc_mux_i)