diff --git a/hdl/corr_ll.vhd b/hdl/corr_ll.vhd
index 69c33ecdb788210832d011bb0d88711e88c3b8cf..4b3447649dd4ba99b0d6953a6ac28147bcc1cef3 100644
--- a/hdl/corr_ll.vhd
+++ b/hdl/corr_ll.vhd
@@ -127,9 +127,9 @@ begin
end process;
-- resize
- rz_mult_a <= resize(mult_a, C_W_COR_SUMSAT);
- rz_mult_b <= resize(mult_b, C_W_COR_SUMSAT);
- rz_mult_d <= resize(mult_d, C_W_COR_SUMSAT);
+ rz_mult_a <= f_resize_sat(mult_a, C_W_COR_SUMSAT);
+ rz_mult_b <= f_resize_sat(mult_b, C_W_COR_SUMSAT);
+ rz_mult_d <= f_resize_sat(mult_d, C_W_COR_SUMSAT);
-- round then sat
rnd_abicd <= f_resize_lsb(mult_ic, rnd_abicd'length) when mult_ic(C_N_COR_RND-1) = '0' else
diff --git a/hdl/lvl_threshold.vhd b/hdl/lvl_threshold.vhd
new file mode 100644
index 0000000000000000000000000000000000000000..ca6ab610cda478b19bebbb119d73f4323d198c46
--- /dev/null
+++ b/hdl/lvl_threshold.vhd
@@ -0,0 +1,67 @@
+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_nor : 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_nor <= not 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_nor = '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;
+
diff --git a/hdl/top_corr_matrix.vhd b/hdl/top_corr_matrix.vhd
index 38ead473b35f122ec9c9d0a47ec2a27c081b1881..bcddbf7618a95cd0d0ba38019b10aaa672953c26 100644
--- a/hdl/top_corr_matrix.vhd
+++ b/hdl/top_corr_matrix.vhd
@@ -65,9 +65,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
@@ -175,7 +182,7 @@ begin
coef_d => signed(mm_a2l.CORR_K1D.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 => corrfirst_valid,
@@ -204,7 +211,7 @@ begin
coef_d => signed(mm_a2l.CORR_K2D.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,
@@ -212,6 +219,7 @@ begin
corrout => corrout
);
+ enable_corr <= mm_a2l.CONTROL.ENABLE_CORR.data(0) and not thresh_reached;
---------------------
-- DATA SERIALIZER --
@@ -234,10 +242,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.RST_THRESH.data(0),
+ thresh_reached => thresh_reached
);
+ mm_l2a.STATUS.THRESH_REACHED.data(0) <= thresh_reached;
+
+
+
+
end architecture;
diff --git a/rdl/corr_matrix.rdl b/rdl/corr_matrix.rdl
index f3c428ef0309ce4dd87c9ae997f30b02b1da849f..aea7b598c3b713933d2d264c6cb19b89738e863c 100644
--- a/rdl/corr_matrix.rdl
+++ b/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];
diff --git a/tcl/main.tcl b/tcl/main.tcl
index 15b89fbbddd532e85b68d52910fd49b24aa6d9f1..616b592835a2d81211c4a72d8233527f41534337 100644
--- a/tcl/main.tcl
+++ b/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"}