csr, verilator, traps

rtl/serv_ctrl.v

@@ -1,35 +1,37 @@
 `default_nettype none
 module serv_ctrl
   (
-   input         clk,
-   input         i_en,
-   input         i_jump,
-   input         i_offset,
-   input         i_rs1,
-   input         i_jalr,
-   input         i_auipc,
-   output        o_rd,
+   input 	 clk,
+   input 	 i_en,
+   input 	 i_jump,
+   input 	 i_offset,
+   input 	 i_rs1,
+   input 	 i_jalr,
+   input 	 i_auipc,
+   input 	 i_trap,
+   input 	 i_csr_pc,
+   output 	 o_rd,
    output [31:0] o_ibus_adr,
-   output reg    o_ibus_cyc = 1'b0,
-   input         i_ibus_ack);
+   output reg 	 o_ibus_cyc = 1'b0,
+   input 	 i_ibus_ack);
 
    parameter RESET_PC = 32'd8;
-   
+
    wire       pc_plus_4;
    wire       pc_plus_offset;
-   
+
    wire       plus_4;
 
    wire       pc;
-   
+
    wire       new_pc;
 
    wire       offset_a;
-   
+
    assign plus_4        = en_2r & !en_3r;
 
    assign o_ibus_adr[0] = pc;
-   
+
    ser_add ser_add_pc_plus_4
      (
       .clk (clk),
@@ -52,11 +54,11 @@ module serv_ctrl
       .o_par (o_ibus_adr[31:1])
       );
 
-   assign new_pc = i_jump ? pc_plus_offset : pc_plus_4;
-   assign o_rd  = i_auipc ? pc_plus_offset : pc_plus_4;
+   assign new_pc = i_trap ? i_csr_pc : i_jump ? pc_plus_offset_aligned : pc_plus_4;
+   assign o_rd  = i_auipc ? pc_plus_offset_aligned : pc_plus_4;
 
    assign offset_a = i_jalr ? i_rs1 : pc;
-   
+
    ser_add ser_add_pc_plus_offset
      (
       .clk (clk),
@@ -66,10 +68,13 @@ module serv_ctrl
       .q   (pc_plus_offset),
       .o_v ());
 
+   wire       pc_plus_offset_aligned = pc_plus_offset & en_r;
+
+
    reg        en_r  = 1'b1;
    reg        en_2r = 1'b0;
    reg        en_3r = 1'b0;
-   
+
    always @(posedge clk) begin
       en_r <= i_en;
       en_2r <= en_r;
@@ -80,6 +85,5 @@ module serv_ctrl
       else if (o_ibus_cyc & i_ibus_ack)
         o_ibus_cyc <= 1'b0;
    end
-   
+
 endmodule
-