From ce72c147a4b89fa7ac04da6b4a9869ae7508658a Mon Sep 17 00:00:00 2001 From: Greek Date: Thu, 29 Oct 2020 11:31:41 +0100 Subject: [PATCH] * Re-wrote "rtps_ahandler" - Compiles --- src/TODO.txt | 7 +- src/rtps_builtin_endpoint.vhd | 58 +- src/rtps_handler.vhd | 1273 ++++++++++++++++----------------- src/rtps_handler.vhd.BAK | 516 ------------- src/rtps_package.vhd | 47 +- src/test.vhd | 41 +- syn/DE10-Nano/top.qsf | 9 +- 7 files changed, 689 insertions(+), 1262 deletions(-) delete mode 100644 src/rtps_handler.vhd.BAK diff --git a/src/TODO.txt b/src/TODO.txt index bafdfa3..998332d 100644 --- a/src/TODO.txt +++ b/src/TODO.txt @@ -102,9 +102,4 @@ DESIGN DECISIONS between Delay and Suppression. This reduces the resolution from 0.23 ns to 0.47 ns -- Input FIFO Guard --- Output FIFO Guard --- Memory Operation Guard --- Ignore in-line QoS --- Only relevant for Endpoint Discovery Protocol --- Check QoS Compatibility (Unmark match on incompatibility) --- COMPATIBLE (DDS v1.4): offered >= requested, with INSTANCE < TOPIC < GROUP \ No newline at end of file +-- Output FIFO Guard \ No newline at end of file diff --git a/src/rtps_builtin_endpoint.vhd b/src/rtps_builtin_endpoint.vhd index a23eaae..f61723a 100644 --- a/src/rtps_builtin_endpoint.vhd +++ b/src/rtps_builtin_endpoint.vhd @@ -120,7 +120,7 @@ architecture arch of rtps_builtin_endpoint is end record; --*****CONSTANT DECLARATION***** - -- Memory Size in 32-bit Words + -- Memory Size in 4-Byte Words constant BUILTIN_BUFFER_SIZE : natural := MAX_REMOTE_PARTICIPANTS*PARTICIPANT_FRAME_SIZE; -- Memory Address Width constant BUILTIN_BUFFER_ADDR_WIDTH : natural := log2c(BUILTIN_BUFFER_SIZE); @@ -189,7 +189,7 @@ architecture arch of rtps_builtin_endpoint is signal rd_sig : std_logic := '0'; -- Signal used to reset the word counter signal reset_read_cnt : std_logic; - -- Word (32-bit) counter (Counts words read from input fifo) + -- Word (4-Byte) counter (Counts words read from input fifo) signal read_cnt : unsigned(13 downto 0) := (others => '0'); -- RTPS Submessage ID Latch signal opcode, opcode_next : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := (others => '0'); @@ -215,7 +215,7 @@ architecture arch of rtps_builtin_endpoint is signal data_in_swapped : std_logic_vector(31 downto 0) := (others => '0'); -- Byte Length of string signal string_length, string_length_next : unsigned(31 downto 0) := (others => '0'); - -- Counter of compared string words (32-bit) + -- Counter of compared string words (4-Byte) signal compare_length, compare_length_next : unsigned(29 downto 0) := (others => '0'); -- Bitmask of local Endpoint Matches signal endpoint_mask, endpoint_mask_next : std_logic_vector(0 to MAX_ENDPOINTS-1) := (others => '0'); @@ -390,27 +390,6 @@ architecture arch of rtps_builtin_endpoint is return ret; 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) return std_logic_vector is - variable ret : std_logic_vector(data'range); - begin - -- Assert that Data Signal is Byte aligned - assert (data'length mod 8 = 0) severity failure; - -- Little Endian - if (endianness = '1') then - -- Reverse byte Order - for i in 0 to (data'length/8)-1 loop - ret(i*8+8-1 downto i*8) := data(((data'length/8)-1-i)*8+8-1 downto ((data'length/8)-1-i)*8); - end loop; - -- Big Endian - else - ret := data; - end if; - return ret; - end function; - begin --*****COMPONENT INSTANTIATION***** @@ -721,7 +700,7 @@ begin -- Reset Latches def_addr_next <= (others => '0'); - meta_addr_next <= DEFAULT_IPv4_MULTICAST_ADDRESS; + meta_addr_next <= DEFAULT_IPv4_META_ADDRESS; def_port_next <= (others => '0'); meta_port_next <= META_IPv4_MULTICAST_PORT; lease_duration_next <= (others => (others => '0')); @@ -840,7 +819,7 @@ begin cnt_next <= 0; -- Unmark all Writers - endpoint_mask_next(NUM_READERS to MAX_ENDPOINTS-1) <= (others => '0'); + endpoint_mask_next <= ENDPOINT_READERS; when others => null; end case; @@ -873,7 +852,7 @@ begin cnt_next <= 0; -- Unmark all Readers - endpoint_mask_next(0 to NUM_READERS-1) <= (others => '0'); + endpoint_mask_next <= not ENDPOINT_READERS; when others => null; end case; @@ -907,7 +886,7 @@ begin cnt_next <= 0; -- Unmark all Writers - endpoint_mask_next(NUM_READERS to MAX_ENDPOINTS-1) <= (others => '0'); + endpoint_mask_next <= ENDPOINT_READERS; when others => null; end case; @@ -960,7 +939,7 @@ begin cnt_next <= 0; -- Unmark all Writers - endpoint_mask_next(NUM_READERS to MAX_ENDPOINTS-1) <= (others => '0'); + endpoint_mask_next <= ENDPOINT_READERS; when others => null; end case; @@ -993,7 +972,7 @@ begin cnt_next <= 0; -- Unmark all Readers - endpoint_mask_next(0 to NUM_READERS-1) <= (others => '0'); + endpoint_mask_next <= not ENDPOINT_READERS; when others => null; end case; @@ -1027,7 +1006,7 @@ begin cnt_next <= 0; -- Unmark all Writers - endpoint_mask_next(NUM_READERS to MAX_ENDPOINTS-1) <= (others => '0'); + endpoint_mask_next <= ENDPOINT_READERS; when others => null; end case; @@ -1907,7 +1886,7 @@ begin null; end case; - -- NOTE: "compare_length" counts the 32-bit words, while "string_length" contains the total string octets/bytes + -- NOTE: "compare_length" counts the 4-Byte words, while "string_length" contains the total string octets/bytes -- End of String (Exit Condition) if ((compare_length & "00") >= string_length) then -- DONE @@ -1990,7 +1969,16 @@ begin else meta_port_next <= data_in_swapped(meta_port'length-1 downto 0); end if; - -- Locator Addr (IPv4) + -- Locator Address 1/4 + when 2 => + null; + -- Locator Address 2/4 + when 3 => + null; + -- Locator Address 3/4 + when 4 => + null; + -- Locator Address 4/4 (IPv4) when 5 => -- Latch Src Addr if (is_meta_addr = '0') then @@ -2547,12 +2535,12 @@ begin -- OUTPUT HEADER -- Src IPv4 Address when 0 => - output_sig <= DEFAULT_IPv4_MULTICAST_ADDRESS; + output_sig <= DEFAULT_IPv4_META_ADDRESS; -- Dest IPv4 Address when 1 => -- Set Default Multicast Announce Address if Participant Announcement if (return_stage = SEND_PARTICIPANT_ANNOUNCEMENT) then - output_sig <= DEFAULT_IPv4_MULTICAST_ADDRESS; + output_sig <= DEFAULT_IPv4_META_ADDRESS; else output_sig <= mem_participant_data.meta_addr; end if; diff --git a/src/rtps_handler.vhd b/src/rtps_handler.vhd index b66e7e8..f368d02 100644 --- a/src/rtps_handler.vhd +++ b/src/rtps_handler.vhd @@ -5,12 +5,8 @@ use ieee.numeric_std.all; use work.math_pkg.all; use work.rtps_package.all; --- TODO: Remove alignment logic for RTPS Submessages, since all Submessages are 32-bit aligned --- TODO: Merge CHECK_SUB_END and SKIP_SUB stages --- TODO: Remove payload length and implement "last_word" bit --- TODO: Change all Endpoint Bit Vectors to "to" Ranges --- TODO: Move to single DOMAIN ID logic -- Checksum has to be checked before +-- TODO: Change "endian-swap" to "big-endian" throughout entity rtps_handler is port ( @@ -19,12 +15,13 @@ entity rtps_handler is 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 - builtin_output : out BUILTIN_ENDPOINT_TYPE; -- Output FIFO (Built-In Endpoints) data signal - builtin_full : in std_logic_vector(NUM_DOMAIN-1 downto 0); -- Output FIFO (Built-In Endpoints) full signal - builtin_wr : out std_logic_vector(NUM_DOMAIN-1 downto 0); -- Output FIFO (Built-In Endpoints) write signal - user_output : out USER_ENDPOINT_OUTPUT; -- Output FIFO (User Endpoints) data signal - user_full : in std_logic_vector(MAX_ENDPOINTS-1 downto 0); -- Output FIFO (User Endpoints) full signal - user_wr : out std_logic_vector(MAX_ENDPOINTS-1 downto 0) -- Output FIFO (User Endpoints) write signal + builtin_output : out std_logic_vector(31 downto 0); -- Output FIFO (Built-In Endpoint) data signal + builtin_full : in std_logic; -- Output FIFO (Built-In Endpoint) full signal + builtin_wr : out std_logic; -- Output FIFO (Built-In Endpoint) write signal + user_output : out std_logic_vector(31 downto 0); -- Output FIFO (User Endpoints) data signal + user_full : in std_logic_vector(0 to MAX_ENDPOINTS-1); -- Output FIFO (User Endpoints) full signal + user_wr : out std_logic_vector(0 to MAX_ENDPOINTS-1); -- Output FIFO (User Endpoints) write signal + last_word_out : out std_logic -- Output FIFO Last Word signal ); end entity; @@ -33,96 +30,84 @@ architecture arch of rtps_handler is --*****COMPONENT DECLARATION****** --*****CONSTANT DECLARATION***** - -- Minimum Packet Length to consider valid (32-bit Words) - -- 2 UDP Header, 5 RTPS Header - constant MIN_PACKET_LENGTH : integer := 7; + -- *MINIMUM SIZES* + -- Minimum Packet Length to consider valid (4-Byte Words) + -- 2 UDP Header, 5 RTPS Header, 1 RTPS Submessage Header + constant MIN_PACKET_LENGTH : natural := 8; -- Minimum ACKNACK Submessage Size (Bytes) - -- 4 Reader ID, 4 Writer ID, 12 SequenceNumberSet (8 BitmapBase, 4 numBits), 4 Count - constant MIN_ACKNACK_SIZE : integer := 24; + -- 4 Reader ID, 4 Writer ID, 12 SequenceNumberSet (8 BitmapBase, 4 numBits), 2 Count + constant MIN_ACKNACK_SIZE : natural := 22; -- Minimum INFO_DST Submessage Size (Bytes) -- 12 GUID Prefix - constant MIN_INFO_DST_SIZE : integer := 12; + constant MIN_INFO_DST_SIZE : natural := 12; -- Minimum INFO_SRC Submessage Size (Bytes) -- 4 unused, 2 ProtocolVersion, 2 VendorID, 12 GUID Prefix - constant MIN_INFO_SRC_SIZE : integer := 20; + constant MIN_INFO_SRC_SIZE : natural := 20; -- Minimum INFO_REPLY Submessage Size (Bytes) - -- 4 Unicast LocatorList (4 numLocators) - constant MIN_INFO_REPLY_SIZE : integer := 4; + -- 4 LocatorList (4 numLocators) [Zero Size Locator List] + constant MIN_INFO_REPLY_SIZE : natural := 4; -- Minimum INFO_REPLY_IP4 Submessage Size (Bytes) - -- 8 Unicast LocatorUPDv4 (4 Addr, 4 Port) - constant MIN_INFO_REPLY_IP4_SIZE: integer := 8; + -- 8 LocatorUPDv4 (4 Addr, 4 Port) + constant MIN_INFO_REPLY_IPv4_SIZE : natural := 8; -- Minimum GAP Submessage Size (Bytes) -- 4 Reader ID, 4 Writer ID, 8 SequenceNumber, 12 SequenceNumberSet (8 BitmapBase, 4 numBits) - constant MIN_GAP_SIZE : integer := 28; + constant MIN_GAP_SIZE : natural := 28; -- Minimum HEARTBEAT Submessage Size (Bytes) - -- 4 Reader ID, 4 Writer ID, 8 SequenceNumber, 8 SequenceNumber, 4 Count - constant MIN_HEARTBEAT_SIZE : integer := 28; + -- 4 Reader ID, 4 Writer ID, 8 SequenceNumber, 8 SequenceNumber, 2 Count + constant MIN_HEARTBEAT_SIZE : natural := 26; -- Minimum DATA Submessage Size (Bytes) -- 2 Extra Flags, 2 octetstoinlineQoS, 4 Reader ID, 4 Writer ID, 8 SequenceNumber - constant MIN_DATA_SIZE : integer := 20; + constant MIN_DATA_SIZE : natural := 20; --*****TYPE DECLARATION***** -- FSM states. Explained below in detail type STAGE_TYPE is (SRC_ADDR_HEADER, DEST_ADDR_HEADER, LEN_HEADER, UDP_HEADER_1, UDP_HEADER_2, RTPS_HEADER_1, RTPS_HEADER_2, - RTPS_HEADER_3, RTPS_SUB_HEADER, EXTRACT_LOCATOR_UDPv4_1, EXTRACT_LOCATOR_UDPv4_2, - EXTRACT_LOCATOR_LIST, EXTRACT_LOCATOR, INFO_SRC_HEADER, EXTRACT_DOMAIN_ID, SRC_ENDPOINT, DATA_HEADER, - MATCH_DST_ENDPOINT, PUSH_PAYLOAD_HEADER, PUSH_DEST, DATA_SKIP_HEADER, PUSH_PAYLOAD, CHECK_SUB_END, SKIP_PACKET, - SKIP_SUB); + RTPS_HEADER_3, RTPS_SUB_HEADER, CHECK_GUID_PREFIX, INFO_SRC_HEADER, EXTRACT_LOCATOR_LIST, EXTRACT_LOCATOR, EXTRACT_LOCATOR_UDPv4, + DATA_HEADER, LATCH_SUBMESSAGE, SKIP_DATA_HEADER, MATCH_DST_ENDPOINT, PUSH_PAYLOAD_HEADER, PUSH_PAYLOAD, SKIP_SUB, SKIP_PACKET); + type IPv4_ADDRESS_TYPE is (DEFAULT_META_ADDR, DEFAULT_ADDR, INVALID); --*****SIGNAL DECLARATION***** -- FSM state signal stage, stage_next : STAGE_TYPE := SRC_ADDR_HEADER; - -- FSM state latch. Used to transition dynamically to different states from the same state. - signal return_stage, return_stage_next : STAGE_TYPE := SRC_ADDR_HEADER; -- 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) + -- 4-Byte Word counter (Counts words read from input FIFO) signal read_cnt : unsigned(13 downto 0) := (others => '0'); - -- 32-bit Word aligned total packet length + -- Total packet length (4-Byte Words) signal packet_length, packet_length_next : unsigned(13 downto 0) := (others => '0'); - -- 32-bit Word aligned end of Submessage - signal sub_end, sub_end_next : unsigned(13 downto 0) := (others => '0'); - -- 32-bit Word aligned end of Data Header (Beginning of inlineQoS/Payload) - signal data_header_end, data_header_end_next : unsigned(13 downto 0) := (others => '0'); - -- Input Signal Latch. Used to read 32-bit Word aligned from input + -- End of Submessage from the beginning of the UDP Packet in Bytes + signal sub_end, sub_end_next : unsigned(15 downto 0) := (others => '0'); + -- End of DATA Submessage Sub-Header (Beginning of inlineQoS/Payload) from the beginning of the UDP Packet in Bytes + signal data_header_end, data_header_end_next : unsigned(15 downto 0) := (others => '0'); + -- Input Signal Latch. Used to read 4-Byte aligned from input (see align_prc) signal align_sig, align_sig_next : std_logic_vector(23 downto 0) := (others => '0'); - -- 32-bit Word Aligned Input - signal aligned_data_in : std_logic_vector(31 downto 0); - -- 32-bit Word Aligned Input, Padded with Zeroes (The last read from input with less than 32-bits gets padded with zeroes) - signal aligned_data_in_padded : std_logic_vector(31 downto 0) := (others => '0'); - -- 32-bit Word alignement offset + -- 4-Byte Aligned Input (see align_prc) + signal data_in_aligned : std_logic_vector(31 downto 0); + -- 4-Byte Alignement offset (see align_prc) signal align_offset, align_offset_next : std_logic_vector(1 downto 0) := (others => '0'); - -- Alignement offset latch + -- 4-Byte Alignement offset latch signal offset_latch, offset_latch_next : std_logic_vector(1 downto 0) := (others => '0'); - -- Alignement offset latch (For Data Header) - signal data_header_offset_latch, data_header_offset_latch_next : std_logic_vector(1 downto 0) := (others => '0'); -- IPv4 Source Address latch signal src_addr, src_addr_next : std_logic_vector(31 downto 0) := (others => '0'); -- UDP Source Port latch signal src_port, src_port_next : std_logic_vector(15 downto 0) := (others => '0'); - -- Denotes if processed Message has Multicast Destination - signal is_multicast, is_multicast_next : std_logic := '0'; - -- Denotes if processed Message is Metatraffic destined for Built-In Endpoints + -- Denotes if processed Message is Metatraffic signal is_metatraffic, is_metatraffic_next : std_logic := '0'; - -- Domain ID/Participant ID target of processed Message - signal domain_id, domain_id_next : integer range 0 to NUM_DOMAIN-1 := 0; -- RTPS Submessage Flag latch signal flags, flags_next : std_logic_vector(7 downto 0) := (others => '0'); - -- Source Endpoint Entity ID latch - signal src_entityid, src_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); - -- Destination Endpoint Entity ID latch - signal dest_entityid, dest_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); + -- Reader Entity ID latch + signal reader_entityid, reader_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); + -- Writer Entity ID latch + signal writer_entityid, writer_entityid_next : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); -- Source GUID Prefix latch - signal guidprefix, guidprefix_next : GUIDPREFIX_ARRAY_TYPE := (others => (others => '0')); + signal src_guidprefix, src_guidprefix_next : GUIDPREFIX_TYPE := (others => (others => '0')); -- Vector denoting the Destination User Endpoints of the Message - signal user_endpoint, user_endpoint_next : std_logic_vector(MAX_ENDPOINTS-1 downto 0) := (others => '0'); - -- Denoting if the Message is destined for the Built-in Endpoints + signal user_endpoint, user_endpoint_next : std_logic_vector(0 to MAX_ENDPOINTS-1) := (others => '0'); + -- Denotes if the Message is destined for the Built-in Endpoints signal builtin_endpoint, builtin_endpoint_next : std_logic := '0'; - -- Length of Payload (32-bit Words) to be sent to Endpoints - signal payload_length, payload_length_next : std_logic_vector(13 downto 0) := (others => '0'); -- numLocator latch -- NOTE: Since Submessages are limited to 2^16 Bytes, we can limit this also to 16 bits signal numlocators, numlocators_next : unsigned(15 downto 0) := (others => '0'); @@ -134,13 +119,21 @@ architecture arch of rtps_handler is signal output_sig : std_logic_vector(31 downto 0) := (others => '0'); -- Intermediate Write Enable Signal signal wr_sig : std_logic := '0'; - -- Submessage ID latch (Used as OPCODe by Endpoints) + -- Submessage ID latch (Used as OPCODE by Endpoints) signal opcode, opcode_next : std_logic_vector(SUBMESSAGE_ID_WIDTH-1 downto 0) := (others => '0'); -- Denotes if Message is DATA Submessage signal is_data, is_data_next : std_logic := '0'; -- General Purpose counter -- TODO: Recheck range - signal cnt, cnt_next : integer range 0 to max(GUIDPREFIX_WIDTH/32, 6) := 0; + signal cnt, cnt_next : natural range 0 to 8 := 0; + -- Input in Big Endian representation + signal data_in_swapped : std_logic_vector(31 downto 0) := (others => '0'); + -- Sequence Number latch + signal seq_nr, seq_nr_next : DOUBLE_WORD_ARRAY := (others => (others => '0')); + -- Indicates the Destination IP Address + signal ip_addr_type, ip_addr_type_next : IPv4_ADDRESS_TYPE := INVALID; + -- Alias "substitution" + signal rtps_sub_length, rtps_sub_data_length : unsigned(15 downto 0) := (others => '0'); --*****ALIAS DEFINATION***** -- UDP HEADER @@ -152,70 +145,41 @@ architecture arch of rtps_handler is 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(15 downto 0) is aligned_data_in(15 downto 0); + alias rtps_sub_id : std_logic_vector(7 downto 0) is data_in(31 downto 24); + alias rtps_sub_flags : std_logic_vector(7 downto 0) is data_in(23 downto 16); + -- Apparently illegal alias expression + --alias rtps_sub_length : unsigned(15 downto 0) is unsigned(endian_swap(rtps_sub_flags(0), data_in(15 downto 0))); -- RTPS SUBMESSAGE FLAGS - alias rtps_sub_endianness : std_logic is flags(0); - alias rtps_sub_multicast : std_logic is flags(1); - alias rtps_sub_qos : std_logic is flags(1); - alias rtps_sub_data : std_logic is flags(2); - alias rtps_sub_key : std_logic is flags(3); + alias rtps_sub_endianness : std_logic is flags(0); + alias rtps_sub_multicast : std_logic is flags(1); + alias rtps_sub_multicast_next : std_logic is flags_next(1); + alias rtps_sub_qos : std_logic is flags(1); + alias rtps_sub_data : std_logic is flags(2); + alias rtps_sub_key : std_logic is flags(3); -- RTPS DATA SUBMESSAGE HEADER - alias rtps_sub_data_length : std_logic_vector(15 downto 0) is aligned_data_in(15 downto 0); + -- Apparently illegal alias expression + --alias rtps_sub_data_length : unsigned(15 downto 0) is unsigned(endian_swap(rtps_sub_endianness, data_in(15 downto 0))); --*****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. + -- This is used to round the byte length to 4-Byte 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)); + tmp := std_logic_vector(unsigned(tmp) + 1); 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 others => -- "11" - ret := input(55 downto 24); - end case; - return ret; - end function; - -- Compares argument 'ref' with every element of 'ar', and returns the index of the last match. -- If no match is found, array length is returned. - function match_id_port ( ref : std_logic_vector(UDP_PORT_WIDTH-1 downto 0); - ar : IPv4_PORT_TYPE) return integer is - variable id : integer := 0; + function match_id_endpoint (ref : std_logic_vector(ENTITYID_WIDTH-1 downto 0); ar : ENTITYID_TYPE) return natural is + variable id : natural := 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; - function match_id_endpoint ( ref : std_logic_vector(ENTITYID_WIDTH-1 downto 0); - ar : ENTITYID_TYPE) return integer is - variable id : integer := 0; - begin - id := ar'length(1); + id := ar'length; for i in 0 to ar'length-1 loop if(ref = ar(i)) then id := i; @@ -224,90 +188,69 @@ architecture arch of rtps_handler is 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) return std_logic_vector is - variable ret : std_logic_vector(data'range); - begin - -- Assert that Data Signal is Byte aligned - assert (data'length mod 8 = 0) severity failure; - -- Little Endian - if (endianness = '1') then - -- Reverse byte Order - for i in 0 to (data'length/8)-1 loop - ret(i*8+8-1 downto i*8) := data(((data'length/8)-1-i)*8+8-1 downto ((data'length/8)-1-i)*8); - end loop; - -- Big Endian - else - ret := data; - end if; - return ret; - end function; - - -- Pads (Overwrites) the 'data' Signal with Zeroes depending on the 'align_offset' argument. - function pad_signal( align_offset: std_logic_vector(1 downto 0); - data : std_logic_vector(31 downto 0)) return std_logic_vector is - variable ret : std_logic_vector(31 downto 0); - begin - ret := data; - case(align_offset) is - when "01" => - ret(23 downto 0) := (others => '0'); - when "10" => - ret(15 downto 0) := (others => '0'); - when "11" => - ret(8 downto 0) := (others => '0'); - when others => --"00" - null; - end case; - return ret; - end function; begin + -- ALIAS SUBSTITUTION + rtps_sub_length <= unsigned(endian_swap(rtps_sub_flags(0), data_in(15 downto 0))); + -- NOTE: While "rtps_sub_length" has to use the flag directly form the "data_in" signal to determine Endianness, here the + -- "flags" signal is already valid (and thus also its aliases) + rtps_sub_data_length <= unsigned(endian_swap(rtps_sub_endianness, data_in(15 downto 0))); + rd <= rd_sig; + -- This process swaps the input signal "data_in" to Big Endian Representation. + endian_swap_prc: process(all) + begin + -- DEFAULT + data_in_swapped <= data_in; + + -- If in Little Endian Representation + if (rtps_sub_endianness = '1') then + -- Endian Swap + for i in 0 to 3 loop + data_in_swapped(i*8+8-1 downto i*8) <= data_in((3-i)*8+8-1 downto (3-i)*8); + end loop; + end if; + end process; + -- This process connects the Intermediate Output Signals to the actual output FIFOs output_prc : process(all) begin -- Data Signal - for i in 0 to NUM_DOMAIN-1 loop - builtin_output(i) <= output_sig; - end loop; - for i in 0 to MAX_ENDPOINTS-1 loop - user_output(i) <= output_sig; - end loop; + builtin_output <= output_sig; + user_output <= output_sig; + --Write Enable Signal - builtin_wr <= (others => '0'); + builtin_wr <= '0'; user_wr <= (others => '0'); if (wr_sig = '1') then if (builtin_endpoint = '1') then - builtin_wr(domain_id) <= '1'; + builtin_wr <= '1'; else - user_wr <= user_endpoint; + user_wr <= user_endpoint; end if; end if; end process; - -- This process is responsible for reading the input FIFO 32-bit Word aligned. - -- Since the RTPS Header uses octet Length counters, the Elements are not guaranteed to be 32-bit - -- aligned (Which is how we read from the input FIFO). - -- We store the lower 2 bits of the octet length (Which denotes the 32-bit alignment offset), - -- and at the end of the Byte aligned Element we pad it to 32-bit, and calculate the new - -- alignement of the data following (Done by adding the current offset to the previous offset) + -- This process is responsible for reading the input FIFO 4-Byte Word aligned. + -- Even though DDSI-RTPS 2.3 defines that Submessages begin at 4-byte boundries, meaning that the + -- Submessage Length is always a multiple of 4, the same is not defined for the "octetstoinlineQoS". + -- Therefore alignment is not guaranteed from the "SKIP_DATA_HEADER" stage on and has to be ensured explicitly. + -- We store the lower 2 bits of the octet length (Which denotes the 4-Byte alignment offset), and together + -- with the "align_sig" which stores 3 Bytes from the previous input word, we generate our aligned signal. 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" => + data_in_aligned <= input(31 downto 0); when "01" => - aligned_data_in <= input(55 downto 24); + data_in_aligned <= input(39 downto 8); when "10" => - aligned_data_in <= input(47 downto 16); + data_in_aligned <= input(47 downto 16); when "11" => - aligned_data_in <= input(39 downto 8); - when others => -- "00" - aligned_data_in <= input(31 downto 0); + data_in_aligned <= input(55 downto 24); end case; end process; @@ -322,25 +265,23 @@ begin -- RTPS_HEADER_2 Parse second word of RTPS Header -- RTPS_HEADER_3 Parse GUID Prefix of RTPS Header (3rd-5th Word) -- RTPS_SUB_HEADER Parse RTPS Submessage Header - -- EXTRACT_LOCATOR_UDPv4_1 Read IPv4 Address from LocatorUDPv4 - -- EXTRACT_LOCATOR_UDPv4_2 Read UDP Port from LocatorUDPv4 + -- CHECK_GUID_PREFIX Validate Destination GUID Prefix + -- INFO_SRC_HEADER Parse INFO_SRC content -- EXTRACT_LOCATOR_LIST Parse LocatorList Head -- EXTRACT_LOCATOR Parse Locator - -- INFO_SRC_HEADER Parse INFO_SRC content - -- EXTRACT_DOMAIN_ID Extract Domain ID from GUID Prefix (Works only for GUID Prefixes generated by rtps_package) - -- SRC_ENDPOINT Read EntityID - -- DATA_HEADER Parse Submessage DATA Subheader + -- EXTRACT_LOCATOR_UDPv4 Parse LocatorUDPv4 + -- DATA_HEADER Parse Submessage DATA Sub-header + -- LATCH_SUBMESSAGE Latch routing information from Submessage (And Sequence Number for DATA Submessages) + -- SKIP_DATA_HEADER Skip uknown DATA Submessage Sub-Header part -- MATCH_DST_ENDPOINT Determine destination of Submessage -- PUSH_PAYLOAD_HEADER Write Payload Header into relevant output FIFOs - -- PUSH_DEST Write Destination Entity ID (Only for Built-In Endpoints) - -- DATA_SKIP_HEADER Read known DATA Submessage Subheader Elements, and skip the rest -- PUSH_PAYLOAD Read from input FIFO into relevant output FIFOs - -- CHECK_SUB_END Check if end of Submessage is reached, and handle accordingly - -- SKIP_PACKET Skip rest of UDP Packet -- SKIP_SUB Skip rest of Submessage + -- SKIP_PACKET Skip rest of UDP Packet parse_prc: process(all) - variable tmp : integer range 0 to MAX_ENDPOINTS := 0; + variable tmp : natural range 0 to MAX_ENDPOINTS := 0; variable tmp_length : std_logic_vector(15 downto 0) := (others => '0'); + variable dest : std_logic_vector(ENTITYID_WIDTH-1 downto 0) := (others => '0'); begin --DEFAULT Registered stage_next <= stage; @@ -352,301 +293,313 @@ begin offset_latch_next <= offset_latch; src_addr_next <= src_addr; src_port_next <= src_port; - domain_id_next <= domain_id; flags_next <= flags; - src_entityid_next <= src_entityid; - guidprefix_next <= guidprefix; - dest_entityid_next <= dest_entityid; + reader_entityid_next <= reader_entityid; + src_guidprefix_next <= src_guidprefix; + writer_entityid_next <= writer_entityid; user_endpoint_next <= user_endpoint; builtin_endpoint_next <= builtin_endpoint; - return_stage_next <= return_stage; numlocators_next <= numlocators; - payload_length_next <= payload_length; opcode_next <= opcode; data_header_end_next <= data_header_end; - data_header_offset_latch_next <= data_header_offset_latch; locator_match_next <= locator_match; is_metatraffic_next <= is_metatraffic; - is_multicast_next <= is_multicast; is_data_next <= is_data; src_is_reader_next <= src_is_reader; + seq_nr_next <= seq_nr; + ip_addr_type_next <= ip_addr_type; -- DEFAULT Unregistered + output_sig <= (others => '0'); rd_sig <= '0'; reset_read_cnt <= '0'; - output_sig <= (others => '0'); wr_sig <= '0'; + last_word_out <= '0'; case(stage) is -- Initial/Idle State - -- Src Addr + -- Source Address when SRC_ADDR_HEADER => + -- Input FIFO Guard if (empty = '0') then - rd_sig <= '1'; - -- Latch Src Address - src_addr_next <= data_in; - -- TODO: Reset Flags - is_multicast_next <= '0'; - is_metatraffic_next <= '0'; - -- Next Stage - stage_next <= DEST_ADDR_HEADER; + rd_sig <= '1'; + + src_addr_next <= data_in; + + stage_next <= DEST_ADDR_HEADER; end if; - -- Dest Addr + -- Destination Address when DEST_ADDR_HEADER => + -- Input FIFO Guard 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; - -- Next Stage - stage_next <= UDP_HEADER_1; - -- packet not for us, skip - else - stage_next <= SKIP_PACKET; - end if; + + case (data_in) is + when DEFAULT_IPv4_META_ADDRESS => + ip_addr_type_next <= DEFAULT_META_ADDR; + when DEFAULT_IPv4_ADDRESS => + ip_addr_type_next <= DEFAULT_ADDR; + when others => + ip_addr_type_next <= INVALID; + end case; + + stage_next <= LEN_HEADER; end if; -- UDP Packet Length when LEN_HEADER => - -- Reset packet Byte Counter - reset_read_cnt <= '1'; + -- Input FIFO Guard 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; + rd_sig <= '1'; + + -- Reset packet Byte Counter + reset_read_cnt <= '1'; + packet_length_next <= unsigned(data_in(13 downto 0)); + + -- DEFAULT + stage_next <= UDP_HEADER_1; + + -- SANITY CHECK: Check if Packet Length has minimal size + -- NOTE: This is needed beacause we check for the Packet End only after we reach the Submessage Header Stage + if(unsigned(data_in(13 downto 0)) < MIN_PACKET_LENGTH) then + -- Ignore + stage_next <= SKIP_PACKET; end if; end if; -- First UDP Header word (Fields: Src Port, Dest Port) when UDP_HEADER_1 => + -- Input FIFO Guard if (empty = '0') then - rd_sig <= '1'; - -- Latch Src Port + rd_sig <= '1'; + + -- DEFAULT + is_metatraffic_next <= '0'; + stage_next <= SKIP_PACKET; + src_port_next <= udp_src_port; - -- 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_id_port(udp_dest_port, META_IPv4_MULTICAST_PORT); - if (tmp /= MAX_ENDPOINTS) then + + -- Check Address and Port Destination for validity + case (ip_addr_type) is + -- Only accept Metatraffic form the Default Multicast Address + when DEFAULT_META_ADDR => is_metatraffic_next <= '1'; - domain_id_next <= tmp; - stage_next <= UDP_HEADER_2; - -- Check if User Traffic (via Multicast) - else - tmp := match_id_port(udp_dest_port, USER_IPv4_MULTICAST_PORT); - if (tmp /= MAX_ENDPOINTS) then - stage_next <= UDP_HEADER_2; - domain_id_next <= tmp; - end if; - end if; - else - -- Check if Metatraffic (via Unicast) - tmp := match_id_port(udp_dest_port, META_IPv4_UNICAST_PORT); - if (tmp /= MAX_ENDPOINTS) then - is_metatraffic_next <= '1'; - domain_id_next <= tmp; - stage_next <= UDP_HEADER_2; - -- Check if User Traffic (via Unicast) - else - tmp := match_id_port(udp_dest_port, USER_IPv4_UNICAST_PORT); - if (tmp /= MAX_ENDPOINTS) then - stage_next <= UDP_HEADER_2; - domain_id_next <= tmp; - end if; - end if; - end if; + case (udp_dest_port) is + when META_IPv4_MULTICAST_PORT => + stage_next <= UDP_HEADER_2; + when META_IPv4_UNICAST_PORT => + stage_next <= UDP_HEADER_2; + when others => + -- Ignore + null; + end case; + when DEFAULT_ADDR => + case (udp_dest_port) is + when META_IPv4_MULTICAST_PORT => + is_metatraffic_next <= '1'; + stage_next <= UDP_HEADER_2; + when META_IPv4_UNICAST_PORT => + is_metatraffic_next <= '1'; + stage_next <= UDP_HEADER_2; + when USER_IPv4_MULTICAST_PORT => + stage_next <= UDP_HEADER_2; + when USER_IPv4_UNICAST_PORT => + stage_next <= UDP_HEADER_2; + when others => + -- Ignore + null; + end case; + -- Packet not destined for us + when others => + -- Ignore + null; + end case; end if; -- Second UDP Header Word (Fields: Length, Checksum) when UDP_HEADER_2 => + -- Input FIFO Guard if (empty = '0') then rd_sig <= '1'; - -- If UPD header length does not match actual packet length, skip packet + -- NOTE: The UDP checksum has to be validated before, because we passthrough the input to the output FIFOs before we reach the end of the UDP Packet. + + -- SANITY CHECK: Check if UPD header length matches actual packet length if (normalize_length(udp_length) /= std_logic_vector(packet_length)) then + -- Ignore stage_next <= SKIP_PACKET; else - -- Next Stage stage_next <= RTPS_HEADER_1; end if; end if; -- First RTPS Header word (Fields: Protocolld) when RTPS_HEADER_1 => + -- Input FIFO Guard if (empty = '0') then rd_sig <= '1'; + -- If underlying Protocol is not RTPS, skip packet if(data_in /= PROTOCOL_RTPS) then stage_next <= SKIP_PACKET; - -- Continue Parsing else - -- Next Stage stage_next <= RTPS_HEADER_2; end if; end if; -- Second RTPS Header word (Fields: Protocol Version, Vendor ID) when RTPS_HEADER_2 => + -- Input FIFO Guard 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 - -- Reset GP Counter - cnt_next <= 1; - -- Next Stage stage_next <= RTPS_HEADER_3; - + cnt_next <= 0; end if; end if; -- Rest of RTPS Header (Fields: GUID Prefix) when RTPS_HEADER_3 => + -- Input FIFO Guard if (empty = '0') then rd_sig <= '1'; - -- Increment Counter cnt_next <= cnt + 1; - -- Latch Sender GUID_Prefix - if (cnt = 1) then - guidprefix_next(0) <= data_in; - elsif (cnt = 2) then - guidprefix_next(1) <= data_in; - elsif (cnt = 3) then - guidprefix_next(2) <= data_in; - stage_next <= RTPS_SUB_HEADER; - end if; + case (cnt) is + when 0 => + src_guidprefix_next(0) <= data_in; + when 1 => + src_guidprefix_next(1) <= data_in; + when 2 => + src_guidprefix_next(2) <= data_in; + stage_next <= RTPS_SUB_HEADER; + when others => + null; + end case; 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. - -- NOTE: This Entity only decodes the Destination Endpoint of the Submessages. The decoding + -- NOTE: This Entity only parses the Destination Endpoint of the Submessages. The decoding -- of the actual content is done in the respective Endpoints. - -- This decision was made because the Endpoints need to know the length of the contents beforehand, - -- and determining the length of the variable size Submessage contents adds unnecessary complexity - -- and latency. -- RTPS Submessage Header (Fields: Submessage ID, Flags, Submessage Length) when RTPS_SUB_HEADER => + -- Input FIFO Guard if (empty = '0') then rd_sig <= '1'; - - -- The case that a submessage is the last one (Length=0) is silently handled - -- by the 'SKIP_SUB' stage, which is taken after every Submessage (in order to skip - -- unknown parts of the Submessage and keep backwards Compatibility [see DDSI-RTPS 2.3 Section 8.3.4.1]). - -- TODO: Still valid? - - --DEFAULT + + -- DEFAULT src_is_reader_next <= '0'; is_data_next <= '0'; + flags_next <= rtps_sub_flags; + opcode_next <= rtps_sub_id; + + -- NOTE: DDSI-RTPS 2.3 only defines the 'INFO_TS' and 'PAD' Submessages to be able to have a zero Submessage Length. + -- If an unknown Submessage Type is encountered that can also have a valid zero Submessage length (Without being the last Submessage), + -- this will be seen here as "Last Submessage", and the complete rest of the Packet will be Skipped. + -- If Last Submessage, length is zero and actual size extend until end of packet + if (rtps_sub_length = 0) then + -- EXCEPTION: INFO_TS and PAD can have valid zero Submessage Length + if (rtps_sub_id /= SID_PAD and rtps_sub_id /= SID_INFO_TS) then + -- Fix Submessage End Position + sub_end_next <= packet_length & "00"; + end if; + else + sub_end_next <= (read_cnt & "00") + rtps_sub_length; + end if; + case (rtps_sub_id) is -- INFO_DST (Writer -> Reader, Update Destination GUID Prefix) - -- STAGE ORDER: EXTRACT_DOMAIN_ID -> CHECK_SUB_END -> (SKIP_SUB) -> RTPS_SUB_HEADER + -- STAGE ORDER: CHECK_GUID_PREFIX -> SKIP_SUB -> RTPS_SUB_HEADER when SID_INFO_DST => - -- Check Length - if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_DST_SIZE)) then + -- Check Submessage Length + if (rtps_sub_length /= 0 and rtps_sub_length < MIN_INFO_DST_SIZE) then -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) stage_next <= SKIP_PACKET; else - -- Initialise Counter - cnt_next <= 1; - -- Next Stage - stage_next <= EXTRACT_DOMAIN_ID; - return_stage_next <= CHECK_SUB_END; + stage_next <= CHECK_GUID_PREFIX; + cnt_next <= 0; end if; -- INFO_SRC (RTPS Header in Submessage form) -- STAGE ORDER: INFO_SRC_HEADER -> SKIP_SUB -> RTPS_SUB_HEADER when SID_INFO_SRC => - -- Check Length - if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_SRC_SIZE)) then + -- Check Submessage Length + if (rtps_sub_length /= 0 and rtps_sub_length < MIN_INFO_SRC_SIZE) then -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) stage_next <= SKIP_PACKET; else - -- Initialise Counter - cnt_next <= 1; - -- Next Stage stage_next <= INFO_SRC_HEADER; + cnt_next <= 0; end if; -- INFO_TS (Source Timestamp) - -- STAGE ORDER: SKIP_SUB -> RTPS_SUB_HEADER when SID_INFO_TS => -- IGNORE - stage_next <= CHECK_SUB_END; + stage_next <= SKIP_SUB; -- INFO_REPLY (Source Port and Address) - -- STAGE ORDER: EXTRACT_LOCATOR_LIST <-> EXTRACT_LOCATOR -> CHECK_SUB_END -> (SKIP_SUB) -> RTPS_SUB_HEADER + -- STAGE ORDER: EXTRACT_LOCATOR_LIST <-> EXTRACT_LOCATOR -> SKIP_SUB -> RTPS_SUB_HEADER when SID_INFO_REPLY => - -- Check Length - if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_REPLY_SIZE)) then + -- Check Submessage Length + if (rtps_sub_length /= 0 and rtps_sub_length < MIN_INFO_REPLY_SIZE) then -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) stage_next <= SKIP_PACKET; else - -- Parse Locators stage_next <= EXTRACT_LOCATOR_LIST; end if; -- INFO_REPLY (Source Port and Address) - -- STAGE ORDER: EXTRACT_LOCATOR_UDPv4_1 -> EXTRACT_LOCATOR_UDPv4_2 -> CHECK_SUB_END -> (SKIP_SUB) -> RTPS_SUB_HEADER + -- STAGE ORDER: EXTRACT_LOCATOR_UDPv4 -> SKIP_SUB -> RTPS_SUB_HEADER when SID_INFO_REPLY_IP4 => - -- Check Length - if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_INFO_REPLY_IP4_SIZE)) then + -- Check Submessage Length + if (rtps_sub_length /= 0 and rtps_sub_length < MIN_INFO_REPLY_IPv4_SIZE) then -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) stage_next <= SKIP_PACKET; else - -- Parse Locators - stage_next <= EXTRACT_LOCATOR_UDPv4_1; + stage_next <= EXTRACT_LOCATOR_UDPv4; + cnt_next <= 0; end if; -- Heartbeat (Writer -> Reader, Available SeqNum) - -- STAGE ORDER: MATCH_DST_ENDPOINT -> SRC_ENDPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER + -- STAGE ORDER: LATCH_SUBMESSAGE -> MATCH_DST_ENDPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER when SID_HEARTBEAT => - -- Check Length - if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_HEARTBEAT_SIZE)) then + -- Check Submessage Length + if (rtps_sub_length /= 0 and rtps_sub_length < MIN_HEARTBEAT_SIZE) then -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) stage_next <= SKIP_PACKET; else - -- Next Stage - stage_next <= MATCH_DST_ENDPOINT; - return_stage_next <= SRC_ENDPOINT; + stage_next <= LATCH_SUBMESSAGE; + cnt_next <= 0; end if; -- AckNack (Reader -> Writer, Request SeqNum) - -- STAGE ORDER: SRC_ENDPOINT -> MATCH_DST_ENDPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER + -- STAGE ORDER: LATCH_SUBMESSAGE -> MATCH_DST_ENDPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER when SID_ACKNACK => - -- Check Length - if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_ACKNACK_SIZE)) then + -- Check Submessage Length + if (rtps_sub_length /= 0 and rtps_sub_length < MIN_ACKNACK_SIZE) then -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) stage_next <= SKIP_PACKET; else - -- Mark if destination is for writers + -- ACKNACK has Reader as Source src_is_reader_next <= '1'; - -- Next Stage - stage_next <= SRC_ENDPOINT; + + stage_next <= LATCH_SUBMESSAGE; + cnt_next <= 0; end if; -- GAP (Writer -> Reader, Invalidate SeqNum) - -- STAGE ORDER: MATCH_DST_ENDPOINT -> SRC_ENDPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER + -- STAGE ORDER: LATCH_SUBMESSAGE -> MATCH_DST_ENDPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER when SID_GAP => - -- Check Length - if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_GAP_SIZE)) then + -- Check Submessage Length + if (rtps_sub_length /= 0 and rtps_sub_length < MIN_GAP_SIZE) then -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) stage_next <= SKIP_PACKET; else - -- Next Stage - stage_next <= MATCH_DST_ENDPOINT; - return_stage_next <= SRC_ENDPOINT; + stage_next <= LATCH_SUBMESSAGE; + cnt_next <= 0; end if; -- DATA (Writer -> Reader, SeqNum+Data) - -- STAGE ORDER: DATA_HEADER -> MATCH_DST_ENDPOINT -> SRC_ENDPOINT -> PUSH_PAYLOAD_HEADER -> DATA_SKIP_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER + -- STAGE ORDER: DATA_HEADER -> LATCH_SUBMESSAGE -> SKIP_DATA_HEADER -> MATCH_DST_ENDPOINT -> PUSH_PAYLOAD_HEADER -> PUSH_PAYLOAD -> RTPS_SUB_HEADER when SID_DATA => - -- Check Length - if ( (rtps_sub_length /= (rtps_sub_length'reverse_range => '0')) and (to_integer(unsigned(rtps_sub_length)) < MIN_DATA_SIZE)) then + -- Check Submessage Length + if (rtps_sub_length /= 0 and rtps_sub_length < MIN_DATA_SIZE) then -- Invalid Submessage, Skip Packet (see DDSI-RTPS 2.3 Section 8.3.4.1) stage_next <= SKIP_PACKET; else - -- Next Stage (DEFAULT) - stage_next <= DATA_HEADER; is_data_next <= '1'; - -- Sanity Check + + stage_next <= LATCH_SUBMESSAGE; + cnt_next <= 0; + + -- SANITY CHECK: Check DATA Submessage Flags if (rtps_sub_qos = '0' and rtps_sub_data = '0' and rtps_sub_key = '0') then -- Submessage has no Data, skip stage_next <= SKIP_SUB; @@ -665,423 +618,409 @@ begin -- IGNORE stage_next <= SKIP_SUB; -- PAD (Variable Size Padding) - -- STAGE ORDER: CHECK_SUB_END -> (SKIP_PACKET) -> RTPS_SUB_HEADER when SID_PAD => - stage_next <= CHECK_SUB_END; + -- IGNORE + stage_next <= SKIP_SUB; -- Unknown ID, skip submessage when others => - -- NOTE: DDSI-RTPS 2.3 only defines the 'INFO_TS' and 'PAD' Submessages to be able to have a zero Submessage Length. - -- If an unknown Submessage Type is encountered that also can have a valid zero Submessage length, this will be - -- seen here as "Last Submessage", and the complete rest of the Packet will be Skipped. stage_next <= SKIP_SUB; end case; - -- Latch flags - flags_next <= rtps_sub_flags; - -- Latch Submessage ID - opcode_next <= rtps_sub_id; - -- Check if Last Submessage - if (rtps_sub_length = (rtps_sub_length'reverse_range => '0')) then - -- EXCEPTION: INFO_TS and PAD can have valid zero Submessage Length - if (rtps_sub_id /= SID_PAD and rtps_sub_id /= SID_INFO_TS) then - -- Fix Submessage End Position - sub_end_next <= packet_length; - -- Calculate Submessage Length - -- TODO: Check synthesized code (Should be Subtractor/Adder with carry in) - payload_length_next <= std_logic_vector(packet_length - read_cnt + to_unsigned(1,payload_length'length)); - end if; - else - -- Latch Submessage End - -- TODO: The below conditional +1 adder should be solved with a carry-in - -- TODO: Use GP Adder? - sub_end_next <= read_cnt + unsigned(normalize_length(endian_swap(rtps_sub_endianness,rtps_sub_length))); - -- Latch Submessage Size (+1 to adjust to the format recognised by the Endpoints) - if (rtps_sub_id = SID_DATA) then - -- (+5 to counter Data header subtraction) - payload_length_next <= std_logic_vector(unsigned(normalize_length(endian_swap(rtps_sub_endianness,rtps_sub_length))) + to_unsigned(5,payload_length'length)); - else - -- (+1 to adjust to the format recognised by the Endpoints) - payload_length_next <= std_logic_vector(unsigned(normalize_length(endian_swap(rtps_sub_endianness,rtps_sub_length))) + to_unsigned(1,payload_length'length)); - end if; - end if; - -- Latch Byte offset of next Header - offset_latch_next <= std_logic_vector(unsigned(align_offset) + unsigned(rtps_sub_length(1 downto 0))); - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); end if; - when EXTRACT_LOCATOR_UDPv4_1 => + when CHECK_GUID_PREFIX => + -- Input FIFO Guard + if (empty = '0') then + rd_sig <= '1'; + cnt_next <= cnt + 1; + + -- If Destination GUID Prefix is not us, skip the rest of the packet + case (cnt) is + -- GUID Prefix 1/3 + when 0 => + if (data_in /= GUIDPREFIX(0)) then + -- Ignore + stage_next <= SKIP_PACKET; + end if; + -- GUID Prefix 2/3 + when 1 => + if (data_in /= GUIDPREFIX(1)) then + -- Ignore + stage_next <= SKIP_PACKET; + end if; + -- GUID Prefix 3/3 + when 2 => + if (data_in /= GUIDPREFIX(2)) then + -- Ignore + stage_next <= SKIP_PACKET; + else + stage_next <= SKIP_SUB; + end if; + when others => + null; + end case; + end if; + when INFO_SRC_HEADER => + -- Input FIFO Guard if (empty = '0') then rd_sig <= '1'; - -- Latch Source Port - src_port_next <= endian_swap(rtps_sub_endianness, aligned_data_in)(src_port_next'length-1 downto 0); - -- Next Stage - stage_next <= EXTRACT_LOCATOR_UDPv4_2; - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); - end if; - when EXTRACT_LOCATOR_UDPv4_2 => - if (empty = '0') then - rd_sig <= '1'; - -- Latch Src Addr - -- TODO: Could it be that the address is INVALID, in favor of the possibly following Multicast Address? - src_addr_next <= endian_swap(rtps_sub_endianness, aligned_data_in); - -- NOTE: The Multicast locator that could possibly follow is ignored - -- Next Stage - stage_next <= CHECK_SUB_END; - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); + cnt_next <= cnt + 1; + + case (cnt) is + -- unused + when 0 => + null; + -- Protocol Version & Vendor ID + when 1 => + -- Check Major Protocol Version + if (data_in_swapped(31 downto 24) /= PROTOCOLVERSION_2_4(15 downto 8)) then + -- Protocol not supported, skip rest of Packet + stage_next <= SKIP_PACKET; + end if; + -- GUID Prefix 1/3 + when 2 => + src_guidprefix_next(0) <= data_in; + -- GUID Prefix 2/3 + when 3 => + src_guidprefix_next(1) <= data_in; + -- GUID Prefix 3/3 + when 4 => + src_guidprefix_next(2) <= data_in; + stage_next <= SKIP_SUB; + when others => + null; + end case; end if; when EXTRACT_LOCATOR_LIST => + -- Input FIFO Guard if (empty = '0') then rd_sig <= '1'; - -- Latch Number of Locators - numlocators_next <= unsigned(endian_swap(rtps_sub_endianness, aligned_data_in)(numlocators_next'length-1 downto 0)); - -- Initialise counter - cnt_next <= 1; - -- Next Stage + + numlocators_next <= unsigned(data_in_swapped(numlocators_next'length-1 downto 0)); + stage_next <= EXTRACT_LOCATOR; - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); + cnt_next <= 0; end if; when EXTRACT_LOCATOR => -- Check Number of Locators - if (numlocators = (numlocators'reverse_range => '0')) then + if (numlocators = 0) then -- Check if Multicast Locator List exists if (rtps_sub_multicast = '1') then - -- Parse Multicast Locator List + -- Reset Flag to prevent loop + rtps_sub_multicast_next <= '0'; + stage_next <= EXTRACT_LOCATOR_LIST; - -- Reset Flags to prevent loop - flags_next <= (others => '0'); -- No further Locators, next Submessage else - stage_next <= CHECK_SUB_END; + stage_next <= SKIP_SUB; end if; else + -- Input FIFO Guard if (empty = '0') then - rd_sig <= '1'; - -- Increment Counter (Default) - cnt_next <= cnt + 1; + rd_sig <= '1'; + cnt_next <= cnt + 1; + case (cnt) is -- Locator Kind - when 1 => + when 0 => -- Check if UDPv4 Locator - if (endian_swap(rtps_sub_endianness, aligned_data_in) = LOCATOR_KIND_UDPv4) then + if (data_in_swapped = LOCATOR_KIND_UDPv4) then locator_match_next <= '1'; else locator_match_next <= '0'; end if; -- Locator Port - when 2 => + when 1 => + -- We only store UDPv4 Locators if (locator_match = '1') then - -- Latch Source Port - src_port_next <= endian_swap(rtps_sub_endianness, aligned_data_in)(src_port_next'length-1 downto 0); + -- We only store valid Locators + if (data_in_swapped = (data_in_swapped'reverse_range => '0')) then + locator_match_next <= '0'; + else + src_port_next <= data_in_swapped(src_port_next'length-1 downto 0); + end if; end if; - -- Locator Addr (IPv4) - when 6 => - if (locator_match = '1') then - -- Latch Src Addr - src_addr_next <= endian_swap(rtps_sub_endianness, aligned_data_in); + -- Locator Address 1/4 + when 2 => + null; + -- Locator Address 2/4 + when 3 => + null; + -- Locator Address 3/4 + when 4 => + null; + -- Locator Address 4/4 (IPv4 Address) + when 5 => + -- We only store valid UDPv4 Locators + if (locator_match = '1' and data_in_swapped /= (data_in_swapped'reverse_range => '0')) then + src_addr_next <= data_in_swapped; -- Extract only first matching Locator and ignore the rest - stage_next <= CHECK_SUB_END; + stage_next <= SKIP_SUB; end if; -- Last Word of Locator - -- Decrement Locator Count numlocators_next <= numlocators - 1; - -- Reset Counter - cnt_next <= 1; + cnt_next <= 0; when others => null; end case; - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); end if; end if; - when INFO_SRC_HEADER => + when EXTRACT_LOCATOR_UDPv4 => + -- Input FIFO Guard if (empty = '0') then - rd_sig <= '1'; + rd_sig <= '1'; + cnt_next <= cnt + 1; case (cnt) is - -- Second Word of INFO_SRC Submessage - when 2 => - -- Check Major Protocol Version - if (aligned_data_in(31 downto 24) /= PROTOCOLVERSION_2_4(15 downto 8)) then - -- Protocol not supported, skip rest of Packet - stage_next <= SKIP_PACKET; + -- IPv4 Address + when 0 => + -- Store only valid Locators + if (data_in_swapped = (data_in_swapped'reverse_range => '0')) then + locator_match_next <= '0'; + else + locator_match_next <= '1'; + src_addr_next <= data_in_swapped; + end if; + -- UDPv4 Port + when 1 => + -- Store only valid Locators + if (locator_match = '1' and data_in_swapped /= (data_in_swapped'reverse_range => '0')) then + src_port_next <= data_in_swapped(src_port_next'length-1 downto 0); + stage_next <= SKIP_SUB; + -- Parse Multicast if available + elsif (rtps_sub_multicast = '1') then + locator_match_next <= '0'; + cnt_next <= 0; + -- No valid Locators + else + stage_next <= SKIP_SUB; end if; - when 3 => - guidprefix_next(0) <= aligned_data_in; - when 4 => - guidprefix_next(1) <= aligned_data_in; - when 5 => - guidprefix_next(2) <= aligned_data_in; - stage_next <= CHECK_SUB_END; when others => null; end case; - -- TODO: Check how many adders are synthesized for cnt/cnt_next - cnt_next <= cnt + 1; - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); - end if; - -- Extract Domain/participant ID from GUID Prefix - when EXTRACT_DOMAIN_ID => - if (empty = '0') then - rd_sig <= '1'; - if (cnt = GUIDPREFIX_WIDTH/32) then - -- NOTE: Assumes DOMAIN_ID_WIDTH is <= 32 - assert (DOMAIN_ID_WIDTH <= 32) severity FAILURE; - -- Latch Domain ID - domain_id_next <= to_integer(unsigned(endian_swap(rtps_sub_endianness, aligned_data_in))); - -- Next Stage (Dynamic) - stage_next <= return_stage; - else - cnt_next <= cnt + 1; - end if; - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); - end if; - when SRC_ENDPOINT => - if (empty = '0') then - rd_sig <= '1'; - -- Latch src Entity ID - src_entityid_next <= aligned_data_in; - -- Next Stage - if (src_is_reader = '1') then - stage_next <= MATCH_DST_ENDPOINT; - return_stage_next <= PUSH_PAYLOAD_HEADER; - else - stage_next <= PUSH_PAYLOAD_HEADER; - end if; - -- Initialise Counter - cnt_next <= 1; - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); end if; + -- DATA Submessage header (Extra Flags, octetstoinlineQoS) when DATA_HEADER => + -- Input FIFO Guard if (empty = '0') then rd_sig <= '1'; - -- Latch Data Header Length - data_header_offset_latch_next <= std_logic_vector(unsigned(align_offset) + unsigned(endian_swap(rtps_sub_endianness, rtps_sub_data_length)(1 downto 0))); - -- Latch offset - data_header_end_next <= read_cnt + unsigned(normalize_length(endian_swap(rtps_sub_endianness, rtps_sub_data_length))); - -- Fix Payload Size - payload_length_next <= std_logic_vector(unsigned(payload_length) - unsigned(normalize_length(endian_swap(rtps_sub_endianness, rtps_sub_data_length)))); - -- Next Stage - stage_next <= MATCH_DST_ENDPOINT; - return_stage_next <= SRC_ENDPOINT; - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); + + -- NOTE: Extra Flags are unused + -- Latch Length to skip Uknown Data Header Part and latch offset to ensure 4-Byte alignement (see align_prc) + offset_latch_next <= std_logic_vector(unsigned(align_offset) + rtps_sub_data_length(1 downto 0)); + data_header_end_next <= (read_cnt & "00") + rtps_sub_data_length; + + stage_next <= LATCH_SUBMESSAGE; + cnt_next <= 0; + end if; + when LATCH_SUBMESSAGE => + -- Input FIFO Guard + if (empty = '0') then + rd_sig <= '1'; + cnt_next <= cnt + 1; + + -- NOTE: Endianness is not handled here, and the contents are delivered as is to the Endpoints. + + case (cnt) is + -- Reader Entity ID + when 0 => + reader_entityid_next <= data_in; + -- Writer Entity ID + when 1 => + writer_entityid_next <= data_in; + -- If we Process a DATA Submessage, continue parsing the Sequence Number + if (is_data = '0') then + stage_next <= MATCH_DST_ENDPOINT; + end if; + -- Sequence Number 1/2 + when 2 => + seq_nr_next(0) <= unsigned(data_in); + -- Sequence Number 2/2 + when 3 => + seq_nr_next(1) <= unsigned(data_in); + stage_next <= SKIP_DATA_HEADER; + when others => + null; + end case; + end if; + when SKIP_DATA_HEADER => + -- Input FIFO Guard + if (empty = '0') then + -- End of Data Header + if (read_cnt >= data_header_end) then + stage_next <= MATCH_DST_ENDPOINT; + cnt_next <= 0; + -- Fix alignement + align_offset_next <= offset_latch; + -- Latch Input for alignment purposes + align_sig_next <= data_in(23 downto 0); + else + -- Skip-Read + rd_sig <= '1'; + end if; end if; when MATCH_DST_ENDPOINT => + -- Input FIFO Guard if (empty = '0') then rd_sig <= '1'; - -- Next Stage (Default, Dynamic) - stage_next <= return_stage; - -- *Check Dest Entity ID* - -- Default Values + + if (src_is_reader = '1') then + dest := writer_entityid; + else + dest := reader_entityid; + end if; + + -- DEFAULT user_endpoint_next <= (others => '0'); builtin_endpoint_next <= '0'; - -- Target ALL Endpoints of Domain - if (aligned_data_in = ENTITYID_UNKNOWN) then + stage_next <= PUSH_PAYLOAD_HEADER; + cnt_next <= 0; + + -- *Check Dest Entity ID* + -- Target ALL Endpoints + if (dest = ENTITYID_UNKNOWN) then if (is_metatraffic = '1') then builtin_endpoint_next <= '1'; else - user_endpoint_next <= DOMAIN_ENDPOINT_MAP(domain_id); -- Mark Only Writers - if (src_is_reader = '1') then - user_endpoint_next(MAX_ENDPOINTS-1 downto MAX_ENDPOINTS-NUM_READERS) <= (others => '0'); + if (src_is_reader = '1' and NUM_WRITERS /= 0) then + user_endpoint_next <= not ENDPOINT_READERS; -- Mark Only Readers - else - -- TODO: Needed? - if (NUM_WRITERS /= 0) then - user_endpoint_next(NUM_WRITERS-1 downto 0) <= (others => '0'); - end if; + elsif (NUM_READERS /= 0) then + user_endpoint_next <= ENDPOINT_READERS; end if; end if; -- Target Built-In Endpoints - elsif (is_metatraffic = '1' and aligned_data_in(7 downto 6) = BUILT_IN_ENTITY) then + elsif (is_metatraffic = '1' and dest(7 downto 6) = BUILT_IN_ENTITY) then builtin_endpoint_next <= '1'; -- Match User Entity ID elsif (is_metatraffic = '0') then - tmp := match_id_endpoint(aligned_data_in, ENTITYID); + tmp := match_id_endpoint(dest, ENTITYID); -- Entity non existent, skip Submessage if (tmp = MAX_ENDPOINTS) then stage_next <= SKIP_SUB; -- Entity ID Match else - -- NOTE: Inserting a Sanity Check here that prevents Reader-Reader and Writer-Writer - -- Communication would prevent unnecessary FIFO clutter. But the check needs - -- to compare if 'tmp' is inside a range, which was deemed more complicated than - -- necessary, since the Endpoints can handle traffic not destined to them. - user_endpoint_next(tmp) <= '1'; - builtin_endpoint_next <= '0'; + -- SANITY CHECK: Allow only Reader-Writer Communication + if ((src_is_reader xor ENDPOINT_READERS(tmp)) = '0') then + user_endpoint_next(tmp) <= '1'; + else + stage_next <= SKIP_SUB; + end if; end if; -- Destination Unreachable, skip Submessage else stage_next <= SKIP_SUB; end if; - -- Latch Destination Entity ID - dest_entityid_next <= aligned_data_in; - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); end if; when PUSH_PAYLOAD_HEADER => - -- NOTE: This is a synchronised push on potentially multiple output FIFOs. - -- If one FIFO gets full, the process stalls for all FIFOs. - -- If Output FIFOs not full - if (builtin_endpoint = '1' and builtin_full(domain_id) = '0') or (builtin_endpoint = '0' and ((user_endpoint and user_full) = (user_endpoint'reverse_range => '0'))) then - -- Increment Counter + -- NOTE: This is a synchronised push on potentially multiple output FIFOs. If one FIFO gets full, the process stalls for all FIFOs. + -- Output FIFO Guard + if (builtin_endpoint = '1' and builtin_full = '0') or (builtin_endpoint = '0' and ((user_endpoint and user_full) = (user_endpoint'reverse_range => '0'))) then cnt_next <= cnt + 1; + case (cnt) is - when 1 => - output_sig(payload_length'length-1 downto 0) <= payload_length; - wr_sig <= '1'; - when 2 => + -- OPCODE (Submessage ID), Submessage Flags, UPDv4 Source Port + when 0 => output_sig <= opcode & flags & src_port; wr_sig <= '1'; - when 3 => + -- IPv4 Source Address + when 1 => output_sig <= src_addr; wr_sig <= '1'; - when 4 => - output_sig <= src_entityid; - wr_sig <= '1'; - when 5 => - output_sig <= guidprefix(0); - wr_sig <= '1'; - when 6 => - output_sig <= guidprefix(1); - wr_sig <= '1'; - when 7 => - output_sig <= guidprefix(2); - wr_sig <= '1'; - -- Next Stage - if (builtin_endpoint = '1') then - -- If the destination is a built-in Endpoint, push also the destination ID, - -- so that the sub-entity knows the intended target. - -- For the User-Endpoints this is implicitly decoded by the domain ID and is not necessary. - stage_next <= PUSH_DEST; - else if (is_data = '1') then - stage_next <= DATA_SKIP_HEADER; - -- Reset Counter - cnt_next <= 1; + -- Source Entity ID + when 2 => + if (src_is_reader = '1') then + output_sig <= reader_entityid; else - stage_next <= PUSH_PAYLOAD; + output_sig <= writer_entityid; end if; + wr_sig <= '1'; + -- GUID Prefix 1/3 + when 3 => + output_sig <= src_guidprefix(0); + wr_sig <= '1'; + -- GUID Prefix 2/3 + when 4 => + output_sig <= src_guidprefix(1); + wr_sig <= '1'; + -- GUID Prefix 3/3 + when 5 => + output_sig <= src_guidprefix(2); + wr_sig <= '1'; + -- Destination ID [only for Built-in Endpoints] + when 6 => + if (builtin_endpoint = '1') then + if (src_is_reader = '1') then + output_sig <= writer_entityid; + else + output_sig <= reader_entityid; + end if; + wr_sig <= '1'; + end if; + -- Sequence Number 1/2 [only for DATA Submessages] + when 7 => + if (is_data = '1') then + output_sig <= std_logic_vector(seq_nr(0)); + wr_sig <= '1'; + end if; + -- Sequence Number 2/2 [only for DATA Submessages] + when 8 => + if (is_data = '1') then + output_sig <= std_logic_vector(seq_nr(1)); + wr_sig <= '1'; + end if; + + stage_next <= PUSH_PAYLOAD; when others => null; end case; end if; - -- Only Taken for built-in Endpoints - when PUSH_DEST => - -- If Output FIFOs not full (Only consider Builtin output FIFOs) - if (builtin_endpoint = '1' and builtin_full(domain_id) = '0') then - output_sig <= dest_entityid; - wr_sig <= '1'; - -- Next Stage - if (is_data = '1') then - stage_next <= DATA_SKIP_HEADER; - -- Reset Counter - cnt_next <= 1; - else - stage_next <= PUSH_PAYLOAD; - end if; - end if; - when DATA_SKIP_HEADER => - if (empty = '0') then - -- Known DATA Header Data (Sequence Number Field) - if (cnt <= 2) then - -- If Output FIFOs not full - if (builtin_endpoint = '1' and builtin_full(domain_id) = '0') or (builtin_endpoint = '0' and ((user_endpoint and user_full) = (user_endpoint'reverse_range => '0'))) then - rd_sig <= '1'; - -- Push Payload - output_sig <= endian_swap(rtps_sub_endianness, aligned_data_in); - wr_sig <= '1'; - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); - -- Increment Counter - cnt_next <= cnt + 1; - end if; - -- Uknown Header Portion - else - -- Skip Header Words - rd_sig <= '1'; - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); - end if; - -- End of Data Header - if (read_cnt = data_header_end) then - -- Push Submessage Contents - stage_next <= PUSH_PAYLOAD; - -- Fix alignement - align_offset_next <= data_header_offset_latch; - end if; - end if; when PUSH_PAYLOAD => + -- Input FIFO Guard if (empty = '0') then - -- If Output FIFOs not full - if (builtin_endpoint = '1' and builtin_full(domain_id) = '0') or (builtin_endpoint = '0' and ((user_endpoint and user_full) = (user_endpoint'reverse_range => '0'))) then + -- NOTE: This is a synchronised push on potentially multiple output FIFOs. If one FIFO gets full, the process stalls for all FIFOs. + -- Output FIFO Guard + if (builtin_endpoint = '1' and builtin_full = '0') or (builtin_endpoint = '0' and ((user_endpoint and user_full) = (user_endpoint'reverse_range => '0'))) then rd_sig <= '1'; - -- Push Payload - -- Last Payload Word - if (read_cnt = sub_end) then - -- Push Last Word of Payload with padding - output_sig <= pad_signal(offset_latch, aligned_data_in); - wr_sig <= '1'; - -- Begin parsing of next submessage - stage_next <= RTPS_SUB_HEADER; - -- Fix alignement - align_offset_next <= offset_latch; - else - -- Push Normal (Endianness is not handled here) - output_sig <= aligned_data_in; - wr_sig <= '1'; - end if; + -- Latch Input for alignment purposes align_sig_next <= data_in(23 downto 0); - end if; - end if; - -- Check Submessage/Packet End - when CHECK_SUB_END => - -- NOTE: Due to the behavior of the read counter, the length is +1 in this stage. - -- So we can either check the length+1 for equality, or do a relative comparison on length. - -- End of Packet (No further Submessages) - if (read_cnt > packet_length) then - -- Continue parsing next packet - stage_next <= SRC_ADDR_HEADER; - -- End of Submessage - elsif (read_cnt > sub_end) then - -- Begin parsing of next submessage - stage_next <= RTPS_SUB_HEADER; - -- Fix alignement - align_offset_next <= offset_latch; - -- Submessage has still "unknown" Content - else - stage_next <= SKIP_SUB; - 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_HEADER; + + -- Push Payload + output_sig <= data_in_aligned; + wr_sig <= '1'; + + -- Last Payload Word + if (read_cnt = sub_end) then + last_word_out <= '1'; + -- Begin parsing of next submessage + stage_next <= RTPS_SUB_HEADER; + -- Reset alignement + align_offset_next <= (others => '0'); + end if; end if; end if; when SKIP_SUB => - if (empty = '0') then + -- End of Packet (No further Submessages) + if (read_cnt >= packet_length) then + -- Continue parsing next packet + stage_next <= SRC_ADDR_HEADER; + elsif (read_cnt >= sub_end) then + -- Begin parsing of next submessage + stage_next <= RTPS_SUB_HEADER; + -- Input FIFO Guard + elsif (empty = '0') then + -- Skip-Read + rd_sig <= '1'; + end if; + when SKIP_PACKET => + -- End of Packet + if (read_cnt >= packet_length) then + -- Continue parsing next packet + stage_next <= SRC_ADDR_HEADER; + -- Input FIFO Guard + elsif (empty = '0') then + -- Skip-Read rd_sig <= '1'; - -- End of Packet (No further Submessages) - if (read_cnt = packet_length) then - -- Continue parsing next packet - stage_next <= SRC_ADDR_HEADER; - -- End of Submessage - elsif (read_cnt = sub_end) then - -- Begin parsing of next submessage - stage_next <= RTPS_SUB_HEADER; - -- Fix alignement - align_offset_next <= offset_latch; - end if; - -- Latch Input for alignment purposes - align_sig_next <= data_in(23 downto 0); end if; when others => null; @@ -1095,10 +1034,10 @@ 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); + read_cnt <= (others => '0'); -- Increment read counter each time rd is high elsif (rd_sig = '1') then - read_cnt <= read_cnt + to_unsigned(1, read_cnt'length); + read_cnt <= read_cnt + 1; end if; end if; end process; @@ -1108,36 +1047,32 @@ begin if rising_edge(clk) then if (reset = '1') then stage <= SRC_ADDR_HEADER; - return_stage <= SRC_ADDR_HEADER; - cnt <= 0; + ip_addr_type <= INVALID; align_offset <= (others => '0'); align_sig <= (others => '0'); packet_length <= (others => '0'); sub_end <= (others => '0'); offset_latch <= (others => '0'); src_addr <= (others => '0'); - src_port <= (others => '0'); - domain_id <= 0; + src_port <= (others => '0'); flags <= (others => '0'); - src_entityid <= (others => '0'); - guidprefix <= (others => (others => '0')); - dest_entityid <= (others => '0'); + reader_entityid <= (others => '0'); + writer_entityid <= (others => '0'); user_endpoint <= (others => '0'); numlocators <= (others => '0'); - payload_length <= (others => '0'); opcode <= (others => '0'); data_header_end <= (others => '0'); - data_header_offset_latch <= (others => '0'); + src_guidprefix <= (others => (others => '0')); + seq_nr <= (others => (others => '0')); + cnt <= 0; is_data <= '0'; src_is_reader <= '0'; locator_match <= '0'; builtin_endpoint <= '0'; is_metatraffic <= '0'; - is_multicast <= '0'; else stage <= stage_next; - return_stage <= return_stage_next; - cnt <= cnt_next; + ip_addr_type <= ip_addr_type_next; align_offset <= align_offset_next; align_sig <= align_sig_next; packet_length <= packet_length_next; @@ -1145,23 +1080,21 @@ begin offset_latch <= offset_latch_next; src_addr <= src_addr_next; src_port <= src_port_next; - domain_id <= domain_id_next; flags <= flags_next; - src_entityid <= src_entityid_next; - guidprefix <= guidprefix_next; - dest_entityid <= dest_entityid_next; + reader_entityid <= reader_entityid_next; + writer_entityid <= writer_entityid_next; user_endpoint <= user_endpoint_next; numlocators <= numlocators_next; - payload_length <= payload_length_next; opcode <= opcode_next; data_header_end <= data_header_end_next; - data_header_offset_latch <= data_header_offset_latch_next; + src_guidprefix <= src_guidprefix_next; + seq_nr <= seq_nr_next; + cnt <= cnt_next; is_data <= is_data_next; src_is_reader <= src_is_reader_next; locator_match <= locator_match_next; builtin_endpoint <= builtin_endpoint_next; is_metatraffic <= is_metatraffic_next; - is_multicast <= is_multicast_next; end if; end if; end process; diff --git a/src/rtps_handler.vhd.BAK b/src/rtps_handler.vhd.BAK deleted file mode 100644 index f4742dc..0000000 --- a/src/rtps_handler.vhd.BAK +++ /dev/null @@ -1,516 +0,0 @@ -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; \ No newline at end of file diff --git a/src/rtps_package.vhd b/src/rtps_package.vhd index 6b01f20..bd2aa9e 100644 --- a/src/rtps_package.vhd +++ b/src/rtps_package.vhd @@ -10,7 +10,7 @@ package rtps_package is --*****USER CONFIG***** -- Unicast IPv4 Address used by all RTPS Entities [Default 192.168.0.80] - constant IPv4_UNICAST_ADDRESS : std_logic_vector(31 downto 0) := x"C0A80080"; + constant DEFAULT_IPv4_ADDRESS : std_logic_vector(31 downto 0) := x"C0A80080"; -- Number of RTPS Writer Endpoints constant NUM_WRITERS : natural := 0; -- Number of RTPS Reader Endpoints @@ -142,7 +142,7 @@ package rtps_package is --*****DDSI-RTPS 2.3***** -- Default Multicast Ipv4 Address (239.255.0.1) - constant DEFAULT_IPv4_MULTICAST_ADDRESS : std_logic_vector(31 downto 0) := x"EFFF0001"; + constant DEFAULT_IPv4_META_ADDRESS : std_logic_vector(31 downto 0) := x"EFFF0001"; constant GUIDPREFIX_WIDTH : natural := 96; constant PROTOCOLVERSION_WIDTH : natural := 16; @@ -369,6 +369,10 @@ package rtps_package is length : natural; end record; + -- TODO: Use this everywhere + -- Marks the Reader Endpoint in the Endpoint Array + constant ENDPOINT_READERS : std_logic_vector(0 to MAX_ENDPOINTS-1) := (0 to NUM_READERS-1 => '1', others => '0'); + constant READER_ENDPOINT_DATA : OUTPUT_DATA_TYPE; --Deferred to package body constant WRITER_ENDPOINT_DATA : OUTPUT_DATA_TYPE; --Deferred to package body constant PARTICIPANT_DATA : OUTPUT_DATA_TYPE; --Deferred to package body @@ -403,6 +407,10 @@ package rtps_package is function min(L, R : DOUBLE_WORD_ARRAY) return DOUBLE_WORD_ARRAY; function max(L, R : DOUBLE_WORD_ARRAY) return DOUBLE_WORD_ARRAY; + function endian_swap(endianness : std_logic; data : std_logic_vector) return std_logic_vector; + function endian_swap(endianness : std_logic; data : unsigned) return unsigned; + + end package; package body rtps_package is @@ -910,7 +918,7 @@ package body rtps_package is len := len + 1; ret.data(ind+len) := (others => '0'); len := len + 1; - ret.data(ind+len) := DEFAULT_IPv4_MULTICAST_ADDRESS; + ret.data(ind+len) := DEFAULT_IPv4_META_ADDRESS; -- METATRAFFIC MULTICAST LOCATOR len := len + 1; ret.data(ind+len) := PID_METATRAFFIC_MULTICAST_LOCATOR & std_logic_vector(to_unsigned(24, 16)); @@ -926,7 +934,7 @@ package body rtps_package is len := len + 1; ret.data(ind+len) := (others => '0'); len := len + 1; - ret.data(ind+len) := DEFAULT_IPv4_MULTICAST_ADDRESS; + ret.data(ind+len) := DEFAULT_IPv4_META_ADDRESS; -- DEFAULT UNICAST LOCATOR len := len + 1; ret.data(ind+len) := PID_DEFAULT_UNICAST_LOCATOR & std_logic_vector(to_unsigned(24, 16)); @@ -942,7 +950,7 @@ package body rtps_package is len := len + 1; ret.data(ind+len) := (others => '0'); len := len + 1; - ret.data(ind+len) := DEFAULT_IPv4_MULTICAST_ADDRESS; + ret.data(ind+len) := DEFAULT_IPv4_META_ADDRESS; -- DEFAULT MULTICAST LOCATOR len := len + 1; ret.data(ind+len) := PID_METATRAFFIC_MULTICAST_LOCATOR & std_logic_vector(to_unsigned(24, 16)); @@ -958,7 +966,7 @@ package body rtps_package is len := len + 1; ret.data(ind+len) := (others => '0'); len := len + 1; - ret.data(ind+len) := DEFAULT_IPv4_MULTICAST_ADDRESS; + ret.data(ind+len) := DEFAULT_IPv4_META_ADDRESS; -- LEASE DURATION if (PARTICIPANT_LEASE_DURATION /= DEFAULT_PARTICIPANT_LEASE_DURATION) then len := len + 1; @@ -1155,4 +1163,31 @@ package body rtps_package is return ret; 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) return std_logic_vector is + variable ret : std_logic_vector(data'range); + begin + -- Assert that Data Signal is Byte aligned + assert (data'length mod 8 = 0) severity failure; + -- Little Endian + if (endianness = '1') then + -- Reverse byte Order + for i in 0 to (data'length/8)-1 loop + ret(i*8+8-1 downto i*8) := data(((data'length/8)-1-i)*8+8-1 downto ((data'length/8)-1-i)*8); + end loop; + -- Big Endian + else + ret := data; + end if; + return ret; + 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 : unsigned) return unsigned is + begin + return unsigned(endian_swap(endianness, std_logic_vector(data))); + end function; + end package body; diff --git a/src/test.vhd b/src/test.vhd index ece6a91..66239da 100644 --- a/src/test.vhd +++ b/src/test.vhd @@ -13,41 +13,32 @@ entity test is port ( clk : in std_logic; -- Input Clock reset : in std_logic; -- Synchronous Reset - input : in std_logic_vector(1 downto 0); - cnt : in natural range 0 to 12; - output : out unsigned(5 downto 0) + input : in std_logic_vector(31 downto 0); + cnt : out natural range 0 to MAX_ENDPOINTS ); end entity; architecture arch of test is - constant BUILD : std_logic := '0'; - - signal output_sig : unsigned(5 downto 0) := (others => '0'); + -- Compares argument 'ref' with every element of 'ar', and returns the index of the last match. + -- If no match is found, array length is returned. + function match_id_endpoint (ref : std_logic_vector(ENTITYID_WIDTH-1 downto 0); ar : ENTITYID_TYPE) return natural is + variable id : natural := 0; + begin + id := ar'length; + for i in 0 to ar'length-1 loop + if(ref = ar(i)) then + id := i; + end if; + end loop; + return id; + end function; begin - output <= output_sig; process(all) begin - output_sig <= (others => '0'); - - if (cnt < PARTICIPANT_DATA.length) then - output_sig <= unsigned(PARTICIPANT_DATA.data(cnt))(5 downto 0); - end if; - --- case (input) is --- when "00" => --- output_sig <= to_unsigned(1,6); --- when "01" => --- output_sig <= to_unsigned(2,6); --- when "10" => --- output_sig <= to_unsigned(3,6); --- when "11" => --- if (BUILD = '1') then --- output_sig <= to_unsigned(1,6) + cnt; --- end if; --- end case; + cnt <= match_id_endpoint(input, ENTITYID); end process; diff --git a/syn/DE10-Nano/top.qsf b/syn/DE10-Nano/top.qsf index 58e194d..17cebf9 100644 --- a/syn/DE10-Nano/top.qsf +++ b/syn/DE10-Nano/top.qsf @@ -38,7 +38,7 @@ set_global_assignment -name FAMILY "Cyclone V" set_global_assignment -name DEVICE 5CSEBA6U23I7 -set_global_assignment -name TOP_LEVEL_ENTITY rtps_builtin_endpoint +set_global_assignment -name TOP_LEVEL_ENTITY test set_global_assignment -name ORIGINAL_QUARTUS_VERSION 18.1.0 set_global_assignment -name PROJECT_CREATION_TIME_DATE "12:05:11 MAY 29, 2020" set_global_assignment -name LAST_QUARTUS_VERSION "18.1.0 Lite Edition" @@ -48,13 +48,14 @@ set_global_assignment -name MAX_CORE_JUNCTION_TEMP 100 set_global_assignment -name ERROR_CHECK_FREQUENCY_DIVISOR 256 set_global_assignment -name POWER_PRESET_COOLING_SOLUTION "23 MM HEAT SINK WITH 200 LFPM AIRFLOW" set_global_assignment -name POWER_BOARD_THERMAL_MODEL "NONE (CONSERVATIVE)" -set_global_assignment -name PARTITION_NETLIST_TYPE SOURCE -section_id Top -set_global_assignment -name PARTITION_FITTER_PRESERVATION_LEVEL PLACEMENT_AND_ROUTING -section_id Top -set_global_assignment -name PARTITION_COLOR 16764057 -section_id Top +set_global_assignment -name VHDL_FILE ../../src/rtps_handler.vhd -hdl_version VHDL_2008 set_global_assignment -name VHDL_FILE ../../src/single_port_ram.vhd -hdl_version VHDL_2008 set_global_assignment -name VHDL_FILE ../../src/rtps_builtin_endpoint.vhd -hdl_version VHDL_2008 set_global_assignment -name VHDL_FILE ../../src/rtps_package.vhd -hdl_version VHDL_2008 set_global_assignment -name VHDL_FILE ../../src/test_package.vhd -hdl_version VHDL_2008 set_global_assignment -name VHDL_FILE ../../src/test.vhd -hdl_version VHDL_2008 set_global_assignment -name VHDL_FILE ../../src/math_pkg.vhd -hdl_version VHDL_2008 +set_global_assignment -name PARTITION_NETLIST_TYPE SOURCE -section_id Top +set_global_assignment -name PARTITION_FITTER_PRESERVATION_LEVEL PLACEMENT_AND_ROUTING -section_id Top +set_global_assignment -name PARTITION_COLOR 16764057 -section_id Top set_instance_assignment -name PARTITION_HIERARCHY root_partition -to | -section_id Top \ No newline at end of file