LIBRARY ieee;
USE ieee.std_logic_1164.ALL;
USE IEEE.STD_LOGIC_arith.ALL;
USE IEEE.STD_LOGIC_unsigned.ALL;
 
-- Uncomment the following library declaration if using
-- arithmetic functions with Signed or Unsigned values
--USE ieee.numeric_std.ALL;

Library xpm;
use xpm.vcomponents.all;

entity qsfp_init_fsm is
  port(
    clk_125_in                      : in  std_logic;
    clk_125_aresetn_in              : in  std_logic;

    mode_50g_40g_n_in               : in  std_logic;
    qsfp1_reset_n_in                : in  std_logic;
    qsfp4_reset_n_in                : in  std_logic;

    cmd_strb_out                    : out std_logic;
    cmd_addr_out                    : out std_logic_vector(11 downto 0);
    cmd_write_out                   : out std_logic;
    cmd_sel_out                     : out std_logic_vector( 2 downto 0);
    cmd_wdata_out                   : out std_logic_vector(31 downto 0);
    cmd_ready_in                    : in  std_logic;
    fsm_running_out                 : out std_logic
    );
end entity qsfp_init_fsm;
  
architecture imp of qsfp_init_fsm is

  type fsm_state                    is (IDLE, QSFP1_INIT_WAIT, QSFP1_INIT, QSFP4_INIT_WAIT, QSFP4_INIT, DONE, ERROR);
  signal state_r                    : fsm_state := IDLE;
  signal state_cnt_r                : integer := 0;  

  signal qsfp1_reset_n              : std_logic_vector(0 to 0);   
  signal qsfp1_reset_b              : std_logic_vector(0 to 0);  
  signal qsfp1_reset_n_r            : std_logic_vector(0 to 31) := x"0000_0000";

  signal qsfp4_reset_n              : std_logic_vector(0 to 0);
  signal qsfp4_reset_b              : std_logic_vector(0 to 0);   
  signal qsfp4_reset_n_r            : std_logic_vector(0 to 31) := x"0000_0000";   

  signal qsfp1_reset_r              : std_logic := '0';      

  signal qsfp4_reset_r              : std_logic := '0';      

  signal cmd_addr_r                 : std_logic_vector (11 downto 0) := (others => '0');
  signal cmd_sel_r                  : std_logic_vector ( 2 downto 0) := (others => '0');
  signal cmd_strb_r                 : std_logic := '0';
  signal cmd_wdata_r                : std_logic_vector (31 downto 0) := (others => '0');
  signal cmd_write_r                : std_logic := '0';
  
  signal fsm_running_r              : std_logic := '0';
  
begin

  cmd_strb_out        <= cmd_strb_r;        
  cmd_addr_out        <= cmd_addr_r;        
  cmd_write_out       <= cmd_write_r;        
  cmd_sel_out         <= cmd_sel_r;        
  cmd_wdata_out       <= cmd_wdata_r;        
  fsm_running_out     <= fsm_running_r;
  
--
  qsfp1_reset_n(0) <= qsfp1_reset_n_in;

  i_cdc_qsfp1_reset_n : xpm_cdc_array_single
  generic map (
    DEST_SYNC_FF              => 2,    -- DECIMAL; range: 2-10
    INIT_SYNC_FF              => 0,    -- DECIMAL; 0=disable simulation init values, 1=enable simulation init values
    SIM_ASSERT_CHK            => 0,  -- DECIMAL; 0=disable simulation messages, 1=enable simulation messages
    SRC_INPUT_REG             => 0,   -- DECIMAL; 0=do not register input, 1=register input
    WIDTH                     => 1            -- DECIMAL; range: 1-1024 
    )
  port map (
    dest_out                  => qsfp1_reset_b, 
    dest_clk                  => clk_125_in, 
    src_clk                   => '0',   
    src_in                    => qsfp1_reset_n     
    );

  process(clk_125_in)
  begin
    if (rising_edge(clk_125_in)) then
      qsfp1_reset_n_r         <= qsfp1_reset_n_r(1 to 31) & qsfp1_reset_b(0);
    end if;
  end process;

--          
  qsfp4_reset_n(0)  <= qsfp4_reset_n_in;

  i_cdc_qsfp4_reset_n : xpm_cdc_array_single
  generic map (
    DEST_SYNC_FF              => 2,    -- DECIMAL; range: 2-10
    INIT_SYNC_FF              => 0,    -- DECIMAL; 0=disable simulation init values, 1=enable simulation init values
    SIM_ASSERT_CHK            => 0,  -- DECIMAL; 0=disable simulation messages, 1=enable simulation messages
    SRC_INPUT_REG             => 0,   -- DECIMAL; 0=do not register input, 1=register input
    WIDTH                     => 1            -- DECIMAL; range: 1-1024 
    )
  port map (
    dest_out                  => qsfp4_reset_b, 
    dest_clk                  => clk_125_in, 
    src_clk                   => '0',   
    src_in                    => qsfp4_reset_n     
    );

  process(clk_125_in)
  begin
    if (rising_edge(clk_125_in)) then
      qsfp4_reset_n_r         <= qsfp4_reset_n_r(1 to 31) & qsfp4_reset_b(0);
    end if;
  end process;
   
  process(clk_125_in)
  begin
    if (clk_125_aresetn_in = '0') then
      qsfp1_reset_r                 <= '0';
      qsfp4_reset_r                 <= '0';

      cmd_sel_r                     <= (others => '0');
      cmd_addr_r                    <= (others => '0');
      cmd_wdata_r                   <= (others => '0');
      cmd_write_r                   <= '0';
      cmd_strb_r                    <= '0';
            
      state_cnt_r                   <= 0;
      state_r                       <= IDLE;
    elsif (rising_edge(clk_125_in)) then        
      cmd_strb_r             <= '0';

      if (qsfp1_reset_n_r(1) = '1' and qsfp1_reset_n_r(0) = '0') then
        qsfp1_reset_r               <= '1';
      end if;      
      
      if (qsfp4_reset_n_r(1) = '1' and qsfp4_reset_n_r(0) = '0') then
        qsfp4_reset_r               <= '1';
      end if;      
      
      case (state_r) is
        when IDLE =>
          if (qsfp1_reset_r = '1') then
            fsm_running_r           <= '1';
            cmd_sel_r               <= "000";
            cmd_addr_r              <= x"001";
            if (mode_50g_40g_n_in = '1') then
              cmd_wdata_r           <= x"0000_0001";
            else
              cmd_wdata_r           <= x"0000_0011";
            end if;
            cmd_write_r             <= '1';
            cmd_strb_r              <= '1';
            state_cnt_r             <= 0;
            state_r                 <= QSFP1_INIT_WAIT;
          elsif (qsfp4_reset_r = '1') then
            fsm_running_r           <= '1';
            cmd_sel_r               <= "001";
            cmd_addr_r              <= x"001";
            if (mode_50g_40g_n_in = '1') then
              cmd_wdata_r           <= x"0000_0001";
            else
            cmd_wdata_r             <= x"0000_0011";
            end if;
            cmd_write_r             <= '1';
            cmd_strb_r              <= '1';
            state_cnt_r             <= 0;
            state_r                 <= QSFP4_INIT_WAIT;
          else
            fsm_running_r           <= '0';
            state_r                 <= IDLE;
          end if;

        when QSFP1_INIT_WAIT =>
          if (state_cnt_r = 8) then
            state_cnt_r             <= 0;
            state_r                 <= QSFP1_INIT;
          else
            state_cnt_r             <= state_cnt_r + 1;
            state_r                 <= QSFP1_INIT_WAIT;
          end if;

        when QSFP1_INIT =>
          if (state_cnt_r = 32) then
            qsfp1_reset_r           <= '0';
            state_r                 <= ERROR;
          else
            state_cnt_r             <= state_cnt_r + 1;
            if (cmd_ready_in = '1') then
              qsfp1_reset_r         <= '0';
              state_r               <= DONE;
            else
              state_r               <= QSFP1_INIT;
            end if;
          end if;

        when QSFP4_INIT_WAIT =>
          if (state_cnt_r = 8) then
            state_cnt_r             <= 0;
            state_r                 <= QSFP4_INIT;
          else
            state_cnt_r             <= state_cnt_r + 1;
            state_r                 <= QSFP4_INIT_WAIT;
          end if;

        when QSFP4_INIT =>
          if (state_cnt_r = 32) then
            qsfp4_reset_r           <= '0';
            state_r                 <= ERROR;
          else
            state_cnt_r             <= state_cnt_r + 1;
            if (cmd_ready_in = '1') then
              qsfp4_reset_r         <= '0';
              state_r               <= DONE;
            else
              state_r               <= QSFP4_INIT;
            end if;
          end if;

        when DONE =>
          cmd_write_r               <= '0';
          state_r                   <= IDLE;

        when ERROR =>
          cmd_write_r               <= '0';
          state_r                   <= IDLE;

        when others =>
          state_r                   <= IDLE;

      end case;
    end if;
  end process;





end architecture imp;
