-------------------------------------------------------------------------------
--
-- Project name: JESD204b Receiver core 
--
-- (C) Copyright 2017 DFC Design, s.r.o., Brno, Czech Republic
-- Author: Marek Kvas (m.kvas@dfcdesign.cz)
--
-------------------------------------------------------------------------------
--
-- One JESD 204b Lane implementation.
-- Input is supposed to come from Xilinx JESD204b PHY core, or similar
-- transceiver wrapping core.
-- Data path is 32 bit wide. Bit level alignment and 8b/10b decoding is supposed
-- to be done in PHY core. Character realignment is enabled by Lane when
-- synchronization problem is detected.
-- 
-- ILAS configuraton data are captured to separate register and provided via
-- its own port.
--          
-- The core must be provided with correct K and F values. Product K x F
-- as a multiframe length must be also provided externally.
--
-- Monitoring counters are reset when all core is reset.
--
-- 
--
-------------------------------------------------------------------------------


library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
--use work.tb_print_pkg.all;

entity jesd204b_rx_lane is 
   generic (
      g_stat_cnt_length    : integer := 16;
      g_scrambler_incl     : boolean := true;
      g_lmfc_buffer_size   : integer := 1024
           );
   port (
      CLK                  : in  std_logic;
      RST                  : in  std_logic;
   
      -- JESD204B control signals
      CTL_REINIT           : in  std_logic;
      CTL_CGS_DONE         : out std_logic;
      CTL_REINIT_REQ       : out std_logic;
      CTL_ERR_REPORT_REQ   : out std_logic;
      CTL_IFA_DONE         : out std_logic;
      CTL_RELEASE_DATA     : in  std_logic;
      CTL_ILAS_ERR         : out std_logic;
      CTL_ERR_OVERFLOW     : out std_logic;
      CTL_DATA_READY       : out std_logic;

      -- Connection to PHY (RX transceivers)
      PHY_DATA             : in  std_logic_vector(31 downto 0);
      PHY_CHARISK          : in  std_logic_vector(3 downto 0);
      PHY_DISPERR          : in  std_logic_vector(3 downto 0);
      PHY_NOTINTABLE       : in  std_logic_vector(3 downto 0);
      PHY_COMMA_ALIGN_EN   : out std_logic;
      
      -- Output AXI4-Stream interface
      OUT_M_AXIS_TVALID    : out std_logic;
      OUT_M_AXIS_TDATA     : out std_logic_vector(31 downto 0);
      OUT_SOF              : out std_logic_vector(3 downto 0);
      OUT_EOF              : out std_logic_vector(3 downto 0);
      OUT_SOMF             : out std_logic_vector(3 downto 0);
      OUT_EOMF             : out std_logic_vector(3 downto 0);
      OUT_BYTE_ERR         : out std_logic_vector(3 downto 0);
      OUT_ILAS_CFG_DATA    : out std_logic_vector(16*8 -1 downto 0);
      OUT_ILAS_CFG_DONE    : out std_logic;
      
      -- Configurations
      CFG_SCRAMBLING_EN    : in  std_logic;

      -- Both contain K/F -1 value as 0 is meaningless
      -- FxK is helper value - number of bytes in multiframe (exact no -1)
      CFG_F                : in  std_logic_vector(7 downto 0);
      --CFG_K                : in  std_logic_vector(4 downto 0);
      CFG_FxK              : in  std_logic_vector(10 downto 0);

      CFG_ILAS             : in  std_logic;

      -- Monitoring - error, statistics
      MON_RST              : in  std_logic;
      MON_PHYS_ERR_CNT     : out std_logic_vector(g_stat_cnt_length -1 downto 0);
      MON_PHYS_ERR_RES_CNT : out std_logic_vector(g_stat_cnt_length -1 downto 0);
      MON_ALIGN_F_ERR_CNT  : out std_logic_vector(g_stat_cnt_length -1 downto 0);
      MON_ALIGN_MF_ERR_CNT : out std_logic_vector(g_stat_cnt_length -1 downto 0);
      MON_REPLACE_F_CNT    : out std_logic_vector(g_stat_cnt_length -1 downto 0);
      MON_REPLACE_A_CNT    : out std_logic_vector(g_stat_cnt_length -1 downto 0);
      MON_UNKNOWN_K_CNT    : out std_logic_vector(g_stat_cnt_length -1 downto 0);
      MON_ILAS_ERR_CNT     : out std_logic_vector(g_stat_cnt_length -1 downto 0)
        );
end entity;



architecture synthesis of jesd204b_rx_lane is

   constant LC_K                 : std_logic_vector(7 downto 0) := x"BC";
   constant LC_R                 : std_logic_vector(7 downto 0) := x"1C";
   constant LC_Q                 : std_logic_vector(7 downto 0) := x"9C";
   constant LC_A                 : std_logic_vector(7 downto 0) := x"7C";
   constant LC_F                 : std_logic_vector(7 downto 0) := x"FC";

   constant LC_K_WORD            : std_logic_vector(31 downto 0) :=
                                    LC_K & LC_K & LC_K & LC_K;
   constant LC_ILAS_CNT          : integer := 4;
   constant LC_ILAS_CFG_NO       : integer := 2;
   constant LC_ILAS_CFG_DATA_CNT : integer := 
                                       OUT_ILAS_CFG_DATA'length/PHY_DATA'length;

   constant LC_CS_ERR_THRESHOLD  : integer := 3;

   -- Count  number of ones in given vector
   function count_ones (vec : std_logic_vector) return integer is
      variable res : integer range 0 to vec'length := 0;
   begin
      for i in vec'range loop
         if vec(i) = '1' then
            res := res + 1;
         end if;
      end loop;
      return res;
   end function;

   -- If there is any gap of four or more consecutive zeroes in the vector
   -- return true.
   function four_cons_zeros (vec : std_logic_vector) return boolean is
   begin
      for i in 0 to vec'length -4 loop
         if vec(i+3 downto i) = "0000" then
            return true;
         end if;
      end loop;
      return false;
   end function;

   -- Detect any control character
   function det_char (data : std_logic_vector; 
                   chisk : std_logic_vector;
                   char : std_logic_vector) return std_logic_vector is
      variable res : std_logic_vector(chisk'range) := (others => '0');
   begin
      for i in chisk'range loop
         if chisk(i) = '1' and data(8*(i+1)-1 downto 8*i) = char then
            res(i) := '1';
         end if;
      end loop;
      return res;
   end function;

   -- Detect K that is /K/ as defined by JESD i.e. /K28.5/
   function det_K (data : std_logic_vector; 
                   chisk : std_logic_vector) return std_logic_vector is

   begin
      return det_char(data, chisk, LC_K);
   end function;

   -- Detect /A/
   function det_A (data : std_logic_vector; 
                   chisk : std_logic_vector) return std_logic_vector is

   begin
      return det_char(data, chisk, LC_A);
   end function;

   -- Detect /Q/
   function det_Q (data : std_logic_vector; 
                   chisk : std_logic_vector) return std_logic_vector is

   begin
      return det_char(data, chisk, LC_Q);
   end function;
 
   -- Detect /R/
   function det_R (data : std_logic_vector; 
                   chisk : std_logic_vector) return std_logic_vector is

   begin
      return det_char(data, chisk, LC_R);
   end function;

   -- Detect /F/
   function det_F (data : std_logic_vector; 
                   chisk : std_logic_vector) return std_logic_vector is

   begin
      return det_char(data, chisk, LC_F);
   end function;
   

   -- Skip pos-1 number of elements and return following 4 elements.
   -- Element is ves'length /8 long
   -- It is assumed pos is 0 to 3. and vec lenght 8 or 64 bits
   function align(pos : integer; vec : std_logic_vector) 
         return std_logic_vector is
      variable idx : integer range 0 to 3 := pos;
      variable es  : integer := vec'length/8;
      variable res : std_logic_vector(4*es - 1 downto 0) := (others => '0');
      variable a,b : integer;
   begin
      a := (idx + 4)*es -1;
      b := idx*es;
      --report ">>>> " & integer'image(a) & " " & integer'image(b) & " " & integer'image(es) & " " & integer'image(pos);
      --res := vec(b + 4*es -1 downto b);
      for i in 0 to 4*es -1 loop
         res(i) := vec(i + b);
      end loop;
      return res;
   end function;

   -- Return position of the first zero bit
   -- It searches from 0 to 3
   function first_zero(vec : std_logic_vector) return integer is
   begin
      for i in 0 to vec'left loop
         if vec(i) = '0' then
            return i;
         end if;
      end loop;
      return 0;
   end function;

   function vec_or(vec : std_logic_vector) return std_logic is
   begin
      if unsigned(vec) /= 0 then
         return '1';
      else
         return '0';
      end if;
   end function;

   -- Return byte at postion
   function slice_b(pos : integer; vec : std_logic_vector) 
         return std_logic_vector is
      variable res : std_logic_vector(7 downto 0);
   begin
      res := vec(8 * (pos + 1) - 1 downto 8 * pos);
      return res; 
   end function;

   -- Return empty vector to be used to get range for left hand part
   -- of expressin e.g. a(slice_b(2)'range) <= x"AA" equals
   -- a(15 downto 8) <= x"AA".
   function slice_b(pos : integer) 
         return std_logic_vector is
      variable res : std_logic_vector((pos+1)*8 - 1 downto pos*8) 
            := (others => '0');
   begin
      return res; 
   end function;

   signal cs_icnt                : integer range 0 to LC_CS_ERR_THRESHOLD + 4;

   signal cfg_f_init             : unsigned(CFG_F'range);
   signal cfg_fxk_init           : unsigned(CFG_FxK'range);

   signal phy_data_d             : std_logic_vector(PHY_DATA'range);
   signal phy_data_dd            : std_logic_vector(PHY_DATA'range);
   signal phy_chisk_d            : std_logic_vector(PHY_CHARISK'range);
   signal phy_chisk_dd           : std_logic_vector(PHY_CHARISK'range);
   signal phy_invalid_d          : std_logic_vector(PHY_DISPERR'range);
   signal phy_invalid_dd         : std_logic_vector(PHY_DISPERR'range);

   signal mon_phys_err_det       : std_logic;
   signal mon_align_f_err_cnt_i  : unsigned(MON_ALIGN_F_ERR_CNT'range);
   signal mon_align_mf_err_cnt_i : unsigned(MON_ALIGN_MF_ERR_CNT'range);
   signal mon_replace_f_cnt_i    : unsigned(MON_REPLACE_F_CNT'range);
   signal mon_replace_a_cnt_i    : unsigned(MON_REPLACE_A_CNT'range);
   signal mon_phys_err_cnt_i     : unsigned(MON_PHYS_ERR_CNT'range);
   signal mon_phys_err_res_cnt_i : unsigned(MON_PHYS_ERR_RES_CNT'range);
   signal mon_unknown_k_cnt_i    : unsigned(MON_UNKNOWN_K_CNT'range);
   signal mon_ilas_err_cnt_i     : unsigned(MON_ILAS_ERR_CNT'range);

   signal ctl_err_report_req_i   : std_logic;
   signal ctl_reinit_req_i       : std_logic;
   signal ctl_cgs_done_i         : std_logic;
   signal ctl_ifa_done_i         : std_logic;
   signal ctl_ifa_done_i_d       : std_logic;
   signal ctl_ilas_err_i         : std_logic;
   signal ctl_lane_alignment_err_i : std_logic;
   signal ctl_err_overflow_i     : std_logic;
   signal ctl_data_ready_i       : std_logic;

   signal out_byte_err_i         : std_logic_vector(OUT_BYTE_ERR'range) := 
                                                            (others => '0');

   signal phy_comma_align_en_i   : std_logic;

   type cs_fsm_type is (CS_INIT, CS_CHECK_DATA);
   signal cs_fsm : cs_fsm_type;
   type fs_fsm_type is (FS_INIT, FS_DATA);
   signal fs_fsm : fs_fsm_type;

   signal phy_data_d_K_det       : std_logic_vector(PHY_CHARISK'range);

   signal f_alignment            : integer range 0 to 3;
   signal f_aligned_data         : std_logic_vector(PHY_DATA'range);
   signal f_aligned_chisk        : std_logic_vector(PHY_CHARISK'range);
   signal f_aligned_invalid      : std_logic_vector(PHY_DISPERR'range);

   -- these have nothing in common with f_aligned_invalid,
   -- they just say when user data are present on the outputs.
   signal f_aligned_valid        : std_logic;
   signal f_aligned_valid_d      : std_logic;

   signal f_aligned_data_d       : std_logic_vector(PHY_DATA'range);
   signal f_aligned_chisk_d      : std_logic_vector(PHY_CHARISK'range);

   signal f_aligned_chisk_concat : std_logic_vector(
                                          2*PHY_CHARISK'length - 1 downto 0);
   signal f_aligned_data_concat  : std_logic_vector(
                                          2*PHY_DATA'length - 1 downto 0);
                                          

   signal f_aligned_A_det        : std_logic_vector(PHY_CHARISK'range);
   signal f_aligned_R_det        : std_logic_vector(PHY_CHARISK'range);
   signal f_aligned_F_det        : std_logic_vector(PHY_CHARISK'range);
   signal f_aligned_A_det_d      : std_logic_vector(PHY_CHARISK'range);
   signal f_aligned_F_det_d      : std_logic_vector(PHY_CHARISK'range);
   signal f_aligned_A_err        : std_logic;
   signal f_aligned_R_err        : std_logic;

   signal ilas_alignment         : integer range 0 to 3;
   signal ilas_aligned_data      : std_logic_vector(PHY_DATA'range);
   signal ilas_aligned_chisk     : std_logic_vector(PHY_CHARISK'range);
   signal ilas_calc_alignment    : std_logic;
   signal ilas_cfg_data_inprogress  : std_logic;
   type ilas_fsm_type is (ILAS_MF_START, ILAS_MF_END, ILAS_PARKING);
   signal ilas_fsm : ilas_fsm_type;
   signal ilas_mf_cnt            : integer range 0 to LC_ILAS_CNT;
   signal ilas_cfg_data_cnt      : integer range 0 to LC_ILAS_CFG_DATA_CNT - 1;
   
   signal sof                  : std_logic_vector(OUT_SOF'range);
   signal eof                  : std_logic_vector(OUT_EOF'range);
   signal somf                 : std_logic_vector(OUT_SOMF'range);
   signal eomf                 : std_logic_vector(OUT_EOMF'range);

   signal sof_d                : std_logic_vector(OUT_SOF'range);
   signal eof_d                : std_logic_vector(OUT_EOF'range);
   signal somf_d               : std_logic_vector(OUT_SOMF'range);
   signal eomf_d               : std_logic_vector(OUT_SOMF'range);

   signal sof_dd               : std_logic_vector(OUT_SOF'range);
   signal eof_dd               : std_logic_vector(OUT_EOF'range);
   signal somf_dd              : std_logic_vector(OUT_SOMF'range);
   signal eomf_dd              : std_logic_vector(OUT_SOMF'range);

   signal sof_i                : std_logic_vector(OUT_SOF'range);
   signal eof_i                : std_logic_vector(OUT_EOF'range);
   signal somf_i               : std_logic_vector(OUT_SOMF'range);
   signal eomf_i               : std_logic_vector(OUT_EOMF'range);

   signal eof_concat           : std_logic_vector(2*OUT_EOF'length-1 downto 0);

   signal sof_cnt                : unsigned(CFG_F'range);
   signal somf_cnt               : unsigned(CFG_FxK'range);
   
   signal out_ilas_cfg_data_i    : std_logic_vector(OUT_ILAS_CFG_DATA'range);
   signal out_ilas_cfg_done_i    : std_logic;
   
   signal replace_ref            : std_logic_vector(PHY_DATA'range);

   signal replaced_data          : std_logic_vector(PHY_DATA'range);
   signal replaced_data_valid    : std_logic;

   signal descrambled_data       : std_logic_vector(PHY_DATA'range);
   signal descrambled_valid      : std_logic;
   signal descrambled_sof        : std_logic_vector(OUT_SOF'range);
   signal descrambled_eof        : std_logic_vector(OUT_EOF'range);
   signal descrambled_somf       : std_logic_vector(OUT_SOMF'range);
   signal descrambled_eomf       : std_logic_vector(OUT_EOMF'range);
   signal descrambled_concat      : std_logic_vector(48 downto 0);
   signal out_concat             : std_logic_vector(48 downto 0);

   signal write_ptr              : unsigned(5 downto 0);
   signal read_ptr               : unsigned(write_ptr'range);
   signal buff_fill              : unsigned(write_ptr'range);

   signal tvalid_i               : std_logic;
   signal tdata_i                : std_logic_vector(OUT_M_AXIS_TDATA'range);

begin

   -- Handle NSYNC based on state of all the CGS and Frame alignment
   nsync_proc : process (CLK)
   begin
   end process;

   -- Register input data to decouple from previous core.
   -- Ensure Character alignment.
   -- This process is called Code Group Synchronization in the standard
   -- chapter 7.1. It is reduced to wait for one full correct
   -- word of 4 correct Ks
   -- INFO: CSG is not exactly on
   cgs_proc : process(CLK)
      variable phy_err_number_v : integer range 0 to 4;
      variable phy_invalid_concat : std_logic_vector(7 downto 0);
   begin
      if rising_edge(CLK) then
         phy_data_d <= PHY_DATA;
         phy_data_dd <= phy_data_d;
         phy_chisk_d <= PHY_CHARISK;
         phy_chisk_dd <= phy_chisk_d;
         phy_invalid_d <= PHY_DISPERR or PHY_NOTINTABLE;
         phy_invalid_dd <= phy_invalid_d;

         -- one cycle signals
         mon_phys_err_det <= '0';
         ctl_err_report_req_i <= '0';
         ctl_reinit_req_i <= '0';

         if RST = '1' or MON_RST = '1' then
            -- Clear monitor for reset only.
            mon_phys_err_cnt_i <= (others => '0');
            mon_phys_err_res_cnt_i <= (others => '0');
         end if;


         if RST = '1' or CTL_REINIT = '1' then
            -- Tasks common for reset and restart
            phy_comma_align_en_i <= '0';
            ctl_cgs_done_i <= '0';
            
            cs_fsm <= CS_INIT;
         else
            case cs_fsm is
               when CS_INIT =>
                  cs_icnt <= 0;
                  -- Enable character alignment process in MGT
                  phy_comma_align_en_i <= '1';
                  ctl_cgs_done_i <= '0';
                  -- Wait until 4 correct Ks are received
                  -- INFO: Check for 4 ks in one word; Standard
                  -- is byte oriented and needs 4 consecutive characters
                  -- but in 32 bit implementation it is impractical
                  -- to check across word boundaries. So more than 4 Ks are 
                  -- needed
                  if phy_data_d_K_det = x"F" and phy_invalid_d = x"0" then
                     cs_fsm <= CS_CHECK_DATA;
                     phy_comma_align_en_i <= '0';
                     ctl_cgs_done_i <= '1';
                  end if;
               when CS_CHECK_DATA =>
                  -- CS_CHECK and CS_DATA states are implemented here
                  -- Standard requires 4 errors that are not separated
                  -- by at least 4 valid cause reinit.
                  phy_err_number_v := count_ones(phy_invalid_d);
                  -- Signalize error in any case
                  if phy_err_number_v /= 0 then
                     ctl_err_report_req_i <= '1';
                     mon_phys_err_cnt_i <=  mon_phys_err_cnt_i + 
                                            phy_err_number_v;
                  end if;
                  phy_invalid_concat := phy_invalid_d & phy_invalid_dd;
                  if phy_invalid_d /= "1111" and 
                     four_cons_zeros(phy_invalid_concat) then
                     -- Invalid bytes were separated by four or more valid
                     cs_icnt <= phy_err_number_v;
                     if cs_icnt > 0 or phy_err_number_v > 0 then
                        --print(">>>> Found 4 zeros " & hstr(phy_invalid_concat));
                        --print(">>>> cs_icnt " & integer'image(cs_icnt));
                        --print(">>>> phy_err_number_v " & integer'image(phy_err_number_v));
                     end if;
                  else
                        --print("<<<< Not found 4 zeros " & hstr(phy_invalid_concat));
                        --print("<<<< cs_icnt " & integer'image(cs_icnt));
                        --print("<<<< phy_err_number_v " & integer'image(phy_err_number_v));
                     if phy_err_number_v + cs_icnt >= 4 then
                        -- Four or more invalid bytes not separated by at least
                        -- four valid bytes.
                        -- This is reason for reinit, but it may be suppressed
                        -- by user in upper layer
                        ctl_reinit_req_i <= '1';
                     end if;
                     cs_icnt <= cs_icnt + phy_err_number_v;
                     mon_phys_err_res_cnt_i <= mon_phys_err_res_cnt_i + 1;
                  end if;
            end case;
         end if; -- rst
      end if; -- clk
   end process;

   -- Combinatorial detectors of all needed Ks
   phy_data_d_K_det <= det_K(phy_data_d, phy_chisk_d);





   -- Initial frame Synchronization.
   -- Output of this process is aligned in such a way
   -- that the first byte that is not K is in bit 7 downto 0
   frame_align_and_mon : process(CLK)
      variable phy_data_concat : std_logic_vector(63 downto 0);
      variable phy_chisk_concat : std_logic_vector(7 downto 0);
      variable phy_invalid_concat : std_logic_vector(7 downto 0);
   begin

      if rising_edge(CLK) then
         phy_data_concat := phy_data_d & phy_data_dd;
         phy_chisk_concat := phy_chisk_d & phy_chisk_dd;
         phy_invalid_concat := phy_invalid_d & phy_invalid_dd;

         f_aligned_data <= align(f_alignment, phy_data_concat);
         f_aligned_chisk <= align(f_alignment, phy_chisk_concat);
         f_aligned_invalid <= align(f_alignment, phy_invalid_concat);
         f_aligned_data_d <= f_aligned_data;
         f_aligned_chisk_d <= f_aligned_chisk;

         ilas_aligned_data <= align(ilas_alignment, phy_data_concat);
         ilas_aligned_chisk <= align(ilas_alignment, phy_chisk_concat);

         ctl_ifa_done_i <= '0';
         ilas_calc_alignment <= '0';

         if RST = '1' or ctl_cgs_done_i = '0' then
            fs_fsm <= FS_INIT;
         else

            case fs_fsm is
               when FS_INIT =>
                  if phy_data_d_K_det /= x"F" then
                     fs_fsm <= FS_DATA;
                     ctl_ifa_done_i <= '1';
                     f_alignment <= first_zero(phy_data_d_K_det);
                     ilas_calc_alignment <= '1';
                  end if;
               when FS_DATA =>
                  ctl_ifa_done_i <= '1';        
            end case;
         end if;

         -- Calculate ILAS alignment for future use
         if ilas_calc_alignment = '1' then
            ilas_alignment <= f_alignment + 
                              to_integer(unsigned(CFG_FxK(1 downto 0)));
         end if;
      end if;
   end process;

   -- Derive starts and ends of frames and multiframes
   framing_proc : process(CLK)
      variable tmp_f_v     : unsigned(CFG_F'range);
      variable tmp_fxk_v   : unsigned(CFG_FxK'range);
      variable tmp_sof_v   : std_logic_vector(sof'range);
      variable tmp_somf_v  : std_logic_vector(somf'range);
   begin
      if rising_edge(CLK) then
         eof_d <= eof;
         sof_d <= sof;
         somf_d <= somf;
         eomf_d <= eomf;
         eof_dd <= eof_d;
         sof_dd <= sof_d;
         somf_dd <= somf_d;
         eomf_dd <= eomf_d;
         ctl_ifa_done_i_d <= ctl_ifa_done_i;

         eomf <= (others => '0');

         if RST = '1' or ctl_ifa_done_i = '0' then
            -- Derive init value for sof counter
            tmp_f_v := unsigned(CFG_F) - 3;
            sof_cnt <= tmp_f_v;
            cfg_f_init <= tmp_f_v;
            -- Derive init value for somf counter
            tmp_fxk_v := unsigned(CFG_FxK) - 4;
            cfg_fxk_init <= tmp_fxk_v;
            -- Multiframe is >=17 bytes by definition.
            -- So there is no problem with several starts in one word.
            somf_cnt <= tmp_fxk_v ;
            sof <= (others => '0');
            eof <= (others => '0');
            somf <= (others => '0');
            
         elsif ctl_ifa_done_i_d = '0' then
            somf <= (0 => '1', others => '0');
            
            -- There are special cases where more then one start may be
            -- present
            case to_integer(unsigned(CFG_F)) is
               when 0 =>
                  sof <= "1111";
                  eof <= "1111";
               when 1 =>
                  sof <= "0101";
                  eof <= "1010";
               when 2 =>
                  sof <= "1001";
                  eof <= "0100";
               when 3 =>
                  sof <= "0001";
                  eof <= "1000";
               when others =>
                  sof <= "0001";
                  eof <= "0000";                  
            end case;
         else
            -- frames
            case to_integer(unsigned(CFG_F)) is
               when 0 =>
                  -- Next value is identical
                  null;
               when 1 =>
                  -- Next value is identical
                  null;
               when 2 =>
                  -- Next value based on table
                  case sof is
                     when "1001" =>
                        sof <= "0100";
                        eof <= "0010";
                     when "0100" =>
                        sof <= "0010";
                        eof <= "1001";
                     when "0010" =>
                        sof <= "1001";
                        eof <= "0100";
                     when others =>
                        report "SOF has incorrect value" severity error;
                        sof <= "1001";
                        eof <= "0100";
                  end case;
               when 3 =>
                  -- Next value is identical
               when others =>
                  -- Next value given by algorithm
                  if sof_cnt >= 4 then
                     -- No frame start in this cycle
                     sof_cnt <= sof_cnt - 4;
                     sof <= (others => '0');
                     -- But there can end here
                     eof <= (others => '0');
                     if sof_cnt = 4 then
                        eof <= "1000";
                     end if;
                  else
                     sof_cnt <= sof_cnt + cfg_f_init;
                     tmp_sof_v := (others => '0');
                     tmp_sof_v(to_integer(sof_cnt(1 downto 0))) := '1';
                     sof <= tmp_sof_v;
                     -- We know end of frame is not in msb here and
                     -- it is one bit shifted from start
                     eof <= "0" & tmp_sof_v(3 downto 1);
                  end if;
            end case;

            -- Multiframes
            if somf_cnt >= 4 then
               -- No multiframe starts here
               somf_cnt <= somf_cnt - 4;
               somf <= (others => '0');
               -- But one can end here
               if somf_cnt = 4 then
                  eomf <= "1000";
               end if;
            else
               somf_cnt <= somf_cnt + cfg_fxk_init;
               tmp_somf_v := (others => '0');
               tmp_somf_v(to_integer(somf_cnt(1 downto 0))) := '1'; 
               somf <= tmp_somf_v;
               -- We know end of frame is not in msb here and
               -- it is one bit shifted from start               
               eomf <= "0" & tmp_somf_v(3 downto 1);                  
            end if;

         end if;
      end if;
   end process;

   
   f_aligned_R_det <= det_R(f_aligned_data, f_aligned_chisk);
   f_aligned_A_det <= det_A(f_aligned_data, f_aligned_chisk);
   f_aligned_F_det <= det_F(f_aligned_data, f_aligned_chisk);
   f_aligned_R_err <= '1' when somf /= f_aligned_R_det else '0';   
   f_aligned_A_err <= '1' when eomf /= f_aligned_A_det else '0';


   -- ILA(S) - initial lane alignment sequence handling
   -- There is special align stage for ILAS handling so we know
   -- that ilas_aligned_data/chisk starts with R on 0, Q on 1 and so on.
   -- Let upper layer/sw to deal with this.
   -- Alignment generated locally based on K and F is considered the
   -- correct one. Alignment in data is checked but not used to control
   -- anything.
   ilas_fsm_proc : process(CLK)
   begin
      if rising_edge(CLK) then

         ctl_ilas_err_i <= '0';
         ilas_cfg_data_inprogress <= '0';

         f_aligned_valid_d <= f_aligned_valid;

         if RST = '1' or MON_RST = '1' then
            mon_ilas_err_cnt_i <= (others => '0');
         end if;

         if RST = '1' or ctl_ifa_done_i = '0' then
            ilas_fsm <= ILAS_MF_START;
            ilas_mf_cnt <= 0;
            ilas_cfg_data_cnt <= 0;
            out_ilas_cfg_data_i <= (others => '0');
            out_ilas_cfg_done_i <= '0';
            f_aligned_valid <= '0';
            f_aligned_valid_d <= '0';
         else
            case ilas_fsm is
               when ILAS_MF_START =>
                  -- Capture start of ILAS here. It can never be 
                  -- in the same word as the end of the same multiframe as
                  -- multiframe is at least 17 characters long
                  if vec_or(somf) = '1' then
                     ilas_mf_cnt <= ilas_mf_cnt + 1;
                     ilas_fsm <= ILAS_MF_END;
                     if f_aligned_R_err = '1' then
                        -- Start of multiframe doesn't agree with 
                        -- /A/ character this is a serious error
                        ctl_ilas_err_i <= '1';
                     end if;
                  end if;
               when ILAS_MF_END =>
                  -- Here we check end character /A/ of multiframe.
                  -- For pervese multiframes that don't have length of
                  -- integer multiple of 4 we need to check for start of
                  -- the next multiframe too, as they may be in the same
                  -- word.
                  if vec_or(eomf) = '1' then
                     if ilas_mf_cnt >= LC_ILAS_CNT then
                        -- This is the last end of multiframe of this ILAS
                        ilas_fsm <= ILAS_PARKING;
                        -- End of last ILAS multiframe is followed by user
                        -- data. Depending of position of this multiframe end
                        -- valid data may be is this or next aligned word
                        if eomf(3) = '1' then
                           -- Valid data will be in the next cycle
                           f_aligned_valid <= '1';
                        else
                           -- Valid data are already in this cycle
                           -- TODO - Can this happen?
                           f_aligned_valid_d <= '1';
                           f_aligned_valid <= '1';
                        end if;
                     else
                        -- There is at least one more multframe of ILAS
                        if eomf(3) = '1' then
                           -- Start of multiframe will be in the next word
                           ilas_fsm <= ILAS_MF_START;
                        else
                           -- Start of multiframe must be here in this word
                           ilas_mf_cnt <= ilas_mf_cnt + 1;
                           ilas_fsm <= ILAS_MF_END;
                           if f_aligned_R_err = '1' then
                              -- Start of multiframe doesn't agree with 
                              -- /A/ character this is a serious error
                              ctl_ilas_err_i <= '1';
                              mon_ilas_err_cnt_i <= mon_ilas_err_cnt_i + 1;
                           end if;
                        end if;
                     end if;

                     if f_aligned_A_err = '1' then
                        -- End of multiframe doesn't agree with /R/ character
                        ctl_ilas_err_i <= '1';
                     end if;
                  end if;
               when ILAS_PARKING =>
                  -- ILAS is finished stay here until reset or reinit.
                  null;
            end case;

            -- Capture configuration sequence from the stream
            if (ilas_mf_cnt = 1 and vec_or(somf) = '1') or 
               ilas_cfg_data_inprogress = '1' then
                  out_ilas_cfg_data_i((ilas_cfg_data_cnt + 1)* 32 -1 downto
                                   ilas_cfg_data_cnt * 32) <= ilas_aligned_data;
               if ilas_cfg_data_cnt < LC_ILAS_CFG_DATA_CNT - 1 then
                  ilas_cfg_data_cnt <= ilas_cfg_data_cnt + 1;
                  ilas_cfg_data_inprogress <= '1';
               else
                  -- We are done with configuration data reception
                  out_ilas_cfg_done_i <= '1';
               end if;
            end if;
           
         end if;
      end if;
   end process;

   -- Concatenation of delayed and current signals simplifies
   -- expressions for character replacement
   f_aligned_chisk_concat <= f_aligned_chisk & f_aligned_chisk_d;
   f_aligned_data_concat  <= f_aligned_data  & f_aligned_data_d;
   eof_concat <= eof & eof_d;

   -- Character replacement and alignment check
   -- Don't check for missing replacement. This is busyness
   -- of transmitter. If it doesn't do character replacement,
   -- I don't care.
   char_replace_align_check_proc : process(CLK)
      variable idx_from_v, idx_to_v : integer;
      variable F_more_than_3 : std_logic := '0';
   begin
      if rising_edge(CLK) then

         f_aligned_A_det_d <= f_aligned_A_det;
         f_aligned_F_det_d <= f_aligned_F_det;

         replaced_data <= f_aligned_data_d;

         replaced_data_valid <= f_aligned_valid_d;

         if RST = '1' or MON_RST = '1' then
            mon_align_f_err_cnt_i <= (others => '0');
            mon_align_mf_err_cnt_i <= (others => '0');
            mon_replace_f_cnt_i <= (others => '0');
            mon_replace_a_cnt_i <= (others => '0');
            mon_unknown_k_cnt_i <= (others => '0');
         end if;


         if RST = '1' then
            -- Reset specific
            null;
         else
            for i in 0 to 3 loop
               -- Replace control found charaters
               -- Don't replace characters in ILAS
               if f_aligned_chisk_d(i) = '1' and f_aligned_valid_d = '1' then
                  -- Control character found
                  if f_aligned_A_det_d(i) = '1' then
                     -- Should be end of multiframe
                     if eomf_d(i) = '0' then
                        -- It is not, report error
                        ctl_lane_alignment_err_i <= '1';
                        mon_align_mf_err_cnt_i <= mon_align_mf_err_cnt_i + 1;
                     end if;
                     mon_replace_a_cnt_i <= mon_replace_a_cnt_i + 1;
                  elsif f_aligned_F_det_d(i) = '1' then
                     -- Should be end of frame, but not multiframe
                     if eof_d(i) = '0' or eomf_d(i) = '1' then
                        -- It is not, report error
                        ctl_lane_alignment_err_i <= '1';
                        mon_align_f_err_cnt_i <= mon_align_f_err_cnt_i + 1;
                     end if;
                     mon_replace_f_cnt_i <= mon_replace_f_cnt_i + 1;
                  else
                     -- Other character found. This is probably bit error
                     -- or complete failure of link
                     ctl_lane_alignment_err_i <= '1';
                     mon_unknown_k_cnt_i <= mon_unknown_k_cnt_i + 1;
                  end if;
                  -- Replace control character regardless error.
                  -- Last value is probably closer to reality than
                  -- /A/, /F/ or other character anyway.
                  replaced_data((i+1)*8 - 1 downto 8*i) <= 
                                                   slice_b(i, replace_ref);
               end if;
            end loop;

            -- Capture reference data for replacement
            -- Work partly on not-delayed data to be able to handle
            -- F=1,2,3
            for idx_to_v in 0 to 3 loop
               -- Derive where to get reference from
               case to_integer(unsigned(CFG_F)) is
                  when 0 =>
                     -- F = 1
                     idx_from_v := idx_to_v + 4 - 1;
                  when 1 =>
                     -- F = 2
                     idx_from_v := idx_to_v + 4 - 2;
                     if idx_to_v = 0 or idx_to_v = 2 then
                        -- These positions don't need replacement
                        -- as end of frame can never occur here because
                        -- of frame alignment
                        next;
                     end if;
                  when 2 =>
                     -- F= 3
                     idx_from_v := idx_to_v + 4 - 3;
                  when others =>
                     -- For all cases where F is 4 and more
                     -- we never need to work with current data
                     -- We always copy data to all positions to simplify
                     -- destination calculation
                     idx_from_v := idx_to_v;
                     F_more_than_3 := '1';
               end case;
               
               -- Do the reference copy
               if eof_concat(idx_from_v) = '1' and 
                  f_aligned_chisk_concat(idx_from_v) = '0' then
                  if F_more_than_3 = '0' then
                     replace_ref(slice_b(idx_to_v)'range) <=
                           slice_b(idx_from_v, f_aligned_data_concat);
                  else
                     -- Copy to all positions
                     for i in 0 to 3 loop
                        replace_ref(slice_b(i)'range) <=
                           slice_b(idx_from_v, f_aligned_data_concat);
                     end loop;
                  end if;
               end if;

--               case to_integer(unsigned(CFG_F)) is
--                  when 0 =>
--                     -- Frame is only one octet, so we have to work mostly with
--                     -- current data in replacement.
--                     if i = 3 then
--                        -- This one is in delayed data
--                        if eof_d(i) = '1' and f_aligned_chisk_d(i) = '0' then
--                           replace_ref(7 downto 0) <=
--                              slice_b(i, f_aligned_data_d);
--                        end if;
--                     else
--                        -- These from non delayed
--                        if eof(i) = '1' and f_aligned_chisk(i) = '0' then
--                           replace_ref((i+1+1)*8 - 1 downto 8*(i+1)) <=
--                              slice_b(i, f_aligned_data);
--                        end if;
--                     end if;
--                  when 1 =>
--                     -- Frame is two octets long, so ends may be only at
--                     -- positions 1 and 3. Position 3 needs to work with current
--                     -- data of position 1
--                     if i = 3 then
--                        -- This one is in delayed data
--                        if eof_d(i) = '1' and f_aligned_chisk_d(i) = '0' then
--                           replace_ref(15 downto 8) <=
--                              slice_b(i, f_aligned_data_d);
--                        end if;
--                     elsif i = 1 then
--                        -- These from non delayed
--                        if eof(i) = '1' and f_aligned_chisk(i) = '0' then
--                           replace_ref((3+1)*8 - 1 downto 8*(3)) <=
--                              slice_b(i, f_aligned_data);
--                        end if;
--                     end if;
--                  when 2 =>
--                      -- Not implemented yet               
--                  when others =>
--                     -- For all cases where F is 4 and more
--                     -- we never need to work with current data
--                     if eof_d(i) = '1' and f_aligned_chisk_d(i) = '0' then
--                        for j in 0 to 3 loop
--                           replace_ref((j+1)*8 - 1 downto 8*j) <=
--                              slice_b(i, f_aligned_data);
--                        end loop;
--                     end if;
--               end case;
            end loop;

         end if; -- reset
      end if; -- clock
   end process; -- char_replace_alicgn_check_proc


   -- This will be replaced by scrambler
   descrambled_data <= replaced_data;
   descrambled_valid <= replaced_data_valid;
   descrambled_sof <= sof_dd;
   descrambled_eof <= eof_dd;
   descrambled_somf <= somf_dd;
   descrambled_eomf <= eomf_dd;

--   par_scrambler_inst : entity work.par_scrambler
--	generic map(
--		Data_Width 			=> PHY_DATA'length,
--		Polynomial_Width	=> LC_SCRAMBLER_POLY'length
--		)
--	port map ( 
--		rst					=> RST,
--		clk					=> CLK,
--		scram_rst			=> '0',
--		Polynomial			=> LC_SCRAMBLER_POLY,
--		data_in 			   => replaced_data,
--		scram_en			   => '1',
--		data_out 			=> descrambled_data,
--		out_valid			=> descrambled_valid,
--		);
--

   -- Store to FIFO until released
   buffer_proc : process (CLK)
   begin
      if rising_edge(CLK) then
         if RST = '1' or descrambled_valid = '0' then
            write_ptr <= (others => '0');
            read_ptr <= (others => '0');
            buff_fill <= (others => '0');
            ctl_data_ready_i <= '0';
            ctl_err_overflow_i <= '0';

            eomf_i   <= (others => '0');
            eof_i    <= (others => '0');
            somf_i   <= (others => '0');
            sof_i    <= (others => '0');
            tdata_i  <= (others => '0');
            tvalid_i <= '0';
            
         else
            write_ptr <=  write_ptr + 1;
            ctl_data_ready_i <= '1';

            if CTL_RELEASE_DATA = '1' then
               read_ptr <= read_ptr + 1;
               eomf_i   <= out_concat(48 downto 45);
               eof_i    <= out_concat(44 downto 41);
               somf_i   <= out_concat(40 downto 37);
               sof_i    <= out_concat(36 downto 33);
               tdata_i  <= out_concat(32 downto 1);
               tvalid_i <= out_concat(0);
            else
               -- Count how much buffer is filled
               buff_fill <= buff_fill + 1;
               if unsigned (not write_ptr) = 0 then
                  ctl_err_overflow_i <= '1';
               end if;
            end if;
         end if;
      end if;
   end process;

   descrambled_concat <= descrambled_eomf &
                         descrambled_eof & 
                         descrambled_somf & 
                         descrambled_sof & 
                         descrambled_data & 
                         descrambled_valid;
   
   buffer_ram_inst : entity work.rams_dist_dp
   generic map (
      g_data_width   => descrambled_concat'length,
      g_addr_width   => write_ptr'length
           )
	port map (
		clk   => CLK,
		we    => descrambled_valid,
		wa    => std_logic_vector(write_ptr),
		ra    => std_logic_vector(read_ptr),
		wdi   => descrambled_concat,
		rdo   => out_concat
	);






   -- Outputs mappings
   CTL_CGS_DONE         <= ctl_cgs_done_i;
   CTL_REINIT_REQ       <= ctl_reinit_req_i;
   CTL_ERR_REPORT_REQ   <= ctl_err_report_req_i;
   CTL_IFA_DONE         <= ctl_ifa_done_i;
   CTL_ERR_OVERFLOW     <= ctl_err_overflow_i;
   CTL_DATA_READY       <= ctl_data_ready_i;

   PHY_COMMA_ALIGN_EN   <= phy_comma_align_en_i;
   OUT_SOF              <= sof_i;
   OUT_EOF              <= eof_i;
   OUT_SOMF             <= somf_i;
   OUT_EOMF             <= eomf_i;
   OUT_ILAS_CFG_DATA    <= out_ilas_cfg_data_i;
   OUT_ILAS_CFG_DONE    <= out_ilas_cfg_done_i;
   OUT_M_AXIS_TVALID    <= tvalid_i;
   OUT_M_AXIS_TDATA     <= tdata_i;
   OUT_BYTE_ERR         <= out_byte_err_i;
   MON_PHYS_ERR_CNT     <= std_logic_vector(mon_phys_err_cnt_i);
   MON_PHYS_ERR_RES_CNT <= std_logic_vector(mon_phys_err_res_cnt_i);
   MON_ALIGN_F_ERR_CNT  <= std_logic_vector(mon_align_f_err_cnt_i);
   MON_ALIGN_MF_ERR_CNT <= std_logic_vector(mon_align_mf_err_cnt_i);
   MON_REPLACE_F_CNT    <= std_logic_vector(mon_replace_f_cnt_i);
   MON_REPLACE_A_CNT    <= std_logic_vector(mon_replace_a_cnt_i);
   MON_UNKNOWN_K_CNT    <= std_logic_vector(mon_unknown_k_cnt_i);
   MON_ILAS_ERR_CNT     <= std_logic_vector(mon_ilas_err_cnt_i);

   CTL_ILAS_ERR <= '0';
   OUT_BYTE_ERR <= (others => '0');

end architecture;

