diff --git a/hdl/corr_iir.vhd b/hdl/corr_iir.vhd
new file mode 100644
index 0000000000000000000000000000000000000000..4981a40d42048fb9a583a2236657f5ca63d4d0d6
--- /dev/null
+++ b/hdl/corr_iir.vhd
@@ -0,0 +1,234 @@
+library ieee;
+use ieee.std_logic_1164.all;
+use ieee.numeric_std.all;
+
+library desy;
+use desy.math_signed.all;
+
+use work.pkg_corr_matrix.all;
+
+entity corr_iir is
+ port(
+ clk : in std_logic;
+ rst_n : in std_logic;
+
+
+ coef_a_x : in signed(C_W_COR_COEF-1 downto 0);
+ coef_b_x : in signed(C_W_COR_COEF-1 downto 0);
+ coef_c_x : in signed(C_W_COR_COEF-1 downto 0);
+ coef_d_x : in signed(C_W_COR_COEF-1 downto 0);
+ coef_a_y : in signed(C_W_COR_COEF-1 downto 0);
+ coef_b_y : in signed(C_W_COR_COEF-1 downto 0);
+ coef_c_y : in signed(C_W_COR_COEF-1 downto 0);
+ coef_d_y : in signed(C_W_COR_COEF-1 downto 0);
+
+ enable_corr_x : in std_logic;
+ reset_corr_x : in std_logic;
+ enable_corr_y : in std_logic;
+ reset_corr_y : in std_logic;
+
+ matmult : in signed_array(0 to C_N_MM_PSC-1)(C_W_MM-1 downto 0);
+ matmult_valid : in std_logic;
+ matmult_seq : in std_logic_vector(C_W_BPMSEQ-1 downto 0);
+
+ corrout_valid : out std_logic;
+ corrout_seq : out std_logic_vector(C_W_BPMSEQ-1 downto 0);
+ corrout : out signed_array(0 to C_N_MM_PSC-1)(C_W_COR-1 downto 0)
+ );
+end entity corr_ll;
+
+
+architecture rtl of corr_ll is
+
+ constant C_DELAY : natural := 6;
+
+ constant C_W_MULT : natural := C_W_MM+C_W_COR_COEF;
+ constant C_W_CORFP : natural := C_W_COR+C_W_FP;
+
+ type arr_slv is array (natural range <>) of std_logic_vector;
+ signal delay_valid : std_logic_vector(C_DELAY-1 downto 0);
+ signal delay_seq : arr_slv(0 to C_DELAY-1)(C_W_BPMSEQ-1 downto 0);
+
+
+begin
+
+ ----------------------
+ -- OUTPUT CONNEXION --
+ ----------------------
+ corrout_valid <= delay_valid(delay_valid'left);
+ corrout_seq <= delay_seq(C_DELAY-1);
+
+ --------------------
+ -- DELAY REGISTER --
+ --------------------
+ p_delay:process(clk,rst_n)
+ begin
+ if rst_n = '0' then
+ delay_valid <= (others => '0');
+ delay_seq <= (others => (others => '0'));
+ elsif rising_edge(clk) then
+ delay_valid(0) <= matmult_valid;
+ delay_valid(delay_valid'left downto 1) <= delay_valid(delay_valid'left-1 downto 0);
+
+ delay_seq(0) <= matmult_seq;
+ for I in 1 to C_DELAY-1 loop
+ delay_seq(I) <= delay_seq(I-1);
+ end loop;
+ end if;
+ end process;
+
+ --------------------
+ -- CORRECTOR LINE --
+ --------------------
+ gen_corr:for I in 0 to C_N_MM_PSC-1 generate
+
+ signal mult_a : signed(C_W_MULT-1 downto 0);
+ signal mult_b : signed(C_W_MULT-1 downto 0);
+ signal mult_c : signed(C_W_MULT-1 downto 0);
+ signal mult_d : signed(C_W_CORFP+C_W_COR_COEF-1 downto 0);
+
+ signal sumsat_abd : signed(C_W_MM-1 downto 0);
+ signal reg_dout : signed(C_W_CORFP-1 downto 0);
+ signal reg_din : signed(C_W_MM-1 downto 0);
+ signal rnd_cor : signed(C_W_COR-1 downto 0);
+
+ signal rndsat_mult_a : signed(C_W_MM-1 downto 0);
+ signal rndsat_mult_b : signed(C_W_MM-1 downto 0);
+ signal rndsat_mult_d : signed(C_W_CORFP+C_W_COR_COEF-C_W_COR_COEFFP-1 downto 0);
+
+ signal coef_a : signed(C_W_COR_COEF-1 downto 0);
+ signal coef_b : signed(C_W_COR_COEF-1 downto 0);
+ signal coef_c : signed(C_W_COR_COEF-1 downto 0);
+ signal coef_d : signed(C_W_COR_COEF-1 downto 0);
+
+ signal enable_corr : std_logic;
+ signal reset_corr : std_logic;
+
+ begin
+
+ -- Signal rails for X/Y coef and enable/reset, based on PSCID number
+ gen_x_sigrail:if I < 50 generate
+ coef_a <= coef_a_x;
+ coef_b <= coef_b_x;
+ coef_c <= coef_c_x;
+ coef_d <= coef_d_x;
+ enable_corr <= enable_corr_x;
+ reset_corr <= reset_corr_x;
+ end generate;
+
+ gen_y_sigrail:if I > 49 generate
+ coef_a <= coef_a_y;
+ coef_b <= coef_b_y;
+ coef_c <= coef_c_y;
+ coef_d <= coef_d_y;
+ enable_corr <= enable_corr_y;
+ reset_corr <= reset_corr_y;
+ end generate;
+
+ p_main:process(clk, rst_n)
+ begin
+ if rst_n = '0' then
+ mult_a <= (others => '0');
+ mult_b <= (others => '0');
+ mult_c <= (others => '0');
+ mult_d <= (others => '0');
+ sumsat_abd <= (others => '0');
+ reg_din <= (others => '0');
+ reg_dout <= (others => '0');
+ sat_cor <= (others => '0');
+ rndsat_mult_a <= (others => '0');
+ rndsat_mult_b <= (others => '0');
+ rndsat_mult_d <= (others => '0');
+ elsif rising_edge(clk) then
+
+ ----------------------------------------------------------------------------------------------------------
+ -- Sequence registers (IIR z-1 steps)
+ if reset_corr = '1' then
+ reg_din <= (others => '0');
+ reg_dout <= (others => '0');
+ else
+ if matmult_valid = '1' then
+ reg_din <= matmult(I);
+ end if;
+
+ if delay_valid(delay_valid'left-2) = '1' and enable_corr = '1' then
+
+ -- Round then sat
+ if mult_c(C_W_COR_COEFFP-1) = '0' then
+ reg_out <= f_resize_sat(
+ f_resize_lsb(mult_c, mult_c'length-C_W_COR_COEFFP),
+ C_W_CORFP);
+ else
+ reg_out <= f_resize_sat(
+ f_sum_sat(f_resize_lsb(mult_c, mult_c'length-C_W_COR_COEFFP), to_signed(1,2)),
+ C_W_CORFP);
+ end if;
+ end if;
+ end if;
+
+ ----------------------------------------------------------------------------------------------------------
+ -- Mutlipliers
+ mult_a <= matmult(I) * coef_a;
+ mult_b <= reg_din * coef_b;
+ mult_d <= reg_dout * coef_d;
+ mult_c <= sumsat_abd * coef_c;
+
+
+ ----------------------------------------------------------------------------------------------------------
+ -- Round and resize
+ if mult_a(C_W_COR_COEFFP-1) = '0' then
+ rndsat_mult_a <= f_resize_sat(
+ f_resize_lsb(mult_a, mult_a'length-C_W_COR_COEFFP),
+ C_W_MM);
+ else
+ rndsat_mult_a <= f_resize_sat(
+ f_sum_sat(f_resize_lsb(mult_a, mult_a'length-C_W_COR_COEFFP), to_signed(1,2)),
+ C_W_MM);
+ end if;
+
+ if mult_b(C_W_COR_COEFFP-1) = '0' then
+ rndsat_mult_b <= f_resize_sat(
+ f_resize_lsb(mult_b, mult_b'length-C_W_COR_COEFFP),
+ C_W_MM);
+ else
+ rndsat_mult_b <= f_resize_sat(
+ f_sum_sat(f_resize_lsb(mult_b, mult_b'length-C_W_COR_COEFFP), to_signed(1,2)),
+ C_W_MM);
+ end if;
+
+ if mult_d(C_W_COR_COEFFP-1) = '0' then
+ rndsat_mult_d <= f_resize_sat(
+ f_resize_lsb(mult_d, mult_d'length-C_W_COR_COEFFP),
+ rndsat_mult_d'length);
+ else
+ rndsat_mult_d <= f_resize_sat(
+ f_sum_sat(f_resize_lsb(mult_d, mult_d'length-C_W_COR_COEFFP), to_signed(1,2)),
+ rndsat_mult_d'length);
+ end if;
+
+ ----------------------------------------------------------------------------------------------------------
+ -- global sum
+ sumsat_abd <= f_sum_sat(f_sum_sat(rndsat_mult_a, rndsat_mult_b), rndsat_mult_d);
+
+ ----------------------------------------------------------------------------------------------------------
+ -- Output rounding
+ if reg_dout(C_W_FP-1) = '0' then
+ sat_cor <= f_resize_lsb(reg_dout, C_W_COR);
+ else
+ sat_cor <= f_sum_sat(f_resize_lsb(reg_dout, C_W_COR), to_signed(1,2));
+ end if;
+
+
+ end if;
+ end process;
+
+ -- mapping
+ corrout(I) <= sat_cor;
+
+ end generate gen_corr;
+
+end architecture;
+
+
+
+
diff --git a/hdl/pkg_corrmatrix.vhd b/hdl/pkg_corrmatrix.vhd
index bd5305b5a6f755a2fb0c83149d687ee752b06131..7c0ddb82319b98bcf7504a702efe89160e7b2ec4 100644
--- a/hdl/pkg_corrmatrix.vhd
+++ b/hdl/pkg_corrmatrix.vhd
@@ -32,14 +32,14 @@ package pkg_corr_matrix is
constant C_W_MM_IDCNT : natural := 8; -- > natural(ceil(log2(real(C_N_MM_BPM))));
constant C_W_MM_ACCU : natural := 58; -- C_W_MM_MULT+C_W_MM_IDCNT;
- constant C_N_MM_SAT : natural := 7;
- constant C_N_MM_RND : natural := 15;
+ constant C_N_MM_SAT : natural := 8;
+ constant C_N_MM_RND : natural := 14;
constant C_W_MM : natural := 36; --C_W_MM_ACCU-C_N_MM_SAT-C_N_MM_RND
+ constant C_W_FP : natural := 16; -- decimal bits of the matmul fixed point
- -- PI corrector
- constant C_W_COR_COEF : natural := 17;
- constant C_W_COR_SUMSAT : natural := 36;
- constant C_N_COR_RND : natural := 20;
+ -- IIR corrector
+ constant C_W_COR_COEF : natural := 16;
+ constant C_W_COR_COEFFP : natural := 14; -- decimal bits of the coeff fixed point
constant C_W_COR : natural := 16;
diff --git a/hdl/top_corr_matrix.vhd b/hdl/top_corr_matrix.vhd
index 19fc100b1f95b8befa4d57c4f4a49f216556df1d..ea02fae48a1486b53982afebb90d8cee1d2cf64d 100644
--- a/hdl/top_corr_matrix.vhd
+++ b/hdl/top_corr_matrix.vhd
@@ -223,9 +223,9 @@ begin
);
------------------
- -- LL CORRECTOR --
+ -- IIR CORRECTOR --
------------------
- inst_corr: entity work.corr_ll
+ inst_corr: entity work.corr_iir
port map(
clk => clk,
rst_n => rst_n,
@@ -238,11 +238,11 @@ begin
-- Corr coefs
coef_a_x => signed(mm_a2l.CORR_K1A_X.data.data),
coef_b_x => signed(mm_a2l.CORR_K1B_X.data.data),
- coef_ic_x => signed(mm_a2l.CORR_K1IC_X.data.data),
+ coef_c_x => signed(mm_a2l.CORR_K1C_X.data.data),
coef_d_x => signed(mm_a2l.CORR_K1D_X.data.data),
coef_a_y => signed(mm_a2l.CORR_K1A_Y.data.data),
coef_b_y => signed(mm_a2l.CORR_K1B_Y.data.data),
- coef_ic_y => signed(mm_a2l.CORR_K1IC_Y.data.data),
+ coef_c_y => signed(mm_a2l.CORR_K1C_Y.data.data),
coef_d_y => signed(mm_a2l.CORR_K1D_Y.data.data),
reset_corr_x => mm_a2l.CONTROL_X.RST_CORR.data(0),
@@ -260,7 +260,7 @@ begin
corrfirst_resize(I) <= resize(corrfirst(I), C_W_MM);
end generate;
- inst_corr2: entity work.corr_ll
+ inst_corr2: entity work.corr_iir
port map(
clk => clk,
rst_n => rst_n,
@@ -273,11 +273,11 @@ begin
-- Corr coefs
coef_a_x => signed(mm_a2l.CORR_K2A_X.data.data),
coef_b_x => signed(mm_a2l.CORR_K2B_X.data.data),
- coef_ic_x => signed(mm_a2l.CORR_K2IC_X.data.data),
+ coef_c_x => signed(mm_a2l.CORR_K2C_X.data.data),
coef_d_x => signed(mm_a2l.CORR_K2D_X.data.data),
coef_a_y => signed(mm_a2l.CORR_K2A_Y.data.data),
coef_b_y => signed(mm_a2l.CORR_K2B_Y.data.data),
- coef_ic_y => signed(mm_a2l.CORR_K2IC_Y.data.data),
+ coef_c_y => signed(mm_a2l.CORR_K2C_Y.data.data),
coef_d_y => signed(mm_a2l.CORR_K2D_Y.data.data),
reset_corr_x => mm_a2l.CONTROL_X.RST_CORR.data(0),
diff --git a/rdl/corr_matrix.rdl b/rdl/corr_matrix.rdl
index 866206bc7945aee16fa1e8a757b303e0750d3ee9..1a5f91c54f275910efc5345f34d1b82c65401e99 100644
--- a/rdl/corr_matrix.rdl
+++ b/rdl/corr_matrix.rdl
@@ -67,10 +67,10 @@ addrmap corr_matrix {
} CORR_K1B_X;
reg {
- desc="Correction coefficient (inverse) C.";
+ desc="Correction coefficient C.";
desyrdl_data_type="int32";
field {sw = rw; hw = r;} data[`C_W_COR_COEF];
- } CORR_K1IC_X;
+ } CORR_K1C_X;
reg {
desc="Correction coefficient D.";
@@ -92,10 +92,10 @@ addrmap corr_matrix {
} CORR_K2B_X;
reg {
- desc="Correction coefficient (inverse) C.";
+ desc="Correction coefficient C.";
desyrdl_data_type="int32";
field {sw = rw; hw = r;} data[`C_W_COR_COEF];
- } CORR_K2IC_X;
+ } CORR_K2C_X;
reg {
desc="Correction coefficient D.";
@@ -116,10 +116,10 @@ addrmap corr_matrix {
} CORR_K1B_Y;
reg {
- desc="Correction coefficient (inverse) C.";
+ desc="Correction coefficient C.";
desyrdl_data_type="int32";
field {sw = rw; hw = r;} data[`C_W_COR_COEF];
- } CORR_K1IC_Y;
+ } CORR_K1C_Y;
reg {
desc="Correction coefficient D.";
@@ -141,10 +141,10 @@ addrmap corr_matrix {
} CORR_K2B_Y;
reg {
- desc="Correction coefficient (inverse) C.";
+ desc="Correction coefficient C.";
desyrdl_data_type="int32";
field {sw = rw; hw = r;} data[`C_W_COR_COEF];
- } CORR_K2IC_Y;
+ } CORR_K2C_Y;
reg {
desc="Correction coefficient D.";