working on exceptions

7dc634dc · Tomasz Wlostowski · b57333c6 · 7dc634dc · 7dc634dc · 7dc634dc
Commit 7dc634dc authored Aug 10, 2015 by Tomasz Wlostowski
25 changed files
--- a/rtl/Manifest.py
+++ b/rtl/Manifest.py
@@ -13,4 +13,7 @@ files = [ "rv_cpu.v",
          "rv_regfile.v",
          "rv_writeback.v",
          "rv_shifter.v",
-          "rv_multiply.v"];
+          "rv_multiply.v",
+	  "rv_csr.v",
+          "rv_timer.v",
+          "rv_exceptions.v"];
--- a/rtl/rv_cpu.v
+++ b/rtl/rv_cpu.v
@@ -27,6 +27,8 @@ module rv_cpu
   input 	 clk_i,
   input 	 rst_i,
+   input 	 irq_i,
   // instruction mem I/F
   output [31:0] im_addr_o,
   input [31:0]  im_data_i,
@@ -51,7 +53,7 @@ module rv_cpu
   wire 	 x_kill;
   wire 	 f_kill;
-   wire [31:0] 	 f2d_pc, f2d_pc_plus_4, f2d_ir;
+   wire [31:0] 	 f2d_pc, f2d_ir;
   wire 	 f2d_ir_valid;
   wire [31:0] 	 x2f_pc_bra;
   wire 	 x2f_bra;
@@ -77,13 +79,17 @@ module rv_cpu
   wire d2x_is_signed_alu_op;
   wire d2x_is_add_o;
   wire d2x_is_shift_o;
-   wire [1:0] d2x_rd_source;
+   wire [2:0] d2x_rd_source;
   wire       d2x_rd_write;
+   wire [11:0] d2x_csr_sel;
+   wire [4:0]  d2x_csr_imm;
+   wire        d2x_is_csr, d2x_is_eret, d2x_csr_load_en;
   wire 	 d2x_load_hazard;
   wire 	 d_stall, d_kill;
+   wire [39:0] 	 csr_time, csr_cycles;
   rv_fetch fetch
     (
@@ -142,7 +148,14 @@ module rv_cpu
      .x_is_add_o(d2x_is_add),
      .x_is_shift_o(d2x_is_shift),
      .x_rd_source_o(d2x_rd_source),
-      .x_rd_write_o(d2x_rd_write)
+      .x_rd_write_o(d2x_rd_write),
+      .x_csr_sel_o ( d2x_csr_sel),
+      .x_csr_imm_o ( d2x_csr_imm),
+      .x_is_csr_o ( d2x_is_csr ),
+      .x_is_eret_o ( d2x_is_eret )
      );
      wire [4:0] 	 x2w_rd;
@@ -196,6 +209,8 @@ module rv_cpu
   wire 	 w_stall_req;
   rv_exec execute
     (
@@ -204,11 +219,19 @@ module rv_cpu
      .x_stall_i(x_stall),
      .x_kill_i(x_kill),
+      .irq_i ( irq_i ),
      .x_stall_req_o(x_stall_req),
      .w_stall_req_i(w_stall_req),
      .d_valid_i(d2x_valid),
+      .d_is_csr_i ( d2x_is_csr ),
+      .d_is_eret_i ( d2x_is_eret ),
+      .d_csr_imm_i ( d2x_csr_imm ),
+      .d_csr_sel_i (d2x_csr_sel),
      .d_load_hazard_i(d2x_load_hazard),
      .d_pc_i(d2x_pc),
      .d_rd_i(d2x_rd),
@@ -248,7 +271,12 @@ module rv_cpu
      .dm_data_select_o(dm_data_select_o),
      .dm_store_o(dm_store_o),
      .dm_load_o(dm_load_o),
-      .dm_ready_i(dm_ready_i)
+      .dm_ready_i(dm_ready_i),
+      .csr_time_i (csr_time),
+      .csr_cycles_i (csr_cycles),
+      .timer_tick_i (sys_tick)
   );
@@ -281,6 +309,17 @@ module rv_cpu
      .rf_rd_write_o(rf_rd_write)
   );
+   rv_timer ctimer (
+		    .clk_i(clk_i),
+		    .rst_i(rst_i),
+		    .csr_time_o(csr_time),
+		    .csr_cycles_o(csr_cycles),
+		    .sys_tick_o(sys_tick)
+		    );
   reg 		 x2f_bra_d0, x2f_bra_d1;
   always@(posedge clk_i)

--- a/rtl/rv_csr.v
+++ b/rtl/rv_csr.v
+/*
+ uRV - a tiny and dumb RISC-V core
+ Copyright (c) 2015 twl <twlostow@printf.cc>.
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of the GNU Lesser General Public
+ License as published by the Free Software Foundation; either
+ version 3.0 of the License, or (at your option) any later version.
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ Lesser General Public License for more details.
+ You should have received a copy of the GNU Lesser General Public
+ License along with this library.
+*/
+`include "rv_defs.v"
+`timescale 1ns/1ps
 module rv_csr
  (
   input 	     clk_i,
   input 	     rst_i,
   input 	     x_stall_i,
+   input 	     x_kill_i,
-   input 	     d_csr_write_rd_i,
+   input 	     d_is_csr_i,
+   input [2:0] 	     d_fun_i,
+   input [4:0] 	     d_csr_imm_i,
+   input [11:0]      d_csr_sel_i,
   input [31:0]      d_rs1_i,
-   input [31:0]      d_pc_i, // for exception strage
   output reg [31:0] x_rd_o,
-   output reg 	     x_rd_write_o,
-   input 	     x_exception_i,
+   input [39:0]      csr_time_i,
-   input 	     x_exception_irq_i,
+   input [39:0]      csr_cycles_i,
-   input [2:0] 	     x_exception_id_i
+   // interrupt management
+   output [31:0]     x_csr_write_value_o,
+   input [31:0]      csr_mstatus_i,
+   input [31:0]      csr_mip_i,
+   input [31:0]      csr_mie_i,
+   input [31:0]      csr_mepc_i,
+   input [31:0]      csr_mcause_i
-   output [31:0]     x_exception_pc_o
   );
-   reg [31:0] 	csr_mepc;
+   reg [31:0] 	csr_mscratch = 0; 
-   reg [31:0] 	csr_mscratch;
-   reg [31:0] 	csr_mcause;
-   reg [31:0] 	csr_mstatus;
-   wire 	csr_mie;
+   reg [31:0] 	csr_in1;
+   reg [31:0] 	csr_in2;
+   reg [31:0] 	csr_out;
+   always@*
+     case(d_csr_sel_i)
+       `CSR_ID_CYCLESL: csr_in1 <= csr_cycles_i[31:0];
+       `CSR_ID_CYCLESH: csr_in1 <= { 24'h0, csr_cycles_i[39:32] };
+       `CSR_ID_TIMEL: csr_in1 <= csr_time_i[31:0];
+       `CSR_ID_TIMEH: csr_in1 <= { 24'h0, csr_time_i[39:32] };
+       `CSR_ID_MSCRATCH: csr_in1 <= csr_mscratch;
+       `CSR_ID_MEPC: csr_in1 <= csr_mepc_i;
+       `CSR_ID_MSTATUS: csr_in1 <= csr_mstatus_i;
+       `CSR_ID_MCAUSE: csr_in1 <= csr_mcause_i;
+       `CSR_ID_MIP: csr_in1 <= csr_mip_i;
+       `CSR_ID_MIE: csr_in1 <= csr_mie_i;
+       default: csr_in1 <= 32'hx;
+     endcase // case (d_csr_sel_i)
+   always@*
+     x_rd_o <= csr_in1;
+   genvar 	i;
-   assign x_exception_pc_o = csr_mepc;
+   always@*
+     case (d_fun_i)
+       `CSR_OP_CSRRWI,
+       `CSR_OP_CSRRSI,
+       `CSR_OP_CSRRCI:
+	 csr_in2 <= {27'b0, d_csr_imm_i };
+       default:
+	 csr_in2 <= d_rs1_i;
+     endcase // case (d_fun_i)
+   generate
+      for (i=0;i<32;i=i+1) 
+	begin
+	   always@*
+	     case(d_fun_i)
+	       `CSR_OP_CSRRWI, 
+	       `CSR_OP_CSRRW:
+		 csr_out[i] <= csr_in2[i];
+	       `CSR_OP_CSRRCI,
+	       `CSR_OP_CSRRC:
+		 csr_out[i] <= csr_in2[i] ? 1'b0 : csr_in1[i];
+	       `CSR_OP_CSRRSI,
+	       `CSR_OP_CSRRS:
+		 csr_out[i] <= csr_in2[i] ? 1'b1 : csr_in1[i];
+	       default:
+		 csr_out[i] <= 32'hx;
+	     endcase // case (d_csr_op_i)
+	end // for (i=0;i<32;i=i+1)
+      endgenerate
+   always@(posedge clk_i)
+     if(rst_i) 
+       csr_mscratch <= 0;
+     else if(!x_stall_i && !x_kill_i && d_is_csr_i) 
+       case (d_csr_sel_i)
+	 `CSR_ID_MSCRATCH: 
+	   csr_mscratch <= csr_out;
+       endcase // case (d_csr_sel_i)
+   assign x_csr_write_value_o = csr_out;
+endmodule
--- a/rtl/rv_defs.v
+++ b/rtl/rv_defs.v
@@ -27,6 +27,7 @@
 `define OPC_BRANCH 5'b11000
 `define OPC_LOAD 5'b00000
 `define OPC_STORE 5'b01000
+`define OPC_SYSTEM 5'b11100
 `define BRA_EQ 3'b000
 `define BRA_NEQ  3'b001
@@ -55,7 +56,38 @@
 `define FUNC_MULHSU 3'b010
 `define FUNC_MULHU 3'b011
-`define RD_SOURCE_ALU 2'b00 
+`define RD_SOURCE_ALU 3'b000 
-`define RD_SOURCE_SHIFTER 2'b10
+`define RD_SOURCE_SHIFTER 3'b010
-`define RD_SOURCE_MULTIPLY 2'b01
+`define RD_SOURCE_MULTIPLY 3'b001
-`define RD_SOURCE_DIVIDE 2'b11
+`define RD_SOURCE_DIVIDE 3'b011
+`define RD_SOURCE_CSR 3'b011
+`define CSR_ID_CYCLESH 12'hc80
+`define CSR_ID_CYCLESL 12'hc00 
+`define CSR_ID_TIMEH 12'hc81
+`define CSR_ID_TIMEL 12'hc01 
+`define CSR_ID_MSCRATCH 12'h340
+`define CSR_ID_MEPC 12'h341
+`define CSR_ID_MSTATUS 12'h300
+`define CSR_ID_MCAUSE 12'h342
+`define CSR_ID_MIP 12'h344
+`define CSR_ID_MIE 12'h304
+`define CSR_OP_CSRRW 3'b001
+`define CSR_OP_CSRRS 3'b010
+`define CSR_OP_CSRRC 3'b011
+`define CSR_OP_CSRRWI 3'b101
+`define CSR_OP_CSRRSI 3'b110
+`define CSR_OP_CSRRCI 3'b111
+`define URV_RESET_VECTOR 32'h00000000
+`define URV_TRAP_VECTOR 32'h00000040
+`define EXCEPT_ILLEGAL_INSN 2
+`define EXCEPT_BREAKPOINT 3
+`define EXCEPT_UNALIGNED_LOAD 4
+`define EXCEPT_UNALIGNED_STORE 6
+`define EXCEPT_TIMER 9
+`define EXCEPT_IRQ 10
--- a/rtl/rv_exec.v
+++ b/rtl/rv_exec.v
@@ -41,6 +41,7 @@ module rv_exec
   input [31:0]      rf_rs1_value_i,
   input [31:0]      rf_rs2_value_i,
   input 	     d_valid_i,
   input 	     d_load_hazard_i,
@@ -48,18 +49,25 @@ module rv_exec
   input [4:0] 	     d_opcode_i,
   input 	     d_shifter_sign_i,
+   input 	     d_is_csr_i,
+   input 	     d_is_eret_i,
+   input [4:0] 	     d_csr_imm_i,
+   input [11:0]      d_csr_sel_i,
   input [31:0]      d_imm_i,
   input 	     d_is_signed_compare_i,
   input 	     d_is_signed_alu_op_i,
   input 	     d_is_add_i,
   input 	     d_is_shift_i,
-   input [1:0] 	     d_rd_source_i,
+   input [2:0] 	     d_rd_source_i,
   input 	     d_rd_write_i,
   output reg [31:0] f_branch_target_o,
   output 	     f_branch_take_o,
   output 	     w_load_hazard_o,
+   input 	     irq_i,
   // Writeback stage I/F
   output reg [2:0 ] w_fun_o,
@@ -77,9 +85,16 @@ module rv_exec
   output [3:0]      dm_data_select_o,
   output 	     dm_store_o,
   output 	     dm_load_o,
-   input 	     dm_ready_i
+   input 	     dm_ready_i,
+   input [39:0]      csr_time_i,
+   input [39:0]      csr_cycles_i,
+   input 	     timer_tick_i
   );
+   parameter g_exception_vector = 'h40;
   wire [31:0] 	 rs1, rs2;
   assign rs1 = rf_rs1_value_i;
@@ -87,8 +102,7 @@ module rv_exec
   reg [31:0] 	 alu_op1, alu_op2, alu_result;
   reg [31:0] 	 rd_value;
   reg 		 branch_take;
   reg 		 branch_condition_met;
@@ -106,6 +120,85 @@ module rv_exec
   reg 	       f_branch_take;
+   wire        x_stall_req_shifter;
+   wire        x_stall_req_multiply = 0;
+   wire        x_stall_req_divide   = 0;
+   wire [31:0] rd_shifter;
+   wire [31:0] rd_csr;
+   wire [31:0] rd_mul;
+   wire [31:0] rd_div;
+   wire        exception;
+   wire [31:0] csr_mie, csr_mip, csr_mepc, csr_mstatus,csr_mcause;
+   wire [31:0] csr_write_value;
+   wire [31:0] exception_address;
+   rv_csr csr_regs
+     (
+      .clk_i(clk_i),
+      .rst_i(rst_i),
+      .x_stall_i(x_stall_i),
+      .x_kill_i(x_kill_i),
+      .d_is_csr_i(d_is_csr_i),
+      .d_fun_i(d_fun_i),
+      .d_csr_imm_i(d_csr_imm_i),
+      .d_csr_sel_i (d_csr_sel_i),
+      .d_rs1_i(rs1),
+      .x_rd_o(rd_csr),
+      .x_csr_write_value_o(csr_write_value),
+      .csr_time_i(csr_time_i),
+      .csr_cycles_i(csr_cycles_i),
+      .csr_mstatus_i(csr_mstatus),
+      .csr_mip_i(csr_mip),
+      .csr_mie_i(csr_mie),
+      .csr_mepc_i(csr_mepc),
+      .csr_mcause_i(csr_mmause)
+      );
+   rv_exceptions exception_unit 
+     (
+      .clk_i(clk_i),
+      .rst_i(rst_i),
+      .x_stall_i (x_stall_i),
+      .x_kill_i (x_kill_i),
+      .d_is_csr_i(d_is_csr_i),
+      .d_is_eret_i (d_is_eret_i),
+      .d_fun_i(d_fun_i),
+      .d_csr_imm_i(d_csr_imm_i),
+      .d_csr_sel_i(d_csr_sel_i),
+      .x_csr_write_value_i(csr_write_value),
+      .exp_irq_i(irq_i),
+      .exp_tick_i(timer_tick_i),
+      .exp_breakpoint_i(1'b0),
+      .exp_unaligned_load_i(1'b0),
+      .exp_unaligned_store_i(1'b0),
+      .exp_invalid_insn_i(1'b0),
+      .x_exception_o(exception),
+      .x_exception_pc_i(d_pc_i),
+      .x_exception_pc_o(exception_address),
+      .csr_mstatus_o(csr_mstatus),
+      .csr_mip_o(csr_mip),
+      .csr_mie_o(csr_mie),
+      .csr_mepc_o(csr_mepc),
+      .csr_mcause_o(csr_mcause)
+      );
   // branch condition decoding   
   always@*
@@ -120,13 +213,16 @@ module rv_exec
     endcase // case (d_fun_i)
   always@*
-     case (d_opcode_i)
+     if(d_is_eret_i )
-       `OPC_JAL: branch_target <= d_pc_i + d_imm_i;
+       branch_target <= exception_address;
-       `OPC_JALR: branch_target <= rs1 + d_imm_i;
+     else if ( exception )
-       `OPC_BRANCH: branch_target <= d_pc_i + d_imm_i;
+       branch_target <= g_exception_vector;
+     else case (d_opcode_i)
-       default: branch_target<= 32'hx;
+	    `OPC_JAL: branch_target <= d_pc_i + d_imm_i;
-     endcase // case (d_opcode_i)
+	    `OPC_JALR: branch_target <= rs1 + d_imm_i;
+	    `OPC_BRANCH: branch_target <= d_pc_i + d_imm_i;
+	    default: branch_target<= 32'hx;
+	  endcase // case (d_opcode_i)
   // decode ALU operands
   always@*
@@ -185,12 +281,6 @@ module rv_exec
 	endcase // case (d_fun_i)
     end // always@ *
-   wire x_stall_req_shifter;
-   wire x_stall_req_multiply = 0;
-   wire x_stall_req_divide   = 0;
-   wire [31:0] rd_shifter;
   rv_shifter shifter 
     (
@@ -215,6 +305,7 @@ module rv_exec
     case (d_rd_source_i)
       `RD_SOURCE_ALU: rd_value <= alu_result;
       `RD_SOURCE_SHIFTER : rd_value <= rd_shifter;
+       `RD_SOURCE_CSR: rd_value <= rd_csr;
       default: rd_value <= 32'hx;
     endcase // case (x_rd_source_i)
@@ -266,14 +357,17 @@ module rv_exec
   //branch decision
   always@*
-     case (d_opcode_i)
+     if( exception || d_is_eret_i)
-       `OPC_JAL, `OPC_JALR: 
+       branch_take <= 1;
-	 branch_take <= 1;
+     else
-       `OPC_BRANCH:
+       case (d_opcode_i)
-	 branch_take <= branch_condition_met;
+	 `OPC_JAL, `OPC_JALR: 
-       default: 
+	   branch_take <= 1;
-	 branch_take <= 0;
+	 `OPC_BRANCH:
-     endcase // case (d_opcode_i)
+	   branch_take <= branch_condition_met;
+	 default: 
+	   branch_take <= 0;
+       endcase // case (d_opcode_i)
   // generate load/store requests
@@ -287,6 +381,7 @@ module rv_exec
   assign dm_load_o =  is_load;
   assign dm_store_o = is_store;
   always@(posedge clk_i) 
      if (rst_i) begin
@@ -308,12 +403,11 @@ module rv_exec
 //	 if(!shifter_stall_req)
 	 w_rd_value_o <= rd_value;
-	 w_rd_write_o <= d_rd_write_i && !x_kill_i && d_valid_i;
+	 w_rd_write_o <= d_rd_write_i && !x_kill_i && d_valid_i && !exception;
 	 w_fun_o <= d_fun_i;
-   w_load_o <= is_load;
+	 w_load_o <= is_load && !exception;
+	 w_store_o <= is_store && !exception;
-	 w_store_o <= is_store;
 	 w_dm_addr_o <= dm_addr;

--- a/rtl/rv_fetch.v
+++ b/rtl/rv_fetch.v
@@ -51,13 +51,12 @@ module rv_fetch
   always@*
     if( x_bra_i )
       pc_next <= x_pc_bra_i;
-     else if (f_stall_i || !im_valid_i)
+     else if (!rst_d || f_stall_i || !im_valid_i)
       pc_next <= pc;
     else
       pc_next <= pc + 4;
   assign f_ir_o = ir;
   assign im_addr_o = pc_next;
@@ -78,7 +77,7 @@ module rv_fetch
 	   f_pc_o <= pc;
 	   if(im_valid_i) begin
-	      ir <= im_data_i; // emit nop
+	      ir <= im_data_i; 
 	      f_valid_o <= (rst_d && !f_kill_i);
 	   end else begin// if (i_valid_i)
 	      f_valid_o <= 0;

--- a/rtl/rv_multiply.v
+++ b/rtl/rv_multiply.v
-/*
 `include "rv_defs.v"
 `timescale 1ns/1ps
@@ -8,60 +7,48 @@ module rv_multiply
   input 	 clk_i,
   input 	 rst_i,
   input 	 x_stall_i,
+   input 	 w_stall_req_i,
+   input 	 d_valid_i,
+   input 	 d_is_mul_i,
+   input [31:0]  d_rs1_i,
-   input [31:0]  x_rs1_i,
+   input [31:0]  d_rs2_i,
-   input [31:0]  x_rs2_i,
-   input [4:0] 	 x_opcode_i,
+   input [4:0] 	 d_opcode_i,
-   input [2:0] 	 x_fun_i,
+   input [2:0] 	 d_fun_i,
-   output [31:0] x_rd_o,
+   output reg [31:0] x_rd_o,
-   output 	 x_rd_valid_o, 
   output 	 x_stall_req_o
   );
   parameter g_latency = 2;
-   wire 	 sign_a = ( x_fun_i == `FUNC_MUL || x_fun_i == `FUNC_MULHSU ) ? x_rs1_i[31] : 1'b0;
+   wire 	 sign_a = ( d_fun_i == `FUNC_MUL || d_fun_i == `FUNC_MULHSU ) ? d_rs1_i[31] : 1'b0;
-   wire 	 sign_b = ( x_fun_i == `FUNC_MUL ) ? x_rs2_i[31] : 1'b0;
+   wire 	 sign_b = ( d_fun_i == `FUNC_MUL ) ? d_rs2_i[31] : 1'b0;
-   wire [32:0] 	 a = { sign_a, x_rs1_i };
+   wire [32:0] 	 a = { sign_a, d_rs1_i };
-   wire [32:0] 	 b = { sign_b, x_rs2_i };
+   wire [32:0] 	 b = { sign_b, d_rs2_i };
-   reg [65:0] 	 stage0;
-   reg [3:0] 	 pipe;
+   reg [65:0] 	 stage0, stage1;
-   always@(posedge clk_i)
-     if(rst_i)
-       pipe <= 0;
-     else if (x_opcode_i = `OPC_MUL )
-       pipe <= (1<<g_latency);
-     else
-       pipe <= {1'b0, pipe [3:1]};
   always@(posedge clk_i)
-     begin
+     if(!x_stall_i)
-	stage0 <= $signed(a) * $signed(b);
+       begin
-	if( d_fun_i != `FUNC_MUL )
+	  stage0 <= $signed(a) * $signed(b);
-   	  x_rd_o <= stage0[63:32];
+	  stage1 <= stage0;
-	else
+       end
-	  x_rd_o <= stage0[31:0];
-     end
+   always@*
+     if( d_fun_i != `FUNC_MUL )
+       x_rd_o <= stage1[63:32];
+     else
+       x_rd_o <= stage1[31:0];
 endmodule // rv_multiply
-*/
--- a/rtl/rv_predecode.v
+++ b/rtl/rv_predecode.v
@@ -60,9 +60,14 @@ module rv_decode
 output reg 	   x_is_signed_alu_op_o,
 output reg 	   x_is_add_o,
 output reg 	   x_is_shift_o,
- output reg [1:0]  x_rd_source_o,
+ output reg [2:0]  x_rd_source_o,
- output reg x_rd_write_o
+ output reg 	   x_rd_write_o,
+ output reg [11:0] x_csr_sel_o,
+ output reg [4:0]  x_csr_imm_o,
+ output reg 	   x_is_csr_o,
+ output reg 	   x_is_eret_o
 );
@@ -181,6 +186,8 @@ module rv_decode
 	  if(d_is_shift)
 	    x_rd_source_o <= `RD_SOURCE_SHIFTER;
+	  else if (d_opcode == `OPC_SYSTEM)
+	    x_rd_source_o <= `RD_SOURCE_CSR;
 	  else
 	    x_rd_source_o <= `RD_SOURCE_ALU;
@@ -188,12 +195,28 @@ module rv_decode
 	  case (d_opcode)
 	    `OPC_OP_IMM, `OPC_OP, `OPC_JAL, `OPC_JALR, `OPC_LUI, `OPC_AUIPC:
 	      x_rd_write_o <= 1;
+	    `OPC_SYSTEM:
+	      x_rd_write_o <= (d_fun != 0); // CSR instructions write to RD
 	    default:
 	      x_rd_write_o <= 0;
 	  endcase // case (d_opcode)
       end // if (!d_stall_i)
+   // CSR/supervisor instructions
+   always@(posedge clk_i)
+	if (!d_stall_i)
+	  begin
+	     x_csr_imm_o <= f_ir_i[19:15];
+	     x_csr_sel_o <= f_ir_i[31:20];
+	     x_is_csr_o <= (d_opcode == `OPC_SYSTEM) && (d_fun != 0);
+	     x_is_eret_o <= (d_opcode == `OPC_SYSTEM) && (d_fun == 0) && (f_ir_i [31:20] == 12'b000100000000);
+	  end

--- a/rtl/rv_shifter.v
+++ b/rtl/rv_shifter.v
@@ -101,3 +101,98 @@ module rv_shifter
 endmodule // rv_shifter
+module rv_shifter2
+  (
+   input 	 clk_i,
+   input 	 rst_i,
+   input 	 x_stall_i,
+   input 	 w_stall_req_i,
+   output 	 x_stall_req_o,
+   input d_valid_i,
+   input [31:0]  d_rs1_i,
+   output [31:0] x_rd_o,
+   input [4:0] 	 d_shamt_i,
+   input [2:0] 	 d_fun_i,
+   input 	 d_shifter_sign_i,
+   input 	 d_is_shift_i );
+   wire 	 extend_sign = ((d_fun_i == `FUNC_SR) && d_shifter_sign_i) ? d_rs1_i[31] : 1'b0;
+   wire 	 shifter_req = !w_stall_req_i && d_valid_i && d_is_shift_i;
+   reg 		 shifter_req_d0;
+   always@(posedge clk_i)
+     if(shifter_req_d0 && !x_stall_i)
+       shifter_req_d0 <= 0;
+     else
+       shifter_req_d0 <= shifter_req;
+   assign x_stall_req_o = shifter_req && !shifter_req_d0;
+   reg [31:0] shift_pre, shift_16, shift_8, s1_out;
+   reg [31:0] s2_mask;
+   // stage 1
+   always@*
+     begin
+	shift_16 <= d_shamt_i[4] ? { d_rs1_i[15:0], d_rs1_i[31:16] } : d_rs1_i;
+	shift_8 <= d_shamt_i[3] ? { shift_16[7:0], shift_16[31:8] } : shift_16;
+     end
+   reg [4:0] s2_shift;
+   reg [2:0] s2_func;
+   reg 	     s2_extend_sign;
+   // stage 1 pipe register
+   always@(posedge clk_i)
+     begin : stage1
+	integer i;
+	for(i=0;i<32;i=i+1)
+	  begin
+	     if(d_fun_i == `FUNC_SL)
+	       s2_mask[i] <= (d_shamt_i < i) ? 1'b1 : 1'b0;
+	     else
+	       s2_mask[i] <= (d_shamt_i < (31-i)) ? 1'b1 : 1'b0;
+	  end
+	s2_extend_sign <= extend_sign;
+	s2_shift <= d_shamt_i;
+	s2_func <= d_fun_i;
+	s1_out <= shift_8;
+     end
+   reg [31:0] shift_4, shift_2, shift_1, shift_post;
+   // stage 2
+   always@*
+     begin : stage2
+	integer i;
+	shift_4 <= s2_shift[2] ? { s1_out[3:0], s1_out[31:4] } : s1_out;
+	shift_2 <= s2_shift[1] ? { s1_out[1:0], shift_4[31:2] } : shift_4;
+	shift_1 <= s2_shift[0] ? { s1_out[0], shift_2[31:1] } : shift_2;
+         for(i=0;i<32;i=i+1)
+	  begin
+	     if(s2_extend_sign)
+	       shift_post <= s2_mask[i] ? 1'b1 : shift_1[i];
+	     else
+	       shift_post <= s2_mask[i] ? 1'b0 : shift_1[i];
+	  end
+     end
+   assign x_rd_o = shift_post;
+endmodule // rv_shifter
--- a/rtl/xrv_core.vhd
+++ b/rtl/xrv_core.vhd
@@ -57,6 +57,8 @@ architecture wrapper of xrv_core is
    port(
      clk_i : in std_logic;
      rst_i : in std_logic;
+		irq_i : in std_logic;
      im_addr_o  : out std_logic_vector(31 downto 0);
      im_data_i  : in  std_logic_vector(31 downto 0);
@@ -208,6 +210,7 @@ begin
    port map (
      clk_i           => clk_sys_i,
      rst_i           => cpu_rst,
+		irq_i => '0',
      im_addr_o       => im_addr,
      im_data_i       => im_data,
      im_valid_i      => im_valid,

--- a/sw/common/board.h
+++ b/sw/common/board.h
@@ -24,6 +24,8 @@
 #include <stdint.h>
+#include "riscv.h"
 #define BASE_CLOCK 62500000 // Xtal frequency
 #define BASE_UART 0x20000

--- a/sw/common/crt0.S
+++ b/sw/common/crt0.S
  .section    .boot, "ax", @progbits
+#define MSTATUS_IE 0x00000001
+#define EXCEPT_IRQ (1<<10)
+#define EXCEPT_TIMER (1<<9)
 .global _start
 _start:
+    j _entry
+.org 0x40
+.extern trap_entry
+_exception_handler:
+    j  trap_entry
+_entry:
        la      gp, _gp                 # Initialize global pointer
 	la      sp, _fstack
+	la t0, _fexception_stack
+	csrrw t0, mscratch, t0
 # clear the bss segment
 	la	t0, _fbss
@@ -18,4 +34,5 @@ _start:
 #endif
 	bltu	t0, t1, 1b
        call    main
\ No newline at end of file
--- a/sw/common/ram2.ld
+++ b/sw/common/ram2.ld
@@ -129,5 +129,7 @@ SECTIONS
  /* End of uninitialized data segment (used by syscalls.c for heap) */
  PROVIDE( end = . );
  _end = ALIGN(8);
-  PROVIDE( _fstack = 0xfffc );
+  PROVIDE( _fstack = 0xfffc - 0x400 );
+  PROVIDE( _fexception_stack = 0xfffc );
 }
--- a/sw/hello/Makefile
+++ b/sw/hello/Makefile
@@ -8,12 +8,12 @@ OBJCOPY =	$(CROSS_COMPILE)objcopy
 SIZE =		$(CROSS_COMPILE)size
 CFLAGS = -g -m32 -msoft-float -march=RV32I  -I. -I../common
-OBJS = ../common/crt0.o main.o ../common/uart.o
+OBJS = ../common/crt0.o ../common/irq.o main.o ../common/uart.o
 LDS = ../common/ram2.ld
 OUTPUT=hello
 $(OUTPUT): $(LDS) $(OBJS)
-	${CC} -g -m32 -msoft-float  -march=RV32I -o $(OUTPUT).elf -nostartfiles $(OBJS)  -lm -T $(LDS)
+	${CC} -g -m32 -msoft-float  -march=RV32I -o $(OUTPUT).elf -nostartfiles $(OBJS)  -lm -L../coremark -lcoremark -T $(LDS)
 	${OBJCOPY} -O binary $(OUTPUT).elf $(OUTPUT).bin
 	${OBJDUMP} -D $(OUTPUT).elf > disasm.S
 #	../genraminit $(OUTPUT).bin 1024 0 0 > uart-bootloader.ram

--- a/sw/hello/main.c
+++ b/sw/hello/main.c
@@ -19,15 +19,27 @@ void delay(int v)
    for(i=0;i<v;i++);
 }
+volatile int irq_count = 0;
+void handle_trap()
+{
+    irq_count++;
+}
+extern void coremark_main();
 main()
 {
    uart_init_hw();
+//    coremark_main();
+//    for(;;);
    for(;;)
    {
-        puts("Hello, world!\n\r");
+//	volatile int t = rdtime();
+//        puts("Hello, world!\n\r");
 	gpio_set(0, 1);
 	gpio_set(1, 1);
 	gpio_set(2, 1);

--- a/sw/test2/Makefile
+++ b/sw/test2/Makefile
@@ -8,8 +8,8 @@ OBJCOPY =	$(CROSS_COMPILE)objcopy
 SIZE =		$(CROSS_COMPILE)size
 CFLAGS = -g -m32 -msoft-float -march=RV32I -O2
-OBJS = crt0.o main.o
+OBJS = ../common/crt0.o ../common/irq.o main.o
-LDS = ram2.ld
+LDS = ../common/ram2.ld
 OUTPUT=test2
 $(OUTPUT): $(LDS) $(OBJS)

--- a/sw/test2/main.c
+++ b/sw/test2/main.c
 #include <math.h>
+#include <stdint.h>
 char dupa[64];
@@ -165,6 +166,22 @@ void test_floats()
 }
 #endif
+#define read_csr(reg) ({ unsigned long __tmp; \
+  asm volatile ("csrr %0, " #reg : "=r"(__tmp)); \
+  __tmp; })
+uint32_t sys_get_cycles()
+{
+    return read_csr (cycle);
+}
+volatile int irq_counter = 0;
+void handle_trap()
+{
+    irq_counter++;
+}
 main()
 {
    char *s = hello;
@@ -172,7 +189,12 @@ main()
 //    test_floats();
+    uint32_t start = sys_get_cycles();
    test_sort();
+    uint32_t end = sys_get_cycles();
+    print_hex(end - start);
+    print_hex(irq_counter);
 //    for(i=0;i<5;i++)
 //    float y = cos(x);
@@ -184,7 +206,11 @@ main()
 //    y = x << 2;
 //    print_hex(*(int*)&y);
-    for(;;);
+    for(;;)
+    {
+	for(i=0;i<10000;i++) asm volatile("nop");
+        print_hex(irq_counter);
+    }
 /*
    x = 0xfffffb2e;

--- a/sw/uart_bootloader/boot.c
+++ b/sw/uart_bootloader/boot.c
@@ -24,6 +24,11 @@ uart_write_byte('\n');
 uart_write_byte('\r');
 }
+void trap_entry()
+{
+}
 int read_blocking(uint8_t *what)
 {
 	int cnt = 500000;

--- a/sw/uart_bootloader/boot.ld
+++ b/sw/uart_bootloader/boot.ld
@@ -133,5 +133,9 @@ SECTIONS
  /* End of uninitialized data segment (used by syscalls.c for heap) */
  PROVIDE( end = . );
  _end = ALIGN(8);
-  PROVIDE( _fstack = 0xfffc );
+  PROVIDE( _fstack = 0xfffc - 0x200 );
+  PROVIDE( _fexception_stack = 0xfffc );
 }
--- a/sw/uart_bootloader/crt0.S
+++ b/sw/uart_bootloader/crt0.S
-  .section    .boot, "ax", @progbits
- .global _start
-_start:
-        la      gp, _gp                 # Initialize global pointer
-	la      sp, _fstack
-# clear the bss segment
-	la	t0, _fbss
-	la	t1, _end
-1:
-#ifdef __riscv64
-	sd	zero,0(t0)
-	addi	t0, t0, 8
-#else
-	sw	zero,0(t0)
-	addi	t0, t0, 4
-#endif
-	bltu	t0, t1, 1b
-        call    main
\ No newline at end of file
--- a/sw/uart_bootloader/uart.c
+++ b/sw/uart_bootloader/uart.c
-/*
- * DSI Shield
- *
- * Copyright (C) 2013-2014 twl
- *
- * This program is free software: you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the
- * Free Software Foundation, either version 3 of the License, or (at your
- * option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-/* uart.c - simple UART driver */
-#include <stdint.h>
-#include "uart.h"
-#include "board.h"
-#include <hw/wb_uart.h>
-#define CALC_BAUD(baudrate) \
-    ( ((( (unsigned int)baudrate << 12)) + \
-      (BASE_CLOCK >> 8)) / (BASE_CLOCK >> 7) )
-volatile struct UART_WB *uart;
-void uart_init_hw()
-{
-	uart = (volatile struct UART_WB *)BASE_UART;
-#ifndef SIMULATION
-	uart->BCR = CALC_BAUD(UART_BAUDRATE);
-#else
-	uart->BCR = CALC_BAUD((CPU_CLOCK/10));
-#endif
-}
-void uart_write_byte(int b)
-{
-	if (b == '\n')
-		uart_write_byte('\r');
-	while (uart->SR & UART_SR_TX_BUSY)
-		;
-	uart->TDR = b;
-}
-int uart_poll()
-{
-	return uart->SR & UART_SR_RX_RDY;
-}
-int uart_read_byte()
-{
-	if (!uart_poll())
-		return -1;
-	return uart->RDR & 0xff;
-}
-int puts(const char *s)
-{
-  char c;
-  while(c=*s++)
-    uart_write_byte(c);
-}
--- a/syn/spec/rv_core_test.xise
+++ b/syn/spec/rv_core_test.xise
--- a/tb/cpu/main.sv
+++ b/tb/cpu/main.sv
@@ -108,12 +108,32 @@ module main;
 	dm_data_l <= mem[(dm_addr/4) % mem_size];
   end	   
+   reg irq = 0;
+   wire irq_ack;
+   initial begin
+      repeat(100)
+	@(posedge clk);
+      irq<=1;
+      @(posedge clk);
+      irq <= 0;
+      @(posedge clk);
+   end // initial begin
   rv_cpu DUT
     (
      .clk_i(clk),
      .rst_i(rst),
+      .irq_i ( irq ),
      // instruction mem I/F
      .im_addr_o(im_addr),
      .im_data_i(im_data),
@@ -137,7 +157,8 @@ module main;
   initial begin
-      load_ram("sw/test2/test2.ram");
+//      load_ram("../../sw/test_csr/test_csr.ram");
+      load_ram("../../sw/test2/test2.ram");
      repeat(3) @(posedge clk);
      rst = 0;
   end
@@ -215,6 +236,22 @@ module main;
 	31: return "t6";
      endcase // case (fun)
   endfunction // decode_regname
+   function string decode_csr(bit[11:0] csr);
+      case(csr)
+	`CSR_ID_CYCLESH: return "cyclesh";
+	`CSR_ID_CYCLESL: return "cyclesl";
+	`CSR_ID_TIMEH: return "timeh";
+	`CSR_ID_TIMEL: return "timel";
+	`CSR_ID_MSCRATCH: return "mscratch";
+	`CSR_ID_MEPC: return "mepc";
+	`CSR_ID_MSTATUS: return "mstatus";
+	`CSR_ID_MCAUSE: return "mcause";
+	default: return "???";
+      endcase // case (csr)
+   endfunction // decode_csr
   task automatic verify_branch(input [31:0] rs1, input[31:0] rs2, input take, input [2:0] fun);
@@ -321,9 +358,10 @@ module main;
 	  reg [31:0] imm;
-	  //	  $display("Opcode %x", DUT.d2x_opcode);
+//	  	  $display("Opcode %x", DUT.d2x_opcode);
 	  case (DUT.d2x_opcode)
 	    `OPC_AUIPC: 
@@ -395,13 +433,38 @@ module main;
 //decode_op(DUT.d2x_fun);
 		 args = $sformatf("%-3s %-3s [0x%-08x + %s]", rs2, rs1, DUT.execute.rs1, s_hex($signed(DUT.execute.d_imm_i)));
 	      end
+	    `OPC_SYSTEM:
+	      begin
+		 case (DUT.d2x_fun)
+		   `CSR_OP_CSRRWI:  begin
+		      opc = "csrrwi";
+		      args = $sformatf("%-3s %-3s 0x%08x", rd, decode_csr(DUT.d2x_csr_sel), ((DUT.d2x_csr_imm)));
+		   end
+		   `CSR_OP_CSRRSI:  begin
+		      opc = "csrrsi";
+		      args = $sformatf("%-3s %-3s 0x%08x", rd, decode_csr(DUT.d2x_csr_sel), ((DUT.d2x_csr_imm)));
+		   end
+		   `CSR_OP_CSRRCI:  begin
+		      opc = "csrrci";
+		      args = $sformatf("%-3s %-3s 0x%08x", rd, decode_csr(DUT.d2x_csr_sel), ((DUT.d2x_csr_imm)));
+		   end
+		   `CSR_OP_CSRRW: begin
+		      opc = "csrrw";
+		      args = $sformatf("%-3s %-3s %-3s [0x%08x]", rd, decode_csr(DUT.d2x_csr_sel), rs1, DUT.execute.rs1);
+		   end
+	   endcase // case (d_fun_i)
+	      end
 	  endcase // case (d2x_opcode)
 	  $display("%08x: %-8s %-3s %s", DUT.execute.d_pc_i, opc, fun, args);
  	  $fwrite(f_exec_log,"%08x: %-8s %-3s %s\n", DUT.execute.d_pc_i, opc, fun, args);
-	  $fwrite(f_exec_log,": PC %08x OP %08x\n", DUT.execute.d_pc_i, DUT.decode.x_ir);
+	  $fwrite(f_exec_log,": PC %08x OP %08x", DUT.execute.d_pc_i, DUT.decode.x_ir);

--- a/tb/cpu/run.do
+++ b/tb/cpu/run.do
-vlog -sv main.sv +incdir+. +incdir+../../include/wb +incdir+../../include/vme64x_bfm +incdir+../../include +incdir+../include +incdir+../../sim
+#vlog -sv main.sv +incdir+. +incdir+../../include/wb +incdir+../../include/vme64x_bfm +incdir+../../include +incdir+../include +incdir+../../sim
-vsim -t 1ps work.main -voptargs=+acc
+vsim -t 1ps work.main -novopt
 set StdArithNoWarnings 1
 set NumericStdNoWarnings 1

--- a/tb/cpu/wave.do
+++ b/tb/cpu/wave.do