-------------------------------------------------------------------------------
--
-- Project name: ROCE_TO_100G_UDP
--
-- (C) Copyright 2019 DFC Design, s.r.o., Brno, Czech Republic
-- Author: Jakub Hadamek (j.hadamek@dfcdesign.cz)
--
-------------------------------------------------------------------------------
--
-- This file is part of ROCE_TO_100G_UDP IP core
--
-- It's purpose is to insert RoCE header on the start 
-- of packet. 
-- 
-------------------------------------------------------------------------------



library IEEE;
use IEEE.STD_LOGIC_1164.ALL;

Library xpm;
use xpm.vcomponents.all;

-- Uncomment the following library declaration if using
-- arithmetic functions with Signed or Unsigned values
use IEEE.NUMERIC_STD.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;


-- Uncomment the following library declaration if instantiating
-- any Xilinx leaf cells in this code.
--library UNISIM;
--use UNISIM.VComponents.all;

entity roce_insert_header is
   generic (
      g_extra_data_width   : natural
   );
   port (
      RESET                : in  std_logic;
      CLK                  : in  std_logic;
      -- AXI Stream input
      S_AXIS_TVALID        : in  std_logic;
      S_AXIS_TREADY        : out std_logic;
      S_AXIS_TLAST         : in  std_logic;
      S_AXIS_TKEEP         : in  std_logic_vector(63 downto 0);
      S_AXIS_TDATA         : in  std_logic_vector(511 downto 0);
      S_EXTRA_DATA         : in  std_logic_vector(g_extra_data_width - 1 downto 0) := (others => '0');
      -- AXI Stream output
      M_AXIS_TVALID        : out std_logic;
      M_AXIS_TREADY        : in  std_logic;
      M_AXIS_TLAST         : out std_logic;
      M_AXIS_TKEEP         : out std_logic_vector(63 downto 0);
      M_AXIS_TDATA         : out std_logic_vector(511 downto 0);
      M_EXTRA_DATA         : out std_logic_vector(g_extra_data_width - 1 downto 0);
      -- RoCE header parameters
      ROCE_TX_DATA_LEN     : in  std_logic_vector(15 downto 0);
      UDP_TX_DATA_LEN      : out std_logic_vector(15 downto 0);
      ROCE_TX_DEST_QP      : in  std_logic_vector(23 downto 0);
      ROCE_TX_PSN          : in  std_logic_vector(23 downto 0):=(others => '0');
      -- DETH header fields
      ROCE_TX_Q_KEY        : in  std_logic_vector(31 downto 0);
      ROCE_TX_SRC_QP       : in  std_logic_vector(23 downto 0)
   );
end;

architecture Behavioral of roce_insert_header is


   -- Convert form HOST to NETWORK order
   -- It mix bytes only not bits
   -- Parameter must be vector of length divisible by 8
   function HTON(vec : std_logic_vector) return std_logic_vector is
      variable res   : std_logic_vector(vec'length - 1 downto 0);
      variable vec1  : std_logic_vector(vec'length - 1 downto 0);
      variable bytes : integer := vec'length/8;
      variable j     : integer;
   begin
      vec1 := vec;
      for i in 0 to bytes - 1 loop
         j := bytes - i - 1;
         res((i+1)*8 -1 downto i*8) := vec1((j+1)*8 -1 downto j*8);
      end loop;
      return res;
   end function;

   -- or vector
   function or_vec (vec : std_logic_vector)
      return std_logic is
      variable retval : std_logic := '0';
      begin
         for i in 0 to vec'length-1 loop
               retval := retval or vec(i);
         end loop;
         return retval;
      end or_vec;

   -- and vector
   function and_vec (vec : std_logic_vector)
      return std_logic is
      variable retval : std_logic := '1';
      begin
         for i in 0 to vec'length-1 loop
               retval := retval and vec(i);
         end loop;
         return retval;
      end and_vec;

   signal data_cnt                  : std_logic_vector(ROCE_TX_DATA_LEN'range);

   -- RoCE headers
   constant LC_BTH_HEADER_SIZE      : natural := 12*8;
   constant LC_DETH_HEADER_SIZE     : natural := 8*8;
   constant LC_ROCE_HEADERS_SIZE    : natural := LC_BTH_HEADER_SIZE + LC_DETH_HEADER_SIZE;
   constant LC_ROCE_CRC_SIZE        : natural := 4;
   -- Base transport header
   signal bth        : std_logic_vector(95 downto 0);
   -- Datagram extended header
   signal deth       : std_logic_vector(63 downto 0);
      
   -- These ROCE header fields are defined as constants as spec doesn't
   -- expect necessity to change them even in remote future
   constant LC_ROCE_OPCODE_SEND  : std_logic_vector(7 downto 0):="01100100";
   constant LC_ROCE_SE           : std_logic := '0'; -- Solicited event
   constant LC_ROCE_M            : std_logic := '0';
   constant LC_ROCE_PADCNT       : std_logic_vector(1 downto 0) := "00"; -- Pad Count
   constant LC_ROCE_TVER         : std_logic_vector(3 downto 0) := "0000"; -- Transport header version
   constant LC_ROCE_P_KEY        : std_logic_vector(15 downto 0):=x"ffff";
                                    -- Partition key
   constant LC_ROCE_F            : std_logic := '0'; -- FECN
   constant LC_ROCE_B            : std_logic := '0'; -- BECN
   constant LC_ROCE_RESERVED_V   : std_logic_vector(5 downto 0) := "000000";
   constant LC_ROCE_A            : std_logic := '0'; -- Acknowledge Request
   constant LC_ROCE_RESERVED     : std_logic_vector(6 downto 0) := "0000000";
   constant LC_ROCE_DETH_RES     : std_logic_vector(7 downto 0) := (others=>'0');   

   signal pkt_seq_num      : std_logic_vector(23 downto 0);

   type roce_hdr_fsm_t is (SOP_ST, DATA_ST, LAST_ST);                       
   signal roce_hdr_fsm     : roce_hdr_fsm_t := SOP_ST;


   
   signal m_axis_tvalid_i  : std_logic;     
   signal m_axis_tready_i  : std_logic;    
   signal m_axis_tlast_i   : std_logic;   
   signal m_axis_tkeep_i   : std_logic_vector(M_AXIS_TKEEP'range);    
   signal m_axis_tdata_i   : std_logic_vector(M_AXIS_TDATA'range);            

   signal s_axis_tvalid_d  : std_logic;                    
   signal s_axis_tready_d  : std_logic;                    
   signal s_axis_tlast_d   : std_logic;     
   signal s_axis_tuser_d   : std_logic;                    
   signal s_axis_tkeep_d   : std_logic_vector(63 downto 0);
   signal s_axis_tdata_d   : std_logic_vector(511 downto 0);
   
   signal extra_data_d     : std_logic_vector(S_EXTRA_DATA'length-1 downto 0) := (others => '0');
   
   signal sop              : std_logic;
   signal roce_hdr         : std_logic_vector(20*8-1 downto 0);
   signal tdata_d          : std_logic_vector(roce_hdr'range);
   signal tkeep_d          : std_logic_vector(roce_hdr'length/8-1 downto 0);
   signal tlast_d          : std_logic;
   signal tvalid_d         : std_logic;
   
   signal ready            : std_logic;
begin

   -- Base transport header and Datagram extended header
   bth <=   HTON(pkt_seq_num) & LC_ROCE_RESERVED & LC_ROCE_A & 
            HTON(ROCE_TX_DEST_QP) & LC_ROCE_RESERVED_V & 
            LC_ROCE_B & LC_ROCE_F & LC_ROCE_P_KEY & 
            LC_ROCE_SE & LC_ROCE_M & LC_ROCE_PADCNT & 
            LC_ROCE_TVER & LC_ROCE_OPCODE_SEND;   
   
   deth <=  HTON(ROCE_TX_SRC_QP) & LC_ROCE_DETH_RES & 
            HTON(ROCE_TX_Q_KEY);   

--   process(CLK)
--      begin 
--         if rising_edge(CLK) then 
--            if RESET = '1' then 
--               pkt_seq_num <= ROCE_TX_PSN;
--            else
--               if (m_axis_tvalid_i and M_AXIS_TREADY and m_axis_tlast_i) = '1' then
--                  pkt_seq_num <= pkt_seq_num + 1;
--               end if;
--            end if;
--         end if;
--      end process;
   
   ready <= M_AXIS_TREADY or not m_axis_tvalid_i;
         
   process(CLK) 
      variable tmp   : std_logic_vector(tkeep_d'range);
      begin 
         if rising_edge(CLK) then
            tmp := s_axis_tkeep_d(s_axis_tkeep_d'left downto s_axis_tkeep_d'length - tkeep_d'length); 
            if RESET = '1' then
               M_EXTRA_DATA    <= (others => '0');
               UDP_TX_DATA_LEN <= (others => '0');
               m_axis_tvalid_i <= '0';
               m_axis_tlast_i <= '0';
               m_axis_tkeep_i <= (others => '0');
               m_axis_tdata_i <= (others => '0');
               s_axis_tvalid_d <= '0';            
               s_axis_tlast_d  <= '0';            
               s_axis_tkeep_d  <= (others => '0');
               s_axis_tdata_d  <= (others => '0');
               extra_data_d <= (others => '0');
               tdata_d  <= (others => '0');   
               tkeep_d  <= (others => '0');   
               roce_hdr_fsm <= SOP_ST;
               pkt_seq_num <= ROCE_TX_PSN;
            else     
               if ready = '1' then
                  -- One cycle delayed data
                  s_axis_tvalid_d <= S_AXIS_TVALID;
                  s_axis_tlast_d <= S_AXIS_TLAST;
                  s_axis_tkeep_d <= S_AXIS_TKEEP;
                  s_axis_tdata_d <= S_AXIS_TDATA;
                  extra_data_d <= S_EXTRA_DATA;
                  -- delay part of stream so roce header can
                  -- be insert on packet begining 
                  tdata_d <= s_axis_tdata_d(s_axis_tdata_d'left downto s_axis_tdata_d'length - tdata_d'length);   
                  tkeep_d <= s_axis_tkeep_d(s_axis_tkeep_d'left downto s_axis_tkeep_d'length - tkeep_d'length);   
                  
                  -- output data
                  case roce_hdr_fsm is 
                  when SOP_ST =>
                     M_EXTRA_DATA <= extra_data_d;
                     UDP_TX_DATA_LEN <= ROCE_TX_DATA_LEN + LC_ROCE_CRC_SIZE + deth'length/8 + bth'length/8;
                     m_axis_tvalid_i <= s_axis_tvalid_d;
                     m_axis_tdata_i <= s_axis_tdata_d(s_axis_tdata'left - deth'length - bth'length downto 0) & deth & bth;
                     m_axis_tkeep_i <= s_axis_tkeep_d(s_axis_tkeep'left - deth'length/8 - bth'length/8 downto 0) & x"FFFFF";
                     m_axis_tlast_i <= not or_vec(tmp) and s_axis_tvalid_d;
                     if s_axis_tvalid_d = '1' then
                        if or_vec(tmp) = '0' then 
                           roce_hdr_fsm <= SOP_ST;
                           pkt_seq_num <= pkt_seq_num + 1;
                        elsif s_axis_tlast_d = '1' then 
                           roce_hdr_fsm <= LAST_ST;
                        else
                           roce_hdr_fsm <= DATA_ST;
                        end if;
                     end if;
                  when DATA_ST => 
                     m_axis_tvalid_i <= s_axis_tvalid_d;
                     m_axis_tdata_i <= s_axis_tdata_d(s_axis_tdata'left - deth'length - bth'length downto 0) & tdata_d;
                     m_axis_tkeep_i <= s_axis_tkeep_d(s_axis_tkeep'left - deth'length/8 - bth'length/8 downto 0) & tkeep_d;
                     m_axis_tlast_i <= not or_vec(tmp) and s_axis_tvalid_d;
                     if s_axis_tvalid_d = '1' then
                        if or_vec(tmp) = '0' then 
                           roce_hdr_fsm <= SOP_ST;
                           pkt_seq_num <= pkt_seq_num + 1;
                        elsif s_axis_tlast_d = '1' then 
                           roce_hdr_fsm <= LAST_ST;
                        else
                           roce_hdr_fsm <= DATA_ST;
                        end if;
                     end if;
                  when LAST_ST =>
                     m_axis_tvalid_i <= '1';
                     m_axis_tdata_i(m_axis_tdata_i'left downto tdata_d'length) <= (others => '0');
                     m_axis_tdata_i(tdata_d'range) <= tdata_d;
                     m_axis_tkeep_i(m_axis_tkeep_i'left downto tkeep_d'length) <= (others => '0');
                     m_axis_tkeep_i(tkeep_d'range) <= tkeep_d;
                     m_axis_tlast_i <= '1';
                     roce_hdr_fsm <= SOP_ST;
                     pkt_seq_num <= pkt_seq_num + 1;
                  end case;  
               end if;
            end if;
         end if;
      end process;
      
   M_AXIS_TVALID <= m_axis_tvalid_i;   
   M_AXIS_TLAST <= m_axis_tlast_i;   
   M_AXIS_TKEEP <= m_axis_tkeep_i;   
   M_AXIS_TDATA <= m_axis_tdata_i;   
   S_AXIS_TREADY <= '0' when roce_hdr_fsm = LAST_ST else ready and not RESET;
      
   
end;
