feat: Add output threshold protection

* A simple threhshold comparator on data output.
* When Thresh is reached, disable correction (keep value).
* User can unlock the situation via control bits.
* Same threshold for all pscid.

hdl/lvl_threshold.vhd

+library ieee;
+use ieee.std_logic_1164.all;
+use ieee.numeric_std.all;
+use ieee.std_logic_misc.and_reduce;
+use ieee.std_logic_misc.or_reduce;
+
+use work.pkg_corr_matrix.all;
+
+entity lvl_threshold is
+    generic(
+        G_N_BTH             : natural
+    );
+    port(
+        clk                 : in std_logic;
+        rst_n               : in std_logic;
+
+        s_axis_tdata_cor    : in std_logic_vector(C_W_COR-1 downto 0);
+        s_axis_tvalid       : in std_logic;
+
+        enable_thresh       : in std_logic;
+        rst_thresh          : in std_logic;
+        thresh_reached      : out std_logic
+    );
+end entity lvl_threshold;
+
+
+architecture rtl of lvl_threshold is
+
+    signal sign         : std_logic;
+    signal thresh_and   : std_logic;
+    signal thresh_or    : std_logic;
+
+begin
+
+    sign        <= s_axis_tdata_cor(C_W_COR-1);
+    thresh_and  <= and_reduce(s_axis_tdata_cor(C_W_COR-2 downto C_W_COR-G_N_BTH-1));
+    thresh_or   <= or_reduce(s_axis_tdata_cor(C_W_COR-2 downto C_W_COR-G_N_BTH-1));
+
+    p_main:process(clk, rst_n)
+    begin
+
+        if rst_n = '0' then
+            thresh_reached <= '0';
+
+        elsif rising_edge(clk) then
+            if rst_thresh = '1' then
+                thresh_reached <= '0';
+            else
+                if s_axis_tvalid = '1' and enable_thresh = '1' then
+                    if  sign = '1'then
+                        -- negative
+                        if thresh_or = '1' then
+                            thresh_reached <= '1';
+                        end if;
+                    else
+                        -- positive
+                        if thresh_and = '1' then
+                            thresh_reached <= '1';
+                        end if;
+                    end if;
+                end if;
+            end if;
+        end if;
+    end process;
+
+end architecture;
+

hdl/top_corr_matrix.vhd

@@ -61,9 +61,16 @@ architecture struct of top_corr_matrix is
     signal corrout_valid   : std_logic;
     signal corrout_seq     : std_logic_vector(C_W_BPMSEQ-1 downto 0);
     signal corrout         : signed_array(0 to C_N_MM_PSC-1)(C_W_COR-1 downto 0);
+    signal enable_corr     : std_logic;
 
     -- Serializer
     signal overrun_flag     : std_logic;
+    signal ser_tdata        : std_logic_vector(C_W_COR+C_W_PSCID-1 downto 0);
+    signal ser_tuser        : std_logic_vector(C_W_BPMSEQ-1 downto 0);
+    signal ser_tvalid       : std_logic;
+
+    -- Threshold
+    signal thresh_reached   : std_logic;
 
 
 begin
@@ -171,7 +178,7 @@ begin
         coef_d          => signed(mm_a2l.CORR_KD.data.data),
 
         reset_corr      => mm_a2l.CONTROL.RST_CORR.data(0),
-        enable_corr     => mm_a2l.CONTROL.ENABLE_CORR.data(0),
+        enable_corr     => enable_corr,
 
         -- Corr output
         corrout_valid   => corrout_valid,
@@ -179,6 +186,8 @@ begin
         corrout         => corrout
     );
 
+    enable_corr    <= mm_a2l.CONTROL.ENABLE_CORR.data(0) and not thresh_reached;
+
     ---------------------
     -- DATA SERIALIZER --
     ---------------------
@@ -200,10 +209,38 @@ begin
         corrout         => corrout,
 
         -- AXIS serial output
-        m_axis_tdata    => m_axis_tdata,
-        m_axis_tuser    => m_axis_tuser,
-        m_axis_tvalid   => m_axis_tvalid
+        m_axis_tdata    => ser_tdata,
+        m_axis_tuser    => ser_tuser,
+        m_axis_tvalid   => ser_tvalid
     );
 
+    m_axis_tdata    <= ser_tdata;
+    m_axis_tuser    <= ser_tuser;
+    m_axis_tvalid   <= ser_tvalid;
+
+    --------------------------
+    -- THRESHOLD LEVEL COMP --
+    --------------------------
+    inst_thresh: entity work.lvl_threshold
+    generic map(
+        G_N_BTH     => 10  -- 99.9 % of range
+    )
+    port map(
+        clk         => clk,
+        rst_n       => rst_n,
+
+        s_axis_tdata_cor    => ser_tdata(C_W_COR-1 downto 0),
+        s_axis_tvalid       => ser_tvalid,
+
+        enable_thresh       => mm_a2l.CONTROL.ENABLE_THRESH.data(0),
+        rst_thresh          => mm_a2l.CONTROL.ENABLE_THRESH.data(0),
+        thresh_reached      => thresh_reached
+    );
+
+
+    mm_l2a.STATUS.THRESH_REACHED.data(0) <= thresh_reached;
+
+
+
 
 end architecture;

rdl/corr_matrix.rdl

@@ -29,10 +29,19 @@ addrmap corr_matrix {
 
     reg {
         desc="Global control of the corrector.";
-        field {sw = rw; hw = r;} ENABLE_CORR;
-        field {sw = rw; hw = r;} RST_CORR;
+        default sw=rw; default hw=r;
+        field {} ENABLE_CORR;
+        field {} RST_CORR;
+        field {} ENABLE_THRESH;
+        field {} RST_THRESH;
     } CONTROL;
 
+    reg {
+        desc="Global status of the corrector.";
+        default sw=r; default hw=rw;
+        field {} THRESH_REACHED;
+    } STATUS;
+
     reg {
         desc="Correction coefficient A.";
         field {sw = rw; hw = r;} data[`C_W_COR_COEF];

tcl/main.tcl

@@ -20,6 +20,7 @@ proc setSources {} {
   lappend Sources {"../hdl/pkg_corr_matrix_version.vhd" "VHDL"}
   lappend Sources {"../hdl/pkg_corrmatrix.vhd" "VHDL 2008"}
   lappend Sources {"../hdl/corr_ll.vhd" "VHDL 2008"}
+  lappend Sources {"../hdl/lvl_threshold.vhd" "VHDL 2008"}
   lappend Sources {"../hdl/matrix_mul.vhd" "VHDL 2008"}
   lappend Sources {"../hdl/orbit_error.vhd" "VHDL 2008"}
   lappend Sources {"../hdl/data_serializer.vhd" "VHDL 2008"}