diff --git a/hdl/lvl_threshold.vhd b/hdl/lvl_threshold.vhd
new file mode 100644
index 0000000000000000000000000000000000000000..b39b3340bc247928256f98c84c1e93705c2f0340
--- /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_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;
+
diff --git a/hdl/top_corr_matrix.vhd b/hdl/top_corr_matrix.vhd
index 74443117753207976df2b4355efdaf5a055a6682..4f005572a5be4c486782756e35eb3d108239342c 100644
--- a/hdl/top_corr_matrix.vhd
+++ b/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;
diff --git a/rdl/corr_matrix.rdl b/rdl/corr_matrix.rdl
index 0feacce2082719836ec2e8e325e69e8d4d8a0da6..73487b869bc0f63fc3ee6cc29ad9b4728ff13bc2 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"}