diff --git a/src/Core/Controller.sv b/src/Core/Controller.sv
index 57c2913..ba3e2e5 100644
--- a/src/Core/Controller.sv
+++ b/src/Core/Controller.sv
@@ -67,7 +67,9 @@ module Controller (
   assign ctrl.MCtrl0.HW  = ~inst[30] & ~inst[26] & ~inst[29] & ~inst[28] & ~inst[27] & inst[4] & (~inst[5] & ~inst[1] & inst[0] | inst[3]) | inst[30] & inst[29] & ~inst[1];
   assign ctrl.MCtrl0.LW  = ~inst[30] & ~inst[26] & ~inst[29] & ~inst[28] & ~inst[27] & inst[4] & (~inst[5] & inst[1] & inst[0] | inst[3]) | inst[30] & inst[29] & ~inst[1];
   assign ctrl.MCtrl0.HLS = HLS_t'({(~inst[30] & ~inst[26] & ~inst[27] & ~inst[31] & ~inst[29] & ~inst[28] & inst[4] & inst[3] | inst[30] & inst[29]), inst[1] & ~inst[30], inst[0]});
+`ifdef ENABLE_MADD
   assign ctrl.MCtrl0.MAS = MAS_t'({inst[2], inst[30] & ~inst[2] & ~inst[1]});
+`endif
   assign ctrl.MCtrl0.C0D = inst[15:11];
   assign ctrl.MCtrl0.C0W = ~inst[31] & inst[30] & ~inst[29] & inst[23] & ~inst[3];
   assign ctrl.MCtrl0.SEL = inst[2:0];
diff --git a/src/Core/Datapath.sv b/src/Core/Datapath.sv
index 3983edd..2362072 100644
--- a/src/Core/Datapath.sv
+++ b/src/Core/Datapath.sv
@@ -225,10 +225,12 @@ module Datapath (
   word_t        M_I0_MULTUHF;
 
   logic         M_I0_MAS_bvalid;
+`ifdef ENABLE_MADD
   word_t        M_I0_MASH;
   word_t        M_I0_MASL;
   word_t        M_I0_MUASH;
   word_t        M_I0_MUASL;
+`endif
 
   logic         M_I0_MULT_bvalid;
   word_t        M_I0_MULTLB;
@@ -238,7 +240,9 @@ module Datapath (
   word_t        M_I0_HI;
   word_t        M_I0_LO;
 
+`ifdef ENABLE_TRAP
   logic         M_I1_Trap;
+`endif
   logic         M_I1_NowExcValid;
   logic         M_I1_PrevExcValid;
   logic   [4:0] M_I1_PrevExcCode;
@@ -740,13 +744,13 @@ module Datapath (
       E.en,
       {E.I0.imm, E.I0.sa}
   );
-  ffen #(14) E_I0_ECtrl_ff (
+  ffen #($bits(D.I0.ECtrl)) E_I0_ECtrl_ff (
       clk,
       D.I0.ECtrl,
       E.en,
       E.I0.ECtrl
   );
-  ffenrc #(19) E_I0_MCtrl_ff (
+  ffenrc #($bits(D.I0.MCtrl)) E_I0_MCtrl_ff (
       clk,
       rst | rstM,
       D.I0.MCtrl,
@@ -754,7 +758,7 @@ module Datapath (
       ~D_go | ~D_I0_go,
       E.I0.MCtrl
   );
-  ffenrc #(5 + 1) E_I0_WCtrl_ff (
+  ffenrc #(5 + $bits(D.I0.WCtrl)) E_I0_WCtrl_ff (
       clk,
       rst | rstM,
       {D.I0.RD, D.I0.WCtrl},
@@ -803,17 +807,13 @@ module Datapath (
       E.en,
       {E.I1.imm, E.I1.sa}
   );
-  ffen #(14) E_I1_ECtrl_ff (
+  ffen #($bits(D.I1.ECtrl)) E_I1_ECtrl_ff (
       clk,
       D.I1.ECtrl,
       E.en,
       E.I1.ECtrl
   );
-`ifdef ENABLE_TLB
-  ffenrc #(15)
-`else
-  ffenrc #(11)
-`endif
+  ffenrc #($bits(D.I1.MCtrl))
   E_I1_MCtrl_ff (
       clk,
       rst | rstM,
@@ -832,7 +832,7 @@ module Datapath (
       E.I1.Trap
   );
 `endif
-  ffenrc #(5 + 1) E_I1_WCtrl_ff (
+  ffenrc #(5 + $bits(D.I1.WCtrl)) E_I1_WCtrl_ff (
       clk,
       rst | rstM,
       {D.I1.RD, D.I1.WCtrl},
@@ -1067,7 +1067,7 @@ module Datapath (
       M.en,
       M.I0.ALUOut
   );
-  ffenrc #(19) M_I0_MCtrl_ff (
+  ffenrc #($bits(E.I0.MCtrl)) M_I0_MCtrl_ff (
       clk,
       rst | rstM,
       E.I0.MCtrl,
@@ -1075,7 +1075,7 @@ module Datapath (
       ~E_go | ~E_I0_go,
       M.I0.MCtrl
   );
-  ffenrc #(5 + 1) M_I0_WCtrl_ff (
+  ffenrc #(5 + $bits(E.I0.WCtrl)) M_I0_WCtrl_ff (
       clk,
       rst | rstM,
       {E.I0.RD, E.I0.WCtrl},
@@ -1122,11 +1122,7 @@ module Datapath (
       M.en,
       M.I1.ALUOut
   );
-`ifdef ENABLE_TLB
-  ffenrc #(15)
-`else
-  ffenrc #(11)
-`endif
+  ffenrc #($bits(E.I1.MCtrl))
   M_I1_MCtrl_ff (
       clk,
       rst | rstM,
@@ -1145,7 +1141,7 @@ module Datapath (
       M.I1.Trap
   );
 `endif
-  ffenrc #(5 + 1) M_I1_WCtrl_ff (
+  ffenrc #(5 + $bits(E.I1.WCtrl)) M_I1_WCtrl_ff (
       clk,
       rst | rstM,
       {E.I1.RD, E.I1.WCtrl},
@@ -1182,7 +1178,9 @@ module Datapath (
   assign M.I1.ExcValid    = M_I1_PrevExcValid | M_I1_NowExcValid;
   assign M.I1.REFILL      = M_I1_PrevREFILL | dTLBRefillB;
   assign M.I1.ExcCode     = M_I1_PrevExcValid ? M_I1_PrevExcCode
+`ifdef ENABLE_TRAP
                           : M_I1_Trap         ? `EXCCODE_TR
+`endif
                           : dAddressErrorB    ? M.I1.MCtrl.MWR ? `EXCCODE_ADES : `EXCCODE_ADEL
                           : dTLBRefillB       ? M.I1.MCtrl.MWR ? `EXCCODE_TLBS : `EXCCODE_TLBL
                           : dTLBInvalidB      ? M.I1.MCtrl.MWR ? `EXCCODE_TLBS : `EXCCODE_TLBL
@@ -1214,17 +1212,24 @@ module Datapath (
     {M_I0_MULTLF, M_I0_MULTHF, M_I0_MULTUHF, M_I0_MAS_bvalid}
   );
 
+`ifdef ENABLE_MADD
   // TODO: Optimize ME
   assign {M_I0_MUASH, M_I0_MUASL} = M.I0.MCtrl.MAS[0] ? {HI, LO} + {M_I0_MULTUHF, M_I0_MULTLF}
                                                       : {HI, LO} - {M_I0_MULTUHF, M_I0_MULTLF};
   assign {M_I0_MASH, M_I0_MASL} = M.I0.MCtrl.MAS[0] ? $signed({HI, LO}) + $signed({M_I0_MULTHF, M_I0_MULTLF})
                                                     : $signed({HI, LO}) - $signed({M_I0_MULTHF, M_I0_MULTLF});
+`endif
 
   myBuffer #(96) M_I0_MULT_buffer (
       clk, rst,
+`ifdef ENABLE_MADD
       M_I0_MULT_CNTR[0] & M.I0.MCtrl.MAS == 2'b00 | M_I0_MAS_bvalid & |M.I0.MCtrl.MAS,
       M.I0.MCtrl.MAS == 2'b00 ? {M_I0_MULTL, M_I0_MULTH,  M_I0_MULTUH}
                               : {M_I0_MASL, M_I0_MASH, M_I0_MUASH},
+`else
+      M_I0_MULT_CNTR[0],
+      {M_I0_MULTL, M_I0_MULTH,  M_I0_MULTUH},
+`endif
       M.en,
       M_I0_MULT_bvalid,
       {M_I0_MULTLB, M_I0_MULTHB, M_I0_MULTUHB}
@@ -1387,8 +1392,6 @@ module Datapath (
                                      : M.I1.Trap.TP == LT ? M.I1.ALUOut[0] ==  1'b1
                                      : M.I1.Trap.TP == GE ? M.I1.ALUOut[0] ==  1'b0
                                      : 1'b0);
-`else
-  assign M_I1_Trap = 1'b0;
 `endif
 
   assign M.en = M_go & W.en;
@@ -1458,7 +1461,7 @@ module Datapath (
       W.en,
       W.I0.RDataW
   );
-  ffenrc #(5 + 1) W_I0_WCtrl_ff (
+  ffenrc #(5 + $bits(M.I0.WCtrl)) W_I0_WCtrl_ff (
       clk,
       rst,
       {M.I0.RD, M.I0.WCtrl},
@@ -1472,7 +1475,7 @@ module Datapath (
       W.en,
       W.I1.RDataW
   );
-  ffenrc #(5 + 1) W_I1_WCtrl_ff (
+  ffenrc #(5 + $bits(M.I0.WCtrl)) W_I1_WCtrl_ff (
       clk,
       rst,
       {M.I1.RD, M.I1.WCtrl},
diff --git a/src/Core/Gadgets/decoder2.sv b/src/Core/Gadgets/decoder2.sv
index b23241b..11afed5 100644
--- a/src/Core/Gadgets/decoder2.sv
+++ b/src/Core/Gadgets/decoder2.sv
@@ -94,10 +94,12 @@ module decoder2 (
       32'b01000010000000000000000000000110: ri = 1'b0;  // TLBWR
       32'b01000010000000000000000000001000: ri = 1'b0;  // TLBP
       32'b01000010000000000000000000011000: ri = 1'b0;  // ERET
+`ifdef ENABLE_MADD
       32'b011100??????????0000000000000000: ri = 1'b0;  // MADD
       32'b011100??????????0000000000000001: ri = 1'b0;  // MADDU
       32'b011100??????????0000000000000100: ri = 1'b0;  // MSUB
       32'b011100??????????0000000000000101: ri = 1'b0;  // MSUBU
+`endif
       32'b011100???????????????00000000010: ri = 1'b0;  // MUL
       // 32'b01111100000??????????00000111011: begin cpu = 1'b1; ce = 2'b0; end // RDHWR (CpU)
       32'b100000??????????????????????????: ri = 1'b0;  // LB
diff --git a/src/include/defines.svh b/src/include/defines.svh
index 9042220..b58cc33 100644
--- a/src/include/defines.svh
+++ b/src/include/defines.svh
@@ -7,6 +7,7 @@
 `define ENABLE_TLB
 `define ENABLE_CpU
 `define ENABLE_TRAP
+`define ENABLE_MADD
 
 `ifdef SIMULATION_VERILATOR
   `undef ENABLE_CpU
@@ -136,7 +137,9 @@ typedef struct packed {
   logic [2:0] SEL;
   logic       C0W;    // critical
   HLS_t       HLS;
+`ifdef ENABLE_MADD
   MAS_t       MAS;
+`endif
 } MCtrl0_t;
 
 typedef struct packed {