library ieee; use ieee.std_logic_1164.all; use ieee.numeric_std.all; use work.math_pkg.all; entity rtps_handler is generic( ); port ( clk : in std_logic; -- Input Clock reset : in std_logic; -- Synchronous Reset empty : in std_logic; -- Input FIFO empty flag rd : out std_logic; -- Input FIFO read signal data_in : in std_logic_vector(31 downto 0); -- Input FIFO data signal ); end entity; architecture arch of rtps_handler is --*****COMPOENENT DECLARATION****** entity adder is generic ( PIPELINE_STAGES : integer := 1; DATA_WIDTH : integer := 32 ); port ( clk : in std_logic; reset : in std_logic; cin : in std_logic; A : in std_logic_vector(DATA_WIDTH-1 downto 0); B : in std_logic_vector(DATA_WIDTH-1 downto 0); RES : out std_logic_vector(DATA_WIDTH-1 downto 0); cout : out std_logic ); end entity; --*****CONSTANT DECLARATION***** -- Minimum Packet Length to consider valid -- 2 UDP Header 32-bit Words, 5 RTPS Header 32-bit Words constant MIN_PACKET_LENGTH : integer := 7; constant MAX_ENDPOINTS : integer := NUM_READERS+NUM_WRITERS; --GUIDPREFIX(1 downto 0) <= VENDORID; --*****TYPE DECLARATION***** type STAGE_TYPE is (INIT, SRC_ADDR, DEST_ADDR, UDP_HEADER_1, UDP_HEADER_2, RTPS_HEADER_1, RTPS_HEADER_2, RTPS_HEADER_3, RTPS_HEADER_4, SKIP_PACKET); --*****SIGNAL DECLARATION***** -- FSM state signal stage, stage_next : PARSER_STAGE_TYPE := INIT; -- Intermediate input read signal. (Read from output port not allowed) signal rd_sig : std_logic := '0'; -- Signal used to reset the word counter signal reset_read_cnt : std_logic; -- 32-bit word counter (Counts words read from input fifo) signal read_cnt : unsigned(13 downto 0) := (others => '0'); -- 32-bit aligned total packet length signal packet_length, packet_length_next : unsigned(13 downto 0) := (others => '0'); signal sub_length, sub_length_next : unsigned(13 downto 0) := (others => '0'); signal align_sig, align_sig_next : std_logic_vector(23 downto 0) := (others => '0'); signal aligned_data_in : std_logic_vector(31 downto 0); signal align_offset, align_offset_next : std_logic_vector(1 downto 0) := (others => '0'); signal offset_latch, offset_latch_next : std_logic_vector(1 downto 0) := (others => '0'); signal src_addr, src_addr_next : std_logic_vector(31 downto 0) := (others => '0'); signal src_port, src_port_next : std_logic_vector(15 downto 0) := (others => '0'); signal is_multicast, is_multicast_next : std_logic := '0'; signal is_metatraffic, is_metatraffic_next : std_logic := '0'; signal participant_id, participant_id_next : integer range 0 to NUM_DOMAIN-1 := 0; signal flags, flags_next : std_logic_vector(7 downto 0) := (others => '0'); -- General purpose counter signal cnt, cnt_next : integer := 0; --TODO: Range -- Adder Signals signal add_res, add_a, add_b : std_logic_vector(31 downto 0) := (others => '0'); signal add_cin, add_cout : std_logic := '0'; --*****ALIAS DEFINATION***** -- UDP HEADER alias udp_src_port : std_logic_vector(15 downto 0) is data_in(31 downto 16); alias udp_dest_port : std_logic_vector(15 downto 0) is data_in(15 downto 0); alias udp_length : std_logic_vector(15 downto 0) is data_in(31 downto 16); alias udp_checksum : std_logic_vector(15 downto 0) is data_in(15 downto 0); -- RTPS HEADER alias rtps_version : std_logic_vector(15 downto 0) is data_in(31 downto 16); alias rtps_vendorid : std_logic_vector(15 downto 0) is data_in(15 downto 0); -- RTPS SUBMESSAGE HEADER alias rtps_sub_id : std_logic_vector(7 downto 0) is aligned_data_in(31 downto 24); alias rtps_sub_flags : std_logic_vector(7 downto 0) is aligned_data_in(23 downto 16); alias rtps_sub_length : std_logic_vector(7 downto 0) is aligned_data_in(15 downto 0); -- ACKNACK alias rtps_acknack_final : std_logic is rtps_sub_flags(1); -- MISC --*****FUNCTION DECLARATION***** -- Truncates the lower 2 bits of the input, and if they are not equal zero, adds one to the result. -- This is used to round the byte length to 32-bit word length. function normalize_length (len : std_logic_vector(15 downto 0)) return std_logic_vector is variable tmp : std_logic_vector(13 downto 0) := (others => '0'); begin tmp := len(15 downto 2); if(len(1 downto 0) /= "00") then tmp := std_logic_vector(unsigned(tmp) + to_unsigned(1, tmp'length)); end if; return tmp; end function; -- Depending on the offset argument "off" returns a 32-bit slice from the 56-bit input signal -- This is used to extract 32-bit aligned words from the input signal function align_word ( off : std_logic_vector(1 downto 0), input : std_logic_vector(56 downto 0)) return std_logic_vector is variable ret : std_logic_vector(31 downto 0) := (others => '0'); begin case(off) is when "00" => ret := input(31 downto 0); when "01" => ret := input(39 downto 8); when "10" => ret := input(47 downto 16); when "11" => ret := input(55 downto 24); end case; return ret; end function; -- Compares argument 'ref' with every elemant of 'ar', and returns the index of the last match. -- If no match is found, array length is returned. function match_domain_id ( ref : std_logic_vector(UDP_PORT_WIDTH-1 downto 0), ar : IPv4_PORT_TYPE) return integer is variable id : integer := 0; begin id := ar'length(1); for i in 0 to ar'length-1 loop if(ref = ar(i)) then id := i; end if; end loop; return id; end function; -- Returns the 'data' argument either as is, or with reversed Byte order, depending on the -- 'endianness' argument. function endian_swap( endianness : std_logic, data :std_logic_vector(31 downto 0)) return std_logic_vector is variable ret : std_logic_vector(31 downto 0); begin -- Little Endian if (endianness = '1') then -- Reverse byte Order for i in 0 to 3 loop ret(i*8+8-1 downto i*8) := data((3-i)*8+8-1 downto (3-i)*8); end loop; -- Big Endian else ret := data; end if; return ret; end function; begin rd <= rd_sig; align_prc : process(all) variable input : std_logic_vector(55 downto 0) := (others => '0'); begin input := align_sig & data_in; case(align_offset) is when "00" => aligned_data_in <= input(31 downto 0); when "01" => aligned_data_in <= input(39 downto 8); when "10" => aligned_data_in <= input(47 downto 16); when "11" => aligned_data_in <= input(55 downto 24); end case; end process; checksum_adder : adder generic map ( PIPELINE_STAGES => 1, DATA_WIDTH => 32 ) port map( clk => clk, reset => reset, cin => add_cin, A => add_a, B => add_b, RES => add_res, cout => add_cout ); end entity; parse_prc: process(all) variable tmp : integer range 0 to MAX_ENDPOINTS := 0; begin --DEFAULT stage_next <= stage; reset_read_cnt <= '0'; add_a <= (others => '0'); add_b <= (others => '0'); add_cin <= '0'; cnt_next <= count; align_offset_next <= align_offset; align_sig_next <= align_sig; packet_length_next <= packet_length; sub_length_next <= sub_length; offset_latch_next <= offset_latch; src_addr_next <= src_addr; is_multicast_next <= is_multicast; src_port_next <= src_port; is_metatraffic_next <= is_metatraffic; participant_id_next <= participant_id; flags_next <= flags; case(stage) is -- Initial/Idle State -- Src Addr when SRC_ADDR => if (empty = '0') then rd_sig <= '1'; -- Latch Src Address src_addr_next <= data_in; -- Initiate Checksum Calculation add_a <= data_in; -- TODO: Reset Flags is_multicast_next <= '0'; is_metatraffic_next <= '0'; -- Next Stage stage_next <= DEST_ADDR; end if; -- Dest Addr when DEST_ADDR => if (empty = '0') then rd_sig <= '1'; -- Check Destination Address if (data_in = IPv4_UNICAST_ADDRESS or data_in = DEFAULT_IPv4_MULTICAST_ADDRESS) then -- Latch if Addr is Multicast if (data_in = DEFAULT_IPv4_MULTICAST_ADDRESS) then is_multicast_next <= '1'; end if; -- Checksum Calculation add_a <= data_in; add_b <= add_res; add_cin <= add_cout; -- Next Stage stage_next <= UDP_HEADER_1; -- packet not for us, skip else stage_next <= SKIP_PACKET; end if; else -- Hold Checksum Value add_a <= add_res; end if; when LEN => -- Reset packet Byte Counter reset_read_cnt <= '1'; -- Hold Checksum Value add_a <= add_res; if (empty = '0') then rd_sig <= '1'; -- Read Packet Length from input packet_length_next <= unsigned(data_in(13 downto 0)); -- Check Packet Length if(to_integer(unsigned(data_in(13 downto 0))) < MIN_PACKET_LENGTH) then stage_next <= SKIP_PACKET; else -- Begin Processing stage_next <= UDP_HEADER_1; end if; end if; -- First UDP Header word (Fields: Src Port, Dest Port) when UDP_HEADER_1 => if (empty = '0') then rd_sig <= '1'; -- Latch Src Port src_port_next <= udp_src_port; -- Checksum Calculation add_a <= data_in; add_b <= add_res; add_cin <= add_cout; -- Default Next Stage stage_next <= SKIP_PACKET; -- Check if Dest Port is valid, if not Skip Packet if (is_multicast = '1') then -- Check if Metatraffic (via Mutlicast) tmp := match_domain_id(udp_dest_port, META_IPv4_MULTICAST_PORT); if (tmp != MAX_ENDPOINTS) then is_metatraffic_next <= '1'; participant_id_next <= tmp; stage_next <= UDP_HEADER_2; -- Check if User Traffic (via Multicast) else tmp := match_domain_id(udp_dest_port, USER_IPv4_MULTICAST_PORT); if (tmp != MAX_ENDPOINTS) then stage_next <= UDP_HEADER_2; participant_id_next <= tmp; end if; end if; else -- Check if Metatraffic (via Unicast) tmp := match_domain_id(udp_dest_port, META_IPv4_UNICAST_PORT); if (tmp != MAX_ENDPOINTS) then is_metatraffic_next <= '1'; participant_id_next <= tmp; stage_next <= UDP_HEADER_2; -- Check if User Traffic (via Unicast) else tmp := match_domain_id(udp_dest_port, USER_IPv4_UNICAST_PORT); if (tmp != MAX_ENDPOINTS) then stage_next <= UDP_HEADER_2; participant_id_next <= tmp; end if; end if; end if; else -- Hold Checksum Value add_a <= add_res; end if; -- Second UDP Header Word (Fields: Length, Checksum) when UDP_HEADER_2 => if (empty = '0') then rd_sig <= '1'; -- If UPD header length does not match actual packet length, skip packet if (normalize_length(udp_length) /= std_logic_vector(packet_length)) then stage_next <= SKIP_PACKET; else -- Checksum Calculation add_a <= (udp_length & x"0000"); add_b <= add_res; add_cin <= add_cout; -- Next Stage stage_next <= RTPS_HEADER_1; end if; else -- Hold Checksum Value add_a <= add_res; end if; -- First RTPS Header word (Fields: Protocolld) when RTPS_HEADER_1 => if (empty = '0') then rd_sig <= '1'; -- If underlying Protocol is not RTPS, skip packet if(data_in /= PROTOCOLLD_RTPS) then stage_next <= SKIP_PACKET; -- Continue Parsing else -- Checksum Calculation add_a <= data_in; add_b <= add_res; add_cin <= add_cout; -- Next Stage stage_next <= RTPS_HEADER_2; end if; else -- Hold Checksum Value add_a <= add_res; end if; -- Second RTPS Header word (Fields: Protocol Version, Vendor ID) when RTPS_HEADER_2 => if (empty = '0') then rd_sig <= '1'; -- If RTPS Protocol Major Version is not 2, skip packet if(rtps_version(15 downto 8) /= PROTOCOLVERSION_2_4(15 downto 8)) then stage_next <= SKIP_PACKET; -- Continue Parsing else -- Checksum Calculation add_a <= data_in; add_b <= add_res; add_cin <= add_cout; -- Reset GP Counter cnt_next <= 1; -- Next Stage stage_next <= RTPS_HEADER_3; end if; else -- Hold Checksum Value add_a <= add_res; end if; -- Rest of RTPS Header (Fields: GUID Prefix) when RTPS_HEADER_3 => if (empty = '0') then rd_sig <= '1'; -- Sender GUID_Prefix TODO <= data_in; -- Checksum Calculation add_a <= data_in; add_b <= add_res; add_cin <= add_cout; if (cnt = GUIDPREFIX_WIDTH/32) then -- Next Stage stage_next <= RTPS_SUB_HEADER; else cnt_next <= cnt + 1; end if; else -- Hold Checksum Value add_a <= add_res; end if; -- NOTE: From here on, due to the nature of the RTPS Protocol, 32-bit word alignement -- is not guaranteed, and has to be handled. -- RTPS Submessage Header (Fields: Submessage ID, Flags, Submessage Length) when RTPS_SUB_HEADER => if (empty = '0') then rd_sig <= '1'; case (rtps_sub_id) is when SID_ACKNACK => -- Ignore Submessage if Final Flag not set if (rtps_acknack_final = '1') then -- Next Stage stage_next <= ACKNACK_1; else -- Skip Submessage stage_next <= SKIP_SUB; end if; when SID_PAD => when SID_HEARTBEAT => when SID_GAP => when SID_INFO_TS => --state when SID_INFO_SRC => --state when SID_INFO_REPLY_IP4 => when SID_INFO_DST => --state when SID_INFO_REPLY => --state when SID_NACK_FRAG => when SID_HEARTBEAT_FRAG => when SID_DATA => when SID_DATA_FRAG => -- Unknown ID, skip submessage when others => stage_next <= SKIP_SUB; end case; -- Checksum Calculation add_a <= data_in; add_b <= add_res; add_cin <= add_cout; -- Store Submessage Length -- TODO: The below conditional +1 adder should be solved with a carry-in sub_length_next <= packet_length + unsigned(normalize_length(rtps_sub_length)); -- Store Byte offset of next Header offset_latch_next <= unsigned(align_offset) + unsigned(rtps_sub_length(1 downto 0)); -- Next Stage align_sig_next <= data_in(23 downto 0); else -- Hold Checksum Value add_a <= add_res; end if; when ACKNACK_1 => if (empty = '0') then rd_sig <= '1'; --TODO -- Checksum Calculation add_a <= data_in; add_b <= add_res; add_cin <= add_cout; -- Next Stage align_sig_next <= data_in(23 downto 0); else -- Hold Checksum Value add_a <= add_res; end if; when SKIP_PACKET => if (empty = '0') then rd_sig <= '1'; -- End of Packet if (read_cnt = packet_length) then -- Continue parsing next packet stage_next <= SRC_ADDR; end if; end if; when SKIP_SUB => if (empty = '0') then rd_sig <= '1'; -- End of Packet (No further Submessages) if (read_cnt = packet_length) then -- Continue parsing next packet stage_next <= SRC_ADDR; -- End of Submessage elsif (read_cnt = sub_length) then -- Begin parsing of next submessage stage_next <= RTPS_SUB_HEADER; -- Fix alignement align_offset <= offset_latch; end if; end if; when others => null; end case; end process; -- Process responsible for counting read words -- This process uses the actual FIFO read signals to determine reads word_counter_prc : process(clk, reset) begin if rising_edge(clk) then -- Reset Read counter if (reset = '1' or reset_read_cnt = '1') then read_cnt <= to_unsigned(1, read_cnt'length); -- Increment read counter each time rd is high elsif (rd_sig = '1' or buffer_ren = '1') then read_cnt <= read_cnt + to_unsigned(1, read_cnt'length); end if; end if; end process; end architecture;