Add fixed point corrector and use it

* It is the same corr_ll layout, but with fixed point
* Also align matmul output to DAC units (provided that matrix coeffs are
 in DAC/nm, signed fixed point with 30 decimals)

hdl/corr_iir.vhd

+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;
+
+
+
+

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;
 

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),

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.";