diff --git a/mdu/rtl/mdu_top.v b/mdu/rtl/mdu_top.v
index 3aedd3a..092aaae 100644
--- a/mdu/rtl/mdu_top.v
+++ b/mdu/rtl/mdu_top.v
@@ -3,7 +3,7 @@
 /* Multiplication Division Unit */
 module mdu_top
 #(
-  parameter WIDTH = 1
+  parameter WIDTH = 32
 )(
   input  wire             i_clk,  
   input  wire             i_rst,
@@ -14,8 +14,8 @@ module mdu_top
   output reg              o_mdu_ready,
   output wire [WIDTH-1:0] o_mdu_rd
 );
-  wire [31:0] rdata_a;
-  wire [31:0] rdata_b;
+  reg [31:0] rdata_a;
+  reg [31:0] rdata_b;
   reg  [31:0] rs1_d, rs1_q; 
   reg  [31:0] rs2_d, rs2_q;
   reg  [ 4:0] cnt_d, cnt_q;
@@ -24,115 +24,13 @@ module mdu_top
   reg [31:0]  temp;
   reg [63:0]  result_64;
   reg [31:0]  result_32;
-
-  /* FSM starts*/
-  parameter RESET   = 3'b000;
-  parameter IDLE    = 3'b001;
-  parameter GET     = 3'b010;
-  parameter BUSY    = 3'b011;
-  parameter SEND    = 3'b100;
-
-  reg [2:0] fsm_cs, fsm_ns;
-  reg valid_d, valid_q;
-
-  always @(*) begin
-    en_d   = 1'b1;
-    o_mdu_rd =1'b0;
-    cnt_d  = cnt_q;
-    fsm_ns = fsm_cs;
-    case (fsm_cs)
-      RESET: begin
-        o_mdu_rd = 1'b0;
-        cnt_d  = 5'b0;
-      end
-      IDLE: begin
-        cnt_d = 5'b0;
-        valid_d = 1'b1;
-        if (i_mdu_valid) begin
-          fsm_ns = GET;
-        end else begin
-          fsm_ns = IDLE;
-        end
-      end
-      GET: begin
-        if (&cnt_q) begin
-          fsm_ns = BUSY;
-        end else begin
-          rs1_d  = {rs1_q[30:0], i_mdu_rs1};
-          rs2_d  = {rs2_q[30:0], i_mdu_rs2};
-          en_d   = 1'b0;
-          cnt_d += 1'b1;
-          fsm_ns = GET;
-        end
-      end
-      BUSY: begin
-        valid_d = 1'b0;
-        fsm_ns  = SEND;
-      end
-      SEND: begin
-        valid_d = 1'b1;
-        if (&cnt_q) begin
-          fsm_ns = IDLE;
-        end else begin
-          if (done) begin
-            temp = result_32;
-          end else begin
-            o_mdu_rd = temp[31];
-            temp = {temp[30:0],1'b0};
-            cnt_d += 1'b1;
-            fsm_ns = SEND;
-          end
-        end
-      end
-      default: begin
-        o_mdu_rd = 1'b0;
-        fsm_ns = IDLE;
-      end
-    endcase
-  end
+  
+  assign rdata_a = i_mdu_rs1;
+  assign rdata_b = i_mdu_rs2;
 
   always @(posedge i_clk) begin
-    if (i_rst) begin
-      rs1_q <= 32'b0;
-      rs2_q <= 32'b0;
-      cnt_q <= 5'b0;
-      valid_q <= 1'b0;
-      en_q  <= 1'b0;
-      fsm_cs <= RESET;
-    end else begin
-      rs1_q <= rs1_d;
-      rs2_q <= rs2_d;
-      cnt_q <= cnt_d;
-      valid_q <= valid_d;
-      en_q  <= en_d;
-      fsm_cs <= fsm_ns; 
-    end
+    en_q        <= i_mdu_valid;
   end
-  /* FSM ends*/
-
-/*
-  always @(posedge i_clk) begin
-    if (i_rst) begin
-      rs1 <= 32'b0;
-      rs2 <= 32'b0;
-      cnt <=  5'b0;
-      en  <=  1'b0;
-    end else if (i_mdu_valid | (!(&cnt))) begin
-      rs1 <= {rs1[30:0], i_mdu_rs1};
-      rs2 <= {rs2[30:0], i_mdu_rs2};
-      en  <= 1'b0;
-      cnt <= cnt + 1'b1;
-    end
-
-    if (&cnt) begin
-      cnt <= 5'b0;
-      en  <= 1'b1;
-      o_mdu_ready = 1'b0;
-    end
-  end
-*/
-  assign rdata_a = en_q ? rs1_q : 32'b0;
-  assign rdata_b = en_q ? rs2_q : 32'b0;
 
   always @(posedge i_clk) begin
     if (i_rst) begin
@@ -140,16 +38,17 @@ module mdu_top
       o_mdu_ready = 1'b0;
       result_32   = 32'b0;
       result_64   = 64'b0;
+      // en_q        = 1'b0;
     end else begin
-      o_mdu_ready = 1'b1;
+      o_mdu_ready = 1'b0;
       result_32   = 32'hffffffff;
       result_64   = {result_32,result_32};
       case (i_mdu_op)
-        3'b000: begin  // MUL
-          if (en_q) begin
+        3'b000: begin
+          if (en_q) begin // MUL
             result_64 = rdata_a * rdata_b;
             result_32 = result_64[31:0];
-            o_mdu_ready = 1'b1;
+            o_mdu_ready = en_q;
             done = 1'b1;
           end
         end
@@ -157,7 +56,7 @@ module mdu_top
           if (en_q) begin
             result_64 = $signed(rdata_a) * $signed(rdata_b);
             result_32 = result_64[63:32];
-            o_mdu_ready = 1'b1;
+            o_mdu_ready = en_q;
             done = 1'b1;
           end
         end
@@ -165,7 +64,7 @@ module mdu_top
           if (en_q) begin
             result_64 = $signed(rdata_a) * $unsigned(rdata_b);
             result_32 = result_64[63:32];
-            o_mdu_ready = 1'b1;
+            o_mdu_ready = en_q;
             done = 1'b1;
           end
         end
@@ -173,7 +72,7 @@ module mdu_top
           if (en_q) begin
             result_64 = $unsigned(rdata_a) * $unsigned(rdata_b);
             result_32 = result_64[63:32];
-            o_mdu_ready = 1'b1;
+            o_mdu_ready = en_q;
             done = 1'b1;
           end
         end
@@ -181,7 +80,7 @@ module mdu_top
           if (en_q) begin
             result_32 = $signed(rdata_a) / $signed(rdata_b);
             // result_32 = result_64[31:0];
-            o_mdu_ready = 1'b1;
+            o_mdu_ready = en_q;
             done = 1'b1;
           end
         end
@@ -189,7 +88,7 @@ module mdu_top
           if (en_q) begin
             result_32 = $unsigned(rdata_a) * $unsigned(rdata_b);
             // result_32 = result_64[31:0];
-            o_mdu_ready = 1'b1;
+            o_mdu_ready = en_q;
             done = 1'b1;
           end
         end
@@ -197,7 +96,7 @@ module mdu_top
           if (en_q) begin
             result_32 = $signed(rdata_a) % $signed(rdata_b);
             // result_32 = result_64[31:0];
-            o_mdu_ready = 1'b1;
+            o_mdu_ready = en_q;
             done = 1'b1;
           end
         end
@@ -205,19 +104,20 @@ module mdu_top
           if (en_q) begin
             result_32 = $unsigned(rdata_a) % $unsigned(rdata_b);
             // result_32 = result_64[31:0];
-            o_mdu_ready = 1'b1;
+            o_mdu_ready = en_q;
             done = 1'b1;
           end
         end
         default: begin
           result_32 = 32'hffffffff;
           result_64 = {result_32,result_32};
-          o_mdu_ready = 1'b1;
+          o_mdu_ready = 1'b0;
         end
       endcase
     end
   end
 
-//  TODO: Set the transmission side from core part
+  assign o_mdu_rd = result_32;
+
 endmodule
-`default_nettype wire
\ No newline at end of file
+`default_nettype wire
diff --git a/rtl/serv_bufreg.v b/rtl/serv_bufreg.v
index 7d32b42..7c227c5 100644
--- a/rtl/serv_bufreg.v
+++ b/rtl/serv_bufreg.v
@@ -6,7 +6,8 @@ module serv_bufreg
    input wire 	      i_cnt1,
    input wire 	      i_en,
    input wire 	      i_init,
-   output reg [1:0]   o_lsb,
+   input wire           i_mdu_en,
+   output wire [1:0]    o_lsb,
    //Control
    input wire 	      i_rs1_en,
    input wire 	      i_imm_en,
@@ -17,11 +18,13 @@ module serv_bufreg
    input wire 	      i_imm,
    output wire 	      o_q,
    //External
-   output wire [31:0] o_dbus_adr);
+   output wire [31:0] o_dbus_adr,
+   output wire [31:0] o_mdu_rs1);
 
    wire 	      c, q;
    reg 		      c_r;
    reg [31:2] 	      data;
+   reg [1:0]            lsb;
 
    wire 	      clr_lsb = i_cnt0 & i_clr_lsb;
 
@@ -35,11 +38,12 @@ module serv_bufreg
 	data <= {i_init ? q : (data[31] & i_sh_signed), data[31:3]};
 
       if (i_init ? (i_cnt0 | i_cnt1) : i_en)
-	o_lsb <= {i_init ? q : data[2],o_lsb[1]};
-
+	lsb <= {i_init ? q : data[2],lsb[1]};
    end
 
-   assign o_q = o_lsb[0] & i_en;
+   assign o_q = lsb[0] & i_en;
    assign o_dbus_adr = {data, 2'b00};
+   assign o_mdu_rs1  = {o_dbus_adr[31:2],lsb};
+   assign o_lsb = i_mdu_en ? 2'b00 : lsb;
 
 endmodule
diff --git a/rtl/serv_decode.v b/rtl/serv_decode.v
index b675901..06901b4 100644
--- a/rtl/serv_decode.v
+++ b/rtl/serv_decode.v
@@ -17,6 +17,8 @@ module serv_decode #(
    output reg       o_shift_op,
    output reg       o_slt_op,
    output reg       o_rd_op,
+   output reg       o_mdu_op,
+   output reg [2:0] o_mdu_opcode,
    //To bufreg
    output reg       o_bufreg_rs1_en,
    output reg       o_bufreg_imm_en,
@@ -61,8 +63,13 @@ module serv_decode #(
    reg        op22;
    reg        op26;
 
+   reg       imm25;
    reg       imm30;
 
+   //mdu
+   wire co_mdu_op = ((opcode == 5'b01100) & imm25);
+   wire [2:0]co_mdu_opcode = funct3;
+
    //opcode
    wire op_or_opimm = (!opcode[4] & opcode[2] & !opcode[0]);
 
@@ -190,7 +197,7 @@ module serv_decode #(
 
    wire co_mem_cmd  = opcode[3];
    wire co_mem_signed = ~funct3[2];
-   wire co_mem_word   = funct3[1];
+   wire co_mem_word   = co_mdu_op ? co_mdu_op :funct3[1];
    wire co_mem_half   = funct3[0];
 
    wire [1:0] co_alu_bool_op = funct3[1:0];
@@ -229,6 +236,7 @@ module serv_decode #(
             if (i_wb_en) begin
                funct3 <= i_wb_rdt[14:12];
                imm30  <= i_wb_rdt[30];
+               imm25  <= i_wb_rdt[25];
                opcode <= i_wb_rdt[6:2];
                op20   <= i_wb_rdt[20];
                op21   <= i_wb_rdt[21];
@@ -248,6 +256,8 @@ module serv_decode #(
             o_shift_op         = co_shift_op;
             o_slt_op           = co_slt_op;
             o_rd_op            = co_rd_op;
+            o_mdu_op           = co_mdu_op;
+            o_mdu_opcode       = co_mdu_opcode;
             o_bufreg_rs1_en    = co_bufreg_rs1_en;
             o_bufreg_imm_en    = co_bufreg_imm_en;
             o_bufreg_clr_lsb   = co_bufreg_clr_lsb;
@@ -285,6 +295,7 @@ module serv_decode #(
          always @(*) begin
             funct3  = i_wb_rdt[14:12];
             imm30   = i_wb_rdt[30];
+            imm25   = i_wb_rdt[25];
             opcode  = i_wb_rdt[6:2];
             op20    = i_wb_rdt[20];
             op21    = i_wb_rdt[21];
@@ -304,6 +315,8 @@ module serv_decode #(
                o_shift_op         <= co_shift_op;
                o_slt_op           <= co_slt_op;
                o_rd_op            <= co_rd_op;
+               o_mdu_op           <= co_mdu_op;
+               o_mdu_opcode       <= co_mdu_opcode;
                o_bufreg_rs1_en    <= co_bufreg_rs1_en;
                o_bufreg_imm_en    <= co_bufreg_imm_en;
                o_bufreg_clr_lsb   <= co_bufreg_clr_lsb;
diff --git a/rtl/serv_rf_if.v b/rtl/serv_rf_if.v
index f12946d..cff08bb 100644
--- a/rtl/serv_rf_if.v
+++ b/rtl/serv_rf_if.v
@@ -36,9 +36,6 @@ module serv_rf_if
    input wire 		      i_csr_rd,
    input wire 		      i_rd_csr_en,
    input wire 		      i_mem_rd,
-`ifdef MDU
-   input wire           i_mdu_rd,
-`endif
 
    //RS1 read port
    input wire [4:0] 	      i_rs1_raddr,
@@ -56,17 +53,10 @@ module serv_rf_if
 
    generate
    if (WITH_CSR) begin
-`ifdef MDU
-   wire 	     rd = (i_ctrl_rd ) |
-			  (i_alu_rd & i_rd_alu_en) |
-			  (i_csr_rd & i_rd_csr_en) |
-			  (i_mem_rd) | (i_mdu_rd);
-`else
    wire 	     rd = (i_ctrl_rd ) |
 			  (i_alu_rd & i_rd_alu_en) |
 			  (i_csr_rd & i_rd_csr_en) |
 			  (i_mem_rd);
-`endif
 
    wire 	     mtval = i_mem_op ? i_bufreg_q : i_bad_pc;
 
@@ -129,15 +119,10 @@ module serv_rf_if
    assign o_csr_pc = i_rdata1;
 
    end else begin
-`ifdef MDU
-      wire 	     rd = (i_ctrl_rd ) |
-			  (i_alu_rd & i_rd_alu_en) |
-			  (i_mem_rd) | (i_mdu_rd);
-`else
       wire 	     rd = (i_ctrl_rd ) |
 			  (i_alu_rd & i_rd_alu_en) |
 			  (i_mem_rd);
-`endif
+
       assign 	     o_wdata0 = rd;
       assign	     o_wdata1 = 1'b0;
 
diff --git a/rtl/serv_rf_top.v b/rtl/serv_rf_top.v
index 0fdfafc..3dc2ee5 100644
--- a/rtl/serv_rf_top.v
+++ b/rtl/serv_rf_top.v
@@ -80,12 +80,11 @@ module serv_rf_top
    wire [RF_WIDTH-1:0] rdata;
 
 `ifdef MDU
-   wire       mdu_rs1;
-   wire       mdu_rs2;
-   wire [2:0] mdu_op;
-   wire       mdu_valid;
-   wire       mdu_ready;
-   wire       mdu_rd;
+   wire [2:0]  mdu_op;
+   wire        mdu_valid;
+   wire        mdu_ready;
+   wire [31:0] mdu_rs1;
+   wire [31:0] mdu_rd;
 `endif
 
    serv_rf_ram_if
@@ -130,7 +129,7 @@ module serv_rf_top
    (.i_clk(clk),
     .i_rst(i_rst),
     .i_mdu_rs1(mdu_rs1),
-    .i_mdu_rs2(mdu_rs2),
+    .i_mdu_rs2(o_dbus_dat),
     .i_mdu_op(mdu_op),
     .i_mdu_valid(mdu_valid),
     .o_mdu_ready(mdu_ready),
@@ -189,20 +188,19 @@ module serv_rf_top
       .i_ibus_rdt   (i_ibus_rdt),
       .i_ibus_ack   (i_ibus_ack),
 `ifdef MDU
-      .o_mdu_rs1    (mdu_rs1),
-      .o_mdu_rs2    (mdu_rs2),
-      .o_mdu_op     (mdu_op),
+      .o_mdu_opcode (mdu_op),
       .o_mdu_valid  (mdu_valid),
       .i_mdu_ready  (mdu_ready),
-      .i_mdu_rd     (mdu_rd),
+      // .i_mdu_rd     (mdu_rd),
 `endif
       .o_dbus_adr   (o_dbus_adr),
       .o_dbus_dat   (o_dbus_dat),
       .o_dbus_sel   (o_dbus_sel),
       .o_dbus_we    (o_dbus_we),
       .o_dbus_cyc   (o_dbus_cyc),
-      .i_dbus_rdt   (i_dbus_rdt),
-      .i_dbus_ack   (i_dbus_ack));
+      .i_dbus_rdt   (mdu_ready ? mdu_rd:i_dbus_rdt),
+      .i_dbus_ack   (i_dbus_ack | mdu_ready),
+      .o_mdu_rs1    (mdu_rs1));
 
 endmodule
 `default_nettype wire
diff --git a/rtl/serv_state.v b/rtl/serv_state.v
index 7fa088a..d4535fb 100644
--- a/rtl/serv_state.v
+++ b/rtl/serv_state.v
@@ -22,6 +22,9 @@ module serv_state
    input wire 	     i_slt_op,
    input wire 	     i_e_op,
    input wire 	     i_rd_op,
+   input wire       i_mdu_op,
+   output wire         o_mdu_valid,
+   input wire          i_mdu_ready,
    output wire 	     o_init,
    output wire 	     o_cnt_en,
    output wire 	     o_cnt0,
@@ -75,7 +78,10 @@ module serv_state
    wire      take_branch = i_branch_op & (!i_cond_branch | (i_alu_cmp^i_bne_or_bge));
 
    //slt*, branch/jump, shift, load/store
-   wire two_stage_op = i_slt_op | i_mem_op | i_branch_op | i_shift_op;
+   wire two_stage_op = i_slt_op | i_mem_op | i_branch_op | i_shift_op | i_mdu_op;
+
+   //valid signal for mdu
+   assign o_mdu_valid = i_mdu_op & o_cnt_done;
 
    assign o_dbus_cyc = !o_cnt_en & init_done & i_mem_op & !i_mem_misalign;
 
@@ -87,7 +93,7 @@ module serv_state
    // and the first stage didn't cause a misalign exception
    assign o_rf_wreq = !misalign_trap_sync &
 		      ((i_shift_op & (i_sh_done | !i_sh_right) & !o_cnt_en & init_done) |
-		       (i_mem_op & i_dbus_ack) |
+		       (i_mem_op & i_dbus_ack) | i_mdu_ready |
 		       (stage_two_req & (i_slt_op | i_branch_op)));
 
    assign o_rf_rd_en = i_rd_op & !o_init;
diff --git a/rtl/serv_top.v b/rtl/serv_top.v
index 8eef244..e44eea8 100644
--- a/rtl/serv_top.v
+++ b/rtl/serv_top.v
@@ -47,12 +47,9 @@ module serv_top
    input wire 		      i_rdata0,
    input wire 		      i_rdata1,
 `ifdef MDU
-   output reg         o_mdu_rs1,
-   output reg         o_mdu_rs2,
-   output reg  [ 2:0] o_mdu_op,
+   output reg  [ 2:0] o_mdu_opcode,
    output reg         o_mdu_valid,
    input  wire        i_mdu_ready,
-   input  wire        i_mdu_rd,
 `endif
    output wire [31:0] 	      o_ibus_adr,
    output wire 		      o_ibus_cyc,
@@ -64,7 +61,8 @@ module serv_top
    output wire 		      o_dbus_we ,
    output wire 		      o_dbus_cyc,
    input wire [31:0] 	      i_dbus_rdt,
-   input wire 		      i_dbus_ack);
+   input wire 		      i_dbus_ack,
+   output wire [31:0]   o_mdu_rs1);
 
    wire [4:0]    rd_addr;
    wire [4:0]    rs1_addr;
@@ -72,7 +70,7 @@ module serv_top
 
    wire [3:0] 	 immdec_ctrl;
    wire [3:0] 	immdec_en;
-
+   
    wire          sh_right;
    wire 	 bne_or_bge;
    wire 	 cond_branch;
@@ -83,6 +81,7 @@ module serv_top
    wire 	 shift_op;
    wire 	 slt_op;
    wire 	 rd_op;
+   wire   mdu_op;
 
    wire 	 rd_alu_en;
    wire 	 rd_csr_en;
@@ -201,6 +200,9 @@ module serv_top
       .i_slt_op       (slt_op),
       .i_e_op         (e_op),
       .i_rd_op        (rd_op),
+      .i_mdu_op       (mdu_op),
+      .o_mdu_valid    (o_mdu_valid),
+      .i_mdu_ready    (i_mdu_ready),
       //External
       .o_dbus_cyc     (o_dbus_cyc),
       .i_dbus_ack     (i_dbus_ack),
@@ -231,6 +233,9 @@ module serv_top
       .o_slt_op           (slt_op),
       .o_rd_op            (rd_op),
       .o_sh_right         (sh_right),
+      .o_mdu_op           (mdu_op),
+      .o_mdu_opcode       (o_mdu_opcode),
+      
       //To bufreg
       .o_bufreg_rs1_en    (bufreg_rs1_en),
       .o_bufreg_imm_en    (bufreg_imm_en),
@@ -296,6 +301,7 @@ module serv_top
       .i_cnt1   (cnt1),
       .i_en     (bufreg_en),
       .i_init   (init),
+      .i_mdu_en (mdu_op),
       .o_lsb    (lsb),
       //Control
       .i_sh_signed (bufreg_sh_signed),
@@ -307,7 +313,8 @@ module serv_top
       .i_imm    (imm),
       .o_q      (bufreg_q),
       //External
-      .o_dbus_adr (o_dbus_adr));
+      .o_dbus_adr (o_dbus_adr),
+      .o_mdu_rs1  (o_mdu_rs1));
 
    serv_ctrl
      #(.RESET_PC (RESET_PC),
@@ -393,9 +400,6 @@ module serv_top
       .i_csr_rd    (csr_rd),
       .i_rd_csr_en (rd_csr_en),
       .i_mem_rd    (mem_rd),
-`ifdef MDU
-      .i_mdu_rd    (i_mdu_rd),
-`endif
       //RS1 read port
       .i_rs1_raddr (rs1_addr),
       .o_rs1       (rs1),
@@ -534,23 +538,5 @@ module serv_top
 
 `endif
 
-`ifdef MDU
-   wire [4:0] opcode = i_ibus_rdt[6:2];
-   always @(*) begin
-      if ((opcode == 5'b01100) & i_ibus_rdt[25]) begin
-         o_mdu_valid = 1'b1;
-      end else begin
-         o_mdu_valid = 1'b0;
-      end
-
-      o_mdu_op  = i_ibus_rdt[14:12];
-      
-      if (i_mdu_ready) begin
-         o_mdu_rs1 = rs1;
-         o_mdu_rs2 = rs2;
-      end
-   end
-`endif
-
 endmodule
 `default_nettype wire