1 files changed, 35 insertions, 4 deletions
diff --git a/usrp2/sdr_lib/tx_frontend.v b/usrp2/sdr_lib/tx_frontend.v
index 2817c1510..82476ad0d 100644
--- a/usrp2/sdr_lib/tx_frontend.v
+++ b/usrp2/sdr_lib/tx_frontend.v
@@ -12,6 +12,9 @@ module tx_frontend
    wire [23:0] i_dco, q_dco, i_ofs, q_ofs;
    wire [15:0] i_final, q_final;
    wire [7:0]  mux_ctrl;
+   wire [35:0] corr_i, corr_q;
+   wire [23:0] i_bal, q_bal;
+   wire [17:0] mag_corr, phase_corr;
    
    setting_reg #(.my_addr(BASE+0), .width(24)) sr_0
      (.clk(clk),.rst(rst),.strobe(set_stb),.addr(set_addr),
@@ -21,22 +24,50 @@ module tx_frontend
      (.clk(clk),.rst(rst),.strobe(set_stb),.addr(set_addr),
       .in(set_data),.out(q_dco),.changed());
 
-   setting_reg #(.my_addr(BASE+2), .width(4)) sr_2
+   setting_reg #(.my_addr(BASE+2),.width(18)) sr_2
+     (.clk(clk),.rst(rst),.strobe(set_stb),.addr(set_addr),
+      .in(set_data),.out(mag_corr),.changed());
+   
+   setting_reg #(.my_addr(BASE+3),.width(18)) sr_3
+     (.clk(clk),.rst(rst),.strobe(set_stb),.addr(set_addr),
+      .in(set_data),.out(phase_corr),.changed());
+  
+   setting_reg #(.my_addr(BASE+4), .width(8)) sr_4
      (.clk(clk),.rst(rst),.strobe(set_stb),.addr(set_addr),
       .in(set_data),.out(mux_ctrl),.changed());
 
+   // IQ Balance
+   MULT18X18S mult_mag_corr
+     (.P(corr_i), .A(tx_i[23:6]), .B(mag_corr), .C(clk), .CE(1), .R(rst) ); 
+   
+   MULT18X18S mult_phase_corr
+     (.P(corr_q), .A(tx_i[23:6]), .B(phase_corr), .C(clk), .CE(1), .R(rst) );
+   
+   add2_and_clip_reg #(.WIDTH(24)) add_clip_i
+     (.clk(clk), .rst(rst), 
+      .in1(tx_i), .in2({{4{corr_i[35]}},corr_i[35:16]}), .strobe_in(1'b1), 
+      .sum(i_bal), .strobe_out());
+   
+   add2_and_clip_reg #(.WIDTH(24)) add_clip_q
+     (.clk(clk), .rst(rst), 
+      .in1(tx_q), .in2({{4{corr_q[35]}},corr_q[35:16]}), .strobe_in(1'b1),
+      .sum(q_bal), .strobe_out());
+
+   // DC Offset
    add2_and_clip_reg #(.WIDTH(24)) add_dco_i
-     (.clk(clk), .rst(rst), .in1(i_dco), .in2(tx_i), .strobe_in(1'b1), .sum(i_ofs), .strobe_out());
+     (.clk(clk), .rst(rst), .in1(i_dco), .in2(i_bal), .strobe_in(1'b1), .sum(i_ofs), .strobe_out());
    
    add2_and_clip_reg #(.WIDTH(24)) add_dco_q
-     (.clk(clk), .rst(rst), .in1(q_dco), .in2(tx_q), .strobe_in(1'b1), .sum(q_ofs), .strobe_out());
-   
+     (.clk(clk), .rst(rst), .in1(q_dco), .in2(q_bal), .strobe_in(1'b1), .sum(q_ofs), .strobe_out());
+
+   // Rounding
    round_sd #(.WIDTH_IN(24),.WIDTH_OUT(16)) round_i
      (.clk(clk), .reset(rst), .in(i_ofs),.strobe_in(1'b1), .out(i_final), .strobe_out());
 
    round_sd #(.WIDTH_IN(24),.WIDTH_OUT(16)) round_q
      (.clk(clk), .reset(rst), .in(q_ofs),.strobe_in(1'b1), .out(q_final), .strobe_out());
 
+   // Mux
    always @(posedge clk)
      case(mux_ctrl[3:0])
        0 : dac_a <= i_final;