Handle misaligned jal

rtl/serv_ctrl.v

@@ -11,6 +11,7 @@ module serv_ctrl
    input 	 i_trap,
    input 	 i_csr_pc,
    output 	 o_rd,
+   output 	 o_bad_pc,
    output [31:0] o_ibus_adr,
    output reg 	 o_ibus_cyc = 1'b0,
    input 	 i_ibus_ack);
@@ -31,6 +32,7 @@ module serv_ctrl
    assign plus_4        = en_2r & !en_3r;
 
    assign o_ibus_adr[0] = pc;
+   assign o_bad_pc = pc_plus_offset;
 
    ser_add ser_add_pc_plus_4
      (

rtl/serv_decode.v

@@ -72,6 +72,8 @@ module serv_decode
    wire      shift_op;
    wire      csr_op;
 
+   wire      jump_misaligned;
+
    reg       signbit;
 
    assign o_ibus_active = (state == IDLE);
@@ -95,10 +97,7 @@ module serv_decode
                        (opcode != OP_STORE) &
                        (opcode != OP_BRANCH);
 
-   assign o_rf_rs_en = cnt_en /*(running & (opcode == OP_OPIMM)) |
-                       (state == SH_INIT) |
-                       (state == MEM_INIT)*/;
- //FIXME: Change for addi?
+   assign o_rf_rs_en = cnt_en;
 
    assign o_alu_en = cnt_en;
 
@@ -182,6 +181,8 @@ module serv_decode
 
    assign o_mem_init = (state == MEM_INIT);
 
+   assign jal_misalign  = imm[21] & (opcode == OP_JAL);
+
    reg [4:0] opcode;
    reg [31:0] imm;
 
@@ -194,7 +195,6 @@ module serv_decode
          signbit       <= i_wb_rdt[30];
          opcode        <= i_wb_rdt[6:2];
          imm           <= i_wb_rdt;
-
       end
    end
 
@@ -277,6 +277,8 @@ module serv_decode
    always @(posedge clk) begin
       if (cnt_done)
 	o_ctrl_trap <= i_mem_misalign;
+      if (go)
+	o_ctrl_trap <= jal_misalign;
       state <= state;
       case (state)
         IDLE : begin

rtl/serv_mem_if.v

@@ -16,7 +16,7 @@ module serv_mem_if
    input         i_trap,
    //External interface
    output [31:0] o_wb_adr,
-   output [31:0] o_wb_dat,
+   output reg [31:0] o_wb_dat = 32'd0,
    output [3:0]  o_wb_sel,
    output 	 o_wb_we ,
    output reg 	 o_wb_cyc = 1'b0,
@@ -63,7 +63,6 @@ module serv_mem_if
 
    wire       upper_half = bytepos[1];
 
-   assign o_wb_dat = dat;
    assign o_wb_sel = (is_word ? 4'b1111 :
                       is_half ? {{2{upper_half}}, ~{2{upper_half}}} :
                       4'd1 << bytepos);
@@ -85,6 +84,15 @@ module serv_mem_if
    reg [1:0]  misalign = 2'b00;
 
    always @(posedge i_clk) begin
+      //Async?
+      if (init_r) begin
+	 o_wb_dat[7:0]   <= dat[7:0];
+	 o_wb_dat[15:8]  <= (is_word | is_half) ? dat[15:8] : dat[7:0];
+	 o_wb_dat[23:16] <= is_word ? dat[23:16] : dat[7:0];
+	 o_wb_dat[31:24] <= is_word ? dat[31:24] : is_half ? dat[15:8] : dat[7:0];
+      end
+
+
       if (i_init & !init_r)
 	misalign[0] <= (!is_byte & adr);
       if (init_r & !init_2r)

rtl/serv_top.v

@@ -101,6 +101,8 @@ module serv_top
    wire          mem_busy;
    wire 	 mem_misalign;
 
+   wire 	 bad_pc;
+
    wire 	 csr_en;
    wire [2:0] 	 csr_sel;
    wire [1:0]	 csr_source;
@@ -164,6 +166,7 @@ module serv_top
       .i_trap     (trap | mret),
       .i_csr_pc   (csr_rd),
       .o_rd       (ctrl_rd),
+      .o_bad_pc   (bad_pc),
       .o_ibus_adr (o_ibus_adr),
       .o_ibus_cyc (o_ibus_cyc),
       .i_ibus_ack (i_ibus_ack));
@@ -245,7 +248,7 @@ module serv_top
       .i_csr_source (csr_source),
       .i_trap       (trap),
       .i_pc         (o_ibus_adr[0]),
-      .i_mtval      (o_dbus_adr[0]),
+      .i_mtval      (mem_misalign ? o_dbus_adr[0] : bad_pc),
       .i_load_misaligned (mem_misalign & !mem_cmd),
       .i_store_misaligned (mem_misalign & mem_cmd),
       .i_d          (rs1/* FIXME csr_d*/),